Trajectory Optimization of LiDAR SLAM Based on Local Pose Graph

Abstract

Simultaneous localization and Mapping (SLAM) is one of the key technologies for autonomous navigation of mobile robots. In recent years, researchers have studied diverse sensors and proposed heterogeneous SLAM algorithms. Owing to the high precision and strong anti-interference ability of LiDAR, SLAM algorithm based on laser sensor has been widely studied and applied for autonomous navigation and 3D reconstruction. This paper studies the three-dimensional localization and mapping based on laser sensor. Aiming at the problem of trajectory accumulated error drift caused by sequential registration in LiDAR odometry, an optimization method based on local pose graph is proposed. Firstly, the initial pose estimation of the robot and corrected point cloud are figured out by a LiDAR odometry algorithm. Then, an omnidirectional local map is constructed by using the corrected point cloud, and the point cloud of current frame is registered with the omnidirectional local map to obtain more precise pose. Finally, the ICP algorithm is used to compute the frame to frame transformation which is used for local pose graph, the pose graph based on the G2O frame is designed to realize the further correction of the robot trajectory. The paper carried out four experiments on the KITTI dataset and the field test dataset, experimental results show that the loop-closure error of the proposed algorithm is reduced to 0.01%, the minimum relative error is reduced to 0.49% and the relative error of all scenarios is about 1/2 of that of LOAM which is known as a state-of-the-art LiDAR SLAM algorithm.