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.
-
-
[1] Thrun S, Burgard W, Fox D. Probabilistic robotics[M]. Cambridge, USA:MIT Press, 2005.
[2] Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM:A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163.
[3] Qin T, Li P L, Shen S J. VINS-Mono:A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4):1004-1020.
[4] Forster C, Pizzoli M, Scaramuzza D. SVO:Fast semi-direct monocular visual odometry[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2014:15-22.
[5] Kohlbrecher S, Stryk O V, Meyer J, et al. A flexible and scalable SLAM system with full 3D motion estimation[C]//IEEE International Symposium on Safety, Security, and Rescue Robotics. Piscataway, USA:IEEE, 2011:155-160.
[6] Hess W, Kohler D, Rapp H, et al. Real-time loop closure in 2D LIDAR SLAM[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2016:1271-1278.
[7] Grisetti G, Stachniss C, Burgard W. Improved techniques for grid mapping with Rao-Blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1):34-46.
[8] Li L, Yang M, Wang B, et al. An overview on sensor map based localization for automated driving[C]//2017 Joint Urban Remote Sensing Event. Piscataway, USA:IEEE, 2017. DOI: 10.1109/JURSE.2017.7924575.
[9] Besl P J, McKay N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2):239-256.
[10] Trehard G, Alsayed Z, Pollard E, et al. Credibilist simultaneous localization and mapping with a LIDAR[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2014:2699-2706.
[11] Arth C, Pirchheim C, Ventura J, et al. Instant outdoor localization and SLAM initialization from 2.5D maps[J]. IEEE Transactions on Visualization and Computer Graphics, 2015, 21(11):1309-1318.
[12] Choudhary S, Indelman V, Christensen H I, et al. Information-based reduced landmark SLAM[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2015:4620-4627.
[13] Mäkelä T, Clarysse P, Sipilä O, et al. A review of cardiac image registration methods[J]. IEEE Transactions on Medical Imaging, 2002, 21(9):1011-1021.
[14] Newcombe R A, Izadi S, Hilliges O, et al. KinectFusion:Real-time dense surface mapping and tracking[C]//10th IEEE/ACM International Symposium on Mixed and Augmented Reality. Piscataway, USA:IEEE, 2011:127-136.
[15] Schapire R E, Freund Y, Bartlett P, et al. Boosting the margin:A new explanation for the effectiveness of voting methods[J]. Annals of Statistics, 1998, 26(5):1651-1686.
[16] Wang S F, Wu Z, Zhang W C. An overview of SLAM[M]//Lecture Notes in Electrical Engineering, Vol.528. Berlin, Germany:Springer, 2019:673-681.
[17] Akca D, Gruen A. Fast correspondence search for 3D surface matching[C]//International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Vol.36.2005:186-191.
[18] Zhang Z Y. Iterative point matching for registration of free-form curves and surfaces[J]. International Journal of Computer Vision, 1994, 13(2):119-152.
[19] Liu Y H. Constraints for closest point finding[J]. Pattern Recognition Letters, 2008, 29(7):841-851.
[20] Liu Y H. Improving ICP with easy implementation for free-form surface matching[J]. Pattern Recognition, 2004, 37(2):211-226.
[21] Fitzgibbon A W. Robust registration of 2D and 3D point sets[J]. Image and Vision Computing, 2002, 21(13-14):1145-1153.
[22] Breuel T M. Implementation techniques for geometric branch-and-bound matching methods[J]. Computer Vision and Image Understanding, 2003, 90(3):258-294.
[23] Tiar R, Ouadah N, Azouaoui O, et al. ICP-SLAM methods implementation on a bi-steerable mobile robot[C]//11th IEEE International Workshop of Electronics, Control, Measurement, Signals and Their Application to Mechatronics. Piscataway, USA:IEEE, 2013. DOI: 10.1109/ECMSM.2013.6648973.
[24] Bustos A P, Chin T J, Eriksson A, et al. Fast rotation search with stereographic projections for 3D registration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2016, 38(11):2227-2240.
[25] Yang J L, Li H D, Campbell D, et al. Go-ICP:A globally optimal solution to 3D ICP point-set registration[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2016, 38(11):2241-2254.
[26] Mellado N, Aiger D, Mitra N J. Super 4PCS fast global pointcloud registration via smart indexing[J]. Computer Graphics Forum, 2014, 33(5):205-215.
[27] Olson E B. Real-time correlative scan matching[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2009:4387-4393.
[28] Jost T, Hügli H. A multi-resolution scheme ICP algorithm for fast shape registration[C]//1st International Symposium on 3D Data Processing Visualization and Transmission. Piscataway, USA:IEEE, 2002:540-543.
[29] Back J H, Kim S, Ho Y S. High-precision 3D coarse registration using RANSAC and randomly-picked rejections[C]//24th International Conference on Multimedia Modeling. Cham, Switzerland:Springer, 2018:254-266.
[30] Kummerle R, Grisetti G, Strasdat H, et al. G2o:A general framework for graph optimization[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2011:3607-3613.
[31] Kümmerle R, Steder B, Dornhege C, et al. On measuring the accuracy of SLAM algorithms[J]. Autonomous Robots, 2009, 27(4):387-407.
-
期刊类型引用(3)
1. 赵龙云，伞红军，陈久朋，彭真. 基于ICP算法的移动机器人激光定位研究. 激光与光电子学进展. 2024(08): 99-108 . 
百度学术
2. 陈涛，钱晓明，楼佩煌，彭立军. 基于图搜索点对切线ICP的高精度激光点云配准方法. 机器人. 2024(05): 600-610 . 本站查看
3. 常路，单梁，戴跃伟，戚志东. 未知环境下基于改进DWA的多机器人编队控制. 控制与决策. 2022(10): 2524-2534 . 百度学术
其他类型引用(13)
计量
- 文章访问数: 155
- HTML全文浏览量: 546
- PDF下载量: 360
- 被引次数: 16
下载:
