3D 激光雷达 SLAM 算法综述

作者 | 周治国,曹江微,邸顺帆

单位 | 北京理工大学信息与电子学院

来源 | 《仪器仪表学报》、智车行家

引言：无人平台在大范围环境中 实现自 主定位与导航的能力 需求日 益严苛， 其中 基于激光雷达的同步定位和绘图 技术(SLAM) 是主流的研究方案。在这项工作中， 本文系统概述了 3D 激光雷达SLAM算法框架和关键模块， 分析阐述了近年来的研究热点问题和未来发展趋势， 梳理了3D激光雷达 SLAM 算法性能的评估标准， 并据此选取目前较为成熟的具有代表性的6种开源 3D 激光雷达SLAM 算法在机器人操作系统(ＲOS) 中进行了测试评估， 基于 KITTI 基准数据集， 从 KITTI 官方精度标准、SLAM算法精度指标、算法耗时和处理帧率3方面进行了横向比较， 结果表明， 所选6种算法中 LIO-SAM 算法性能综合表现突出， 其在 00 序列数据集的测试中， 绝对轨迹误差(ATE) 和相对位姿误差(ＲPE) 的 ＲMSE 数据分别为 1. 303 和 0. 028， 算法处理的帧率(fps) 为 28. 6， 最后依据 CiteSpace 分析讨论了 3D 激光雷达 SLAM 技术的应用趋势。

随着移动机器人与无人平台在各类应用场景的不断进阶与探索， 对移动载体定位能力的要求也越来越高。日益升级的智能驾驶应用， 尤其是更先进的无人驾驶， 为保证安全， 需求定位精度达到厘米级， 现有的定位输入源主要是基于全球定位系统 ( global positioning system，GPS) 等卫星定位手段， 而传统的 GPS 定位导航精度只有5 ～10 m， 当前提高定位精度的方案大体上分为两类。

一类是对卫星信号定位输入源进行升级， 如升级为更高精度的实时差分定位(real time kinematic， ＲTK) ， 或者将进行辅助推算导航定位的低成本微机电系统 ( micro electromechanical system， MEMS) 惯导设备升级为更高精度级别的惯导， 虽然此类方式可以在大多数的使用场景下提高定位精度， 但是在 GPS 信号容易丢失的场景下，如停车场、交错高架路以及室内环境等， 仍存在缺陷且成本较高， 而惯导的推算定位本身就存在初始化和累计误差问题;

另一类是研究学者基于第一类缺陷提出的引入新的实现自主定位源的方法， 如使用激光雷达、毫米波雷达、摄像头等感知传感器， 因此基于各类传感器的自主定位方案开始受到科研人员的广泛关注， 尤其是基于激光雷达和深度相机的自主定位， 近年来已成为热点方案， 本文主要阐述基于多线激光雷达的自主定位方案。

同步定位与绘图 ( simultaneous localization and mapping，SLAM) 作为一种在未知环境中进行姿态估计与定位的技术， 广泛应用于移动机器人和无人驾驶等领域。

SLAM 相关概念最早由 Cheeseman 等于 1986 年在 IEEE 机器人与自动化会议被提出， 该技术通过传感器采集信息，生成无人平台所探索环境的地图并对其进行定位， 实现无人平台的自主移动［1］。从该技术的定义可知， 该技术由地图构建和定位两部分组成。地图构建是把通过传感器采集的序贯激光雷达点云或视觉特征从各帧局部坐标系投影至全局坐标系， 之后完成地图拼接和定位， 即获取移动载体在所建地图中的位置和姿态信息。

一方面获取传感器采集的各帧数据对应的局部坐标系的位置和姿态是构建地图的关键， 即建图包含了定位问题; 另一方面构建准确的地图又是精确定位的前提， 因此， 定位与建图两者是高度耦合的，可作为一个问题寻找解决方案。在线实时的SLAM 技术作为自主定位领域的核心正在被广泛研究。

目前 2D 激光 SLAM 技术发展已较为成熟， 既可用于民用服务如扫地机器人， 也可用 于工业现场， 如 KUKA Navigation Solution ［2］ 。基于激光点云的 3D SLAM 技术利用三维激光传感器(一般是多线激光雷达， 也有少部分是用自制的单线激光雷达组合) 获取三维空间点数据， 之后再通过相邻点云帧之间的扫描匹配进行位姿估计， 并建立完整的点云地图， 与 2D 激光 SLAM 具有相通的技术原理。

三维激光雷达通过光学测距可以直接采集到海量具有角度和距离精准信息的点， 这些点的合集被称为点云，可以反映出真实环境中的几何信息。由于其建图直观，测距精度极高且不容易受到光照变化和视角 变化的影响， 是室外大型环境的地图构建应用中必不可少的传感器。

得益于DAＲPA(美国国防部先进研究项目 局地面挑战赛)［3-4］的推动， 美国 Velodyne 公司的多线激光雷达开始用于无人汽车 SLAM ［5］ ， 移动机器人的定位与建图问题由室内逐渐转为室外， 地图构建也由二维拓展到三维，范围成倍扩大， 但多线雷达成本较高。随着多线激光雷达的量产化和普及化以及嵌入式处理器功耗降低、 计算能力的增强， 基于多线激光雷达的 SLAM 技术正在快速发展。

由于基于多线激光雷达的三维 SLAM 方法在帧间匹配方面有着更丰富的匹配方式及更好的鲁棒性， 并可以同图像信息、物理模型等信息进行融合［6］使定位精度得到提升， 因而具有巨大的发展潜力。关于 SLAM 的相关工作前人已有总结， 尤其是基于视觉的 SLAM 综述出现较多， 但基于激光的 SLAM 总结工作比较少， 且主要是基于 2D 激光， 涉及 3D 激光雷达SLAM 算法的介绍不多。在本文中， 我们将系统的介绍和分析三维激光 SLAM 技术， 并对选取的代表性算法进行性能比较及客观评价。这项工作为我们之后对三维激光 SLAM 技术进行更深入的研究做下了铺垫， 也希望本文能够为其他对 3D 激光雷达 SLAM 技术感兴趣的研究人员提供些许帮助。

1、

3D激光雷达SLAM方案

依赖激光雷达建立地图的激光 SLAM 方案按求解方式可以分为基于滤波器和基于图优化两类， 基于滤波器的方法源于贝叶斯估计理论， 是早期解决 SLAM 问题的方法， 在室内或小范围场景应用中具有不错的效果， 但由于只考虑移动载体的当前位姿状态和当前环境观测信息， 且不具有回环检测能力， 存在线性化以及更新效率低等问题［7-8］， 在程序运行中还会随着场景的增大占用成倍增加的计算资源， 这使得它在室外大型场景中的表现效果比较差， 现阶段基于滤波器的激光 SLAM 方案主要应用在二维室内小范围场景。

基于图优化的 SLAM 方案考虑了移动载体历程中全部的位姿状态和环境观测信息， 用节点和边形成的图来表示一系列的移动机器人位姿和约束， 建立和维护关联数据，可独立出前端实现并行计算， 是一种更为高效和普适的优化方法。

相较于早期基于滤波器的 SLAM 方法， 通常可以得出全局一致性更好的地图， 且随着求解方法的不断发展，在相同计算量的前提下， 图优化 SLAM 的求解速度也已经超过滤波器方法， 是目前 SLAM 领域内的主流方法，也是三维激光 SLAM 采取的主要方案，Hauke 等［9］研究了图优化方法为什么较滤波器方法能取得更优的效果。

Lu 等［10］于上世纪九十年代首次提出基于图优化的2D SLAM 算法， 他们用带约束的位姿网络实现了数据关联， 具备图优化的原型。Gutmann 等［11］于 1999 年正式提出了图优化框架， 该框架与目 前主流的图优化框架大致相同， 具备前端扫描匹配、 全局优化以及闭环检测模块，但由于当时技术发展和认知局限， 没有认识到系统的稀疏性， 并未实现实时 SLAM。之后， 国内外诸多研究学者也不断探索， 为图优化 SLAM 方法的发展做出了 巨大贡献， 图优化 SLAM 方案框架的各个模块也在逐步完善。

基于二维激光的 SLAM 算法相对成熟， Santos 等［12］对 5 种 具有代表性的 基于单线 激光 雷 达的 2D 激光SLAM 算法进行了测试评估和总结， 结果表明 Gmapping和 KartoSLAM 算法在定位和建图的准确性和效率上要更加优越。之后 Google 开源的 Cartographer 算法［13］

采用目前主流的基于图优化的激光 SLAM 算法框架， 提出了分支定界的方法解决子地图的构建以及与全局地图的匹配问题， 实现了闭环检测和较好效果的全局优化， 是目前较为先进和成熟的二维激光 SLAM 技术的代表。

二维激光雷达在确定高度的水平面上通过测量旋转扫描的激光信号与其回波的时间差、 相位差确定环境中目标的距离和角度， 并依据这两类数据在水平面上建立二维极坐标系来表示所感知的环境信息， 可视为一种单线程的三维激光雷达。

相比于只能感知环境中单个平面信息、适用于室内几何结构实现小型区域地图构建的二维激光雷达， 三维激光雷达可以进一步获取高程上更丰富的信息， 对于室外大型场景也有更好的感知效果。

激光雷达根据线数可分为单线、4 /8 线以及 16 /32 /64 /128线 3 类， 随着线束的增多， 激光雷达能够感知环境的信息更丰富， 所得的数据量也相应更大， 设备的成本也成倍增加， 因此基于激光的 SLAM 算法需要在线束上有所考量，要达到更好的实时性就需要处理减少每帧的输入数据量， 而较少的初始数据量因为线束稀疏又不能很好的反映环境信息。

目前的3D 激光雷达 SLAM 算法研究多基于16 /32 /64 线激光雷达， 而面向无人驾驶的应用则追求更高精度的 128 线， 如图 1 所示。3D 激光雷达 SLAM 技术基于多线激光雷达， 沿用并发展了 基于图优化的 SLAM算法框架， 并将其应用于无人驾驶等领域解决大型场景的定位与建图问题。

1. 1 图优化方法

图优化 SLAM 的研究基础是基于图论， 图(graph) 是一种数据结构， 由顶点(vertex) 与连接顶点的边(edge) 组成表示为 G(V， E) ， 其中 G 表示图， 顶点的集合表示为 V，边的集合表示为 E， 其思想是用顶点表示事物， 而连接不同顶点之间的边则用 于表示事物之间 的关系， 如果在图 G 中存在一个顶点上连接两个以上的边， 则称该图为超图， 在 SLAM 中研究的就是根据已有的观测数据实现超图的构建以及优化的过程。

假设移动载体的位姿节点用 μ = { μ 1， μ2 ， …， μ n } 表示， 将环境中的地标表示为 S = {S 1 ， S 2 ， …， S n } ， 则移动平台的位姿和地标可以用图 2 表示。

如果某时刻 k， 移动载体在位置通过激光传感器进行扫描观测得到数据， 则传感器的观测方程为:

由于系统参数和传感器观测存在误差， 使得上式不可能精确相等， 因此误差便存在，如果把:

式(2) 作为目 标函数， 把作为变量进行优化， 便可以求解得到移动载体位姿的估计值， 从而计算估计出平台移动的轨迹。具体到 SLAM 问题中， 顶点表示为激光雷达的位姿以及特征点的位姿， 而边表示观测方程， 观测方程的表达形式有多种， 可定义为移动平台不同位姿之间的约束， 也可以定义为移动平台在某位置观测得到的某空间点坐标表达式。

一般为位姿之间由里程计(odometry) 或者匹配(registration) 计算出的转换矩阵， 这样对移动平台位姿的求解过程就转化为求解图中的优化问题。图优化 SLAM 的模型表示形式也可以从弹簧能量模型［14-15］的视角来解释， 如图 3 所示， 在 SLAM 中是对位姿的最大似然估计， 弹簧模型中则是对应系统的最小能量状态， 而二者的本质问题都可以转换为非线性最小二乘问题。

1.2 图优化 SLAM 方案框架

基于图优化的 SLAM 方案可以分为扫描匹配、 闭环检测、后端优化、点云地图存储表示 4 个部分。扫描匹配利用激光雷达、惯性测量单元(IMU) 及编码器等传感器数据进行扫描匹配， 利用相邻帧之间的关系估计当前帧的运动姿态， 得到短时间内的相对位姿和地图， 考虑的是局部数据关联问题;

由于长时间的增量式扫描匹配会不可避免地造成误差累积， 而闭环检测可以通过比较当前帧与历史关键帧来优化位姿， 检查匹配确立节点间的约束关系， 减少全局地图的漂移误差， 考虑的是全局数据关联; 如果是从基于图优化的表示形式来看， 扫描匹配和闭环检测都是为了根据观测信息建立图的节点以及节点间的约束， 即完成图的构建。研究学者们将两者一起划分为图优化 SLAM 的前端部分［14-15］。

由于系统参数误差、观测噪声以及匹配误差的存在，通过前端模块所构建的位姿图一致性较差， 且通常情况下构建图的边与边的约束存在“冲突”。若用表示帧间匹配的相对变换矩阵， 则 T 0 ， T 1 ， T 2 ， …， T n 构成一个闭环， 在理想情况下应当满足 T 0 T 1 T 2 …T n = I， 其中 I 表示单位矩阵， 但实际工程中通过前端得到的相对变换矩阵一般是达不到此结果的。与前端部分不同， 图优化部分是对前端构建的图信息进行非线性优化， 取得尽量满足所有约束关系的最优解， 最后输出姿态估计结果和全局点云地图， 这一部分也被称为 SLAM 后端， 与 SLAM 前端共同组成整个图优化 SLAM 框架。

1.2 .1 扫描匹配

对于前端扫描匹配， 代表性的三维点云匹配算法大体可分为两类: 基于匹配的方法和基于特征的方法。基于匹配的方法根据算法建立的目 标评价函数可以分为基于距离判断和基于概率模型判断两种， 基于概率模型判断的方法主要是正态分布变换( normalized distribution transform， NDT) 算法［16］， 基于距离判断的方法主要是 ICP算法及其变种算法， 适合 3D 激光雷达 SLAM 的算法包括PP-ICP ［17］ 、NICP ［18］ 、 IMLS-ICP ［19］ 等， 其中代表性的算法是广义迭代最近点 ( generalized iterative closest point，GICP)［20］， 算法原理是将 ICP 算法和 PL-ICP 算法结合到概率框架模型上进行点云配准， 提升了算法的适用性和精确度。

基于匹配的算法一般通过直接使用扫描点来实现准确估计， 需要使用大量的点进行稳定配准， 虽然匹配精度较高但通常计算效率不高。目前最新的能够快速准确的实现三维激光点云匹配配准的算法是一种体素化的广义迭代最近点算法［21］， 该方法通过体素化扩展了GICP 方法， 避免了高代价的最近邻搜索， 同时保持了 算法的精度， 结合了类 ICP 和 NDT 算法的优势。

基于特征的方法通过从扫描点云中提取特征点来提高计算效率， 包括使用角点和平面点特征的 LOAM ［22］ 以及使用面元特征的三维栅格匹配算法多分辨率栅格地图(multi-resolution surfel map)［20］等， 也有许多专注于雷达点云特征描述进行点云配准的研究［23-27］， 如快速点特征直方图 ( fast point feature histograms， FPFH )［23］、 VFH(viewpoint feature histogram)［24］等， 这种方法能够改善计算成本提高实时性从而得到广泛研究。一个经典的基于特征的点云配准算法通常包括关键点检测、 特征描述符提取、真实匹配、异常值剔除和转换估计几个步骤。

1.2 .2 闭环检测

闭环检测基于全局数据关联， 是实现鲁棒 SLAM 的核心步骤， 通过识别是否到达历史场景促使地图闭环的能力， 能够校正累积误差， 从而产生全局一致性的映射地图。但相应的， 错误闭环的结果则会严重影响后端优化的准确度， 甚至会直接导致最终地图的效果不佳。闭环检测的难点主要体现在:

1 ) 感知歧义。例如在长廊、 隧道、 楼梯等结构化十分相似的场景， 会加剧判断难度。

2) 由于激光传感器本身的稀疏性造成的观测数据鲁棒性和区分性受限问题， 即如何建立易于处理的环境有效表征方式。

3) 数据规模会随着运行时间增加而导致需要判断的帧数据不断增长， 会降低建图的实时性。

基于激光的场景识别致力于寻求一种有效而简明的位置描述符， 目 前已有的 闭 环检测 技术有基于 MonteCarlo 的节点搜索算法［28］， 也可依据 GPS 辅助法进行辅助闭环判断; 有基于描述子的回环检测算法， 通过提取局部或全局场景描述子进行场景识别， 局部描述子代表算法有 FPFH ［23］ : 利用 局部表面法向量计算局部描述子，Bosse 等［29］提出一种基于 Gasalt3D 描述符的概率投票方法， 由几何信息和强度信息组成局部描述符 ISHOT ［30］ ;

全局描述子代表算法有将关键点对的相对几何位置编码成直方图的 GLAＲOT 方法［31］， 将激光扫描投影到全局描述符的扫描上下文 Scan Context 方法［32］等， BoW(bag of words)［33］一种基于词袋模型的场景识别算法， 还有 FAB-Map(fast appearance based mapping)［34］和 DBoW2 ［35］ 等方法， 但 这 些 方 法 起 初 是 被 用 于 视 觉 SLAM， 如 OＲB SLAM ［36］ 和 LDSO ［37］ 。

1.2 .3后端图优化

后端优化是将各帧雷达的位姿和帧间运动约束综合起来达到整体优化的一个过程， 可以消除局部累计误差，在大尺度的建图中， 一般需要具备一个“监管者”来时刻协调之前的轨迹， 这便是 SLAM 的后端优化。

综述［38-39］对优化方法已有系统的详细介绍， 基于图优化 SLAM 的后端优化方法可概括分为 4 类: 基于最小二乘法的优化方法、基于松弛迭代的优化方法、基于随机梯度下降的优化方法以及基于流形迭代方法。目前基于图优化的开源优化库有 iSAM(incremental smoothing and mapping)［40］、GTSAM ( georgia tech smoothing and mapping)［41］、 G2O( general graph optimization )［42］、 Ceres ［43］ 、 BA ( bundle adjustment)［44］等， 借助于这些优化库可节省后端迭代求解优化值的时间。

1.2 .4 地图表示

通过 3D 激光雷达 SLAM 算法最容易得出的是点云地图， 如图 4(a) ， 它可以直观描述周围环境， 具备基本的环境展示功能， 但因点云数量庞大， 一般需要通过体素滤波进行一定的降采样才能正常显示。

此外由于单纯的点云无法表示障碍物信息且不具备特征， 所以无法直接用于导航和避障以及基于特征的定位， 但可用于实现基于点云配准的定位研究。在点云地图的基础上进一步处理， 根据使用目的， 将从点云地图中提取出的特征聚在一起可以构成使用特征进行定位的稀疏特征地图， 如图 4(c) 所示， 还可以构成用于导航避障和路径规划的占据网格地图以及压缩性能更好的八叉树地图， 如图 4(b)所示。

占据网格地图常采取栅格或者体素的方式， 在最小化栅格或者体素中用概率或者 0 ～ 1 的形式表示被占据的状态， CSM ［45］中构建了二维占据栅格地图，OctoMap ［46］ 中提出了将点云地图转变为三维占据地图的方法， 八叉树地图是一种特殊的占据栅格地图， 该结构中占据概率相同的栅格可进行合并， 从而降低存储地图的空间。

近年来语义地图的出现与发展也提高了机器对周围环境感的内 容理解， 如图 4(c) 所示， 语义地图联合几何和内容两个层次的感知， 帮助提高 SLAM 建图和定位的精度， 也能更好的提升机器智能化理解环境的能力。

2、

研究热点及发展趋势

激光雷达点云具有局部稀疏性、数据量大和动态对象会引起噪声的特征， 这也成为了基于激光点云的SLAM 研究中的难点。与图像匹配问题相比， 点云的稀疏性使得从源点云和目标点云中找到两个精确匹配点通常是不可行的， 而且激光扫描仪从不同角度观察到的同一物体的外观差异很大， 这就增加了 特征提取的难度。

其次由于激光扫描仪每秒会产生数百万个点， 需要高效的算法和强大的计算单元， 对于算法的实时性有着较高的要求。最后动态对象点云会引起噪声干扰， 对其进行处理的方法是否得当对于能否实现理想的高精度估计也是至关重要的。此外， 场景的不断变化被认为是解决 3D激光雷达 SLAM 问题的最大挑战。

利用激光点云解决 SLAM 中三维地图的构建问题，最早是在 2014 年由卡耐基梅隆大学的 Zhang 博士等提出的， 即著名的 LOAM(lidar odometry and mapping)［22］算法， 该算法以新颖的方法提取激光点云线面特征减少计算量， 并创造性的将运动估计问题分成两个独立算法来共同完成， 一个算法执行高频率的里程计但是低精度的运动估计， 另一个算法运行频率较低的匹配建图但返回高精确的运动估计， 最终将两个数据融合成高频率高精度的运动估计， 很好的权衡了精度和效率， 实时性高， 唯一的不足点在于缺少回环检测。

之后 LeGO-LOAM ［47］ 在LOAM 的基础上衍生出新的框架， 在特征提取上提升轻量化和地面优化， 并增加了回环检测， 整个框架与目前成熟的图优化3D激光雷达SLAM 方案框架吻合， 构建的地图更加完善。hdl_gragh_slam ［48］ 也是标准的图优化 3D激光雷达 SLAM 框架， 新颖点在于后端优化过程中融合了 GPS、IMU 和路面约束信息， 可以更好的构建全局一致性地图。随着多线激光雷达的问世， 激光 SLAM 的研究学者们在三维激光 SLAM 领域开拓进展， 主要的研究热点与趋势也聚焦在图优化 3D 激光雷达 SLAM 框架的关键模块上。

2. 1 激光惯性里程计

激光里程计(LiDAＲ odometry， LO) 仅基于点云配准算法获取当前帧的全局位姿， 但由于 Lidar 本身会受稀疏性和运动扰动影响而导致感知准确度降低， 以及在一些Lidar 退化情况下其感知点数量会严重减少， 这都会使激光里程计的精度受到较大程度影响。 研究学者们通过融合 IMU 的数据来保证和提升激光里程计的精度。一种类别是松耦合方法， 该方法分别考虑 Lidar 和 IMU 的估计， 以损失信息为代价来换取较小的计算负载。

例如在LOAM 中依赖于 IMU 解算朝向作为辅助但假设匀速运动， 将 IMU 作为整个系统的先验， 但它不能利用 IMU 测量用于进一步优化。

另一种类别是紧耦合方法， 通过 IMU 测量提供状态预测， 同时测量更新被用于修正预测的状态， 领域内也称之为 激 光 惯 性 里 程 计 ( lidar inertial odometry， LIO ) ，LIO_mapping ［49］ 初次提出了 紧耦合的 Lidar-IMU 融合方法，通过联合优化 IMU 和 Lidar 的测量数据， 即使在 Lidar退化的情况下也没有明显漂移， 相较于松耦合的方法， 紧耦合的方法会使精度大幅提升。

Liom 提出一种新的紧耦合激光惯性里程计和建图方法来达到低漂移和鲁棒的位姿估计，激光惯性里程计模块采用误差状态卡尔曼滤波器(ESKF) 来实现激光扫 描仪和 IMU 的 传感器融合［50］。

MC2SLAM 提出一种新的紧耦合方法， 把点云畸变补偿和点云匹配统一到一个优化任务中，并在后端使用 IMU 预积分进行位姿优化， 精度被进一步提高

［51］。LIO-SAM 作为LeGO-SLAM 的作者，在图优化框架的基础上实现紧耦合激光 － 惯性里程计， 采用的是因子图而非滤波方式进行优化［52］。

激光里程计模块作为整个3D 激光雷达 SLAM 方案的基础模块， 可直接进行位姿估计与构建地图， 在理想情况下该模块的输出结果可直接导出使用， 其精度直接影响最后的结果，因此找到一种高效通用的方法实现激光和惯导数据的紧耦合并提升精度仍是未来的研究重点。

2. 2 场景识别

场景识别是 SLAM 的关键任务之一。场景识别具有两个功能， 一是用于历史场景的识别， 二是用于相对于存在地图的位姿估计即重定位。Dube 等［53］首次提出基于分割的 3D 点云场景识别方法 SegMatch， 把传统基于局部特征和全局特征进行场景识别的方法进行了 折中，能够实现实时、 可靠的检测闭环。Segmap 方法［54］采用与 Segmatch 相同的基于分割的思想， 提出 了 一种基于深度学习的 3D 点云分割描述子， 提升了 定位性能且能够提取语义信息。

SUMA 方法［55］使用 Surfel 地图 高效地生成投影数据关联并实现闭环检测， 此前 Surfel 地图曾被用在 ＲGBD-SLAM 中， SUMA 将其首次应用于激光SLAM 中， 该方法只基于激光点云就可以建立大尺度环境下的全局一致性地图。

DELIGHT 仅依靠激光雷达实现全局定位， 采用 雷达获取的反射强度信息而非常规的距离信息进行场景识别， 算法分为两个阶段， 包含一个基于密度描述子的 先验估计和 一个基于几何的验证， 从而解决了“机器人唤醒”问题［56］。

ISC 方法［57］是基于激光雷达点云的全局回环检测算法， 利用点云的几何信息和密度信息构建新型全局描述子 ISC。场景识别的基本能力 是能够有效地描述地图 信息， 因此地图描述的高效性和鲁棒性至关重要， 能够增加机器人理解环境的能力， 几何信息与语义信息结合的描述形式将是未来的一个可能趋势。

2. 3 后端优化

如何高效的通过后端优化来修正运动位姿和提高构建地图的精度也是研究关注的热点之一。C-T SLAM ［58］是一种分层、连续时间的 3D 激光雷达 SLAM 算法， 采用高效的局部地图和分层的优化后端， 允许实时建图期间优化修正。BALM 将视觉 SLAM 领域中取得极大成功的BA 框架引入激光建图模块， 以此降低建图过程中的累计误差［59］。

LIO-SAM 的［52］后端优化采用因子图优化， 融合激光里程计因子、IMU 预积分因子、GPS 因子和回环因子， 得到更鲁棒的运动估计。现有的后端优化都是建立在正确的数据关联基础上的， 否则会产生错误的地图结果， 这将使得后端优化的使用性受到限制， 因此能够自动甄别并删除错误的关联数据， 实现在存有错误约束数据的情况下仍能构建正确地图的鲁棒性后端优化方法也是未来的发展趋势。

2. 4 动态物体

在动态变化的环境中实现准确的定位是一件困难的事情， 大多数激光 SLAM 方法假设环境是静态的， 因为这些方法依赖激光提供的测量， 它们很难处理物体剧烈运动导致环境几何退化的问 题。因此如何在高速运动场景中实现鲁棒性的位姿估计亦是研究的热点之一。

SuMa + +［60］基于 SUMA 框架提出， 利用每帧点云中逐点的语义标号构建全局语义地图， 利用 ＲangeNet + +可靠滤除场景中的动 态物体， 提高构 建地图 的 精度。LIO-LiDAＲ ［61］ 融合激光惯性里程计(LIO) 和 LiDAＲ 全局定位模块到一个位姿图优化框架中， 利用二者的互补性解决动态场景中长期定位的精准问题。

LIOM 考虑到动态物体在环境地图构建过程中的几何退化， 不利于构建大尺度环境， 设计动态物体检测模块用于检测和移除每帧的动态物体扰动产生的点云［50］。但 LIOM 一次性处理所有测量， 因此不能达到较高的实时性。

2. 5 结合深度学习

为了有效地利用所有可用的扫描数据， 基于深度学习的方法为直接从激光雷达数据估计机器人的姿态提供了潜在的解决方案。类似的方法已经成功地应用于相机数据， 并证明了有希望的结果［62］。在使用激光雷达数据进行基于学习的机器人姿态估计领域， 将深度学习用于解决 SLAM 问题也逐渐成为趋势， 研究人员开始利用深度学习方法对 3D 激光雷达 SLAM 框架中的关键模块进行改进， 如 LO-Net ［63］ 、 DMLO ［64］ 、 DeepLO ［65］等针对激光里程计任务设计了基于深度学习框架的特征匹配估计;百度团队提出的 DeepICP ［66］ 端到端 3D 点云配准深度学习框架， 考虑到动态对象的干扰， 充分利用静态对象的显著特点， 从而达到高鲁棒性; SUMA + +［60］中也利用ＲangeNet + + 网络滤除场景中的动态物体。

在闭环检测模块中利用深度神经网络设计 OverlapNet ［67］ ， 该方法基于 LiDAＲ 数据的不同线索搜寻回路闭合， 基于 LiDAＲ 数据的不同维度信息搜寻回路闭合， 将里程计结果结合范围、法向量、强度和语义等信息定义重叠率， 对闭环结果进行检测、修正。

除以上研究热点外， 点云匹配算法的优化也在不断创新， 如 Fossel 等［68］提出 一个 LiDAＲ SLAM 前端， 称之为 NOctoSLAM。该方案使用点面 ICP进行帧图位姿匹配， 并使用 octree 替代传统的 kd-tree 构建地图， 从而可以快速地寻找数据关联和计算近似法向量。与传统的基于kd-tree 地图的点面 ICP 算法相比， 效率提高了 近两倍。

文献［69-70］ 考虑到室内较于室外多结构化特征环境的特点， 提出面向室内 环境的激光惯性里程计方法和点云平面特征提取方法。除此之外， 研究人员还考虑到地图的长期重复定位问题， 如 Egger 等［71］基于 3D 点云特征提出了一种新的点云地图表示方式 PoseMap， 保证了在动态环境中机器人可靠的长期定位， 新的地图表示方法允许地图在线更新扩展。

3、

算法评估测试

3. 1 数据集

研究应用 3D 激光雷达 SLAM 的移动载体有无人车、无人船以及无人机， 而目前的激光点云数据集主要是针对自动驾驶场景， 室外场景下的数据收集工作规模大而繁琐， 涉及各种传感器间的时间同步、 坐标校准和标定等， 公开数据集节省了算法研究的数据准备工作， 提供的序列和基准框架也有利于算法开发。

目前领域内公开的激光点云数据集有: KITTI 数据集［72-73］，是目前国际上最大的自 动驾驶场景下的评测数据集，也是学术界最常使用的数据集; Waymo 数据集［74］， 自动驾驶公司 Waymo 的数据开放项目 ; PandaSet 数 据集［75］， 采集场景位于旧金山， 用以开发复杂环境和极端 天气下安全可控的自动驾驶技术; Oxford Ｒobotcar 数据集［76］， 由牛津大学机器人实验室提出 的 公开数据集;

USVInland 数据集［77］， 仿照 KITTI 数据集模式， 清华大学与西北工业大学的研究人员 联合欧卡智舶公开的一组内陆水道中 无人船的多传感器数据集， 也是全球第一个内河场景下包含激光点云数据的无人船数据集。而针对无人机平台的激光点云公开数据集目 前尚未出现， 学术界研究仍多是自收集数据。

3. 2 算法性能评估标准

近年来学术界有关 3D 激光雷达 SLAM 算法的研究几乎都是在 KITTI 数据集上进行的［47，51-52，55，57-67］， 该数据集是汽车在具有动态对象和植被的各种道路环境中驾驶期间捕获的， 例如高速公路、 乡村道路和城市区域， 行驶速度高达 90 km/h。由车载 Velodyne HDL-64ES2 捕获的点云已经完成去偏移处理， 此外数据包中也包含有 IMU、GPS 以及图像数据。

因此， 该数据集被允许对任何由 3D 激光雷达 SLAM 方法获得的轨迹进行评估， 可以被直接馈送到所提出 的方法中 进行评估比较。对于所提出的算法性能一般基于以下 3 个标准进行性能评估:

1) KITTI 官方标准［72-73］:

KITTI 里程计基准总共包括 22 个序列数据， 官方只提供 11(序列号为 00-10) 个带有真实轨迹的数据集， 另外 11 (序列号为 11-21) 个没有真实轨迹的数据集序列用于 KITTI 官方的评估对比。评估方式为计算数据集中相对于真实轨迹不同长度(100 ～800 m) 的平移误差和旋转误差， 并计算平均值。

2) 精度指标: 相 对 位 姿 误 差 ( relative pose error，ＲPE)［78］用于描述相隔一定时间 差的两帧位姿差的精度， 即在用时间戳对齐之后， 每隔一段相同时间分别计算真实位姿和估计位姿的变化量， 然后对两者做差， 从而获得相对位姿误差， 之后可以用均方根误差 ＲMSE 统计各段时间的相对位姿误差从而获得总体值; 绝对轨迹误差(absolute trajectory error， ATE)［78］描述的是估计位姿和真实位姿之间的直接差值， 可以非常直观地反映算法精度和轨迹的全局一致性。

3) 算法耗时和处理帧率: 作为计算效率的指标， 统计了处理 KITTI 里程数据集的序列所花费的时间并计算出帧率， 能够直观反映计算负载和实时性能。

除 KITTI 数据集等公开数据集以外， 也可以使用通过多线激光雷达录制的数据包， 依据精度指标进行激光SLAM 算法的性能评估。

3. 3 算法性能评估实验

本文选取了 6 种目前开源的3D 激光雷达 SLAM 算法进行测试与评估， 如表 1 所示。我们将其应用于机器人操作系统(robot operating system， ＲOS) 中， 所有的算法都在基于 KITTI 公开数据集基准的实验中进行了评估和比较。为了评价所测试算法的性能， 在相同的条件下收集实验结果， 并进行性能度量。

我们将序列号为 07 的数据输入选取的各算法， 得到构建的点云地图。图 5 所示为其中具有代表性的 5 种算法所构建的点云地图， SUMA 算法构建的点云地图效果基本相同， 可以看到， 仅凭地图构建效果无法判断算法在性能上的差异， 仅能在图 5(e) 判断 Cartographer 算法在建图结果上稍有逊色， 图 5(e) 中圆圈 A 代表为同一部分的不同视角， 没有形成闭环， 因此我们需采用 3. 2 节所述算法性能评估标准进行评估。

1) KITTI 官方标准

表 2 列举了 KITTI 数据集中序列号分别为 00、05 和07 的数据测评结果， 该表中数据来源于 LITAMIN2 ［79］ 其作者使用 的实验设备为 具有 32 GB ＲAM 的 Intel Corei9-9900K和 NVIDIA GeForce ＲTX 2080 Ti 的 台 式 机。

KITTI 官方也提供了专门用于数据平移误差和旋转误差的精度测评工具 KITTI_odometry_evaluation_tool， 其测评效果如图 6 所示。

2) 精度指标

本文借助于 evo 测评工具进行测试实验， 主要通过精度指标 ＲPE 以及 ATE 对算法性能评估。实验硬件设备为具有 16 GB ＲAM 的 Intel Core i7-10700 和 NVIDIA GeForce ＲTX 3070 的 台 式 机， 系 统 环 境 为 基 于Ubuntu18. 04 操作系统的 ＲOS 系统。选取了 KITTI 数据集中序列号为 00、05 和 07 的数据进行算法的测试评估。

如图 7 所示， 对比的是 00 序列数据五种算法最终输出的轨迹和位姿结果， 其中 KITTI_00_gt 虚线表示的是该序列的轨迹真值。从图 7(a) 中 y 方向可以较为直观的得出 hdl_graph_slam 和 LIO-SAM 算法的输出结果最贴近真实轨迹， 说明该算法较其他算法有更加精确的估计轨迹。

图 7(b) 中可以看出姿态角每个算法输出相较于真实值各不相同， 但从俯仰角(pitch) 也可以看出 hdl_graph_slam 和 LIO-SAM 算法的输出结果最贴近真实值。如图 8 所示， 通过 evo 工具得到的 LeGO-LOAM 算法输出轨迹平移误差的 ATE、 ＲPE 结果， 图中的 APE 等同于 ATE。表 3 中统计了除 Cartographer 算法 00 序列 5 种算法平移误差 ATE、 ＲPE 的 rmse 数据， 表中数据也映证了 LIO-SAM 算法较于其他算法在轨迹误差上的优越性。

3) 算法耗时和处理帧率

本文测试实验对 5 种算法的耗时和处理帧率进行了统计， 表 4 数据为本文实验统计数据， 硬件设备为16 GB ＲAM 的 Intel Core i7-10700 和 NVIDIA GeForceＲTX3070。

表 4 数据可以看到， Lego-LOAM 算法在耗时和处理帧率上表现最佳， SUMA 算法紧随其后， LIO-SAM 算法也有着不凡的表现， LOAM 和 hdl_graph_slam 两种算法表现一般， 而 Cartographer 算法相较其他算法的实时性差距过大， 在实时建图需求下并非良好的选择。

本测试实验中， 除了 Cartographer 算法其他算法均实现了实时性， 甚至可以加速处理。依据表 3 和表 4 的测试数据， Lego-LOAM 虽然优化 LOAM 算法框架实现了计算上的轻量化， 使得算法在计算效率上大幅提升， 但在精度方面却有所欠缺; 所选算法中 LIO-SAM 算法在精度方面展现出优越性的同时， 实时性也得到了保障， 在所选取的算法中表现优越; Cartographer 算法虽然在室内 2D 建图上性能较为优越， 但在室外大范围环境中实现 3D 建图

则捉襟见肘。

4、

技术应用趋势分析

3D 激光雷达 SLAM 技术近年来的蓬勃发展是有目共睹的， 其核心课题是同步定位与地图创建。我们针对3D 激光雷达 SLAM 技术应用领域， 运用 Citespace 进行科学知识图谱绘制， 从而研究技术应用趋势。我们分别从CNKI 学术期刊库和 WOS 核心合集检索到文献的关键词共现分析结果， 图 9 展示了检索文献的关键词联合突现分析结果， 针对 2000 ～2021 年的论文， 其中 CNKI 检索主题词为“3D 激光雷达 SLAM”， WOS 检索主题词为“3D Lidar SLAM”， 再对文献进行领域相关筛选， 最终得到与3D 激光雷达 SLAM 技术应用领域密切相关的 207 条中文文献和 346 条英文文献。从共现结果可以看到， 目前应用领域主要包括移动机器人领域和测绘领域。

移动机器人领域: 当下服务型机器人已经成为 3D 激光雷达 SLAM 技术的热门应用场景， 主要有物流配送机器人，导游机器人，巡检机器人等。从原理上来说， 无人驾驶汽车也属于移动机器人，目前无人驾驶领域的装备竞演愈发火热，随着城市物联网和智能系统的完善， 无人驾驶已是大势所趋。无人船、无人机移动载体能够规避障碍物、路径规划，实现自主智能化也需要 SLAM 技术的支持。针对单个平台激光雷达建图周期长、算力需求大的现状， 研究多车协同建图方法， 增加平台间的地图重用性， 提高建图效率，实现有效负载均衡是未来发展的趋势。

测绘领域: 高效实时地获取 3D 精细化模型， 对目标区域进行快速三维重建是测绘领域的核心课题，3D 激光雷达 SLAM 技术使得实时快速重建成为可能， 现下已成功应用于各种移动测绘平台，如移动背包 SLAM、 手持SLAM 扫 描 系 统、 推 车 SLAM、 无 人 机 SLAM、 无 人 船SLAM 等， 应用场景有室内测绘、矿井测绘、林业测量、工地测量， 海洋平台测绘等。

由于测绘需求的高精度， 多数移动测绘平台无法分配更多的计算资源用于后端优化构建， 一般采用离线处理或将实时数据上传到高性能服务器上进行计算， 从而取得高精度重建结果。在保证精度的前提下， 如何改进优化算法提高数据处理速度实现实时性， 如何克服大量相似场景间的误匹配， 仍是未来探索的方向和难点。

结 论

3D 激光雷达 SLAM 算法在传统关键模块上仍需实现更好的轻量化、 精确性、 鲁棒性以及通用化， 语义地图和深度学习的融入已成为趋势， 与其他能实现自 主定位源的传感器如深度相机、 毫米波雷达等进行多源融合亦是当前研究热点，3D 激光雷达 SLAM 技术对无人平台实现自主智能化的发展必将产生深远影响。

本文依据梳理的算法性能评估标准对开源算法进行了测试实验， 并初步得出评估结论， 而由于算法的开源性问题， 如 LOAM 算法原项目 未开源， 以及 hdl_graph_slam和 LIO-SAM 算法融入了 GPS 数据， 受实验条件和开源算法输入限制， 本文仅利用点云和 IMU 数据进行测试， 实验本身还具有一定缺陷， 无法对各算法进行更加深入的测试评估， 在未来工作中我们将尝试融入多源数据， 进行更加全面且深刻的研究。

参考文献：

参考文献

［1 ］SMITH Ｒ，SELF M， CHEESEMAN P．Estimating uncertain spatial relationships in robotics ［ C ］．Proceedings of Conference on Uncertainty in Artificial Intelligence， 1988．

［2］ CADENA C， CAＲLONE L， CAＲＲILLO H， et al． Past，present， and future of simultaneous localization and mapping: Towords the robust-perception age［J］ ． IEEETransactions on Ｒobotics， 2016， 32(6) : 1309-1332．

［3］ MONTEMEＲLO M， BECKEＲ J， BHAT S， et al． Junior:The stanford entry in the urban challenge［J］ ． Journal of Field Ｒobotics， 2008， 25(9) :569-597．

［4］ LEVINSON J， ASKELAND J， BECKEＲ J， et al．Towards fully autonomous driving: Systems and algorithms［C］ ． Intelligent Vehicles Symposium， IEEE，2011．

［5］ SHAN T， ENGLOT B， MEYEＲS D， et al． LIO-SAM:Tightly-coupled lidar inertial odometry via smoothing and mapping［C］ ． International Conference on IntelligentＲobots and Systems (IＲOS) ， IEEE， 2020．

［6］ 王常虹， 窦赫暄， 陈晓东， 等． 无人平台 SLAM 技术研究进展［J］ ． 导航定位与授时，2019，6(4) :12-19．WANG CH H， DOU H X， CHEN X D， et al． Ｒesearchprogress in simultaneous localization and mapping forunmanned vehicles ［J］ ． Navigation Positioning andTiming， 2019， 6(4) : 12-19．

［7］ HUANG S D， DISSANAYAKE G． Convergence andconsistency analysis for extended Kalman filter basedSLAM ［J］ ． IEEE Transactions on Ｒobotics， 2007，23(5) :1036-1049．

［8］ THＲUN S， BUＲGAＲD W， FOX D． Probabilisticrobotics［M］ ． Cambridge， USA: MIT Press， 2005．

［9］ HAUKE S， MONTIEL J M， ANDＲEW DAVISON J．Visual SLAM: Why filter? ［J］ ． Image and VisionComputing， 2012， 30(2) :65-77．

［10］ LU F， MILIOS E． Globally consistent range scanalignment for environment mapping ［J］ ． AutonomousＲobots， 1997， 4 (4) : 333-349．

［11］ GUTMANN J S， KONOLIGE K． Incremental mapping of large cyclic environments ［ C ］ ． Proc of IEEE International Symposium on Computational Intelligence in Ｒobotics and Automation． Piscataway， NJ: IEEE Press，

［12］ SANTOS J M， POＲTUGAL D， ＲOCHA Ｒ P． Anevaluation of 2D SLAM techniques available in robotoperating system ［C］ ． In: Proceedings of IEEE International Symposium on Safety， Security， and Ｒe