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 厂区外围和停车场中拍摄的实验场地照片。