基于FastDTW地磁匹配的快速闭环检测方法研究*

罗恒杰， 鲍 泓， 徐 成

(北京联合大学 北京市信息服务工程重点实验室,北京 100101)

0 引 言

即时定位与建图(simultaneous localization and mapping，SLAM)是获取环境地图和定位信息的关键技术。SLAM技术考虑到了环境的真实拓扑结构，可以直接有效地获取环境地图，并且可以随着环境的改变更新地图。基于激光雷达(LiDAR)的SLAM可以精确地测量障碍物点的角度和距离，工作时不受光照影响，被广泛应用于无人车、室内外机器人导航和三维重建等领域。但当建图场景的结构具有一定的相似性时，激光雷达点云将不足以清楚地描述真实环境，并且在SLAM闭环检测时容易产生误报，从而降低SLAM的性能，导致地图与真实环境不一致。融合辅助信息可以有效地减少SLAM中由于高局部相似性引起的误检，提高闭环检测的效率，降低累积误差[1]。使用超宽带(ultra wide band，UWB)和2D激光雷达进行融合，利用UWB精确定位的特性，可以很好地降低累积误差，但由于UWB信号的遮挡和衰减问题，这种方法不适用于有遮挡的大场景[2]。结合视觉传感器和激光雷达传感器提出了同时考虑扫描和图像数据的成本函数，将具有视觉特征的词袋模型应用到闭环检测中，加快了闭环检测的速度，但在光照条件不好的情况下这种方法的效果将会变的很差。

随着对地磁的深入研究[3]，文献[4]首次利用地磁来解决SLAM问题，并证明了实验的可行性。文献[5]证明了建筑物内部磁场的异常几乎是静态的,并且它们具有足够的局部可变性，可以提供用于定位的唯一磁性指纹。由于地磁定位不需要基础设施，越来越多的研究人员开始了对地磁定位的研究[5]。文献[6]提出了一种使用智能手机室内定位系统来定位室内目标行人的位置，文献[7]认为手机中的低成本磁力计无法满足定位精度需求，提出将地磁定位与其他定位方法结合使用。文献[8]将地磁与运动模式关联，提出了一种基于关键帧的姿势图SLAM。文献[9]提出了一种WiFi和磁场融合的多点定位的室内定位方法。使用中值滤波算法处理原始磁场数据，并使用协方差插值算法生成磁场图，有效地减少了地磁波动引起的干扰，从而提高定位精度。为了增强地磁定位的适应性和准确性,文献[9～12]将动态时间规整(dynamic time warping,DTW)应用到了地磁定位中以提高地磁匹配速度和定位精度。文献[13]将蚁群算法与DTW算法融合，虽然获得了较高的地磁定位精度，但该方法复杂度高，实时性较差。文献[14]针对地磁序列匹配定位中DWT算法实时性差的问题提出一种基于快速动态时间规整(fast dynamic time warping,FastDTW)的地磁定位算法，在保持了较高定位精度的情况下，采用减小搜索的策略提高了定位的实时性。

针对激光雷达SLAM大规模建图时闭环检测匹配速度慢和闭环误检测的问题，本文提出了一种基于FastDTW地磁匹配和激光雷达点云匹配相结合的快速闭环检测方法。实验结果表明，通过FastDTW地磁序列搜索减小闭环检测时激光雷达点云搜索匹配的范围，可以有效地提高闭环检测的实时性，同时因为搜索范围的控制，误检的问题也得到了很好解决。

1 点云匹配策略

(1)

(2)

所以有

(3)

(4)

(5)

(6)

因此,每次匹配都会有误差增量

(7)

当激光雷达沿直线运动时，角度变化不大，可认为直线运动时的角度变化视为i-1=0，因此，此时的累积误差为

(8)

因此,不管是否有角度变化，随着匹配帧数的增加，累积误差会逐渐增加。

闭环检测指的是无人车到达已经建好地图的区域时执行的一个扫描—搜索—匹配的过程。对已经建好的地图进行第二次扫描观测，当获得新的扫描帧时，在附近一定范围内搜索最优匹配帧。如果最优匹配帧满足要求，则认为它是一个闭环。并通过两次扫描观测的差异对地图进行调整，减少SLAM的累积误差，搜索匹配的问题可以描述为

(9)

式中W为搜索空间，Mnearest为与该定位点对应的网格点的M值。在W空间中寻找可靠性最高的最优解。最简单粗暴的办法是一帧一帧地进行匹配，即将当前帧与搜索空间中的每一帧执行式(9)进行非线性最小二乘计算，匹配求解的刚性变换ξ=(ξx，ξy，ξθ)，将无人车的初始位置与姿态相乘，即可得到此时的位姿(xi,yi,θi)。然后根据当前帧位姿与最优匹配帧位姿对地图进行调整。

2 地磁匹配与激光雷达SLAM融合策略

DTW可以很好地比较两个序列的相似度，其核心思想是计算出这条路径经过的所有点的坐标(i,j)对应的X和Y两个时间序列的点Xi和Yj的欧氏距离之和，距离越小，两个序列越相似。如图1根据动态规划的思想有

D(i,j)=Dist(i,j)+min[D(i-1,j),D(i,j-1)，

D(i-1,j-1)]

(10)

图1 具有最小距离扭曲路径的成本矩阵

DTW算法会不停地按照式(10)去寻找成本矩阵元素D(i,j)，每次执行都会搜索所有元素，执行效率低。当两个序列都是长序列时，算法的实时性将会很低，缩减算法的搜索空间可以有效地降低算法的复杂度，FastDTW[14]对DTW算法的改进主要有两点：

1)约束:限制在成本矩阵中评估的单元格的数量，例如Sakoe-Chuba Band约束[15]和Itakura Parallelogram约束[16],如图2所示。

图2 两种约束

2)抽象：首先将两个长时间序列组成的大矩阵进行粗粒度化，然后使用DTW算法算出最短路径，接着在控制路径的搜索范围的条件下进行细粒度化，如图3所示。

图3 FastDTW算法

使用FastDTW进行地磁匹配时，目标是找到地磁数据序列的最小相似距离。选择地磁场的模值(三轴地磁分量的平方和的算术平方根)作为地磁特征量。例如X轴方向的地磁分量为dx，Y轴方向的地磁分量为dy，Z轴方向的地磁分量为dz，则地磁的模值为

(11)

假设待匹配的地磁数据序列长度为m，DCM=(DC1,DC2,…,DCm)，地磁数据库序列长度为n(n>m),DCN=(DC1,DC2,…,DCn),在进行地磁匹配时，从DCN上截取与DCM相同长度的数据进行匹配，即DCN的第1到m个数据与DCM匹配，然后是第2到m+1个数据与DCM匹配，直到DCN最后的m个数据与DCM进行匹配。根据FastDTW求取每对数据的相似距离L，所有数据匹配完之后得到一个相似距离集合L=(l1,l2,…,ls)，集合中最小值对应的数据即为匹配数据。在地磁匹配与激光雷达SLAM融合时，无人车在进行环境地图扫描的同时获取对应的三轴地磁数据，计算地磁模值并加入地磁数据库。当环境地图扫描完成时，地磁库数据DCN建立完成。在激光雷达SLAM进行闭环搜索时，采用子图—子图的匹配，当新的子图submapnew构建完成时，对应生成一个地磁数据序列DCnew，使用FastDTW算法在DCN中搜索与DCnew存在最小相似距离的地磁序列DCmatch，然后将与DCmatch对应的子图submapmatch中的位姿节点posematch加入到闭环检测候选位姿集合p中,然后使用式(9)计算出最佳闭环位姿ξ。地磁匹配与激光雷达SLAM融合算法的伪代码如下：

算法1 地磁匹配与激光雷达SLAM融合算法

输入：新生成的地磁序列DCnew，与DCnew对应的位姿posenew，地磁数据库序列DCN

输出：最佳闭环位姿ξ

1：定义相似距离集合L，相似距离l，空值序列DC′，实数index，候选位姿集合p

2:fori=1 tolength(DCN)do

3: if(length(DCnew)+i)>(length(DCN))

4: end if

5: else

6:DC′=(DCi,DCi+1,…,DCi+m-1)

7:l=FastDTW(DC′,DCnew)

8:Li=l

9: end if

10:end for

11:Lindex=min(L1,L2,L3,…)

12:DCmatch=DCindex

13:posematch=poseindex

14:posematchadd top

15:W=p.num

16:fori=1 toWdo

18:end for

19:returnξ

3 实验与结果分析

本文所使用的实验平台为北京联合大学联合彩虹系列无人车，所使用到的传感器有镭神智能16线激光雷达和Sparkfun 9轴IMU，地磁匹配与激光雷达SLAM实验平台如图4所示，本次实验所选的实验场景为北京联合大学实验楼五楼，平面图如图5所示，实验所使用的数据均为实验场景实地采集，图6为实验场景的地磁强度采样图。

图4 地磁匹配与激光雷达SLAM实验平台

图5 实验场景平面图

图6 实验场景的地磁强度采样图

闭环检测可以根据达到闭环时的位姿差异减少累积误差，但是在发生闭环误检测的时候，地图会发生漂移，导致地图不能使用，如图7所示。

图7 正确闭环与闭环误检测建图结果对比

使用辅助搜索匹配算法可以提升闭环检测的效率，对逐帧法、分支定界法、DTW融合法和FastDTW融合法做了对比实验，在其他条件不变的基础上，每个算法运行了100次，记录结果，取其均值进行闭环检测和误差分析。在进行闭环检测耗时对比时，为了保证实验的公平，让小车行驶到地图上的随机位置，选用前100帧扫描帧进行搜索匹配，对比结果如表1。

表1 搜索匹配算法效率对比

逐帧法的搜索匹配效率最低，满足闭环检测得分阈值的候选集一共157个，单次搜索匹配耗时为12 968 ms。分支定界法在搜索步长上进行了优化，候选集减少为58个，单次搜索匹配耗时为6 638 ms。DTW融合法根据地磁匹配的结果极大地缩小了搜索范围，候选集数量为23个，单次搜索匹配耗时为943 ms，本文使用的FastDTW融合法，不但缩小了搜索范围，而且加快了地磁匹配的速度，候选集数量减少至14个，单次搜索匹配耗时仅为806 ms。从实验结果可知，FastDTW融合法与逐帧法和分支定界法相比，大大缩短了闭环检测的耗时，与表现较优的DTW融合法相比，闭环检测速度提升了17 %，可以有效地提升闭环检测的实时性。

在进行闭环检测时，正确闭环位姿仅有一个，而候选集的位姿却有多个，所以，候选集的个数与误检发生的概率是成反比的，候选集越多，发生误检的可能性越大，每种算法100次运行的结果证明此观点是正确的，如表2所示。因此本文算法与其他三种算法相比有着较低的误检概率。

表2 100次实验中误检次数对比

在进行误差对比实验时，均取100次实验结果的平均，表3为四种算法的实验误差对比。

表3 四种算法的实验误差对比

从表3可知,本文算法与分支定界算法相比，平移误差和旋转误差均降低了30 %，与DTW融合算法相比，平移误差降低了16 %，旋转误差降低了10 %。由此可知，本文算法可以有效地提高建图的精确度。

4 结束语

本文针对激光雷达SLAM的闭环检测环节进行改进，提出了一种FastDTW地磁匹配与激光SLAM融合的快速闭环算法。实验结果表明该方法可以提高闭环检测的速度，保持较低的误检水平，还能有效地降低激光雷达SLAM的平均绝对误差。