IMU/Camera/GNSS/UWB Integrated Localization Method Based on Factor Graph Optimization

Abstract

To address the challenges encountered by mobile robots in complex environments, such as satellite navigation failures, error accumulation in inertial navigation, environmental degradation and interference in visual navigation, and non-line-of-sight (NLOS) errors in ultra-wideband (UWB) positioning, this paper presents a novel multi-source fusion navigation algorithm for mobile robots. The proposed algorithm integrates data from IMU, camera, GNSS, and UWB, optimized through factor graph techniques. Initially, the algorithm fuses IMU with visual information to construct a point-line feature-based visual-inertial system. Subsequently, GNSS data is incorporated to establish a loosely coupled IMU/camera/GNSS system. Furthermore, when UWB signals are available and anchor positions are known, UWB ranging constraints are added, and sensor factor residual models are constructed to form a tightly coupled IMU/camera/GNSS/UWB navigation system. Pose estimation is achieved via factor graph optimization. Experimental results demonstrate that integrating GNSS data into the IMU/camera fusion significantly reduces positioning errors, achieving a performance improvement of 53.66%. Additionally, the IMU/camera/GNSS/UWB positioning system exhibits enhanced stability, with a performance improvement of 22.14%. The proposed algorithm effectively mitigates drift caused by error accumulation in the visual-inertial system and improves positioning accuracy and robustness in GNSS-denied environments.