第七章：移动机器人定位:Markov & Gassian
本章介绍用于移动机器人定位的各种具体算法。移动机器人定位是在给定地图的环境中确定机器人的位置的问题，有时称之为位置估计position estimation，是一般定位问题的具体案例， 也是机器人学中最基本的感知问题。几乎所有的机器人任务都需要知道操作对象的位置。本章和下章讲述用于物体定位任务的技术。
图7.1用一个图模型中描述了移动机器人定位问题，给定运行环境的地图，根据机器人对环境的感知和运动过程，确定其相对于地图的位置。
 |
| 图 7.1. 机器人定位的图模型。其中\(m\)为地图，\(z\)为测量值，\(u\)为控制量，带有阴影的节点的值是已知的。 定位的目标是推导出机器人的位置变量\(x\)。 |
移动机器人定位可以看作是一个坐标变换问题。地图可以描述为一个全局的坐标系，与机器人的位置无关。定位过程就是建立地图坐标系与机器人局部坐标系之间的关系。 这个坐标变换使得机器人能够以自己的坐标系来描述感兴趣的目标点位置，这是机器人导航的必备条件。很容易验证，如果位姿表示在一个与地图相同的坐标系下， 只要知道了机器人的位姿\(x_t = \begin{pmatrix} x & y & \theta \end{pmatrix}^T\)我们就能够进行这一坐标变换。
很不幸，通常不能直接测量机器人的位姿。大多数机器人都没有用于测量位姿的无噪声传感器，因此必须从数据中推导出位姿信息。 这就引出了一个核心问题，单次测量通常不足以确认位姿，机器人就不得不通过整合长时间的数据来实现。我们可以假想一种情况，机器人在一个建筑里， 这个建筑的很多走廊都是相似的。在这种场景下一次测量通常不足以确定机器人到底在哪个走廊里。
针对各种不同的地图表示方法，人们开发了很多定位技术。我们已经讨论了基于特征的feature-based和基于位置的location-based两种地图。 后者的一个典型例子就是占用栅格地图，将在本书的后续章节中讨论。图7.2中还示意了一些其它类型的地图，比如手绘的2D测量地图、类图的拓扑地图、占用栅格地图、 天花板马赛克图片(它也可以用作地图)。后续的章节中将研究各种类型的地图，并讨论从数据中获取地图的算法。 定位算法总是假设存在一个准确的地图。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.2. 机器人定位的地图示例。(a)一个手动构建的2D测量图, (b)一个类图的拓扑地图, (c)一个占用格点地图, (d)一个天花板马赛克图片。 | |||
本章和下一章中，我们将讨论一些用于移动定位的基本概率算法。 所有的这些算法都是第2章中基本的贝叶斯滤波器的变形。 对于本书中列出的每一种算法，我们都会讨论它的优缺点。本章也将根据如下的分类方法，讨论针对不同类型定位问题的算法扩展。
7.1 定位问题的分类
并不是所有定位问题的难度都是相同的。为了了解定位问题的难度，我们先根据机器人所具有的初始知识，以及环境的自然属性等重要的维度，对定位问题进行分类。
局部定位VS全局定位:我们根据机器人所具有的初始知识，以及在运行时可以获得的知识，将定位问题从简单到困难划分为如下三类:
位置跟踪Position tracking假设机器人的初始位置是已知的。定位需要对机器人运动过程中的噪声具有一定的容忍度，而这种噪声通常比较小。 因此，位置跟踪方法通常依赖于位置误差较小这一假设。位置的不确定性通常近似于一个单峰的分布(比如说高斯分布)。位置跟踪问题是一个局部local的问题， 因为不确定性是局部的，并且集中在在机器人真实位置附近。
全局定位Global localization问题中，机器人的初始位置是未知的。机器人一开始在环境中的某个位置上，但是它并不知道自己在哪里。 全局定位算法不能假设位置误差是有界的。在本章中我们会看到，单峰分布通常不适用。因此，全局定位比位置跟踪问题更难；实际上它包含了位置跟踪问题。
机器人绑架问题kidnapped robot problem是全局定位问题的一个变形，但是更难。在操作过程中，机器人可能被绑架并运送到其他地方。 机器人绑架问题之所以比全局定位问题更难，是因为机器人可能仍然认为它还在原来的地方。而在全局定位问题中，机器人知道它并不清楚自己的位置。 有人可能认为实际的机器人很少能够被绑架。但是这个问题具有很重要的实际意义，因为目前大多数定位算法都不能确保不会失效。 从失效状态恢复过来是真正自主机器人的基本能力，我们可以通过绑架它来测试机器人的这种能力。
静态环境VS动态环境:影响定位方法难度的第二个维度就是环境。环境可以是静态的，也可能是动态的。
所谓的静态环境static environment是指只有机器人的位置状态可以变动的环境。换句话说，只有机器人在静态环境中运动，环境中所有其他障碍物始终保持原位。 静态环境具有一些很好的数学属性，在有效的概率估计算法面前显得很温顺。
而动态环境dynamic environment中除了机器人之外，还有其他物体的位置或者状态随着时间在变动。特别需要关注那些随着时间的推移一直在变化的属性， 以及那些不仅仅对一个传感器读数有影响的状态。因为那些不能测量的变化自然与定位无关， 而只影响了一个测量值的状态一般会被认定为噪声(参考2.4.4节)。 一些总是变动的因素包括：人类，光线(对于装备了摄像头的机器人而言)，可移动的家具，或者门。显然大多数真实的环境都是动态的，只是状态变动的范围各不相同而已。
显然，在动态环境中定位是一个比在静态环境中更难的一个问题。有两种基本的方法来处理动态环境。其一，可以把动态的属性添加到状态向量中。 这样马尔科夫假设Markov assumption就可能适用了，但是这种方法会给计算带来额外的负担而且模型的复杂度也要提升。其二，在有些情况下还可以对传感器数据进行滤波， 这样能够削弱未建模的动态特性带来的负面影响。这样的方法将在第8.4节中介绍。
被动方法VS主动方法:划分定位问题的第三个维度，就是定位算法是否控制机器人的运动，我们据此将之分为被动方法和主动方法两类。
在被动定位过程中，定位模块只是在观察observes机器人的操作。通过其它的一些方法控制机器人，而机器人的运动也不是用来辅助定位的。比如说机器人可能随机的运动， 或者执行每日的任务。
主动定位算法控制机器人来最小化定位误差，或者降低定位很差的机器人在危险空间中的风险。它通常能够得到比被动方法更好的定位效果。 在本书的介绍中，我们已经讨论了一个例子——coastal navigation。图7.3中则示例了第二个例子。 图中机器人在一个对称的走廊里，一段时间之后它的置信度收敛到了两个对称的位置上。走廊的对称特性使得机器人不能完全确定自己的位置。 它只能通过走进一间屋子来消除歧义，进而明确具体位置。在这样的场景中，主动定位方法会给出更好的结果。与其等待机器人偶然间进入一个房间，不如识别出这种僵局并打破之。
 |
| 图 7.3. 这个例子中描述了在一个局部对称环境中进行全局定位中的一个典型置信度。为了确定机器人的确切位置，它需要先进入一个房间才行。 |
主动定位方法的一个关键问题是，需要控制机器人。因此在很多场景下只有主动定位算法是不够的，机器人在定位自己的同时还需要执行定位之外的其他任务。 一些主动定位技术是建立在被动技术的基础之上的。其他的一些方法则在控制机器人的过程中将任务目标和定位目标组合起来。本章只讨论被动定位算法， 我们将在第17章中探讨主动定位方法。
单个机器人VS多个机器人:定位问题的第四个维度就是机器人的数量。
单个机器人定位single-robot localization是目前研究最多的定位问题。它只处理单个机器人的定位问题。在这一问题中所有的数据都来自于一个机器人平台， 不存在通信问题。
多个机器人定位multi-robot localization研究的是机器人编队的问题。乍一眼看来，每个机器人都可以独立的定位自己的位置，因此多机器人定位问题可以转化为单个机器人定位问题来解决。 如果机器人能够互相探测，我们有可能获得更好的效果。因为，一旦我们获知了两个机器人之间的相对位置关系，那么我们就可以用一个机器人的置信度来调整另一个机器人的置信度。 多机器人定位带来了一些表达置信度的有趣而非凡的问题，而且机器人之间需要通信交互。
这四个维度涵盖了移动机器人定位问题的四个最重要的特点。还有其他影响问题复杂度的因素，比如说机器人测量值所提供的信息，移动过程中丢失的信息。 此外，对称的环境要比非对称的环境更难定位，因为它具有较高的歧义性。
7.2 马尔可夫定位
基于概率的定位算法都是贝叶斯滤波器的变型。将贝叶斯滤波器直接应用到定位问题上的方法就是马尔可夫定位Markov localization。算法7.1描述了其基本过程， 它是从算法2.1中描述的Bayes_filter变型而来的。 注意Markov_localization还需要地图\(m\)作为输入， 地图将在第4行中的测量模型\(p(z_t | x_t, m)\)中起作用。在第3行中的运动模型\(p(x_t | u_t, x_{t-1}, m)\)通常不需要地图信息，但并不总是这样的。与贝叶斯滤波器一样， 马尔可夫定位在\(t-1\)时刻的置信度的基础上计算\(t\)时刻的置信度。马尔可夫定位可以解决在静态环境下的全局定位问题、位置跟踪问题、和机器人绑架问题。
 |
| 算法7.1 马尔可夫定位 |
初始置信度\(bel(x_0)\)反映了机器人所具有的初始信息，在不同类型的定位问题中取值不同。
位置跟踪:如果知道初始位置，就用一个质点分布point-mass distribution来初始化\(bel(x_0)\)。令\(\bar{x}_0\)表示已知的初始位置，有 $$ \begin{equation}\label{f1} bel(x_0) = \begin{cases} 1 & if\ x_0 = \bar{x}_0 \\ 0 & otherwise \end{cases} \end{equation} $$ 质点分布是一个离散分布，因此不存在概率密度。
实际上初始位姿通常是近似的。置信度\(bel(x_0)\)通常初始化为一个狭窄的高斯分布，分布中心为\(\bar{x}_0\)，如下： $$ \begin{equation} \begin{matrix} bel(x_0) & = & \underbrace{\det(2\pi\Sigma)^{-\frac{1}{2}} \exp \left\{ -\frac{1}{2}(x_0 - \bar{x}_0)^T \Sigma^{-1} (x_0 - \bar{x}_0) \right\}} \\ & & \sim \mathcal{N}(x_0;\bar{x}_0, \Sigma) \end{matrix} \end{equation} $$ 其中\(\Sigma\)是初始位置的协方差矩阵，用于描述其不确定度。
全局定位:如果不知道初始位姿，可以用一个均匀分布来初始化\(bel(x_0)\)，表示机器人可能在地图上的所有合法的区域上。 $$ \begin{equation} bel(x_0) = \frac{1}{|X|} \end{equation} $$ 其中\(|X|\)表示空间中所有可能位置的体积(Lebesgue measure)。
其他:通常可以很容易通过机器人位置的部分信息转化为一个合适的初始分布。比如说，已知机器人是从门旁边开始运动的，并且可以确定是在走廊里， 但并不清楚具体在门旁边的哪个位置上，可以在走廊的区域使用均匀分布，同时将其他区域的概率设定为0。
7.3 马尔可夫定位实例
在本书的介绍中，我们已经讨论过马尔可夫定位了，并用它举例说明了概率学机器人的研究动机。 现在我们以具体的数学框架再来看一下这个例子。图7.5a示意了一个有3个门的一维走廊，其初始的置信度\(bel(x_0)\)是对所有位置的均匀分布。
 |
| 图 7.5a. 移动机器人定位的示例环境，初始置信度为对所有位置的均匀分布 |
 |
| 图 7.5b. 乘上测量概率\(p(z_t | x_t, m)\)后的置信度\(bel(x)\)更新 |
如图7.5c所示，随着机器人向右移动，马尔可夫定位在其第3行中，对机器人位置置信度和运动模型\(p(x_t | u_t, x_{t-1})\)做卷积运算。 运动模型\(p(x_t | u_t, x_{t-1})\)并不关注某个特定位姿，而是考察无噪声运动后的，以预期位姿为中心的连续位姿空间。 结果如图7.5c所示，因为卷积运算的存在，置信度在平移的同时也变得越来越平滑。
 |
| 图 7.5c. 随着机器人的运动，置信度的峰值在平移的同时也在变平 |
最终的测量如图7.5d所示。此时马尔可夫定位算法将当前的置信度与感知概率\(p(z_t | x_t)\)相乘，那么最大概率密度就聚焦到了当前的位置上， 机器人就很确信自己的位置了。
 |
| 图 7.5d. 再次乘上测量概率后，因为获得了更多信息，确认了机器人的位置 |
下图7.5e显示了机器人继续运动下去的置信度分布图形。
 |
| 图 7.5e. 随着机器人继续运动，分布更趋于平缓，但峰值仍然是存在的 |
我们已经注意到马尔可夫定位与背后状态空间的表达形式无关。 实际上马尔可夫定位可以用第3章中讨论的任何形式来实现。 这里讨论三种不同的表达形式，并推导在实时环境下能够定位移动机器人的实际算法。我们从卡尔曼滤波器开始，这种方法通过一阶矩和二阶矩来表示置信度。接着讨论离散的，格点表示方法， 最后介绍使用粒子滤波器的算法。
7.4 EKF定位
扩展卡尔曼滤波定位extern Kalman filter localization算法，或者EKF定位(EKF localization)，是马尔可夫定位的一个特例。 EKF定位用\(bel(x_t)\)的一阶矩和二阶矩表示置信度，分别是均值\(μ_t\)和方差\(\Sigma_t\)。 基本的EKF算法在算法3.3中已经描述过了。
EKF定位算法通常假设地图由一系列特征表示。在任何时刻\(t\)，机器人观测到一个关于附近特征的向量\(z_t = \{ z_t^1, z_t^2, \cdots \}\)。 一开始我们认为所有的特征都是唯一的，这并不是一个孬假设。我们很少会搞混巴黎的一个地标——埃菲尔铁塔，并且在巴黎的很多地方都可以看到它。 特征的身份identity由关联度变量correspondence variable集合来表示，记为\(c_t^i\)，分别对应着每一个特征\(z_t^i\)。 我们已经在6.6节中讨论了关联度变量。 首先假设关联度是已知的，更进一步的我们允许特征之间存在歧义，即有些特征是不容易区分的，得到一种更一般的形式。对这个更一般的形式我们使用极大似然估计， 获得潜在的关联度变量的值，并用之作为实际的值。
7.4.1 示例
图7.6描述了将EKF定位算法应用在一维走廊环境下对移动机器人定位的例子。为了适应EKF算法中置信度的单峰分布，我们做了两个假设。首先，假设关联度已知。 我们为每一个门赋予一个标签(1,2,3)，并用\(p(z_t | x_t, m, c_t)\)表示测量模型，其中\(m\)表示地图，\(c_t \in \{1,2,3\}\)是在\(t\)时刻观察到的门的ID。 其次，假设初始位姿已知。图7.6a中显示了一个高斯分布下的初始置信度，中心在门1附近，并且具有一定的方差表示不确定度。
 |
| 图 7.6a. 高斯分布下的初始置信度 |
随着机器人向右移动，不断地对置信度和运动模型进行卷积运算。导致高斯分布的峰值向右漂移，如图7.6b所示，并且方差在不断增加。
 |
| 图 7.6b. 机器人向右运动，高斯分布的峰值随之移动，并且不确定性增加 |
现在假设机器人检测到在它面前有一扇门标记为\(c_t = 2\)。图7.6c中上面那个概率密度表示测量概率\(p(z_t | x_t, m, c_t)\)，也是一个高斯分布。 将这一测量概率融合进机器人的置信度就得到了图7.6c中下面那个概率密度。可以看到置信度的方差比融合之前的置信度方差以及测量概率的方差都小。 这很正常，因为融合两个独立的估计应当为机器人带来了更多的信息，其确定度要比两个独立的分布都大。
 |
| 图 7.6c. 检测到\(c_t = 2\)后的机器人置信度 |
随着机器人继续在走廊中行走，机器人位置的不确定度就逐渐增大了，因为EKF不断地将运动的不确定度融合进机器人的置信度中。下图7.6d就显示了这样的一种分布。
 |
| 图 7.6d. 随着机器人的运动，其位置的不确定度逐渐增加 |
7.4.2 EKF定位算法
到目前为止的讨论都还是很抽象的，我们假设已经有了合适的运动模型和测量模型，就剩下确认在EKF算法更新过程的一系列关键变量了。 我们现在讨论一个针对基于特征的地图的EKF算法的具体实现。在6.2节中已经提到， 基于特征的地图由路标点构成。我们使用6.6节中的常用的测量模型来描述路标点， 并采用5.3节中定义的速度运动模型。读者可能需要花点时间先了解一下相关内容。
算法7.2描述了EKF_localization_known_correspondences已知相似度的EKF定位算法， 该算法是从算法3.3细化而来的。 它以\(t-1\)时刻机器人位姿高斯估计的均值\(μ_{t-1}\)和方差\(\Sigma_{t-1}\)作为输入。此外，还需要用到\(t\)时刻的控制量\(u_t\)、地图\(m\)、 一个特征集合\(z_t = \{ z_t^1, z_t^2, \cdots \}\)，以及相似度变量\(c_t = \{ c_t^1, c_t^2, \cdots \}\)。其输出是新修订的估计\(μ_t\)和\(\Sigma_t\)， 以及观测特征的似然度\(p_{z_t}\)。该算法并不处理\(\omega_t = 0\)的直线运动的情况，这一特殊情况将留做一个练习。
第3和4行计算了线性化运动模型所需的雅可比矩阵Jacobians。第5行计算因为控制而带来的运动噪声的协方差矩阵。 第6和7行实现了我们已经熟悉的运动更新，在第6行中估计运动之后的位姿\(\bar{μ}_t\)，第7行计算相关的不确定度。
测量更新操作在第8到21行实现，该更新的核心操作就是一个遍历\(t\)时刻所有特征\(i\)的循环。第10行中为\(j\)赋予了测量向量中第\(i\)个特征的相似度。 接着计算了一个预估的测量值\(\hat{z}_t^i\)和测量模型的雅可比矩阵\(H_t^i\)。使用这个雅可比矩阵，该算法确定了与估计测量值\(\hat{z}_t^i\)相关的不确定度\(S_t^i\)。 接着在第15行中计算卡尔曼增益\(K_t^i\)。在第16和17行更新估计。如此循环对所有的特征都要进行一遍该操作。
19和20行设定了新的位置估计，并在第21行计算测量的似然度。在这个算法中，需要考虑连续两个角度之间的差，因为结果可能偏差了\(2\pi\)。
7.4.3 EKF定位算法的数学推导
按照惯例我们先跳过数学推导过程。
$$ \begin{equation}\label{f8} G_t = \frac{\partial g(u_t, \mu_{t-1})}{\partial x_{t-1}} = \begin{pmatrix} \frac{\partial x'}{\partial \mu_{t-1,x}} & \frac{\partial x'}{\partial \mu_{t-1, y}} & \frac{\partial x'}{\partial \mu_{t-1,\theta}} \\ \frac{\partial y'}{\partial \mu_{t-1,x}} & \frac{\partial y'}{\partial \mu_{t-1, y}} & \frac{\partial y'}{\partial \mu_{t-1,\theta}} \\ \frac{\partial \theta'}{\partial \mu_{t-1,x}} & \frac{\partial \theta'}{\partial \mu_{t-1, y}} & \frac{\partial \theta'}{\partial \mu_{t-1,\theta}} \end{pmatrix} \end{equation} $$ $$ \begin{equation}\label{f9} G_t = \begin{pmatrix} 1 & 0 & \frac{v_t}{\omega_t}\left(-\cos \mu_{t-1,\theta} + \cos\left(\mu_{t-1,\theta} + \omega_t\Delta_t\right)\right) \\ 0 & 1 & \frac{v_t}{\omega_t}\left(-\sin \mu_{t-1,\theta} + \sin\left(\mu_{t-1,\theta} + \omega_t\Delta_t\right)\right) \\ 0 & 0 & 1 \end{pmatrix} \end{equation} $$ $$ \begin{equation}\label{f16} p(z_t | x_t, c_t, m) = \prod_i p(z_t^i | x_t, c_t^i, m) \end{equation} $$
7.4.4 物理实现
我们以在RoboCup机器人足球场上定位四足标准机器人AIBO为例介绍EKF定位算法的实现。 如图7.7所示，AIBO以球场中的6个不同颜色的地标定位。正如算法7.2中描述的EKF算法那样， 通过平移和转动速度建立运动控制模型\(u_t = \begin{pmatrix} v_t & \omega_t \end{pmatrix}^T\)， 观测量\(z_t = \begin{pmatrix} r_t & \Phi_t & s_t \end{pmatrix}^T\)测量了到地标的距离和方位角。为了简化问题，我们假设机器人同一时间只检测到了一个地标。
 |
| 图 7.7. RoboCup机器人球场上的AIBO机器人。在球场的四角和中场各有一个地标。 |
预测阶段(第3到7行)。下图7.8中描述了EKF定位算法的预测阶段。在算法的第5行中用四个参数\(\alpha_1,\alpha_2,\alpha_3,\alpha_4\)描述了不同的运动噪声。 在所有的子图中\(\alpha_2\)和\(\alpha_3\)都被设定为5%。从图7.8(a)到图7.8(d)， 其平移和转动的主要噪声参数\(\alpha_1\)和\(\alpha_4\)分别为\(\langle 10\%, 10\% \rangle\), \(\langle 30\%, 10\% \rangle\), \(\langle 10\%, 30\% \rangle\), \(\langle 30\%, 30\% \rangle\)。 在每一幅图中，机器人都执行了控制量\(u_t = \langle 10cm/sec, 5°/sec \rangle\)9秒钟，导致机器人沿着一个90cm的圆弧运动并左转了45°。 机器人的上一个时刻的位姿估计由一个均值为\(μ_{t-1} = \langle 80, 100, 0\rangle\)的椭圆表示。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.8. EKF算法的预测阶段。这些图都是在不同运动噪声下生成的。 机器人的初始估计有中心在\(μ_{t-1}\)的椭圆表示。在一个弧长为90cm，左转45度的运动过后，得到预估的位置中心\(\bar{μ}_t\)。 (a)在平移和转动上的运动噪声相对较小(b)具有较高的平移噪声(c)具有较高的转动噪声(d)平移和转动的运动噪声都比较大。 | |||
在第6行中，EKF算法通过对上一时刻的估计\(\mu_{t-1}\)按照没有噪声的控制量进行平移得到预测的均值\(\bar{\mu}_t\)。其对应的不确定椭圆为\(\bar{\Sigma}_t\)， 有两个部分，一个是因为初始位置的不确定性带来的不确定度，另一个则是按照算法的第7行估计了由运动噪声所带来的不确定度。 算法中第7行的第一项，\(G_t\Sigma_{t-1}G_t^T\)，没有考虑运动噪声，而是通过线性近似的方式将上一时刻的不确定度\(\Sigma_{t-1}\)投射到运动方程中。 根据公式(\(\ref{f8}\))和公式(\(\ref{f9}\))计算线性近似度的矩阵\(G_t\)，它是运动方程关于上一时刻的机器人位姿的雅可比矩阵。
因为运动噪声而带来的不确定度由\(\bar{\Sigma}_t\)的第二项\(V_tM_tV_t^T\)来建模。矩阵\(M_t\)就是第5行中控制空间的运动噪声， 通过与\(V_t\)相乘将运动噪声矩阵映射到了状态空间中。其中\(V_t\)是在第4行中计算的运动方程的雅可比矩阵。可以从图7.8(b)和(d)中看到， 较大的平移速度误差\(\alpha_1 = 30\%\)将导致运动方向上具有较大的不确定度。整个预测的不确定度\(\bar{\Sigma}_t\)由这两个部分求和得到。
修正阶段:测量预测(第8到14行)。修正阶段的第一步就是使用刚刚预测的机器人位置和不确定度来预测测量值\(\hat{z}_t\)。 图7.9是测量预测的示意图。其中图(a)和(c)显示了预测的机器人位置和他们的不确定度椭圆。机器人的真实位置由白色的圆表示。 现在假设机器人观测到在它的右前方有一个地标，这个观测用实线表示。图(b)和(d)表示了测量空间下的预测值和实际测量值。 预测的测量值\(\hat{z}_t\)是根据预测均值\(\bar{\mu}_t\)与观测到的路标之间的相对距离和方位角计算得到的，如算法的第12行所示。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.9. 测量预测。图(a)和(c)中描述了两个预估的机器人位置以及它们的不确定度。 机器人真实的位置以及它的观测分别用白色的圆和实线表示。图(b)和(d)则显示了测量预测的结果，其中白色的箭头表示更新量，是实际测量值与预测值之间的距离。 | |||
这一阶段的不确定度由椭圆\(S_t\)表示，与预测阶段类似，它也是由两个高斯部分构成。椭圆\(Q_t\)对应算法的第8行，表示因为测量噪声带来的不确定度。 椭圆\(H_t\bar{\Sigma}_tH_t^T\)则表示机器人定位的不确定度所引入的不确定度。 机器人定位的不确定度\(\bar{\Sigma}_t\)通过与测量函数的雅可比矩阵\(H_t\)相乘映射到观测不确定度。\(H_t\)的计算在第13行中进行。 在第14行中将这两个不确定度相加得到整体的测量预测不确定度。
图(b)和(d)中的白色箭头就表示所谓的更新向量\(z_t - \hat{z}_t\)，就是观测值与预测值之间的简单求差而已。这个向量在后续的更新过程中起到了至关重要的作用。 它也提供了测量值\(z_t\)的似然度，如算法的第21行所示，通过计算关于更新向量的零均值方差为\(S_t\)的高斯函数来获得。 也就是说更新向量越短(以马氏距离Mahalanobis Distance的形式估计)，测量值就越值得相信。
修正阶段:估计更新(第15到21行)。EKF算法的修正阶段基于更新向量和测量值预测不确定度更新位置估计，如图7.10所示。 为了方便，在图(a)和(c)中再次显示了对测量值的预测结果。在图(b)和(d)中用白色的箭头表示最终位姿估计的修正量。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.10. EKF算法的修正阶段。图(a)和(c)中描述了对测量值的预测结果， 图(b)和(d)则显示了更新后的结果，更新了均值估计量，降低了位置的不确定度。 | |||
这些修正向量在第16行中通过对测量更新向量(图(a)和(c)中的白色箭头所示)在状态空间中的比例映射(scaled mapping)计算得到。 这里的映射(mapping)和比例缩放(scaling)操作通过第15行的卡尔曼增益矩阵\(K_t\)计算得到。直觉上，测量更新向量给出了测量的预测值和观测值之间的偏差。 将这一偏差映射到状态空间中，用于沿着测量更新向量减小的方向修正位置估计。卡尔曼增益对测量更新向量做了缩放，如此就将测量预测中的不确定度考虑进来了。 观测值越明确，卡尔曼增益就越大，因此位置修正的作用就越强。在第17行中更新了位置估计的不确定度椭圆。
示例序列。下面图7.11中显示了两个具有不同测量不确定度的EKF更新序列。 其中图(a)和(c)描述了机器人根据运动控制计算的轨迹(虚线)和实际的运动轨迹(实线)。用细线表示路标检测，图(a)和(b)中的机器人具有较小的测量噪声。 图(b)和(d)中的虚线表示使用EKF定位算法估计的轨迹。正如我们所预期的那样，测量值的不确定度越小将导致更小的不确定度椭圆和估计误差。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.11. 基于EKF的定位算法。图(a)和(c)中机器人具有准确的路标探测传感器， 图(b)和(d)中机器人的路标探测传感器不准确。图(a)和(c)中的虚线表示根据运动控制估计的机器人轨迹，实现表示机器人实际的轨迹。 机器人在5个不同位置上对路标的观测用细线表示。图(b)和(d)中的虚线显示了修正后的机器人轨迹， 以及整合了路标检测修正前(浅灰色,\(\bar{\Sigma}_t\))和修正后(深灰色,\(\bar{\Sigma}_t\))的不确定度。 | |||
7.5 关联度估计
7.5.1 未知关联度的EKF定位
我们现在所讨论的EKF定位算法都要求能够用绝对的不确定度来确定路标的关联度。实际上，这种情况很少见。 因此大多数的实现都是在定位的过程中确定路标的身份。在本书中，我们将讨论各种策略来处理关联度的问题。 其中最简单的方法被称为最大似然关联度maximum likelihood correspondence，它首先确定关联度变量中最可能的值，然后将其用作观测到的路标。
如果很多关联度变量中都有相同的似然度，那么最大似然方法就不适用了。但是，我们总是可以设计一个系统，让这种情况不出现。 为了降低得到错误的关联数据的风险，有两种基本的技术：其一，尽可能地让各个路标都不一样并且让它们相隔很远，以至于很难混淆。 其二，确保机器人的位置不确定度很小。不幸的是，这两种策略是相互对立的，确定环境中路标的间隔需要一些技巧。
此外，极大似然技术具有很实际的意义。算法7.3描述了应用极大似然估计计算关联度的EKF定位算法。 其中第2到7行实现的是与算法7.2相同的运动更新操作。两种算法的关键不同在于测量更新环节，在第10到15行中，对于每个观测值， 我们先计算地图中所有的路标\(k\)，得到一个定量的表述，使得我们可以确定最相似的关联度。在第16行中，根据使得测量值\(z_t^i\)的似然概率最大化， 选择关联度变量\(j(i)\)。注意这里的似然度方程与已知关联度的EKF算法中的似然度方程是一样的。在第18和19行中只根据最可能的关联度变量执行EKF更新操作。
我们注意到算法7.3可能效果并不好，这点可以通过在第10行中使用一种更周到的路标选择方法来提高。 在大多数设定下，机器人只会看到一小部分路标，通过简单的测试就可以拒绝地图中大量不可能的路标。
7.5.2 极大似然数据关联度的数学推导
按照惯例我们先跳过数学推导过程。
7.6 多假设跟踪
有很多对基本EKF算法的扩展，用于无法充分可靠的确定正确的关联数据的场景。本书的后续章节中将讨论一些这样的技术， 因此我们这里简要的浏览一下这些方法。
一个经典的克服数据关联困难的技术就是多假设跟踪滤波器multi-hypothesis tracking filter，简称MHT。MHT可以用多个高斯函数来表示： $$ \begin{equation}\label{f24} bel(x_t) = \frac{1}{\sum_l \psi_{t,l}} \sum_l \psi_{t,l} \det \left(2\pi\Sigma_{t,l}\right)^{-\frac{1}{2}} \exp \left\{ -\frac{1}{2}(x_t - μ_{t,l})^T \Sigma_{t,l}^{-1} (x_t - μ_{t,l}) \right\} \end{equation} $$ 其中，\(l\)为混合要素的索引。对于每一个这样的要素，或者说用MHT的术语说是"track"，都是一个均值为\(μ_{t,l}\)方差为\(\Sigma_{t,l}\)的高斯分布。 标量\(\psi_{t,l} ≥ 0\)是混合权重mixture weight。因为后验概率通过\(\sum_{l}\psi_{t,l}\)归一化，每一个\(\psi_{t,l}\)是一个相对权重， 第\(l\)个混合要素的贡献度依赖于相比于其他混合权重的重要度。
在后面描述MHT算法的时候我们应当看到，每个混合要素都依赖于一个唯一的数据关联决策序列a unique sequence of data association decisions。 因此可以用符号\(c_{t,l}\)描述第\(l\)个track的数据关联向量，并用\(c_{1:t,l}\)表示所有过去和现在所有与第\(l\)个混合要素的数据关联度。 这样，我们可以将混合要素看作是对一个对唯一数据关联序列的局部置信度函数： $$ \begin{equation}\label{f25} bel_l(x_t) = p(x_t | z_{1:t}, u_{1:t}, c_{1:t,l}) \end{equation} $$ 其中，\(c_{1:t,l} = \{ c_{1,l}, c_{2,l}, \cdots, c_{t,l} \}\)表示第\(l\)个track的关联度向量序列。
在描述MHT之前，先讨论派生出MHT的一个完全不可跟踪的算法。这个算法是位置数据关联度EKF算法的完全贝叶斯实现。它很简单，并不是选择最可能的数据关联向量， 而是同时维护所有的这些数据。更具体的，在时刻\(t\)每个混合都被拆分为很多个新的混合，它们每一个都对应着一个唯一的相关度向量\(c_t\)。 令\(m\)为一个新高斯分布的索引，\(l\)为生成这个新高斯分布的高斯索引，有记号\(c_{t,l}\)描述其相关度。这个新混合的权重就可以按如下方式计算: $$ \begin{equation}\label{f26} \psi_{t,m} = \psi_{t,l} p(z_t | c_{1:t-1,l}, c_{t,m}, z_{1:t-1}, u_{1:t}) \end{equation} $$ 这是混合权重\(\psi_{t,l}\)与产生新混合权重的特定相关度向量下测量值\(z_t\)的似然度之积。这种方法的一个好的方面是， 我们已经知道如何计算公式(\(\ref{f26}\))中的测量似然度\(p(z_t | c_{1:t-1,l}, c_{t,m}, z_{1:t-1}, u_{1:t})\)， 只需要按照算法7.2的第21行描述的那样计算测量值似然度。因此，我们可以增量的计算每一个新要素的混合权重。 这种算法的唯一缺点就是混合要素(或者说是tracks)的数量随着时间呈指数级增长。
MHT算法通过保持混合要素在一个较小的规模近似实现了完全不可跟踪算法，这个过程称为剪枝pruning。当每一个要素的相对混合权重 $$ \begin{equation}\label{f27} \frac{\psi_{t,l}}{\Sigma_m \psi_{t,m}} \end{equation} $$ 小于一个阈值\(\psi_{min}\)时终止剪枝。容易证明混合要素的数量最多为\(\psi_{min}^{-1}\)。因此，MHT维护了一个可以高效更新的后验概率。 这个后验概率由很少的一组高斯函数近似，实际上机器人的合理位置也是很少的。
这里省略了MHT算法的形式化描述，但在本书中为读者提供了大量的相关算法。我们注意到在实现一个MHT算法之前， 如果能够找到策略区分出低置信度的跟踪值将有助于算法实现。
7.7 UKF定位
UKF定位是一种使用无迹卡尔曼滤波器的基于特征的机器人定位算法。正如3.4节中描述的那样， UKF使用无迹变换来线性化运动和测量模型。这种算法并不计算这些模型的导数，而是用\(\sigma\)点表示高斯函数，进而描述模型。 算法7.4中描述了应用于基于路标的机器人定位的UKF算法。假设观测数据\(z_t\)中只有路标检测，并且机器人已知各个路标。
7.7.1 UKF定位的数学推导
按照惯例我们先跳过数学推导过程。
7.7.2 示例
我们现在以一个例子说明UKF定位算法，这个例子的设定与介绍EKF定位算法时的例子一致。读者可以将下面的图表与7.4.4节中的图表做对比。
预测阶段(第2到9行)。图7.12中举例说明了在不同噪声配置下UKF算法的预测阶段。 \(\sigma\)点的定位要素\(\mathcal{X}_{t-1}^x\)由上一时刻在\(μ_{t-1}\)附近的路标下置信度生成。15个\(\sigma\)点有7个不同的机器人位置， 其中只有五个能够在\(x\)-\(y\)投影平面中看到。在均值\(\sigma\)点中在顶部和底部附加了两个点表示机器人的不同朝向。弧线描述了第7行中的运动估计。 可以看出，从之前定位和运动噪声的不同组合中，生成了11个不同的估计。这些图中显示了不同运动噪声下的更新结果。 通过预测后的\(\sigma\)点生成预估机器人位置的均值\(\bar{μ}_t\)和椭圆不确定度\(\bar{\Sigma}_t\)。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.12. UKF算法的预测阶段。这些图对应着不同的噪声参数。 机器人的初始位置估计由圆心在\(μ_{t-1}\)的椭圆表示。机器人沿着一个90厘米长，左转45度的圆弧运动。图(a)中在平移和转动两个方面的噪声都很小。 图(b)具有较高的平移噪声，(c)的转动噪声较大，(d)的平移和转动噪声都比较大。 | |||
测量预测(第10到12行)。在第10行中，用预测得到的机器人位置\(\bar{\mathcal{X}}_t^x\)来生成测量\(\sigma\)点\(\bar{\mathcal{Z}}_t\)。 下图13(a)(c)中的黑叉表示定位\(\sigma\)点，图13(b)(d)中白叉表示融合测量值的\(\sigma\)点。注意到11个不同的定位\(\sigma\)点生成了15个不同的测量预测值， 这是因为第10行中在\(\mathcal{X}_t^z\)上加入的不同测量噪声导致的。这些图中也显示了第11和12行提取的预测测量值的均值\(\hat{z}_t\)和不确定度\(S_t\)。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.13. UKF算法的测量预测。图(a)(c)中显示了从两种不同具有不确定度的运动更新中生成的\(\sigma\)点。 分别用白色圆圈和实线表示机器人的真实位置和观测朝向。图(b)(d)中显示了导致测量估计的\(\sigma\)点。白色箭头表示更新量，是观测到的与预测的测量值。 | |||
修正阶段:估计更新(第14到16行)。修正阶段的UKF定位算法基本与EKF修正阶段相同。更新向量和测量预测的不确定度在图7.14中用白色的箭头表示。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.14. UKF算法的修正阶段。图(a)(c)中显示了测量估计。 图(b)(d)中显示了修正结果，更新了估计均值降低了不确定度。 | |||
示例。下图7.15中分别显示了由粒子滤波器(图(b))、EKF(图(c))和UKF(图(d))的位置估计结果。图(a)中显示了根据运动控制生成的轨迹(虚线所示)， 和真实的运动轨迹(实线所示)，并用细线标识检测到的路标。其它三个图中虚线表示对应技术的估计结果。 粒子滤波器的协方差是根据算法8.2在测量跟新之前和之后采样得到的粒子集合计算生成的。 这里之所以拿粒子滤波器作为对照组，是因为粒子滤波器没有做任何线性化的近似。我们可以看到，EKF和UKF的估计结果都与对照组很接近，并且UKF要更为接近一些。
 |
 |
 |
 |
| (a) | (b) | (c) | (d) |
| 图 7.15. UKF与EKF估计对比。图(a)中虚线描述了根据运动控制得到的机器人运动轨迹， 实线表示真实的轨迹。在各个位置上检测到的路标用细线标注出来。图(b)是有一个粒子滤波器生成的估计。图(c)和(d)分别是EKF和UKF的估计。 | |||
下面图7.16中显示了改进线性化方法后的UKF的定位效果，改进的效果十分显著。机器人沿着细线所示的圆弧进行了两次运动， 图中的椭圆显示了两次运动的不确定度(机器人并没有做出任何测量)。这里以准确的基于样本的运动更新作为对照组。 对照组的采样由算法5.3中的sample_motion_model_velocity生成。EKF的线性化方法在均值和方差的"形状"上都产生了显著的偏差， 而UKF估计则基本与对照组一致。这个例子也说明了EKF与UKF估计之间的一个细微的不同。 EKF估计的均值总是与控制的位置估计一致(参见算法7.2的第6行)。 而UKF估计的均值完全是由\(\sigma\)点导出的(参见算法7.4的第7行)，因此会与控制的估计均值存在一定的偏差。
 |
 |
| (a) | (b) |
| 图 7.16. 因为线性化所产生的误差。机器人沿着一个圆弧运动。图(a)中显示了EKF的估计， 图(b)则显示了UKF估计。对照组是一个准确的基于样本的估计(an accurate, sample-based prediction)。 | |
7.8 实际应用中考虑
EKF定位算法以及其它的亲戚MHT定位算法是解决位置跟踪问题的流行方法。实际上关于这两种算法有大量的的变型，在效率和鲁棒性上得到了提升。
- 高效搜索(Efficient search)。首先，像针对未知关联度的EKF定位算法那样，遍历地图中所有的路标\(k\)通常是不切实际的。 通常，有一些简单的测试方法来获取出色的候选路标。比如说，仅仅是把测量值投影到\(x\)-\(y\)空间中。这样我们就可以只处理有限数量的候选路标， 这样的方法可以比平凡实现要快上几个数量级。
- 互斥(Mutual exclusion)。我们所提到的这些算法实现的关键限制来自于一个假设——特征噪声是独立的。读者可能还记得公式(\(\ref{f16}\))， 它使得我们能够按照顺序处理独立的特征，因而避免了潜在的对空间中所有关联度向量的指数级搜索。不幸的是，这种允许指定多个观测特征的方法， 比如\(z_t^i,z_t^j, i\neq j\)会被赋予地图上相同的路标，即\(\hat{c}_t^i = \hat{c}_t^j\)。对于很多传感器，这种关联度就是错误的。 比如说，如果从单目相机图像中抽取的一个特征向量，我们知道图中的两个不同的区域一定对应着物理世界中的两个不同位置。 总是有\(i\neq j \rightarrow \hat{c}_t^i \neq \hat{c}_t^j\)。这个硬约束被称为数据关联中的互斥原则mutual exclusion principle in data association。它降低了所有可能的关联度向量的搜索空间。改进的实现就考虑了这种约束。比如说，我们可以像EKF定位方法那样，先分别搜索每一个关联度， 再用一个"修复"过程修改违背互斥原则的关联度值。
- 抛弃异常点(Outlier rejection)。我们的实现并没有考虑异常点的存在。读者可能还记得在6.6节中，我们允许一个关联度\(c=N+1\)， 其中\(N\)是地图中路标的数量。我们可以很容易的在EKF定位算法中加入异常点检测。特别的，如果我们用\(\pi_{N+1}\)表示异常点的先验概率， 那么算法7.3中第16行中的最大化阶段可以取值为\(N+1\)，如果异常点是测量向量最可能的解释值。显然，异常点不能够提供任何关于机器人位置的信息， 因此，算法7.3中第18和19行中基于位置的因子就可以省略了。
EKF和UKF定位方法只能用于位置跟踪问题。一般的，线性的高斯技术倾向于在位置不确定度较小的环境下工作。对于这一现象有一些比较合理的解释:
- 一个单峰分布的高斯函数通常是跟踪问题中不确定度的一个较好的表达方式，但在更一般的全局定位问题中就不适用了。
- 即使在跟踪问题中，单峰分布的高斯函数也不能够很好的表示，像"机器人在墙边但是不可能在墙里面"，这样的一些硬性约束。 这个限制的严重性增加了机器人位置的不确定度。
- 一个窄的高斯分布降低了关联度决策出错的机率。这一点很重要，尤其是在EKF中。因为只需要一个错误关联度可以导致跟踪器出错，它将导致整个定位序列出错。
- 线性化只在线性化点附近才有较好的效果。根据经验，如果方向角\(\theta\)的偏差超过±20度，线性化将导致EKF和UKF算法失效。
MHT算法以增加计算复杂度的代价克服了其中大多数的问题。
- 通过初始化一个混合高斯模型的置信度可以解决全局定位问题。这一模型可以通过第一次测量值来初始化。
- 机器人绑架问题可以通过在混合模型中注入附加的高斯项得以解决。
- 硬性空间约束仍然很难建模，但是可以使用混合高斯模型得到一个相对较好的结果。
- 对于容易出错的关联度问题，MHT是一个更鲁棒的算法。当然如果正确的关联度不能够在混合高斯模型中表达出来，它也是可能失效的。
- 这里讨论的MHT算法适用了与EKF相同的线性化方法，因此也会面对类似的近似效果。MHT算法也可以使用UKF的形式实现。
在高斯定位算法中设计合适的特征需要一些技巧。这是因为需要满足多个相互竞争的目标。一方面，我们希望获取环境中足够多的特征， 这样机器人位姿估计的不确定度可以保持很小。小的不确定度完全是至关重要的，原因我们已经讨论过了。另一方面，我们希望最小化混淆路标， 或者检测到得到一个虚假路标的可能性。很多环境中没有太多的路标具有很高的可靠性，因此很多实现都是依赖于相对稀疏的路标分布。 MHT算法有一个明显的优势，它对于数据关联错误问题具有更强的鲁棒性。根据经验，大量的路标一般比少量路标的效果要好，即使是EKF和UKF中也是这样。 但是，如果路标很密的话就需要用数据关联中的互斥原则了。
最后，我们注意到EKF和UKF定位过程只使用了所有传感器测量值的一个子集。从原始测量值到特征，需要处理的信息量已经大幅的降低了。 此外，EKF和UKF定位不能够处理否定信息negative information。否定信息包含了特征确实的信息。显然，我们期望看到一个特征但是并没有看到它也携带一部分信息。 比如说，在巴黎没有看到埃菲尔铁塔意味着我们没有在它附近。否定信息问题降低了非高斯置信度，它不能够表示均值和方差。 介于此，EKF和UKF的实现仅仅是忽略了负面消息的问题，而不是从观测的特征中整合这些信息。标准的MHT算法也抛弃了否定信息。但是，它可能在混合权重中引入否定信息， 通过降低没有观测到的路标的权重来实现。
在所有的这些限制下，意味着高斯技术是一种脆弱的定位技术吗?不。EKF,UKF尤其是MHT都具有惊人的鲁棒性。实际上，成功定位的关键依赖于成功的数据关联。 在本书的后续内容中，我们将介绍比目前讨论的算法更成熟的技术来处理关联性。很多这些技术都可以用高斯形式来表示，而且这些算法也都是广为人知的。
7.9 总结
在本章中，我们介绍了移动机器人定位问题，并设计了一系列的算法来解决它。
- 定位问题是估计机器人相对与环境地图的机器人位置的估计。
- 位置跟踪问题已知机器人的初始位置，计算机器人的局部不确定度。全局定位问题是一个更一般的问题，它从头开始计算机器人位置。机器人绑架问题是将已经定位了的机器人搬运到其它位置却不告诉它， 这是三种定位问题中最难的那个。
- 定位问题的难点也是一个环境随时间变动程度的函数。目前讨论的所有算法都是针对静态环境的。
- 被动定位方法是各种滤波器。它处理从机器人请求来的数据但是并不控制机器人。主动技术在定位机器人的同时控制机器人，其目标是最小化机器人的不确定度。到目前为之，我们已经研究了被动算法。 主动的算法将在第17章中讨论。
- 马尔科夫定位只是贝叶斯滤波器应用于移动机器人定位问题的别称。
- EKF定位应用扩展卡尔曼滤波器进行定位。EKF定位主要应用于基于特征的地图。
- 处理关联度问题最常用的技术就是极大似然法。这种技术只是在每个时间点都以最可能的关联度作为正确值。
- 多假设跟踪算法(MHT)跟踪多个关联度，使用混合高斯模型表示后验概率。混合元素都是动态创建的，而且如果它们所有的似然度都低于一个用户声明的阈值就终止创建混合元素。
- 对于数据关联度而言MHT是一种比EKF更鲁棒的技术，但是计算复杂度增加了。MHT也可以使用UKF的形式实现。
- UKF定位使用无迹变换来线性化运动和测量模型。
- 所有的高斯滤波器都能很好的解决不确定度有限的在有区分特征的环境中进行局部位置跟踪的问题。EKF和UKF很少用于全局定位或者有很多相似物体的环境中。
- 选择高斯滤波器的特征需要一些技巧。特征必须具有足够的非奇异性，从而能够最小化混淆特征的几率。它们还必须足够多，机器人总是能够获得特征。
- 线性化高斯算法的表现可以通过一些测量值得到提高，比如说在数据关联过程中的互斥特性。
在下一章中，我们将讨论通过机器人置信度不同表示方式来解决EKF限制的定位技术。
7.10 参考资料
定位问题被称为是"具有自主能力的最基本的问题"(Cox 1991)。Dickmanns and Graefe (1988)是将EKF应用到室外机器人状态估计的先驱，他们用EKF从视频图像中估计高速公路的曲率。 Borenstein et al. (1996)以及Feng et al. 1994)综述了很多早期的室内机器人定位技术。Cox andWilfong (1990)提供的一些早期的移动机器人的文献中也涵盖了定位问题。 很多早期的技术都需要改造环境，添加一些人工的信标。Leonard and Durrant-Whyte (1991)使用EKF来匹配声纳传感器采集到的几何信标。 现如今人工信标的技术仍然有很多实际应用(Salichs et al. 1999)，因为改造环境经济有效。一些早期的学者(Hinkel and Knieriemen 1988)还使用激光雷达来扫描没有改造过的环境。
除了点特征之外，学者们还开发了很多其他几何特征用于定位。Cox (1991)开发了使用红外传感器进行测距匹配的算法。Weiss et al. (1994)则提出了一种使用距离测量值进行定位的方法。 地图匹配的思想，尤其是使用局部占用栅格地图与全局地图匹配，是Moravec (1988)提出。基于这一思想Thrun (1993)提出了一种梯度下降的定位器， 并首次用在了1992年的AAAI竞赛上(Simmons et al. 1992)。Schiele and Crowley (1994)系统的对比了不同的根据占用地图和超声传感器跟踪机器人位置的方法。 他们发现将局部占用地图与全局栅格地图匹配的结果在定位效果上很相似，就好像是匹配是基于从两个地图中提取的特征进行的。Shaffer et al. (1992)对比了地图匹配和基于特征的技术的鲁棒性， 结果显示这两种方法都具有很好的效果。Yamauchi and Langley (1997)的研究表明在变化的环境中地图匹配算法依然具有很强的鲁棒性。 使用扫描匹配进行定位的思想则要追溯到Lu and Milios (1994); Gutmann and Schlegel (1996); Lu and Milios (1998)，其基本原理在其他的领域中也很流行(Besl and McKay 1992)。 Arras and Vestli (1998)也提出了类似的技术，结果显示扫描匹配技术有可能使得机器人定位结果十分准确。Ortin et al. (2004)发现使用相机数据和激光条可以增加距离扫描匹配的准确性。
还有一个流派，专门研究用于定位的几何技术Betke and Gurvits 1994。术语"机器人绑架问题"由Engelson and McDermott (1992)提出，"马尔可夫定位"是由Simmons and Koenig (1995)起名的， 他们的定位器使用栅格来表示后验概率。但是这些思想的源头是Nourbakhsh et al. (1995)，他们提出了移动机器人定位的"正确度因子, certainty factors"。 虽然正确度因子的更新规则并不完全遵循概率法则，但是他们已经把握了多假设估计的基本思想。文献Cox and Leonard (1994)也提出了类似的思想，通过动态的管理假设树的方式定位机器人。 Saffiotti (1997)还有Driankov and Saffiotti (2001)提出了移动定位的模糊逻辑。