图 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四核处理器的小米笔记本电脑，接收各种信息进行处理的工作。上层是三维激光传感器 VelodyneVLP-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 序列的视觉图像