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

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.