崔 洋，顾恒之，徐 震

（长安大学 汽车学院，陕西 西安 710064）

随着现代汽车产业的发展，无人驾驶技术成为当前汽车行业的高新前沿研究方向之一。同步定位与地图重建（Simultaneous Localization And Mapping, SLAM）技术是无人驾驶技术中实现无人化的关键技术之一[1]。

国内外学者对基于视觉和激光雷达的 SLAM算法进行了大量的研究[2]。由于激光雷达具有测距精度高、不容易受照明以及视角变化的影响，以及在全天候条件下的工作能力，因此广泛应用于无人驾驶领域，更适合在复杂多变的汽车行驶环境中进行 SLAM。本文基于轻量化和地面优化的激光雷达里程计和地图（Lightweight and Groud Optimized Lidar Odometry and Mapping, LeGOLOAM）算法，并采用迭代最近点（Iterative Closest Point, ICP）算法对回环获取的全局地图进行优化，以提高地图绘制的准确性和鲁棒性，并在长安大学汽车学院周围进行实际建图，以检测算法优化的可行性，从而更好地为无人车感知技术提供技术支持。

1 LeGO-LOAM算法

1.1 LOAM算法以及存在的问题

使用高精度激光雷达进行建图，有时候不能满足无人车对环境的实时测绘与定位的要求，且成本较高。研究者基于规则和广义的 ICP的轻量级环路检测和优化算法，提出了基于广义迭代最近点算法（Generalized ICP, GICP）的3D点云配准的SLAM优化方法，但定位和映射精度仍然需要提高。激光雷达里程计和建图[3]（LOAM）是目前最具有代表性的基于特征匹配的实时三维激光SLAM 算法。他的计算量和运动补偿量较小，但是没有环路检测，导致漂移误差随时间累计，映射精度差，并且在开放环境中存在退化问题。LeGO-LOAM 以在减少计算资源的情况下达到类似甚至更好的精度。

1.2 LeGO-LOAM算法

LeGO-LOAM是SHAN T等人[4]于2018年在LOAM算法的基础上提出的一种基于 3D激光雷达的轻型实时定位和建图算法。它主要由点云分割、特征提取、雷达里程计、雷达建图和变换融合组成，如图1所示。

图1 LeGO-LOAM的系统结构图

1.3 具体过程

为了提高处理效率和特征提取的准确性，通过点云分割模块，将点云划分为不同的簇，并标记为地面点云或分割点[5]。同时，为后续模块获取每个点的三个特征即点的标签（地面点云或分割点）、深度图中的行和列索引以及和传感器的距离值。首先，将点云投影到深度图上，并将点云中的点映射到深度图上的像素点上。在分割之前，估计深度图的地平面以提取地面特征，并将代表地面的点标记为不参与点云分割的点。特征提取模块从地面点或者分割点提取边缘和平面特征，提取过程如下：

（1）计算每个点的平滑度c

式中，S为同一行的连续点Pi的点集，S中有一半位于Pi的两侧，ri和rj分别是从集合S中的点Pi和Pj到激光雷达的欧氏距离。

（2）将深度图水平分割成几个相等的子图像，以均匀地提取特征。

（3）根据设定的阈值cth来分割不同类型的特征。将平滑度c大于cth的点分割成边缘特征点组成集合Fme。将平滑度c小于cth的点分割成平面特征点组成集合Fmp。

（4）再进行一次筛选，得到精炼的边缘点Fe和平面点集Fp，其中Fe∈Fme，Fp∈Fmp。

激光雷达里程计模块在两个连续的帧中估计车的运动，使用从特征提取模块提取的特征在连续扫描帧中找到车的位置变换。

激光雷达建图模块将特征点集{Fe,Fp}中的特征与周围的点云地图匹配，进一步细化姿态变换，然后使用L-M优化得到的最终变换姿态在点云地图的新节点和历史选择的节点之间添加空间约束，并且通过循环检测添加新的约束，然后将姿态地图发送给基于因子图的C++库（Georgia Tech Smoothing And Mapping, GTSAM）进行图形优化，并通过传感器更新估计的姿态。转换模块融合来自激光雷达里程计模块和激光雷达建图模块的姿态估计结果，并输出最终的姿态估计。

2 算法改进

2.1 回环检测

LeGO-LOAM的回环检测模块采用 KD树模型，根据欧几里德距离找到与当前姿态相似的历史姿态及其附近的点云，使用 ICP计算其匹配度并估计姿态，并使用最相似的历史帧的车的姿态约束当前的姿态估计，更新点云地图以获取全局一致性地图。算法计算量大，检测效率低。考虑到实时性和准确性，采用了低频环路检测，在长距离、大场景的建图中仍存在很大的误差。

2.2 扫描上下文算法

扫描上下文算法是KAIST大学的KIM G和KIM A[6]提出的算法，该算法使用非直方图的全局描述符来更快、更高效的搜索数据（当前的以前的数据）。扫描上下文，通过降维将3D点云转化为 2.5D，并使用一种搜索算法匹配当前帧和历史帧的点云数据，实现回环检测。

扫描上下文算法使用一次扫描的点云数据构造矩形图像扫描上下文，并构造 KD树，执行最邻近搜索，找到可能与当前帧回环的多个相似帧；然后，分成扇形区域计算最小偏移量和相似性分数，选择相似性分数最高的帧作为回环帧，求解当前帧和回环帧之间的姿态关系，实现回环检测。

2.3 回环改进

在LeGO-LOAM算法中，进行回环检测环节采用的是根据欧几里德距离找到与当前姿态相似的历史姿态及其附近的点云，使用 ICP计算其匹配度并估计姿态，在此基础上，加上扫描上下文算法，将计算后的姿态约束添加到 GTASM 中进行全局姿态优化，通过扫描上下文算法与 KD树的结合，来对回环检测环节进行改进，从而使得回环检测精度得到提高。通过此方法进行全局建图，可以提高建图精度。

3 实车建图

3.1 建图场景

实际实验场景位于长安大学汽车学院，通过安装在无人车上的镭神16线激光雷达进行围绕汽车学院一周进行建图。所用无人车如图 2所示搭载车载惯性组合导航系统，平台搭载导远INS570D高精度 RTK_SINS组合导航。算法运行计算机为神舟战神Z7M-CT7NA，系统为Ubuntu系统。

图2 实验采用的无人驾驶平台

3.2 实验过程

本次实验通过采用无人驾驶平台，通过环绕长安大学汽车学院的方法进行建图，并且对改进后的算法的外参进行标定。并对所建地图精准度进行对比。重新建图过程如图 3所示，可以看到图中里程计坐标随着车辆行驶而进行变化，这种组合惯导方式构建地图可以为后来无人驾驶车辆更好提供信息。实验过程中，采用镭神16线激光雷达，分别运行改进前以及改进后的算法，通过两种不同算法的建图效果对比来验证实验的可行性，并且更换镭神32线激光雷达，以此验证不同型号雷达对算法的适用性。

图3 无人驾驶平台惯性组合导航系统连接图

3.3 实验结果

实验通过环绕汽车学院行驶一周，在运行改进过后的 LeGO-LOAM算法后，运行过程如图 4所示，运行结束后的建图结果如图 6所示，相比于图 5可以很明显看见建图精度较之前的有所提高。图7是改进后用镭神32线激光雷达进行建图。可以看出更换激光雷达算法的可行性依然适用。

图4 算法的建图运行过程

图5 改进前的镭神16线激光雷达建图

图6 改进后的镭神16线激光雷达建图

图7 改进后的镭神32线激光雷达建图

4 结论

本文针对LeGO-LOAM算法建图中精度低的问题，结合了扫描上下文算法，对回环检测问题进行改进，并且通过对外参进行重新的标定，通过在实际场景下进行建图，并且对建图效果进行对比可知，改进后的LeGO-LOAM算法，可以提高了建图的精度。并且通过更换为镭神32线的激光雷达，通过采用不同型号的雷达的方法来验证实际场景下建图的可行性。