A Fast Iterative Closest Point-Based Autonomous Mobile Robot Localization Method Using Map Preprocessing
摘要
Matching LiDAR scans with map using the Iterative Closest Point (ICP) algorithm is one of the classic methods for robot pose estimation. However, ICP requires finding the nearest reference point in the map for each scan point during each iteration, which is extremely time-consuming. Inspired by the Euclidean Distance Field (EDF), this paper proposes a map preprocessing method to greatly improve the ICP matching efficiency. Firstly, the Breadth First Search (BFS) algorithm is used to find the nearest \(k\) obstacle grid points for each non obstacle grid point in the map and store the results in a hash table. Then, the set of nearest neighbors for each scan point during the iteration is obtained by querying the hash table. Finally, the closed form solution of Singular Value Decomposition (SVD) is applied to obtain the pose transformation. Experimental results show that our method significantly improves the computational efficiency of the algorithm while ensuring matching accuracy.