Factor Graph-Based Tightly Coupled RTK/INS/LiDAR System With De-Drifting LiDAR Data Association in Urban Areas

Accurate, reliable, and continuous positioning is crucial for applications like autonomous driving and mobile robots. Integrating multiple sensors, such as the global navigation satellite system (GNSS), inertial navigation system (INS), and light detection and ranging (LiDAR), in a tightly coupled manner has become a promising solution to leverage their complementary advantages. However, LiDAR pose constraints may face significant inconsistencies with GNSS absolute measurements when using the frame-to-map data association method. Alternatively, frame-to-frame LiDAR data association suffers from limited accuracy due to sparse feature point matching. This article presents a tightly coupled GNSS real-time kinematic (RTK)/INS/LiDAR positioning system based on a de-drifting LiDAR data association method. The LiDAR keyframe selection strategy is designed by accounting for the availability and reliability of both GNSS and LiDAR data. A frame-to-last-GNSS-available-frame data association method is developed, utilizing both plane and edge features to construct LiDAR relative constraints. These measurements, alongside INS data and GNSS pseudorange and carrier phase measurements, are integrated within a factor graph optimization framework for pose estimation. Experimental results from an autonomous vehicle in urban environments demonstrate that the proposed tightly coupled system significantly outperforms other integration approaches using various sensor combinations, integration types, and LiDAR data association methods, improving 3-D positioning accuracy in root mean square (rms) from 12.40 m (using GNSS RTK) to 0.20 m. Moreover, the proposed method maintains high-accuracy absolute positioning with a 3-D position error of 0.28 m and a 3-D maximum error of 0.44 m during a 20-s GNSS outage.