视觉SLAM十四讲：第2讲 初识SLAM

第2讲：初识SLAM

2.1 引言

定位和建图是感知的“内外之分”：前者任务是明白自身的状态（即位置）；后者任务是了解外在的环境（即地图）。

传感器：

1. 携带于机器人本体上的，例如机器人的轮式编码器、相机、激光、惯性测量单元（inertial measurement unit，IMU）等；

2. 安装于环境之中，例如前导轨、二维码标志等等。

携带于机器人本体上的传感器测到的通常都是一些间接的物理量而不是直接的位置数据，只能通过一些间接的手段，从这些数据推算自己的位置。它没有对环境提出任何要求，使得这种定位方案可适用于未知环境。

安装于环境中的传感设备，通常能够直接测量到机器人的位置信息，简单有效地解决定位问题。然而，由于它们必须在环境中设置，在一定程度上限制了机器人的使用范围。

视觉SLAM传感器——相机：按照相机的工作方式，相机分为三个大类，单目（monocular）、双目（stereo）和深度相机（RGB-D）

单目相机：只使用一个摄像头进行SLAM的做法称为单目SLAM（monocular SLAM）。

照片本质上是拍照时的场景（scene），在相机的成像平面上留下的一个投影，以二维的形式反映了三维的世界。这个过程丢掉了场景的一个维度：即深度（或距离）。在单目相机中，无法通过单个图片来计算场景中物体离我们的距离（远近）。在单张图像里，无法确定一个物体的真实大小。它可能是一个很大但很远的物体，也可能是一个很近但很小的物体。由于近大远小的原因，它们可能在图像中变成同样大小的样子。

单目SLAM必须移动相机才能估计运动（motion），及估计场景中物体的远近和大小（结构，structure）。

1. 运动信息推测：如果相机往右移动，则图像里的东西就会往左边移动；

2. 结构信息推测：近处的物体移动快，远处的物体则运动缓慢。当相机移动时，这些物体在图像上的运动，形成了视差。通过视差，能定量地判断物体的远近。

单目SLAM估计的轨迹、地图与真实的轨迹、地图相差一个因子，即尺度（scale）。由於单目SLAM无法仅凭图像确定这个真实尺度，所以又称为尺度不确定性。

双目相机和深度相机：通过某种手段测量物体离相机的距离，通过单个图像恢复场景的三维结构，消除尺度不确定性。

双目相机（stereo）：由两个单目相机组成，两相机之间的距离（基线，baseline）已知，通过基线可以估计每个像素的空间位置。

1. 通过双目相机估计每个像素点的深度需要大量的计算并且结果不太可靠；

2. 双目相机测量深度的范围与基线有关，基线距离越大，能够测量的距离越远；

3. 双目相机的距离估计是比较左右眼的图像获得的，不依赖其他传感设备，室内、室外均可使用；

4. 双目或多目相机的缺点是配置与标定均较为复杂，其深度量程和精度受双目的基线与分辨率限制，而且视差的计算非常消耗计算资源，需要使用GPU和FPGA设备加速，才能实时输出整张图像的距离信息。

深度相机（RGB-D相机）：通过红外结构光或time-of-flight（ToF）原理，通过主动向物体发射光并接收返回的光，测出物体离相机的距离。距离是由物理测量得到，因此节省了大量计算。现阶段，多数RGB-D相机存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质等诸多问题。深度相机主要用于室内SLAM，室外则较难应用。

2.2 经典视觉SLAM框架

视觉SLAM流程：

1. 传感器信息读取，主要为相机图像信息的读取和预处理。如果在机器人中，还可能有码盘、惯性传感器等信息的读取和同步；

2. 视觉里程计（visual odometry，VO），视觉里程计任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又称为前端（front end）；

3. 后端优化（optimization），后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对其优化，得到全局一致的轨迹和地图。由于接在 VO之后，又称为后端（back end）。

4. 回环检测（loop closing），测判断机器人是否曾经到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。

5. 建图（mapping），根据估计的轨迹，建立与任务要求对应的地图。

2.2.1 视觉里程计

**视觉里程计（VO）**通过相邻帧图像估计相机运动，并恢复场景的空间结构。由于VO只计算相邻时刻间的运动，而与历史信息无联，因此VO是一种短时记忆机制。

VO是视觉SLAM的关键问题之一。通过吃饭VO相邻帧图像间相机运动的估计，并将相邻时刻的运动“串联”，可以构成机器人的运动轨迹，从而解决了定位问题；同时，根据每时刻相机的位置，可以计算出各像素对应的空间点位置，继而得到地图。然而仅通过VO估计轨迹，将出现累计漂移（accumulating drift）（由于VO只估计相邻图像间的运动，而每次估计都存在误差，先前时刻的误差将会传递到下一时刻，因此，经过一段时间后，估计的轨迹将不再准确。）

后端优化和回环检测用于解决漂移（drift）问题，其中，回环检测负责把“机器人回到原始位置”的事情检测出来；而后端优化则根据该信息，校正整个轨迹的形状。

2.2.2 后端优化

后端优化主要处理SLAM过程中的噪声，从带有噪声的数据中，估计整个系统的状态，以及这个状态估计的不确定性有多大，即最大后验概率估计（maximum a posteriori，MAP）。

在SLAM框架中，前端负责为后端提供待优化的数据，以及这些数据的初始值；后端负责整体的优化过程，它处理的只有数据，无需关心这些数据的来源。在视觉SLAM中，前端和计算机视觉相关，如图像特征提取与匹配等；后端主要是滤波与非线性优化算法。

早期的SLAM问题是一个状态估计问题，即后端优化。SLAM问题的本质为：对运动主体自身和周围环境空间不确定性的估计。为解决SLAM，需要状态估计理论描述定位和建图的不确定性，然后采用滤波器或非线性优化，估计状态的均值和不确定性（方差）。

2.2.3 回环检测

回环检测又称闭环检测（loop closure detection），主要解决位置估计随时间漂移的问题。假设机器人经过一段时间运动后回到了原点，回环检测通过识别原点计算位置估计值的漂移。回环检测的关键在于使机器人具有识别曾到达过的场景的能力。视觉回环检测通常基于图像间相似性实现，其本质是一种计算图像数据相似性的算法。

回环检测与“定位”和“建图”密切相关，事实上，地图存在的主要意义，是为了让机器人知晓自己到达过的地方。

检测到回环后，将A与B是同一个点的信息输入后端优化算法。后端根据这些信息，可以把轨迹和地图调整到符合回环检测的结果。如果能充分且正确的回环检测，就可以消除累积误差，得到全局一致的轨迹和地图。

2.2.4 建图

**建图（mapping）**是指构建地图的过程。地图是对环境的描述，构建过程视具体应用而定。

地图为度量地图与拓扑地图两种。

度量地图（metric map）：精确表示地图中物体的位置关系，通常分为**稀疏（sparse）与稠密（dense）**两类：稀疏地图对地图中的物体进行抽象，选择一部分具有代表意义的物体，称为路标（landmark），并忽略非路标部分，稀疏地图是由路标组成的地图；稠密地图着重于建模所有物体。稠密地图通常按照某种分辨率，由多个基本单元组成，二维地图的基本单元为网格（grid），三维地图的基本单元为体素（voxel）。一般基本单元包含占据、空闲、未知三种状态，以表达该单元内是否包含物体。当查询某个空间位置时，稠密地图能够给出该位置是否可以通过的信息。

对于定位任务，可采用稀疏路标地图，而对于导航任务时，通常需要稠密的地图。由于稠密地图需要存储每个格点的状态，因此占用大量的存储空间，而且多数情况下地图的许多细节部分是无用的，此外，大规模度量地图会出现一致性问题。

**拓扑地图（topological map）**强调地图元素之间的关系。拓扑地图是一个图（graph），由节点和边组成，只考虑节点间的连通性。它放松了地图对精确位置的需要，去掉地图的细节，是一种紧凑表达方式。然而，拓扑地图不擅长表达具有复杂结构的地图。

2.3 SLAM问题的数学表述

假设地图由$N$个路标（$\mathbf{y}_{1}, \mathbf{y}_{2}, \dots, \mathbf{y}_{N}$）组成的，在每个时刻$t_{k}$（$i = 1, 2, \dots, K$），机器人自身位置记为$\mathbf{x}_{k}$，同时传感器会观测到一部分路标点，则“携带传感器的机器人在环境中运动”可由如下两件事情描述：

1. 运动：考虑从$t_{k - 1}$时刻到$t_{k}$时刻，机器人的位置$\mathbf{x}$如何变化；

2. 观测：假设机器人在$t_{k}$时刻，于$\mathbf{x}_{k}$处探测到了某个路标$\mathbf{y}_{j}$，这件事情如何用数学语言来描述。

运动方程：描述运动的数学模型

$\mathbf{x}_{k} = f(\mathbf{x}_{k − 1}, \mathbf{u}_{k}, \mathbf{w}_{k}) \tag {2-1}$

其中，$\mathbf{u}_{k}$表示运动传感器的读数（输入），$\mathbf{w}_{k}$表示噪声。

观测方程：描述机器人在位置$\mathbf{x}_{k}$处，观测到某个路标点$\mathbf{y}_{j}$，产生一个观测数据$\mathbf{z}_{k, j}$。

$\mathbf{z}_{k, j} = h(\mathbf{y}_{j}, \mathbf{x}_{k}, \mathbf{v}_{k, j}) \tag {2-2}$

其中，$\mathbf{v}_{k, j}$表示观测噪声。

针对不同的传感器，这两个方程的参数化形式不同。这两个基本方程共同描述了SLAM基本问题：已知运动观测的读数$\mathbf{u}$ 、传感器的读数$\mathbf{z}$时，如何求解定位问题（估计$\mathbf{x}$）和建图问题（估计$\mathbf{y}$）

$\begin{cases} \mathbf{x}_{k} & = & f(\mathbf{x}_{k − 1}, \mathbf{u}_{k}, \mathbf{w}_{k}) \\ \mathbf{z}_{k, j} & = & h(\mathbf{y}_{j}, \mathbf{x}_{k}, \mathbf{v}_{k, j}) \end{cases}$

SLAM问题可以建模成一个状态估计问题：通过带有噪声的测量数据，估计内部的、隐藏着的状态变量。状态估计问题的求解与两个方程的具体形式，以及噪声分布有关。按照运动和观测方程是否为线性，噪声是否服从高斯分布，状态估计问题可分为线性、非线性和高斯、非高斯系统，其中线性高斯系统（linear gaussian，LG）最简单，它的无偏的最优估计可以由卡尔曼滤波器（Kalman filter，KF）给出。对于复杂非线性非高斯系统（non-linear non-Gaussian，NLNG），可使用扩展卡尔曼滤波器（extended Kalman filter，EKF）和非线性优化两大类方法求解。

早期实时视觉SLAM系统是基于EKF开发的。随后，为克服EKF的缺点（线性化误差、噪声高斯分布假设），人们开始使用粒子滤波器（particle filter）和非线性优化方法。当今主流视觉SLAM使用以图优化（graph optimization）为代表的优化技术进行状态估计。

三维空间的运动由三个轴构成，所以机器人的运动要由三个轴上的平移，以及绕着三个轴的旋转来描述，称为六自由度位姿（位置和姿态）。