基于可观性分析的GNSS/IMU/视觉融合定位算法

Fusion Localization Through Integrating GNSS/IMU/Camera based on Observability Analysis

摘要:

作为决策、规划和控制的基础，精确的位姿估计对于智能汽车极为重要。为提高车辆的位姿中的航向估计精度，本文提出一种基于可观性的全球导航卫星系统（GNSS）、惯性测量单元（IMU）的视觉融合定位算法。首先，为分析GNSS/IMU组合定位误差状态的可观性，提出一种新的相对可观性分析方法，结果表明传统的GNSS/INS组合定位算法存在4个弱可观状态；随后，基于相对可观性理论，利用视觉惯性里程计估计的相对航向角，提出了一种基于可观性的融合定位算法；最后，实验验证了相对可观性分析的有效性，实验结果显示所提定位算法的最大航向误差为2.76°，航向误差RMS为1°，表明所提算法可以有效提高弱可观状态下车辆的航向估计精度。

Abstract:

Accurate pose estimation is of paramount importance for intelligent vehicles， serving as the foundation for decision-making， planning， and control. To enhance the accuracy of heading estimation in vehicle pose， a novel fusion localization algorithm based on observability is proposed in this paper， utilizing GNSS （Global Navigation Satellite System）， IMU （Inertial Measurement Unit）， and vision sensors. Firstly， to assess the observability of the error state in the GNSS/IMU system， a novel method for relative observability analysis is introduced， revealing the existence of four weakly observable states within the traditional GNSS/IMU system. Subsequently， a fusion localization algorithm grounded in relative observability is proposed， utilizing the relative heading angle estimated by Visual Odometry. The experimental results indicate that the proposed localization algorithm achieves a maximum heading error of 2.76° and an RMS heading error of 1°， highlighting the effective enhancement of vehicle heading accuracy in weakly observable states by the proposed algorithm.