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

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. • The most prominent contribution of this paper is that relative angle measurements between leader AUV and slave AUV are introduced into FG-based CN algorithm in addition to range measurements for the first time. Furthermore, with the introduction of relative angle measurements, a CFG model is constructed with range and bearing angle measurements. • To get solutions on a CFG model, we use the clustering method to transfer the CFG model into a cycle-free FG model. Furthermore, SPA is adopted to pass and calculate messages in the proposed CFG model to obtain position and orientation estimation.