Part I The Essential Algorithms
0、摘要
本教程介绍了同时定位和建图(SLAM)，以及在过去十年中对SLAM进行的广泛研究。SLAM是移动机器人可以构建环境地图，同时使用该地图来计算其自身位置的过程。在过去的十年里，在解决SLAM问题和许多令人信服的SLAM方法的实现方面，已经取得了快速而令人兴奋的进展。本教程的第一部分（本文），描述了SLAM问题的概率形式、基本的解决方法和重要的实现。本教程的第二部分将关注大规模和复杂环境的SLAM问题的最新进展和新公式。
1、介绍
SLAM问题：移动机器人被放置在未知环境中的未知位置，然后逐步构建环境一致地图，同时确定自己在该地图中的位置。因为SLAM问题的解决方案将提供使机器人真正自主的手段，因此被视为移动机器人社区的“圣杯”。
SLAM问题的“解决方案”是过去十年机器人社区的显著成果之一。SLAM问题已经通过许多不同形式的理论所表述和解决。SLAM也在许多不同的领域实现，从室内机器人，到室外、水下和空中系统。在理论和概念层面上，SLAM现在可以被认为是一个已解决的问题。然而，在实际实现更通用的SLAM解决方案方面，特别是在构建和使用感知丰富的地图作为SLAM算法的一部分方面，仍然存在实质性的问题。
第一部分结构：
第二节首先提供了SLAM早期发展的简要历史
第三节介绍了现在的标准贝叶斯形式的SLAM问题的结构，并解释了SLAM过程的演变
第四节描述了SLAM问题的两个关键计算解决方案：通过使用扩展卡尔曼滤波器(EKF-SLAM)和Rao布莱威尔德粒子滤波器(RaoBlackwellised particle fifilters FastSLAM)。其他最近（相对于2006年而言）的解决方案将在的第二部分中讨论
第五节描述了SLAM的一些重要的现实实现，强调了那些可以供学者自由下载和研究的传感器数据和软件
本教程的第二部分描述了SLAM中计算、收敛性和数据关联中的主要问题。在过去的五年里，这些主题一直是SLAM研究社区的主要焦点。
2、SLAM早期历史
2.1 起源
SLAM问题起源于1986年在旧金山举行的IEEE机器人和自动化会议上。这时，概率方法才刚刚开始被引入机器人技术和人工智能。许多研究人员一直在研究将估计理论的方法应用于建图和定位问题；这些人包括彼得·切斯曼、吉姆·克劳利和休·杜兰特-怀特。在会议过程中，充满了关于一致建图的长期讨论。在这个过程中，拉贾·查蒂拉、奥利弗·弗杰拉斯、兰德尔·史密斯和其他人也为这次对话做出了有用的贡献。
2.2 发展受阻
这次对话的结果是认识到一致概率建图是机器人技术中的一个基本问题。在接下来的几年里，人们产生了一些关键的论文。史密斯和切斯曼[39]和Durrant-Whyte[17]的工作为描述地标和操纵几何不确定性之间的关系建立了一个统计基础。这项工作的一个关键要素表明了地图中不同地标的位置的估计之间一定有高度的相关性，而这些相关性确实会随着连续的观测而增长。
与此同时，阿亚奇和福杰拉斯[1]正在从事早期的视觉导航工作，Crowley[9]和Chatila和Laumond[6]正在使用卡尔曼滤波型算法进行移动机器人基于声纳的导航。这两股研究有很多共同点，不久之后就在史密斯、赛尔夫和芝士曼[40]的具有里程碑意义的论文中发表。这篇论文表明：当移动机器人在未知环境中对地标进行相对观察时，由于估计机器人位置[27]的共同误差，对这些地标的估计也都必然相互相关。这一点的含义是深远的，这说明全局一致定位与建图问题的解决方案将由机器人姿态和每一个地标位置组成联合状态，在每个地标观察之后更新。反过来说，这将需要估计器使用一个巨大的状态向量（在地图中维护地标的数量顺序），计算规模也将是地标数量的平方级。
至关重要的是，这项工作并没有研究地图的收敛特性或其稳态行为。事实上，当时人们普遍认为，地图估计的误差不会收敛，而是会表现出具有无限误差增长的随机游走行为。因此，出于定位问题的计算复杂性以及对地图的敛散性的无知，研究人员反而专注于一系列近似一致的建图问题解决方案，这些方案假设甚至强制使地标之间的相关性最小化，把完整的滤波器filter简化成了机器人filter上一系列解耦的地标landmarks（例如[28]，[38]）。同样出于这些原因，关于同步结合定位和建图问题的理论工作暂时停止，工作通常集中在定位或建图的独立问题上。
2.3 理论突破
随着意识到同步结合定位和建图问题一旦被表述为一个单一的估计问题实际是收敛的，理论上也就出现了突破。最重要的是，人们认识到，大多数研究人员试图最小化的地标之间的相关性实际上是问题的关键部分，相反，这些相关性增长得越多，解决方案就越好。在1995年[16]机器人研究国际研讨会上发表的一篇移动机器人调查论文中，有关SLAM问题的结构、收敛结果和“SLAM”缩写等首次亮相。Csorba[11]，[10]提出了关于收敛的基本理论和许多初步结果。已经有几个在地图和定位方面工作的小组，特别是在麻省理工学院的[29]、萨拉戈萨[5]，[4]、悉尼[20]的ACFR、[45]以及其他人[7]，[13]，开始认真研究SLAM在室内、室外和海底环境中的应用。
2.4 理论改进
此时，工作集中于提高计算效率和解决数据关联或“回路闭环”中的问题。1999年的机器人研究国际研讨会(ISRR‘99)[23]是一个重要的会议点，在这里举行了第一次SLAM会议，并在基于卡尔曼滤波器的SLAM方法与Thrun[42]引入的概率定位和建图方法之间实现了一定程度的收敛性。2000年IEEEICRA关于SLAM的研讨会吸引了15名研究人员，并重点关注了算法复杂性、数据关联和实现挑战等问题。接下来在2002年ICRA举办的SLAM研讨会吸引了150名具有广泛兴趣和应用的研究人员。2002年，由斯德哥尔摩的亨里克·克里斯汀森在KTH举办的大满贯夏季学校吸引了所有关键研究人员和来自世界各地的大约50名博士生，在建设该领域方面取得了巨大的成功。近年来，人们对SLAM的兴趣呈指数级增长，研讨会继续在ICRA和IROS举行。SLAM暑期学校也于2004年在托卢斯，2006年在牛津大学开办。
3、SLAM公式和结构
SLAM是指移动机器人可以构建环境地图，同时使用此地图来推断其自身位置的一个过程。在SLAM中，平台的轨迹和所有地标的位置都是在线估计的，其不需要任何先验的位置知识。
3.1 前期
考虑一个移动机器人在一个环境中移动，使用位于机器人上的传感器对一些未知地标进行观测，如下图所示。在时间时刻 $ k$ ，定义以下变量：
$ x_{k} $：状态向量，描述机器人在时间 $ k$ 时的位置和姿态的向量
$ u_{k} $：控制向量，驱动机器人由 $ k-1$ 时刻 $ x_{k-1}$ 姿态 至 $ k$ 时刻 $ x_{k}$ 姿态
$ m_{i}$：描述第 $ i^{th}$ 个地标位置的向量，其真实位置假设为时不变的
$ z_{ik}$：机器人在 $ k$ 时刻"观测"到第 $ i^{th}$ 个地标时产生的观测数据（当任意时刻有多个地标被观测到或当特定地标与此次讨论discussion无关时，观测数据被简写为 $ z_{k}$ ）
此外，还定义了以下集合：
\[ \mathbf{X}_{0: k}=\left\{\mathbf{x}_{0}, \mathbf{x}_{1}, \cdots, \mathbf{x}_{k}\right\}=\left\{\mathbf{X}_{0: k-1}, \mathbf{x}_{k}\right\}：机器人轨迹历史 \]
\[ \mathbf{U}_{0: k}=\left\{\mathbf{u}_{1}, \mathbf{u}_{2}, \cdots, \mathbf{u}_{k}\right\}=\left\{\mathbf{U}_{0: k-1}, \mathbf{u}_{k}\right\}：控制输入历史 \]
\[ \mathbf{m}=\left\{\mathbf{m}_{1}, \mathbf{m}_{2}, \cdots, \mathbf{m}_{n}\right\} ：路标集合 \]
\[ \mathbf{Z}_{0: k}=\left\{\mathbf{z}_{1}, \mathbf{z}_{2}, \cdots, \mathbf{z}_{k}\right\}=\left\{\mathbf{Z}_{0: k-1}, \mathbf{z}_{k}\right\}：所有的路标观测集合 \]
3.2 概率SLAM
在概率形式中，同时定位和地图构建(SLAM)问题要求计算所有时间 $ k$ 的概率分布： \[ P\left(\mathbf{x}_{k}, \mathbf{m} \mid \mathbf{Z}_{0: k}, \mathbf{U}_{0: k}, \mathbf{x}_{0}\right) \] 这种概率分布描述了地标位置 $ m$ 和机器人位姿 $ x_{k}$ (在时间 $ k$ 时 )的联合后验密度（后验：其实也就是知道了控制输入和观测数据的结果，反过来推测地标和位姿），给定传感器记录的观测数据集合 $ {0: k}$和从时间 $ 0$ 到 $ k$ 的控制输入历史 $ U{0:k}$，以及机器人的初始位姿 $ x_{0}$ 。一般来说，对SLAM问题的递归解是可取的：从时刻 $ k-1$ 的分布 $ P({k-1}, {0: k-1}, {0: k-1})$ 开始，根据控制 $ u{k} $ 和观测数据 $ z_{k} $ ，用贝叶斯定理进行计算。该计算需要定义一个运动模型（state transition model）和一个观测模型（observation model），分别描述控制输入和观测数据的影响。
观测模型描述了在知道机器人位姿和地标位置时观测 $ z_{k} $ 的概率，并且通常以下式进行描述： \[ P\left(\mathbf{z}_{k}\mid \mathbf{x}_{k},\mathbf{m}\right) \] 可以合理地假设，一旦定义了机器人的位姿和地标集，那么给定地图和当前的机器人位姿，观测结果是有条件地相互独立（independent）的。
运动模型可以用状态转换形式的概率分布来描述： \[ P\left(\mathbf{x}_{k}\mid \mathbf{x}_{k-1},\mathbf{u}_{k}\right) \] 也就是说，状态转换被假设是一个马尔可夫过程，其下一个状态 $ x_{k}$ 只依赖于立即进行的状态 $ x_{k-1}$ 和所应用的控制向量 $ u_{k}$ ，并且独立于观测结果和地标。
SLAM算法现在以标准的有序两步递归：预测（时间更新）+ 校正（测量更新）形式实现：
- 时间更新（运动模型）
\[ \begin{array}{l} P\left(\mathbf{x}_{k}, \mathbf{m} \mid \mathbf{Z}_{0: k-1}, \mathbf{U}_{0: k}, \mathbf{x}_{0}\right) \\ =\int P\left(\mathbf{x}_{k} \mid \mathbf{x}_{k-1}, \mathbf{u}_{k}\right) \times P\left(\mathbf{x}_{k-1}, \mathbf{m} \mid \mathbf{Z}_{0: k-1}, \mathbf{U}_{0: k-1}, \mathbf{x}_{0}\right) \mathrm{d} \mathbf{x}_{k-1} \end{array} \]
- 测量更新（观测模型）
\[ \begin{array}{l} P\left(\mathbf{x}_{k}, \mathbf{m} \mid \mathbf{Z}_{0: k}, \mathbf{U}_{0: k}, \mathbf{x}_{0}\right) \\ =\frac{P\left(\mathbf{z}_{k} \mid \mathbf{x}_{k}, \mathbf{m}\right) P\left(\mathbf{x}_{k}, \mathbf{m} \mid \mathbf{Z}_{0: k-1}, \mathbf{U}_{0: k}, \mathbf{x}_{0}\right)}{P\left(\mathbf{z}_{k} \mid \mathbf{Z}_{0: k-1}, \mathbf{U}_{0: k}\right)} \end{array} \]
两个等式提供了一个递归程序，用于计算机器人位姿 $ x_{k}$ 和 $ x_{k}$ 在时间 $ k$ 上的联合后验概率 $ P({k}, {0: k}, {0: k}, {0})$。
值得注意的是，建图问题可以表述为计算条件密度： \[ P\left(\mathbf{m} \mid \mathbf{X}_{0: k}, \mathbf{Z}_{0: k}, \mathbf{U}_{0:k}\right) \] 这假设机器人 $ x_{k}$ 的位置在服从初始位置的知识上，一直以来都是已知的（或至少是确定性的）。然后通过融合来自不同位置的观测结果来构造一个地图 $ $。
反之，定位问题可以表述为计算概率分布： \[ P\left(\mathbf{x}_{k} \mid \mathbf{Z}_{0: k}, \mathbf{U}_{0:k},\mathbf{m}\right) \] 这假设地标位置是确定已知的，其目标是基于这些地标来计算机器人位置的估计。
3.3 概率SLAM的结构
为了简化本节中的讨论，我们将删除方程式 $ P({k}, {0: k}, {0: k}, {0})$ 中对历史变量的条件处理，并在上下文允许的情况下，将地标和机器人位姿上所需的联合后验写为 $ P({k}, {k})$ ，甚至写成$ P(_{k},)$ 。
观测模型 $ P({k}{k},)$ 明确说明了观测结果对机器人和地标位置两者的依赖性。由此可见，联合后验不能以如下所示的明显方式进行分割： \[ P\left(\mathbf{x}_{k}, \mathbf{m} \mid \mathbf{z}_{k}\right) \neq P\left(\mathbf{x}_{k} \mid \mathbf{z}_{k}\right) P\left(\mathbf{m} \mid \mathbf{z}_{k}\right) \] 事实上，从关于一致建图[39]，[17]的早期论文中就知道，这样的分割会导致不一致的估计。然而，SLAM问题的结构比这些方程更明显。
再次参考3.1节的机器人位姿地标观测图，可以看出，估计地标和真实地标位置之间的共同误差在地标之间很常见，而且误差的来源比较单一；在机器人进行地标位置的观测时，误差就产生了。反过来说，这意味着地标位置估计中的误差是高度相关的。实际上，这意味着任何两个地标之间的相对位置，如 $ {i}-{j}$ ，可以高精度的测量，即使其中的一个地标（如$ {i}$）的绝对位置可能相当不确定。在概率形式中，这意味着即使边缘密度 $ P({i})$ 可能相当分散，但一对地标 $ P({i},{j})$ 的联合概率密度也会达到峰值。
在SLAM中最重要的认识是，随着越来越多的观察结果的出现，地标估计之间的相关性在单调增加。实际上，这意味着地标的相对位置的估计总是不断改善，永远不会发散，不管机器人的运动如何。在概率术语中，这意味着随着地标观测结果的增加，所有地标 $ P()$ 上的联合概率密度将单调地达到峰值。
这种收敛的原因是机器人的观测地标之间相对位置的是“几乎独立”的测量。再次参考3.1节的机器人位姿地标观测图，考虑机器人在位置 $ x_{k} $ 观察两个地标 $ {i}$ 和 $ {j}$ 。此时，观察到的地标的相对位置显然与机器人的坐标系无关，从这个固定位置进行的连续观测将产生进一步独立的地标之间的相对关系的测量。现在，当机器人移动到位置 $ x_{k+1} $ 时，它再次观察到地标 $ {j}$ ，这允许机器人和地标的估计位置相对于之前的位置 $ x{k} $ 进行更新。反过来，即使 $ {i}$ 地标没有从新的位置看到，信息也会传播回来，以此更新地标 $ {i}$ 。这是因为从之前的测量结果相比，这两个地标是高度相关的（它们的相对位置是已知的）。
此外，相同的测量数据被用来更新这两个地标，这一事实使它们更加具有相关性。术语“几乎独立”的测量是合适的，因为观测误差将通过连续的机器人运动相关联。还要注意，在图中位置 $ x_{k+1} $ 中，机器人观察到两个相对于 $ {j}$ 的新地标。这些新的地标也立即与地图的其他部分联系或相关。稍后更新这些地标时也将更新地标 $ {j}$ ，在更新到 $ _{i}$ 等等。也就是说，所有的地标最终都形成了一个由相对位置或相关性连接起来的网络，每当观察到它们时，其精度或值都会增加。
此过程可视为将所有地标连接在一起的弹簧网络，或者作为嵌入所有地标的橡胶板，如下图：
在某地附近的观测行为就像对弹簧系统或橡胶板的位移，因此它在某地附近的效果很大，并且依赖于局部刚度（相关性）特性，效果又随着与其他地标的距离增大而减小。当机器人在这个环境中移动并观察地标时，弹簧将变得越来越（而且单调地）坚硬。在观测次数极多至极限条件下，就得到了刚性的地标图或准确的相对环境图。在构建地图时，机器人相对于地图测量的位置精度仅受地图质量和相对测量传感器的限制。在理论极限下，机器人的相对位置精度等于给定地图所可实现的定位精度。
4、SLAM解决方案
概率SLAM问题的解决方案包括为观测模型方程（6）和运动模型方程（7）寻找一个适当的表示，该表示允许有效和一致地计算方程（8）和（9）中的先验分布和后验分布。
到目前为止，最常见的表示形式是具有加性高斯噪声的状态空间模型，由此便有了使用扩展的卡尔曼滤波器(EKF)来解决4.1节的SLAM问题；
一个重要的替代表示形式是将方程式（7）中的运动模型描述为一组更一般的非高斯概率分布的样本，这导致了使用 Rao-Blackwellised粒子滤波器或FastSLAM算法 来解决4.2节中所述的SLAM问题。
虽然EKF-SLAM和FastSLAM是两种最重要的解决方法，但较新的替代方案也提供了很大的潜力，包括使用信息状态形式的[43]。本教程的第二部分将进一步讨论这些问题。
4.1 EKF-SLAM
待看
4.2 Rao-Blackwellised 滤波器
待看
5、SLAM的实现
近年来，概率SLAM的实际实现越来越令人印象深刻，其在更具有挑战性的环境中覆盖了更大的领域。在这里，我们将讨论两个有代表性的实现，并提到了其他值得注意的应用程序。
待看
6、结论
本文描述了SLAM问题，解决SLAM问题的基本方法，并总结了方法的关键实现和演示。虽然还有许多实际问题需要克服，特别是在更复杂的户外环境中。一般的SLAM方法现在是机器人技术的一个被充分理解和确立的一部分。本教程的第二部分将总结最近解决SLAM中一些剩余问题的工作，包括；计算、特征表示和数据关联。