视觉SLAM: 专治机器路痴

背景介绍

生活中身边有很多的朋友爱好旅行，在旅行中通常需要记住很多的路线及它们的关联以帮助我们快速到达目的地。世界上最可怕的事莫过于旅行的人是路痴。因为路痴走路从来不记路，当实在无路可走的时候就酱紫。机器人界里也存在路痴，然而机器人界的路痴却是很平常的事，毕竟科技还没进步到那种“机器如人一样智能”的程度。

定位、定向、测速、授时四大方向相关的话题无时无刻渗透到我们的生活中。古时人们依靠夜观天象和结合司南仪来做大范围的方向判定。现代GPS和手机的使用，大大降低技能的操作要求，使得我们确定大范围方向的便利性进一步提高。但是GPS并不是时刻都会存在的。有些环境下并不存在GPS，亦或GPS信息不可用。在无法正常获取GPS信息或不使用GPS的情况下，SLAM大法就可以大展身手了。SLAM全称为Simultaneous Localization and Mapping，即同步定位与导航。当下火热的AR/VR、室内机器人、空中的无人机及水下的探测机器人等，都能找到SLAM技术的身影。SLAM技术以室内应用场景为主，它对于室内无人车和室内机器人的重要性，就如手机技术之于WIFI移动网络和数据网络的重要性一样。

SLAM技术试图解决如何在未知的环境中，根据自身对环境的观测(Observation)确定自身的运动轨迹(Locating)同时构建环境地图(Mapping)的问题，即解决自己如何在陌生的环境中建立未知环境的地图并利用地图为自己定位的问题。Locating和Mapping两者互相依赖，密不可分。一个栗子：假设你需要到一个蛮荒之地参加部落大会时（前提假设你知道部落大会地点的概貌），因为找不到任何人问路，所以你需要做这么几个步骤确定自己的方向：

• 用眼睛（摄像头）观察周围的地标(例如树木)，地标的共同特性是在当前环境中其特征相对其他物体来说相对比较特别；
• 在自己的脑袋中把观察过的地标进行重建，得到周围描述的地图；
• 前进时，不断地扫描周围的环境寻找新地标，将新地标的位置信息加入到地图中，同时根据地标的特性校正头脑中的地图模型；
• “偶尔可以停下来休息，也可以蹲下来张望；走了一条路的时候,记得常回头看看”。当无意中走了很长一段路的时候，和脑海中的地标进行匹配，看看是否走了回头路。

视觉 SLAM的种类

SLAM技术跟所使用的环境感知传感器（俗称法宝）有很大关联，其技术难度也与所使用的法器（传感器）密切相关。从数据特性上看，法器主要有激光传感器和视觉传感器两种。激光相对较为稳定，而视觉受光照影响较大。

激光传感器可以直接提供机器人与测量物体的距离信息，优点是精度高，速度快，计算量不大，容易实时。缺点是分辨率不够，影响建模精度，而且需要很多“人民币”才能拿下，不利于普及到家庭产品中。

与激光传感器的高昂成本不同，摄像头的成本就非常亲民。随着视觉技术的深度发展和CPU运算速度的提升，以前无法实时的视觉算法，现在也能容易做到了。硬件成本的降低及计算能力的提升，促使人们更愿意采用低成本的摄像头做SLAM产品的相关技术开发。视觉SLAM主要用视觉传感器来实现SLAM的功能。当采用视觉方案时，视觉SLAM的视场角（Field of View, FoV，可理解为视力范围）通常也不大，高速运动时秒变“近视”。此外，由于摄像头不能很好的地捕捉到机器人的实时姿态（如速度、加速度等），惯性测量单元(Inertial Measurement Unit, IMU)可作为这一缺陷的弥补。IMU也是一种低成本的硬件设备，常被用在需要进行运动控制的设备（例如汽车、飞机、导弹等）上面，用来对姿态进行精密位移推算，它包含有三个单轴的加速度计（测量加速度）和三个单轴的陀螺仪（测量角速度）。以所使用的传感器类型对SLAM系统作划分的话，视觉SLAM的技术主要分为三大类：单目SLAM、双目SLAM和RGBD-SLAM。

单目SLAM，通常简称为MonoSLAM，指用一个摄像头就完成SLAM算法的所有功能。它的好处是传感器简单，成本足够亲民，很受需要将SLAM进行产品化的研究者的欢迎。当然由于它使用的传感器简单，所带来的后果就是结果里面没有确切的深度信息，它得到的是一个相对的深度值，并且在相似变换空间Sim(3)中求解， 而不是传统意义上的欧氏空间SE(3)。若搭配其他一些传感器例如GPU或IMU，则可以在SE(3)空间中求解，这样求解出来的结果的就可以确保地图和轨迹地图的一致性。另外，单目SLAM使用一张图像来求取物体中的深度信息（即物体离自己有多远的相对值），为了求取这个估计的数值，单目SLAM靠的是运动中的三角测量：轨迹和地图只有相机运动后才能收敛，否则无法收敛；而且相机运动不仅是旋转，必须包含一定量的位移。（“只有一个法宝我容易吗”）

双目SLAM， 常称为Stereo SLAM, 通过使用两个并列的相机来实现SLAM算法的功能。双目相机通过依靠两个相机之间的基线和像素位置的差异估计空间的位置。双目相机可以在运动时估计深度，也可以在静止时估计深度。从某种程度上看，双目相机简化了单目的操作流程，但它的标定非常复杂，计算深度信息非常耗时。双目测距通常用FPGA实现加速。

RGBD相机最大的特别之处是通过红外结构光或Time-of-Flight(ToF)直接测出各个像素的距离。由于每个像素都有一个距离信息，这使得它能提供的信息更为丰富，也没有单目或双目所面临的计算代价过高的问题。市面上常用的双目相机有微软的Kinect系列及华硕出品的Xtion系列等。RGBD相机存在的主要问题测量范围过小，受噪声影响大，视野小等，通常用于室内场景。

有研究者认为，视觉SLAM主要集中在以下四个方面的研究和实现上：

• 地图的表示问题。地图有稠密地图、稀疏地图及半稠密地图等类型，这需要根据机器人所使用的环境和实现方式去综合考虑。
• 信息的感知问题。视觉SLAM使用摄像头感知周围环境，选用单目摄像头、双目摄像头还是多目摄像头实现，需要前期的充分调研。 信息的感知问题。视觉SLAM使用摄像头感知周围环境，选用单目摄像头、双目摄像头还是多目摄像头实现，需要前期的充分调研。
• 数据关联问题。摄像头数据如何和其他类型的数据进行关联，以保证时间戳、坐标系统的一致性，需要额外功夫。
• 定位和构图问题。在机器人前进的过程中估计实时姿态、对地图进行建模和更新、对各种结果进行优化，涉及很多复杂的数学理论。

典型的视觉SLAM计算框架按模块功能可划分为四个主要步骤(除去传感器数据获取)：前端的视觉里程计(Visual Odometry, VO)、后端优化(Backend Optimization)、回环检测(Loop Closure Detection)、和建图(Mapping)。

视觉里程计

该模块的主要功能是估算机器人的位姿（Pose），即机器人的运动量（包括旋转大小、速度等）。在欧氏空间中，这个运动量可以用一个变换矩阵描述，因而求解这个矩阵是视觉里程计的核心功能。在求解思路上，可以根据是否使用特征分为特征点法和直接法两大流派。
特征点法：顾名思义，即用特征描述辅助寻找变换矩阵的一套方法集，它是目前主流的计算方式。视觉SLAM的数据输入为图像，而特征可以描述图像的主要目标（例如树木、车辆等）。计算视觉领域常用的特征包括Harris角点、SIFT特征、SURF特征和ORB特征等。相邻的图像可以进行特征提取后，进行特征匹配，再利用RANSAC算法移除噪声点，可得到较为可信的匹配特征点对。此时，对于非共面情况，可以使用对极几何求取匹配特征点的相对运动； 共面情况下(比如无人携带的俯视相机或扫地机携带的顶视相机)，一般是通过求解单应矩阵间接获取机器人的相对运动量。
直接法：可按字面意义理解为直接使用传感器的数据计算运动量的一套方法集。这套方法直接对像素建立位姿估计方程然后通过求解优化目标得到运动。对于单目SLAM，可以根据将两个像素直接与一个全局模型进行匹配。使用直接法的典型SLAM方法主要有SVO、LSD-SLAM。这两种方案都是使用单目摄像头，效果较好。相比特征点法，直接法的计算方式简单粗暴，但带来的问题是计算量较大，不容易实时。

后端优化

前端估算出的相对运动较为粗糙，虽然也可以做为机器人的轨迹，但存在着累积误差的问题。当把你蒙上眼睛放在一个大房间里走动的时候，走前面几步感觉还行；然而当走了一段距离后，方向的不确定性会越来越大，轨迹也会越来越漂。为消除这种相对运动造成的误差漂移，需要把机器人走过的轨迹放到后端进行慢慢加工和处理。

在SLAM技术发展的早期，人们倾向于采用滤波器实现后端优化功能。典型的方法是卡尔曼滤波(Kalman Filter, KF)、扩展卡尔曼滤波(Extended Kalman Filter, EKF)和粒子滤波（Particle Filter, PF）等。借鉴于EKF的实现方式，SLAM问题可以写成一个运行方程和一个观测方程，优化的目的是消除两个方程的噪声。机器人的所能获取的测量数据有两个，一个是码盘（或IMU）可以获取当前时刻的速度和加速度的数值；另一个是摄像机读取路标点的输入。这两个信息可以预测出机器人在当前时刻的位置（预测）。另外根据以往的路标点，可计算出一个卡尔曼增益，它可以补偿噪声的影响(更新)。最后，对于机器人的估计，变成了预测与更新不断迭代的过程。

使用卡尔曼滤波解决SLAM问题有个马尔科夫假设，即当前时刻的状态只和前一时刻相关，而和之前的状态量没有关系。实际中，这种假设很难成立，因为当前帧的数据也可以和以前的数据产生关联，需要一种基于整体或部分整体数据的考虑方案。此外，EKF需要频繁求逆，对于需要实时计算的移动平台压力很大。在21世纪，出现了运动恢复结构（Structure from Motion, SfM）技术，它是一种从序列二维运动图像恢复空间的三维结构的方法，属于三维重建范畴。SLAM借鉴了SfM的研究成果，将光束平束法(Bundle Adjustment, BA)引入后端优化。它和滤波器的方式的差别是，BA考虑过去帧中的所有信息，是一种整体优化。经过BA优化后，误差会被平均到每一次的观测结果中，精度有了很大提升。在实现方式上，BA常常是用Graph建立模型的，因此也常称为图优化(Graph Optimization)。得益于图理论的成熟，BA现在可以用代数的方法快速求解，所以它现在是视觉SLAM的主流优化引擎了。在实现方面，g2o（General Graph Optimization）是一个基于最小二乘为主的解决BA误差最小化问题的框架；另一个是TORO（Tree-based netwORk Optimizer），是通过梯度下降实现图约束的优化算法框架。

回环检测

别名闭环检测或闭环，指机器人检测是否经过了同一个地方。回环检测的目的是一方面消除整体的累积误差，另一方面构建全局一致的地图。视觉里程为系统提供特征点、运动轨迹和地图的初值，后端对这些结果进行整体优化。前端和后端都是考虑相邻时间的关联，由于相邻时刻总存在误差，即使优化再好，“好模型架不住烂数据”，后端能做的事并不是很多。回环检测可以利用过去很长一段时间的历史数据，为后端提供时隔更加久远的一些约束，进而获得全局一致的估计。直观上看，它是一种检测观测数据相似性的算法。对于视觉SLAM来说，目前采用的多为成熟的词袋模型(Bag of Words, BoW)。BoW首先离线把样本差异足够多的图片提取特征描述(例如SIFT, SURF，ORB等)，建立视词典，对于一张需要描述的图片，BoW的输出结果是这张含有的哪些视觉单词。回环检测需要在精度(Precision)和召回率(Recall)之间做协调，但通常要求精度为第一原则，即所检测的回环必须为真实的回环。

建图

是视觉SLAM的两大目标之一（另一个为定位）。注意到SLAM是一种底层技术，它是为上层应用提供服务的。对于扫地机器人来说，应用的开发者希望地图能够做导航；对于VR/AR来说，更希望在地图上能够叠加虚拟物体。所以不同实用场景下地图的表示方式是不同的。对于视觉SLAM来说，地图主要用于定位、导航、避障、重建和交互。
建图的首要问题是解决地图的表示问题(Map Representation)。生活中地图按类型大致可以分为三类：栅格地图、几何信息地图和拓扑地图等三大类。但在视觉SLAM中，无论采用哪种传感器方案，核心都是为了估计深度信息，因而它建立的地图属于3D点云地图。SLAM的地图按特征点和周围环境信息的完整性划分为稀疏地图与稠密地图两大类。
稀疏地图是指SLAM算法只建模了感兴趣的部分，也就是特征点。例如假如感兴趣的目标是桌子的四个脚，则SLAM算法仅对属于这四个脚的特征点建模。而稠密地图是指SLAM算法建模了所有看过的部分。对于前面的例子，虽然SLAM算法仅对四个脚感兴趣，但它也把看到过的桌面的模型也建立了起来。对于将SLAM算法用于导航和避障的任务来说，稠密地图才能适用。下面从三种类型的传感器出发，简介一下三大类型的传感器是如何建立稠密地图的。

• 单目稠密建图：单目相机仅能得到图像的像素信息，所以只能通过运动，得出运动的轨迹，获得运动量，再通过匹配特征点的关系利用三角测量推算出像素深度。这是一种非常吃力又不讨好的方案，但也实属无奈之举，毕竟法宝只有一件。好处是到处都可以套用。
• 双目稠密建图：利用两个相机看到同一个点的视差计算像素的深度。相比于单目复杂且可能失灵的初始化算法，因为多了一个相机，所以就多了一件法宝。它的好处也是那里都可以套用。
• RGBD稠密建图：自带深度测量法器，直接得到深度信息。吸收了前两种方案的不足，既有像素信息又有深度信息。不过运气就没那么好了，不能用于室外和大场景环境。

最后，视觉SLAM在地图表示方式上，采用对导航比较有用的八叉树地图表示方案。具体来说，就是整个环境用一个大方体表示，每次对大方体的某一个面切一刀，就相对于它的一次划分。这个过程可以迭代到建模所要求的粒度。最后，很重要的一点，当一个大房间的一个大区域下存在箱子时，这个箱子对应的区域就没有必要划分了，因为它的属性都是一样的。建模时，每个小方块当作一个节点。其中根节点为整个大空间。

视觉SLAM的应用场景

下面看看视觉SLAM的应用案例
1） VR/AR：将虚拟的信息应用到真实世界。在计算机的虚拟现实中叠加虚拟物体达到以假乱真的目的。SLAM在里面可用于定位和导航。

2） 无人机：无人机在飞行过程中需要知道哪里有障碍物，并且对整个环境定位自己的位置，规划一条能够避开障碍物到达目的地的路线。显然，这种需求与SLAM的出发点不谋而合，所以SLAM在无人机领域算是典型的应用。

3） 室内机器人：例如扫地机器人，需要定位自己在房间的位置，并对整个房间的环境进行建图，规划清扫路线等。所以视觉SLAM技术在扫地机器人的应用也是很自然的应用。如果解决得足够好，视觉SLAM的成本比较低，使产品的价格有竞争优势。

4） 水下机器人：水下机器人在探测水下环境的情况时，需要知道自己在局部环境的位置和区域，以作出较好的探测路线。这实际也是SLAM技术灵感的出发点。

结语

本文仅仅是介绍了SLAM技术的相关概念和用途。未来，视觉SLAM的潜在方向是多传感器融合（例如加入IMU和雷达）、优化关联的数据、异构处理器集成等，提升系统对于噪声数据的抗干扰能力和重定位精度。机器人相关的产业是个很大的生态系统，视觉SLAM仅是生态系统的一员。相信在不久的将来，视觉SLAM相关的产品飞像陀螺仪一样进入寻常百姓家。