自动驾驶基础（十五)之SLAM 简介

前文讲述自动驾驶的环境感知和实时定位时提到了SLAM，今天就具体学习一下SLAM的基本原理和技术方向。

SLAM是Simultaneous Localization and Mapping的缩写，意为“同时定位与建图 （同步定位与地图构建）”，是业界公认视觉领域空间定位技术的前沿方向。它是指运动物体自身位置不确定的条件下，根据传感器的信息，一边计算自身位置，一边构建环境地图的过程,同时利用地图进行自主定位和导航。它主要用于解决机器人在未知环境运动时，如何通过对环境的观测确定自身的定位和运动轨迹，同时构建出周围环境的地图。



机器人从一个未知的位置出发, 在一个分布着人造或者自然路标(或称为特征) 的未知环境中运动。机器人根据己知的运动学模型或者里程计信息推定自己的位置(Dead Reckoning),同时利用自身安装的传感器测量其附近的路标相对于自己的位置,同时建造增量式地图。SLAM 技术的目标就是在实现机器人全局定位的同时, 根据观测到的环境路标递增地建立机器人的导航地图。



目前，SLAM的应用领域主要有机器人、汽车自动驾驶、虚拟现实和增强现实。其用途包括传感器自身的定位，以及后续的路径规划、场景理解。

那么为什么SLAM技术这么重要呢？无论是一个机器人也好，还是无人驾驶汽车也好，要在一个未知环境中，实现智能化行动，其中最核心，最根本的问题有四个，定位技术（Localization）, 第二个是建图(Mapping)， 第三个是导航（Navigation）包括路径规划技术（Path Planning）和跟踪技术（Tracking）, 第四个就是控制执行技术（Controlling）。 而这四个问题中的前三个，SLAM都扮演了最核心的功能。 其意义，打个比方，就像移动互联网时代中，WIFI和3/4G数据传输技术对手机的意义，是一样的。扮演一个最核心，也是最不可或缺的角色。

一个成熟可用的SLAM系统， 必须包括四个基本方面：
1. 如何进行环境描述，即环境地图的表示方法；比如dense和sparse都是它的不同表达方式，这个需要根据实际场景需求去抉择。
2. 怎样获得环境信息，机器人在环境中漫游并记录传感器的感知数据，这涉及到机器人的定位与环境特征提取问题；需要考虑如何全面的感知这个环境，RGBD摄像头FOV通常比较小，但激光雷达比较大。
3. 怎样表示获得的环境信息，并根据环境信息更新地图，这需要解决对不确定信息的描述和处理方法；不同的sensor的数据类型、时间戳、坐标系表达方式各有不同，需要统一处理。
4. 发展稳定、可靠的SLAM方法。这里面涉及到很多数学问题，物理模型建立，状态估计和优化。

针对环境的描述（地图），目前各国研究者已经提出了多种表示法，大致可分为三类：栅格表示、几何信息表示和拓扑图表示，每种方法都有自己的优缺点。

栅格地图表示法即将整个环境分为若干相同大小的栅格，对于每个栅格，各自指出其中是否存在障碍物。这种方法最早由Elfes和Moravec提出，而后Elfes进行了进一步的研究。优点是：(1)创建和维护容易， (2) 尽量保留了整个环境的各种信息， (3)借助于该地图，可以方便地进行自定位和路径规划。而缺点在于：当栅格数量增大时(在大规模环境或对环境划分比较详细时)，对地图的维护行为将变得困难，同时定位过程中搜索空间很大，如果没有较好的简化算法，实现实时应用比较困难。

几何信息地图表示法是指机器人收集对环境的感知信息，从中提取更为抽象的几何特征，例如线段或曲线，使用这些几何信息描述环境。该表示法更为紧凑，且便于位置估计和目标识别。几何方法利用卡尔曼滤波在局部区域内可获得较高精度，且计算量小，但在广域环境中却难以维持精确的坐标信息。几何信息的提取需要对感知信息作额外处理，且需要一定数量的感知数据才能得到结果。

拓扑地图抽象度高，特别在环境大而简单时。这种方法将环境表示为一张拓扑意义中的图(Graph)，图中的节点对应于环境中的一个特征状态、地点。如果节点间存在直接连接的路径则相当于图中连接节点的弧。优点是：(1)有利于进一步的路径和任务规划，(2)存储和搜索空间都比较小，计算效率高，(3)可以使用很多现有成熟、高效的搜索和推理算法。缺点在于对拓扑图的使用是建立在对拓扑节点的识别匹配基础上的，如当环境中存在两个很相似的地方时，拓扑图方法将很难确定这是否为同一点。

SLAM中定位与环境特征提取：移动机器人自定位与环境建模问题是紧密相关的。环境模型的准确性依赖于定位精度，而定位的实现又离不开环境模型。在未知环境中，机器人没有什么参照物，只能依靠自己并不十分准确的传感器来获取外界信息，如同一个盲人在一个陌生环境中摸索的情况。这种情况下，定位是比较困难的。有地图的定位和有定位的地图创建都是容易解决的，但无地图的定位和未解决定位的地图创建如同"鸡--蛋"问题，无从下手。已有的研究中对这类问题的解决方法可分为两类：一类利用自身携带的多种内部传感器(包括里程仪、罗盘、加速度计等)，通过多种传感信息的融合减少定位的误差，使用的融合算法多为基于卡尔曼滤波的方法。

这类方法由于没有参考外部信息，在长时间的漫游后误差的积累会比较大。另一类方法在依靠内部传感器估计自身运动的同时，使用外部传感器(如激光测距仪、视觉等)感知环境，对获得的信息进行分析提取环境特征并保存，在下一步通过对环境特征的比较对自身位置进行校正。但这种方法依赖于能够取得环境特征。

环境特征提取的方法有：
Hough transform，是一类基于灰度图探察直线和其他曲线的方法。该方法需要一簇能被搜索的预准备的特定曲线，并根据显示的灰度图中一簇曲线产生曲线参数。

Clustering分析是一种数据探测工具，对于未分类样例是有效的，同时，它的目标就是把所针对对象分组成自然类别或基于相似性或距离的簇类。在被提取对象类别未知的情况中，簇技术是一类比Hough Transform更有效的技术。簇类应是以"凝聚"为中心，而不是支离破碎的、不相交的。而环境特征有时是很难提取出的，例如：
·环境特征不够明显时；
·传感器信息比较少，难以从一次感知信息中获得环境特征。
在未知环境中，环境信息的不确定性尤为明显。对于不确定信息的描述和处理方法，研究人员已经提出了多种用来处理不确定性的度量方法，如模糊度量、概率度量、信任度量、可能性度量、证据理论度量等等。目前在SLAM中使用较多的主要是模糊度量和概率度量的方法。

以概率描述信息的不确定性，其优点在于：适应不确定模型，对于性能差的传感器也适用，在实际应用中鲁棒性较好，在很多实现中得到应用。缺点是计算量较大，算法建立在一定假设条件上，而假设的合理性有争议。因此，有些学者认为使用模糊度的描述方法更适合于该任务。他们认为模糊逻辑(fuzzy logic)提供了一个关于处理不确定信息更自然的框架，是一个更为鲁棒且有效的工具(尤其对于声纳感知过程引起的不确定性)。同时，在对多种不确定性信息进行建模和融合时有更多的操作符供选择。



SLAM研究自1988年提出以来，已经过了近三十年。早期SLAM研究侧重于使用滤波器理论。目前SLAM方法大致可分为两类：一类为基于概率模型的方法，另一类为非概率模型方法。许多基于卡尔曼滤波的SLAM方法如完全SLAM、压缩滤波、Fast SLAM就属于概率模型方法。非概率模型方法有SM-SLAM、扫描匹配、数据融合(data association)、基于模糊逻辑等。
下图是一个典型的卡尔曼滤波的SLAM方法的框图：



状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。比如对飞行器状态估计。状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。最常用的是最小二乘估计，线性最小方差估计、最小方差估计、递推最小二乘估计等。其他如风险准则的贝叶斯估计、最大似然估计、随机逼近等方法也都有应用。

受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是最优估计。真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推最优估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。

卡尔曼滤波理论的提出，克服了威纳滤波理论的局限性使其在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代工程方面。



SLAM 技术涵盖的范围非常广，按照不同的传感器、应用场景、核心算法，SLAM 有很多种分类方法。如果按照传感器的不同，则可以分为基于激光雷达的 2D/3D SLAM、基于深度相机的 RGBD SLAM、基于视觉传感器的 visual SLAM（以下简称 vSLAM）、基于视觉传感器和惯性单元的 visual inertial odometry（以下简称 VIO）。

基于激光雷达的 2D SLAM 相对成熟，早在 2005 年，Sebastian Thrun 等人的经典著作《概率机器人学》将 2D SLAM 研究和总结得非常透彻，基本确定了激光雷达 SLAM 的框架。目前常用的 Grid Mapping 方法也已经有 10 余年的历史。2016 年，Google 开源了激光雷达 SLAM 程序 Cartographer，可以融合 IMU 信息，统一处理 2D 与 3D SLAM 。目前 2D SLAM 已经成功地应用于扫地机器人中。
下图是2006 年使用激光雷达生成的某实验室地图：



基于深度相机的 RGBD SLAM 过去几年也发展迅速。自微软的 Kinect 推出以来，掀起了一波 RGBD SLAM 的研究热潮，短短几年时间内相继出现了几种重要算法，例如 Kinect Fusion、Kintinuous、Voxel Hashing、Dynamic Fusion 等。微软的 Hololens 应该集成了 RGBD SLAM，在深度传感器可以工作的场合，它可以达到非常好的效果。

视觉传感器包括单目相机、双目相机、鱼眼相机等。由于视觉传感器价格便宜，在室内室外均可以使用，因此 vSLAM 是研究的一大热点。早期的 vSLAM 如 monoSLAM 更多的是延续机器人领域的滤波方法。现在使用更多的是计算机视觉领域的优化方法，具体来说，是运动恢复结构（structure-from-motion）中的光束法平差（bundle adjustment）。在 vSLAM 中，按照视觉特征的提取方式，又可以分为特征法、直接法。当前 vSLAM 的代表算法有 ORB-SLAM、SVO、DSO 等。

视觉传感器对于无纹理的区域是没有办法工作的。惯性测量单元（IMU）通过内置的陀螺仪和加速度计可以测量角速度和加速度，进而推算相机的姿态，不过推算的姿态存在累计误差。视觉传感器和 IMU 存在很大的互补性，因此将二者测量信息进行融合的 VIO 也是一个研究热点。按照信息融合方式的不同，VIO 又可以分为基于滤波的方法、基于优化的方法。VIO 的代表算法有 EKF、MSCKF、preintegration、OKVIS 等。Google 的 Tango 平板就实现了效果不错 VIO。

激光雷达优点是可视范围广，但是缺点性价比低，低成本的雷达角分辨率不够高，影响到建模精度。vSLAM的话，缺点就是FOV通常不大，只有50-60 degree，这样高速旋转时就容易丢，解决方案就是，做vSLAM跟雷达还有IMU的组合。总的来说，相比于基于激光雷达和基于深度相机的 SLAM，基于视觉传感器的 vSLAM 和 VIO 还不够成熟，操作比较难，通常需要融合其他传感器或者在一些受控的环境中使用。

我们通过分析传感器的测量信息做个定性的分析。激光雷达或者 RGBD 相机可以直接获取环境的点云。对于点云中的一个点，它告诉我们在某个方位和距离上存在一个障碍点。而视觉传感器获取的是灰度图像或者彩色图像。对于图像中的一个像素，它只能告诉我们在某个方位有障碍点、障碍点周围的表观（local appearance）如何，但它不能告诉我们这个障碍点的距离。要想计算该点的距离，需要把相机挪动一个位置再对它观察一次，然后按照三角测量的原理进行推算。

原理上很清晰，实际做起来并不简单。首先需要在两幅图像中寻找点的对应，这涉及到特征点的提取和匹配、或者准稠密点之间的匹配。计算机视觉发展到今天，其实还不存在性能和速度上很好满足 vSLAM 的特征提取和匹配算法。常见的特征点提取算法，性能上大致可以认为SIFT>SURF>ORB>FAST，效率上可以认为FAST>ORB>SURF>SIFT（大于号左边代表更优。性能主要包括匹配精度、特征点的数量和空间分布等）。为了在性能和效率上取得折中，通常采用 FAST 或者 ORB，只能舍弃性能更好的 SIFT、SURF 等。

其次，匹配点的图像坐标与空间坐标之间的关系是非线性的，例如 2D-2D 点的对应满足对极几何、2D-3D 点的对应满足 PnP 约束。这些匹配数量较多，前后两帧图像中一般有几十至数百的匹配。这些匹配会引入众多约束关系，使得待估计变量的关系错综复杂。为了得到一个较优的估计，通常需要建立优化问题，整体优化多个变量。说起来这无非是一个非线性最小二乘优化问题，但实现起来并不简单，因为存在非线性约束、约束数量很多、存在误差和野值点，并且要将计算时间控制在允许范围。目前广泛采用关键帧技术，并且通过很多方法来控制问题规模、保持问题的稀疏性等。

非线性优化问题可以用下图来形象阐述。圆饼代表待优化的变量（相机姿态、特征点的空间坐标），杆子代表约束（对线几何、PnP 等）。



前面分析了 vSLAM 的两个困难。前者导致了前端的特征跟踪不易，后者导致了后端的优化不易。想做出一个高效率、鲁棒的 vSLAM 系统还是一个非常有挑战的任务。效率方面，SLAM 必须是实时运行的。如果不能做到实时，就不能称作 SLAM。不考虑实时性，采用从运动恢复结构（structure-from-motion）效果会更好。鲁棒性方面，一个脆弱的系统会导致用户体验很差，功能有限。
下图是使用structure-from-motion 对玲珑塔进行三维重建的示例图：



vSLAM 的核心算法在预备阶段，主要包括传感器的选型和各种标定。Visual SLAM 自 PTAM 算法以来，框架基本趋于固定。通常包括 3 个线程，前端 tracking 线程、后端 mapping 优化线程、闭环检测（loop closure）线程。前端 tracking 线程主要涉及到：特征的提取、特征的匹配；多视图几何的知识，包括对极几何、PnP、刚体运动、李代数等。而后端优化线程则涉及到非线性最小二乘优化，属于数值优化的内容。闭环检测线程涉及到地点识别，本质上是图像检索问题。对于 VIO，还涉及到滤波算法、状态估计等内容。

将 SLAM 算法拆解了看，用到的技术是偏传统的。与当前大热的深度学习「黑箱模型」不同，SLAM 的各个环节基本都是白箱，能够解释得非常清楚。但 SLAM 算法并不是上述各种算法的简单叠加，而是一个系统工程，里面有很多 tradeoff。如果仅仅跑跑开源程序，没有什么核心竞争力。不论是做产品还是做学术研究，都应当熟悉各种技术，才能有所创造。

VSLAM 的发展目前来看，感觉是中规中矩，各个环节在前人的基础上一点点优化，同时不断吸收其他方向的最新成果。短期内肯定会在现有框架下不停地改进。至于长远一些的趋势，IEEE TRO 2016 有一篇综述文章 Past, present,and future of SLAM: Towards the robust-perception age。几位有声望的学者在文中对 SLAM 的趋势做了非常好的总结。

新型传感器的出现会不停地为 SLAM 注入活力。如果我们能够直接获取高质量的原始信息，SLAM 的运算压力就可以减轻很多。举例来说，近几年在 SLAM 中逐渐有使用低功耗、高帧率的 event camera（又称 dynamic vision system, DVS）。如果这类传感器的成本能降下来，会给 SLAM 的技术格局带来许多变化。

自从深度学习在诸多领域所向披靡，不少研究者试图用深度学习中 end-to-end 的思想重构 SLAM 的流程。目前有些工作试图把 SLAM 的某些环节用深度学习代替。不过这些方法没有体现出压倒性优势，传统的几何方法依然是主流。在深度学习的热潮之下，SLAM 涉及的各个环节应该会逐渐吸收深度学习的成果，精度和鲁棒性也会因此提升。也许将来 SLAM 的某些环节会整体被深度学习取代，形成一个新的框架。

SLAM 原本只关注环境的几何信息，未来跟语义信息应该有更多的结合。借助于深度学习技术，当前的物体检测、语义分割的技术发展很快，可以从图像中可以获得丰富的语义信息。这些语义信息是可以辅助推断几何信息的，例如已知物体的尺寸就是一个重要的几何线索。

因为计算机视觉技术的快速发展，摄像头和激光雷达成本的降低，SLAM正在AR、机器人、无人机、无人驾驶等领域火速入侵，用夸张点的说法，就像移动互联网时代的手机地图定位一样，只要是跟位置相关的应用，谁抛弃了这张地图，谁将抛弃了自己的未来。
1. 无人驾驶汽车
因为Google无人驾驶车的科普，很多人都知道了基于激光雷达技术的Lidar Slam。Lidar Slam是指利用激光雷达作为外部传感器，获取地图数据，使机器人实现同步定位与地图构建。虽然成本高昂，但目前为止是最稳定、最可靠、高性能的SLAM方式。



自动（无人）驾驶一般用组合定位，首先本体感受传感器如里程计（Odometry）、陀螺仪(Gyroscopes)等，通过给定初始位姿，来测量相对于机器人初始位姿的距离和方向来确定当前机器人的位姿，也叫做航迹推测。然后用激光雷达或视觉感知环境，用主动或被动标识、地图匹配、GPS、或导航信标进行定位。位置的计算方法包括有三角测量法、三边测量法和模型匹配算法等。从这个角度而言，IMU也是自动驾驶必备的部件。

2. 室内机器人
扫地机要算机器人里最早用到SLAM技术这一批了，国内的科沃斯、塔米扫地机通过用SLAM算法结合激光雷达或者摄像头的方法，让扫地机可以高效绘制室内地图，智能分析和规划扫地环境，从而成功让自己步入了智能导航的阵列。



3. AR/VR
目前基于SLAM技术开发的代表性产品有微软的Hololens，谷歌的Project Tango以及同样有名的Magic Leap，很多VR应用需要用到SLAM技术，定位只是一个feature，路径记录、3D重构、地图构建都可以是SLAM技术的输出。根据 SLAM 得到地图和当前视角对叠加虚拟物体做相应渲染，这样做可以使得叠加的虚拟物体看起来比较真实，没有违和感。

4. 无人机
国内大家非常熟悉的大疆精灵四避障用的双目视觉+超声波。P4里面呈现的主动避障功能就是一种非常非常典型的Slam的弱应用，可以使用 SLAM 构建局部地图，辅助无人机进行自主避障、规划路径。无人机只需要知道障碍物在哪，就可以进行Planning，并且绕开障碍物。当然Slam能做的事情远远不止这些，包括灾区救援，包括探洞，包括人机配合甚至集群，所有的关于无人机的梦想都建立在Slam之上，这是无人机能飞（具有定位，姿态确定以后）的时代以后，无人机最核心的技术。