FGO-GIL: Factor Graph Optimization-Based GNSS RTK/INS/LiDAR Tightly Coupled Integration for Precise and Continuous Navigation

Abstract

As a relative positioning technique, LiDAR-Inertial Odometry (LIO) is known to suffer from drifting and can only provide local coordinates. To compensate for these shortages of LIO, an effective way is to integrate global navigation satellite system (GNSS) with LIO. In this contribution, we proposed a tightly coupled GNSS RTK/INS/LiDAR system under the factor graph optimization framework, termed as FGO-GIL, to achieve high-precision and continuous navigation in urban environments. This integration system fuses raw GNSS measurements (i.e., pseudorange and carrier phase measurements) with IMU and LiDAR information at the observation level atop a factor graph. Moreover, a keyframe-based nonlinear optimization scheme is designed to efficiently utilize measurements from the mixed heterogeneous sensors. Specifically, non-keyframes are united with IMU pre-integration for inter-frame optimization, which can provide accurate and high-frequency state predictions for the scan-matching of keyframes. Sparse keyframes are used to construct LiDAR factors through matching with the submap for sliding window optimization. To evaluate the effectiveness of our approach, real-world experiments are conducted in both campus and urban environments. The results demonstrate that our system can achieve continuous decimeter-level positioning accuracy in these complex environments, outperforming other state-of-the-art frameworks.