一、SLAM的定义

1.1 SLAM是什么

​ SLAM：Simulaneous Localization and Mapping, 即同时定位与地图构建；指的是搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。当使用的传感器主要为相机时，那就称为“视觉SLAM”，当传感器主要为激光雷达时，称为“激光SLAM”。

机器人的定位与建图：

​ 机器人需要时刻感知自身的状态与周边的环境，这通过传感器来完成。一类是携带在机器人本体上的传感器，如机器人的轮式编码器、相机、激光传感器等等。另一类是安装在环境中的，如导轨、二维码标志等等。安装在环境中的传感器设备，通常能够直接测量机器人的位置信息，简单有效地解决定位问题。然而，这类传感器要求环境中必须人工配置，在未知环境中则无法使用。

​ 因此，当机器人位于未知环境中时，便需要通过机器人本体携带传感器来完成定位与建图。此时，由于这些传感器测量的都是一些间接的物理量而不是直接的位置数据，只能通过一些间接的手段，从这些数据中推算出自己的位姿。因此这种定位方案没有对环境提出要求，可适用于未知环境。

相机可以分为：

• 单目（Monocular）相机
• 双目（Stereo）相机
• 深度（RGB-D）相机

1.2 经典视觉SLAM框架

视觉SLAM流程包括以下步骤：

1. 传感器信息读取。
2. 前端视觉里程计（Visual Odometry，VO）。视觉里程计的任务是估算相邻图像间相机的运动，以及局部相机的样子。VO又称前端。
3. 后端（非线性）优化：后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对他们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端（Back end）。
4. 回环检测（Loop Closure Detection）:回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
5. 建图（Mapping）：它会根据估计的轨迹，建立与任务要求对应的地图。

1.3 视觉里程计

​ 视觉里程计关心的是相邻图像之间的相机运动。

​ 视觉里程计能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。

​ 里程计：只计算相邻时刻的运动，而和过去的信息没有关联。

​ SLAM问题的解决方案：假定我们以及有了一个视觉里程计，估计了这两张图像间的相机运动。那么一方面，只要把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，从而解决了定位问题。另一方面，我们根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。

​ 由于仅仅使用视觉里程计将会带来累计误差，每次估计都有误差，先前时刻的误差会传递到下一时刻，导致经过一段时间之后，估计的轨迹讲不再准确，即漂移。因此为了解决漂移问题，还需要两种技术：后端优化和回环检测。

​ 回环检测负责把“机器人回到原始位置”的事情检测出来，后端优化则负责根据该信息，校正整个轨迹的形状。

1.4 后端优化

​ 后端优化主要指处理SLAM过程中的噪声问题。后端要考虑的问题就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大，这称为最大后验概率估计（MAP）。这里的状态既包括机器人自身的轨迹，也包含地图。

1.5 回环检测

​ 又称闭环检测，主要解决位置估计随时间漂移的问题。需要让机器人具有识别到过的场景的能力。如果回环检测成功，则可以显著地减少累计误差。所以视觉回环检测实质上是一种计算图像数据相似性的算法。

二、SLAM问题的数学表述

机器人在未知环境中的运动，可以由以下两种方程描述：

• 运动方程

  

• 观测方程


把SLAM问题建模成一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。

线性高斯系统：无偏的最优估计可以由卡尔曼滤波给出。

非线性非高斯系统：求解方法：

• 扩展卡尔曼滤波
• 非线性优化
• 粒子滤波器
• 图优化

三、三维空间刚体运动

如何描述刚体在三维空间中的运动？

• 运动由一次旋转加一次平移组成。