在机器人技术中估计不确定的空间关系

Smith R, Self M, Cheeseman P. Estimating uncertain spatial relationships in robotics[J]. Autonomous robot vehicles, 1990: 167-193.

摘要

在许多机器人应用中，对于表示和推理空间关系的需求非常重要。然而，我们对于特定空间关系的认知往往是不确定的。处理这种不确定性最常用的方法是通过预先工程化来解决问题，即通过结构化工作环境和使用特定的高精度设备。然而，在某些先进的机器人研究领域，如自动任务规划、离线机器人编程和自主车辆操作中，由于环境不断变化或对推理灵活性的需求，先前的结构化方法并不可行。空间推理的复杂性进一步增加，因为关系通常不是明确描述的，而是通过不确定的相对信息给出。当涉及到使用多个不同的参考框架时，这种不确定关系会产生一个不确定关系的网络。与将空间不确定性视为几何推理中的附带问题不同，我们认为它必须成为空间表示的固有部分。在本文中，我们介绍了一种称为随机地图的空间信息表示，并提供了构建、读取信息以及随着新信息的获得而逐步修正地图的相关过程。该地图始终包含对地图中对象之间关系的最佳估计及其不确定性。这些过程提供了一种通用解决方案，用于估计不确定的相对空间关系。这些估计具有概率性质，相对于以往非常保守的最坏情况方法而言是一种进步。最后，我们还介绍了这些过程在状态估计和滤波理论的背景下的开发，为进一步扩展提供了坚实的基础。

1 引言

在诸如工业自动化和自主车辆等机器人应用中，需要表示和推理空间不确定性。过去，通过特殊目的的方法，如精密工程、非常精确的传感器以及使用夹具和校准点来解决这个需求。虽然这些方法有时提供足够的准确性，以避免明确表示不确定性的需要，但它们通常成本高昂。另一种方法是使用多个重叠的低分辨率传感器，并将所有来源的空间信息（包括不确定性）结合起来，以获得最佳的空间估计。这种综合信息通常能够提供足够的准确性，以避免硬件工程方法的需要。除了降低硬件成本之外，明确估计不确定的空间信息还可以事先判断由于累积不确定性而导致提议的操作可能失败的情况，并确定提议的传感器信息是否足以将不确定性降低到可容忍的限制。在其他情况下，例如对于廉价移动机器人，获得足够的准确性的唯一途径是结合来自多个传感器的（不确定的）信息。

将不确定的空间信息进行组合的一个困难在于，它通常以不确定的相对信息的形式出现。这在使用许多不同参考框架的情况下尤为明显，不确定的空间信息必须在这些框架之间传播。本文提出了一种通用解决方案，用于估计不确定的空间关系，不论信息是以哪个框架呈现，或者需要在哪个框架中得到答案。基本理论假设误差是“小”的，以便从一个框架到另一个框架的非线性转换近似为线性转换。

早期的表示空间不确定性的方法（例如[Taylor，1976]）在典型机器人应用中数值计算了误差的最小-最大边界。Brooks将这种分析扩展到符号计算的最小-最大边界[Brooks，1982]。与本文中的概率方法相比，这种最小-最大方法非常保守，因为在组合信息时总是假设最坏情况。
最近，为HILARE机器人开发了一种概率表示的不确定性[Chatila，1985]，它与此处介绍的方法类似，只是它仅使用标量表示位置不确定性，而不是多变量表示。在最近的一篇论文中，Brooks基于边界圆柱体和基于这些圆柱体的相交操作，开发了一种表示空间不确定性的方法[Brooks，1985]。Smith和Cheeseman（[Smith，1984]，[Smith，1985]）在解决工业自动化任务的离线编程问题时，提出了能够将不确定关系的图（由多变量概率分布表示）减少为某个感兴趣关系的单一最佳估计的操作。本文对该工作进行了扩展，但采用了估计理论的形式设置，并未使用图形变换。

总结起来，许多重要应用需要表示空间不确定性。此外，需要方法来组合不确定的空间信息，并将这些信息从一个框架转换到另一个框架。本文提出了一种矩阵表示的空间不确定性，明确表示了感兴趣世界中每个自由度的不确定性。给出了一种方法，可以组合不确定信息，无论它是在哪个框架中呈现，并允许描述一个框架相对于任何其他框架的空间不确定性。所需的过程以矩阵形式呈现，适合进行高效的实现。特别地，给出了逐步构建最佳估计“地图”及其不确定性的方法，当添加新的不确定的空间信息时。

2 随机地图

我们对对象之间的空间关系的认知本质上是不确定的。由于制造容差，人造物体与其几何模型并不完全匹配。即使匹配，由于测量误差，传感器也无法精确测量几何特征，从而无法精确定位对象。即使能够精确测量，使用传感器的机器人也无法像预期那样精确操作对象，因为存在手部定位误差。对于某些任务，通过”预先工程化”解决方案（即结构化工作环境和使用特制的高精度设备），这些误差可以减小到可忽略的限度，但这需要付出巨大的时间和成本。

然而，与其将空间不确定性视为几何推理中的次要问题，我们认为它必须作为空间表示的固有部分来处理。

在本文中，不确定的空间关系将通过一种称为随机地图的表示相互关联。该地图包含空间关系的估计值、不确定性和相互依赖关系。

首先，将描述地图的结构，然后介绍从地图中提取信息的方法。最后，将给出一种逐步构建地图的过程，随着获取新的空间信息。为了说明理论，我们将以移动机器人为例，通过在不同时间和不同位置进行传感器观测，获得有关其位置和环境组织的知识。

2.1 表示

为了表达上述想法，我们将定义以下术语。空间关系将由其空间变量的向量表示，记为$\mathcal{X}$。例如，移动机器人的位置和方向可以通过其在二维笛卡尔参考框架中的坐标$x$和$y$以及绕$z$轴的旋转, $\phi$ ,来描述：

$$\mathcal{X} =\begin{bmatrix} x\\ y\\ \phi \end{bmatrix}$$

而且，不确定的空间关系可以由其空间变量上的概率分布表示，即通过一个概率密度函数，该函数为空间变量$\mathcal{X}$的每个特定组合分配一个概率：

$$P(\mathcal{X}) = f(\mathcal{X})d\mathcal{X}$$

通常，对概率分布的如此详细的了解对于做出决策是不必要的，例如确定机器人能否完成给定的任务（例如通过门口）。此外，大多数测量设备只提供被测量关系的名义值，我们可以从传感器规格估计平均误差。基于这些原因，我们选择通过估计概率分布的前两个矩来建模不确定的空间关系-均值$\hat{\mathcal{X}}$和协方差$C(\mathcal{X})$，定义如下：

$$\begin{equation} \begin{matrix} \hat{\mathcal{X}} \triangleq E(\mathcal{X}),\\ \tilde{\mathcal{X}} \triangleq \mathcal{X} - \hat{\mathcal{X}}\\ C(\mathcal{X}) \triangleq E(\tilde{X}\tilde{X}^T) \end{matrix} \end{equation}$$

其中 $E$ 是期望运算符，$\tilde{X}$ 是均值的偏差。

对于我们的移动机器人示例来说，这些是：

$$\hat{\mathcal{X}} = \begin{bmatrix} \hat{x} \\ \hat{y}\\ \hat{\phi} \end{bmatrix} ,C(\mathcal{X})= \begin{bmatrix} \sigma^2_x& \sigma_{xy} & \sigma_{x\phi} \\ \sigma_{xy}& \sigma^2_y& \sigma_{y\phi}\\ \sigma_{x\phi}& \sigma_{y\phi}& \sigma^2_{\phi} \end{bmatrix}$$

在这里，协方差矩阵的对角元素就是空间变量的方差，而非对角元素是空间变量之间的协差。把协方差看作它们的相关系数$\rho_{ij}$是有用的：

$$\rho_{ij}\triangleq \frac{\sigma_{ij}}{\sigma_i \sigma_j}=\frac{E(\tilde{x}_i \tilde{x}_j)}{\sqrt{E(\tilde{x}^2_i)E(\tilde{x}^2_j)}},-1\leq \rho_{ij}\leq 1$$

同样，为了建模一个包含$n$个不确定空间关系的系统，我们构建所有空间变量的向量，我们称之为系统状态向量。如之前一样，我们将估计状态向量 $\hat{\mathcal{X}}$ 的均值和系统协方差矩阵 $C(\mathcal{X})$：

$$\begin{equation} \mathcal{X} = \begin{bmatrix}\mathcal{X_1}\\ \mathcal{X_2}\\ \vdots\\ \mathcal{X_n} \end{bmatrix},\hat{\mathcal{X}} = \begin{bmatrix} \hat{\mathcal{X}}_1\\ \hat{\mathcal{X}}_2\\ \vdots \\ \hat{\mathcal{X}}_n \end{bmatrix}, C(\mathcal{X}) = \begin{bmatrix}C(\mathcal{X}_1)& C(\mathcal{X}_1,\mathcal{X}_2)&\cdots & C(\mathcal{X}_1,\mathcal{X}_n) \\ C(\mathcal{X}_2,\mathcal{X}_1)& C(\mathcal{X}_2)&\cdots &C(\mathcal{X}_2,\mathcal{X}_n) \\ \vdots& \vdots& \ddots& \vdots \\ C(\mathcal{X}_n,\mathcal{X}_1)&C(\mathcal{X}_n,\mathcal{X}_2)&\cdots&C(\mathcal{X}_n)\end{bmatrix} \end{equation}$$

其中:

$$\begin{equation} \begin{matrix} C(\mathcal{X}_i,\mathcal{X}_j)\triangleq E(\tilde{X}_i \tilde{X}^T_j),\\ C(\mathcal{X}_j,\mathcal{X}_i) = C(\mathcal{X}_i,\mathcal{X}_j)^T \end{matrix} \end{equation}$$

在这里，$\mathcal{X}_i$ 是各个不确定空间关系的空间变量向量，$C(\mathcal{X}_i)$ 是相关的协方差矩阵，就像我们之前讨论的那样。

$C(\mathcal{X}_i,\mathcal{X}_j)$ 是不确定空间关系之间的交叉协方差矩阵，它允许不同空间关系的不确定性之间存在依赖关系。这些非对角矩阵提供了将新添加到地图中的信息反向传播的机制，以改进之前的空间估计，这比以前的方法更为精细。

在我们的示例中，每个不确定的空间关系都具有相同的形式，因此 $\mathcal{X}$ 具有 $m = 3n$ 个元素，我们可以写为：

$$\mathcal{X}_i = \begin{bmatrix} x_i\\ y_i\\ \phi_i \end{bmatrix}, \hat{\mathcal{X}}_i = \begin{bmatrix} \hat{x}_i\\ \hat{y}_i\\ \hat{\phi}_i \end{bmatrix},$$ $$C(\mathcal{X}_i,\mathcal{X}_j) = \begin{bmatrix} \sigma_{x_i x_j}& \sigma_{x_i y_j}& \sigma_{x_i \phi_j}\\ \sigma_{x_i y_j}& \sigma_{y_i y_j}& \sigma_{y_i \phi_j}\\ \sigma_{x_i \phi_j}& \sigma_{y_i \phi_j}& \sigma_{\phi_i \phi_j} \end{bmatrix}$$

我们的”地图”由当前对系统状态向量均值的估计组成，它给出了地图中物体相对于世界参考框架的名义位置，以及相关的系统协方差矩阵，该矩阵给出了地图中每个点的不确定性以及这些不确定性之间的相互依赖关系。

2.2 解释

对于基于不确定空间关系的某些决策，我们必须假设一个适合估计矩的特定分布。
例如，机器人可能需要能够计算某个物体出现在其视野中的概率，或者成功通过门的概率。仅给定多元概率分布的均值$\mathcal{X}$和协方差矩阵$C(\mathcal{X})$，最大熵原理表明，假设信息最少的分布是正态分布。
此外，如果空间关系是通过结合许多独立观测的证据计算得出的，中心极限定理表明，得到的分布将趋向于正态分布。

$$\begin{equation} P(\mathcal{X}) = \frac{exp[-\frac{1}{2}(\mathcal{X}-\hat{\mathcal{X}})^TC^{-1}(\mathcal{X})(\mathcal{X}-\hat{\mathcal{X}})]}{\sqrt{(2\pi)^m |C(\mathcal{X})|}}d\mathcal{X} \end{equation}$$

我们将通过绘制具有给定均值和协方差信息的正态分布的等概率轮廓来表示不确定的空间关系。这些轮廓实际上是同心椭圆体（对于二维是椭圆），其参数可以从协方差矩阵$C(\mathcal{X}_i)$中计算出来[Nahi, 1976]。重要的是要强调，我们并不假设不确定的空间关系由正态分布描述。我们估计其分布的均值和方差，并仅在需要计算特定概率轮廓时使用正态分布。

在本文的图中，绘制的点显示了物体的实际位置，这些位置只有模拟器知道，并且为了我们的利益而显示出来。机器人的信息通过椭圆来展示，这些椭圆以关系的估计均值为中心绘制，并且使其包围一个99.9%的置信区域（大约四个标准差）的关系。

2.3 示例

在本文中，我们将参考一个涉及移动机器人三个自由度导航的二维示例。在这个示例中，机器人执行以下操作序列：

• 机器人感知物体＃1
• 机器人移动。
• 机器人感知一个物体（物体＃2），它确定不是物体＃1。
• 再次尝试，机器人成功地感知到物体＃1，从而有助于定位自身、物体＃1和物体＃2。

图1显示了不确定的空间关系的两个示例——物体＃1的感知位置和机器人预定运动的终点。机器人最初位于标记为’O’的位置。在我们的随机地图中，有足够的信息供机器人决定在进行运动时与物体发生碰撞的可能性有多大。在这种情况下，概率非常小。

图2展示了这种空间知识如何在机器人运动后从其新的参考框架中呈现出来。正如预期的那样，物体＃1位置的不确定性在与机器人运动的不确定性相结合时变得更大。

从这个新位置，机器人感知到物体＃2（图3）。机器人能够根据其随机地图中的信息确定这是一个新的物体，并且不是它之前观察到的物体＃1。

在图4中，机器人再次感知到物体＃1。这个新的闭环传感器测量作为约束条件被纳入地图中，减少了机器人、物体＃1和物体＃2位置的不确定性（图5）。

3 阅读地图

3.1 不确定关系

在了解了我们如何通过系统状态向量的均值和协方差的估计来表示不确定的空间关系之后，现在我们讨论估计未知多元概率分布的前两个矩的方法。详细的理论基础可以参考[Papoulia, 1965]。

3.1.1 线性关系

最简单的情况涉及在随机变量中是线性关系的关系，例如：

$$\mathbf{y}=\mathbf{M}\mathbf{X}+\mathbf{b}$$

其中，$\mathbf{X}(n \times 1)$ 是一个随机向量，$\mathbf{M}(r \times n)$ 是非随机系数矩阵，$\mathbf{b}(r \times 1)$ 是一个常向量，$\mathbf{y}(r \times 1)$ 是结果随机向量。利用公式（1）的定义和期望运算符$E$的线性性，可以很容易地验证关系$\hat{\mathbf{y}}$的均值为：

$$\begin{equation} \hat{\mathbf{y}} = \mathbf{M}\hat{\mathbf{X}} + \mathbf{b} \end{equation}$$

协方差矩阵$C(\mathbf{y})$ 为：

$$\begin{equation} C(\mathbf{y}) = \mathbf{M} C(\mathbf{X}) \mathbf{M}^T \end{equation}$$

我们还需要能够计算关系$\mathbf{y}$和另一个关系$\mathbf{Z}$之间的协方差，给定了关系$\mathbf{X}$和$\mathbf{Z}$之间的协方差：

$$\begin{equation} \begin{matrix} C(\mathbf{y},\mathbf{Z}) = \mathbf{M} C(\mathbf{X},\mathbf{Z}),\\ C(\mathbf{Z},\mathbf{y}) = C(\mathbf{Z},\mathbf{X})\mathbf{M}^T \end{matrix} \end{equation}$$

在给定$\mathbf{X}$的正确矩的情况下，可以精确计算$\mathbf{y}$的多元分布的前两个矩。此外，如果$\mathbf{X}$服从正态分布，则$\mathbf{y}$也服从正态分布。

3.1.2 非线性关系

通过下面的公式计算非线性关系在随机变量上的前两个矩将是真实值的一阶估计。
要计算实际值，需要了解空间变量的完整概率密度函数，但在我们的应用中通常不会提供这样的信息。通常的方法是近似非线性函数。

$$\mathbf{y} = f(\mathbf{X})$$

通过围绕估计均值$\hat{\mathbf{X}}$展开的泰勒级数近似非线性函数，得到如下公式：