A novel cooperative navigation algorithm based on factor graph with cycles for AUVs

Abstract

A novel leader-slave cooperative navigation (CN) algorithm based on factor graph with cycles (CFG) is proposed for multiple Autonomous Underwater Vehicles (AUVs) in this paper. To estimate the positioning and orientation of the slave AUV simultaneously, a CFG is constructed with range and bearing angle measurements. Aiming at cycles existing on a factor graph (FG) due to bearing angle measurements, the clustering method is utilized to convert a CFG model into a cycle-free FG model, and then Sum-Product Algorithm (SPA) is adopted to obtain an estimation of the slave AUV's position and orientation. Compared with existing popular CN algorithms based on Extended Kalman Filter (EKF) and Particle Filter (PF), the simulation results show the superiority of the proposed CN algorithm in terms of the computation complexity and the estimation accuracy. In addition, the simulation results illustrate that the positioning accuracy is effectively improved by the introduction of bearing angle measurements compared with the FG-based CN algorithm with only range measurements. The validity of the proposed CN algorithm is also evaluated on field trial data, and experimental results demonstrate that the proposed CN algorithm has better performance than the EKF-based CN algorithm.