GNSS/IMU/LiDAR fusion for vehicle localization in urban driving environments within a consensus framework

The continuous, reliable, and accurate acquisition of position information is a fundamental requirement for autonomous vehicles. It underpins perception, decision-making, and planning modules in automated driving systems. However, the performance of multi-modal sensors, including the Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), and LiDAR, typically found in autonomous vehicles, is easily influenced by complex environmental factors encountered in urban driving conditions. This creates challenges for existing localization algorithms. More specifically, while GNSS performs accurately in open-sky areas, it is susceptible to the multi-path effect. Although the IMU-based Inertial Navigation System (INS) provides continuous position data, it suffers from cumulative errors. LiDAR-based map-matching is highly effective when sufficient point cloud features are present but fall short in plain environments. To address these individual sensor deficiencies, we propose the application of a consensus framework for GNSS/IMU/LiDAR fusion, and it is able to be expanded for multi-modal sensor fusion. Our approach seeks to take advantage of the redundancy inherent in these sensor systems. Our method begins with the derivation of the INS mechanization and its error dynamics. Subsequently, based on the Consensus Kalman Filter (CKF), we establish a GNSS measurement node and a Normal Distribution Transformation (NDT) map-matching measurement node. These are designed to estimate position errors in the INS. To counter sensor measurement failure or degradation, we introduce an adaptation mechanism for the CKF. This ensures position accuracy and continuity by normalizing measurement quality across different measurement nodes. Finally, to validate our consensus-based localization framework, we conducted extensive real-world vehicle tests in urban driving conditions. The results have confirmed the superior accuracy and robustness of our approach, even in the face of individual sensor failure. In our experiments, the proposed method reduces the localization mean absolute error by around 17% compared to INS/map-matching fusion, and around 59% compared to INS/GNSS fusion while map-matching fails. While the GNSS and map-matching measurements contain large noises, our framework achieves at least approximately a 30% improvement compared to traditional methods.