即时定位与地图构建技术浅析

2018 年 7 月 13 日 计算机与网络安全

一次性进群，长期免费索取教程，没有付费教程。

教程列表见微信公众号底部菜单

进微信群回复公众号：微信群；QQ群：16004488

一、SLAM概述

SLAM：Simultaneous Localization and Mapping，即时定位与地图构建技术。它指的是：机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的环境特征定位自身位置和姿态，再根据自身位置构建周围环境的增量式地图，从而达到同时定位和地图构建的目的。

利用SLAM技术，可以让机器人在未知的环境里实时地知道自己在哪里，并同步绘制环境地图。在机器人的定位、跟踪和路径规划技术中，SLAM都扮演了核心角色。没有SLAM的机器人就好比在移动互联网时代不能上网的智能手机，发挥不了重要作用。非要向不了解SLAM的人简单地描述SLAM的一个应用场景的话，扫地机器人应该就是用来解释的最佳神器。

从图中我们可以看出，扫地机器人能将其所经过的区域绘制成地图，规划清扫路径，从而进行清扫。这种即时定位与地图构建技术，就是我们所说的SLAM技术。要利用SLAM技术，首先需要机器人配置至少一个测距装置，通过测距装置获取机器人周围环境的信息。目前比较常见的测距装置主要有激光测距、超声波测距和图像测距。

	激光测距	超声波测距	图像测距
优点	测量精度高、效率高、输出不需要太多数据处理	水下穿透力强、价格便宜	模仿人类思考，可获得更多信息，
缺点	价格比较昂贵、不能水下测量、测距玻璃无效	测量精度低	输出需要大量计算，较暗的房间内无法使用

由表可见，使用激光测距方法进行距离测量，可获取较高的空间定位精度，且运算要求较低。这样的特性使得激光测距可以很容易地应用到SLAM中。为方便大家理解，接下来我们将以二维空间的激光定位技术来解释SLAM技术应用的具体过程。

SLAM技术的发展推动了定位（Localization）、跟踪（Tracking）以及路径规划（Path Planning）技术的发展，进而对无人机、无人驾驶、机器人等热门研究领域产生重大影响。

我们来看看SLAM技术都在哪些领域发挥作用吧。

图1 SLAM应用领域

VR/AR 方面：辅助增强视觉效果。SLAM技术能够构建视觉效果更为真实的地图，从而针对当前视角渲染虚拟物体的叠加效果，使之更真实没有违和感。VR/AR代表性产品中微软Hololens、谷歌ProjectTango以及MagicLeap都应用了SLAM作为视觉增强手段。

无人机领域及机器人定位导航领域：地图建模。SLAM可以快速构建局部3D地图，并与地理信息系统（GIS）、视觉对象识别技术相结合，可以辅助无人机识别路障并自动避障规划路径，还可以辅助机器人执行路径规划、自主探索、导航等任务。曾经刷爆美国朋友圈的Hover camera无人机，正是应用SLAM技术。国内的科沃斯、塔米扫地机通过用SLAM算法结合激光雷达或者摄像头的方法，让扫地机可以高效绘制室内地图，智能分析和规划扫地环境，从而成功让自己步入了智能导航的阵列。

无人驾驶领域：视觉里程计。SLAM 技术可以提供视觉里程计功能，并与GPS等其他定位方式相融合，从而满足无人驾驶精准定位的需求。例如，应用了基于激光雷达技术Lidar Slam的Google无人驾驶车以及牛津大学Mobile Robotics Group11年改装的无人驾驶汽车野猫（Wildcat）均已成功路测。

二、SLAM框架

自从上世纪80年代SLAM概念的提出到现在，SLAM技术已经走过了30多年的历史。SLAM技术采用的硬件、算法也在“与时俱进”，SLAM技术框架逐渐丰富。在硬件方面，SLAM系统使用的传感器从早期的声呐，逐渐演变到后来的2D/3D激光雷达，再到单目、双目摄像机，RGBD、ToF等深度摄像机，以及与惯性测量单元（IMU）等传感器的融合。在软件算法方面，也从开始的基于滤波器的方法，如扩展卡尔曼滤波器（EKF）、粒子滤波器（PF）等，向基于优化的方法转变。

SLAM系统一般分为五个模块，系统框架如图2所示，包括：

传感器数据：主要用于采集实际环境中的各类型原始数据。包括激光扫描数据、视频图像数据、点云数据等。
视觉里程计：主要用于不同时刻间移动目标相对位置的估算。包括特征匹配、直接配准等算法的应用。
后端：主要用于优化视觉里程计带来的累计误差。包括滤波器、图优化等算法应用。
建图：用于三维地图构建。
回环检测：主要用于空间累积误差消除。

图2 SLAM系统框架

其工作流程大致为：

传感器读取数据后，视觉里程计估计两个时刻的相对运动（Ego-motion），后端处理视觉里程计估计结果的累积误差，建图则根据前端与后端得到的运动轨迹来建立三维地图，回环检测考虑了同一场景不同时刻的图像，提供了空间上约束来消除累积误差。

三、SLAM技术的发展

依据传感器的特点不同，SLAM技术也分为激光和视觉两个技术路线，其特点如图4所示。

激光雷达传感器

激光雷达利用激光在同一空间参考系下获取物体表面每个采样点的空间坐标，可得到一系列表达目标空间分布和目标表面特性的海量点的集合。由于激光雷达距离测量比较准确，误差模型简单，在强光直射以外的环境中运行稳定，因此点云的处理也比较容易。同时，点云信息本身包含直接的几何关系，使得机器人的路径规划和导航变得直观。激光SLAM理论研究也相对成熟，落地产品更丰富。

视觉传感器

早期的视觉SLAM基于滤波理论，其非线性的误差模型和巨大的计算量成为了它实用落地的障碍。近年来，随着具有稀疏性的非线性优化理论（Bundle Adjustment）以及相机技术、计算性能的进步，实时运行的视觉SLAM已经不再是梦想。

图3 SLAM两大类传感器

图4 SLAM两大传感器特点

1、激光雷达SLAM发展

基于激光雷达的SLAM（Lidar SLAM）采用2D或3D激光雷达（也叫单线或多线激光雷达）。在室内机器人（如扫地机器人）上，一般使用2D激光雷达，在无人驾驶领域，一般使用3D激光雷达。

现任Udacity创始人CEO、前Google副总裁、谷歌无人车领导者Sebastian Thrun在他2005年的经典著作《ProbabilisticRobotics》一书中详细阐述了利用2D激光雷达基于概率方法进行地图构建和定位的理论基础，并阐述了基于RBPF（Rao-blackwellizedparticle filter）粒子滤波器的FastSLAM方法，成为后来2D激光雷达建图的标准方法之一GMapping的基础，该算法也被集成到机器人操作系统（Robot Operation System，ROS）中。2013年，对ROS中的几种2DSLAM的算法HectorSLAM，KartoSLAM，CoreSLAM，LagoSLAM和GMapping做了比较评估。2016年，Google开源其激光雷达SLAM算法库Cartographer，它与GMapping相比，能够有效处理闭环，达到了较好的效果。

2、视觉SLAM发展

现在主流的视觉SLAM系统的特性主要体现在前端和后端，如图5所示。

前端对应视觉里程计，主要用于数据关联，研究帧与帧之间变换关系，完成实时的位姿跟踪，对输入的图像进行处理，计算姿态变化。由于与后端还存在反馈，因此也检测并处理闭环，当有惯性测量单元（IMU）信息时，还可以参与融合计算。

后端主要对前端的输出结果进行优化，利用滤波理论，如扩展卡尔曼滤波（EKF）、粒子滤波（PF）等优化理论进行树或图的优化，得到最优的位姿估计和地图。

图5 典型SLAM的前端和后端

2007年，第一个成功基于单目摄像头的纯视觉SLAM 系统MonoSLAM诞生。同年，Murray和Klein提出了实时SLAM系统PTAM（ParallelTracking and Mapping），PTAM实现了架构创新，它将姿态跟踪（Tracking）和建图（Mapping）两个线程分开并行进行，这在当时是一个创举，第一次让大家觉得对地图的优化可以整合到实时计算中。这种设计为后来的实时SLAM（如ORB-SLAM）所效仿，成为了现代SLAM系统的标配。

图6 PTAM系统与EKF-SLAM系统产生结果对比

2015年，Mur-Artal 等提出了开源的单目ORB-SLAM，并于2016年拓展为支持双目和RGBD传感器的ORB-SLAM2，它是目前支持传感器最全且性能最好的视觉SLAM系统之一，也是所有在KITTI数据集上提交结果的开源系统中排名最靠前的一个。

如图7所示，ORB-SLAM主要分为三个线程进行，即跟踪（Tracking）、建图（LocalMapping）和闭环检测（LoopClosing）。跟踪部分是主要是从图像中提取ORB特征，根据上一帧进行姿态估计，或者进行通过全局重定位初始化位姿，然后跟踪已经重建的局部地图，优化位姿，再根据一些规则确定新的关键帧。建图部分主要完成局部地图构建。包括对关键帧的插入，验证最近生成的地图点并进行筛选，然后生成新的地图点，使用局部捆集调整（Local BA），最后再对插入的关键帧进行筛选，去除多余的关键帧。闭环检测部分主要分为两个过程，分别是闭环探测和闭环校正。闭环检测先使用二进制词典匹配检测（WOB）进行探测，然后通过Sim3算法计算相似变换。闭环校正，主要是闭环融合和Essential Graph的图优化。由于ORB-SLAM系统是基于特征点的SLAM系统，故其能够实时计算出相机的轨线，并生成场景的稀疏三维重建结果。

图7 ORB-SLAM2的三线程结构

2016年，TUM机器视觉组的Engel等人又提出了DSO系统，该系统是一种新的基于直接法和稀疏法的视觉里程计，它将最小化光度误差模型和模型参数联合优化方法相结合。为了满足实时性，不对图像进行光滑处理，而是对整个图像均匀采样（如图8）。该系统在TUM monoVO、EuRoC MAV和ICL-NUIM三个数据集上进行了测试，达到了很高的跟踪精度和鲁棒性。

图8 DSO系统

2017年，香港科技大学的沈绍劼老师课题组提出了融合惯性测量单元（IMU）和视觉信息的VINS系统，同时开源手机和Linux两个版本的代码，这是首个直接开源手机平台代码的视觉IMU融合SLAM系统。这个系统可以运行在iOS设备上，为手机端的增强现实应用提供精确的定位功能，同时该系统也在应用在了无人机控制上，并取得了较好的效果。

【推荐书籍】