Multi-Stage Registration and Feature Based Robust and Precise LiDAR SLAM

In a complex and ever-changing environment, achieving stable and precise SLAM (Simultaneous Localization and Mapping) presents a significant challenge. The existing SLAM algorithms often exhibit limitations in design that restrict their performance to specific scenarios; they are prone to failure under conditions of perceptual degradation. SLAM systems should maintain high robustness and accurate state estimation across various environments while minimizing the impact of noise, measurement errors, and external disturbances. This paper proposes a three-stage method for registering LiDAR point cloud. First, the multi-sensor factor graph is combined with historical pose and IMU pre-integration to provide a priori pose estimation; then a new method for extracting planar features is used to describe and filter the local features of the point cloud. Second, the normal distribution transform (NDT) algorithm is used as coarse registration. Third, the feature to feature registration is used for fine registration to achieve iterative optimization of pose. This method also publishes high-frame-rate real-time pose data through IMU, achieving the simultaneous improvement of registration convergence speed, robustness, precision and real-time performance. In addition, this method is suitable for a variety of application scenarios of mechanical and solid-state LiDARs. In terms of experimental verification, this paper uses M2DGR dataset and outdoor testing to verify the precision of the algorithm. The ablation experiments of coarse registration and fine registration are carried out respectively to illustrate the importance of different stages. The results show that the multi-stage registration method is beneficial to improve the precision and robustness of the algorithm.