第十章：即时定位和建图(SLAM)

10.1 介绍

本章和后续章节中处理在机器人学中的最基本的问题之一，即使定位和建图问题simultaneous localization and mapping，一般简称为SLAM，也称作Concurrent Mapping and Localization, CML。当机器人没有环境地图或者不清楚自身的位置时就会产生SLAM问题。机器人所具有的信息就只有历次测量值$z_{1:t}$和控制量$u_{1:t}$。在SLAM问题中，机器人构建地图的同时确定自生相对于地图的位置。SLAM问题远比我们目前讨论过的问题困难的多。它比在已知地图中进行定位更难，需要沿路估计地图(译者按:原文是the map is unknown, 根据上下文关系，我怀疑这里笔误写错了，所以翻译为已知地图)。SLAM问题也比已知位置来建图容易的多，因为SLAM问题中不知道自身的位置，需要沿途估计之。

从概率学的角度来看，SLAM问题有两种主要的形式，它们具有同等重要的地位。其一称为在线SLAM问题online SLAM problem，它估计的是瞬时位姿以及地图的后验概率: $$ \begin{equation}\label{f1} p(x_t, m | z_{1:t}, u_{1:t}) \end{equation} $$ 其中$x_t$是时刻$t$的位姿，$m$则是地图，而$z_{1:t}$和$u_{1:t}$分别是历次测量值和控制量。图10.1中描述了一个在线SLAM问题的图模型。


图 10.1. 在线SLAM问题的图模型。其目标就是估计当前机器人的位姿以及地图的后验概率。	图 10.2. 完全SLAM问题的图模型。这里估计机器人的整条路径以及地图的后验概率。

第二种SLAM问题成为完全SLAM问题full SLAM problem。在完全SLAM中，我们尝试计算整个路径$x_{1:t}$以及地图的后验概率，而不仅仅是当前的位置$x_t$，如图10.2所示。 $$ \begin{equation}\label{f2} p(x_{1:t}, m | z_{1:t}, u_{1:t}) \end{equation} $$ 在线和完全SLAM问题之间的细微差异在于，他们使用了不同类型的算法。特别的，在线SLAM问题是对完全SLAM问题的历史位姿的积分 $$ \begin{equation}\label{f3} p(x_t, m | z_{1:t}, u_{1:t}) = \int \int \cdots \int p(x_{1:t}, m | z_{1:t}, u_{1:t}) dx_1 dx_2 \cdots dx_{t-1} \end{equation} $$ 在线SLAM中，这些积分一般都是一次进行的。这将导致SLAM算法依赖结构发生一些有趣的变化，我们将在下一章中全面的探索它们。

SLAM问题的第二个关键特征与其估计问题的本质有关，即有连续的估计问题，又有离散的问题。连续的估计问题是指，对环境中障碍物的位置估计，以及对机器人自身的位置估计。在基于特征的表示方法中，障碍物可以是路标，也可以是距离传感器探测到的物体碎片。离散问题则是要解决关联度的问题，当检测到一个障碍物， SLAM算法需要推算出该物体与之前检测到的物体之间的关系。这个推导过程是一个典型的离散问题，该物体要么与之前的某个物体一样，要么不一样。

在之前的章节中，我们已经讨论过类似的连续-离散估计问题。比如说在7.4节中使用EKF定位方法估计机器人位姿的问题本身是连续的，但是它还需要估计测量值与地图上路标之间的关联度，这就是离散的。本章和后续章节中，我们将要讨论各种不同的方法，来解决SLAM算法连续的一面和离散的一面。

有时，显式的描述关联度变量是很有用的，就像我们在第七章中处理定位问题那样。那么在线SLAM就可以写作: $$ \begin{equation}\label{f4} p(x_t, m, c_t | z_{1:t}, u_{1:t}) \end{equation} $$ 而完全SLAM问题则是: $$ \begin{equation}\label{f5} p(x_{1:t}, m, c_{1:t} | z_{1:t}, u_{1:t}) \end{equation} $$ 在线后验概率是对完全后验概率的积分: $$ \begin{equation}\label{f6} p(x_t, m, c_t | z_{1:t}, u_{1:t}) = \int \int \cdots \int \sum_{c_1} \sum_{c_2} \cdots \sum_{c_{t-1}} p(x_{1:t}, m, c_{1:t} | z_{1:t}, u_{1:t}) dx_1 dx_2 \cdots dx_{t-1} \end{equation} $$ 估计式($\ref{f4}$)或式($\ref{f5}$)的完全后验概率是解决SLAM问题的黄金法则。完全后验概率包含了所有已知的地图信息和机器人的位置或者路径信息。

实际上计算全后验概率是不现实的，主要有两方面的原因。其一，连续参数空间的维度很高。其二，大量的离散关联度变量。很多目前的SLAM算法所构建的地图中有成千上万个特征。即使已知关联度，光这些地图的后验概率所涉及到的空间维度都在$10^5$甚至更高的量级上。这与定位问题形成了鲜明的对比，在定位问题中所需要估计的后验概率只是关于三维空间中的连续状态。此外，在很多应用中关联度都是未知的。关联度变量的可能取值随着时间呈指数级增长，因此实际的SLAM算法在处理关联度问题时一定是近似的方法。

在后续的几章中还要讨论SLAM问题。本章剩余的内容介绍一种用于在线SLAM的EKF算法，用到了很多在3.3节中介绍的关于EKF的内容，以及7.4节中应用于定位的EKF算法。我们将介绍EKF算法的推导过程，并首先将之应用于已知关联度的SLAM问题中，再进一步的扩展，将之用于解决未知关联度的SLAM问题。

10.2 扩展卡尔曼滤波的SLAM算法

10.2.1 设定和描述

历史上最早的，也可能是影响最大的，SLAM算法是建立在扩展卡尔曼滤波器上的。EKF SLAM算法通过EKF进行在线SLAM，使用极大似然来估计数据关联度。因此EKF SLAM受限于一些近似和假设。

再EKF SLAM算法中，地图是基于特征的，由路标点构成的。出于计算复杂度的考虑，路标的数量一般都比较少，小于1000的量级。此外，EKF算法要求路标要尽可能少的存在歧义。因此，EKF SLAM算法需要在特征检测器上下一番功夫，有时还需要人为的制造一些特征。

与任何EKF算法一样，EKF SLAM假设机器人的运动噪声和感知噪声都是高斯形式的。并且后验概率中的不确定度要尽量小一些，否则EKF的线性化操作会引入很大的误差。和7.4节中介绍的EKF定位算法一样，EKF SLAM只能处理路标的正面positive信息，不能处理负面negative信息，比如说传感器测量值中没有表现出某个路标。这都归咎于高斯形式的执行度表示方法。

10.2.2 已知关联度的SLAM算法

在已知关联度的情况下，SLAM算法只需要处理连续部分的问题。这类算法的实现在很多方面都与7.4节中的EKF定位算法类似，但是有一个关键的不同之处，在估计机器人位姿$x_t$的同时， EKF SLAM算法还需要估计运动过程中检测到的所有路标的坐标值。这使得我们有必要将路标的坐标也包含进状态向量中。为了方便，我们称这种组合了机器人位姿和地图特征的状态向量为组合特征向量Combined State Vector，记为$y_t$：

$$ \begin{equation}\label{f7} y_t = \begin{pmatrix} x_t \\ m \end{pmatrix} = \begin{pmatrix} x & y & \theta & m_{1,x} & m_{1,y} & s_1 & m_{2,x} & m_{2,y} & s_2 & \cdots & m_{N,x} & m_{N,y} & s_N \end{pmatrix}^T \end{equation} $$

其中$x, y, \theta$分别是$t$时刻机器人的位置坐标和方向角，注意必要与状态向量$x_t, y_t$搞混了。$m_{i,x}, m_{i,y}$分别是第$i$个路标的坐标值，$s_i$则是该路标的身份标识， $i = 1, \cdots, N$。这个状态向量的维度是$3N + 3$，其中$N$为地图中路标的数量。显然对于一个合理的数值$N$，该向量的维度都要远大于7.4节中EKF定位算法所估计的位姿向量。 EKF SLAM算法计算在线的后验概率$p(y_t | z_{1:t}, u_{1:t})$。

算法10.1中描述了EKF SLAM算法，注意它与算法7.2中的EKF定位算法的相似度。第2到5行中实现了运动更新，而6到20行则处理了测量向量。

第3到5行中操作关联与运动模型的置信度的均值和方差。该操作只影响了那些与机器人位姿相关的置信度分布，保留地图的所有均值和协方差参数，以及位姿地图(pose-map)协方差不变。 7到20行遍历了所有的测量值。

只有当检测到没有初始化过位置估计的路标时，第9行才会返回"TRUE"。此时，在第10行中通过将距离和方向角的测量值投影到位置坐标中完成对这些未检测到的路标的初始化。在后面我们会看到，这一步对于EKF的线性化而言是很重要的，对于线性卡尔曼滤波而言，就不需要这一操作。

对于每个测量值，在第14行中都要计算一个"预测值"，并在第17行中计算卡尔曼增益Kalman gain。需要注意，卡尔曼增益是一个$3 × (3N + 3)$的矩阵，而且一般情况下该矩阵都不是稀疏的。在整个状态估计过程中，信息都是前向传播的。在第18、19行更新滤波器的时候，又会反过来影响机器人的置信度。

实际上，卡尔曼增益是针对所有状态变量的，而不仅仅是观测到的路标和机器人位姿的，这一点很重要。在SLAM中，观测到一个路标不仅仅要更新该路标的位置估计，同时也要更新其它路标的估计。对其它路标的更新是以机器人位姿为媒介的，观测到了一个路标，=就会更新机器人的位姿估计，也因此消除了同一个机器人之前所观测到的路标的一些不确定性。这一效应使得我们不用显式的对过去的位姿建模，那样将使得我们进入完全SLAM问题的范畴，也导致EKF成为一个非实时的算法。实际上，这种关联可以通过高斯后验概率来捕获，更具体地，体现在协方差矩阵$\Sigma_t$地非对角元上。

图10.3中以一个例子说明了EKF SLAM算法。机器人从一个起始位置出发开始导航，并将该起始点设定为其坐标系统的原点。随着机器人移动，其自身的不确定度在增加，如图中逐渐变大的不确定度椭圆所示。同时它也感知到了附近的路标，并将之标记在地图上，这些路标具有一定的不确定度，它们与固有的测量不确定度一起增加了位姿的不确定性。

机器人位姿的不确定性导致之后检测到的路标的不确定度也在不断地增加。在图10.3d中刻画了一个有意思地变换，此时机器人又观测到了它在起始时刻看到的那个路标，其位置已经相对比较确定了。通过这个信息，机器人地位置误差就降低了，在图10.3d中在机器人最终的位置上可以看到一个很小的误差椭圆! 这一信息也降低了地图中其它路标的不确定度。

这一现象的产生源自于高斯后验概率中的协方差矩阵所描述的关联关系。因为早期路标的不确定性大多数都是由于机器人位姿的不确定性所导致的，而且这些不确定性随着时间的流逝一直存在着，对于这些路标的位置估计也受到了牵连。当关于机器人位姿的信息增加了，该信息将传播给之前感测到的路标。这一效应可能使SLAM后验概率中最重要的特性。帮助定位机器人的信息将通过地图传播出去，也因此提高了地图中其它路标的定位精度。


(a)	(b)	(c)	(d)
图10.3. 应用于在线SLAM问题的EKF算法。机器人的路径用一个点线来表示，对其位置的估计则用一个阴影的椭圆来描述。在图中有8个可以分辨的路标，但是不知道它们的具体位置，用小点来表示，对于它们的位置估计则用一个白色的椭圆来描述。从(a)图到(c)图，机器人位置的不确定度在不断的增加，这是因为它所观测到的路标的不确定度在不断增长。在(d)图中，机器人又感知到了第一个路标，此时所有的路标的不确定度都下降了，关于其自身位置的不确定度也随之降低了。这些图片是由斯坦福大学的Michael Montemerlo提供的。

10.2.3 EKF SLAM的数学推导

暂略

$$ \begin{equation}\label{f8} μ_0 = \begin{pmatrix} 0 & 0 & 0 & \cdots & 0 \end{pmatrix}^T \end{equation} $$ $$ \begin{equation}\label{f9} \Sigma_0 = \begin{pmatrix} 0 & 0 & 0 & 0 & \cdots & 0 \\ 0 & 0 & 0 & 0 & \cdots & 0 \\ 0 & 0 & 0 & 0 & \cdots & 0 \\ 0 & 0 & 0 & \infty & \cdots & 0 \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & 0 & \cdots & \infty \\ \end{pmatrix} \end{equation} $$

10.3 未知关联度的EKF SLAM算法

10.3.1 通用的EKF SLAM算法

现在我们使用增量式的极大似然估计maximum likelihood, ML来确定关联度，将已知关联度的EKF SLAM算法扩展为通用的EKF SLAM算法。算法10.2描述了这种未知关联度的算法。

因为关联度式位置的，算法EKF_SLAM的输入缺少关联度变量$c_t$，取而代之的式当前地图的尺寸$N_{t-1}$。在第3到第6行中的运动更新过程与算法10.1中的一样。但是其测量更新循环则是不同的。

从第8行开始，它首先创建一个新的路标并标记为$N_t + 1$，并在第9行初始化这个新路标的位置。它通过机器人位姿的估计以及测量值中的距离和方向角信息计算新路标的期望位置。在第9行中，也为这个新路标赋予了一个身份。

在接下来的第10到18行，为所有的$N_t + 1$个路标计算了各种更新量。第19行设定了创建新路标的阈值，当新路标与目前地图中已知的所有路标的马氏距离超过了数值$\alpha$时就创建之。在第20行中选择具有最大似然度的关联度。如果测量值更多的关联于这个之前未探测过的路标，那么就在第21行中接受该路标并增加路标数量。相应的状态向量和协方差矩阵也要跟着扩展，这个过程比较单调在算法10.2中没有体现。最后在第23行和24行中更新EKF。算法EKF_SLAM返回了新的路标数量$N_t$以及均值$μ_t$和协方差$\Sigma_t$。

对于这个EKF SLAM算法的推导可以只接从前面的推导过程中得到。具体的，该算法第9行所述的初始化过程与算法10.1中的第10行一样。算法10.1的第10到18行对应着这里的第12到17行，只是这里多了一个变量$\pi_k$用于计算极大似然关联度。

在第20行中选择极大似然关联度的方式，以及17行中定义的马氏距离与在7.5节中的讨论类似。算法7.3中的第16行就是使用类似的方法确定最可能的路标的。在算法10.2的第23和24行中进行的测量更新操作也与那些已知关联度的EKF算法类似，假设扩展后的地图的向量和矩阵都具有相同的维度。

通过限制第10到18行只考虑机器人附近的路标，这里的EKF_SLAM实现示例还可以更高效。此外，很多在内层循环中计算的数值和矩阵都可以通过遍历一个不止一个特征的测量值向量$z_t^i$缓存起来。实际上，地图中特征的一个比较好的组织方式，对内层循环的精致优化可以大大的降低运行时间。

10.3.2 示例

图10.4中显示了一个在仿真环境中的已知关联度的EKF SLAM算法。上面的三个子图描述了后验概率分布，对于独立的路标和机器人位置而言显得微不足道。下图中描述了扩展的状态向量$y_t$的协方差矩阵。

从图10.4c中可以很容易看出，一段时间之后所有的$x$和$y$坐标估计就都变成完全关联的了。这意味着，地图已经相对比较清晰了，只留下了不能消除的全局定位不确定度。这突显出了SLAM问题的一个重要特征，地图的绝对坐标与定义机器人初始位姿坐标系的关联，只能通过近似的方式来确定，而相对坐标系可以通过非对称的不确定度来建立。(译者按:完全不知所云)

实际上，EKF SLAM已经在很多导航问题中得到了成功的应用，比如说天上飞的、水里游的、屋里跑的等等各种各样的场景中都有使用。图10.5中给出了一个水下机器人Oberon的应用示例，该机器人是由悉尼大学开发的，如图10.6所示。

Oberon装备有一个声纳系统，可以以较高的分辨率扫描环境，并检测到50米开外的障碍物。为了辅助建图问题，学者们在水中树立了一些细长的障碍物，它们可以相对比较容易地被声纳系统探测到。在这种特殊的环境中，有一排这样的物体，间隔10米排开。此外，远处还有一个悬崖提供了附加的点特征，可以供使用声纳系统地机器人定位。

如图10.5所示的实验，机器人沿着这些地标前进，走到尽头在调头返回。实验过程中，机器人测量了路标，并使用了本章中介绍的EKF SLAM算法将其信息融合进地图中。在地图中还用三角标记了机器人的路径。在每一个三角附近都有一个椭圆，表示卡尔曼滤波器估计的机器人$x$-$y$坐标的协方差矩阵。方差越大，椭圆越大，机器人对其自身的位置就越不确定。

图中的小圆点对应着地图中的路标，通过声纳扫描的细小的高反射率的障碍物来获得。大部分的地标都是互斥的，适用了下一节中将要介绍的机制。有些被认为是地标而添加到地图上了。直到机器人运动结束，一共将14个这样的障碍物标记为路标了，它们都在图10.5中用一个椭圆圈出来了。这些路标认为添加的，也包含了机器人附近的一些地理特征。剩余的位姿不确定度很小。


(a)	(b)	(c)
图10.4. 在仿真环境下的已知数据关联度的EKF SLAM算法。上图是地图，其中灰度等级是对应的每个路标的不确定度。下图则是协方差矩阵，是归一化之后的后验概率估计的协方差矩阵。一段时间之后，所有的$x$和$y$坐标的估计都变成了完全关联的了。


图10.5. 用卡尔曼滤波器估计地图和机器人位置。图片由澳大利亚户外机器人中心的Stefan Williams和Hugh Durrant-Whyte提供。	图10.6. 悉尼大学开发的水下机器人Oberon。图片由澳大利亚户外机器人中心的Stefan Williams和Hugh Durrant-Whyte提供。

下图10.7中显示了另一个EKF SLAM算法的实现。图(a)中是一个MIT的RWI B21移动机器人，呆在测试环境旁边。测试环境是一个网球场，中间的拦网就是障碍物。为了对比算法结果，手工的测量了它们的位置，精度为厘米级。图b中是原始的里程计路径，在图c中给出了EKF SLAM的结果，覆盖在手工绘制的地图上。读者可以看到这就是一个准确的地图。


(a) RWI B21移动机器人和测试环境	(b) 原始里程计	(c) EKF SLAM的运行结果
图10.7. (a) MIT B32移动机器人。(b) 机器人的原始里程计，由人手动控制它在整个场地中运动。(c) EKF SLAM算法输出的高精度地图。这里将估计的地图覆盖子手工绘制的地图上了。所有的图片和结果都是由MIT的John Leonard和Matthew Walter提供的。

10.3.3 特征选择和地图管理

在实际应用中提高EKF SLAM算法的鲁棒性需要一些地图管理map management的技术。很多这些技术都是建立在高斯噪声假设不准确这一事实上的，而且噪声分布的远处末端存在很多不准确的测量值。这些不准确的测量值可能导致在地图中创建一个假地标，从而对机器人定位造成负面影响。

很多目前的技术都有处理测量空间中异常值outliers的机制。这些异常值就是那些不准确的路标，超出了地图中任何路标的不确定度范围。拒绝这些异常值的最简单技术就是维护一个临时的路标列表provisional landmark list。每当测量时发现了新路标，在扩展地图将新路表添加进来之前，先将其放置到一个临时路标列表中。这个列表就好像是一个地图，但是其中的路标并不用于调整机器人的位姿，对应的测量值梯度设定为0。一旦发现有一个路标总是被观测到，而且其不确定度椭圆在收敛，就将之键入正则地图中。

在实际实现过程中，这个机制可以有效地降低地图中路标的数量，同时保留所有物理路标的高概率值。下一步，在目前的实现中也很常见，就是维护一个路标存在概率landmark existence probability。这个后验概率可能以对数差异比的形式实现，并标记为$o_j$，其中$j$为地图中的路标索引。当第$j$个路标$m_j$被探测到时，就将$o_j$增加一个固定的值。如果路标应该在机器人传感器探测范围内，却没有检测到$m_j$就减少$o_j$。因为永远不能确定的直到一个路标是否应该在机器人的感知范围内，所以减少$o_j$值的时候应当将其考虑在内。当$o_j$低于某个阈值的时候，就将对应的路标从地图中移除。在无高斯测量噪声的情况下，这样的技术将生成一个更为轻量级的地图。

初始化新路标的估计时，从协方差矩阵中的一个非常大的元素开始，正如公式($\ref{f9}$)所描述的那样，可能导致数值不稳定numerical instabilities。这是因为第一次协方差更新将及大大的改变这个值。更好的策略是为每一个之前未观测到的特征，引入一个显式的初始化过程。具体的，这个过程应当直接使用实际的路标不确定度来初始化协方差矩阵$\Sigma_t$，来替代算法10.2的第24行的计算，对于其第23行计算均值的方法也要如此操作。

如之前所提到的，用极大似然法来估计数据关联度有一个明显的限制。该限制来自于极大似然方法偏离了概率机器人学中对整个后验概率进行估计的思想。与其维护扩展状态和数据关联度之间的联合后验概率，该算法将数据关联度问题简化为一个确定性的决策问题，认为具有最大似然度的数据关联总是正确的。这一限制使得EKF在面对路标混淆问题时，变得很脆弱，它将导致错误的结果。实际上，学者们通常通过选择下面两种方法之一来补救这一问题，这两种方法都能降低路标混淆的概率。

空间排列(Spatial arrangement)。路标之间的距离越远，将他们混淆的几率就越小。因此在一般实现的时候，都会先择让路标之间具有足够的距离，这样混淆它们的概率就会变得很小。这就带来了一个很有意思的权衡: 增大路标的数量将增加混淆它们的风险，但是太少的路标就很难定位机器人。目前我们对最优的路标密度知之甚少，学者们通常都是凭直觉来放置路标。
标记(Signatures)。在选择合适的路标时，其实质是要最大化路标的可识别度。比如说，门可以有不同的颜色，走廊可能由不同的宽度。这些标记是成功SLAM技术的基础。

借助这些辅助手段，EKF SLAM算法已经在很多实际的建图问题中得到了应用。

EKF SLAM算法额关键限制是其需要选择合适的路标。通过减少波束来探测路标是否存在，将会丢弃很多传感器数据。在没有大量的进行前滤波pre-filtering就应用传感器将导致SLAM算法的信息丢失。此外，二次方的更新时间限制了EKF算法的使用范围，一般其处理的地图中的特征点都少于1000个。而在实际应用中，地图的特征通常有$10^6$的量级甚至更高，在这种情况下EKF就不再适用了。

地图的维度过低将增大数据关联问题的难度。这很容易验证，当我们睁开眼睛看到所在的房间的全景的时候，不难确定我们的位置。但是如果我们只被告知了一小部分路标的位置，再来判断自己的位置就比较困难了。在后续的章节中我们将介绍其它SLAM算法，能够更轻松的处理数据关联度的问题，而且可以处理更多的特征。这就是EKF SLAM算法的基本困境fundamental dilemma，增量的极大似然数据关联方法可能在有几百万个特征的密集地图上工作的很好，但是面对稀疏的地图它就嗝屁了。在后续的章节中，我们将讨论更高效的SLAM算法可以处理更大的地图。我们也要讨论更鲁棒的数据关联技术。因为EKF SLAM算法有很多限制，本章中所讨论的技术的价值都已经成为了历史。

10.4 总结

本章中描述了一般的SLAM问题并介绍了EKF方法。

SLAM问题是同时进行定位和建图的问题，机器人在尝试定位自己在地图中的位置的同时也在构建环境的地图。
SLAM问题有两个版本:在线的和全局的。这两个问题都要估计地图。在线的问题所要估计的是机器人当前的位姿，而全局的问题则要确定所有的位姿。在实际应用中，这两个问题具有同等的重要性，本书将平等的看待它们。
EKF SLAM算法被认为是最早的SLAM算法。它使用扩展卡尔曼滤波器解决在线SLAM问题。在已知关联度的情况下，其算法就是增量形式的。其每个更新过程的时间复杂度是关于地图路标点数量的二次方。
在未知关联度时，EKF SLAM算法使用增量的极大似然估计器来解决关联度问题。当路标具有足够的区分度时该算法工作的很好。
本章还讨论了一些地图管理的技术。有两种常用的策略来确定异常点。其一是维护一个路标的临时列表，记录了那些不经常观测到的路标。其二是根据路标证据计数器来计算路标存在的后验概率。
EKF SLAM算法已经在很多机器人建图问题中得到了成功的应用。它的主要缺陷就是需要路标具有足够的辨识度，而且其更新滤波器的计算复杂度也很高。

实际上，EKF SLAM算法已经比较成功了。当路标具有足够的辨识度，这种方法就能很好的近似后验概率。计算完整的后验概率的优势是多方面的，它能抓住所有残存的不确定度，使得机器人在推导自身控制的时候能够考虑到其真实的不确定度。但是EKF SLAM算法受限于其巨大的更新复杂度，以及稀疏地图的限制。使其很难处理数据关联度问题，因此当路标具有高度的歧义性的时候， EKF SLAM算法就表现得很差了。EKF SLAM算法还依赖于一个增量的极大似然估计数据关联度的技术，使得它更加脆弱。该技术使得它不可能修改过去的数据关联度，当ML数据关联出错的时候将导致算法失效。

应用于在线SLAM问题的EKF SLAM算法，是不能用于解决完全SLAM问题的。在完全SLAM问题中，每次迭代都要在状态向量中增加新的变量，这将导致状态向量和协方差矩阵不断地增长，而且没有边界。因此需要更新的协方差矩阵就会不断地增加，最终用尽所有的计算资源。

10.5 参考文献

SLAM问题早在现代机器人发明之前的几个世纪就已经提出了。从一个声纳平台对一个物理结构建模的问题是很多领域的核心，比如说地质学geosciences，摄影测量学photogrammetry以及计算机视觉computer vision。如今的SLAM算法额核心数学技术最初都是为了计算行星轨道而开发的。比如说，最小二乘法可以追溯到Johann Carl Friedrich Gauss (1809)。SLAM本质上是一个地质探索问题(geographic surveying problem)。将之迁移到机器人身上，为人类探索者很少面对的挑战，比如说关联度问题，寻找合适的特征的问题。

在机器人学中，Cheeseman and Smith (1986); Smith and Cheeseman (1986); Smith et al. (1990).的一系列影响深远的论文将EKF引到SLAM问题中。这些论文首先描述了本章中讨论的EKF算法。就像我们在本书中介绍的那样，Smith et al.讨论了将EKF算法应用于在具有点路标的已知数据关联度的的基于特征的地图的上下文中。第一个EKF SLAM算法的实现要归功于Moutarlier and Chatila (1989a,b) and Leonard and Durrant-Whyte (1991), 有些使用人工的标记作为路标。当很多作者都在研究在建图的同时维护准确的位姿估计(Cox 1991)时，EKF算法变得很时髦。Dickmanns and Graefe (1988)的早期工作，在自动驾驶的汽车上估计道路曲率就与之有高度的联系，参见文档(Dickmanns 2002)。

最近一些研讨会(Leonard et al. 2002b)指出，SLAM是一个高度活跃的研究领域。在Thrun (2002)中可以找到大量的关于SLAM的文献， Leonard and Durrant-Whyte (1991)则称之为同时建图和定位(concurrent mapping and localization, CML)。在地图中维持关联度的重要性是由Csorba (1997)在其博士论文中指出来的，它还建立了一些基本的收敛结果。自此，很多作者将这一基本的方法扩展成了很多不同的方法。本章所讨论的特征管理技术归功于Dissanayake et al. (2001, 2002); see also Bailey(2002)。 Williams et al. (2001)提出了在SLAM中的临时特征列表的思想，降低了特征检测误差的影响。Leonard et al. (2002a)讨论了特征初始化，显式的维护了对之前位姿的估计，来适应只能提供关于特征坐标的不完整信息的传感器。 Castellanos et al. (1999)提出通过显式的在后验概率中加入扰动"perturbations"因子来避免奇异解的表达形式。他们的报告显著的增加了vanilla EKF的稳定度。 Jensfelt et al. (2002)发现使用基本几何约束可以显著的提高室内SLAM算法效果，比如说大多数墙壁都是平行的或者垂直的。早期使用声纳进行SLAM工作可以追溯到Rencken (1993), 目前用于SLAM的声纳系统可以在文献Tardós et al. (2002)中找到。Castellanos et al. (2004)提供了一个关于EKF的重要的一致性分析。在文献Vaganay et al. (2004)中可以找到一个对多个算法的实验。 Salichs and Moreno (2000)还讨论了一些开放性的问题。关于重要的数据关联度问题的研究将在后续章节中综述。

注意到，SLAM问题的EKF解决方案的限制源自于其协方差矩阵的二次方本质。这一缺陷并不是没有被注意到。在过去的几年，有很多学者都说，通过将地图分解为子地图，EKF SLAM算法可以获得显著的可扩展性，可以分别维护各自的协方差。该领域的一些初始工作是由Leonard and Feder (1999), Guivant and Nebot (2001), and Williams (2001)提出的。Leonard and Feder’s (1999)的解耦随机地图decoupled stochastic mapping算法将地图分解为一系列的更小的，更容易管理的子地图。这个方法也具有很高的计算效率，但是不能够提供通过局部地图网络传播信息的机制(Leonard and Feder 2001)。 Guivant and Nebot (2001, 2002)相比之下，提供了一个关于协方差矩阵的近似分解，降低了EKF的复杂度。Williams (2001) and Williams et al. (2002)提出了带约束的局部子地图滤波器constrained local submap filter, CLSF，依赖于在机器人附近所创建的独立局部子地图。Williams et al. (2002)提供了水下建图的结果，图10.5是他的早期工作。 Tardós et al. (2002)所描述的序列地图联合技术sequential map joining techniques是一个关联分解过程。Bailey (2002)提出了类似的技术，层级的表示SLAM地图。 Folkesson and Christensen (2003)描述了一种技术，频繁的更新机器人附近小范围的地图，而地图中其它地方的更新频率则相对较低。所有的这些技术都具有与完全EKF解决方案相同的收敛速度，但是有一个$O(n^2)$的计算边界。但是它们在具有几万个特征的角度尺度的问题上工作的比较好。

很多学者都研究过混合SLAM技术，将EKF类的SLAM技术与类似占用栅格地图这样的体积式技术像结合。 Guivant et al. (2004) and Nieto et al. (2004)提出的混合尺度地图Hybrid metric map, HYMM使用类似占用栅格地图这样的尺度地图，将地图分解为一个个三角区域(LTR)，作为区域的基本表示形式。局部地图通过EKF来整合。Burgard et al. (1999b)也将地图分解为局部的占用栅格地图，但在将局部地图组合为一个联合的全局地图时，使用的是EM算法expectation maximization，参见Dempster et al. (1977))。 Betgé-Brezetz et al. (1995, 1996)的工作在SLAM框架下融合了两种类型的表达形式，用位图来表示户外地形，用特征来表示稀疏的户外物体。

将SLAM技术扩展到动态环境中的工作可以在文献Wang et al. (2003), Hähnel et al. (2003c), and Wolf and Sukhatme (2004)中找到。Wang et al. (2003)开发了一种称为SLAM with DATMO的算法，是对SLAM with the Detection And Tracking of Moving Objects的简称。它们的方法基于EKF，但是允许存在可能移动的特征。Hähnel et al. (2003c)研究了在一个有很多移动物体的环境中进行SLAM的问题，它们成功的应用EM算法过滤了那些可能与移动物体相关的测量值。如此，它们就能够获得传统SLAM技术所不能够的环境地图。Wolf and Sukhatme (2004)的方法维护了两个环境地图，一个是静态地图，另一个则是移动的物体。通过一个正则的基于路标的SLAM算法获得SLAM类的定位。

SLAM系统已经在很多系统中得以应用。Rikoski et al. (2004)为潜艇的声纳里程计提供SLAM技术，提供了一个新的听觉里程计。Nüchter et al. (2004)描述了一种在废弃矿井中使用SLAM的方法，它将传统范式扩展到完整的六维位置空间中。对多机器人LAM问题的扩展也已经被很多学者研究过了。一些早期的工作可以在文献Nettleton et al. (2000)中找到，它开发了一种技术，各个机器人都维护一个局部的地图，并使用后验概率的信息来融合它们。另一种技术要归功于Rekleitis et al. (2001a)，他用一组固定的和移动的机器人来降低使用SLAM技术的定位误差。Fenwick et al. (2002)提供了一个综合的方法论来研究多机器人地图融合，尤其是基于路标的SLAM。Konolige et al. (1999); Thrun et al. (2000b); Thrun (2001)开发了用于融合扫描的技术。

很多学者已经研究过不同类型传感器的SLAM系统。一种重要的传感器就是相机，到那时，相机只提供了特征的方位信息。这一问题在机器视觉中已经充分研究过了，称为来自运动的结构structure from motion, SFM，参见Tomasi and MOTION Kanade 1992; Soatto and Brockett 1998; Dellaert et al. 2003),以及图像测绘学(Konecny 2002)。Deans and Hebert (2000, 2002)的研究工作开创了只依赖于方位角的SLAM技术。他们的方法迭代的估计环境中与机器人位置无关的环境特征，将位置误差与地图误差解耦。还有很多研究，使用摄像头作为主要传感器进行SLAM(Neira et al. 1997; Cid et al. 2002; Davison 2003)。 Davison (1998)提供了用于SLAM上下文中的主动视觉技术。Dudek and Jegessur’s (2000)的工作依赖于基于外表的空间识别，而Hayet et al. (2002) and Bouguet and Perona (1995)使用视觉路标。 Diebel et al. (2004)开发了一种SLAM滤波器，使用非线性噪声分布的立体距离传感器。Devy and Bulata (1996)开发了用于SLAM的传感器融合技术。 Castellanos et al. (2001)通过实验发现融合了雷达和摄像头的算法比单独使用某一个传感器的效果要好。

SLAM也扩展用于构建密集三维模型问题。早期用于构建室内移动机器人三维模型的工作可以在Reed and Allen (1997); Iocchi et al. (2000); Thrun et al. (2004b).中找到。 Devy and Parra (1998)使用参数曲线获取三维模型。Zhao and Shibasaki (2001), Teller et al. (2001), and Frueh and Zakhor (2003)则开发了一个很赞的系统，来构建大型有纹理的三维城市环境地图。因为这些系统都可以使用户外GPS，所以它们都没有解决完全SLAM问题，但是它们都高度依赖于SLAM的数学基础。这些技术与城市环境航拍技术有着广泛的结合(Jung and Lacroix 2003; Thrun et al. 2003)。

接下来的章节中将要讨论朴素EKF算法的替代方案。那些技术与本章讨论的各种扩展的思想有很多相似之处，因为不同类型的滤波器之间的界限已经很难划分清楚了。在下一章讨论完使用信息论形式的SLAM算法后，将继续我们的文献综述。

10.6 练习

暂略