李鑫, 仲训昱, 彭侠夫, 龚赵慧, 仲训杲. 基于多分辨率搜索与多点云密度匹配的快速ICP-SLAM方法[J]. 机器人, 2020, 42(5): 583-594. DOI: 10.13973/j.cnki.robot.190595
引用本文: 李鑫, 仲训昱, 彭侠夫, 龚赵慧, 仲训杲. 基于多分辨率搜索与多点云密度匹配的快速ICP-SLAM方法[J]. 机器人, 2020, 42(5): 583-594. DOI: 10.13973/j.cnki.robot.190595
LI Xin, ZHONG Xunyu, PENG Xiafu, GONG Zhaohui, ZHONG Xungao. Fast ICP-SLAM Method Based on Multi-resolution Search andMulti-density Point Cloud Matching[J]. ROBOT, 2020, 42(5): 583-594. DOI: 10.13973/j.cnki.robot.190595
Citation: LI Xin, ZHONG Xunyu, PENG Xiafu, GONG Zhaohui, ZHONG Xungao. Fast ICP-SLAM Method Based on Multi-resolution Search andMulti-density Point Cloud Matching[J]. ROBOT, 2020, 42(5): 583-594. DOI: 10.13973/j.cnki.robot.190595

基于多分辨率搜索与多点云密度匹配的快速ICP-SLAM方法

Fast ICP-SLAM Method Based on Multi-resolution Search andMulti-density Point Cloud Matching

  • 摘要: 针对激光SLAM(同步定位与地图创建)的实时性和定位精度问题,为了克服初始位姿不准确情况下增大搜索范围和位姿匹配分辨率对实时性的影响,本文在传统ICP-SLAM(迭代最近邻SLAM)基础上进行改进,提出了一种分层搜索与匹配的快速ICP-SLAM方法.首先,在搜索范围内采用由粗到细的分辨率进行全局搜索,并通过逐渐增加待匹配点的密度进行分步匹配计算.点云匹配过程中,通过构建距离像计算待匹配点的最近邻距离值,其计算复杂度降低为O(1).其次,通过对点云匹配结果进行优先排序和剪枝,快速排除非最优解.最后,以半数全局最优与全数局部最优原则作为搜索结束判断条件,提高搜索效率.SLAM Benchmark数据集上的测试结果表明,相比于流行的激光SLAM算法Cartographer,所提出的方法取得了更小的平均误差和平方误差,计算效率为Cartographer算法的2~5倍.同时,工业AGV(自动导引车)的实际应用实验验证了在初始位姿未知的情况下,可实现实时的位姿估计与建图,重复定位精度优于1.5 cm.因此,这种快速ICP-SLAM方法能够保证实现准确的定位估计,具有良好的实时性.

     

    Abstract: For better real-time performance and localization accuracy of LiDAR SLAM (simultaneous localization and mapping), a fast ICP-SLAM (iterative closest point SLAM) method with hierarchical search and matching is proposed on the basis of classic ICP-SLAM, in order to overcome the influence of the increased search range and pose matching resolution on real-time performance when the initial pose is not accurate. Firstly, global search is performed from coarse to fine in the search range, and calculation for matching is carried out step by step through gradually increasing the density of the points to be matched. In the process of point cloud matching, the distance from the nearest neighbour to the matched points is calculated by constructing the distance map, and the calculation cost is reduced to O(1). Secondly, the non-optimal solutions are eliminated quickly by prioritizing and pruning the points matching results. Finally, half-globally optimal principle and full-number locally optimal principle are taken as the judgment conditions for the end of the search to improve search efficiency. The test results on the benchmark dataset show that the proposed method achieves smaller average error and square error compared with the popular LiDAR SLAM algorithm Cartographer, and the computation efficiency is about 2~5 times faster than Cartographer algorithm. Meanwhile, the practical application to the industrial AGV (automated guided vehicle) demonstrates that the real-time pose estimation and mapping can be realized without initial pose knowledge, and the repeated positioning accuracy is smaller than 1.5 cm. In conclusion, the proposed fast ICP-SLAM method can ensure accurate localization and good real-time performance.

     

/

返回文章
返回