基于激光雷达的室外实时闭环检测算法*

陈学攀 任明武

（南京理工大学 南京 210094）

1 引言

在过去的几十年里，随着云计算、5G、人工智能的快速发展和普及［1］，自动驾驶、移动智能机器人等智能化自主系统逐步进入大众视野，不断影响着人们的日常生活，这不仅包括常见的家用扫地机器人、仓储智能移动物流机器人，还包括目前突飞猛进的无人驾驶汽车，甚至是目前出现的新型无人机送货服务［2］。基于激光雷达的同时定位和建图（Simultaneous Localization and Mapping，SLAM）技术在面对无GPS 信号或未知的环境中表现出了其独具优势的一面［3］，包括无需预先布置场地、不存在尺度漂移、受环境光照变化影响小、可生成三维点云地图用于导航等。状态估计、定位和建图作为实现反馈控制、避障和规划等许多功能的必要条件，实现成功自主运行的先决基础条件［4］。

然而，在面对室外规模场景环境时，无论是机器人还是自主驾驶汽车其在无绝对信号输入的前提下，长时间SLAM 算法运行时不可避免地会累积误差［5］，从而导致不正确的位姿估计。闭环检测作为SLAM 算法中的一个重要模块［6］，它是指机器人或移动的车辆是否能够识别当前位置之前是否已经过，是消除累积里程漂移误差实现更准确定位、构建更精确的全局三维点云地图的最有效途径［7］。然而，在面对大规模室外环境下，关键帧的数目随着SLAM 算法的长时间运行其不断增长，这会使得闭环检测算法的计算时长以及会面对反向匹配、不同车道匹配、旋转匹配等多种情况，这时提出一种高效准确的闭环检测算法以面对各种情况，通过多种形式的传感器数据实现对此前已经经过的位置进行识别用于更新当前移动平台的估计位姿就显得非常重要［8］。

本文通过改进现有闭环检测算法SCAN-CONTEXT［9］提出的全局描述符，提出了一种新型的基于位置和改进的点云全局描述符相融合的闭环检测算法，并设计了快速新型的适用于本算法的点云帧间相似度匹配计算公式，最后使用时间一致性和基于OpenMP 加速的FPFH［10］点云特征以提供初始初始位姿估计，随后基于此初始估计位姿使用ICP［11］匹配来进行几何一致性校验以可能避免误匹配。

2 相关工作

闭环检测问题也即位置识别问题，其被定义为当机器人在环境中移动时，传感器测量数据会随着时间的增加而形成一组序列化数据，其中每个传感器测量数据zt是指t时刻在某一空间位置lt获取的［12］。所以，位置识别就可以定义为在由测量数据形成的数据库地图中为当前位置查询找到之前已经访问过的位置的测量数据，然后找到匹配关系借助传感器数据进行相对位姿变化输出。位置识别算法主要由两个模块组成，第一个为了轻量化处理原始测量数据，原始测量zt进行更紧凑的编码，也即使用描述函数f(·) 的描述模块。第二个是通过相似度函数(·) 来度量两个原始测量描述符相似度的度量模块。

SLAM 算法根据主要传感器分为视觉SLAM 和激光SLAM 两大类［13］。基于视觉的闭环检测研究已经进行了很长一段时间，并提出了许多优秀成功的方法。大多数基于图像的方法提取特征描述符，然后用单词袋（BoW）［14］，VLAD［15］和Fisher Vector（FV）［16］等方法进行编码。

与基于的视觉闭环检测研究的蓬勃发展相反，基于激光雷达的研究在解决这一全局定位问题上仍缺乏强有力的解决方案。由于激光雷达数据对光照和季节变化更加稳健，因此也逐步受到关注，大多数现有的激光雷达闭环检测方法使用局部结构和点分布等低层次特征，由于些特征对遮挡和旋转变化敏感，因此不适用于大规模室外环境。Bosse 和Zlot［17］从局部三维描述符的数据库中为每个关键帧位置查询恒定数量的最近邻进行投票，并将具有足够数量投票的地点确定为可能的匹配位置。然而，这种局部关键点特征通常缺乏分辨性描述的能力来区分相似的局部结构，因此不够可靠。Scan Context 方法保留在投影到二维极坐标系下栅格的最大高度，为了实现旋转不变性而逐行计算所有可能的列移动距离，为得到两帧点云描述符之间的最小距离，需要较长搜索时间并且保留栅格单点最大高度使得算法的稳定性不够。

3 本文算法描述

3.1 全局描述符编码

由于每帧激光雷达均携带大量数据，本文使用的禾赛Pandar40 型号激光雷达其每帧数据点数超过七万个，因此可利用旋转激光雷达的特性将三维点云按旋转方向和激光发射器射线方向进行极坐标系扇形栅格投影到二维平面，点云中某一点pi其对应的投影点坐标为

其中，ri为投影之后的行索引，si为投影后的列索引，Nr为行最大值，Ns为列最大值，di为pi距离激光雷达局部坐标系原点的距离，θi为水平方位角，Tr、Ts为激光发射器射线方向和激光雷达旋转方向的栅格分辨率。

原始Scan Context算法在生成每个栅格特征时使用投影到当前栅格的所有原始点云点的高度也即z轴方向上的最大值，但是仅靠单点即代表本栅格的特征必然会存在一定的偶然性，其特征不够稳定。在分析以上问题后，本文提出基于投影到某栅格的点云点取三个高度的值的平均值，如果不足三个点在计算平均高度时仅计算有效点数取均值。这使得特征更加鲁棒，降低噪点对整体算法的影响。

3.2 基于欧氏距离和范数的快速特征相似度匹配

在得到特征矩阵后，为实现满足旋转不变性的快速匹配，本算法采用和Scan Context 相同的基于行向量均值建立Kd-Tree 索引，快速选取候选关键帧。

在得到候选关键帧之后，为进一步验证是否正确的进行位置识别需要进行当前帧和候选帧之间的特征矩阵匹配，本算法改进使用两列向量范数之间的欧氏距离来改进原有使用列向量余弦值进行相似度计算的方式，实现特征矩阵相似度的快速计算，提高了效率，具体步骤为

1）将特征矩阵分解为列特征向量，得到两特征矩阵对应列的范数值p和q，按照式（3）计算其相似性。

2）将特征向量每次循环右移一个位置，再将得到的新的特征矩阵，直至移动次数正好小于列数减一时截止，每次移动均计算与当前帧特征向量的相似度，选择循环移动时最大的相似度。

其中，qw为候选帧循环右移时得到的新的特征矩阵。

3）但是由于基于特征矩阵的相似度匹配可能在面临场景存在动态目标或行驶车道等发生变化时，此相似度就会发生较大的改变，因此本算法加入两帧之间位置的欧氏距离判断来进行弥补，改进了相似度计算的方式以实现更鲁棒的闭环检测算法，如下所示：

其中，RS为基于特征矩阵匹配的阈值，RE为基于欧式距离回环匹配的阈值，最终相似度D(p，q) 分数越低越好，α和β分别为两得分系数。

3.3 一致性校验

使用时间一致性和基于OpenMP 加速的FPFH点云特征以提供初始初始位姿估计，随后基于此初始估计位姿使用ICP 匹配来进行几何一致性校验以可能避免误匹配。

4 数据和实验

本实验采用的是基于数据采集车在校内采集的一段长达8km左右的室外数据，数据量大其满足室外大规模场景，可有效验证本文提出的算法的有效性，在此数据中有较多的回环，不仅存在正向回环也存在逆向回环。

为有效验证本文提出闭环检测算法的有效性，共进行了三组实验，第一组为闭环检测为简单的基于欧式距离的形式，第二组实验闭环检测算法为原始Scan Context 算法，第三组实验为本文提出的算法，其最终位姿预测的绝对误差结果如表1所示。

表1 实验结果

图1 数据采集车

图2 数据采集路线

三组实验的结果如图3 所示，可以基于本文的回环检测算法的SLAM 系统取得了最后的位姿估计结果，在满足实时性的前提下，可基于本算法有效的对里程累积误差进行消除，提升整个SLAM 算法的位姿估计的准确性和建图的精确性。

图3 回环检测算法结果对比

特别是，原始Scan Context 算法在本实验逆向再此经过图4 位置时不能正确的进行回环匹配，而由于本算法加入了欧氏距离的匹配因素，因此即便在基于特征矩阵不能很好地进行匹配时，本文提出的算法仍可以有效地校验出回环，以完成位置识别，如图5所示。

图4 基于Scan Context未检测到反向回环

图5 基于本文算法成功反向回环匹配

5 结语

本文提出了一种可适用于室外大规模场景的基于激光雷达数据的闭环检测算法，点云数据作为一种非结构化的数据，没有恒定的点云密度且数据稀疏性随着距离的增加而增加，环境目标对象的密度也即分辨率随传感器也发生较大改变。本文在分析以上问题后，提出了一种基于欧式距离和全局特征描述符相融合的闭环检测算法，并通过实验验证了其相比于当前流行的闭环检测算法存在的优势，在之后的研究中，由于室外场景车辆行驶的车道会经常发生变化，可着重对此问题进行解决。