Multi-Sensor Fusion Localization with Factor Graphs for UGVs Under Adverse Conditions

Precise and robust localization is an established cornerstone of autonomous navigation, motion planning, and control, especially in complex environments. However, the degradation and loss of sensor measurements are likely to occur under adverse conditions like GNSS-denied environments and visually-degraded environments, rendering accurate state estimation a considerable challenge. In this paper, we propose a multi-sensor fusion localization method aiming at an accurate and robust pose estimation of vehicles in these challenging environments. We introduce the validity identification algorithms to detect possible sensor failures according to different sensor characteristics and mitigate their effects through covariance adjustment. As the backbone to fuse asynchronous sensor data, factor graph optimization is utilized to guarantee the accuracy within computational budgets. Our approaches are experimentally validated in practical and challenging situations. The results indicate that our approaches can cope with degraded situations, leading to an average localization error less than 1m even when multiple sensors fail to offer correct observations.