SLAM 02.整体框架

 上一篇文章是从人类角度来分析SLAM技术，其实任何计算机技术的实现都是从人类思维出发去解决实际问题。本篇从技术实现角度讲解SLAM的实现框架。

SLAM在自主导航中的位置

在整个移动机器人自主导航（包括自动驾驶）中，区分为几大块：
感知（位姿、地图）；
规划（局部、全局）；
控制（包括轨迹跟踪）。

上图中的橙黄色部分的感知是可以加入人工智能技术进行目标识别，然后把目标识别出来的语义反馈给SLAM进行进一步的定位和建图。在整个机器人自主导航中SLAM占据中间枢纽的作用，是导航不可缺失的关键部分。
SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写，最早由Hugh Durrant-Whyte 和 John J.Leonard提出。SLAM主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。SLAM更像是一个概念而不是一个算法。
这里说的同步定位和地图构建，其中的定位是指机器人对自身的定位，包括当前的位姿（位置和姿态）。地图构建是对周围环境的的重建，一般是3D环境。同步定位和建图是同步进行的，没有先后顺序。这里就存在一个先有鸡还是现有蛋的问题。

SLAM的整体框架

SLAM的整个框架如下：

一个完整的SLAM系统分为：传感器数据处理、前端视觉里程计、后端优化、建图和回环检测。

传感器数据标定和融合

通常SLAM按照传感器的实现分为：基于视觉SLAM和基于激光雷达的SLAM。但是个人觉得这种按照传感器方式划分的方法不科学。比较科学的划分方式是把外部所有的传感器都划分到一个模块中，进行传感器数据的统一处理。
对外界感知的传感器包括：相机、激光雷达和毫米波雷达等，这类传感器就如同人的眼睛和耳朵。相机通过被动接受光线感知外界，雷达通过发射光波或者声波反射后测定物体表面纹理和距离。
同时机器人也要知道自己的位姿和位置，例如需要小脑、前庭觉、本体感觉（感知肌肉和骨骼运动）等的配合。IMU（惯性测量单元）内部的陀螺仪及加速度计能知道机器人的位姿以及加速度，里程计数器通测定行走距离。

以上各种传感器的数据都要进入系统进行统一处理，这就需要数据融合和标定。这是因为不同传感器侧重点不同，数据类型不同，采样频率不同，时钟不同，安装位置和误差不同等等造成数据的差异性，所以需要从时间和空间上进行标定，把数据统一起来，这就叫数据融合。传感器数据的标定和融合是一个很基础性的问题，决定了将来数据处理的效果。

前端视觉里程计（VO）

机器人一边行走一边要知道自己的位置，同时要在自己的大脑中构建外部的环境地图：同时定位和建图。机器人只有知道了外部地图地标，才能准确知道自己的位置。同时只有知道自己的位置、位姿和行进路线，才能反过来在自己大脑中精确构架外部地图。

运动方程和观测方程结合

以人类为例，一种极端情况是对于一个盲人行走，这时没有眼睛得到外部信息，靠自身的运动感知。因为人体大脑和耳蜗中有方向性感知，腿部运动也能感知，例如向左了几步，向右走了几步，前进了几步，倒退了几步。通过这些盲人就基本能知道自己的位置。机器人自身位置数据通过对机器人轮胎运行圈数的估计可以得到机器人自身位置的一个估计。也可以通过IMU传感器，测量物体三轴姿态角(或角速率)以及加速度的装置。加上行程计数器，就能知道自身走了多少米，以及方向，自己的姿态等。这样也能得到一个基本的位置和姿态。然后在机器人中，根据运动方程式就能绘制出自己的轨迹和定位。
但是只根据运动方程式偏差较大，这时如果能借助周围的路标进行矫正则位置更准确。所以一个视力正常的人走路的定位要远远好于盲人，因为借助了外部环境的矫正。在使用机器人运动方程得到机器人位置估计后，我们可以使用测距单元得到的周围环境信息更正机器人的位置。例如我们从摄像机或者激光雷达获取的信息中心提取环境特征值，把这样叫做路标。
这种方法就是把运动方程和观测方程结合方式进行定位。
上图中三角形表示机器人，星号表示路标；机器人首先使用测距单元测量地标相对于机器人的距离和角度。

然后机器人开始进行运动，到达一个新的位置，机器人根据其运动方程预测其现在所处于的新的位置。

在新的位置，机器人通过测距单元重新测量各个地标相对于机器人的距离和角度，测量得到的距离和角度与上述预测结果可能并不一致，因而，上述预测值可能并不是机器人准确位置。

在机器人看来，通过传感器获得的信息相对于通过运动方程得到的信息更为准确，因而，机器人将通过传感器的数据更新对机器人位置的预测值，如上图中实线三角形所示(虚线为第一步中通过运动信息预测的机器人位置)。

经过上述结合，我们重新估计得到的新的机器人位置如上图实线三角形所示，但由于测距单元精度有限，因而，此时，机器人可能实际处于上图点状三角形位置，但此时估计结果相对于初始预测结果已经有明显的改善。

特征点匹配

在机器人观测外部环境中，关键的就是要处理不同图像之间的关系，尤其是特征点之间的匹配。不同的图像帧虽然是在不同位置角度观测到的，但是在时空上具有连续性和关联性。所以机器人能够通过不同图像的变化来确定自己为位姿，同时把新的特征增加到地图中，更新地图。首先要找到两张图像中所有相同的特征点（代表同一个点），接着要把特征点匹配起来。
特征点一般是拐角、区块和边等灰度发生突变的地方，这些点容易被识别，找出特征点接着就是匹配了。寻找特征点有很多种方法，例如SIFT，SURF，ORB。其中ORB比较经典。

定位

两张图中特征点匹配的过程就是为了找出相同点，然后从相同点反过来推算机器人是如何移动。

根据相机成像原理，相邻的两帧图像中能观测出外界同一个点P的位置变化。反过来也可以将P作为不变外界点反推出相机坐标系的变化，从而给相机定位。

当相机为单目时，我们只知道 2D 的像素坐标，因而问题是根据两组 2D 点估计运动。该问题用对极几何来解决。（一个纯数学的方法）。双目、RGB-D 相机或其他已经得到了距离信息的方式：根据两组3D 点估计运动。该问题通常用 ICP 来解决。当知道了相机定位和周围物体的深度，即可反过来进行三维地图建模了。

建图

机器人知道了自己的位置，就可根据观测到的外部数据以及得到的特征值（路标）构架环境3D地图了。稀疏的地图只用特征点构造。但是只知道特征点的地图不行的，例如要避障、规划路径需要知道特征点之间的信息，所以需要更稠密的地图，这时使用稠密的点云而不是稀疏的点云。

小结

下图总结了视觉里程计的结构关系。本文重点讲了特征值法，对于直接法没有讲解。

后端优化

前端的视觉里程计只能求出短时间或者说相邻两帧的转换关系，并建立局部意义的地图。然而当时间延长或者规模变大之后，那么将会出现累积误差。而后端就是要解决这个长时间、大规模建图的问题。因此希望构建一个针对全局的更大规模的优化问题，得到最优的轨迹和地图，这里主要有两种解决思路：

（1）基于马尔可夫性假设的卡尔曼滤波器：马氏性假设可以简单地理解为“当前时刻状态只与上一时刻有关”。针对SLAM问题（非线性）的卡尔曼滤波器给出了单次线性近似下的最大后验估计，或者说是优化过程中一次迭代的结果。

（2）非线性优化：非线性优化考虑了当前及之前所有时刻的状态，同时优化所有时刻的相机位姿以及各个特征点的空间位置。使用光束法平差（BA）求得最优解。

一般情况下认为非线性优化算法比滤波器占有明显优势，应该重点关注非线性优化中的BA求解与图优化。使用图优化做后端已经成为当之无愧的首选，卡尔曼滤波的方法已经越来越少了。
后端优化在后面文章有具体讲解，本篇不展开论述。

回环检测

回环检测也在后端完成，主要目的是识别机器人是否达到了原先的位置，这样可以大大减少运算量。
回环检测要解决如下几个问题：

• 是否达到了原先的地方
• 识别达到的场景
• 计算图像之间的相似性（使用词袋模型）。
  回环检测是一个太重要的话题了，即便到现在也是一个没有被完全解决的问题。其中最关键的核心在于当前端误差累计达到一定程度以后，场景重识别和位姿匹配都不能再以开环位姿为基础做判断，需要全局搜索和匹配了。