Delay-CLD：一种基于三维激光雷达的闭环检测算法

汪小龙 滕滕

摘要：目的：针对三维激光SLAM闭环检测存在的难以提取高重复性的独特关键点，或者局部匹配始终遭受描述能力缺乏的问题，提出了一种三维激光SLAM闭环检测算法——Delay-CLD。方法：该算法首先利用前端里程计设置一个延迟阈值，然后利用全局3D描述符匹配算法M2DP高效的确定闭环候选者，接着在确定的闭环候选者之间进行基于Super G-4PCS的粗配准，以确保不会处理错误的闭环检测，最后，使用改进的ICP算法来完善转换估计，最终完成闭环检测。结论：论文的最后通过两组实验证明了这种方法的优越性。

关键词：三维激光SLAM;闭环检测;3D全局描述符;Super G-4PCS;ICP

中图分类号：TP391文献标志码：A文章编号：1008-4657（2020）03-0005-07

0 引言

使用可移动平台搭载传感器同时定位与绘制环境是一个研究了三十多年的话题，自从1988年SLAM技术被首次提出[1]之后，一直是一个高度活跃的研究领域。其中，面向3D激光SLAM的闭环检测算法作为一种能够有效降低SLAM建图、定位误差的方法也吸引了众多研究学者的目光。

不局限于三维激光雷达获取的3D点云数据，当前的基于3D数据进行闭环检测的技术大致可以分为三类：基于局部關键点检测结合词袋（BoW）的匹配方法、基于几何图元或对象整体的匹配方法以及基于全局描述符匹配方法。

第一类方法最早是由Steder B等[2]在2011年提出，此类方法通常是检测点云中的显著关键点，通过计算这些关键点位置的签名，建立BoW，最后在不同的扫描中进行匹配，对每次匹配的相似度进行评分，如果评分分数高于可接受阈值，则认为此次匹配就确定了闭环检测的候选者。很显然，这类方法的关键是对于点云数据中关键点的检测以及描述，很多文献也提出了许多关键点检测器，例如固有形状签名（ISS）[3]，Harris 3D[4]，Sift 3D[5]，NARF[6]，以及许多描述符，例如Spin Image[7]和SHOT[8]。然而，尽管有这么多的选择，但是具有高重复性的独特关键点的检测提取仍然是一个挑战。

第二类方法着重于基于完整对象或平面的位置识别。Fernandez-Moral E等[9]在2013年提出了一种基于平面地图的位置识别算法：将这些平面累积在一个graph图中，并使用树的形式来匹配子图，最后对匹配子图的平面进行几何一致性检验。这项工作由Fernández-Moral E等[10]于2016年进行了扩展，用平面参数的协方差代替平面上的点数进行匹配。但是，他们的方法仅适用于小的室内人造环境，无法扩展到室外场景。除此之外，还有Finman R等[11]在2015年提出的使用RGB-D摄像机在室内环境中进行基于对象的闭环检测方法以及Dubé R等[12]在2016年提出的基于段的方法。但是这些方法同样受到工作环境的强大限制，会由于不适应某些场景而导致最终闭环检测失败。

第三类方法即使用全局描述符，全局描述符通常以直方图的形式出现，比较典型的是Rusu R B等[13]在2009年提出的快速点特征直方图FPFH。最近研究人员还倾向于使用卷积神经网络（CNN）来学习特征描述符以及以统一的方式匹配它们，但是这种方法需要大量的训练数据，这给此类方法带来了很大的局限性。2016年，He L等[14]提出了一种新型全局3D描述符——多视图2D投影（M2DP）。这种全局描述符方法相比直方图形式和CNN学习方法，在准确性和效率方面都有了很大的提升，但是尽管如此，这种全局描述符与其局部的匹配仍然会受到描述能力或不变性不足的限制，从而会出现检测不到闭环或者检测到过多的错误闭环。基于以上分析，本文提出了一种完整的3D激光SLAM闭环检测算法Delay-CLD（Delay Closed-loop Detection）来解决这些问题。

1 Delay-CLD结构

整个闭环检测系统主要包括两个步骤：

1.1 确定强闭环候选者

首先，利用SLAM前端里程计的位姿信息来判断移动平台当前位置与初始位置之间的位置关系，从而设定一个延迟检测阈值，即Delay检测。这一步的目的是避免从SLAM运行初始就开始检测闭环候选者所带来大量的无意义计算。然后，利用全局3D描述符M2DP[14]不依赖点云中表面法线的估计，且在描述符的匹配准确性和效率方面有良好表现的特点来检测出激光雷达数据中的强闭环候选者。

1.2 闭环候选者验证

当确定了闭环候选者之后，为了确保我们不会达成错误的闭环，下一步将寻求使两片点云对齐的变换：首先采用基于Super G-4PCS[15]的全局对齐技术，以获得对闭环两端之间转换的粗略估计;粗略对齐后，再使用改进的ICP算法来完善转换估计，如果最终匹配的残差很小，那么闭环检测条件达成。

整个闭环检测系统的结构如图1所示。

2 Delay-CLD算法实现

2.1 Delay检测

Delay检测的主要部分是SLAM前端里程计，这里我们采用LOAM算法[16]的里程计算法。假设激光雷达的运动是匀速的，算法以激光雷达最后一次扫描的点云集P-k、当前扫描的增长点云集P-k+1和最后一次递归的姿态TLk+1作为输入。在开始新的扫描时，将TLk+1设置为0，然后从P-k+1中提取特征点，构造特征点集εk+1（边缘特征点集）和Hk+1（平面点集）。对于每个特征点，在P-k中找到它们的对应关系，并根据它们的对应关系为每个特征点分配双平方权重：互相之间距离较大的特征点被分配较小的权重，而距离大于阈值的特征点则被视为离群点并被分配零权重。然后将姿态变换更新一次，如果找到收敛或达到最大迭代次数，则非线性优化终止。其中优化公式如下

如果当前帧计算完成，则将当前的点云P-k+1重新投影到时间戳tk+2，否则，仅返回变换TLk+1，并进行下一轮递归。到这里，我们已经可以获得激光雷达在每一时刻的位姿，接下来我们就可以通过测试每个新计算的位置是否接近我们之前已经访问过的位置，来决定是否进入下一阶段的任务。

设定一个阈值D，该阈值等于自SLAM运行开始以来总轨迹长度的15%，只有当检测出当前的姿态距离初始姿态在阈值距离以外，则认为该位置时可以进行下一步任务的，这样就可以避免前期大量的无意义计算。

2.2 M2DP全局描述符匹配

大多数现有的3D描述符在构建的时候都利用点的法线，但是对于具有嘈杂数据的点云，通常很难获得一个点的准确法线，而且这一过程通常很耗时。除了用签名和直方图，使用诸如Spin Image[7]或ESF[17]之类的非常规方法获取的描述符也会由于缺乏空间信息从而阻碍它们捕获一个有效且准确的描述符。而由He L等[14]提出的M2DP算法所提取的全局描述符并不依赖点云中点的法线，且易于计算并能够捕捉点云的局部几何细节，这是在闭环检测中非常重要的两个属性。

在闭环检测中，描述符在3D空间中的移动和旋转不变性是至关重要的。借鉴了直方图全局描述符如VFH、CVFH中的方法，为确保平移、旋转不变，M2DP使用输入点云的质心作为描述符参考系的原點。然后，通过使用主元分析法PCA估计点云的两个主要方向来定义参考系，将两个主要方向设置为描述符参考帧的x轴和y轴。在构建3D描述符时，先将3D数据投影到穿过原点的不同的2D平面上，其中，每个平面都可以使用方位角θ和仰角φ表示，而每对参数θ，φ就决定了一个唯一平面X，该平面的法向矢量m表示为

而投影在该平面上的点ux通过ux=u-uTmm22m给出。

为了捕获投影平面上的点的结构，将2D平面划分成数个仓：以投影质心为中心，生成l个同心圆，这些同心圆的半径依次为r，22r，…，l2r，且最大半径设置为与质心和最远点之间的距离。将每个环划分为t个仓，因此定义了l × t个不同的仓。对于每个仓，计算其中的投影点数，生成一个lt × 1的描述投影在平面X上的点的特征向量vx

其中，n是平面上投影的总点数。

通过使用p个不同的方位角θs和q个不同的仰角φs生成多个2D平面，其中方位角上的步幅为πp，仰角上的步幅为π2q。因此，共存在pq个不同的平面，对于每个2D平面，我们生成上一部分中描述的2D签名，它是lt × 1的矢量，这样就构成了一个pq × lt的签名矩阵A，矩阵的每一行对应一个签名矢量vx。最后，在矩阵A上进行SVD分解，并且将第一个左右奇异矢量连接起来以形成最终描述符。

2.3 Super G-4PCS全局粗配准

前面描述的算法提供了闭环候选者，但是，正如前文所说，仅仅基于3D全局描述符的闭环检测方法会受到描述符描述能力不足从而可能导致的闭环检测结果呈假阳性的结果，即当前检测出的闭环候选者并不足以达成闭环检测。如果想进一步的提高闭环检测的准确性，我们需要对检测出来的闭环候选者与起点处的点云进行配准，如果配准的结果显示误差较大，那么将当前的闭环候选者舍弃，继续下一轮的闭环候选者检测。

由于检测出的闭环候选者处的点云与起点处的点云发生了很大的方向变化，为此，我们需要一种能够处理方向变化较大的配准算法，即点云的粗配准方法。面向点云的粗配准方法有很多，大体可分为两类：一类是基于穷举搜索的配准方法，通过遍历整个变换空间以选取使误差函数最小化的变换关系或者列举出使最多点对满足的变换关系，如RANSAC算法[18]、四点一致集配准算法（4-PCS）[19]等;另一类是基于特征匹配的配准算法，通过被测物体本身所具备的形态特性构建点云间的匹配对应，然后采用相关算法对变换关系进行估计，如基于直方图的FPFH点特征的SAC-IA、FGR等算法以及基于点SHOT特征的AO算法等。

但是在SLAM的闭环检测过程中，激光雷达所经过的路径往往都是动态的，特别是面向室外的SLAM系统，室外场景中的行人、车辆等物体的出现与消失很容易会导致基于局部特征的配准失败。2008年由Aiger等[19]提出的四点一致集配准算法4-PCS是一种不依赖特征提取的全局点云配准技术。4-PCS算法的基本思想是基于RANSAC算法，区别在于将RANSAC原本的随机选择三个不同的点修改为以源点云中共面的四点为基，在目标点云中寻找近似仿射相等的共面四点对，再确认这些共面四点对与选定的共面四点标准对在一定的距离约束下是否相等。4-PCS算法的运行时间复杂度为On2+k，其中n是源点集Q中的点数，k是要报告的全等基数。

4-PCS算法有两个主要的瓶颈：第一个是从源点集Q中提取四点对，这是在On2的时间复杂度内穷举执行的;第二个就是大量报告的一致集，这导致4-PCS算法的验证步骤代价高昂。针对这一点，Mohamad M等[15]在Super-4PCS中通过解决这两个瓶颈，将算法的总运行时间复杂度降低到On+k1+k2。但是这两种配准方法都强制选取共面的4点，这在处理3D数据时常常会由于点云的对称导致配准出错。2015年Mellado N等[20]在4-PCS和Super-4PCS的基础上提出了Super G-4PCS，该算法不限制4点必须在一个平面内，如图2所示。

在Super G-4PCS中，非共面4点对的仿射不变性将由重新定义的两个不变比来描述：

此外，定义了ab与cd之间的夹角为α，x、y之间的距离为3D交点距离d3：

Super G-4PCS算法主要分为以下几个步骤：

（1）给定固定不变的d1、d2（分别是ab和cd的距离），用Super-4PCS高效地提取出源点集Q和目标点集P中所有满足不变性的线段。穷举计算源点集Q中提取的每个d1点对和所有d2点对构成的直线之间的潜在3D交点，如果计算出这两个点对不平行，则获得一组r1、r2、d3、α的值。为了确定这对3D交叉点对是有效的，设定一个参数σ，若此处的d3与源点集Q的直径的比值小于σ，则认为这对3D交叉点对有效。

（2）初始化一个正四维哈希表H，将所有有效3D交叉的线段索引存储在哈希表中，该4维哈希表的维数对应于不变量r1、r2、d3、α。这样做的好处是只需要构建一次四维哈希表，而且对于之后的每个RANSAC迭代，仅需查询一次与源点集Q中的4点基一致的目标点集P中的四点基。

（3）Super G-4PCS的最后一步是使用最大公共點集（LCP）标准检验利用四点基及其一致集计算的刚性变换。LCP标准规定应计算变换后源点集中与目标点云集中的点的距离在参数δ以内的点数，产生最大LCP的转换被认为是真正的转换。

2.4 ICP精匹配

Super G-4PCS方法产生了从源到目标点云的粗略转换，但不能完美的对齐它们。因此，还需要对经过Super G-4PCS配准的闭环候选者进行进一步的精配准。一般地，在对三维激光雷达连续扫描的点云进行精配准时，大多数文献提出的方法都是基于迭代最近点（ICP）算法。顾名思义，它是一种迭代算法，每次迭代主要包含四个主要步骤：（1）点选择（Point Selection）;（2）对应估计（Correspondence Estimation）;（3）加权（Weighting）;（4）变换估计（Transfor-mation Estimation）。标准ICP算法如图3所示。

但是标准的ICP算法在处理三维激光雷达扫描生成的稀疏且密度不均匀的点云时有一些严重的局限性，很难在两个连续的点云（源点云和目标点云）之间找到良好的对应关系，因为在大多数情况下，确定的对应点对并不代表空间中的同一物理点。这导致即使经过大量迭代后点云也无法准确对齐，因此会错误估计激光雷达的姿态，且收敛过程将非常缓慢。因此，我们通过使用一种基于ICP的改进算法[21]来完善转换。该ICP算法提供了额外的验证，可以得出两点云产生真实的闭环的结论。如果ICP的残差（定义为所有对应点之间的平均距离）太大，仍将丢弃闭环候选。

3 实验及结果分析

3.1 实验环境

操作系统：Ubuntu14.04

硬件开发环境：Intel Core i5-7300HQ CPU 2.50 GHz，8G RAM，64位操作系统

算法开发环境：ROS Indigo

3D SLAM算法框架：LOAM

3.2 实验数据集采集

实验平台搭载“北科天绘”公司生产的16线三维激光雷达“R-Fans-16”，实验在“北科天绘”公司4楼办公室内进行，数据采集场景如图4所示。数据采集过程中小车匀速行驶，在ROS环境下通过rosbag命令录制出bag格式的“R-Fans“数据集。该数据集包含桌椅，窗户，墙壁，廊道等典型室内场景，可以对本文提出的Delay-CLD闭环检测算法进行比较全面的精度验证。

另外，为了更好的测试出Delay-CLD的准确性，我们还对比了4种使用不同描述符的闭环检测算法，依次为：基于M2DP的原始方法、基于ESF的方法、基于Spin Image的方法和基于SHOT的方法。对比实验使用了KITTI基准的“00”和“05”两个序列数据集。

3.3 实验过程及结果分析

为了更直观的显示出本文提出的Delay-CLD闭环检测算法在实际应用中的效果，将Delay-CLD应用到具有代表性的3D激光SLAM算法LOAM算法中，并将原始LOAM算法和加入了Delay-CLD闭环检测算法的LOAM算法在数据集“R-Fans”上的运行效果进行对比。结果如图5所示。

从运行的结果来看，原始LOAM算法在建图和定位过程中发生了明显的漂移，而且随着时间的推移，漂移越来越大，到最后小车完成实际的闭环路径之后，起点处和终点处的点云之间产生了约45o的偏差。而通过Delay-CLD对L-OAM进行改进之后，在建图和定位时无明显漂移，起点和终点处的点云重合，里程计轨迹闭合，完成了对室内环境的准确建图与定位。提取出两次运行的里程计信息，进行误差分析，结果如表1所示。

分析表1中数据，可以看出在平均误差上，改进后要比改进前提高了4.57倍的精度;在中间误差上改进后比改进前提高了5.08倍的精度。同时可以看出原始LOAM的平均误差与中间误差存在明显变化，这符合图5中原始LOAM在中后程发生明显漂移的实验结果。

按照准确率的计算公式

计算在KITTI基准中的“00”和“05”号序列的数据集上对不同回路检测的准确率实验结果，如表2所示。式中，TP表示真阳性（True Positive），FP表示假阳性（False Positive）。

从实验结果来看，在KITTI “00”和“05”序列的数据集上，基于Delay-CLD的方法的闭环检测准确率均要优于其他4种闭环检测方法，特别时在“05”序列数据集上，Delay-CLD方法的准确率更是达到了94.2%，相较于其他四种方法中表现最好的原始M2DP方法在“05”序列数据集上的闭环检测准确率更是高出10.9%。

4 结论

对于现有的各种激光SLAM闭环检测算法，大都存在闭环检测准确率偏低的困境，究其原因是因为现有的激光雷达扫描生成的点云数据存在稀疏、密度不均匀以及信息丰富度（如纹理信息）不足等。本文在全局3D描述符匹配闭环检测思路的基础上，通过两次配准将前期工作检测出的闭环候选者加以筛选，最终形成了一个新的闭环检测方法Delay-CLD。通过在KITTI数据集上的实验证明了Delay-CLD算法相对于其他闭环检测算法的优越性，并在具有代表性的3D激光SLAM算法LOAM上用Delay-CLD对其进行了改造实验，实验结果证明了Delay-CLD在实际应用中的效果是优秀的。

參考文献：

[1] Smith R，Self M，Cheeseman P.Estimating Uncertain Spatial Relationshipsin Robotics[C]//Autonomous Robot Vehicles.New York：Springer，1990：167-193.

[2]Steder B，Ruhnke M，Grzonka S，et al.Place Recognition in 3D Scans Using a Combination of Bag of Words and Point Feature Based Relative Pose Estimation[C]//International Conference on Intelligent Robots and Systems.San Francisco：IEEE，2011：1 249-1 255.

[3]Zhong Y.Intrinsic Shape Signatures：A Shape Descriptorfor 3D Object Recognition[C]//International Conference on Computer Vision Workshops.Kyoto：IEEE，2009：689-696.

[4]Sipiran I，Bustos B.A Robust 3D Interest Points Detector Based on Harris Operator[C]//Eurographics Workshop on 3D Object Retrieval.Norrkoping，Swenden：ACM，2010：7-14.

[5]Scovanner P，Ali S，Shah M.A 3-Dimensional Sift Descriptor and its Application to Action Recognition[C]//International Conference on Multimedia.New York：ACM，2007：357-360.

[6]Steder B，Rusu R B，Konolige K，et al.NARF：3D Range Image Featuresfor Object Recognition[C]//Intelligent Robots and Systems （IROS）.Taipei：IEEE/RSJ，2010：44.

[7]Johnson A.Spin-Images：A Representation for 3-D Surface Matching[D].Pittsburgh：Carnegie Mellon University，1997.

[8]Salti S，Tombari F，Di Stefano L.Shot：Unique Signatures of Histograms for Surface and Texture Description[J].Computer Vision and Image Understanding，2014，125：251-264.

[9]Fernandez-Moral E，Mayol-Cuevas W，Arevalo V，et al.Fast Place Recognitionwith Plane-based Maps[C]//International Conference on Robotics and Automation.Karlsruhe，Germany：IEEE，2013：2 719-2 724.

[10]Fernández-Moral E，Rives P，Arévalo V，et al.Scene Structure Registration for Localization and Mapping[J].Robotics and Autonomous Systems，2016，75：649-660.

[11]Finman R，Paull L，Leonard J J.Toward Object-based Place Recognitionin Dense Rgb-D Maps[C]//ICRA Workshop Visual Place Recognition in Changing Environments.Seattle：ICRA，2015：76.

[12]Dubé R，Dugas D，Stumm E，et al.Segmatch：Segment Based Loop-closure for 3D Point Clouds[C]//International Conference on Robotics and Atuomation.Marina Bay，Singapore：IEEE，2017：5 266-5 272.

[13]Rusu R B，Blodow N，Beetz M.Fast Point Feature Histograms （FPFH） for 3D Registration[C]//International Conference on Roboticsand Automation.Kobe，Japan：IEEE，2009：3 212-3 217.

[14]He L，Wang X，Zhang H.M2DP：A Novel 3D Point Cloud Descriptorand its Application in Loop Closure Detection[C]//Intelligent Robots and Systems （IROS）.Daejeon，South Korea：IEEE，2016：231-237.