移动机器人三维激光SLAM 算法研究

欧阳仕晗，刘振宇，赵怡巍，秦圣然，刘 潇

（1.沈阳工业大学信息科学与工程学院， 沈阳110870；2.中国华能集团燃料有限公司营口分公司，营口115007；3.东北大学机器人科学与工程学院，沈阳110016）

1 引 言

移动机器人（Mobile Robot）相对于固定工作的臂式机器人具有更大的工作范围，是当今机器人研究的热点。同时定位与地图构建（Simultaneous Localization and Mapping， SLAM）技术是移动机器人实现自主移动与定位的关键，得到广泛的研究与应用[1]。与视觉传感器相比，激光雷达可提供更加稳定和准确的信息，不受外部干扰，因此能够为SLAM 提供一个更为可靠的解决方案。在过去20 年里基于激光雷达的SLAM 一直是移动机器人导航研究的基础。随着现代激光SLAM 技术的发展，三维激光SLAM成为目前最先进的移动机器人SLAM 技术之一。主流的三维激光SLAM 算法有：基于关键点的LOAM及其改进算法LeGO_LOAM 和A_LOAM、LIO；基于数据驱动的SegMatch 及改其改进算法SegMap；基于扫描点的BLAM、IMLS-SLAM、ICP-LO；基于图优化的Karto SLAM 和Cartographer 等[2]。张吉等人提出一个低漂移实时激光雷达测距建图算法[3-4]（Lidar Odometry and Mapping，LOAM），在公开数据集KITTI（Karlsruhe Institute of Technology and Toyota Technological Institute）里程计数据集上仅使用激光雷达来估计移动机器人位姿的精度最高的算法[5]，但LOAM没有闭环检测功能，导致了漂移误差会随时间增加。铁小山等人提出一种轻量级和地面优化的激光雷达测距和建图算法[6]（Lightweight and Ground-Optimized Lidar Odometry and Mapping，LeGO_LOAM），相比于LOAM，在减少计算量的情况下实现了相似或更好的精度及建图工作，其在线执行闭环检测的能力使其成为长时间导航任务的有用工具，但它的闭环检测有时会出现错误或识别遗漏。秦通等人提出了LOAM的高级实现（Advanced implementation of LOAM， A_LOAM），用Eigen 和Ceres Solver 来简化代码结构[7]，相比于LOAM，代码更为简洁，无需复杂的数学推导和冗余操作。R. Dubé 等人提出了在三维点云中基于分割匹配的位置识别算法[8]（Segment based loopclosure for 3D pointclouds， SegMatch），它可以用在室内和室外环境，然而算法依赖于初始位姿，没有全球定位系统传感器就无法工作。随后R.Dubé 等人又提出了SegMatch 的改进算法：使用数据驱动描述子进行3D 片段建图算法[9]（3D Segment Mapping using Data-Driven Descriptors， SegMap），该算法引入一种新颖的数据驱动的片段描述符，提取的语义信息可以提高对环境变化的鲁棒性，但基于分割匹配的三维激光SLAM 算法SegMatch 及其改进算法SegMap产生的漂移随着时间推移累计成显著漂移。Google在ICRA 会议上公布了Cartographer 开源项目[10]，在工程实现上构建了一套完整的室内雷达SLAM 系统而非简单的算法，其技术框架完整，建图效率很高。文献[11]提出了一种新的基于三维激光雷达数据的SLAM 系统，使用隐式移动最小二乘（IMLS）表面来表示模型，提高了系统鲁棒性。Behley 和Stachniss提出了一种基于面元的运动估计建图方法SuMa[12]，该方法允许表示大规模环境并且维护点云的详细几何信息，但是稀疏点云对其是一个挑战。廖瑞杰等人提出了以边匹配为主要依据的基于K-公共子图判定的室外场景三维点云闭环检测算法并命名为SegGraph[13]，采用公开数据集KITTI 进行评估，实验结果显示该算法具有良好的准确度和运行效率。除了上述几种传统SLAM 方法外，研究人员们还研究了使用深度学习的里程估算方法[14-15]，但尚未达到同等水平。

针对上述研究现状及存在的问题，鉴于LOAM及其改进算法LeGO_LOAM 和A_LOAM 是非常低漂移、实时的激光雷达测距方法，以下详细讨论分析新松大尺度室外环境和公开数据集KITTI 下这三种算法的工作原理和相对位姿估计误差。

2 三维激光SLAM 算法

2.1 LOAM 算法原理

LOAM 是一种使用3D 激光雷达进行状态估计和建图的实时、低漂移的激光雷达测距方法。其关键思想是将复杂的SLAM 问题划分为高频的运动估计和低频的环境建图，利用测程算法和建图算法的协作寻求同时优化大量变量，实现实时精确的运动估计和建图。其中测程算法以高频率但低保真度（低精度）进行里程测量以估计激光雷达的速度，消除了激光雷达运动引起的点云失真，通过保证快速计算来找到特征点的对应关系。建图算法利用未失真的点云以低一个数量级的频率但高保真度（高精度）运行用于点云的精确匹配和配准。将两个估计融合在一起即可产生高频率和高精度的单个运动估计。一方面，两种算法的组合允许实现实时建图，并行算法结构保证了实时求解问题的可行性，能够实现低漂移和低计算复杂度且无需高精度测距或惯性测量；另一方面，算法的基于特征的激光扫描数据匹配方法对边缘/平面执行点-特征扫描匹配，以此找到扫描之间的对应关系。相比于基于散点的传统方法，此法定位精度更高，易于扩展到大场景。LOAM 主要有四个模块组成：扫描匹配、激光雷达测程算法、激光雷达建图算法和变换融合。其软件系统如图1 所示。

图1 LOAM 软件系统结构图

首先令Pˆ为激光雷达一次扫描接收到的点，每次扫描期间，Pˆ在激光雷达坐标系中配准，在扫描K期间，配准后的点云形成PK。用两种算法处理PK。激光雷达测程算法获取点云并计算两次连续扫描之间的激光雷达运动。估计的运动用于校正PK的失真。该算法以大约10Hz 的频率运行。输出通过激光雷达建图算法进一步处理，算法以1Hz 的频率将未失真的点云配准到地图上。最后将两种算法发布的位姿变换集成在一起，以产生相对于地图上的激光雷达位姿大约10Hz 的变换输出。

2.2 LeGO_LOAM 算法原理

LeGO_LOAM 是一种轻量级和地面优化的激光雷达测距和建图方法，适用于具有可变地形的复杂环境，对移动机器人进行位姿估计，并实时估计地面车辆的六自由度位姿。与原始广义的LOAM 框架相比，LeGO_LOAM 旨在提高地面车辆的效率和准确性。算法主要由五个模块组成：分割、特征提取、激光雷达测距算法、激光雷达建图算法和变换融合。其软件系统和激光雷达测程算法框架分别如图2 和图3 所示。

图2 LeGO_LOAM 软件系统结构图

图3 LeGO_LOAM 激光雷达测距模块两步优化

首先在分割模块中采用一次扫描的点云，并将其投影到深度图像上进行分割，利用点云分割滤除噪声。然后将分割的点云发送到特征提取模块进行特征提取，以获得独特的平面和边缘特征，丢弃在地面分离之后可能表示不可靠特征的无价值点。接着在激光雷达测程模块中使用从特征提取模块中提取的特征来找到在连续连续扫描过程中机器人位置的相关转换，采用两步Levenberg-Marquardt 优化方法分别计算位姿变换的不同分量来估计位姿，使用平面和边缘特征来解决连续扫描中六自由度变换的不同分量。从地面提取的平面特征用于在第一步中获得[tz，θroll，θpitch]，从地面特征中确认机器人当前位姿z、roll 和pitch。在第二步中通过匹配从分割的点云提取的边缘特征来获得变换的其余部分[tx，ty，θyaw]，在每一帧点云数据匹配的过程中得到机器人位姿x、y 和yaw。之后这些特征在激光雷达建图模块中进一步处理，配准到全局点云图，用于激光雷达的点云建图。最后的变换集成模块融合激光雷达测程算法和激光雷达建图算法的位姿估计结果并输出最终位姿估计。LeGO_LOAM 依赖于PCL、OpenCV、GTSAM 三个库。其中，GTSAM 是一个在机器人领域和计算机视觉领域用于平滑和建图的C++库[16]，它与g2o 不同，后者采用稀疏矩阵的方式求解一个非线性优化问题[17]，而GTSAM 则是采用因子图（factor graphs）优化方法和基于贝叶斯树的iSAM2方法的方式最大化后验概率。

2.3 A_LOAM 算法特点

A_LOAM 是LOAM 的高级实现，其原理类似于LOAM，使用Eigen 和Ceres Solver 来简化代码结构。相比于LOAM，该算法的可读性更高、非常适合学习、无需复杂的数学推导和冗余操作。尤其是雅各比的部分，LOAM 对其的推导方法为了解析地求出雅各比的表达式使用了一些小技巧来统一不同特征点的残差函数导数表达，较难理解；而A_LOAM 直接使用Google 开发的在C++环境下的非线性最小二乘问题的自动求导工具包Cere Solver[7]72，节省开发的时间，得到准确的解。整个SLAM 最复杂的运算不需要推导。

3 软硬件实验平台

为验证算法的可行性、可靠性、有效性、准确性和优劣性，已在环境复杂度和路径长度依次增长的新松智慧产业园三期C5 厂区外围及停车场、公开数据集KITTI 的五个大尺度室外场景中进行了LOAM及其改进算法LeGO_LOAM 和A_LOAM 的相对位姿估计误差实验。实验选择一点作为移动机器人的行驶起点，同时也将该点设为移动机器人的行驶终点，即移动机器人路径为一个闭环。行驶路线为从起点开始，行驶一段距离，分别围绕C5 厂区外围一周和停车场，最终驶回初始位置。实验环境中既有结构化场景又有非结构化场景；移动机器人行驶路面整体较为平坦，但也有局部的起伏；行驶距离分别约为300m、650m、960m、2200m 和3700m，实验环境的规模与复杂程度符合预定要求。如图4 所示，即为从新松智慧产业园三期C5 厂区外围和停车场中拍摄的实验场地照片。

图4 新松C5 厂区室外场景

如图5 所示为所搭建的硬件实验验证平台——新松室外移动机器人平台。它的中层配备了1.8GHz四核处理器的小米笔记本电脑，接收各种信息进行处理的工作。上层是三维激光传感器Velodyne VLP-16，是系统的传感单元。惯性导航使用A0010。底层是移动机器人BlueCar，它是整个系统的执行单元，最高移动速度为1m/s。

图5 实验硬件平台

选取Velodyne 公司生产的型号为Velodyne VLP-16 的激光传感器，如图6 所示。该型号保留了电机转速可调节功能，能够实时上传周围距离和反射率的测量值，具有100 米的远量程测量距离。Velodyne VLP-16 的外观设计十分精巧，重量只有830g，非常便于安装，尤其适于小型无人机和小型移动机器人，对移动机器人的大众市场需求来说有重大的意义。Velodyne VLP-16 具有每秒高达30 万点数据输出、±15°的垂直视场和360°的水平视场扫描，支持两次回波接收，可以测量第一次回波和最后一次回波的距离值和反射强度值，能够扫描探测出16 个层面的环境信息，而成本较之于64 级的下降了10 倍，因此多应用在汽车、建图测量、无人机、机器人导航避障、环境三维建模、自动化等行业。

图6 Velodyne VLP-16 激光传感器实物图

导航系统通过USB 接口将激光获得到的距离数据传输给笔记本电脑，用于精确的地图构建。此处选取基于MEMS（微机电系统）惯性传感元件的超小型、高精度惯性测算系统A0010 陀螺转角仪，如图7所示。该系统可在任意时刻、任意运动状态下精确输出载体的航向角以及其它辅助传感信号（加速度、温度等）。由于系统采用了可借助先进MEMS 技术进行标准化、可批量化大规模生产的元器件，大幅度降低了产品成本。该模块可被广泛应用于AGV 小车、航空、航天、航海、无人机、机器人、工业设备监控、汽车电子等场合。

图7 高性能陀螺传感器A0010

选取Ubuntu 16.04 操作系统下Kinetic 版本的ROS 作为软件平台。ROS 的众多优点，如点对点设计、支持多种编程语言、代码复用及免费与开源等，使其成为机器人研究的热点。导航系统的调试及界面显示使用ROS 中的可视化工具Rviz，可在其中构建移动机器人模型和环境地图，同时实时查看机器人从初始点到设定目标点之间执行导航任务的过程。可利用ROS 中的tf 坐标转换功能包完成激光传感器和移动机器人坐标的自动转换。

在此采用的数据集测试平台是公开数据集KITTI。KITTI 由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办，是目前国际上最大的自动驾驶场景下的计算机视觉算法评测数据集。该数据集用于评测立体图像、光流、视觉测距、3D 物体检测和3D 跟踪等计算机视觉技术在车载环境下的性能。该数据集主要涵盖三种类型的环境：周围有建筑物的“城市”，在场景中有植被的“乡村小路”，以及道路宽阔、周围环境相对干净的“公路”。KITTI 包含这些场景下采集的真实图像数据，每张图像中最多达15 辆车和30 个行人，还有各种程度的遮挡与截断。整个数据集由389 对立体图像和光流图、39.2km 视觉测距序列以及超过200k3D 标注物体的图像组成，以10Hz 的频率采样及同步。

如图8 所示，公开数据集KITTI 采集平台包括一个Velodyne 3D 激光雷达、两个彩色立体摄像机、两个灰度立体摄像机、四个光学镜头以及一个高精度GPS 导航系统。激光雷达数据以10Hz 的频率记录。图9 给出了两个来自城市住宅区环境的相应视觉图像。

图8 公开数据集KITTI 的数据采集平台

图9 序列的视觉图像

4 实验结果及分析

为验证地图的全局一致性，在此进行多次实验，将每个实验的初始位姿设置为[0，0，0，0]作为真值，在LOAM、LeGO_LOAM 和A_LOAM 算法中将移动机器人返回起始位置时计算的最终位姿数据取多次实验的平均值并导出，与真值进行对比，从而比较相对位姿估计误差，如表1 所示。从表中可见，对于前四个场景，有IMU 辅助的新松“C5”大楼周围的道路及停车场和序列05 的一致性较好。在五个场景中，LeGO_LOAM 和A_LOAM 的误差都相对较小。对于前三个场景，有IMU 辅助的新松“C5”大楼周围道路及停车场中，LeGO_LOAM 和A_LOAM 的误差在1m以内。在新松“C5”大楼周围的道路中，LeGO_LOAM的位姿估计精度，相对于LOAM，在平移和旋转上分别提高了91.67%和26.13%；A_LOAM 的位姿估计精度在平移和旋转上分别提高了83.33%和70.13%；在新松“C5”大楼周围道路及小范围停车场中，LeGO_LOAM 的位姿估计精度在平移和旋转上分别提高了95.15%和24.97%；A_LOAM 的位姿估计精度在平移和旋转上分别提高了86.94%和53.94%；在新松“C5”大楼周围道路及大范围停车场中，LeGO_LOAM 的位姿估计精度在平移和旋转上分别提高了91.79%和81.11%；A_LOAM 的位姿估计精度在平移和旋转上分别提高了91.79%和83.54%；在序列05 中，LeGO_LOAM 的位姿估计精度在平移和旋转上分别提高了18.32%和96.06%，A_LOAM 的位姿估计精度在平移和旋转上分别提高了24.50%和97.09%；在序列00 中，LeGO_LOAM的位姿估计精度在平移和旋转上分别提高了92.70%和92.03%。A_LOAM 的位姿估计精度在平移和旋转上分别提高了92.41%和80.83%。

表1 移动机器人返回起始位置时的相对位姿估计误差

结果表明，由于LeGO_LOAM 具有闭环检测功能且A_LOAM 的算法复杂度较低，因此与LOAM 相比，LeGO_LOAM 和A_LOAM 可以达到更小的相对位姿估计误差。

5 研究难题与思路

5.1 需进一步研究的问题

虽然三维激光SLAM 已得到深入的研究，但在以下两方面还存在问题：

1）LOAM 及其改进算法A_LOAM 无法实现闭环。由于LOAM 及其改进算法A_LOAM 是一个非常完整的系统，所以在LOAM 及其改进算法A_LOAM中集成闭环并不是一种简单易行的方法。未来要做的工作主要涉及通过闭环修复LOAM 及其改进算法A_LOAM 的运动估计漂移。

2）仅使用视觉传感器或激光传感器在SLAM应用中都有局限性和缺点。一方面，视觉测距方法中的视觉传感器，例如深度相机，需要为每个图像计算ORB 特性，这需要很长时间，在特征点稀少处无法正确定位，且需要适度的照明条件，如果不能充分获得不同的视觉特征则会失败。在极端情况下，特征点提取和匹配算法由于外部光线的干扰而无法追踪特征点。另一方面，由于激光雷达在连续运动期间的不同时间接收距离测量导致通过移动激光雷达的运动估计涉及点云中的运动失真，因此运动通常必须用大量变量来解决。在诸如由平面区域支配的退化场景中，扫描匹配也难以奏效。

基于激光和视觉传感器融合的SLAM 方法能够有效利用各个传感器的优势，弥补传感器在某些特殊环境下的劣势，成为当前研究的热点之一。还有待做出更多的研究工作，进一步实现将视觉传感器与激光传感器融合并，研究融合算法，使之提高导航的精度和鲁棒性。

5.2 未来研究思路及展望

鉴于上述问题的存在，今后的研究应主要应在以下方面实现突破：

1、LOAM 及其改进算法A_LOAM 的闭环检测方面。具体思路有两个：第一，使用FPGA 代替ROS功能包从激光雷达波束中心提取点的传感器中提取信号，并将其与ANN（人工神经网络）与GA（遗传算法）相结合，以更好地更新当前状态；第二，鉴于LOAM 及其改进算法A_LOAM 的映射是提高SLAM性能的关键，应在LOAM 及其改进算法A_LOAM 的低频高精度的环境建图模块中修改相关参数。

2、传感器融合方面，借鉴例如张吉等人提出的V_LOAM 方案[18]。针对传感器数据融合这一关键问题，采用如估计理论数据融合、分类数据融合、推理数据融合、人工智能数据融合等方法，根据所处环境合理地选择单个传感器或传感器融合数据作为关键帧构建地图[19]，这种根据环境状况合理选择传感器数据的方式能够降低数据融合的计算复杂度并且更好地适应于复杂的动态环境。

除此之外，在后续研究中，还将研究原始点云的滤波方法，以快速去除动态物体并提高抗干扰能力；同时，将应用范围扩大到地下车库场景，以解决无人驾驶车辆的自动停车问题。

6 结 束 语

对三维激光SLAM 技术进行了探讨总结，从激光雷达测距与建图算法LOAM 及其改进算法LeGO_LOAM 和A_LOAM 的关键组成模块及其具体作用进行了深入和充分的回顾与归纳。鉴于LOAM 及其改进算法LeGO_LOAM 和A_LOAM 的低漂移、强实时的特性，以新松智慧产业园三期C5厂区外围及停车场为实例，详细讨论分析了大尺度室外环境，以及公开数据集KITTI 下的LOAM 及其改进算法LeGO_LOAM 和A_LOAM 的相对位姿估计误差。实验结果表明，相对于LOAM，LeGO_LOAM和A_LOAM 可以达到更小的相对位姿估计误差。对于当前存在的问题及难点，也提出了详细合理的研究思路及展望。