《视觉SLAM十四讲》第2讲

一视觉SLAM中的传感器
二经典视觉SLAM框架
三 SLAM问题的数学表述

注：原创不易，转载请务必注明原作者和出处，感谢支持！

本讲主要内容：
（1）视觉SLAM中的传感器
（2）经典视觉SLAM框架
（3） SLAM问题的数学表述

一视觉SLAM中的传感器

想象一个在室内的移动机器人在自由地探索室内的环境，那么定位与建图可以直观地理解成：
（1）我在什么地方？——定位
（2）周围环境是怎样的？——建图

而要完成定位和建图则需要各种传感器的支持。传感器一般可以分为两类，一类是搭载于机器人本体上的传感器，另一类是安装于环境中的传感器。
（1）搭载于机器人本体上的传感器
轮式编码器
相机

激光传感器

惯性测量单元IMU

（2）安装于环境中的传感器
GPS设备

二维码

导轨

安装于环境中的传感器，虽然简单可靠，但它们在一定的程度上限制了机器人的使用范围。要想得到一个普遍的，通用的SLAM方案，更多的还是得依靠搭载于机器人本体上的传感器。

在视觉SLAM中，常用的视觉传感器有：单目相机，双目相机和RGB-D相机（深度相机）。单目相机的优点是结构简单，成本低，缺点是具有尺度不确定性。双目相机能够通过左眼和右眼的两张图像计算每个像素点的深度，但存在着标定复杂，计算量大的缺点。RGB-D相机能够通过物理手段直接得到像素的深度，但存在着测量范围窄，噪声大，视野小等缺点。

二经典视觉SLAM框架

整个视觉SLAM流程包括以下步骤：
（1）传感器信息读取，视觉SLAM中主要是相机图像信息的读取和预处理。可能还有其它的传感器，比如码盘，IMU等信息等
（2）视觉里程计（Visual Odometry, VO），视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又被称为前端
（3）后端优化（Optimization），后端优化接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于在VO之后，又被称为后端。
（4）回环检测（Loop Closing Detection），回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
（5）建图（Mapping），它根据估计的轨迹，建立与任务要求对应的地图。

VO能够通过相邻帧间的图像估计相机运动，并恢复场景的空间结构。称它为里程计是因为它和实际的里程计一样，只计算相邻时刻的运动，而和再往前的过去的信息没有关系。然而，光靠VO是没法完成整个SLAM任务的。仅通过VO来估计轨迹，将不可避免地出现累积漂移。这是由于VO在最简单的情况下，只估计两个图像间的运动造成的。

回环检测主要解决位置随时间漂移的问题。为了实现回环检测，我们需要让机器人具有识别到过的场景的能力。我们希望机器人能使用携带的传感器——也就是图像本身来完成这一任务。例如，我们可以判断图像间的相似性来完成回环检测。所以视觉回环检测实质上是一种计算机图像数据和相似性的算法。

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

建图（Mapping）是指构建地图的过程。地图可以分为度量地图和拓扑地图两种。度量地图强调精确地表示地图中物体的位置关系，通常可分为稀疏（Sparse）和稠密（Dense）两种。

稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。而稠密地图着重于建模所有看到的东西。对于定位，稀疏路标地图就足够，而用于导航则需要稠密地图。

相比于度量地图的精确性，拓扑地图则更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性，例如A，B点是连通的，而不考虑如何从A点到达B点。

三 SLAM问题的数学表述

详细内容请参考《视觉SLAM十四讲》。

考虑一段连续时间里的$K$个离散时刻$t=1, 2, ..., K$。用$x$表示机器人自身的位置，则各个时刻的标记为$x_1, x_2, ..., x_K$。设路标共有$N$个，分别为$y_1, y_2, ..., y_N$。

考虑下面两个问题：
（1）什么是运动？所谓运动就是考虑机器人从$k-1$时刻到$k$时刻，机器人的位置$x_k$是如何变化的，如何用数学表示这一过程。
（2）什么是观测？假设机器人在$k$时刻于位置$x_k$处探测到了某一个路标$y_j$，如何用数学表示该事件。

对于问题（1），我们可以使用一个通用的，抽象的数学模型来表示
\[
x_k = f(x_{k-1}, u_k, w_k)
\]
其中，$u_k$是运动传感器的读书，$w_k$为噪声。$f$为一个通用的一般函数。这个方程被称为运动方程

对于问题（2），我们可以用一个观测方程来表示，该方程描述的是：当机器人在$x_k$位置上观测到某个路标点$y_j$，产生了一个观测数据$z_{k,j}$。同样，用一个一般的通用函数$h$表示这个关系，其中$v_{k,j}$表示观测噪声。
\[
z_{k,j} = h(y_j, x_k, v_{k,j})
\]

下面举一个具体的参数化例子。

假设机器人在平面中运动，其位姿由两个位置和一个转角来表示，即$x_k=[x,y, \theta]_k^T$。同时，运动传感器能够测量到机器人在任意两个时间间隔的位置和转角变化量$u_k=[\Delta x,\Delta y, \Delta \theta]_k^T$，于是，运动方程可以具体化如下
\[
\begin{bmatrix}x\\y\\ \theta \end{bmatrix}_k =
\begin{bmatrix}x\\y\\ \theta \end{bmatrix}_{k-1} +
\begin{bmatrix}\Delta x\\ \Delta y\\ \Delta \theta \end{bmatrix}_k +
w_k
\]

SLAM过程可以总结为两个基本方程：
\[
x_k = f(x_{k-1}, u_k, w_k)\\
z_{k,j} = h(y_j, x_k, v_{k,j})
\]

对于观测方程，假设机器人携带了一个二维激光传感器，二维激光传感器观测一个2D路标时，能够测量到两个量：路标点与机器人本体之间的距离$r$和夹角$\phi$。记路标点为$y=[p_x, p_y]^T$（下标已忽略），观测数据为$z=[r, \phi]^T$，那么观测方程为
\[
\begin{bmatrix}r\\ \phi\end{bmatrix} = \begin{bmatrix}\sqrt{(p_x-x)^2 + (p_x-x)^2}\\ arctan(\frac{p_y-y}{p_x-x})\end{bmatrix} + v
\]

对于简单的线性高斯系统（LG系统），其无偏最优估计可以由卡尔曼滤波（KF）给出。

对于复杂的非线性非高斯系统（NLNG系统），我们一般使用扩展卡尔曼滤波器（EKF）和非线性优化方法求解。时至今日，主流视觉SLAM使用以图优化的优化方法进行状态估计。