【光电智造】激光雷达SLAM算法综述

1.1 摘 要

即时定位与地图构建（simultaneous localization and mapping，SLAM）是自主移动机器人和自动驾驶的关键 技术之一，而激光雷达则是支撑SLAM算法运行的重要传感器。基于激光雷达的SLAM算法，对激光雷达SLAM总体框架进行介绍，详细阐述前端里程计、后端优化、回环检测、地图构建模块的作用并总结所使用的算法；按由2D到 3D，单传感器到多传感器融合的顺序，对经典的具有代表性的开源算法进行描述和梳理归纳；介绍常用的开源数据集，以及精度评价指标和测评工具；从深度学习、多传感器融合、多机协同和鲁棒性研究四个维度对激光雷达SLAM 技术的发展趋势进行展望。

1.2问题导向

SLAM最早是由Smith和Cheeseman于1986提出，发展至今已有30多年。激光雷达可 以直接测量距离，对环境的感知更加准确，可以获取物 体的空间位置和形状信息，构建高精地图进行精确定 位，对长时间运行的SLAM系统也更加可靠和稳定。激 光SLAM被广泛应用于室内导航、三维重建和自动驾驶。本文将系统地对激光SLAM进行综述，对相关的激光SLAM算法进 行分析总结。

传感器数据采集处理、前端里程计、后端优化、回环检测和地图构建。

2.1前端里程计

2.1.1点云去畸变

激光SLAM中的点云畸变是指点云的运动畸变：激光雷达在扫描过程中，载体机器人是在不断运动的，这就会导致同一帧中的点云数据，是由在不同位置下的激光雷达坐标系测量得到的。常见的点云运动畸变去除方法有纯估计法和传感器辅助法。

纯估计法
VICP（velocity updating ICP，VICP）[9]是ICP算法的变种，考虑了激光的运动畸变。VICP算法假设在 一帧激光雷达数据中机器人是匀速运动，对ICP估计的 位姿采用线性插值法对点云作运动补偿，将补偿后的数 据再次放入 ICP中求解，形成一个迭代闭环求解最优值。

传感器辅助法
传感器辅助法主要是利用高频率传感器如惯性测量单元（inertial measurement unit，IMU）极高测量频率的特点，直接测量角速度和线速度，得出每个时刻的位 姿，对点云作运动补偿

2.1.2点云配准

ICP
1.PP-ICP （point-to-plane ICP）原来计算点与点之间的距离改成计算的是点到面之间的距离，相对于 ICP的一阶收敛速度，PP-ICP是二阶的，收敛速度更快。

2.PL-ICP（point-to-line ICP）与PP-ICP类似，通过求取点线之间的最小距离使算法快速 收敛，提高匹配精度

3.NICP（normal ICP）将点云表面法向量信息加入到了 ICP 算法中，用于提高配准的准确度和鲁棒性

4.IMLS-ICP （implicit moving least square ICP）采用隐式函数来表示点云表面，并且通过最小化MLS逼近和隐式函数之间的距离来进行点云配准，能有效处理非刚性物体的配准问题

5.广义迭代最近点法（generalized ICP，GICP）引入一个全局参考框架来增强ICP算法的全局优化能力，解决了ICP算法运行缓慢、容易陷入局部最优解等问题

6.VGICP（voxelized GICP）是一种扩展的广义迭代最近点法，通过将点云转换为一组小体素网格，提出一种多点分布聚合方法估计体素分布，使匹配更加高效。

基于数学特征NDT

P2D-NDT（point-todistribution NDT）：NDT算法从2D推广到3D场景

基于极大团的配 准假设生成方法MAC：实现了 无需样本、无需训练、同时兼顾精度和效率的三维配准重建效果。

基于学习的方法

基于深度学习的点云配准方法通过数据驱动方式学习，能获得比人工设计更精准的模型，但其模型的前 期训练和模型部署需要消耗大量的GPU资源，暂时无 法实现在CPU上的实时计算运行。

1.FCGF提出一种全卷积几何特征，摆脱了先前SOTA方案需要低级特征输入的限制，但在对具有不同旋转分布的点云配准时效果不好；

2.SpinNet：由一个空间点转换器和一个基于3D卷积神经网络的特征提取器组成,能够提取点云表面特征,具有旋转不变性

3.Lepard：算法引入Transformer注意力机制，通过结合 RANSAC和ICP完成点云配准

4.REGTR：使用注意力机制取代显性特征匹配和RANSAC，提出一个包含自注意和交叉注意的 transformer网络架构

2.2 后端优化

2.2.1基于滤波

相比于传统的EKF，研究学者们往往更倾向于使用误差状态卡尔曼滤波器ESKF，相比于其他卡尔曼滤波算法，ESKF估计的是状态的误差量，维度较小且总是在 原点附近，离奇异点较远，不会由于离工作点太远而导致线性化近似不够的问题，在现有的开源算法中已展现其优秀的性能。

滤波算法缺点是无法校正累计误差，在噪声大、回环多的场景下长时间运行会产生严重的误差漂移。

2.2.1基于图优化

图优化（graph optimization）利用图论的方法对系统进行建模求解。因子图优化（factor graph optimization）在图优化的基础上将图划分为变量节点和因子节点。因子图用变量节点表示问题的状态变量，用 因子节点连接它们并表示约束条件，利用迭代最小化算 法，在因子图上反复更新变量节点和因子节点的估计值，直至达到收敛状态。源非线性优化算法库有

Ceres-Solver
G2O
iSAM
GTSAM
SE-Sync
SE

2.3 回环检测

回环检测模块用于消除累计误差，提高定位和地图精度，对在大环境、回环多、长时间下运行的SLAM系统至关重要。

Scan Context++
Scan Context
FPFH

2.4 地图构建

2D地图用栅格
3D地图形式如下

3.1 2D激光SLAM

Fast-SLAM：采用粒子滤波技术， 可以处理非线性系统。大环境下或者在里程计误差较大时需要更多的粒子才能得到较好的估计，这可能会造成内存 爆炸，而粒子的重采样会导致粒子耗散。

GMapping：通过降低粒子数量的 方法大幅度缓解内存爆炸；采用选择性重采样方法，对粒子进行重要性权重排序，对权值低的粒子进行重采样，解决RBPF算法中粒子耗散问题。基于滤波的算法且无 回环检测，长时间运行存在较大累计误差，无法在室外 构建大环境地图。

Karto：第一个基于图优化的开源SLAM算法

Hector-SLAM：创新性地采用高斯牛顿法直接进行帧-图匹配（scan-to-map）优化，构建栅格地图。缺少闭环检测，没有修正能力

Cartographer：是目前精度最高、实时性、鲁棒性最好、二次开发最为便捷的开源SLAM算法之一。引入子图（submap）概念，使用 CSM和梯度优化相结合的方法使帧与子图进行匹配。后端基于图优化算法，将当前激 光帧和之前建立的所有子图都加入闭环检测模块，采用分支定界法提升搜索速度。

## 3.2 3D激光SLAM

LOAM：根据曲率大小将特征点分类为边缘特征点和平面特征点，缺少后端优化和回环检 测模块。

A-LOAM：使用Eigen 矩阵库（https：//eigen.tuxfamily.org）和Ceres-Solver非线性优化 库代替原本复杂的数学推导

LeGO-LOAM：引入关键帧概念，使用关键帧及其局部范围内的数据帧组成 loop-submap。基于地面点的优化更加鲁棒精确，计算量更小

Loam_livox：针对Livox新型固态激光雷达扫描特性，对有效点筛选和特征提取部分进行优化

NASA提出一种基于稠密点云快速定位的直接激光里程计将关键帧和相关联点云储存在字典中，节约帧图匹配的计算资源和降低基于关键 帧构成的子图所含有的重复信息

CP-ICP：是一种弹性的带回环的激光雷达里程计法，考虑了帧内数据的连续性和帧间数据的非连续性。

Intensity-SLAM：利用强度信息构建强度地图，协助特征点提取和位姿估计

提出一种基于强度信息的纯激光雷达 SLAM算法，算法的关键在于利用点云的强度信息生成一张强度图像，后续的特征提取和配准、回环检测与位 姿图估计都是基于强度图像进行的

3.3 多传感器融合SLAM

相较于纯激光雷达方案，使用激光雷达与IMU进行信息融合，利用IMU高频率输出运动信息，校正点云运动畸变，提供一个良好的初值可以使算法避免陷入局部最小值，提高算法精度和减少计算量。

激光雷达与IMU融合可以分为紧耦合和松耦合

松耦合存在两个优化过程，它将激光雷达数据和IMU数据分别进行参数估计，再使用估计出的参数进行融合

紧耦合表现出更好的鲁棒性和准确性，它使用激光雷达数 据和 IMU数据共同构建参数向量，再进行优化和估计。

LIO-SAM：提取特征点，并使用IMU数据 校正点云畸变，提供数据帧之间位姿变换的初始值；后端采用因子图优化架构，消除累计误差，进行全局优化。

LVI-SAM：由激光-惯性系统和视觉-惯性系统组成。LIS为VIS提供准确的深度信息，提 高VIS精度；反过来LIS利用VIS的初步位姿估计进行扫描匹配。

R2LIVE：使用误差状态迭代卡尔曼滤波融合三个传感器的测量值进行状态估计，并通过因子图进一步优化提高整体精度，但其视觉系统采用特征点法，时间开销大且在非结构化环境下易失效。

R3LIVE：使用光流法代替原本的特征点法进行帧间跟踪，通过最小化帧到 地图的光度误差融合视觉数据，并为地图渲染RGB颜色。

R3LIVE++：在R3LIVE++基础上，考虑了相机光度校准和对相机曝光时间的在线估计，进一步提高了定位和建图的精度。

FAST-LIO：使用激光雷达与 IMU 紧耦合的误差状态迭代卡尔曼滤波算法。提 出一个新的计算卡尔曼增益的公式，使计算量不再依赖于测量维数，而是依赖于状态维数，极大地减少了计算量。

FAST-LIO2：为了使算法能自然适应不同扫描模式的激光雷达和实现更 快的计算，省略了耗时的特征提 取模块，采用直接法将所有点云数据统一处理。

Faster-LIO：在FAST-LIO2的基础上使用一种增量式稀疏体素（iVox）数据结构代替ikd-tree，可以有效降低点云配准的时间

FAST-LIVO：在FAST-LIO2的LIO系统基础上使用相机传感器加入VIO子系统，在测量层面实现两个传感器的耦合，在激光雷达退化场景和光线变化剧烈的场景下依旧能可靠运行。

Point-LIO：以单个点云为数据处理单位，更新每个点的状态， 具有极高的里程计输出频率和带宽，在自身运动存在严重振动和高角速度高线速度的情况下也能鲁棒运行，从根本上解决了运动失真。

3.4 基于深度学习的3D激光SLAM

深度学习与激光雷达SLAM的结合主要应用于系统中的几个模块，如点云的特征提取和配准、回环检测、构建语义.

特征提取：FCGF、SpinNet
配准方法：Lepard、 REGTR
回环：OverlapNet、Overlap-Transformer

SUMA++：基于激光雷达的语义 SLAM，使用 RangeNet++[70]对点云进行语义分割，根据语义信息剔除动态目标，再将语义信息融入系统，与几何信息一起建 立约束，从而提高定位和建图精度。

SA-LOAM：提出一种语义辅助的 ICP算法，并在闭环检测模块中集成基于语义图的位置识别方法，适合在大规模场景中构建全局一致性语义地图。

ASL-SLAM：提出一种基于活动语义的回环检测方法，通过IMU传感器对Z轴角速度和加速度的分析来检测转弯和通过减速带两种语义。

提出一种自监督的激光里程计，在投影的强度图像上使用SuperPoint提取特征点，使用传统ICP方法估算位姿后用于在线训练网络

EfficientLO-Net：是第一个完全端到端的高效3D激光雷达里程计框架，提出了基于投影感知的三维点云表示 方法和特征学习方法

4.1 数据集

KITTI：包含视觉图像、激 光雷达点云、IMU、GPS等数据

SemanticKITTI：在KITTI数据集的基础上对点云数据进行标注，分类为28类语义，实现全场景分割

ApolloScape：数据集使用了高精度地图采集车，是目前行业内环境最复杂、标注最精准、数据量最大的三维 自动驾驶公开数据集

NTU-VIRAL：搭载激光雷达、相机、IMU和超带宽填补了SLAM领域无人机数据集的空白

TUM-RGBD

EuRoC

Oxford RobotCar

USVInland

4.2评价指标

（1）绝对轨迹误差（absolute trajectory error，ATE）用于衡量估计的轨迹与真实轨迹之间的误差，可以非常 直观地反映算法精度和轨迹全局一致性。首先，将估计 轨迹与真实轨迹进行时间戳对齐，再计算每个时间步估 算的位姿矩阵与真实位姿矩阵之间的欧式距离，最后对 所有时间步的相对位姿误差取均方根误差（root mean square error，RMSE）得到ATE。

（2）相对位姿误差（relative pose error，RPE）用于衡量估计的相对位姿变换与真实相对位姿变换之间的误 差，适合估计系统的漂移。同样的，RPE第一步也是进 行时间戳对齐，接着对每一段时间间隔计算估计值的相 对位姿矩阵和对应真实值的相对位姿矩阵，最后使用 RMSE求平均值得到RPE。

5.1 结合深度学习

深度学习已经在计算机视觉领域取得了巨大成功，在SLAM中也有巨大的潜力。深度学习可以用于改善SLAM中的关键技术，比如特征提取、匹配、姿态估计等方面。例如，使用深度学习进行语义分割，可以提取更加语义丰富的地图信息；使用深度学习进行图像去噪或者对抗扰动，可以提高SLAM系统在复杂环境下的鲁棒性。

5.2 多传感器融合

多传感器融合是提高SLAM系统精度和鲁棒性的关键。将来，SLAM系统可能会结合更多种类的传感器，如高精度惯性测量单元（IMU）、全景相机、多频段激光雷达等，以获取更丰富的环境信息。同时，如何有效地融合这些异构传感器的数据，是未来研究的重点之一。

5.3 分布式多智能体协同作业

随着智能体数量的增加和应用场景的复杂化，分布式多智能体协同作业在SLAM中的应用也将变得更加重要。未来的SLAM系统可能会由多个智能体组成，它们分布在不同位置，共同完成地图构建和定位任务。这就需要研究分布式算法和通信机制，以实现多智能体之间的协同工作。

5.4 特殊场景下的鲁棒性研究

SLAM系统在特殊场景下的鲁棒性是一个挑战，比如光照变化大、大规模动态物体存在、传感器故障等情况。未来的研究可能会集中在如何提高SLAM系统在这些特殊场景下的鲁棒性，可能通过引入更多的传感器、优化算法或者结合深度学习等方法来解决这些问题。

本文仅供学习交流，如有侵权请联系后台删除。