A Factor Graph With Mercator Projection-Based Cooperative Localization Algorithm for Multiple AUVs

To address the discrepancy between geodetic coordinates provided by navigation equipment on autonomous underwater vehicles (AUVs) and Cartesian coordinates employed for calculating Euclidean distance within a cooperative localization (CL) system, a factor graph (FG) with Mercator projection-based CL algorithm for multiple AUVs (MAUVs) is proposed. First, the Mercator projection is utilized to formulate the range measurement equation, and with the kinematic equation of the AUV, a state-space model of a CL system for MAUVs is constructed. Furthermore, the homologous FG model of the CL system is established. Leveraging the independence of the coordinates inherent in the Mercator projection, the FG model avoids the introduction of additional cycles. The sum-product algorithm (SPA) is utilized to calculate navigation states of the slave AUV. Then, a performance evaluation function (PEF) for a CL system with two master AUVs in the Mercator projection-based coordinate system is established, and the consistency of PEFs across different coordinate systems is demonstrated. Finally, both simulation and field test results indicate that the proposed CL algorithm offers superior localization accuracy and lower execution time compared to conventional extended Kalman filter (EKF)-based CL algorithms.