A robust factor graph optimization method of GNSS/INS/ODO integrated navigation system for autonomous vehicle

Abstract The GNSS/INS/ODO integrated navigation system provides accurate positioning for autonomous vehicles. However, the observation quality of GNSS and ODO can decrease in complex environments and various driving conditions, leading to a decline in the accuracy and robustness of the integrated navigation system. An FGO-Robust method is proposed to resolve this challenge by fusing the INS/ODO-Slip factor and adaptive GNSS factor. The INS/ODO-Slip factor compensates for ODO measurement values by estimating the wheel slip ratio in real-time during optimization, thereby reducing ODO measurement errors’ impact on factor graph optimization. Additionally, the adaptive GNSS factor minimizes the impact of GNSS outliers in the optimization process by designing a dynamic observation weight function. The vehicle experiments are conducted to evaluate the performance of the proposed method in different simulated conditions with ODO and GNSS measurement failures. The experimental results demonstrate that the FGO-Robust method can effectively improve the positioning accuracy of the integrated navigation system under various measurement failure conditions, with improvements of 83.4%, 66.1%, and 68.1% relative to FGO. Moreover, the FGO-Robust method improves the robustness of the integrated navigation system in complex environments and various driving conditions.