A Novel Cooperative Navigation Algorithm Based on Factor Graph and Lie Group for AUVs

Traditional cooperative navigation algorithms for multiple AUVs are typically designed for a single specific configuration, such as parallel or leader-slave. This paper proposes a novel cooperative navigation algorithm based on factor graph and Lie group to address the multi-AUV localization problem, which is applicable to various multi-AUV configurations. First, the motion state of an AUV is represented within the two-dimensional special Euclidean group (SE(2)) space from Lie theory. Second, the motion of the AUV and acoustic-based range and bearing measurements are modeled to derive the motion error function and the range and bearing error function, respectively. Depending on the formulation of the motion error function, the proposed approach comprises two methods: Method 1 and Method 2. Third, the Gauss-Newton method is employed for nonlinear optimization to obtain the optimal estimates of the motion states for all AUVs. Finally, a parameter-level simulation system for AUV cooperative navigation is established to evaluate the algorithm’s performance under two different multi-AUV configurations. Method 1 is designed for parallel configurations, reducing the average RMSE of position and orientation errors by 29% compared to the EKF. Method 2 is tailored for leader-slave configurations, reducing the average RMSE of position and orientation errors by 38% compared to the EKF. Simulation results demonstrate that the proposed algorithm achieves superior performance across different AUV configurations compared to conventional EKF-based approaches.