一种基于地磁信号的激光同时定位与建图的闭环检测方法

陈贝章 李慧云

1(中国科学院深圳先进技术研究院 深圳 518055)

2(粤港澳人机智能协同系统联合实验室 深圳 518055)

1 引 言

同时定位与建图(Simultaneous Localization and Mapping，SLAM)技术自 1988 年被提出以来，主要用于研究移动机器人的智能化。目前配备了激光雷达、摄像头、惯性导航模块(Inertial Measurement Unit，IMU)等关键传感设备的智能载体借助 SLAM 技术已经能满足室内场景的自主导航任务[1]。SLAM 技术是在未知环境中实现自主定位的关键技术，而基于激光雷达同时定位与地图构建(激光 SLAM)的技术广泛应用在无人驾驶、室内外机器人导航及三维重建等领域[2]。激光 SLAM 之所以能作为无人驾驶定位方案中不可或缺的技术，其主要原因在于：(1)激光 SLAM 无需对定位场景进行预先布置，即能在未知的环境中进行定位服务；(2)激光能准确测量障碍点的角度与距离；(3)能在光线较差、光照变化较大的环境下工作；(4)能够生成便于导航的栅格地图与直观的点云地图。

前谷歌无人车领导者 Sebastian Thrun 在其 2005 年出版的经典著作《概率机器人》[3]一书中介绍了如何基于概率滤波的方法使用二维激光雷达进行地图构建和定位。该书详细阐述了从基于 RB 粒子滤波(Rao-Blackwellised Particle Filter，RBPF)的经典 FastSLAM 算法，逐步发展成为基于激光雷达建图标准 GMapping 算法的过程[4]。2016 年，Google 开源其优化的激光 SLAM 算法 Cartographer[5]，该算法改进了 GMapping 计算耗时且无法有效处理闭环的缺点，利用帧间点云地图匹配与子地图构建思想有效地弥补了 GMapping 的缺陷。

尽管激光 SLAM 技术可以在一定场景下满足定位精度的要求，但是难以在全工况的自动驾驶场景中提供持续、稳定的高精度定位输出。另外，由于定位场景的结构相似性且激光点云对环境的描述能力不足，容易发生 SLAM 的闭环检测误报(False Positives)现象，从而降低系统定位与建图性能[6]。本研究旨在通过将地磁信号与激光 SLAM 的闭环检测环节融合，提出激光 SLAM 在应对结构相似性及激光强干扰场景下的定位与建图任务时的创新解决方法。同时，本研究将此技术应用到无人驾驶的定位与建图领域，实现了无人驾驶汽车全天候、全地域的精准定位，提升了无人驾驶汽车定位算法的精准性与稳定性。

2 相关研究进展

2.1 激光 SLAM 闭环检测

近年来一些国外学者为解决闭环检测误报现象对 SLAM 系统的影响，提出了闭环验证算法。Bosse 与 Roberts[7]的工作提出，利用熵序列之间的最大相关性之和与投影直方图的验证度量来估计闭环检测的正确性，并在此基础上提升了闭环检测的正确率[8]。Granstrom 等[9]提出了基于变换后扫描并在最后配准的闭环检测验证标准。Corso[10]提出在室内环境中严格处理闭环验证时，由于闭环的现象可以从多种传感器的数据源中被检测到，因此应将闭环验证算法与闭环检测算法分成两个方向进行研究。此外，Gao 与 Harle[11]在已有报道的基础上，提出将地磁序列信号的匹配定位方法用于矫正基于 PDR(Pedestrian Dead Reckoning)的 SLAM 闭环检测环节，以此来提高行人步态检测与定位的精度。

2.2 闭环检测模型

闭环检测是指无人车经过一段较长行程并再次回到曾经历过的位置时，对地图中曾观测到的特征进行二次观测，同时通过两次观测的差异对全局地图进行调整，以减少定位与建图过程中的误差累计[12]。目前主要的闭环检测方法包括：(1)帧与帧闭环检测；(2)帧与子图闭环检测；(3)子图与子图闭环检测[13]。以上闭环检测算法都是采用相关性扫描匹配，通过旋转矩阵R与平移向量T判断两帧激光点云的相似性。当激光雷达获得新的扫描帧时，在其附近一定范围搜索最优匹配帧，若该最优匹配帧符合要求，则认为是一个回环。该匹配问题可以描述为如下公式：

其中，ξ为匹配解算的刚性变换，ω为搜索空间，k为搜索空间的位姿数量，Mnearest表示观测激光点云hk和位置变换矩阵Tξ的乘积与地图激光栅格点云最接近重合的置信度。对于每一帧激光点云插入子地图上时的信度和来说，信度越高则认为越相似。在ω空间中寻找出该信度和最大的匹配帧，需要在ω空间中寻找出匹配结果的最优解。一种常见的方法是暴力匹配法，即在搜索空间范围内，对每一帧与当前帧进行上式非线性最小二乘计算。匹配算法解算的刚性变换ξ与智能车初始的位姿相乘后便得出此时的位姿(xi,yi,θi)。当匹配的两帧之间存在对激光光束产生干扰的材质(反射率过低的物体或对光束有透射作用的玻璃等)时，场景中的这些干扰因素是激光点云匹配阶段误差累积的主要成分。其中，该累积误差可以通过闭环检测的方法进行消除。以下场景容易引起激光 SLAM 的闭环检测误检现象：(1)室内的长廊、空旷的室内大堂、结构布局相似的房间，如图 1(a)所示；(2)场景中反射率过低的物体，如黑色吸光墙面、家具等；(3)场景中存在玻璃或对激光光束产生透射现象的物体，如图 1(b)所示；(4)场景中存在镜子、不锈钢等对激光光束产生镜面反射的物体。

随着地图扩大，匹配搜索效率决定了闭环检测的实时性。当 SLAM 在应对较大场景和复杂干扰的环境时，实时定位与建图对闭环检测算法的效率提出了更高要求。此外，对于空间相似性较高的环境，由于低线束激光雷达对环境特征的表达能力不足，导致闭环检测误报现象十分常见。对激光光束存在干扰的场景(玻璃、镜子、不锈钢等)，无人车运行定位与建图算法时，会出现一些未完全探明的区域，此处区域显示为灰色，如图 1(d)红圈处所示。对于这些干扰，传统激光雷达定位主要是在干扰激光扫描平面处贴上反射片，或是通过人工校验的方法进行解决。而传统的人工后处理方法是通过使用机器人仿真软件 RoboStudio 中的画图工具将建图结果中的误判区域进行擦除，使其变为已探明区域[14]，因此该方法既不智能也费时耗力。而本文通过对地磁序列信号的研究，提出了地磁序列信号与激光雷达融合的闭环检测方法，“一站式”地解决了上述干扰对整个定位与建图系统的影响。

图1 常见闭环误检干扰及错误定位与建图Fig. 1 Common closed-loop false detection interference and incorrect positioning and mapping

3 地磁信号与激光 SLAM 融合算法设计

本文基于地磁导航技术与激光 SLAM 算法，针对无人驾驶载体定位与建图的闭环检测环节，设计了地磁序列搜索与激光点云匹配融合的闭环检测算法。算法框架如图 2 所示，其中红色虚线框中是本文提出的闭环粗匹配与地磁特征筛选闭环检测算法。

3.1 基于地磁信号与激光雷达融合定位的算法设计

地磁信号与激光雷达融合定位建图算法是在激光 SLAM 算法框架基础上设计的，因此和激光 SLAM 框架一样拥有前端扫描匹配、后端优化和闭环检测等环节。本文提出的闭环粗匹配与地磁特征筛选闭环检测算法是将地磁序列匹配技术引入激光 SLAM 闭环检测环节。整个地磁信号与激光雷达融合定位建图的算法流程如图 3 所示。闭环粗匹配与地磁特征筛选闭环检测算法位于整个系统框右侧红色虚线框中。算法总体框架参数设置分为传感器参数设置与位姿地图参数设置。其中传感器参数设置分为来自激光雷达的输入与来自 IMU 的输入。激光雷达的参数设置包括：扫描角度的范围、扫描角度间隔、扫描点数、扫描时间、一次扫描的线束数等。

本文算法首先初始化上述参数，以运动载体的初始位置为原点，将 IMU 估计的载体坐标附近划定为激光点云匹配的搜素区域。在该区域半径内调用激光 SLAM 前端点云匹配算法估计载体更精细的坐标，载体的姿态由当前时刻的 IMU 提供，此时已完成前端位姿的估计。但是在激光点云的匹配过程中，由于受到环境的干扰，不可避免地出现累计误差，这些误差中一些由后端的优化环节进行消除，另一些则由闭环检测环节进行消除。本文提出的闭环粗匹配与地磁特征筛选闭环检测算法利用地磁序列快速搜索与匹配的特点，能为激光 SLAM 提供稳定精准的闭环候选集合。

图2 地磁信号与激光雷达融合定位建图算法框架Fig. 2 Algorithm framework for geomagnetic signal and lidar fusion positioning mapping

图3 基于地磁信号与激光雷达融合定位与建图算法流程Fig. 3 Algorithm flow of fusion positioning and mapping based on geomagnetic signal and lidar

3.2 闭环粗匹配与地磁特征筛选闭环检测算法

接着按照激光点云的匹配算法，匹配新构建的点云子地图与历史子地图。本文将满足子地图匹配得分阈值的点云子地图加入候选子图集，对应的地磁序列信号加入候选子序列集合。若不存在满足条件的点云子地图，则当前位姿不存在闭环，并跳出闭环检测环节。然后，通过使用动态时间规划(Dynamic Time Warping，DTW)算法计算候选地磁子序列与新加入的地磁子序列的距离。由于考虑到即使在闭环检测的地方，运动轨迹也不可能完全重合，因此闭环检测附近区域的运动必定是时而靠近上一次运动的轨迹，时而远离上一次运动的轨迹。针对这种无约束的运动(无固定运动轨迹的运动)，通过设置固定 DTW 距离来约束阈值是不适用的。故使用迭代最近邻(Iterative Closest Point，ICP)算法计算候选集中的运动轨迹，并与当前轨迹进行匹配，记录算法迭代至收敛的步数。最后，对于满足迭代步数约束，即运动轨迹相似的候选集地磁子序列，放宽 DTW 阈值的约束。从而使即使偏离原来轨迹的运动在闭环检测的地方也能满足 DTW 地磁匹配的阈值筛选条件。

图 4 为闭环粗匹配与地磁特征筛选闭环检测算法流程图。本文算法可在激光闭环检测到的候选子图中，用地磁匹配的 DTW 和轨迹匹配的 ICP 算法再次筛选出准确的闭环检测点，从而解决了闭环检测误检问题(室内建筑结构相似性引起)和定位建图扰动问题(室内玻璃、光滑镜面材质对激光光束透射、反射现象引起)。其中，ICP 迭代至收敛步数作为轨迹相似评价，并自适应调节闭环检测点处匹配的地磁序列阈值，从而保证闭环检测的准确性与稳定性。

图4 闭环粗匹配与地磁特征筛选闭环检测算法Fig. 4 Closed-loop coarse matching and geomagnetic feature screening closed-loop detection algorithm

当新的地磁序列Snew生成时，用 DTW 算法计算已生成的地磁序列Si与Snew的欧式距离。当 DTW(Snew,Si) 的距离小于闭环检测的阈值DLC时，将此闭环检测的可能位姿节点加入闭环检测候选集。表 1 给出了地磁信号与激光雷达融合闭环检测算法的伪代码。图 5 所示为地磁序列与激光 SLAM 融合原理图。三轴地磁信号通过求平方和的形式形成序列Si，与相同时戳位姿对应的示意图形成新位姿节点Xi，体现地磁序列信号与激光解算位姿的融合原理。

表1 地磁信号与激光雷达融合的闭环检测的算法伪代码Table 1 Pseudocode for closed-loop detection algorithm of geomagnetic signal and lidar fusion

图5 地磁序列与激光 SLAM 融合原理图Fig. 5 Schematic diagram of fusion of geomagnetic sequence and lidar SLAM

4 实 验

4.1 无人驾驶硬件测试平台设计

本研究主要采用为码头仓库重载自动导引运输车(Automated Guided Vehicle，AGV)提供高精度定位与建图作为研究背景，图 6 所示为地磁与激光融合 SLAM 实验平台总框图。该实验平台的定位与建图主要使用的传感设备包括，Hokuyo 单线激光雷达、禾赛 40 线激光雷达和 Xsens IMU。其中 Xsens IMU 作为地磁序列信号的感知设备，通过安装了 ROS(Robot Operating System)系统的笔记本电脑采集空间三轴地磁信号。实验平台的工控机采用英伟达的 Jetson TX2，笔记本只作为与用户交互的终端，通过 WIFI 向工控机收发指令。激光雷达与采集地磁信号的 IMU 一同接在 Jetson TX2 上，用以建图与定位。本实验平台不仅能满足空间地磁序列作为定位匹配信号的验证性实验要求，还是基于地磁信号的激光 SLAM 闭环检测方法的主要实验平台。

图6 地磁与激光融合 SLAM 实验平台总框图Fig. 6 General block diagram of geomagnetic and laser fusion SLAM experiment platform

4.2 定位与建图效果对照实验

为对比说明 Google 的 Cartographer 激光 SLAM 算法[5]与本文的地磁与激光融合 SLAM 算法在建图与定位效果上的优劣，选取对激光雷达产生大量干扰的场景，如图 1(b)(中国科学院深圳先进技术研究院 D 栋 2 楼)实验场景中存在大片的玻璃落地窗，进行对比实验。图 7 为实验场景平面图。

图7 实验场景平面图Fig. 7 Experimental scene plan

定位与建图算法对比实验步骤：(1)对照实验采用 AGV 实验平台，控制实验无关变量进行实验(在相同场景、时间段、控制实验车运行大致相同的轨迹)；(2)启动实验小车同时运行 Cartographer SLAM 算法，实时观察 SLAM 算法输出的定位轨迹与建立的栅格地图；(3)让实验小车完整地运行一周，运行结束后保存运行轨迹与建立的栅格地图；(4)运行本文提出的地磁与激光融合 SLAM 算法，重复(2)、(3)步骤。

4.3 实验结果与分析

4.3.1 定位与建图效果对比实验

搭载了激光雷达的 AGV 在室内场景运行一周后构建的栅格地图与推算的运动轨迹如图 8(a)所示。结果显示，与小车实际行驶轨迹的最大出入是刚开始的一段错误定位轨迹，这是由于场景中结构相似性导致的。因为无论是从左到右还是从右到左行驶，激光雷达的观测量都存在极大相似性。在经过该结构相识性较高的环境后，Cartographer 的定位才与实际情况相符合。而本文算法在相同干扰环境中定位的轨迹如图 8(b)所示，符合无人车的实际运动。

在进行定位与建图效果对照实验中，传统 SLAM 算法在应对环境中的玻璃干扰时，常常错误建图与定位，具体如图 8(c)所示。原因是激光雷达对场景的描述能力不足，帧间匹配特征不够明显，闭环检测环节在应对上述干扰时不起作用。因为玻璃等光滑镜面材质对激光光束透射、反射，所以此算法容易构建出实际中不存在的可行使区域(被玻璃材质隔断的区域)，这对于使用该地图进行导航的任务来说将会产生碰撞后果。针对这些问题，基于地磁信号在空间分布的差异性与稳定性，本文提出的地磁与激光融合的解决方案在应对上述干扰时有更好的建图与定位效果，如图 8(c)、8(d)中的圆圈区域对应图 1(b)中的玻璃干扰区域。

图8 定位与建图对比实验Fig. 8 Comparison experiment of positioning and mapping

4.3.2 算法运行效率实验与结果分析

为体现地磁信号与激光雷达融合的闭环检测算法对激光点云匹配速度的提升性，实验选用 100 帧激光数据进行匹配比较。同时将本文算法、暴力匹配的搜索算法[15]与 Cartographer 的分支定界加速闭环检测算法[5]进行比较。为确保实验的公平性，在 AGV 运行一周时采集的激光点云与地磁数据中，随机选取 100 帧激光点云，测试 3 种闭环检测算法完成 100 次匹配后的运行时间。从图 9 可看出，在同样匹配次数下，暴力匹配算法消耗的时间大约为 13 s/次；Cartographer 算法大约为 6.7 s/次；而具有地磁匹配检测算法仅为 0.9 s/次。可见地磁匹配筛选能减少产生的候选集，在 100 帧的匹配任务中本文算法匹配速度相比于 Cartographer 的分支定界加速匹配算法提升了 10%。

图9 三种闭环检测算法耗时比较Fig. 9 Time-consuming comparison of three closed-loop detection algorithms

4.3.3 算法稳定性实验与结果分析

闭环检测误检率和召回率分布情况如图 10 所示。本文算法通过地磁匹配算法筛选出候选闭环检测位姿节点方法后，由于地磁匹配算法能快速地过滤掉很多容易造成误检的结果，因此本文算法的误检率在 3 种算法中一直维持在较低水平。从图 10 可看出，不论激光点云帧与子地图匹配的得分阈值如何设置，在相同召回率的情况下，本文算法的闭环检测误检率都低于分支定界加速匹配算法与暴力匹配算法；在 0.8 召回率的情况下闭环检测的误检率降低了 23%，明显提升了闭环检测算法的稳定性。

图10 三种闭环检测算法误检率与召回率对比Fig. 10 Comparison of the false rate and recall rate of three algorithms for closed-loop detection calculation

5 总结与展望

本文以激光 SLAM 闭环检测环节中的误检现象，及在定位与建图环境中由干扰引起的闭环误检测与建图失真现象为研究课题。首先，在对激光 SLAM 闭环检测的相关研究进行调研后，将地磁导航技术中的地磁匹配与激光 SLAM 中的闭环检测环节相结合。通过从激光 SLAM 算法中必要的 IMU 传感设备里提取地磁信号作为闭环的判断依据，有效地解决了建图与定位技术中闭环检测匹配速度缓慢及闭环误检的问题。然后基于本文提出的地磁序列搜索与激光点云匹配融合的闭环检测算法以及地磁与激光融合 SLAM 实验平台设计，将传统 SLAM 算法与本文算法进行对比。最后对传统 SLAM 算法定位与建图失效的原因进行详细分析，并提出了在上述 SLAM 失效场景中的解决方案。

实验结果表明，与激光 SLAM 技术的行业标杆 Cartographer 算法相比，本文算法降低了局部相似度较高引起的误检情况；同时在定位与建图环境中，改善了激光光束反射与透射干扰引起的闭环误检与建图失真现象。在稳定性方面，本文算法与 Cartographer 算法相比，激光 SLAM 在匹配速度和闭环检测稳定性上都有明显提升。

传统的基于滤波的激光 S L A M，如C o r e S L A M、G M a p p i n g 与基于优化的HectorSLAM[16]、KartoSLAM、LagoSLAM[17]和 Cartographer，无法在激光干扰的场景中确保定位与建图的准确性。而本文算法由于使用了地磁匹配技术改进激光闭环检测环节，在应对上述干扰场景时均能稳定输出精确的定位与建图结果。经过多次实验测试，证明本文算法在闭环检测速度提升 31% 的前提下，定位精度在无激光干扰场景中仍可与 Cartographer 算法持平，从而提高了定位与建图的效率。此外，在激光干扰的场景中，本文算法的定位精度与建图效果优于上述所有激光 SLAM 算法。

室外的复杂环境为无人驾驶精准定位与建图带来更多的挑战，同时室外对激光雷达感知与 IMU 地磁信号的测量存在更多干扰。如何在保证定位精度与建图效果的前提下，将地磁与激光融合 SLAM 技术拓展到室外驾驶环境是我们未来探索的方向。