基于快速回环检测的室外环境下激光雷达SLAM 算法

徐晓苏，李 诺，姚逸卿

（1. 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2.东南大学 仪器科学与工程学院，南京 210096）

同步定位与地图构建（Simultaneous Localization and Mapping,SLAM）是移动机器人在未知环境中实现自主导航[1]的一项基本要求。激光雷达SLAM 是利用激光雷达传感器进行自身位姿估计和构建周围环境地图的一项技术。由于激光雷达具有测距精度高、视角广的优势，并且对光照不敏感等特点，激光雷达SLAM 在自动驾驶、无人自主导航领域已取得较大的发展。作为激光雷达SLAM 的关键环节，回环检测通过计算帧间点云相似性判断移动机器人是否经过同一位置，并在后端位姿图中增加闭环位姿，最终利用位姿图优化校正位姿并生成全局一致的点云地图。尤其是在室外环境下，没有回环检测会导致激光雷达SLAM 位姿误差随时间累积，从而影响SLAM 后端建图的精度和效果。

针对回环检测，Shan 等人于2016 年提出了LeGO-LOAM[2]，该算法引入地面优化进行位姿估计，并集成了回环检测校正轨迹漂移，但是存在回环检测失败或漏检的情况；Kim 等人于2018 年提出了一种用于回环检测的扫描上下文Scan Context[3]，该算法不依赖直方图[8]和先验信息实现回环帧的搜索和匹配，但是该算法仅有最大高度信息被编码在图像中，会丢失大量信息，并且采用了暴力匹配方案，耗时增加；Lin等人[4]提出了一种基于2D 直方图的回环检测算法，该算法采用2D 直方图的相似性作为当前关键帧与地图中关键帧的相似性度量，该方案克服了旋转的影响，产生了可靠和准确的回环检测；Shi 等人[5]在Scan Context 的基础上加以改进，通过建立局部参考帧，使其对视点变化更具鲁棒性，并提出了一种三阶段匹配算法进行有效的回环检测，有效地提升了回环检测的精度和速度；Intensity SLAM[6]在Scan Context 基础上提出了强度扫描上下文描述符用于回环检测，并采用基于强度图和几何特征的方式进行回环帧间配准，有效提高了定位精度和建图准确性；仉等人[7]于2019 年提出了一种基于四元数卷积神经网络的闭环检测算法，利用超像素分割和四元数CNN 对图像进行处理，有效减少了视觉里程计的累计误差；Jiang 等人[8]于2020 年提出了一种基于特征的回环检测算法，采用平面特征点的局部分布来计算特征方向，构建点云子地图的描述符，通过过滤并增强结构化环境中的垂直障碍物权重，有效地提高了回环检测算法的效率；郑等人[9]于2020年提出了一种基于惯导和激光雷达的采煤机定位算法，该算法以激光雷达为辅助，基于组合导航系统卡尔曼滤波模型，有效地提高了采煤机的定位精度；Jiang 等人于2020 年提出了基于点云平面的回环检测算法LiPMatch[10]，该算法使用几何约束来进行相似性评估，利用平面的法向量约束检查符合几何约束的候选关键帧，最后通过改进的迭代最近点方法验证；Wang 等人[11]于2020 年提出了一种新的全局描述符Lidar Iris，该算法采用滤波和阈值计算在由Lidar Iris 表示的图像上获取特征图像，通过计算两帧点云之间的特征图像的汉明距离计算相似度，实现了快速和精确的闭环检测；Kim 等人于2021 年在Scan Context 的基础上提出了鲁棒性的回环检测描述子Scan Context++[12]，该算法增加了对横向和旋转变化的鲁棒性，并改进了暴力匹配效率低下的问题。

本文针对现有的激光雷达SLAM 方法在室外环境下位姿误差随时间积累导致定位精度低、回环检测速度慢、建图准确度不高等问题，提出用扫描上下文（Scan Context）提取全局特征，用Kd-tree 加速搜索获取候选的回环帧，并结合激光雷达虹膜（Lidar Iris）计算回环帧间的相似度得分，通过基于残差项加权的特征匹配获取回环帧间的位姿变换，有效提升回环检测的速度，最终生成全局一致的点云地图。

1 A-LOAM 算法

1.1 点云预处理

A-LOAM 算法流程框图如图1 所示，从激光雷达采集的原始点云数据量大，在进行激光里程计位姿估计前需要对点云数据进行预处理，主要包含点云滤波和特征点云提取。

图1 A-LOAM 算法流程框图Fig.1 A-LOAM algorithm flow diagram

首先要滤除原始点云中的不可靠点云，这些点云最主要是因为距离激光雷达太近或太远，使得激光雷达返回信号变弱甚至丢失，导致数据出现丢失。因此，本文利用点云PCL 库中的无效点去除函数将这些点云作为噪声点去除。进行点云滤波后的每一帧点云提供了在空间直角坐标系中以激光雷达中心为原点的x、y、z值和激光的反射强度信息。

在特征提取阶段，将滤波后的点云根据曲率提取边缘特征点和平面特征点。对于给定的第k帧点云，记表示第m个和第n个点云的坐标（上标L表示激光雷达坐标系），S表示第m个点的临近点数量，为了保证能正确反映点云的局部信息和实时运算速度，一般取10。利用激光雷达局部曲面点云的曲率对点云进行特征提取，记第m个点云的曲率为c，计算公式为：

1.2 激光里程计位姿估计

对于第一帧点云，直接存储在特征子地图中；对于第k(k≥2)帧点云，通过体素滤波对点云降采样，与上一帧生成的全局特征子地图匹配，利用匹配得到的位姿将第k帧点云加入到上一帧生成的全局特征子地图中，然后对全局子地图进行阈值划分，提取距激光雷达合适区域的点云。

其中，ρ为鲁棒核函数，N(e)、N(s)表示角和面特征点的个数，通过高斯牛顿法求解此非线性方程得到激光里程计位姿估计。

2 基于快速回环检测的激光SLAM 算法

2.1 算法整体框架

本文算法在机器人软件平台（Robot Operating System，ROS）上实现基于快速回环检测的室外环境下激光SLAM 方法，其整体流程图如图2 所示。

图2 算法流程框图Fig.2 Algorithm frame work

在室外环境下，传统的激光雷达 SLAM 如A-LOAM 等由于缺乏回环检测，位姿误差会随着时间积累导致定位精度低甚至定位失败，从而建图准确度不高。基于迭代最近点（Iterative Closest Point,ICP）匹配的回环检测算法存在匹配耗时、实时性不高，并且在室外环境下，经过递推获得的回环帧间初值已经存在较大累计误差，依然用此初值会陷入局部最优，导致迭代不能收敛到正确结果。针对传统A-LOAM 存在的问题，提出利用增加混合Scan Context和Lidar Iris的回环检测模块和位姿图优化模块。在激光里程计线程中，输入为激光雷达得到的点云，对点云进行角点和面点的提取，然后进行当前帧与局部地图匹配，通过点到直线和点到平面的距离构建残差方程，迭代求解残差方程获得激光里程计位姿。在建图线程中，点云经过由激光里程计线程获取的位姿变换到世界坐标系下，建立点云特征地图。在回环检测线程前，进行关键帧的选取，然后利用关键帧点云获取Scan Context描述子，通过Kd-tree 查找与当前关键帧相似的候选关键帧，然后利用Lidar Iris描述子进行特征图像选取，计算关键帧与候选关键帧之间的相似度得分，获取与当前关键帧最相似的最优关键帧。最后通过基于残差加权的特征匹配约束求解当前关键帧与最相似的候选关键帧之间的位姿变换，并将此变换加入到位姿图中，利用全局位姿图优化进行求解，获取经回环校正后的激光SLAM 位姿和全局一致的地图。

2.2 关键帧选择

在获取激光里程计位姿之后，需进行关键帧的选取。考虑到回环帧间点云配准是由特征匹配而不是ICP 匹配完成的，在关键帧中需要保证有足够多的特征点云。如果满足以下所有条件，则将激光雷达帧选取为关键帧：1）提取的角点和面点的特征点数量都需大于阈值εnum，以保证在回环检测中有足够的特征进行点云配准；2）里程计的平移变化显著，需大于阈值εdis；3）里程计旋转变化显著，大于阈值εdeg。在室外环境下，设置更高的阈值可以降低成本，选取的关键帧存储在位姿图中。

2.3 提取描述子

假设激光雷达最大探测距离为Lmax，以激光雷达为原点建立极坐标系，并将以Lmax为半径的圆沿径向划分为Nr个环区，沿角度方向划分为Ns个扇区，形成弯曲网格，然后计算每个特征点云的极径和极角，获取该点云弯曲网格坐标，并将每个网格中点云的最大高度作为网格值，沿径向方向扩展为二维图像。

经过上述过程，最终Scan Context 表示为N r×Ns的二维矩阵r，如图3 所示，图中不同的颜色表示每个弯曲网格内的点云最大高度。

图3 Scan Context 描述子矩阵Fig.3 Scan Context descriptor matrix

2.4 快速的最优回环帧选取算法

由于Scan Context 描述符在选取最优回环帧时采用暴力匹配，存在耗时的缺点，而Lidar Iris 特征则能快速高效的选取最优回环帧。因此，本文在识别回环帧同时利用Scan Context 和Lidar Iris 特征进行最优回环帧的选取。在这一模块，本文提出了一种快速的回环帧选取算法。算法的具体流程和步骤如下：

第一步：利用Scan Context 从关键帧中提取环向量作为描述符，每个环向量都被用来构造Kd-tree，在Kd-tree 中通过最近邻搜索选取与当前关键帧的环键相似的环键对应的关键帧作为回环候选关键帧。

使用L0范数计算 Scan Context 环向量平均值φ(rm)，计算公式为：

式中，rm表示第m行环向量，rj,m表示第m行向量的第j个元素。

利用Nr维φ(rm)组成对旋转不敏感的描述符环向量，记为向量k，可表示为：

相比于二维矩阵Scan Context，环向量表示的信息更少，但是用做对于回环候选帧的选取具有更快的速度。

第二步：利用Lidar Iris 对多个回环候选关键帧进行编码，生成Lidar Iris 图像，使用快速傅里叶变换对Lidar Iris 图像进行处理并对齐图像，然后通过Log-Gabor 滤波器提取特征，利用获取的二值化图像进行匹配求解两片点云之间的变换，采用Hamming距离表示两幅Lidar Iris 图像之间的相似性。对于多个回环候选帧，选取Hamming 距离最小的候选帧作为最优回环帧。

如图4 所示，通过八位二进制码对每个弯曲网格内点云的高度信息进行编码，从激光雷达特征点云中提取Lidar Iris 图像，图像的像素强度用八位二进制码转化的十进制数来表示，通过Log-Gabor 滤波器从Lidar Iris 图像中提取特征：

图4 提取Lidar Iris 图像Fig.4 Extract Lidar Iris image

其中，G(f)表示滤波器的频率响应；f表示滤波器的中心频率，依据激光雷达Iris 图像生成，f0、σ为滤波器的参数，在Log-Gabor 滤波器中提取图像Iris特征时，为定值，取0.75。滤波的结果为一个相位二值化图像和一个幅值二值化图像。

第三步：利用位姿阈值判断唯一的最优关键帧。为了剔除与当前帧Lidar Iris图像高度相似但是距离较远的最优回环帧，我们采用当前帧与最优回环帧之间的位姿平移量是否小于某一阈值进行判断。此方案既保证了效率，又能有效地剔除实际不可靠的最优候选帧。记表示当前点云帧C到世界坐标系W的位姿变换，表示回环帧到世界坐标系的位姿变换，当前帧到最优回环帧的位姿变换表示为：

计算上述两帧之间的位姿平移量t，如果小于阈值dthre，则粗略检测回环成功：

其中，t x、t y和tz分别为t的x轴、y轴和z轴的分量，dthre取值为25。利用此方案能有效地减少错误的回环检测，从而提高SLAM 效率。

2.5 加权的回环帧间点云配准

本文采用的回环帧间点云配准方式为基于特征点的匹配方式，与1.2 节相似。基于ICP 的方式需要一个较好的初值作为迭代初值，但是在室外环境下，经过递推获得的回环帧间的初值已经存在较大累计误差，依然用此初值会陷入局部最优，导致迭代不能收敛到正确的结果。

考虑特征点云的位置和局部特性，根据特征点云到激光雷达的距离、特征点云的强度信息以及特征点云到平面或线的距离设计权重模型。记wi为参与点云匹配的第i个特征点云的总权重，其由三个权重因子组成：

其中，每个特征点云的权重wi、wj通过式(10)计算。

3 实验结果及分析

本文采用KITTI 数据集[13]来验证本文算法在室外环境下的定位精度、回环检测效率和建图效果。该数据集包含22 个不同室外环境的数据序列，11 个序列（00-10）提供了由GPS 获取的相机轨迹真值，每个数据序列包含灰度图像、彩色图像和激光点云数据，由两个高分辨率的彩色和灰度相机以及velodyne 激光雷达获取。该数据集已成为室外验证视觉和激光SLAM 算法性能的基准数据集。

验证数据集的实验平台为具有16GB 运行内存、AMD Ryzen5-5600H 计算处理单元的个人电脑，操作系统为Ubuntu18.04LTS，ROS 版本为melodic。

本文采用带有环路闭合的KITTI 里程计数据，即序列00、05、06 和08 来验证算法，四个序列的环境、位姿数量和路程如表1 所示。此外，我们还采用无人车（Unmanned Ground Vehicle,UGV）实验平台在东南大学四牌楼校区记录的序列对算法进行了评估，UGV 实验平台如图5 所示。

表1 KITTI 数据集四个室外环境序列信息Tab.1 Four outdoor scene sequence information in the KITTI dataset

图5 实验平台Fig.5 Experiment platform

本文使用的评价指标为KITTI 数据集定义的相对平移误差（Relative Translation Error，RTE）和相对旋转误差(Relative Rotational Error，RRE)：

其中，Erot表示相对旋转误差（RRE），Etrans表示相对平移误差（RTE），i和j表示帧号，l(i,j)表示第i帧和第j帧之间的距离，F表示为一个KITTI 序列帧的集合，Rot(·) 和Trans(·) 分别表示取位姿的旋转量和平移量，⊗符号表示计算两帧之间的相对位姿变换，和p分别表示位姿的估计值与真实值。

3.1 位姿精度实验

为了验证本文算法在室外环境下的性能，我们使用KITTI 数据集的00、05、06 和08 序列分别对本文的算法与真值进行比较。这四个序列行驶路程均超过1000 m，并且包含回环检测，能较合适地验证我们提出的改进回环检测算法的有效性，图6-9 为在四个序列下本文算法（在图中用our表示）、A-LOAM与KITTI数据集真值的轨迹对比。

图6 KITTI00 俯视图轨迹Fig.6 KITTI00 top view Trajectory

图7 KITTI05 俯视图轨迹Fig.7 KITTI05 top view trajectory

图8 KITTI06 俯视图轨迹Fig.8 KITTI06 top view trajectory

图9 KITTI08 俯视图轨迹Fig.9 KITTI08 top view trajectory

表2 为A-LOAM 算法与本文算法在四个室外环境下的相对平移误差和相对旋转误差。可以看出，与A-LOAM 算法相比，本文算法的RTE 和RRE 都明显减少，其中平均RTE 减少了0.9404%，平均RRE 减少了0.0032 °/m，平均性能提升50%左右。

表2 A-LOAM 和本文算法的RTE 和RRE 指标对比Tab.2 Comparison of the RTE and RRE between A-LOAM and our algorithm

3.2 回环检测实验

为了验证本文算法在回环检测部分的性能，选取室外环境下05 序列来测试本文算法。如图10 所示，A-LOAM 由于没有回环检测，产生的累计漂移使得轨迹产生了漂移，而本文算法应用全局描述子进行回环检测，能够准确地搜索到正确的回环关键帧，并且能够通过位姿图优化减小累计误差的影响。

图10 KITTI05 轨迹比较Fig.10 KITTI05 Trajectory comparison

为了体现本文算法在回环检测算法的快速性，在获取候选回环帧后，计算Lidar Iris 描述子和Scan Context 描述子提取最优回环帧的时间。表3 为本文回环检测算法与Scan Context 回环检测算法的耗时性比较，可以看出，在提取最佳回环帧的阶段，本文采用的 Lidar Iris 方案提取最佳回环帧过程耗时仅为150 ms 左右；在回环帧间点云配准阶段，可以从图11看出，本文算法较传统的ICP 方案，极大地缩短了回环帧间配准时间。因此，本文算法能够更快的执行回环检测。

表3 耗时性比较Tab.3 Time-consuming comparison

图11 回环帧间配准时间对比Fig.11 Comparison of registration time between loop frames

3.3 实测建图实验

为了验证本文算法在室外环境下的准确性，用UGV 实验平台在东南大学四牌楼校区记录的序列对算法进行评估，卫星地图如图12 所示，蓝色区域为回环部分。图13 为本文算法建图，图14 为ALOAM 算法建图，从图中可以看出，A-LOAM 算法建立的地图存在累计漂移，而本文算法建立的地图消除了大部分由累计漂移产生的“鬼影”。因此，本文算法具有较高的实际场景鲁棒性和定位精度。

图12 卫星地图Fig.12 Satellite trajectory map

图13 本文算法建图Fig.13 Mapping of our algorithm

图14 A-LOAM 建图Fig.14 Mapping of A-LOAM

4 结论

本文提出了一种基于快速回环检测的室外环境下激光SLAM 算法。在回环检测上采用扫描上下文描述子粗选取多个回环帧，再利用激光雷达虹膜描述子精选取最优的回环帧，有效的提高了回环帧的选取效率。在回环帧的匹配上，采用残差项加权的特征匹配方案，快速准确地实现了回环帧间的配准。通过KITTI 数据集对本文算法进行实验验证，实验结果表明，与A-LOAM 相比，本文算法的平均性能提升50%左右，回环耗时有效减少，提升了室外环境下位姿估计精度和建图准确度。在实测的室外实验中，与A-LOAM 算法相比，本文算法在场景建图的鲁棒性上有较大提升。