A post-processing algorithm based on factor graph optimization for autonomous underwater vehicle integrated navigation

Abstract

Aiming at the requirements of offline high-precision navigation and positioning in the mapping of ocean floor, survey, and other fields for autonomous underwater vehicle (AUV)integrated navigation system, a post-processing algorithm based on factor graph optimization is proposed in this paper. First, the factor graph model is established by the factor graph formulation to address the asynchronous and heterogeneous problem of multi-source information fusion. Then the equivalent inertial measurement unit (IMU) factor is introduced to replace several consecutive IMU factors between two adjacent measurements to further aid high-rate navigation solutions. Second the forward and backward message passing processes are performed independently using the sum-product algorithm in the factor graph model. Finally, the optimal navigation solution is obtained by weighted smoothing the results of the forward and backward message passing processes. Simulation results show this algorithm proposed in this paper provides more accurate navigation accuracy and smoothness, especially in the case of signals loss or sensors fault. Compared with the federal Kalman filtering method, the horizontal positioning errors of the un-simplified factor graph, the simplified factor graph, and the factor graph optimization are improved by about 30%, 34%, and 80%, respectively. The semi-physical environment results also clearly verify the reliability and effectiveness of the proposed method, and its horizontal positioning accuracies are kept within −2∼2 m.