Multi-AUV Cooperative Navigation Algorithm Based on Factor Graph With Stretching Nodes’ Strategy

Unknown ocean currents usually lead to trajectory drift of autonomous underwater vehicle (AUV) and even cause localization and navigation failures. A multi-AUV cooperative navigation (CN) algorithm based on factor graph (FG) with stretching nodes’ strategy is presented to address the issue of ocean currents disruption in the current FG-based algorithm. Initially, the system state-space model and corresponding FG model are established under the disturbance of unknown ocean currents for the CN system. Ocean current velocities are introduced as variable nodes into the FG model, which resulted in the model occurring cycles with six nodes. Stretching nodes’ strategy then transforms the FG into a cycle-free FG, thereby avoiding iteration and approximation of messages. Based on this, a parametric sum-product algorithm (SPA) is presented to realize the synchronous estimation of AUV navigation information and ocean current velocities by computing and passing the FG messages. The simulation results show that the proposed algorithm effectively resolves the simultaneous estimation of ocean current velocities and AUV navigation information, even when AUVs randomly access or exit the system. The outcome of the lake-water field trials demonstrates that the proposed algorithm can improve the AUV localization accuracy and the average current velocity estimation accuracy by 7.71% and 14.60%, respectively, compared with the CN algorithm based on FG with message iterations for a cycle while meeting the demand of computational complexity.