An Fusion SLAM Method for LiDAR Visual and IMU Based on Factor Map Elimination Optimization

Addressing the limitations of single-sensor SLAM (Simultaneous Localization And Mapping) techniques, degraded perception, and poor reliability in complex environments, this paper proposes a multi-factor graph fusion SLAM algorithm with IMU as the dominant system (ID-MFG-SLAM). Firstly, the utilization of a multi-factor graph model, with the IMU (Inertial Measurement Unit) as the primary system and visual and LIDAR sensors as secondary systems. This nov⁃ el structure incorporates observation factors from the secondary systems to constrain IMU biases and integrates IMU odome⁃ try factors for motion prediction and fusion. To reduce the optimization cost after fusion, a sliding window mechanism is in⁃ troduced for historical state information backtracking. Additionally, a QR decomposition elimination method based on Householder transformation is employed to convert the factor graph into a Bayesian network, simplifying the graph's struc⁃ ture and improving computational efficiency. Furthermore, an adaptive interpolation algorithm between quaternion spheri⁃ cal linear interpolation and linear interpolation is introduced. This algorithm projects LIDAR point clouds onto a unit sphere, enabling depth estimation of visual feature points. The experimental results show that compared to other classic al⁃ gorithms, this method can achieve absolute trajectory errors of about 0.68 m and 0.24 m in complex large and small scenes, respectively, with higher accuracy and reliability. © 2023 Chinese Institute of Electronics. All rights reserved.