An Advanced Cooperative Positioning Algorithm Based on Improved Factor Graph and Sum-Product Theory for Multiple AUVs

Abstract

In this research, a novel autonomous underwater vehicle (AUV) cooperative positioning algorithm is proposed to solve the implementation problem of multi-sensor-fusion applications. Different from the traditional methods [i.e., the extended Kalman filter (EKF), unscented Kalman filter (UKF), and iteration extended Kalman filter (IEKF)], which have large linearity error under the condition of nonlinear observation equation when multiple AUV are cooperative positioning, the proposed algorithm utilized the Baysis filter to solve the AUV cooperative problem. Factor graph and sum-product (FGS)-based cooperative positioning algorithm is established to mathematically implement the Bayse filter by converting the global function estimation problem into a local function sum-product estimation problem. Furthermore, to improve the performance of the proposed algorithm, a robust data processing method is presented by introducing a transform matrix to the estimated position information. To demonstrate and verify the proposed methods, the simulation and real tests in different scenarios are performed in this research. Compared with the traditional EKF, UKF, and IEKF cooperative positioning algorithm, the positioning error of the proposed improved FGS (IFGS) cooperative positioning algorithm is obviously smaller than that of the other three algorithms. Moreover, the IFGS algorithm can reduce the complexity of the algorithm, available improving the computational speed of the whole system. This proposed algorithm has important theoretical and practical value for the both industry and academic areas.