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

《一种基于地磁信号的激光同时定位与建图的闭环检测方法.docx》由会员分享，可在线阅读，更多相关《一种基于地磁信号的激光同时定位与建图的闭环检测方法.docx（19页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、一种基于地磁信号的激光同时定位与建图的闭环检测方法1引言同时定位与建图(SimultaneousLocalizationandMapping，SLAM)技术自1988年度被提出以来，主要用于研究挪动机器人的智能化。目前装备了激光雷达、摄像头、惯性导航模块(InertialMeasurementUnit，IMU)等关键传感设备的智能载体借助SLAM技术已经能知足室内场景的自主导航任务。SLAM技术是在未知环境中实现自主定位的关键技术，而基于激光雷达同时定位与地图构建(激光SLAM)的技术广泛应用在无人驾驶、室内外机器人导航及三维重建等领域。激光SLAM之所以能作为无人驾驶定位方案中不可或者缺的技

2、术，其主要原因在于：(1)激光SLAM无需对定位场景进展预先布置，即能在未知的环境中进展定位效劳;(2)激光能准确测量障碍点的角度与间隔;(3)能在光线较差、光照变化较大的环境下工作;(4)可以生成便于导航的栅格地图与直观的点云地图。前谷歌无人车指导者SebastianThrun在其2005年度出版的经典著作?概率机器人?一书中介绍了怎样基于概率滤波的方法使用二维激光雷达进展地图构建和定位。该书详细阐述了从基于RB粒子滤波(Rao-BlackwellisedParticleFilter，RBPF)的经典FastSLAM算法，逐步开展成为基于激光雷达建图标准GMapping算法的经过4。2016

3、年度，Google开源其优化的激光SLAM算法Cartographer，该算法改良了GMapping计算耗时且无法有效处理闭环的缺点，利用帧间点云地图匹配与子地图构建思想有效地弥补了GMapping的缺陷。尽管激光SLAM技术可以在一定场景下知足定位精度的要求，但是难以在全工况的自动驾驶场景中提供持续、稳定的高精度定位输出。另外，由于定位场景的构造相似性且激光点云对环境的描绘才能缺乏，容易发生SLAM的闭环检测误报(FalsePositives)现象，进而降低系统定位与建图性能。本研究旨在通过将地磁信号与激光SLAM的闭环检测环节交融，提出激光SLAM在应对构造相似性及激光强干扰场景下的定位与

4、建图任务时的创新解决方法。同时，本研究将此技术应用到无人驾驶的定位与建图领域，实现了无人驾驶汽车全天候、全地域的精准基于矩阵补全的无人车定位，提升了无人驾驶汽车定位算法的精准性与稳定性。2相关研究进展2.1激光SLAM闭环检测近年度来一些国外学者为解决闭环检测误报现象对SLAM系统的影响，提出了闭环验证算法。Bosse与Roberts的工作提出，利用熵序列之间的最大相关性之和与投影直方图的验证度量来估计闭环检测的正确性，并在此根底上提升了闭环检测的正确率。Granstrom等提出了基于变换后扫描并在最后配准的闭环检测验证标准。Corso提出在室内环境中严格处理闭环验证时，由于闭环的现象可以从多

5、种传感器的数据源中被检测到，因此应将闭环验证算法与闭环检测算法分成两个方向进展研究。此外，Gao与Harle在已有报道的根底上，提出将地磁序列信号的匹配定位方法用于矫正基于PDR(PedestrianDeadReckoning)的SLAM闭环检测环节，以此来进步行人步态检测与定位的精度。(a) (c)闭环误检与错误定位(d)环境干扰中的错误建图图1常见闭环误检干扰及错误定位与建图2.2闭环检测模型闭环检测是指无人车经过一段较长行程并再次回到曾经历过的位置时，对地图中曾观测到的特征进展二次观测，同时通过两次观测的差异对全局地图进展调整，以减少定位与建图经过中的误差累计。目前主要的闭环检测方法包括

6、：(1)帧与帧闭环检测;(2)帧与子图闭环检测;(3)子图与子图闭环检测。以上闭环检测算法都是采用相关性扫描匹配，通过旋转矩阵R与平移向量T判断两帧激光点云的相似性。当激光雷达获得新的扫描帧时，在其附近一定范围搜索最优匹配帧，假设该最优匹配帧符合要求，那么认为是一个回环。该匹配问题可以描绘为如下公式： (1)其中，为匹配解算的刚性变换，为搜索空间，k为搜索空间的位姿数量，Mnearest表示观测激光点云hk和位置变换矩阵T的乘积与地图激光栅格点云最接近重合的置信度。对于每一帧激光点云插入子地图上时的信度和来讲，信度越高那么认为越相似。在空间中寻找出该信度和最大的匹配帧，需要在空间中寻找出匹配结

7、果的最优解。一种常见的方法是暴力匹配法，即在搜索空间范围内，对每一帧与当前帧进展上式非线性最小二乘计算。匹配算法解算的刚性变换与智能车初始的位姿相乘后便得出此时的位姿(xi，yi，i)。当匹配的两帧之间存在对激光光束产生干扰的材质(反射率过低的物体或者对光束有透射作用的玻璃等)时，场景中的这些干扰因素是激光点云匹配阶段误差累积的主要成分。其中，该累积误差可以通过闭环检测的方法进展消除。以下场景容易引起激光SLAM的闭环检测误检现象：(1)室内的长廊、空旷的室内大堂、构造布局相似的房间，如图1(a)所示;(2)场景中反射率过低的物体，如黑色吸光墙面、家具等;(3)场景中存在玻璃或者对激光光束产生

8、透射现象的物体，如图1(b)所示;(4)场景中存在镜子、不锈钢等对激光光束产生镜面反射的物体。随着地图扩大，匹配搜索效率决定了闭环检测的实时性。当SLAM在应对较大场景和复杂干扰的环境时，实时定位与建图对闭环检测算法的效率提出了更高要求。此外，对于空间相似性较高的环境，由于低线束激光雷达对环境特征的表达才能缺乏，导致闭环检测误报现象特别常见。对激光光束存在干扰的场景(玻璃、镜子、不锈钢等)，无人车运行定位与建图算法时，会出现一些未完全探明的区域，此处区域显示为灰色，如图1(d)红圈处所示。对于这些干扰，传统激光雷达定位主要是在干扰激光扫描平面处贴上反射片，或者是通过人工校验的方法进展解决。而传

9、统的人工后处理方法是通过使用机器人仿真软件RoboStudio中的画图工具将建图结果中的误判区域进展擦除，使其变为已探明区域，因此该方法既不智能也费时耗力。而本文通过对地磁序列信号的研究，提出了地磁序列信号与激光雷达交融的闭环检测方法，“一站式地解决了上述干扰对整个定位与建图系统的影响。3地磁信号与激光SLAM交融算法设计本文基于地磁导航技术与激光SLAM算法，针对无人驾驶载体定位与建图的闭环检测环节，设计了地磁序列搜索与激光点云匹配交融的闭环检测算法。算法框架如图2所示，其中红色虚线框中是本文提出的闭环粗匹配与地磁特征挑选闭环检测算法。图2地磁信号与激光雷达交融定位建图算法框架3.1基于地磁

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

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

12、测算法首先，获取当前激光点云数据与对应的三轴地磁信号，并将当前帧点云插入子地图中。由三轴地磁信号组成的三维列向量为(X,Y,Z)，收集到的X、Y、Z三轴方向上的地磁序列幅值分别为mx、my、mz，计算地磁序列的模值并参加地磁子序列Si，其中Si表示第i个10s地磁序列的模值。 (2)接着按照激光点云的匹配算法，匹配新构建的点云子地图与历史子地图。本文将知足子地图匹配得分阈值的点云子地图参加候选子图集，对应的地磁序列信号参加候选子序列集合。假设不存在知足条件的点云子地图，那么当前位姿不存在闭环，并跳出闭环检测环节。然后，通过使用动态时间规划(DynamicTimeWarping，DTW)算法计算

13、候选地磁子序列与新参加的地磁子序列的间隔。由于考虑到即使在闭环检测的地方，运动轨迹也不可能完全重合，因此闭环检测附近区域的运动必定是时而靠近上一次运动的轨迹，时而远离上一次运动的轨迹。针对这种无约束的运动(无固定运动轨迹的运动)，通过设置固定DTW间隔来约束阈值是不适用的。故使用迭代最近邻(IterativeClosestPoint，ICP)算法计算候选集中的运动轨迹，并与当前轨迹进展匹配，记录算法迭代至收敛的步数。最后，对于知足迭代步数约束，即运动轨迹相似的候选集地磁子序列，放宽DTW阈值的约束。进而使即使偏离原来轨迹的运动在闭环检测的地方也能知足DTW地磁匹配的阈值挑选条件。图4闭环粗匹配

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

15、姿节点参加闭环检测候选集。表1给出了地磁信号与激光雷达交融闭环检测算法的伪代码。图5所示为地磁序列与激光SLAM交融原理图。三轴地磁信号通过求平方和的形式形成序列Si，与一样时戳位姿对应的示意图形成新位姿节点Xi，表达地磁序列信号与激光解算位姿的交融原理。表1地磁信号与激光雷达交融的闭环检测的算法伪代码图5地磁序列与激光SLAM交融原理图4实验4.1无人驾驶硬件测试平台设计本研究主要采用为码头仓库重载自动导引运输车(AutomatedGuidedVehicle，AGV)提供高精度定位与建图作为研究背景，图6所示为地磁与激光交融SLAM实验平台总框图。该实验平台的定位与建图主要使用的传感设备包括

16、，Hokuyo单线激光雷达、禾赛40线激光雷达和XsensIMU。其中XsensIMU作为地磁序列信号的感悟设备，通过安装了ROS(RobotOperatingSystem)系统的笔记本电脑收集空间三轴地磁信号。实验平台的工控机采用英伟达的JetsonTX2，笔记本只作为与用户交互的终端，通过WIFI向工控机收发指令。激光雷达与收集地磁信号的IMU一同接在JetsonTX2上，用以建图与定位。本实验平台不仅能知足空间地磁序列作为定位匹配信号的验证性实验要求，还是基于地磁信号的激光SLAM闭环检测方法的主要实验平台。图6地磁与激光交融SLAM实验平台总框图4.2定位与建图效果对照实验为比照讲明G

17、oogle的Cartographer激光SLAM算法5与本文的地磁与激光交融SLAM算法在建图与定位效果上的优劣，选取对激光雷达产生大量干扰的场景，如图1(b)(中国科学院深圳先进技术研究院D栋2楼)实验场景中存在大片的玻璃落地窗，进展比照实验。图7为实验场景平面图。图7实验场景平面图定位与建图算法比照实验步骤：(1)对照实验采用AGV实验平台，控制实验无关变量进展实验(在一样场景、时间段、控制实验车运行大致一样的轨迹);(2)启动实验小车同时运行CartographerSLAM算法，实时观察SLAM算法输出的定位轨迹与建立的栅格地图;(3)让实验小车完好地运行一周，运行完毕后保存运行轨迹与建

18、立的栅格地图;(4)运行本文提出的地磁与激光交融SLAM算法，重复(2)、(3)步骤。4.3实验结果与分析4.3.1定位与建图效果比照实验搭载了激光雷达的AGV在室内场景运行一周后构建的栅格地图与推算的运动轨迹如图8(a)所示。结果显示，与小车实际行驶轨迹的最大出入是刚开场的一段错误定位轨迹，这是由于场景中构造相似性导致的。因为无论是从左到右还是从右到左行驶，激光雷达的观测量都存在极大相似性。在经过该构造相识性较高的环境后，Cartographer的定位才与实际情况相符合。而本文算法在一样干扰环境中定位的轨迹如图8(b)所示，符合无人车的实际运动。在进展定位与建图效果对照实验中，传统SLAM算

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

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

21、定界加速匹配算法提升了10%。图9三种闭环检测算法耗时比拟4.3.3算法稳定性实验与结果分析实验数据采用图7场景中收集的一圈激光点云与地磁序列信号。记实验数据待匹配激光点云总帧数为N，闭环检测的得分阈值为M，经算法挑选后的候选集合帧数为S。由于实验的一圈运行数据中仅存在一个正确闭环检测结果，因此闭环检测的召回率可定义为、误检率为，那么闭环检测的正确率为。为验证本文算法在闭环检测中的稳定性，实验通过调整匹配算法的得分阈值M，得到不同召回率下的闭环检测误检率。然后，分别对暴力匹配算法、Cartographer的分支定界加速匹配算法与本文算法进展比照实验与分析。闭环检测误检率和召回率分布情况如图10

22、所示。本文算法通过地磁匹配算法挑选出候选闭环检测位姿节点方法后，由于地磁匹配算法能快速地过滤掉很多容易造成误检的结果，因此本文算法的误检率在3种算法中一直维持在较低程度。从图10可看出，不管激光点云帧与子地图匹配的得分阈值怎样设置，在一样召回率的情况下，本文算法的闭环检测误检率都低于分支定界加速匹配算法与暴力匹配算法;在0.8召回率的情况下闭环检测的误检率降低了23%，明显提升了闭环检测算法的稳定性。图10三种闭环检测算法误检率与召回率比照5总结与展望本文以激光SLAM闭环检测环节中的误检现象，及在定位与建图环境中由干扰引起的闭环误检测与建图失真现象为研究课题。首先，在对激光SLAM闭环检测的

23、相关研究进展调研后，将地磁导航技术中的地磁匹配与激光SLAM中的闭环检测环节相结合。通过从激光SLAM算法中必要的IMU传感设备里提取地磁信号作为闭环的判断根据，有效地解决了建图与定位技术中闭环检测匹配速度缓慢及闭环误检的问题。然后基于本文提出的地磁序列搜索与激光点云匹配交融的闭环检测算法和地磁与激光交融SLAM实验平台设计，将传统SLAM算法与本文算法进展比照。最后对传统SLAM算法定位与建图失效的原因进展详细分析，并提出了在上述SLAM失效场景中的解决方案。实验结果说明，与激光SLAM技术的行业标杆Carto-grapher算法相比，本文算法降低了部分相似度较高引起的误检情况;同时在定位与

24、建图环境中，改善了激光光束反射与透射干扰引起的闭环误检与建图失真现象。在稳定性方面，本文算法与Cartographer算法相比，激光SLAM在匹配速度和闭环检测稳定性上都有明显提升。传统的基于滤波的激光SLAM，如CoreSLAM、GMapping与基优化的HectorSLAM、KartoSLAM、LagoSLAM和Cartographer，无法在激光干扰的场景中确保定位与建图的准确性。而本文算法由于使用了地磁匹配技术改良激光闭环检测环节，在应对上述干扰场景时均能稳定输出准确的定位与建图结果。经太多次实验测试，证明本文算法在闭环检测速度提升31%的前提下，定位精度在无激光干扰场景中仍可与Cartographer算法持平，进而进步了定位与建图的效率。此外，在激光干扰的场景中，本文算法的定位精度与建图效果优于上述所有激光SLAM算法。室外的复杂环境为无人驾驶精准定位与建图带来更多的挑战，同时室外对激光雷达感悟与IMU地磁信号的测量存在更多干扰。怎样在保证定位精度与建图效果的前提下，将地磁与激光交融SLAM技术拓展到室外驾驶环境是我们将来探究的方向。0