1. 什么是激光雷达？

激光雷达是一种雷达系统，是一种主动传感器，所形成的数据是点云形式。其工作光谱段在红外到紫外之间，主要发射机、接收机、测量控制和电源组成。

工作原理为：首先向被测目标发射一束激光，然后测量反射或散射信号到达发射机的时间、信号强弱程度和频率变化等参数，从而确定被测目标的距离、运动速度以及方位。除此之外，还可以测出大气中肉眼看不到的微粒的动态等情况。

激光雷达的作用就是精确测量目标的位置（距离与角度）、形状（大小）及状态（速度、姿态），从而达到探测、识别、跟踪目标的目的。

2. 什么是激光SLAM？

室内外导航、无人驾驶、增强现实等方面的应用需要实时定位搭载传感器的主体在环境中的位姿。定位技术虽然已有很多种，但多数的技术要么在实际应用中存在诸多局限，要么代价过高难以普及。如 GPS 无法在室内及严重遮挡的室外环境中使用，且定位精度较低；高精度的惯导系统成本过高；基于无线信号（WiFi、蓝牙、 LED 可见光）的定位方案需事先布置使用场景等。

而基于激光雷达的同时定位与地图构建技术（simultaneous localization and mapping， SLAM）以其准确测量障碍点的角度与距离、无须预先布置场景、可融合多传感器、在光线较差环境工作、能够生成便于导航的环境地图等优势，成为目前定位方案中不可或缺的新技术。

激光 SLAM 任务是搭载激光雷达的主体于运动中估计自身的位姿，同时建立周围的环境地图。而准确的定位需要精确的地图，精确的地图则来自于准确的定位，定位侧重自身了解，建图侧重外在了解。

3. 激光SLAM关键模块

激光 SLAM 所需要的传感器一般有激光雷达（Lidar）、惯性测量单元（IMU）、里程计（Odometry）。通常室内采用二维激光雷达，室外采用三维激光雷达，里程计采用轮式里程计。

由于 IMU 具有较高的角速度测量精度，相比 IMU 里程计具有较高的局部位置测量精度，一般用 IMU 计算角度信息，里程计计算位置信息，配合激光雷达进行 SLAM 过程。

当前，激光 SLAM 框架一般分为前端扫描匹配、后端优化、闭环检测、地图构建四个关键模块。

前端扫描匹配是激光 SLAM 的核心步骤，工作内容是已知前一帧位姿并利用相邻帧之间的关系估计当前帧的位姿；前端扫描匹配能给出短时间内的位姿和地图，但由于不可避免的误差累积， 后端优化正是当长时间增量式扫描匹配后优化里程计及地图信息；闭环检测负责通过检测闭环而减少全局地图的漂移现象，以便生成全局一致性地图；地图构建模块负责生成和维护全局地图。

下面从算法的角度分别对四个模块进行总结。

3.1 前端扫描配准

目前在激光 SLAM 中主流的扫描匹配算法包括：迭代最临近点及变种、相关性扫描匹配、基于优化方法、正态分布变换、基于特征的匹配以及其他匹配算法。

迭代最临近点（Iterative Closest Point）及变种

ICP 算法利用待匹配的两帧点云欧式距离最小化，恢复相对位姿变换信息。 ICP 方法分为已知对应点的求解和未知对应点的求解两种，其中已知对应点的情况能够直接计算出 R 和 T 的闭式解，而未知对应点的求解需要进行迭代计算，是 EM 算法的一个特例。

算法流程如下图。

ICP算法在激光匹配中的缺点：没有考虑激光的运动畸变

ICP 变种算法Point-to-Line ICP（PL-ICP），算法流程如下图，适用于 2D 激光 SLAM。

ICP 变种算法Point-to-Plane ICP（PP-ICP），适用于 3D激光 SLAM。

ICP、 PL-ICP 与 PP-ICP 比较如下表所示。

相关性扫描匹配（Correlation Scan Match）

CSM 算法匹配的似然场模型高度非凸，存在很多局部极值。由于进行暴力匹配，排除初值敏感的影响，这是与前面三种匹配方法的最大区别。

CSM 算法流程如图所示，该方法是对环境分辨率细分的，精度会受限于分辨率。另外该种方法能够通过加速策略来降低计算量，例如分枝定界方法。

基于优化方法（Optimization-based Method）

给定一个目标函数，把激光数据扫描匹配问题建模成非线性最小二乘优化问题，该方法帮助限制误差的累积。基于优化的方法最大问题是对初值敏感，若初值选择恰当，由于对地图进行插值，建图精度往往会比较高。典型代表是Hector-SLAM中的匹配方法。

正态分布变换（Normal Distribution Transformation）

NDT 算法，最开始作为一种二维的匹配方法，由 Magnusson M 等人将该算法应用到三维的匹配中，是把地图看成很多高斯分布的集合，不需要通过搜索，直接最小化目标函数便能得到转换关系，计算量小，速度较快。NDT 方法在 3D 激光 SLAM 与纯定位中使用较多。

基于特征的匹配（Feature-based Method）

该方法通过匹配从激光扫描提取的特征点来改善计算成本，类似于视觉 SLAM 中的基于特征匹配的方法。Zhang J 等人在每次扫描中专注于边和平面特征并将它们保存在地图中以进行边线和平面与平面的匹配，该方法适用于 3D 激光 SLAM。

其他匹配方法

基于特定的采样策略和扫描到模型（scan-to-model）的匹配方式提出一种纯 3D 激光 SLAM 算法。

在结合深度学习方面，可以利用 VLP-16 采集 3D 点云数据，将其投影到 2D 平面生成深度图像，利用CNN 网络训练，得到端到端的匹配结果，其运行速度明显快于传统 ICP 匹配方法。

另外，针对无法获得较好初始条件的激光雷达数据匹配问题，提出了基于投影直方图和熵序列为迭代扫描匹配直接计算初始条件的新方法，将旋转和平移估计分解成一系列的相关性直方图。

目前，在 2D 激光 SLAM 中，最流行的匹配方法是 CSM与梯度优化结合使用，典型的开源方案是 Cartographer。在3D 激光 SLAM 中，最流行的匹配方法是基于特征的匹配，典型的开源方案是 LOAM。

3.2 后端优化

在激光 SLAM 中，对于解决遗留的局部误差累积的两种普遍的做法是基于滤波器的后端和基于图优化的后端。

基于滤波器

基于滤波器的激光 SLAM 是一个贝叶斯估计的过程。包括里程计预测、测量预测、进行测量、数据关联、状态更新以及地图更新。下面对几种滤波的形式进行总结。

在基于滤波器的激光 SLAM 中常常采用粒子滤波（PF）作为数学优化的框架， 当表达较小的尺寸环境时，利用不需要每个粒子的栅格地图，能够减少资源需求量，当需要更新栅格地图时，建议计算子图，在必要时更新，使得最终的图是所有子图的栅格化。

基于图优化

利用图论的方式来表示机器人 SLAM 过程，将机器人的位姿用节点（Node）表示，将节点之间的空间约束关系用边（Edge）表示，机器人在建图的过程中会积累误差，通过非线性最小二乘方法来优化建图过程中累积的误差，即优化的方式同时考虑所有帧间约束，迭代线性化求解。Bosse M等人介绍的基于图优化的方法、局部扫描到扫描的匹配以及基于子图特征的直方图的匹配应用在室外 SLAM 的系统。

激光 SLAM 目前最流行的后端优化方法是基于图优化的数学框架。

3.3 闭环检测

闭环问题是数据关联问题的重要一类，利用闭环这一约束条件来达成拓扑一致的轨迹地图，闭环检测的问题是激光数据扫描匹配结果相似性的问题。

1）帧与帧闭环检测（Scan-to-Scan）。

采用的相关性扫描匹配，通过旋转和平移判断两帧激光数据的相似性，达到闭环检测的效果。但由于单帧激光数据的信息量少，容易和其他相似度高的数据发生错误匹配，不适用 2D 激光 SLAM。

2）帧与子图闭环检测（Scan-to-Map）。

谷歌的 Cartographer 通过帧与子图的闭环检测来消除建图过程中产生的累积误差。由 Zakhor A 等人提出分段遗传扫描匹配算法（FGSM）利用二维的激光数据在闭环约束之间自动的估计变换，基于遗传搜索方法以及特殊建模离群值的距离度量，即使在近似的初始条件未知的情况下也能提供鲁棒的变换估计。

3）子图与子图闭环检测（Map-to-Map）。

该种检测方法改善了激光数据信息量少的缺点，将当前的 N 帧激光数据整合成局部子图，与之前的子图进行匹配。

对于全局匹配利用现有的直方图互相关技术，引入熵序列投影直方图和穷举相关方法在非结构化环境中实现可靠匹配，适用于大比例尺环境的地图构建。文国成等人提出采用子地图与子地图匹配的方法进行闭环检测，通过定位筛选和压缩表，对闭环检测匹配候选集优化，有效解决了在大尺度地图中匹配速度缓慢及误检的问题。

4）其他的闭环检测方法

Olson E 等人提出的像素精确的扫描匹配方法，可进一步减少局部误差的累积。虽然计算成本更高，但该方法对于环路的闭合检测也很有用。

在结合深度学习的方面，Himstedt M 等人采用基于直方图的匹配， Nieto J I 等人利用检测扫描数据中的特征点，并结合深度学习方法进行匹配。

5）闭环检测的加速策略

谷歌的 Cartographer 方案提出的分枝定界算法，分枝定界（Branch and Bound）算法是常用的树型搜索剪枝算法，这是一个对闭环检测加速的策略。最初由 Land A H 等人在求解整数规划问题背景下提出的， Clausen J 等人对分枝定界的原理和例子进行概述总结，把最优求解问题转换为树型搜索问题，对于搜索树中的每一个节点，确定以该节点为根节点的子树的上界进行搜索来达到减枝加速的目的。

目前，激光 SLAM 最流行的闭环检测方法是帧与地图之间的匹配，利用分枝定界策略加速匹配速度以及结合延时决策（Lazy Decision）保证匹配的准确性。

3.4 地图构建

在 SLAM 领域，构建的地图主要分为三类：尺度地图、拓扑地图以及语义地图。

为强调基于激光 SLAM 系统中建图的实时性，地图构建模块通常采用计算量较少的占据栅格建图算法，是基于贝叶斯估计的方式。

占据栅格地图把环境分解成一个个栅格，栅格是二元随机变量，有占据或空闲两种状态。占据栅格地图中的每一个栅格是独立的，估计环境的地图只需要对每一个独立的栅格进行估计，该算法对某一个栅格进行操作时， 只有加法计算，因此具有非常高的更新速度，更新地图时， 需要知道传感器的逆观测模型。占据栅格地图以周围环境是否被遮挡来鲜明区分可通行区域，适用于避障与导航路径规划。

目前，无论是 2D 激光 SLAM 还是 3D 激光 SLAM，应用最广泛的地图种类是占据栅格地图。另外实际应用中在评定基于激光 SLAM 建图的精度指标时可以分别与全站仪、钢尺、地面式激光扫描仪进行比较，将标靶球中心的点位误差作为一个精度指标，将特征物体尺寸的中误差作为一个精度指标，将点云模型的配准误差作为一个精度指标。

参考：

1. 刘浩敏, 章国锋, 鲍虎军. 基于单目视觉的同时定位与地图构建方法综述

2. Low K L. Linear least-squares optimization for point-to-plane ICP surface registration,

3. 文国成, 曾碧, 陈云华. 一种适用于激光 SLAM 大尺度地图的闭环检测方法

4. 《视觉SLAM十四讲：从理论到实践第2版》高翔等人

5. https://zhuanlan.zhihu.com/p/29411764

从零开始学习三维视觉核心技术SLAM，扫描查看介绍，3天内无条件退款

欢迎关注公众号：计算机视觉life，一起探索计算机视觉新世界~ 早就是优势，学习切忌单打独斗，这里有教程资料、练习作业、答疑解惑等，优质学习圈帮你少走弯路，快速入门！

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~