激光雷达SLAM算法综述

刘铭哲，徐光辉，唐 堂，钱晓健，耿 明

陆军工程大学 通信工程学院，南京 210000

即时定位与地图构建技术可以描述为无人装备在未知环境中，通过本体携带的传感器进行数据采集，进行位姿估计和定位，构建增量式地图，从而实现自主定位和导航。SLAM 最早是由Smith 和Cheeseman[1]于1986提出，发展至今已有30 多年。根据使用的传感器类别不同，当前主流的SLAM 系统主要分为激光SLAM、视觉SLAM以及各类传感器辅助激光/视觉的多传感器融合SLAM技术。视觉SLAM以相机为基础传感器，其获取信息丰富、轻量、低成本的特点受到研究者的青睐。视觉SLAM系统需要处理大量图像数据，且由于相机对光线较为敏感和无法直接测距的缺点，导致视觉SLAM在光照条件不理想的情况下无法正常工作、精确度不高且系统的实时性难以保证。以激光雷达为主要传感器的激光SLAM，可以很好地解决上述问题。激光雷达可以直接测量距离，对环境的感知更加准确，可以获取物体的空间位置和形状信息，构建高精地图进行精确定位，对长时间运行的SLAM系统也更加可靠和稳定。激光SLAM被广泛应用于室内导航、三维重建和自动驾驶等领域，是人工智能时代的研究热点。

激光SLAM 主要分为2D 激光SLAM 和3D 激光SLAM。激光雷达根据激光线数，可以分为单线激光雷达和多线激光雷达两种类型。单线激光雷达也称为一线扫描激光雷达，是2D激光SLAM的主要传感器，通过单个探测器从一个角度扫描环境，只能获取一个平面的点云数据，无法获取三维物体的高度信息，因此应用场景有限，主要应用于室内地面环境，扫地机器人是2D激光SLAM的一个典型应用。多线激光雷达应用于3D激光SLAM，采用多个探测器同时从不同的角度扫描环境，可以获取含有三维物体坐标信息的点云数据。多线激光雷达根据探测器数目，即线数的不同，又可分为16、32、64、128等不同型号；随着线数增加，获取点云数据的速度越快、数据量更丰富，建立的地图精度越高。多线激光雷达较为昂贵，主要应用于自动驾驶等需要进行高精度环境感知和地图构建的领域。固态激光雷达是一种新型的激光雷达传感器，与传统的机械扫描式激光雷达不同，它是一种窄视野（field of view，FoV）的激光雷达，通过光学相位阵列或光电子扫描来实现宽视角的测量。固态激光雷达具有低成本、小尺寸、高可靠性、高稳定性、高速的优点，被认为是未来激光雷达的发展趋势之一。图1是Livox Mid40扫描轨迹图。

图1 固态激光雷达Livox Mid40扫描轨迹（颜色代表采样时间）Fig.1 Solid-state LiDAR Livox Mid40 scan trajectory (colors represent sampling time)

经过调研，近年来关于SLAM 的综述文章逐渐增多，这是因为自动驾驶、智能制造、机器人等人工智能产品正高速发展，但其中大部分是关于视觉SLAM 的文章[2-4]，而关于激光SLAM的文章则较少[5-7]。本文将系统地对激光SLAM进行综述，对相关的激光SLAM算法进行分析总结。

1 SLAM系统框架

SLAM系统框架（图2）大致分为五个部分：传感器数据采集处理、前端里程计、后端优化、回环检测和地图构建。激光雷达及其他辅助传感器采集数据，交由前端里程计处理分析，快速估算出相邻激光雷达数据帧之间的位姿变换，此时计算出的位姿含有累计误差，不够准确；后端优化负责全局轨迹优化，得出精确位姿，构建全局一致性地图。在此过程中，回环检测一直在执行，它用于识别经过的场景，实现闭环，消除累计误差。本章主要针对后四部分进行阐述，对数据采集部分不做过多介绍。

1.1 前端里程计

1.1.1 点云去畸变

激光SLAM中的点云畸变是指点云的运动畸变：激光雷达在扫描过程中，载体机器人是在不断运动的，这就会导致同一帧中的点云数据，是由在不同位置下的激光雷达坐标系测量得到的。点云畸变会导致较大的定位误差，使系统不稳定。常见的点云运动畸变去除方法有纯估计法和传感器辅助法。迭代最近点（iterative closest point，ICP）[8]是一种经典的纯估计方法，由给定的两个点云集合通过最小二乘法迭代求解位姿变换：

其中，R为所求的旋转矩阵，t为所求的平移向量，它们通过不断迭代求解得到。事实上ICP 算法并没有考虑点云的运动畸变，而是通过两组点云集合直接估计位姿变换。VICP（velocity updating ICP，VICP）[9]是ICP 算法的变种，考虑了激光的运动畸变。VICP 算法假设在一帧激光雷达数据中机器人是匀速运动，对ICP估计的位姿采用线性插值法对点云作运动补偿，将补偿后的数据再次放入ICP 中求解，形成一个迭代闭环求解最优值。事实上，纯估计方法在本质上未能解决点云的运动畸变问题，而是使用一些技巧和方法尽可能减小点云运动畸变带来的影响。要较好地解决点云运动畸变所带来的影响，引入外部传感器进行辅助是当前的最优方案。传感器辅助法主要是利用高频率传感器如惯性测量单元（inertial measurement unit，IMU）极高测量频率的特点，直接测量角速度和线速度，得出每个时刻的位姿，对点云作运动补偿。

1.1.2 点云配准

前端里程计的另一个重要功能是进行点云配准，计算相邻数据帧之间位姿。采用的配准算法主要是ICP及其变种算法、基于数学特征NDT（normalized distribution transform，NDT）类方法和基于学习的方法。PP-ICP（point-to-plane ICP）[10]基于ICP 算法，将原来计算点与点之间的距离改成计算的是点到面之间的距离，相对于ICP的一阶收敛速度，PP-ICP是二阶的，收敛速度更快，适用于3D 激光SLAM；PL-ICP（point-to-line ICP）[11]与PP-ICP类似，通过求取点线之间的最小距离使算法快速收敛，提高匹配精度，适用于2D和3D激光SLAM；NICP（normal ICP）[12]将点云表面法向量信息加入到了ICP算法中，用于提高配准的准确度和鲁棒性；IMLS-ICP（implicit moving least square ICP）[13]采用隐式函数来表示点云表面，并且通过最小化MLS逼近和隐式函数之间的距离来进行点云配准，能有效处理非刚性物体的配准问题；广义迭代最近点法（generalized ICP，GICP）[14]通过引入一个全局参考框架来增强ICP算法的全局优化能力，解决了ICP算法运行缓慢、容易陷入局部最优解等问题；VGICP（voxelized GICP）[15]是一种扩展的广义迭代最近点法，通过将点云转换为一组小体素网格，提出一种多点分布聚合方法估计体素分布，使匹配更加高效。

NDT[16]是一种基于数学特性的点云配准算法，利用高斯分布对离散点云数据进行建模，将点云转化为一个点分布函数，进而计算点云之间的相对位姿关系，具有速度快，稳定性好，精度高的优点；Magnusson 等[17]将NDT算法从2D推广到3D场景，提出P2D-NDT（point-todistribution NDT）。文献[18]首次提出基于极大团的配准假设生成方法MAC（maximal cliques，MAC），实现了无需样本、无需训练、同时兼顾精度和效率的三维配准重建效果。

传统的点云配准方法尽管在某些场景取得了不错的效果，但依然没有一种能够适应全场景的点云配准方案。由于深度学习领域的快速发展，基于深度学习的点云配准的方案成为研究者们新的研究方向。FCGF[19]提出一种全卷积几何特征，摆脱了先前SOTA方案需要低级特征输入的限制，但在对具有不同旋转分布的点云配准时效果不好；SpinNet[20]由一个空间点转换器和一个基于3D 卷积神经网络的特征提取器组成，能够提取点云表面特征，具有旋转不变性。Li等[21]提出了一种利用三维位置知识，可用于刚体和可变性场景下的点云配准算法Lepard；算法引入Transformer 注意力机制，通过结合RANSAC和ICP完成点云配准；局限性在于Lepard是一种粗匹配方法，精细度有待提高，在数据低重叠情况下具有挑战性。Yew 等[22]使用注意力机制取代显性特征匹配和RANSAC，提出一个包含自注意和交叉注意的transformer 网络架构REGTR，直接预测点云刚性变换，解决了点云降采样带来的分辨率问题，算法在3DMatch数据上取得了最先进的性能。

基于深度学习的点云配准方法通过数据驱动方式学习，能获得比人工设计更精准的模型，但其模型的前期训练和模型部署需要消耗大量的GPU 资源，暂时无法实现在CPU上的实时计算运行。关于其他的激光扫描匹配方法，文献[23]中有详细介绍，表1对上述点云配准方案进行归纳总结。

表1 点云配准方案归纳Table 1 Summarization of point cloud registration methods

1.2 后端优化

1.2.1 基于滤波

滤波器优化算法主要是基于贝叶斯估计，通过先验概率、传感器测量和状态转移模型来计算后验概率。

在状态估计中，常使用的滤波器算法有卡尔曼滤波（Kalman filter，KF）、扩展卡尔曼滤波（extended Kalman filter，EKF）、无迹卡尔曼滤波（unscented Kalman filter，UKF）、状态误差卡尔曼滤波（error-state Kalman filter，ESKF）、粒子滤波（particle filter，PF），表2 对这几种滤波器做了总结。

表2 状态估计中的滤波器Table 2 Filters in state estimation

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

1.2.2 基于图优化

滤波算法缺点是无法校正累计误差，在噪声大、回环多的场景下长时间运行会产生严重的误差漂移。图优化（graph optimization）利用图论的方法对系统进行建模求解，是当前大多数SLAM 算法所采用的优化方案，被广大研究者所喜爱。因子图优化（factor graph optimization）[24]是一种特殊的图优化方法，针对概率模型应用优化；在图优化的基础上将图划分为变量节点和因子节点。因子图用变量节点表示问题的状态变量，用因子节点连接它们并表示约束条件，利用迭代最小化算法，在因子图上反复更新变量节点和因子节点的估计值，直至达到收敛状态。

当前常用的开源非线性优化算法库有Ceres-Solver（http：//ceres-solver.org）、G2o（general graph optimization）[25]、iSAM（incremental smoothing and mapping）[26]、GTSAM（Georgia tech smoothing and mapping）[27]、SE-Sync（synchronization over special Euclidean group SE（n））[28]，它们都是基于C++语言编写的，利用这些开源算法库，可以使开发者专注于系统图模型的构建而非具体的数学计算过程，达到简便、高效开发的目的。文献[29]对最常用的四种优化方法库G2o、Ceres-Solver、GTSAM和SE-Sync进行实验评估，结论归纳见表3。

表3 四种非线性优化库Table 3 Four nonlinear optimization libraries

1.3 回环检测

回环检测模块用于消除累计误差，提高定位和地图精度，对在大环境、回环多、长时间下运行的SLAM系统至关重要。基于点云扫描匹配的方法也可以作用于回环检测部分，通过检测两个激光雷达数据帧中点云数据之间的相似性来判断它们是否来自同一位置。由于一帧点云数据量较大，含有的信息复杂不够直观，所以多采用的是基于特征点提取的方法，匹配特征点以提高回环检测的准确率和鲁棒性。Kim 等[30]提出基于扫描上下文的方法Scan Context，这是一种非直方图的全局描述符，它分割每一帧点云为若干区域，选取具有代表性的点保存到二维数组中，通过kd-tree[31]查找历史Scan Context 描述子进行相似度计算，检测是否回环。Scan Context算法的优点在于它具有较低的计算复杂度和较高的鲁棒性，它能够处理不同视角、噪声和局部遮挡等现实世界中的挑战，并在大规模环境中实现高效的回环检测。Scan Context++[32]是Scan Context 的改进版，在Scan Context的旋转不变性基础上，增加了平移不变性，并且提升了速度；快速点特征直方图（fast point feature histograms，FPFH）[33]在PFH（point feature histograms）[34]的基础上改进而来，利用局部范围内点的表面法向量计算局部描述子，具有旋转不变性和尺度不变性，计算速度快。基于深度学习的回环检测首先应用于视觉SLAM 领域，因为相机可以采集到丰富的语义信息；近年来，有不少学者将语义信息融入激光雷达回环检测模块中，实现鲁棒的效果，关于这部分内容，将在后面激光雷达SLAM结合深度学习的章节（2.2.3小节）进行介绍。

1.4 地图构建

SLAM 中的地图随着位姿和坐标点的优化而增量式生成，实际上就是坐标点的集合。图3展示了3D激光SLAM常用的三种地图表达形式。对于2D激光SLAM来说，一般使用的是栅格地图：将真实环境划分为一个个网格，用概率0～1表示该网格在实际环境中被物体占据的概率；3D 激光SLAM 构建的是3D 点云地图，使用体素滤波器[35]进行降采样。Ruan等[36]在SLAMesh中提出一种基于高斯过程重构和顶点连接的网格划分策略，实现网格地图的快速重建；八叉树地图（OctoMap）[37]是一种基于八叉树数据结构（Octree）的三维空间地图表示方法，通过将三维空间递归分解为8个相等的子单元来高效地处理和存储大规模环境信息；语义地图通过语义分割将环境中的物体进行识别分类，使机器人能更好地感知环境，这也是SLAM 建图模块的发展方向。文献[38]提出将建图过程分为地图初始化和移动扫描两个过程，通过创建静态的初始地图来提升移动扫描过程中定位与建图的精度，但该方法只适用于室内静态环境。

图3 地图形式Fig.3 Map forms

2 激光SLAM方案

2.1 2D激光SLAM

Fast-SLAM[39]是一种基于粒子滤波器的SLAM 算法，由Thrun 等于2002 年提出；Fast-SLAM 提出RBPF（Rao-Blackwellised particle filter）算法，将SLAM 问题分解为两个相互独立的子问题：机器人位姿估计和地图坐标点的后验概率分布估计；其中，机器人位姿估计使用粒子滤波器实现，地图坐标后验概率分布则通过扩展卡尔曼滤波估计。Fast-SLAM由于采用粒子滤波技术，可以处理非线性系统。

Fast-SLAM在大环境下或者在里程计误差较大时需要更多的粒子才能得到较好的估计，这可能会造成内存爆炸，而粒子的重采样会导致粒子耗散。GMapping[40]在Fast-SLAM的基础上进行改进：通过降低粒子数量的方法大幅度缓解内存爆炸；采用选择性重采样方法，对粒子进行重要性权重排序，对权值低的粒子进行重采样，解决RBPF算法中粒子耗散问题。GMapping算法在室内环境下实时性好，精度高，是早期乃至现在最常用的SLAM算法之一。GMapping是基于滤波的算法且无回环检测，长时间运行存在较大累计误差，无法在室外构建大环境地图。

Karto[41]是第一个基于图优化的开源SLAM算法，并且认识到系统的稀疏性。前端部分，当前数据帧与相邻数据帧组成局部地图，采用相关性扫描匹配（correlative scan matching，CSM）[42]方法让数据帧与局部地图匹配（scan-to-map）计算位姿；后端部分采用基于图优化的方法，进行全局优化；闭环检测使用候选数据帧构建局部地图，采用scan-to-map方式进行匹配，检测闭环。

Hector-SLAM[43]不需要里程计信息，创新性地采用高斯牛顿法直接进行帧-图匹配（scan-to-map）优化，构建栅格地图。但由于缺少回环检测，其对地图没有修正能力，一旦地图出错将影响后续的匹配和建图效果；scan-to-map 计算量较大，为保证获取较好的建图效果，需要机器人以较低的速度移动。

Google在Karto的基础上进行改进，于2016提出并开源Cartographer[44]算法，加入传感器同步、位姿外推器、激光数据预处理功能；引入子图（submap）概念，使用CSM 和梯度优化相结合的方法使帧与子图进行匹配，提升匹配精度和效率；后端基于图优化算法，将当前激光帧和之前建立的所有子图都加入闭环检测模块，采用分支定界法（branch and bound）[45]提升搜索速度。Cartographer是目前精度最高、实时性、鲁棒性最好、二次开发最为便捷的开源SLAM 算法之一。表4 对2D 激光SLAM算法进行归纳。

表4 2D激光SLAM方案Table 4 2D LiDAR SLAM

2.2 3D激光SLAM

2.2.1 纯激光里程计

Zhang 等[46]于2014 年提出的LOAM（lidar odometry and mapping），是经典且具有代表性的算法，后续许多激光SLAM 算法都借鉴其思想。图4 是LOAM 算法框架，它采用特征点法，利用局部范围内的点计算点的曲率，根据曲率大小将特征点分类为边缘特征点和平面特征点；算法对激光雷达做匀速运动假设，使用线性插值法进行运动补偿。香港科技大学Qin 根据LOAM 论文复现代码，开源A-LOAM 算法，使用Eigen 矩阵库（https：//eigen.tuxfamily.org）和Ceres-Solver 非线性优化库代替原本复杂的数学推导，代码简洁易懂，适合初学者学习。LOAM 算法的缺点是缺少后端优化和回环检测模块，在大规模场景和回环较多场景下会产生漂移，精度会大幅度降低。2018年，Shan等[47]在LOAM的基础上加入回环检测功能，提出并开源LeGO-LOAM，引入关键帧概念，使用关键帧及其局部范围内的数据帧组成loop-submap，这样可以极大地减少计算量，过滤冗余数据，让当前数据帧与loop-submap进行匹配，达到回环检测的目的。LeGO-LOAM是一个轻量级的SLAM算法，比LOAM 更加鲁棒精确，计算量更小，但它也有局限性，相比于LOAM，LeGO-LOAM 提取特征的策略是将三维点云投影到二维图像上，以此来分离地面点和非地面点，去除噪声，这就使得算法对地面环境要求苛刻。作者后期在算法中以松耦合的方式融合了IMU 传感器，但相比于其他紧耦合算法，松耦合的提升有限。

图4 LOAM算法框架Fig.4 Algorithm framework for LOAM

Loam_livox[48]由香港大学Lin提出，针对Livox新型固态激光雷达扫描特性，对有效点筛选和特征提取部分进行优化，对后续其他基于固态激光雷达的SLAM方案具有重要参考意义。NASA 提出一种基于稠密点云快速定位的直接激光里程计[49]，将关键帧和相关联点云储存在字典中，节约帧图匹配的计算资源和降低基于关键帧构成的子图所含有的重复信息；同时提出一种定制的迭代最近点解算器NanoGICP，用于轻量级点云配准，具有跨对象数据共享功能；大量实验证明，该算法能够在高速条件下实时处理稠密点云数据，提高定位精度。使用匀速模型校正点云畸变的做法虽然在大多数驾驶场景下表现不错，但无法适应方向和速度快速变换的场景，CP-ICP[50]是一种弹性的带回环的激光雷达里程计算法，考虑了帧内数据的连续性和帧间数据的非连续性，以实现对高频剧烈运动更鲁棒的目的；但其回环检测部分约束过多，限制了传感器的运动，在某些应用场景具有局限性。

传统的激光雷达SLAM 方法主要关注几何信息和时间信息，而往往会忽略激光雷达反射的强度信息。Wang等认为一个鲁棒的激光雷达SLAM系统应该考虑强度信息，提出强度信息辅助的Intensity-SLAM[51]，利用强度信息构建强度地图，协助特征点提取和位姿估计；在回环模块使用基于强度信息的Scan Context描述符，对旋转具有鲁棒性。Li 等[52]同样利用强度信息增强激光雷达在退化环境下的可靠性，为了避免边缘强度特征点退化为平面点，采用基于图像处理的线型图管理模块来剔除边缘强度点异常值，实现退化环境下的鲁棒性运行。文献[53]提出一种基于强度信息的纯激光雷达SLAM算法，算法的关键在于利用点云的强度信息生成一张强度图像，后续的特征提取和配准、回环检测与位姿图估计都是基于强度图像进行的。

2.2.2 多传感器融合SLAM

相较于纯激光雷达方案，使用激光雷达与IMU 进行信息融合，利用IMU高频率输出运动信息，校正点云运动畸变，提供一个良好的初值可以使算法避免陷入局部最小值，提高算法精度和减少计算量。激光雷达与IMU融合可以分为紧耦合和松耦合；松耦合存在两个优化过程，它将激光雷达数据和IMU 数据分别进行参数估计，再使用估计出的参数进行融合；相较于松耦合，紧耦合表现出更好的鲁棒性和准确性，它使用激光雷达数据和IMU 数据共同构建参数向量，再进行优化和估计。紧耦合也是目前多传感器融合的研究热点。

LIO-SAM[54]算法是经典的基于平滑和建图（smoothing and mapping）的紧耦合激光-惯性里程计。算法里程计部分延续LOAM的思想，提取特征点，并使用IMU数据校正点云畸变，提供数据帧之间位姿变换的初始值；后端采用因子图优化架构（图5），消除累计误差，进行全局优化。由于LIO-SAM 算法是基于特征点法，且激光雷达传感器本身具有局限性，导致算法在退化场景和非结构场景下会缺乏有效观测而失效。要解决激光雷达在退化场景无法正常工作的问题，可以考虑加入相机传感器，相机可以采集到丰富的语义信息，弥补激光雷达的缺陷。由激光雷达、IMU、相机组成的多传感器融合是当前最受欢迎的组合，可以应对绝大部分场景；它通常由激光-惯性里程计（lidar-inertial odometry，LIO）和视觉惯性里程计（visual-inertial odometry，VIO）通过IMU紧密结合，达到紧耦合目的（图6[55]）。LIO-SAM作者在LIO-SAM中加入相机传感器，提出激光-视觉-惯性紧耦合的系统LVI-SAM[56]。LVI-SAM由激光-惯性系统（lidarinertial system，LIS）和视觉-惯性系统（visual-inertial system，VIS）组成；LIS 为VIS 提供准确的深度信息，提高VIS 精度；反过来LIS 利用VIS 的初步位姿估计进行扫描匹配。算法的优点在于VIS或LIS其中一个发生故障，LVI-SAM也能正常工作，这使得算法在纹理较少和缺少特征的环境下具有较好的鲁棒性。同样的激光-惯性-视觉里程计还有香港大学Lin等[57]提出的R2LIVE，使用误差状态迭代卡尔曼滤波融合三个传感器的测量值进行状态估计，并通过因子图进一步优化提高整体精度，但其视觉系统采用特征点法，时间开销大且在非结构化环境下易失效。为了提高算法的实时性和精度，作者在改进版R3LIVE[58]中对视觉系统进行重构，使用光流法代替原本的特征点法进行帧间跟踪，通过最小化帧到地图的光度误差融合视觉数据，并为地图渲染RGB 颜色。实验证明，R3LIVE 实现目前最先进的多传感器融合算法精度和鲁棒性。R3LIVE++[59]在R3LIVE基础上，考虑了相机光度校准和对相机曝光时间的在线估计，进一步提高了定位和建图的精度。

图5 LIO-SAM算法因子图框架Fig.5 Factor graph framework of LIO-SAM

图6 激光视觉惯性紧耦合系统架构Fig.6 Lidar-visual-inertial tightly coupled system architecture

为了应对快速运动、噪声和杂波环境条件，Xu等[60]提出一个高计算效率和鲁棒性强的紧耦合激光-惯性里程计框架FAST-LIO。FAST-LIO 使用激光雷达与IMU紧耦合的误差状态迭代卡尔曼滤波算法，IMU数据前向传播状态估计量，反向传播对点云数据作运动补偿；提出一个新的计算卡尔曼增益的公式，使计算量不再依赖于测量维数，而是依赖于状态维数，极大地减少了计算量。FAST-LIO2[61]在FAST-LIO的基础上进行改进，为了使算法能自然适应不同扫描模式的激光雷达和实现更快的计算，省略了耗时的特征提取模块，采用直接法将所有点云数据统一处理，计算点面残差；使用一种新的数据结构动态kd-Tree——ikd-Tree[62]存储点云数据，与现有的动态数据结构（octree、R*-tree 和nanoflann k-d tree）相比，它有更好的整体性能。大量实验证明，FAST-LIO2 相较于其他激光-惯性紧耦合方案具有更高效率，在速度方面有巨大提升，在高速环境下鲁棒性更好。但在大范围场景下，点云数量庞大，ikd-Tree 维护树结构动态平衡的时间不可忽略，这也决定了FAST-LIO2无法实时实现大规模场景下的建图任务。Faster-LIO[63]在FAST-LIO2的基础上使用一种增量式稀疏体素（iVox）数据结构代替ikd-tree，可以有效降低点云配准的时间，也不会大幅度影响里程计的精度表现，能够达到FAST-LIO2 的1.5～2 倍的速度。Zheng 等[64]提出的激光-惯性-视觉紧耦合算法FASTLIVO在FAST-LIO2的LIO系统基础上使用相机传感器加入VIO 子系统，在测量层面实现两个传感器的耦合，在激光雷达退化场景和光线变化剧烈的场景下依旧能可靠运行；但在剧烈运动场景中，融合模糊的图像对里程计精度并没有帮助，在低速运动场景下更能体现其优势。FAST-LIVO 可以在Intel 和ARM 处理器上实时运行，并有着显著的计算余量。

目前为止的激光雷达SLAM算法处理点云的方式，都是根据激光雷达的扫描模式，将一定时间内扫描的点云封装组帧，以点云数据帧为单位进行后续处理。这种组帧的过程不可避免地会使点云产生运动畸变。虽然使用IMU 测量补偿运动畸变是一种有效的方法，但它也受限于IMU 测量频率、IMU 测量噪声和偏置估计。He 等[65]提出一种新颖的激光惯性里程计算法Point-LIO，以单个点云为数据处理单位，更新每个点的状态，具有极高的里程计输出频率和带宽，在自身运动存在严重振动和高角速度高线速度的情况下也能鲁棒运行；并且因为没有人工组帧的过程，所以从根本上消除了运动失真。Point-LIO 也具有一些缺陷，以单个点云为处理单位难以建立点与点之间的关系，一些离群点的加入会影响算法精度，并且无法实时利用点云的语义信息。

关键帧的生成对SLAM算法的性能有很大影响，选取正确的关键帧可以节约计算资源，剔除冗余数据，提高计算速度和精度。现有的激光雷达SLAM系统如LeGOLOAM，LIO-SAM都采用固定的关键帧生成机制，以两帧之间的距离和时间作为阈值选取关键帧。文献[66]提出一种自适应选择关键帧算法，可以根据周围环境的变换做出相应的调整，在宽阔特征富足的环境下增大关键帧之间的距离和时间，在狭小随时可能发生突变的环境下加快关键帧选取频率，适用于复杂的地底环境。

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

深度学习与激光雷达SLAM 的结合主要应用于系统中的几个模块，如点云的特征提取和配准、回环检测、构建语义地图。在点云处理部分主要分为两种方式：（1）基于学习的特征提取，如FCGF、SpinNet；（2）基于深度学习网络的端到端的点云配准方法，如Lepard、REGTR。这部分的相关内容在1.1.2 小节已有相关介绍。准确的回环检测一直是激光雷达SLAM 有待解决的问题，利用深度学习构建合理的网络模型，通过大量的学习可以使算法提取点云中的关键特征信息，提高回环准确率。OverlapNet[67]是为激光雷达SLAM设计的回环检测算法，它无需两帧点云数据的相对位姿，而是利用图像重叠率的方式来检测回环。算法使用深度图、强度图、法向量图和语义图作为模型输入，输出图像重叠率和偏航角的预测。但在环境突变的情况下，算法的准确率会降低，虽然所处位置相似，但此时两帧激光雷达所映射的图像差别很大，重叠率低。Ma等[68]提出Overlap-Transformer 同样利用OverlapNet 中图像重叠率的思想训练网络，它仅需要三维点云投影的距离图像，生成全局描述子，使用Transformer 结构增强全局描述子的特异性，具有偏航角旋转不变性。

SUMA++[69]是基于激光雷达的语义SLAM，使用RangeNet++[70]对点云进行语义分割，根据语义信息剔除动态目标，再将语义信息融入系统，与几何信息一起建立约束，从而提高定位和建图精度。SA-LOAM[71]是基于LOAM的语义辅助激光SLAM，提出一种语义辅助的ICP算法，并在闭环检测模块中集成基于语义图的位置识别方法，适合在大规模场景中构建全局一致性语义地图。现有的回环检测方法大多基于图像或者点云，环境结构或者纹理相似时，这些方法会产生误差。针对这一问题，ASL-SLAM[72]提出一种基于活动语义的回环检测方法，通过IMU 传感器对Z 轴角速度和加速度的分析来检测转弯和通过减速带两种语义，但在复杂的城市道路，两种活动语义远远不够，这也使得算法检测到较少的回环。文献[73]提出一种自监督的激光里程计，在投影的强度图像上使用SuperPoint[74]提取特征点，使用传统ICP 方法估算位姿后用于在线训练网络。该算法可以以高于传感器频率的频率输出里程计，但由于提取的特征点是稀疏的，无法用于构建可导航的地图。EfficientLO-Net[75]是第一个完全端到端的高效3D 激光雷达里程计框架，提出了基于投影感知的三维点云表示方法和特征学习方法，其性能优于近期所有基于学习的激光雷达里程计，甚至优于基于几何方法的LOAM。表5归纳了近年来优秀的3D激光SLAM方案。

表5 3D激光雷达SLAM方案归纳Table 5 Summary of 3D LiDAR SLAM method

3 数据集与评价指标

3.1 数据集

在公开数据集上运行SLAM 算法，观察运行结果，是判断算法是否可行的直观方式。使用公开数据集，可以暂时避免各种繁琐的工作如传感器的标定、时间同步、数据打标签等，节省了数据准备工作所需的时间。当然，一个算法是否真的鲁棒，还需要借助硬件平台，在真实环境条件下进行测试，因为每个算法的运行场景都是不一样的，数据集只是提供仿真条件。

KITTI[77-78]数据集是一个广泛用于计算机视觉和自动驾驶研究的公开数据集，它由德国卡尔斯鲁厄理工学院和日本丰田技术研究所联合创建，包含视觉图像、激光雷达点云、IMU、GPS等数据，被广泛运用于SLAM算法的评估实验；SemanticKITTI[79]在KITTI数据集的基础上对点云数据进行标注，分类为28类语义，实现全场景分割，可用于训练和测试评估点云分割模型；百度公司的ApolloScape[80]数据集使用了高精度地图采集车，是目前行业内环境最复杂、标注最精准、数据量最大的三维自动驾驶公开数据集；南洋理工大学开源的自主飞行器数据集NTU-VIRAL[81]，搭载激光雷达、相机、IMU 和超带宽（ultra wide band，UWB），涵盖室内室外多个场景，填补了SLAM领域无人机数据集的空白。除此之外，开源的数据集有TUM-RGBD[82]、EuRoC[83]、Oxford Robot-Car[84]、USVInland[85]等。

3.2 评价指标

SLAM算法的精度指标主要有以下两种：

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

公式（2）中，Qi代表第i帧的真实位姿；Pi代表第i帧的估计位姿；单目相机具有尺度不确定性，S是从估计位姿到真实位姿的相似转换矩阵；Fi代表第i帧绝对位姿误差值；公式（3）中n是总数，Δ为时间间隔，m为实际参与计算结果个数，其值为m=n-Δ；trans(Fi)代表取绝对位姿误差的平移部分。

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

公式（4）中，Ei代表第i帧的相对位姿误差；公式（5）中trans(Ei)表示取相对位姿误差的平移部分，其余参数与ATE表示一致。

KITTI 官方提供了专用于评测数据集平移误差和旋转误差的工具KITTI_odometry_evaluation_tool；evo（https：//github.com/MichaelGrupp/evo）是一款受到研究者喜爱的开源轨迹评测工具，它可以根据时间戳将轨迹对齐，评测轨迹误差、ATE、RPE等指标，并且支持KITTI、TUM、EuRoc、ROS bag等多种数据格式。

4 研究展望

4.1 结合深度学习

人工智能是工业4.0 重要的研究内容和发展方向，深度学习与SLAM 相结合可以提升系统运行效率和鲁棒性。归功于深度学习技术在图像处理领域大放异彩，深度学习在视觉SLAM 领域已有深入研究[86-88]，并已应用于图像特征匹配、闭环检测和语义地图构建模块；而在激光SLAM 中，由于点云数据缺少纹理信息、所含信息量较少，所以与深度学习的融合不如视觉SLAM那样成熟，主要应用于点云特征提取和配准、点云语义分割。PointNet[89]是一个优秀的点云分类与分割模型，提取的全局特征能够很好地完成分类任务，但局部特征提取能力较差；PointNet++[90]提出多层次特征提取结构，提高局部特征提取的能力，弥补了PointNet的缺点。当前深度学习与SLAM 算法结合的困难点在于深度学习需要大量的训练样本和GPU 资源，难以部署在嵌入式平台上实时运行，并且模型的泛化能力不足。

4.2 多传感器融合

激光雷达能够提供精确的坐标信息，但缺少纹理信息，且在雨雾环境下会失效；相机能够提取丰富的环境信息，便于目标识别和语义分析，但对光线要求较为苛刻，且无法提供比肩激光精度的深度信息。无人装备往往运行在复杂、动态变化的环境中，而单一传感器作用场景有限，无法很好地应对这种复杂环境。解决复杂环境下SLAM算法的鲁棒性问题，多传感器融合技术是关键。毫米波雷达不受雨雪、烟尘的影响，具有更好的穿透力，可以与激光雷达融合，弥补激光雷达的缺陷；IMU本身不依赖外部环境，又能实现高频率测量，是多传感器融合必不可少的一部分，当前主流的SLAM算法几乎都有IMU参与。常见的多传感器组合方案有激光-惯性里程计（LIO）、视觉-惯性里程计（VIO）、激光-视觉-惯性里程计（LVIO）。LIO 和LVIO 在第二部分已有详细介绍；VIO 也是当前学术界的研究热点，著名的开源算法有VINS-Mono[91]、VINS-Fusion[92]、ORB-SLAM3[93]；各类新型传感器的应用为多传感器融合领域注入新活力，期待研究者们探索新型传感器的应用以解决现有问题。

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

多智能体集群控制是当前的研究热点和发展方向，分布式协同SLAM 意在通过多个智能体在未知环境下协同工作，并通过通信协议交换彼此获取的地图、自身位置等信息，达到快速感知环境、精确定位和地图构建的目的，对某些特殊任务如搜救、战场环境勘测具有重大意义。分布式通信机制、数据共享与地图融合、环境闭环检测和异常处理是多机SLAM 需要解决的问题。分布式协同SLAM需要机器人之间进行频繁的通信，传输地图数据、位置等信息。然而，通信带宽限制和延迟问题可能会导致通信瓶颈，影响数据传输，导致数据无法实时同步，影响协同建图的实时性和准确性。如何设计通信机制，解决数据同步和一致性问题是一个挑战。分布式协同SLAM系统应具备较强的鲁棒性和容错性，能够应对机器人故障、通信中断或其他异常情况。如何设计机制来处理这些问题，并保证整体系统的稳定运行，也是一个重要的挑战。目前比较好的多机SLAM工作有DOOR-SLAM[94]、Kimera-Multi[95]、D2SLAM[96]、Swarm-LIO[97]。

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

研究改进一个SLAM算法，使其在各个场景下都具有较好的鲁棒性是一件非常困难的事。事实上大部分智能体都是在较为固定的场景下工作的，特别是工业机器人，如室内服务机器人、水下作业机器人[98]、矿井作业机器人[99]、电网巡检空中机器人[100]等。针对这些特定场景，对环境特征进行建模分析并结合考虑应用场景的侧重点，对算法进行应用优化，这也是SLAM 技术在工业界发展的趋势。在城市场景下，动态SLAM技术扮演着重要的角色。城市环境中存在着大量的动态物体，例如车辆、行人、自行车以及其他交通参与者。动态SLAM旨在实现对这些动态物体的实时检测和跟踪，并在建立地图的过程中将其与静态部分分离开来，以确保地图的准确性和稳定性。如何使SLAM 算法能够在复杂动态的环境下鲁棒运行，是一项非常具有挑战性的任务。

5 总结

SLAM技术是自主移动机器人实现路径规划、避障和导航的基础。随着激光雷达技术向高精度、轻量化、低成本的发展，激光SLAM 也取得了丰富的研究成果：从最开始的室内单线激光雷达实现定位，建立二维栅格地图，到多线激光雷达和固态激光雷达室外大场景下的定位和高精三维地图和语义地图建立。本文从SLAM的系统框架出发，对激光SLAM系统的各个模块做系统的介绍，再对优秀的开源方法进行阐述和总结，最后对激光SLAM的发展方向做出展望，希望对有兴趣研究激光SLAM的研究者能够起到帮助作用。