第八章：移动机器人定位:Grid & Monte Carlo

8.1 介绍

本章介绍两种可以解决全局定位问题的算法。这些算法与单峰的高斯技术相比有很多不同之处。

他们可以处理原始的传感器测量值，不需要从传感器数值中提取特征。作为一种直接的实现，他们也可以处理否定消息negative information。
这些算法是非参的，它们不像EKF定位器那样局限于单峰分布。
他们也可以处理全局定位问题，有些方法还可以解决机器人绑架的问题。EKF算法不能解决这样的问题，不过对MHT(多假设跟踪)进行一些修改也可以解决全局定位问题。

这里所提到的技术已经在很多机器人系统中展现出了优异的特性。

第一种方法称为栅格定位grid localization。它使用直方图滤波器表示后验概率。栅格定位有很多问题，对于一个精细划分的栅格，朴素实现的算法效率很低。而对于粗糙的栅格，离散化操作会丢失一些附加信息，对滤波器有负面的影响，如果处理不当，可能使滤波器不工作。

第二种方法是蒙特卡洛定位(Monte Carlo Localization, MCL)，可以说是目前最流行的定位算法。它使用粒子滤波器来估计机器人的位置及其后验概率。我们也将讨论MCL的各种缺点，用于解决机器人绑架问题的技术，以及在动态环境中的应用。

8.2 栅格定位

8.2.1 基本算法

栅格定位Grid Localization对状态空间进行栅格分解，并使用直方图滤波器来近似表示后验概率。我们已经在算法4.1中讨论了贝叶斯滤波器。它使用一系列的离散概率值来维护后验概率。 $$ \begin{equation}\label{f1} bel(x_t) = \{ p_{k,t} \} \end{equation} $$ 其中$p_{k,t}$表示每个栅格$x_k$的概率。所有栅格的集合构成了一个状态空间的分割: $$ \begin{equation}\label{f2} \mathrm{domain}(X_t) = x_{1,t} \cup x_{2,t} \cup \cdots x_{K,t} \end{equation} $$ 在大多数栅格定位的基础版本中，对空间中所有位置进行分割是时不变的(time-invariant)，而且每个栅格的尺寸都一样。一般室内环境的划分粒度为，在$x$和$y$轴上15厘米，转动纬度上5度。越精细的划分所得到的结果就越好，但会增大计算量。

栅格定位与派生它的基本直方图滤波器在很大程度上是一样的。算法8.1中给出了大多数基本实现的伪代码。它以离散概率值$\{p_{t-1,k}$，以及最近的测量值、控制量和地图为输入。在内层循环中遍历所有的栅格。第3行中实现了运动模型更新，在第4行中进行测量值更新，最后通过归一化算子$\eta$计算概率密度。函数motion_model和measurement_model，可以是第5章和第6章中提到的任何运动方程和测量方程。算法8.1假设所有的栅格都具有相同的尺寸。

算法8.1. 栅格定位，一种离散贝叶斯滤波器的变形。函数motion_model实现了一种运动模型， measurement_model则实现了一个传感器模型。函数"mean"返回了栅格$x_k$的质心(center-of-mass)。

图8.1中描述了在一维的走廊中应用栅格定位的例子。除了栅格定位与生俱来的离散特性之外，它与一般的贝叶斯滤波器一样。和之前的例子一样，机器人一开始不知道自己在全局中的哪个位置上，用一个均匀直方图来表示。接着机器人对环境作出了感知，对应的栅格的概率值就会增加。这个例子凸显了栅格定位算法表示多峰分布的能力。

图 8.1. 细粒度分解的栅格定位。每幅图都描述了机器人在走廊中的位置及其置信度$bel(x_t)$，用对应栅格的直方图来表示。

8.2.2 栅格分辨率

栅格定位器的一个关键变量就是栅格的分辨率。表面上这只是一个微小的细节而以，实际上，可用的传感器模型的类型，更新置信度的计算量，以及期望结果的形式都依赖于栅格分辨率。

有两种极端的表示形式，他们都在实际的机器人系统中得到了成功的应用。

拓扑逻辑topological是一种常用的定义栅格的方法。这种方法得到的栅格都极其粗糙，而且其分辨率要根据环境结构做出改变。拓扑逻辑表示法将状态空间分割为若干个区域，它们分别对应着环境中重要的位置significant places。这样的位置可能由特定路标的存在或者不存在来定义，比如说某处有个门、窗什么的。在走廊的环境中，这些位置可以对应着交叉路口、T型路口，死胡同等。拓扑逻辑表示法通常比较粗糙，而且其对环境的分割依赖于环境的结构。下图8.2中显示了这样对一维走廊的一种粗糙的表示方式。

图 8.2. 应用于移动机器人定位的粗粒度的拓扑逻辑表示。每个状态对应着环境中的一个特定的位置，这里是门的位置。机器人的置信度$bel(x_t)$由圆圈的大小来表示。 (a)所有位置的初始置信度都是一样的。(b)经过一个状态变换机器人监测到了一个门，此时机器人就不太可能仍在最左边的位置上。

度量表示法metric representations是一种常用的更精细的栅格表示。这种方法以较细的粒度将状态空间分割为相同尺寸的栅格。这种分割的分辨率通常比拓扑逻辑栅格要高很多。比如说在第7章中的一些例子，使用15厘米甚至更小的尺寸进行栅格划分。因此，这类方法更准确，但是需要更多的计算资源。图8.3中举例说明了这样的一个固定分辨率的栅格。这种精细的分辨率通常与空间的度量表示形式有关。

图 8.3. 对机器人位姿变量$x,y,\theta$进行固定分辨率栅格划分。每个栅格单元对应着环境中的一个位置。机器人的不同朝向对应着栅格中的不同平面，这里只有三个朝向。

使用粗分辨率的栅格定位时，补偿传感器和运动模型的粗糙分辨率是很重要的。特别是类似激光测距仪这样的高分辨率传感器，测量模型$p(z_t | x_t)$的值在每个栅格单元$x_{k,t}$中变化可能非常大。对于这种情况，只是估计质心的取值，常常会产生很差的结果。类似的，根据质心的运动来预测机器人也会产生很差的结果。对一个以10厘米每秒的速度运动的机器人，如果每秒更新一次，而栅格分辨率是1米，那么朴素的实现将永远不能捕获到状态的改变。这是因为质心运动了接近10厘米的距离后仍然处于相同的栅格中。

一种常用的补偿这种效应的方法就是放大测量模型和运动模型的噪声量。比如说，可以将测距仪的主高斯锥放大为栅格单元直径的一半。这样的新模型就会更平滑，而且它的读数将更不容易被关于正确机器人位置的精确样本点所干扰。但是，这种修改后的测量模型降低了从传感器测量值中提取的信息。

类似的，运动模型可以预测按照一定概率向附近的栅格单元的随机运动，其概率正比于运动弧长与单元半径的比值。放大之后的运动模型，即使连续两次更新之间的运动相比于一个栅格单元而言很小，它也能够描述机器人从一个单元移动到另一单元。但是，因为在机器人的运动过程中不合理地加入了一个大概率，使得机器人运动的比指令快，所得到的后验概率是不合适的。

图8.4中绘制了针对两种距离传感器在不同分辨率下的栅格定位效果。正如我们所期望的那样，随着分辨率降低定位误差不断增加，但是随着栅格变得越来越粗糙，定位机器人所需要的计算量也就越来越少。


(a)	(b)
图 8.4. 不同分辨率下基于超声和激光的栅格定位效果。(a) 关于栅格单元尺寸的平均定位误差;(b)进行全局定位所需的平均CPU时间。

8.2.3 计算量的考量

使用精细粒度的栅格进行定位时，基本的实现往往不能够满足算法的实时需求。运动和测量更新中都存在误差。运动更新需要卷积运算，对于三维的栅格其计算运算过程则是6维的。测量更新则是三维的操作，但是计算一个完整扫描的似然度却是一个花销很大的操作。

有很多用于降低栅格定位的计算复杂度的技术。模型预缓存Model pre-caching致力于解决计算花销大的测量模型。比如说，对于需要ray casting操作的测量模型，可以在任何固定的地图上进行预缓存。正如第6.3.4节中所描述的那样，一种常用的策略就是对每一个栅格单元计算基本统计量，用于辅助测量更新。特别的，当使用波束模型的时候，通常都会为每个栅格单元缓存一个正确的距离。此外，传感器模型也可以对细粒度的距离概率矩阵进行预处理操作。这样测量模型的计算就转换成了更快的查表操作。

传感器二次采样Sensor subsampling通过计算所有距离的一个子集，能够进一步的加速测量模型的计算过程。一些系统中，我们只用8个方位角的距离测量值来代替360度的扫描结果，同样能够取得出色的结果。二次采样技术可以空间上和时间上进行。

延迟运动更新Delayed motion updates应用于壁机器人的控制或者测量频率更低的运动更新操作。这是通过在几何上整合了短时间内的控制量或者里程计读数来实现的。一个好的延迟运动更新技术可以轻易地将算法加速一个数量级。

选择更新selective updating已经在第4.1.4节中描述过了。在更新栅格时，选择更新技术每次只更新一部分栅格单元。一种常见的实现只更新那些后验概率值超过了用户指定的阈值的栅格单元。选择更新技术可以降低更新置信度的计算花销好几个数量级。但是在机器人绑架问题中，需要特别关注低置信度相对较低的栅格单元。

因为这些改进，栅格定位可以很高效，即使是在10年前，低端的PC机都足以得到本章中所展示的结果。但是我们的改进为程序员门增加了额外的负担，使得最后的实现比算法8.1中介绍的算法要更为复杂。

8.2.4 例子

下图8.6中显示了一个使用尺度栅格进行马尔科夫定位的例子，空间和角度分辨率分别为15厘米和5度。图中是一个装备有两个激光雷达的机器人进行全局定位，一开始它对自己的位姿一无所知。我们通过6.3节和算法8.1中的波束模型对距离扫描仪建立概率学模型。


(a)	(c)	(e)

(b)	(d)	(f)
图 8.6. 在一个地图中使用激光雷达数据进行全局定位。(a)机器人在起始位置上的激光扫描示意图，图中略去了读数为雷达的最大量程的扫描波束。 (b)图显示了这次扫描之后，在均匀分布的基础上更新的置信度。(c)第二次扫描(d)测量更新后的置信度。在图(e)中整合了最后一次扫描，从图(f)中看到后验概率集中在真实的机器人附近。

一开始，机器人的置信度均匀的分布在整个位姿空间下。图8.6a中描述了在机器人起始位置上的第一次扫描。其中抛弃了最大距离测量值，与地图相关的部分用灰色标记。融合了这次扫描的数据后，机器人的位置集中在了空间中少数几个区域上，这些区域具有很高的非对称性，如图8.6b的灰色部分所示。需要注意的是这些置信度投影到$x$-$y$空间上，而真实的置信度定义在一个三维的空间中，还有机器人的方向$\theta$，在图中并没有表现。图8.6d中，机器人移动了2米，对应的第二次扫描如图8.6c所示。位置估计的确定度得到了增加，置信度分布的最大值已经对应着机器人的真实位置了。图8.6e中描述了，在置信度中融合了此次扫描数据后，机器人最终从传感器获得的扫描数据。如图8.6f所示，基本上所有的概率密度都集中在了真实的机器人位置上了。直觉上，我们称机器人成功地定位了自己的位置。这个例子说明了栅格定位能够有效的对机器人进行全局定位。

图8.7中显示了第二个例子，其中的环境是局部对称的，这就导致定位过程中对称模式的出现。


(a). 路径以及相关位置	(b). 在位置1上的置信度	(c). 在位置2上的置信度

(d). 在位置3上的置信度	(e). 在位置4上的置信度	(f). 在位置5上的置信度
图 8.7. 在一个办公室环境中使用声纳数据进行全局定位。(a) 机器人的运动路径。(b)在位置1时机器人的置信度。 (c) 机器人运动了几米之后，发现自己在走廊里。(d) 机器人到达了位置3时，声纳传感器扫描到了走廊的尽头，所以其置信度收敛到了两个局部的极大值。其中标记为I的极大值对应着机器人的真实位置，另一个极大值则是因为走廊的对称性而产生的标记为II，它是位置I关于对称中心转动了180度的结果。(e) 机器人通过房间A之后，机器人在真实位置I上的概率就比在位置II上的概率高。(f)最终机器人的置信度集中在了正确的位置上。

显然，全局定位通常不是只需要几次传感器扫描就能够成功的。这是一个在对称环境中进行定位的特例，而且其传感器精度由爱比激光传感器低。图8.8到图8.10中都是用一个只装备有声纳传感器的机器人进行全局定位，而且运行环境中有很多走廊，走廊的宽度基本都一样。图8.8中显示了一个占用栅格地图。

图 8.8. 1994年AAAI移动机器人竞赛场地的占用栅格地图

图8.9a中显示了机器人沿着走廊运动并转到另一个走廊过程中采集的数据集。图8.9a中的每个测量波束都对应着一个声纳测量值。在这种特定的环境下，墙面都是平滑的会产生大量的镜面反射。这里所用的概率模型仍然是6.3节中提到的波束模型。在图8.9中还给出了机器人运动在标记点"A", "B", "C"时的置信度。如图8.9b所示，机器人运动了几米的过程中进行了5此声纳扫描，其置信度基本上均匀的分布在所有的宽度接近的走廊里。几秒钟过后，置信度就如图8.9c所示那样集中在了几个不同的点上。最后随着机器人在拐角处转弯到达了标记点"C"，此时传感器数据就足以唯一的确定机器人的位置了。这个例子中说明了栅格标识法在具有大量噪声的声纳数据和对称环境下仍然能够很好的工作，在整个全局定位过程中都会维护多个位置假设。


(a)	(b)	(c)	(d)
图 8.9. (a)对图8.8所示的环境采集的数据集，包括里程计和声纳距离扫描数据。这些数据足够栅格定位器进行全局定位了。图(b),(c),(d)分别表示标记"A", "B", "C"的置信度。

图8.10中说明了栅格方法通过在占用栅格地图中匹配声纳数据来修正航位推测的累积误差。图8.10a中显示了一个长达240米的轨迹的原始里程计数据。显然里程计的转动误差增长的很快。 40米之后，方向角的累积误差就达到了50度。图8.10b中显示了根据栅格定位器估计的机器人路径。


(a)	(b)
图 8.10. (a) 里程计信息;(b)修正后的机器人路径。

显然，离散化表示的分辨率时栅格马尔科夫定位的关键参数。给定足够的计算和存储资源，精细粒度的方法一般都能够取得比粗粒度更好的效果。正如我们在2.4.4节中提到的，直方图表示方法所产生的系统误差可能打破贝叶斯滤波器的马尔科夫假设。分辨率越精细，这种误差越小，结果越好。细粒度的近似方法也较少出现灾难性的失效catastrophic failures，即机器人的置信度严重偏离了实际情况。

8.3 蒙特卡洛定位

现在我们把目光转向一个流行的定位算法，该算法用粒子的形式表示置信度$bel(x_t)$，称为蒙特卡洛定位Monte Carlo Localization，或者MCL。与基于栅格的马尔科夫定位类似， MCL也可以用于局部定位和全局定位问题。虽然MCL提出的时间比较晚，但是它已经成为机器人学中最流行的定位算法之一。它很容易实现而且在大范围的定位问题中工作的很好。

8.3.1 示例

图8.11中的列举了在一维走廊的环境中使用蒙特卡洛定位的例子。初始的全局不确定度通过对整个位姿空间的均匀的随机抽取的位姿粒子来表示，如图8.11a所示。因为机器人检测到了一个门，MCL就为每一个粒子计算一个重要性因子。图8.11b中显示了更新之后的粒子，图中各粒子的高度表示其重要性因子。有一点很重要需要强调一下，该图中的粒子集合与图8.11a中的是同一个，它们的唯一区别就是测量更新过程更新了重要性因子。

图 8.11. 蒙特卡洛定位, 一种应用于移动机器人定位的粒子滤波器

图8.11c中显示了重采样之后的粒子，它们融合了机器人的运动。这个过程就得到了一个新的粒子集合，每个粒子的重要性因子都是一样的，但是机器人可能存在的位置附近的粒子数量增加了。新的测量为集合中的粒子赋予了不一样的重要性因子，如图8.11d所示。此时，累积的概率密度大部分都集中在了第二个门上，这也正是当前最可能的位置。以后的运动都需要一次重采样操作，和根据运动模生成新的粒子集合的操作，如图8.11e所示。从这个例子中可以明显看出，用粒子集合近似正确的后验概率，可以达到贝叶斯滤波器的计算效果。

8.3.2 MCL算法

算法8.2中列出了基本的MCL算法，该算法用合适的运动模型和感知模型替代了粒子滤波器的概率模型。在基本的MCL算法中用$M$个粒子构成的集合$\mathcal{X}_t = \{ x_t^{[1]}, x_t^{[2]}, \cdots, x_t^{[M]} \}$表示置信度$bel(x_t)$。算法8.2的第4行以当前置信度为起点，对运动模型采样。在第5行中应用测量模型来确定各个粒子的重要性因子。初始的置信度$bel(x_0)$通过对先验概率$p(x_0)$采样所生成的$M$粒子来表示，对于每个粒子赋予相同的重要性因子$M^{-1}$。与栅格定位类似，函数motion_model和measurement_model可以分别由第五章和第六章中提到的任意运动模型和测量模型来实现。使用

算法8.2. 蒙特卡洛定位，一种基于粒子滤波器的定位算法

8.3.3 物理实现

对于第七章中提到的基于路标的定位场景，MCL算法可以直接应用。只需将算法8.2的第四行以算法5.3所提到的sample_motion_model_velocity的形式实现，并以算法6.4中的landmark_model_known_correspondence计算似然度模型，用于更新粒子的重要性因子。

如图8.12所示上述MCL算法的一种实现。其工作场景与图7.15一样。为了方便，在图8.12a中再次画出了机器人的路径和测量值。图8.12c中显示了蒙特卡洛算法生成的样本集合序列。其中实现表示机器人的真实路径，电话线表示基于控制信息生成的路径，虚线则是MCL算法的样本均值所估计的路径。不同时刻的预测样本集合$\bar{\mathcal{X}}_t$用黑色的点表示，重采样后的样本$\mathcal{X}_t$则用浅灰色的点表示。每一个粒子集合都是对三维位姿空间的采样，但是这里的图片都是$x$-$y$坐标系。图8.12b中是从粒子集合中提取的均值和方差信息。


(a)	(b)	(c)
图 8.12. 用于基于路标定位的MCL算法。(a)虚线表示根据运动控制的轨迹，实线是真实的轨迹，路标的检测则通过细线来表示。 (b)重采样前后样本的协方差。(c)重采样前后样本的散点。

图8.13中描述了一个在实际办公环境中应用MCL算法的结果，机器人装有声纳距离扫描阵列。该版本的MCL使用算法6.1来计算测量值的似然度。三幅图分别描述了机器人运动了5米，28米和55米后的粒子分布。


(a)	(b)	(c)
图 8.13. 蒙特卡洛定位的示例。机器人在一个54m×18m的办公环境中运动。所有的计算都是实时的运行在一台低端的PC上的。 (a). 运动了5米之后，机器人仍然不是很确定它在全局中的位置，粒子散布在空闲空间中。 (b). 即使机器人达到了地图的左上角，其置信度仍然集中在四个不同的地方。(c). 最终，运动了大概55米，歧义得到了消解，机器人完全知道了自己的位置。

图8.14给出了第三种实现，在这个例子中机器人用一个摄像头对着天花板，其测量模型是计算图像中心的亮度与预存的天花板地图的关联度。

图 8.14. 使用指向天花板的相机进行全局定位

8.3.4 MCL算法特性

蒙特卡洛定位几乎可以逼近任何实际的分布，并不像EKF定位那样依赖于分布的参数。增加粒子的总数将增加近似的准确度。粒子的数量$M$是一个可以让用户调节的参数，在准确度和计算资源上作出权衡。一种常用的设定$M$的方法就是持续采样，直到下次$u_t$和$z_t$到来。这样的实现能够根据计算资源自适应的调整，处理器越快，定位算法就越好。但是，我们将在第8.3.7节中看到，只有足够多的粒子才能够避免滤波器发散。

MCL还有一个优点，就是其与生俱来的非参数估计。上面提到的这些例子告诉我们，MCL可以表示复杂的多峰概率分布，并能无缝地将它们与集中的高斯类型分布结合。

8.3.5 随机粒子MCL: 从失效中恢复

目前MCL的实现已经可以处理全局定位问题了，但是尚不能够解决机器人绑架的问题，或者说是全局定位失效。从图8.13的结果可以很容易看出，随着定位过程的进行，那些对应着不可能的位置上的粒子逐渐的消失了。在一定程度上，只有在一个位置附近的粒子可以存活下来，如果这个位置碰巧不正确那么算法就失效了。

这个问题很严重。实际上，任何类似MCL这样的随机算法在重采样的过程中，都有可能抛弃掉正确位置附近的粒子。当粒子的数量很小，分布范围很广的时候这一问题尤为突出。

幸运的是，通过简单的启发式规则就可以解决这一问题。我们已经在4.3.4节中讨论过这种启发式规则了，其思想就是在粒子集合中添加随机的粒子。从数学的角度来看，假设机器人以一个很小的概率被绑架，因而产生了一部分随机状态，那么这种注入随机粒子injection of random particles的方法是合理的。即使机器人没有被绑架，加入随机粒子也能提高鲁棒性。

加入随机粒子的方法引入了两个问题。在算法的每次迭代中应该加入多少粒子？我们应该根据那个分布来生成这些粒子？我们可以在每次迭代都加入固定数量的随机粒子。不过更好的思想是根据对定位效果的一些预测来添加粒子。

这种思想的一种实现形式就是监视传感器测量值的概率 $$ \begin{equation}\label{f3} p(z_t | z_{1:t-1}, u_{1:t}, m) \end{equation} $$ 并将之与平均测量概率(可以很容易从数据中学的)关联。在粒子滤波器中，容易从重要性因子中得到这个量的近似。因为根据定义重要性因子就是对这一概率的随机估计。 $$ \begin{equation}\label{f4} \frac{1}{M}\sum_{m=1}^M w_t^{[m]} \approx p(z_t | z_{1:t-1}, u_{1:t}, m) \end{equation} $$ 用时间轴上多次测量结果的平均值来平滑这一估计是一个比较好的方式。除了失效之外还有很多原因导致测量概率值很低，比如说，传感器的噪声高的离谱，在全局定位之后粒子的分布仍然很松散。对于这类因素，一种比较好的处理方式就是维护一个短时间内的测量似然度均值，并在确定随机样本数量的时候将之与长期平均值联系起来。

对于第二个问题，确定使用哪个分布进行采样的方法有两种。一种是针对整个状态空间的均匀分布进行采样，并用当前的观测值为各个样本赋予一个重要性因子。

而有一些传感器模型，可以只接从测量分布中生成粒子。在6.6节中讨论的路标检测模型就是这样一种传感器模型。在这种情况下，可以只接根据观测似然度只接将附加的粒子放置到对应的位置分布中，参见算法6.5。

算法8.3给出了一种添加随机粒子的MCL算法变型。该算法是自适应的，通过对比短期和长期似然度$p(z_t | z_{1:t-1}, u_{1:t}, m)$的均值来确定添加样本的数量。它的第一部分与算法8.2中的基本MCL一样，在第5行中使用运动模型对旧粒子集采样生成新的粒子，并在第6行中通过测量模型为这些粒子赋予重要性因子。

算法8.3. 一种自适应的添加随机样本的MCL算法。通过对比短期和长期的传感器测量值的似然度来确认随机样本的数量。

扩张的MCL算法(Augmented_MCL)在第8行中计算经验测量似然度，并在第10,11行中维护短期和长期似然度均值。该算法要求$0 ≤ \alpha_{slow} ≤ \alpha_{fast}$。参数$\alpha_{slow}$和$\alpha_{fast}$分别是估计长期和短期均值的指数滤波器的衰减速率。该算法的关键在第13行中，在重采样的过程中，以如下的概率加入随机样本: $$ \begin{equation} \label{f5} \max \{ 0.0, 1.0 - w_{fast} / w_{slow} \} \end{equation} $$ 否则我们就以所熟知的方式进行重采样。添加随机样本的概率考虑了短期和长期的测量似然度均值的差异。如果短期似然度不必长期的似然度更差，就不需要新增样本。一旦短期似然度比长期的差，就按照两者之比的商来增加样本。如此一来，当测量似然度突然下降就会新增很多随机样本。这种指数平滑方法可以抵消由于传感器噪声产生的瞬时误差，进而降低了输出很差的定位效果的风险。

图8.16示例了一个实际的扩张的MCL算法。图中是奔跑在3×2米的RoboCup机器人足球赛场上的足式机器人，它用彩色摄像头进行全局定位和重定位。图中的小蝌蚪是定位过程中生成的粒子。蝌蚪的尾巴表示粒子的方向。在场地周围是六个路标点，如图7.7所示。这里使用算法6.4来计算检测的似然度。此外，我们可以很容易用算法6.5来替代算法8.3的第14行，来对最新的测量值进行采样。


(a)	(b)	(c)	(d)

(e)	(f)	(g)	(h)
图 8.16. 随机粒子的蒙特卡洛定位。每幅图都显示了一个粒子集合表示机器人的位置估计(短线表示粒子的方向)。大圆表示粒子的均值，机器人的真实位置由小白圆表示。标记检测由以标记为圆心的圆弧表示，图(a)-(d)为全局定位的示例，图(e)-(h)则是重定位的示例。

图8.16的(a)到(d)图描述了全局定位的过程。一开始粒子基本上是均匀的分布在赛场中，如图(a)所示。第一次检测到左下角的路标，如图(b)所示，重新生成的粒子几乎都是根据这一测量值得到的。此时短期的平均测量概率远小于长期的。几次检测之后，粒子就聚集在真实的位置附近，如图(d)所示，短期和长期的平均测量概率都得到了增加。定位进行到这一刻，机器人就基本上掌握了自己的位置。观测的似然度非常高，偶尔会添加极少的随机粒子。

有时裁判会把机器人挪到另外一个位置上，这在机器人足球锦标赛上是一件稀松平常的事情，就会导致测量概率下降。在新位置，机器人第一次检测到路标时，还不会触发添加大量粒子，因为平滑的$w_{fast}$估计仍然很高，如图(e)所示。在图(f)和(g)中，机器人又检测了几次路标，相比于$w_{slow}$，$w_{fast}$以更快的速度下降，因此添加了一些新的粒子。最后在图(h)中，机器人成功的重新找到了自己的位置。这说明，扩张的MCL算法有能力在被绑架之后生还下来。

8.3.6 修改建议分布

MCL的建议分布机制是另一个使得MCL效率低下的因素。正如我们在4.3.4节中讨论的那样，粒子滤波器使用运动模型作为建议分布，但它企图对该分布与感知似然度的乘积近似。建议分布与目标分布之间地差异越大，就需要越多的粒子。

这一特性会导致MCL产生一个意外的失效方式，如果我们有一个非常好的传感器，不存在测量噪声，每次测量都能够准确无误的报告机器人的位置，MCL反而会失效。对于那些没有携带充分的位置信息的无噪声传感器也存在这种情况。比如说一个一维的无噪声距离传感器，所测量的距离实际上是3维位姿空间中的2维子空间的测量。在4.3.4节中提到过，对运动模型进行采样的时候，进入二维子流形2D-submanifold的概率为0。因此在一些特定的场景下，我们会面临很尴尬的情况，使用MCL定位的时候使用低准确度的传感器反而可以取得更好的效果。EKF定位算法就不会出现这种问题，因为在计算新的均值时EKF需要考虑到测量值，而不是只通过运动模型来更新均值。

幸运的是，有一种简单的补救措施，在测量模型中人为的放大传感器的噪声。我们可以认为这个放大操作不仅是为了适应测量的不确定性，还有粒子滤波器算法本身的近似特性所带来的不确定性。在4.3.4节中，我们还简单提过另外一种更靠谱的方案。其思想就是，反转一部分粒子的运动模型和测量模型的角色，根据测量模型来生成这些粒子: $$ \begin{equation}\label{f6} x_t^{[m]} \sim p(z_t | x_t) \end{equation} $$ 并按照下式计算其重要性因子 $$ \begin{equation}\label{f7} w_t^{[m]} = \int p(x_t^{[m]} | u_t, x_{t-1})bel(x_{t-1})dx_{t-1} \end{equation} $$ 这个新的采样过程是对朴素粒子滤波器的一种合理的替代方式。只是用这一种过程也是低效的，因为它忽略了生成粒子的历史过程。但我们可以用两种机制分别生成一部分粒子，并将这些粒子融合为一个粒子集合，这种算法称为混合建议分布的蒙特卡洛定位MCL with mixture proposal distribution，简记为混合MCLMixture MCL。实际上使用这种新的过程生成一小部分粒子(比如说5%)就足够了。

不幸的是，这种思想不是没有挑战。对$p(z_t | x_t)$进行采样，以及计算重要性因子$w_t^{[m]}$，这两个过程都不是那么容易实现的。只有当测量模型的逆具有容易采样的闭式解的时候，才能轻松地对测量模型采样。但是在大多数情况下都不是这样的，如果我们要对激光距离传感器对整个位姿空间的扫描过程进行采样，计算重要性因子的公式($\ref{f7}$)中的积分项就很难计算，因为$bel(x_{t-1})$本身就是由粒子集表示的。

还没有进行深入的研究，我们发现这两个过程都是可以实现的，但是需要一些额外的近似操作。图8.17中以两个真实世界的数据，对比了朴素MCL、用随机采样进行扩张的MCL(Augmented MCL)、和混合MCL算法。在这两种实验环境下，$p(z_t | x_t)$都是从数据中学习出来的并表示成密度树的形式，这是一个精心设计的过程，对其详细的介绍已经超出了本书的范畴。计算重要性因子的时候，使用随机积分来代替积分操作。为每一个粒子卷积一个窄高斯分布，从而形成一个充满整个空间的连续概率分布，用于表示先验置信度。不在意那些细节，我们可以看到混合MCL算法虽然能产生更好的效果，但是实现起来比较困难。


(a)	(b)
图 8.17. (a)朴素MCL实现(最上面的曲线)、随机样本的MCL实现(中间的曲线)、和混合MCL(最下面的曲线)。以机器人丢失自身位置的时间百分比来计算错误率error rate。图中的数据都是在一个拥挤的博物馆中采集的。(b)使用天花板地图定位的标准CML和混合MCL算法关于时间的误差函数。

从图中，我们也可以看到混合MCL算法提供了一种有效的机器人绑架问题的解决方案。只使用最近的测量值作为粒子采样的种子，我们就可以持续不断的生成在传感器瞬时输入附近的粒子，不用考虑过去的控制量和测量值。很多文献都说这种方法可以很好的解决完全定位失效的问题，图8.17b中描述的正好是朴素MCL算法的这种失效情形，因此混合MCL算法在实际使用过程中更鲁棒。

8.3.7 KLD采样:自适应地调整样本数量

用于表示置信度的粒子集合的大小是影响粒子滤波器计算效率的重要因素。到目前为止，我们只讨论了使用固定大小的样本集合的粒子滤波器。但是为了避免因为MCL的样本损耗而导致的定位发散，我们就需要选择一个很大的样本集合，来让移动机器人进行全局定位并跟踪自己的位置。这是很耗费计算资源的。在图8.13中，整个样本集中有十万个粒子。在定位的早期阶段，维持一个如此大的样本集合是有必要的，一旦机器人确定了自己的位置之后只需要一小部分粒子就足以跟踪机器人的位置了。

KLD采样是MCL定位的一种变形，它根据时间自适应的调整粒子数量。这里我们不提供KLD采样的数学推导，只是陈述算法过程并展示一些实验结果。 KLD采样的全称是库尔贝克-莱布勒散度Kullback-Leibler Divergence，它是一种计算两个概率分布之间差异的方法。KLD采样背后的思想就是根据基于采样近似质量的统计界限来确定粒子数量。更特殊的，在粒子滤波器的每次迭代过程中，KLD采样以概率$1 - \sigma$来确定样本数量，真实的后验概率与基于采样的近似分布之间的差异小于$\varepsilon$。还有几个假设可以让我们有效的实现这一思想，这里先不详细解释这些假设了。

算法8.4中描述了KLD采样算法。该算法以之前的样本集合、地图数据，以及最近的控制量和测量值为输入。相比于MCL算法，KLD采样以一个加权的样本集作为输入。就是说，样本$\mathcal{X}_{t-1}$并不是重采样的。此外，该算法还需要统计误差边界$\varepsilon$和$\sigma$。

算法8.4. KLD采样的MCL算法自适应的调整样本集大小。该算法不断生成样本直到触碰到了近似误差的统计边界了。

KLD采样会不断的生成粒子直到满足了第16行中的统计边界为止。这个边界是对粒子集所要覆盖的状态空间的体积的度量。粒子所覆盖的体积可以通过叠加在状态空间上的直方图或者栅格来测量。直方图$H$中的每个直方要么是空的，要么就被至少一个粒子占用着。一开始，该算法在第4到第6行中将每一个直方都置为空。在第8行中，从上一个样本集合中采样生成一个粒子。在第9-11行中，基于这个粒子，预测并计算一个新的粒子的权重，然后将之加入新的样本集中。

第12到19行是KLD采样的核心。如果新生成的粒子落在了一个空的直方中，那么就增加非空直方计数$k$，并将对应的直方置为非空。如此，$k$就测量了至少有一个粒子的直方数量。它是第16行中计算统计边界的主角。数量$M_{\chi}$给出了达到边界还需粒子的数量。给定$\varepsilon$，$M_{\chi}$基本上是$k$的线性函数，此外随着$k$增加，与之相关的非线性项可以忽略不计。项$z_{1-\sigma}$基于参数$\sigma$，它表示标准正态分布的$1-\sigma$的上分位点。给定$\sigma$，$z_{1-\sigma}$的取值可以通过查询统计表得到。

该算法不断地生成粒子，直到粒子数量$M$超过了$M_{\chi}$或者用户定义的上限$M_{\chi_{min}}$。可以看出$M_{\chi}$是$M$的一个运动目标(moving target)。生成越多的样本，就有更多的直方非空，阈值$M_{\chi}$就越高。

实际上，该算法是由于如下的原因才停止的。在采样的早期阶段，基本上每生成一个新样本$k$都要增加，因为此时基本上所有的直方都是空的。$k$的增加将导致阈值$M_{\chi}$上升。但是，随着时间的推移，越来越多的直方都是非空的了，$M_{\chi}$偶尔才会增加一次。而每次产生新样本的时候$M$都会增加的，最终$M$会达到阈值$M_{\chi}$算法停止。计算置信度的时候，粒子分布越分散，就填充了越多的直方，因而需要更大的阈值$M_{\chi}$。在跟踪定位过程中，KLD采样会产生较少的粒子，这是因为粒子集中在少数几个不同的直方中。我们需要注意到，直方图对于粒子的分布并没有什么影响。它的唯一目的就是测量置信度的复杂度，或者说是状态空间的体积。在每次迭代之后这个直方图或者栅格就会被抛弃掉。

图8.18给出了使用KLD采样进行全局定位过程中所用的样本数量。图中的实线时使用激光传感器进行定位的粒子数量曲线，虚线则是超声传感器的曲线。在这两种情况下，一开始使用的粒子数量都很大。一旦确定了机器人的位置，粒子的数量就急速的下降了，此时的粒子数量还不到一开始的1%。从全局定位到位置跟踪转变的时机和速度依赖于环境的类型和实际使用的传感器。这里高精度的激光传感器就更早的进行了切换。

图8.18. KLD采样: 随着时间的推移，全局定位算法所需的样本数量。样本数量以对数的形式表示。实线是使用激光距离扫描仪进行定位时所用的样本数量曲线，虚线则是声纳传感器的曲线。

图8.19中对比了KLD采样和固定样本数量的MCL定位算法近似误差。近似误差通过不同数量的样本生成的置信度与最优的置信度之间的KL距离表示。这个最优的置信度是在一个数量为200,000的样本集上进行MCL定位所生成的置信度，它远比实际进行位置估计所需要的粒子数量高。正如我们所预期的那样，这两种方法所用的样本数量越多，近似误差就越小。图中的虚线是使用不同固定数量的样本进行MCL定位的结果。可以看到当样本数量超过了50,000的时候，KL距离就小于了0.25。更大的误差表明粒子的分布更分散，机器人还没有成功定位。图中的实线是使用KLD采样进行定位的结果。其中样本集的数量是进行全局定位时的平均值，这些不同的数据点是通过从0.4到0.015调整误差边界$\varepsilon$得到的，曲线从左向右下降。平均使用30,000个样本时KLD采样的误差就已经很小了。由于误差边界过宽，图中左边的实线说明KLD采样没有成功的定位。这说明KLD采样不能够保证可以精确的跟踪最优的置信度。

图8.19. KLD采样与固定样本数量的MCL定位的对比。$x$轴表示平均样本数量，$y$轴是参考置信度与样本集所生成的分布之间的KL距离。

KLD采样可以用于任何粒子滤波器，而不仅仅是MCL定位。直方图的实现可以时固定的、多维度栅格，也可以是更简洁的树形结构。在机器人定位的上下文中，KLD采样已经表现出了优于固定样本数量的MCL算法的效果。这种算法的优势就是巧妙地将全局定位问题和位置跟踪问题结合起来了。实际上误差边界$(1-\sigma)$取0.99，$\varepsilon$取0.05、栅格分辨率为$50cm × 50cm × 15deg$就可以取得很好的结果。

8.4 在动态环境中定位

到目前为止，我们所讨论的所有定位算法都是在静态环境中进行的，存在马尔科夫假设。我们对有很多人的环境更有兴趣，但是在这种环境中有很多动态特性没有在状态向量$x_t$中建模。在一定程度上，概率学的方法对于这些未建模的动态特性时鲁棒的，因为它们具有兼容传感器噪声的能力。但是正如我们先前所提到的那样，对传感器噪声的兼容特性，是建立在概率学滤波框架在各个时间段上都是独立的这一假设上的。但是未建模的动态特性在多个时间段上都会影响传感器的测量值。当这种效应很明显的时候，建立在静态环境假设上的概率学定位算法就会失效。

如图8.20所示，是一个很好的说明这种失效情形的例子。在德国的波恩博物馆中有一个轮式的导游机器人，需要在一个挤满了人的房间里穿梭。这些人的位置、速度、意图等等，都是与定位算法相关的隐藏状态，但是我们目前所讨论的算法都还不能够捕获它们。那么这个问题有多麻烦呢？假设人们像一堵墙一样排成一排站在机器人面前，机器人的每一次测量都会增加状态"它在一堵墙旁边"的置信度。因为机器人假设每次采集的信息都是独立的，最终机器人会在真实的墙附近赋予较高的似然度。对于独立的传感器噪声也可能出现这种效应，但是其似然度就小了很多。


(a)	(b)
图8.20. 德国波恩博物馆中的移动机器人Rhino经常被很多人围观


(a)	(b)
图8.21. 激光传感器的扫描光束经常被周围的人类挡住。此时应该如何保持正确的定位呢?

有两种处理动态环境的基本方法。第一种就是状态扩张技术state augmentation，将隐藏的状态添加到滤波器的状态估计中。另一种是剔除异常值技术，对传感器测量值进行预处理，从中移除受隐藏状态影响的测量值。数学上，第一种方法是一种更一般的形式，不仅仅要估计机器人的位置，我们还可以得到估计人的位置和速度的滤波器。我们将在后续内容中，以移动机器人建图算法的扩展形式来讨论这个方法。

估计隐藏状态的关键劣势就是其计算复杂度。此时机器人不仅仅是要估计3个变量，还需要计算大量变量的后验概率。实际上，变量的数量本身也是一个变量，因为环境中的人员总是流动的。因此，算法将变得比目前讨论过的定位算法都更为复杂。

另一种方法提出异常值，在一些特定的场景下工作的很好。这些场景也包括因为人的活动而影响了距离传感器的测量值或者是摄像机图像。我们在6.3节中讨论的波束模型的基础上开发这一技术。

其思想就是调查研究造成传感器测量值的原因，并将可能被未建模的环境动态特性所影响的测量值从中移除。目前讨论的传感器模型都是在研究产生各种不同测量值的方法。如果我们能够以某种方式找到不期望的动态效应以及测量值之间的关系，那么剩下的就是抛弃那些由于未建模的特性而产生的很高似然度的测量值。

这种方法比我们想象中的要更为通用，实际上，其数学原理基本上与6.3节中的EM学习算法EM Learning Algorithm类似，只是应用在在线的形式上。在公式6.12中，我们将距离传感器的基于波束的测量模型定义为如下的四个部分的线性组合: $$ \begin{equation}\label{f8} p(z_t^k | x_t, m) = \begin{pmatrix} z_{hit} \\ z_{short} \\ z_{max} \\ z_{rand} \end{pmatrix}^T \cdot \begin{pmatrix} p_{hit}(z_t^k | x_t, m) \\ p_{short}(z_t^k | x_t, m) \\ p_{max}(z_t^k | x_t, m) \\ p_{rand}(z_t^k | x_t, m) \end{pmatrix} \end{equation} $$ 正如我们在推导这一模型时提到的那样，其中的$z_{short}, p_{short}$对应着非预期的物体。为了计算与非预期的物体相关的测量值$z_t^k$的概率，我们还需要引入一个新的关联度变量$\bar{c}_t^k$来表示四项$\{ \mathrm{hit}, \mathrm{short}, \mathrm{max}, \mathrm{rand} \}$中的一项。

关联于一个"short"读数的距离测量值$z_t^k$的后验概率可以通过贝叶斯公式和丢弃不相关的变量来得到。 $$ \begin{align} \label{f9} p(\bar{c}_t^k = short | z_t^k, z_{1:t-1}, u_{1:t}, m) & = \frac{p(z_t^k | \bar{c}_t^k = short, z_{1:t-1}, u_{1:t}, m)p(\bar{c}_t^k = short | z_{1:t-1}, u_{1:t}, m)} {\sum_c p(z_t^k | \bar{c}_t^k = c, z_{1:t-1}, u_{1:t}, m)p(\bar{c}_t^k = c | z_{1:t-1}, u_{1:t}, m)} \\ & = \frac{p(z_t^k | \bar{c}_t^k = short, z_{1:t-1}, u_{1:t}, m)p(\bar{c}_t^k = short)} {\sum_c p(z_t^k | \bar{c}_t^k = c, z_{1:t-1}, u_{1:t}, m)p(\bar{c}_t^k = c)} \end{align} $$ 其中变量$c$表示$\{\mathrm{hit}, \mathrm{short}, \mathrm{max}, \mathrm{rand} \}$四个之中的任意一个。在公式($\ref{f8}$)中使用这种符号表示，先验概率$p(\bar{c}_t^k = c)$分别是变量$z_{hit}, z_{short}, z_{max}, z_{rand}$。公式($\ref{f9}$)中剩余的概率通过对$x_t$积分得到: $$ \begin{align} \label{f10} p(z_t^k | \bar{c}_t^k = c, z_{1:t-1}, u_{1:t}, m) & = \int p(z_t^k | x_t, \bar{c}_t^k = c, z_{1:t-1}, u_{1:t}, m) p(x_t | \bar{c}_t^k = c, z_{1:t-1}, u_{1:t}, m) dx_t \\ & = \int p(z_t^k | x_t, \bar{c}_t^k = c, m) p(x_t | z_{1:t-1}, u_{1:t}, m) dx_t \\ & = \int p(z_t^k | x_t, \bar{c}_t^k = c, m) \overline{bel}(x_t) dx_t \end{align} $$ 按照6.3节中符号，将概率$p(z_t^k | x_t, \bar{c}_t^k = c, m)$简记为$p_{hit}, p_{short}, p_{max}, p_{rand}$。这样公式($\ref{f9}$)可以化简为: $$ \begin{equation} \label{f11} p(\bar{c}_t^k = short | z_t^k, z_{1:t-1}, u_{1:t}, m) = \frac{\int p_{short}(z_t^k | x_t, m) z_{short} \overline{bel}(x_t) dx_t} {\int \sum_c p_c(z_t^k | x_t, m) z_c \overline{bel}(x_t) dx_t} \end{equation} $$ 一般，公式($\ref{f11}$)中的积分没有闭式的解。为了计算他们，状态$x_t$上的后验概率$\overline{bel}(x_t)$的代表性样本就足以对其作出近似。这些样本可能是栅格定位器中具有高似然度的栅格单元，或者是MCL算法中的粒子。如果判定测量值是由非预期的障碍物引起的概率超过了用户选择的阈值$\chi$就拒绝该测量值。

算法8.5描述了这种技术在粒子滤波器中的一种实现。它以一个用来表示置信度$\overline{bel}(x_t)$的粒子集合$\bar{\mathcal{X}}_t$，距离测量值$z_t^k$和地图作为输入。如果测量值对应于未期望的物体的概率值大于$\chi$就返回"拒绝",否则就返回"接受"。这个过程在MCL中的测量值积分之前进行。

算法8.5. 在动态环境中测试距离测量值的算法

图8.22示例了这种滤波器的效果。这两个子图分别描述了机器人在不同位置上的距离扫描情形。浅灰色表示因为超过了阈值而拒绝的测量值。这种拒绝机制的关键特性就是它能够滤除意外的短测量值，而保留其他位置上的意外的长值。这种不对称性反映的是人类的存在总是使得传感器采集到比期望的距离更短的测量值。通过接受那些意外的长值，这种方法可以从全局定位失效中恢复过来。


(a)	(b)
图8.22. 测量拒绝算法的示例。图中浅色的读数是拒绝的测量值。

图8.23中描述的机器人在一个挤满了人的环境中进行导航的过程。机器人根据融合到定位器中的扫描端点来估计路径。该图中还显示了移除那些不对应于地图中物体的测量值的定位算法的效果。如图(b)所示在空闲区域中只有很少一部分距离测量值通过了阈值检测并存活下来。


(a) 朴素MCL定位	(b) 异常点移除MCL定位
图8.23. 朴素MCL定位算法V.S.移除异常点的MCL算法。两幅图都显示了机器人的路径和用于定位的扫描端点。

根据经验来说，剔除测量值中的和异常点通常是一个很好的想法。几乎不存在静止的环境，即使是在办公室的环境中，家具都是可以移动的，门都是可以开关的。这里的这种特殊实现方式得益于距离测量的非对称性，人类的存在总是使距离测量值变短，而不是变长。将这种思想应用于其他数据中(比如说，视觉数据)，或者其他类型的环境变化方式(比如说，移除了障碍物)，非对称性就可能不存在了。尽管如此，我们还是可以进行相同的概率学分析的。缺少这种非对称性的缺点可能是我们将无法从全局定位失效中恢复过来，因为所有的测量值都被当作异常给扔了。在这种情况下，我们可以引入一些额外的约束，比如说对可能被打断的测量值部分加上限制。

我们注意到拒绝测试即使是在高度静止的环境中也能够成功应用(译者按:我是觉得环境越是静态的就越容易定位呀?这不足为奇呀，不知道原文中的那个even到底想表达什么)，其原因也很微妙。基于波束的传感器模型是非连续的，位置的微小改变都会造成传感器测量值的后验概率的戏剧性的变化。这是因为ray casting的结果不是一个关于机器人方向这样的位置参数的函数。在有很多物体的环境中，这种非连续性增加了成功定位所需要的粒子数量。如果手动的从地图中移除杂乱的物体，而不是让滤波器来管理意外的短测量值，那么粒子的数量将急剧下降。相同的策略不能用于似然场模型，因为这个模型在位置参数上是平滑的。

8.5 一些实际的考量

下表8.6中总结和对比了本章和上一章中提到的主要的定位算法。选择一个技术的时候，需要对很多因素作出权衡。我们所面临的第一个问题总是，从传感器测量值中提取特征是否更好。提取特征可能在计算方面受益了，但是它降低了准确性付出了鲁棒性的代价。

表8.6. 马尔科夫定位的不同实现的对比

在本章中，我们在MCL算法的上下文中讨论了处理动态环境的技术，类似的思想也可以用于其他定位技术上。实际上，这里所讨论的技术只是沧海中的一叶。

在实现定位算法的时候，玩一玩不同的参数设定是值得的。比如说，融合了附近的测量值之后条件概率总会膨胀，可以用来调整机器人学中普遍存在的未建模的依赖。一个好的策略就是选择相关的数据集，并调整算法直到总体的结果满意为止。这是很必要的，因为无论数学模型是多么复杂，总是有一些未建模的依赖项，以及一些对称的噪声源影响着总体的结果。

8.6 总结

在本章中，我们讨论了两种概率学定位算法，栅格技术和蒙特卡洛定位(MCL)。

栅格技术通过直方图表示后验概率。
栅格的粗细粒度通常是在准确度和计算效率之间权衡的筹码。对于粗栅格，通常需要调整传感器和运动模型，来考虑粗略的表达形式所带来的影响。对于细粒度的栅格，可能需要有选择的更新栅格单元，来降低总体的计算量。
蒙特卡洛定位算法用粒子来表达后验概率。粒子的数量是准确度V.S.计算量的战场。
栅格定位方法和MCL都可以全局地定位机器人。
通过添加随机粒子，MCL也可以用于解决机器人绑架问题。
混合MCL是一种蒙特卡洛定位的扩展，对所有粒子的一部分反转了粒子生成过程。它提高了定位效果，尤其是在机器人装备了低噪声传感器的情况下，但是实现起来比较复杂。
KLD采样能够自适应的调整样本的数量，增加了粒子滤波器的效率。如果置信度的复杂程度随着时间发生了戏剧性的变化，那么KLD采样的优势就充分体现出来了。
未建模场景的动态特性可以通过过滤传感器数据，拒绝那些大概率的对未建模物体的测量数据来解决。当时用距离传感器的时候，机器人倾向于拒绝那些意外的短数据。

MCL算法的流行可能因为两个原因，它是定位算法中最容易实现的一种，并且是最有效的一种算法，它可以对几乎所有的分布作出近似。

8.7 参考文献

Simmons和Koenig在Nourbakhsh et al. (1995)提出的维护确定度因子的方法的基础上介绍了基于栅格的蒙特卡洛定位。自从Simmons and Koenig’s (1995)的开创性论文的发表，涌现出了大量的技术来维护用于定位的直方图(Kaelbling et al. 1996)。最初的工作都是用相对较粗的栅格来降低更新栅格时的巨大计算量，Burgard et al. (1996)引入的选择更新技术可以处理更高分辨率的栅格。这通常被视为从粗糙的拓扑逻辑的马尔科夫定位转向细致的度量定位的标志。Koenig and Simmons (1998); Fox et al. (1999c)对这些研究工作做了综述。

基于栅格的技术被认为是移动机器人定位的最新技术已经有很多年了。有很多基于栅格的马尔科夫定位算法的成功案例。比如Hertzberg and Kirchner (1996)将这一技术用于污水管道机器人， Simmons et al. (2000b)用这种方法在办公室环境和定位，Burgard et al. (1999a)将之用于估计机器人在博物馆中的位置。Konolige and Chou (1999)将地图匹配的思想引入到马尔科夫定位中，通过使用快速卷积技术来计算位置概率。Burgard et al. (1998)描述了一种组合全局定位和高精度跟踪的扩展算法，将之称为动态马尔科夫定位dynamic Markov localization。 Oore et al. (1997)介绍了使用机器人学习的技术来学习识别位置。Thrun (1998a)通过学习组件来确定环境中合适的路标，该工作是基于Greiner and Isukapalli (1994)的相关工作的。 Mahadevan and Khaleeli (1999)对数学框架作出了扩展提出了半马尔科夫决策过程semi-Markov decision process，有可能推导出从一个单元转移到另一个单元的确切时间。 Gutmann et al. (1998)通过饰演对比了栅格方法和卡尔曼滤波技术。Burgard et al. (1997)介绍了在基于栅格的范式中进行主动定位的方法。后来Austin and Jensfelt (2000); Jensfelt and Christensen (2001a)将之扩展到了多假设跟踪问题中。Fox et al. (2000) and Howard et al. (2003)将这种方法扩展到了多机器人定位问题中。 Jensfelt and Christensen (2001a); Roumeliotis and Bekey (2000); Reuter (2000)从基于栅格的范式中离开，展示了同样能够进行全局定位的多假设扩展卡尔曼滤波器。

受机器视觉中著名的压缩算法condensation algorithm的启发，Dellaert et al. (1999); Fox et al. (1999a)首先开发了用于移动机器人定位的粒子滤波器。他们也称之为蒙特卡洛定位，目前已经是机器人学中类似技术的通称。添加随机样本的思想可以在文献Fox et al. (1999a)中找到。 Lenser and Veloso’s (2000)提出了一种改进的处理机器人绑架问题的传感器重置sensor resetting技术，一部分粒子只是用最新的测量来重启jump-started。 Fox以此技术为基础提出了扩张的MCL算法确定添加的粒子数量(Gutmann and Fox 2002)。混合MCL算法归功于Thrun et al. (2000c);，也可以参考van der Merwe et al. (2001)。它提供了从测量值中生成样本的数据基础。KLD采样作为粒子滤波器的自适应版本，由Fox (2003)提出。Jensfelt et al. (2000) and Jensfelt and Christensen (2001b)将MCL应用于基于特征的地图中， Kwok et al. (2004)提出了实时版本的MCL来调整粒子的数量。最后，有很多论文介绍在装备摄像头的机器人上引用MCL算法(Lenser and Veloso 2000; Schulz and Fox 2004; Wolf et al. 2005)，包括全向摄像头(Kröse et al. 2002; Vlassis et al. 2002)。

粒子滤波器也用于很多跟踪和定位问题中。Montemerlo et al. (2002b)研究了使用嵌套的粒子滤波器来同时定位并跟踪人的问题。Schulz et al. (2001b)描述了跟踪可变人数的粒子滤波器，展示了移动的机器人在未知的环境中如何可靠的跟踪人类(Schulz et al. 2001a)。

8.8 练习

暂略。