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

Abstract

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.

1. Introduction

In the field of marine exploration and development, the cooperative operation mode of Autonomous Underwater Vehicle (AUV) clusters demonstrates great potential in applications such as seabed mapping, resource exploration, and military operations [1,2,3,4]. For large-scale underwater monitoring missions, the multi-AUV cooperative exploration and data fusion mode offers advantages including extensive coverage, strong mission fault tolerance, and high data acquisition efficiency. The underwater environment is characterized by limited communication capabilities and rapid attenuation of positioning signals, making high-precision cooperative navigation (CN) systems a core technology for achieving multi-AUV coordination [5,6,7,8]. Through CN technology, multiple AUVs can share positional information and environmental perception data, effectively mitigating navigation error accumulation in individual AUVs, and significantly enhancing the positioning accuracy and mission reliability of cluster operations.

At present, CN algorithms can be broadly categorized into the following three types: filter-based, graph theory-based, and optimization theory-based.

1.1. Filter-Based CN Algorithms

Filter-based CN and localization algorithms predominantly adopt the Kalman filter (KF) framework. The Kalman filter is an optimal estimation method based on the principle of minimum variance, capable of estimating system states, eliminating redundancy, and predicting future states. Consequently, filter-based CN algorithms have been extensively researched.

Harris et al. [9] utilized numerical simulations to assess the influence of range-rate observations on the performance of the Centralized Extended Kalman Filter (CEKF) CN algorithm. Their results demonstrate that the inclusion of range-rate measurements can offer benefits compared to range-only approaches, particularly in scenarios where the accuracy of range measurements is degraded. Gao et al. [10] developed a Huber-based iterated divided difference filter (HIDDF) for cooperative localization of AUVs using a single surface leader. This technique delivers improved accuracy and faster convergence relative to the standard DDF under conditions of weak observability and significant initial errors, while also exhibiting robustness against outliers. Zhao et al. [11] introduced an unscented particle filter framework for multi-leader AUV cooperative localization aimed at overcoming limitations related to low accuracy and non-Gaussian noise. The approach effectively mitigates particle depletion and enhances positioning precision across diverse noise environments. Huang et al. [12] presented a new adaptive extended Kalman filter (EKF) to tackle the issue of unknown noise covariance in AUV cooperative localization. By employing an online expectation-maximization strategy, the method dynamically estimates both prediction error and measurement noise covariance. Zhang et al. [13] put forward a consistent EKF-based cooperative localization method for multi-AUV systems to resolve state estimation inconsistencies. Through correction of linearized measurements in the Jacobian matrix, their strategy ensures estimation consistency. Kim et al. [14] formulated a cooperative algorithm for multi-AUV navigation and sea current estimation that utilizes fused Kalman filters. Requiring only one leader AUV equipped with Doppler Velocity Log (DVL) and Ultra Short Baseline (USBL), the method enhances group localization performance in environments with unknown currents, as supported by simulation studies. Li et al. [15] constructed a robust leader–slave CN algorithm for AUVs based on Student’s t extended Kalman filter (SEKF), which strengthens resistance to outliers arising from acoustic communication and unreliable sensors in comparison to conventional Gaussian-based EKF. Zhu et al. [16] proposes a leader-slave AUV CN method to overcome accuracy degradation and limited beacon calibration range in underwater missions. To counter drift from ocean currents, a navigation model incorporating current velocity as a state variable is developed using five filtering methods.

1.2. Graph Theory-Based CN Algorithms

Graph theory-based algorithms address complex systems by constructing simple yet effective graphical models and applying fundamental graph theory techniques to solve related problems. In cooperative localization systems, this involves first establishing a graphical model of the system, then utilizing measurement data as connecting edges between graph nodes, and finally employing graph theory algorithms to compute optimal estimates.

Fan et al. [17] proposes a novel factor graph and sum-product (FGS) based cooperative positioning algorithm for autonomous underwater vehicles (AUVs) to address the limitations of traditional Kalman filter methods in multi-sensor fusion applications. The proposed improved FGS (IFGS) algorithm employs a Bayesian framework to reduce linearization errors and incorporates a robust data processing mechanism to enhance estimation accuracy. Ben et al. [18] proposes a novel factor graph with cycles (CFG)-based leader–slave CN algorithm for multi-AUV systems. By incorporating range and bearing measurements, the method estimates the slave AUV’s position and orientation. Cycles introduced by bearing data are resolved via clustering, enabling efficient inference using the sum-product algorithm. Ma et al. [19] proposes a factor graph-based parallel CN algorithm for AUVs in unknown underwater environments. The method constructs a factor graph model that distributes local navigation and inter-AUV observation data across platform nodes. Message passing within the graph enables optimal state estimation for each AUV. Li et al. [20] proposes HTACL, a hybrid TOA/AOA cooperative localization algorithm for multi-AUV systems in anchor-free environments. By jointly utilizing TOA and AOA measurements, the method reduces inertial error accumulation and enhances relative positioning accuracy. Li et al. [21] proposes a factor graph-based cooperative localization algorithm for multi-AUV systems that uses Mercator projection to bridge geodetic and Cartesian coordinate discrepancies. The method constructs a cycle-free factor graph model to efficiently estimate slave AUV states via the sum-product algorithm, avoiding additional computational cycles. Guo et al. [22] addresses localization delays in multi-AUV cooperative systems caused by slow underwater acoustic communication. A prediction model is proposed that uses least squares to estimate unreceived distance data and incorporates maximum correntropy criterion into factor graph optimization. Ben et al. [23] proposes a factor graph-based CN algorithm with a stretching nodes’ strategy to mitigate unknown ocean current disturbances in multi-AUV systems. By incorporating ocean current velocities as variable nodes and transforming cyclic graphs into cycle-free structures, the method enables simultaneous estimation of AUV states and current velocities using a parametric sum-product algorithm.

1.3. Optimization Theory-Based Cooperative Navigation Algorithms

Optimization algorithms enhance CN and localization performance by breaking down complex problems into simpler subproblems. The process involves establishing constraint equations for the system, defining an objective function, and solving for the optimum value of this function to obtain an optimized solution.

Zhang et al. [24] proposes a dual-leader CN method for AUVs using the Cross-Entropy (CE) algorithm and Markov Decision Process (MDP). By predefining slave AUV trajectories, the approach optimizes leader AUV paths probabilistically to minimize observation error and maintain suitable ranging distances. Nerurkar et al. [25] proposes a distributed Maximum A Posteriori (MAP) estimator for multi-robot cooperative localization. The method distributes data and computations across robots to reduce memory and processing demands compared to centralized approaches. It incorporates a distributed data-allocation scheme and conjugate gradient optimization to improve computational efficiency and robustness. Zhou et al. [26] proposes SAPSO-AUFastSLAM, an improved algorithm for AUV simultaneous localization and mapping (SLAM) that adaptively estimates time-varying observation noise and optimizes particle resampling. It incorporates adaptive noise correction and simulated annealing particle swarm optimization to prevent filter divergence and avoid local minima. Bo et al. [27] enhances a previously developed Huber-based robust cooperative localization algorithm for AUVs by incorporating adaptive noise estimation. The proposed method uses a covariance matching technique to estimate both Gaussian and non-Gaussian process and measurement noise in real time, thereby adjusting the filter gain adaptively. Zhu et al. [28] proposes a hierarchical reinforcement learning-based method for leader-slave multi-AUV CN to mitigate positioning errors caused by low-precision sensors. The approach formulates the problem as a semi-Markov decision process and combines hierarchical reinforcement learning with a polar Kalman filter for trajectory planning.

CN algorithm based on factor graph enables highly accurate and robust state estimation by providing a transparent framework for modeling and optimizing over complex measurement relationships. In this paper, we design a CN algorithm based on factor graph and Lie group. To perform nonlinear optimization, the motion states of AUVs are described using the two-dimensional special Euclidean group (SE(2)) space. To calibrate and fuse data, error functions are derived by modeling AUV motion and acoustic ranging and bearing measurements. The Gauss-Newton method is then employed to solve this nonlinear optimization problem for optimal state estimation. To accurately simulate the actual performance of AUVs, a parameter-level cooperative navigation simulation system is established. Simulation experiments demonstrate that the proposed algorithm outperforms the EKF algorithm across different AUV configurations.

Unlike prior factor-graph works for AUV CN that rely on the sum-product algorithm, our method leverages a nonlinear optimization backend (Gauss-Newton) on Lie groups, yielding higher accuracy and robustness. The main contributions of this paper and its distinctions from the state of the art are summarized as follows:

(1)

Novel State Representation: Unlike existing factor graph-based CN methods [17,18,19,20,21,22,23] that typically represent the AUV state in vector space (e.g., $x={[p_x,p_y,\theta ]}^T$, this work is the first to utilize the two-dimensional Special Euclidean Group SE(2) from Lie theory to describe the motion state of AUVs. This representation provides a more natural framework for handling the rigid-body motions of AUVs.

(2)

Dual-Mode Optimization Framework: We propose two distinct optimization methods tailored for different operational configurations. Method 1 is designed for parallel configurations where all AUVs possess similar sensor accuracy. Method 2 is specifically tailored for leader-slave configurations, which strategically leverages the high-precision attitude information from a well-equipped leader AUV, preventing the degradation of its attitude estimate by noisy acoustic measurements—a common issue in standard EKF or single-method factor graph approaches.

(3)

Comprehensive Performance Validation: A parameter-level simulation system that incorporates AUV dynamics is established for evaluation. The proposed methods demonstrate superior performance compared to the conventional EKF, with significant reductions in average RMSE for both parallel (29% reduction with Method 1) and leader-slave (38% reduction with Method 2) configurations.

2. Structures and Devices of Multiple AUVs

A CN system for multiple AUVs has two structures: parallel configuration and leader-slave configuration.

2.1. Parallel Configuration

Parallel configuration, as shown in Figure 1, means that there is no distinction between primary and secondary AUVs. Each AUV has the same function and uses its own navigation sensor to determine its own position. Through underwater acoustic communication, it exchanges information with other AUVs in the system and obtains the position information of neighboring AUVs. Parallel configuration, due to the equal status of each AUV and flexible system configuration, the failure of one AUV will not have a significant impact on the system. Therefore, it has good robustness and is suitable for performing tasks that are easily damaged by some aircraft.

2.2. Leader-Slave Configuration

The leader-slave configuration refers to a small number of AUVs equipped with high-precision navigation equipment as the main reference nodes, while other vehicles are equipped with relatively low-precision navigation equipment. According to the different number of leader AUVs, the positioning system can be divided into two modes: single leader and multi leader. The single leader system selects one AUV equipped with high-precision navigation equipment as the leader AUV, and the rest are slave AUVs. The single leader system has simple navigation, easy deployment, relatively low cost, and good operability. However, because the overall navigation performance of the CN system depends on the main vehicle, high requirements are placed on the positioning ability, reliability, and stability of the leader AUV. The typical leader-slave structure is shown in Figure 2.

The algorithm of CN between two AUVs can be extended to multiple AUVs. This article studies the algorithm for CN between two AUVs. Without appointing configuration, one AUV is referred to as the leader AUV and the other AUV is referred to as the slave AUV. As shown in Figure 3, for CN system, AUVs will be equipped with IMU, DVL, depth gauge, and acoustic transmission/receiving equipment. In addition, the distance and direction measurement modes of cluster AUVs studied by different scholars are not completely the same. The mode studied in this article is the transmission from the leader AUV to the receiving from the slave AUV, which means that only from slave AUV has measuring functions.

3. Factor Graph Optimization Theory

This section reviews the well-established theories of factor graph and maximum a posteriori estimation, which form the foundational framework for our proposed algorithm [29,30].

Factor graph is a bipartite graph model used to represent the joint probability distribution of random variables. The factor graph model $G$ can be defined as a set of state nodes $X$, a set of factor nodes $U$, and an edge set $E$ connecting nodes. State nodes $x_i\in X$ represent random variables of the probability distribution. Factor nodes ${\varphi }_i\in U$ represent the probability function of local state variables and are also local functions of factorization. The edge $e_z\in E$ represents the relationship between factor nodes and state variables. The factor graph model represents the joint probability distribution of state variables through state nodes, factor nodes, and edges, and the posterior estimation solution of state variables is the optimal estimation value under joint constraints. According to Bayes’ formula, if there are $m$ state variables $x_{0:m}\in X$ and $n$ measurement values $z_{0:n}\in Z$ at a given time, the posterior probability density $p(X|Z)$ of the set of state variables $X$ can be decomposed into:

$$p(X|Z)={\displaystyle \frac{p(Z|X)p(X)}{p(Z)}}\propto p(Z|X)p(X)$$

(1)

In the decomposition results of the above equation, $p(X)$ and $p(Z\left|X\right.)$ correspond to the likelihood probability density of the state variables and measurement information in the factor graph, respectively. In the factor graph, both the posterior probability density $p(X\left|Z\right.)$ and the prior probability $p(X)$ are represented by factor nodes $\varphi (X)$. Therefore, the posterior probability density of the state variable $X$ in Equation (1) can be rewritten as a factorization form:

$$p(X|Z)\infty {\displaystyle \prod _i{\varphi }_i(x_i)=\varphi (X)}$$

(2)

where $x_i$ is the subset of state nodes connected to factor ${\varphi }_i$. The maximum posterior estimate of the state variable $X$ in the factor diagram is:

$$X^{MAP}=\underset{X}{\arg \max }{\displaystyle \prod _i{\varphi }_i(x_i)}$$

(3)

For Gaussian distributed noise, factors have probability distributions in the following form:

$${\varphi }_i(x_i)\infty \exp \{-{\displaystyle \frac{1}{2}}{‖h_i(x_i)\boxminus z_i‖}_{{\sigma }_i}^2\}$$

(4)

Among them, $h_i()$ represents the observation function of the state variable, $z_i$ represents the actual observation value, $\boxminus$ represents generalized subtraction, and ${\sigma }_i$ represents the weight matrix. The nonlinear least squares optimization objective of factor graph can be written as

$$X^{MAP}=\underset{X}{\arg \min }F(X)=\underset{X}{\arg \min }({\displaystyle \sum _i{‖h_i(x_i)\boxminus z_i‖}_{{\sigma }_i}^2})=\underset{X}{\arg \min }({\displaystyle \sum _i{‖f_i(x_i)‖}_{{\sigma }_i}^2})$$

(5)

Among them, $f_i(x_i)={‖h_i(x_i)\boxminus z_i‖}_{{\sigma }_i}^2$, $F(X)={\displaystyle \sum _i{f}_i(x_i)}$ represents the error function of nonlinear optimization in the factor graph.

The specific factor graph model for our multi-AUV cooperative navigation system, which visualizes these state nodes and factors (e.g., motion, range, bearing), is depicted in Figure 7 in Section 4.3.

4. Modeling Based on Li Group

4.1. Model Design

As shown in Figure 4, the navigation process of AUV is generally divided into navigation coordinate system $O_n{x}_n{y}_n$ and body coordinate system $O_b{x}_b{y}_b$. In CN system, all AUVs use the same navigation coordinate system.

The depth gauge equipped on AUVs usually has high accuracy, and the cooperation tasks of AUVs are usually carried out in a fixed depth state, so it can be considered that all AUVs move in a two-dimensional plane. The state of AUV at time $i$ can be represented as:

$$x_i=\left\{p_{i,x},p_{i,y},{\theta }_i\right\}$$

(6)

where $p_{i,x}$ and $p_{i,y}$ represent the position of the AUV in the $x_n$ and $y_n$ directions of the navigation coordinate system, and ${\theta }_i$ represents the heading angle of the AUV in the navigation coordinate system. Obviously, $x_i$ is in the vector space.

In the factor graph, the set of AUV states from time 0 to time m can be represented as:

$$X=[x_0,x_1,\dots ,x_m]$$

(7)

The pose of a vehicle in a two-dimensional plane can also be represented using the SE(2) space in the Lie group, and the motion state of the AUV at the time $i$ can also be expressed as pose matrix:

$$M_i=\left[\begin{array}{l}{R}_i T_i\\ 0 1\end{array}\right]$$

(8)

where $R_i=Exp({\theta }_i)=\left[\begin{array}{l}\cos {\theta }_i -\sin {\theta }_i\\ \sin {\theta }_i \cos {\theta }_i\end{array}\right]$ represents the rotation matrix from the navigation coordinate system to the body coordinate system, and $T_i=\left[p_{i,x} p_{i,y}\right]$ represents the translation transformation vector from the navigation coordinate system to the body coordinate system. $M_i$ in SE(2) space and $x_i=\left\{p_{i,x}, p_{i,y}, {\theta }_i\right\}$ in the vector space correspond to each other, and their conversion relationship is represented by the symbols Log and Exp as follows:

$$\begin{array}{l}{M}_i=\mathrm{Exp}(x_i)\\ x_i=\mathrm{Log}(M_i)\\ M_{i+1}=M_i·\Delta M=M_i·\mathrm{Exp}(\Delta x)\end{array}$$

(9)

where $\Delta M$ is the variation in SE(2) space with respect to the increment $\Delta x$ in the vector space. Two-dimensional Lie group SE(2) uses a single matrix multiplication to describe complex transformations uniformly, represents orientation via rotation matrices, naturally avoids singularities, and enables unconstrained optimization on its corresponding Lie algebra, resulting in more efficient and stable computations. The set operation definitions for the two-dimensional Lie group SE(2) are as follows:

$$M·p=T+Rp,p={[p_x p_y]}^T$$

(10)

Its Jacobian matrix can be derived using the following formula:

$$\begin{array}{l}{J}_M^{M·p}={\displaystyle \frac{\partial (M·p)}{\partial M}}=\left[R R{\left[1\right]}_{\times } p\right]\\ J_p^{M·p}={\displaystyle \frac{\partial (M·p)}{\partial p}}=R\\ {\left[1\right]}_{\times }=\left[\begin{array}{l}0 -1\\ 1 0\end{array}\right]\end{array}$$

(11)

When $p=p_0=[0 0]$, Equation (11) can be transformed into:

$$J_M^{M·p_0}={\displaystyle \frac{\partial (T)}{\partial M}}=\left[R 0\right]$$

(12)

The adoption of the SE(2) Lie group for state representation is motivated by the limitations of optimizing over states parameterized in vector space (e.g., using Euler angles for orientation). Such parameterizations can suffer from singularities (gimbal lock) and constraints, leading to instability and inefficiency in nonlinear optimization. The Lie group framework provides a more natural and robust foundation for describing the rigid-body motions of AUVs. By representing the state on the SE(2) manifold, we achieve a more stable and efficient estimation process. This constitutes a fundamental aspect of our novel methodology.

4.2. Observation Model Based on Li Group

The AUV CN process includes motion observation and measurement of range and bearing.

4.2.1. Motion Model

Assuming that at time $i$, the velocity measurements of DVL are ${\tilde{v}}_{i,x}^l,{\tilde{v}}_{i,y}^l$ and the angle measurement of IMU is ${\tilde{\theta }}_i$, as shown in Figure 5.

The pose change measurements from time 0 to time m can be represented by SE(2). So, the error function f of the pose change measurement can be expressed as:

$$f_{i,p}(x)={‖Log(Z_{i,i+1}^{-1}{M}_i^{-1}{M}_{i+1})‖}_{{\sigma }_p}^2$$

(13)

The notation ${‖·‖}_A$ denotes the A-norm induced by a symmetric positive definite matrix A. It is defined for a vector $b$ as: ${‖b‖}_A=\sqrt{b^{T}Ab}$. Where ${\sigma }_p$ denotes the weight matrix of pose transformation measurement.

4.2.2. Range and Bearing Model

At time $i$, the pose of the leader AUV is $\left(p_{i,x}^l, p_{i,y}^l, {\theta }_i^l\right)$, the pose of the follower AUV is $\left(p_{i,x}^f, p_{i,y}^f, {\theta }_i^f\right)$, and the measurements of range and bearing are $\left({\tilde{L}}_i,{\tilde{\phi }}_i\right)$. The schematic diagram of direction and distance measurement process is shown in Figure 6:

The range error function $f_{i,L}(x)$ at time $i$ can be expressed as:

$$f_{i,L}(x)={‖{\tilde{L}}_i-\sqrt{{(p_{i,x}^l-p_{i,x}^f)}^2+{(p_{i,y}^l-p_{i,y}^f)}^2}‖}_{{\sigma }_L}^2$$

(14)

where ${\sigma }_L$ represents the weight matrix of ranging. The bearing error function $f_{i,\phi }(x)$ at time $i$ can be expressed as:

$$f_{i,\phi }(x)={‖{\tilde{\theta }}_i^f+{\tilde{\phi }}_i-\arctan {\displaystyle \frac{p_{i,x}^l-p_{i,x}^f}{p_{i,f}^l-p_{i,f}^f}}‖}_{{\sigma }_{\phi }}^2$$

(15)

where ${\sigma }_{\phi }$ represents the weight matrix of bearing.

4.3. System Structure of Factor Graph

From time 0 to time m, the leader AUV has a pose set $\left(p_{0,x}^l, y_{0,y}^l, {\theta }_0^l\right)$…$\left(p_{m,x}^l, p_{m,y}^l, {\theta }_m^l\right)$, and the slave AUV has a pose set $\left(p_{0,x}^f, y_{0,x}^f, {\theta }_0^f\right)$…$\left(p_{m,x}^f, p_{m,y}^f, {\theta }_m^f\right)$. Figure 7 depicts the motion model and range and bearing factors of the entire process.

According to (5), the nonlinear least squares optimization objective of the factor graph can be written as:

$$\begin{array}{l}{X}^{MAP}=\underset{X}{\arg \min }F(x)=\underset{X}{\arg \min }{\displaystyle \sum _i{f}_i(x_i)}\\ \left\{\begin{array}{l}{f}_i^l(x_i)={‖Log\left({\left(Z_{i,i+1}^l\right)}^{-1}{\left(M_i^l\right)}^{-1}{M}_{i+1}^l\right)‖}_{{\sigma }_p^l}^2\\ f_i^f(x_i)={‖Log\left({\left(Z_{i,i+1}^f\right)}^{-1}{\left(M_i^f\right)}^{-1}{M}_{i+1}^f\right)‖}_{{\sigma }_p^f}^2\\ f_{i,L}(x_i)={‖{\tilde{L}}_i-\sqrt{{(p_{i,x}^l-p_{i,x}^f)}^2+{(p_{i,y}^l-p_{i,y}^f)}^2}‖}_{{\sigma }_L}^2\\ f_{i,\phi }(x_i)={‖{\tilde{\theta }}_i^f+{\tilde{\phi }}_i-\arctan {\displaystyle \frac{p_{i,x}^l-p_{i,x}^f}{p_{i,f}^l-p_{i,f}^f}}‖}_{{\sigma }_{\phi }}^2\end{array}\right.\end{array}$$

(16)

Optimizing the above equation can obtain the optimal estimation value of the master and slave AUV poses at all times, which is denoted as $\left({\widehat{p}}_{0,x}^l, {\widehat{p}}_{0,y}^l, {\widehat{\theta }}_0^l\right)$…, $\left({\widehat{p}}_{m,x}^l, {\widehat{p}}_{m,y}^l, {\widehat{\theta }}_m^l\right)$, $\left({\widehat{p}}_{0,x}^f, {\widehat{p}}_{0,y}^f, {\widehat{\theta }}_0^f\right)$…, $\left({\widehat{p}}_{m,x}^f, {\widehat{p}}_{m,y}^f, {\widehat{\theta }}_m^f\right)$.

The modeling method for the error function in the factor diagram mentioned above is called Method 1, which optimizes the heading angles of both the master and slave AUVs simultaneously. This is suitable for situations where both AUVs use low-precision IMUs.

The aforementioned factor graph error function modeling approach is referred to as Method 1. It simultaneously optimizes the heading angles of both the master and slave AUVs, which is suitable for scenarios where both AUVs are equipped with low-precision IMUs. During the factor graph fusion process, this method essentially performs a weighted average between the leader AUV’s attitude angle measurements and the slave AUV’s attitude angle measurements, which are augmented with underwater acoustic range and angle measurements. However, in cases where the leader AUV is equipped with a high-precision IMU, this fusion approach may in fact compromise the accuracy of its attitude estimates.

Therefore, Method 2 is adopted to model the motion measurement errors of the leader AUV. The error function $f_i^l(x)$ for its position change measurements can be expressed as:

$$f_i^l(x)={‖{(p_{i,x}^l+{\tilde{v}}_{i,x}^{l}dt\cos {\tilde{\theta }}_i^l-{\tilde{v}}_{i,y}^{l}dt\sin {\tilde{\theta }}_i^l-p_{i,x+1}^l)}^2+{(p_{i,y}^l+{\tilde{v}}_{i,x}^{l}dt\sin {\tilde{\theta }}_i^l+{\tilde{v}}_{i,y}^{l}dt\cos {\tilde{\theta }}_i^l-p_{i,y+1}^l)}^2‖}_{{\sigma }_p^l}^2$$

(17)

Compared to Equation (13), the error function of the leader AUV motion model only includes the measurement error of position, without considering the measurement error of attitude. The nonlinear least squares optimization objective of factor graph for Method 2 is as follows:

$$\begin{array}{l}{X}^{MAP}=\underset{X}{\arg \min }F(x)=\underset{X}{\arg \min }{\displaystyle \sum _i{f}_i(x_i)}\\ \left\{\begin{array}{l}{f}_i^l(x_i)={‖{(p_{i,x}^l+{\tilde{v}}_{i,x}^{l}dt\cos {\tilde{\theta }}_i^l-{\tilde{v}}_{i,y}^{l}dt\sin {\tilde{\theta }}_i^l-p_{i,x+1}^l)}^2+{(p_{i,y}^l+{\tilde{v}}_{i,x}^{l}dt\sin {\tilde{\theta }}_i^l+{\tilde{v}}_{i,y}^{l}dt\cos {\tilde{\theta }}_i^l-p_{i,y+1}^l)}^2‖}_{{\sigma }_p^l}^2\\ f_i^f(x_i)={‖Log\left({\left(Z_{i,i+1}^f\right)}^{-1}{\left(M_i^f\right)}^{-1}{M}_{i+1}^f\right)‖}_{{\sigma }_p^f}^2\\ f_{i,L}(x_i)={‖{\tilde{L}}_i-\sqrt{{(p_{i,x}^l-p_{i,x}^f)}^2+{(p_{i,y}^l-p_{i,y}^f)}^2}‖}_{{\sigma }_L}^2\\ f_{i,\phi }(x_i)={‖{\tilde{\theta }}_i^f+{\tilde{\phi }}_i-\arctan {\displaystyle \frac{p_{i,x}^l-p_{i,x}^f}{p_{i,f}^l-p_{i,f}^f}}‖}_{{\sigma }_{\phi }}^2\end{array}\right.\end{array}$$

(18)

The factor graph serves as a transparent and flexible framework for probabilistic modeling, representing the joint probability distribution of all AUV states given the measurements. The innovation of this work lies in the synergy between this modeling framework and the Lie group representation. While the factor graph elegantly encapsulates the system’s probabilistic constraints, the use of the SE(2) Lie group provides the mathematical foundation for the state representation and the subsequent optimization on the manifold. This combination allows for a principled and accurate formulation of the cooperative navigation problem.

4.4. Method of Solving Factor Graph

The Gauss-Newton method is used to solve the nonlinear least squares problem. The key step is to linearize the error functions on the manifold. we define a perturbation $\Delta X$ in the vector space. The error function is approximated as:

$$f_i(X+\Delta X)\approx f_i(X)+J_i\Delta X$$

(19)

where $J_i$ is the Jacobian matrix of the error function $f_i(X)$ with respect to $X$. Substituting it into the least squares optimization objective of (13) (15):

$$\begin{array}{l}F(X+\Delta X)\approx {\displaystyle \sum _i{(f_i(X)+J_i\Delta X)}^T(f_i(X)+J_i\Delta X)}\\ ={\displaystyle \sum _i{f}_i^T(X)f_i(X)}+2{\displaystyle \sum _i\Delta X^T{J}_i^T{f}_i(X)}+{\displaystyle \sum _i\Delta X^T{J}_i^T{J}_i\Delta X}\end{array}$$

(20)

By making the first-order derivative of $F(X+\Delta X)$ is equal to 0, we can obtain:

$${\displaystyle \sum _i{J}_i^T{J}_i\Delta X}=-{\displaystyle \sum _i{J}_i^T{f}_i}$$

(21)

By iteratively solving the above equation to obtain $\Delta X$, the optimal estimate of $X$ can be obtained. The flowchart of the Gauss Newton method is shown in Figure 8.

The Lie group formulation inherently facilitates the computation of Jacobian matrices by providing a structured manifold framework. To demonstrate this advantage, the range error function $f_{i,L}(x_i)$, excluding weighting factors, is analyzed as a representative case for Jacobian derivation

$$\begin{array}{rl}{f}_{i,L}(x_i)& ={‖{\tilde{L}}_i-\sqrt{{(p_{i,x}^l-p_{i,x}^f)}^2+{(p_{i,y}^l-p_{i,y}^f)}^2}‖}_2^2\\ & ={\tilde{L}}_i-{‖M_i^l·p_0-M_i^f·p_0‖}_2\\ {\displaystyle \frac{\partial f_{i,L}(x_i)}{\partial x_i^l}}& ={\displaystyle \frac{M_i^l·p_0-M_i^f·p_0}{{‖M_i^l·p_0-M_i^f·p_0‖}_2}}[R_i^l 0]\\ {\displaystyle \frac{\partial f_{i,L}(x_i)}{\partial x_i^f}}& =-{\displaystyle \frac{M_i^l·p_0-M_i^f·p_0}{{‖M_i^l·p_0-M_i^f·p_0‖}_2}}[R_i^f 0]\end{array}$$

(22)

In practical applications of solving factor graphs for cooperative navigation, a key distinction of our approach lies in the solver selection. Unlike prior factor graph methods for AUVs that often rely on the Sum-Product Algorithm [17,18,19], the core innovation of this work is the adoption of the Gauss-Newton method as the backend solver on the Lie group manifold. This paradigm shift from a probabilistic inference algorithm to a nonlinear optimization technique is crucial. Operating directly on the SE(2) manifold, the Gauss-Newton method provides higher linearization accuracy and better estimation consistency by avoiding the approximations often required by the Sum-Product Algorithm, especially in graphs with cycles.

To ensure the practical feasibility of this optimization-based approach, a sliding window strategy is employed. Only a limited number of recent states are maintained for optimization, as the influence of earlier historical states on the current state is negligible. This approach effectively controls computational complexity by focusing on the most relevant states. For a sliding window of size W time steps, the number of state variables optimized in each iteration remains constant at 2 × W × 3 (for two AUVs in SE(2)), preventing unbounded growth in computational cost over time. For example, with a window size of W = 30, the number of optimized variables is 180.

We analyzed the computational efficiency to demonstrate the real-time capability of the proposed algorithm. The simulations were conducted on a standard desktop CPU. The average time required to solve the nonlinear optimization problem per update was empirically measured to be below 50 ms. Given that the acoustic ranging and communication updates typically occur at a 1 s interval in AUV applications, the computational demand of our algorithm is well within the available time budget, confirming its potential for real-time operation. It is important to note that the EKF, due to its recursive nature, has a lower and constant computational cost per update. The significant gain in estimation accuracy (29–38% reduction in RMSE, as shown in Section 5.2 and Section 5.3) achieved by our method justifies this acceptable increase in computational load. The scalability of the approach for larger teams of AUVs will be a focus of our future work.

4.5. AUV Cooperative Navigation Simulation System

It is often difficult to obtain accurate ground truth poses of AUVs in underwater navigation experiments, so precise simulation experiments can be used to compare and verify the above algorithms. According to reference [31], the dynamic equation of AUV is as Equation (23), and the meanings represented by each variable are shown in

Table 1. Description of symbols in simulation system.

Description	Symbol
Linear velocity in body frame	$\dot{u},\dot{v},\dot{w}$
Angular velocity in body frame	$\dot{p},\dot{q},\dot{r}$
Restoring force and moments	$X_{HS},Y_{HS},Z_{HS},K_{HS},M_{HS},N_{HS}$
Added mass coefficient	$X_{\dot{u}},Y_{\dot{v}},Z_{\dot{w}},K_{\dot{p}},M_{\dot{q}},N_{\dot{r}}$
Linear damping coefficient	$Y_{uv},Y_{ur},Z_{uw},Z_{uq},M_{uw},N_{uv},N_{ur}$
Quadratic damping coefficient	$X_{uu},Y_{vv},Z_{ww},K_{pp},M_{qq},N_{rr}$
Cross-flow damping coefficient	$\begin{array}{l}{X}_{wq},X_{vr},X_{qq},X_{rr},Y_{wp},Y_{pq},\\ Z_{vp},Z_{rp},M_{rp},M_{vp},N_{pq},N_{wp}\end{array}$
Control coefficients	$Y_{uudr},N_{uudr},Z_{uuds},M_{uuds}$
Propulsion Coefficients	$X_{prop},K_{prop}$

.

$$\begin{array}{l}\left[\begin{array}{cccccc}(m-X_{\dot{u}})& 0& 0& 0& m{z}_g& -m{y}_g\\ 0& (m-Y_{\dot{v}})& 0& -m{z}_g& 0& -(m{x}_g-Y_{\dot{r}})\\ 0& 0& (m-Z_{\dot{w}})& m{y}_g& -(m{x}_g+Z_{\dot{q}})& 0\\ 0& -m{z}_g& m{y}_g& (I_{xx}-K_{\dot{p}})& 0& 0\\ m{z}_g& 0& -(m{x}_g+M_{\dot{w}})& 0& (I_{yy}-M_{\dot{q}})& 0\\ -m{y}_g& (m{x}_g-N_{\dot{v}})& 0& 0& 0& (I_{zz}-N_{\dot{r}})\end{array}\right]\left[\begin{array}{l}\dot{u}\\ \dot{v}\\ \dot{w}\\ \dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]=\\ \left[\begin{array}{l}{X}_{HS}+X_{u\left|u\right|}u\left|u\right|+(X_{wq}-m)wq+(X_{qq}+x_{g}m)q^2+(X_{vr}+m)vr+(X_{rr}+x_{g}m)r^2-m{y}_{g}pq-m{z}_{g}pr+X_{prop}\\ Y_{HS}+Y_{v\left|v\right|}v\left|v\right|+Y_{r\left|r\right|}r\left|r\right|+m{y}_g{r}^2+(Y_{ur}-m)ur+(Y_{wp}+m)wp+(Y_{pq}-m{x}_g)pq+Y_{uv}uv+m{y}_g{p}^2-m{z}_{g}qr+Y_{uu{\delta }_r}{u}^2{\delta }_r\\ Z_{HS}+Z_{w\left|w\right|}w\left|w\right|+Z_{q\left|q\right|}q\left|q\right|+(Z_{uq}+m)uq+(Z_{vp}-m)vp+(Z_{rp}-m{x}_g)rp+Z_{uw}uw+m{z}_g(p^2+q^2)-m{y}_{g}rq+Z_{uu{\delta }_s}{u}^2{\delta }_s\\ K_{HS}+K_{p\left|p\right|}p\left|p\right|-(I_{zz}-I_{yy})qr+m(uq-vp)-m{z}_g(wp-ur)+K_{prop}\\ M_{HS}+M_{w\left|w\right|}w\left|w\right|+M_{q\left|q\right|}q\left|q\right|+(M_{uq}-m{x}_g)uq+(M_{vp}+m{x}_g)vp+(M_{rp}-(I_{xx}-I_{zz}))rp+m{z}_g(vr-wq)+M_{uw}uw+M_{uw}uw+M_{uu{\delta }_s}{u}^2{\delta }_s\\ N_{HS}+N_{v\left|v\right|}v\left|v\right|+N_{r\left|r\right|}r\left|r\right|+(N_{ur}-m{x}_g)ur+(N_{wp}+m{x}_g)wp+(N_{pq}-(I_{yy}-I_{xx}))pq-m{y}_g(vr-wq)+N_{uv}uv+N_{uu{\delta }_r}{u}^2{\delta }_r\end{array}\right]\end{array}$$

(23)

By iteratively calculating Equation (23) using the Euler method, the linear velocity and angular velocity of the AUV at each moment can be obtained, as well as the true position and angle.

AUVs can acquire simulated measurements such as acceleration, angular velocity, velocity, and inter-AUV distance and angle, corresponding to IMU, DVL, and sonar sensors for ranging and bearing. The $\dot{u},\dot{v},\dot{w}$ of Equation (23) refer to the acceleration $a_b$ of the AUV in the body coordinate system, while the measurement of IMU is the acceleration in the navigation coordinate system. The relationship between the velocity of the body coordinate system and the navigation coordinate system is:

$$v_n=R{v}_b$$

(24)

where $v_n$ is the velocity of navigation coordinate system, and $v_b$ is velocity of the body coordinate system. By taking the derivative, the acceleration of AUV in the navigation coordinate system can be obtained:

$$a_n=R{\dot{v}}_b+\dot{R}{v}_b=R{a}_b+\omega \times v_b$$

(25)

where $a_n$ is the velocity of navigation coordinate system, and $\omega$ is angular velocity of AUV. By adding Gaussian noise to the above measurements, a parameter level CN simulation system can be constructed.

5. Experiments and Results

5.1. Experimental Setup

The relative deviation between the planned trajectories of the leader AUV and the follower AUV is set to 10 m. The controller receives the real motion data of the AUVs to ensure they follow the planned trajectories. During this process, Gaussian noise is added to the true values of the AUV’s acceleration, angular velocity, linear velocity, relative distance, and relative angle to simulate the corresponding measured values.

Two simulation conditions were set up for the AUV cluster structure, corresponding to parallel and leader-slave configurations. In the first experiment, a parallel configuration is adopted: both the leader and follower AUVs are equipped with the same low-precision IMU1. In the second experiment, a leader-slave configuration is used: the leader AUV is equipped with a high-precision IMU2, while the follower AUV uses the low-precision IMU1. The relevant equipment parameters are provided in

Table 2. Equipment parameters.

Equipment	Parameter	Value
IMU1	Constant Bias of Gyroscope	$10(°)/\mathrm{h}$
	Angular Random Walk	$0.35(°{)/\mathrm{h}}^{1/2}$
	Constant Bias of Accelerometer	$15\times {10}^{-6} \mathrm{g}$
	Velocity Random Walk	$60\times {10}^{-6} \mathrm{g}$
	Update Frequency	$100 \mathrm{Hz}$
IMU2	Constant Bias of Gyroscope	$5(°)/\mathrm{h}$
	Angular Random Walk	$0.03(°{)/\mathrm{h}}^{1/2}$
	Constant Bias of Accelerometer	$10\times {10}^{-6} \mathrm{g}$
	Velocity Random Walk	$30\times {10}^{-6} \mathrm{g}$
	Update Frequency	$100 \mathrm{Hz}$
DVL	Velocity Measurement Noise	$0.1 \mathrm{m}/\mathrm{s}$
	Update Frequency	$10 \mathrm{Hz}$
Sonar	Range Measurement Noise	$3 \mathrm{m}$
	Bearing Measurement Noise	$2.55°$
	Update Frequency	$1 \mathrm{Hz}$

below.

5.2. Simulation Result of Parallel Configuration

Under the first set of simulation conditions, the trajectory of the AUV was estimated using dead reckoning (Dr), extended Kalman filtering (EKF) [32], Method 1, and Method 2, as shown in Figure 9.

It can be observed that due to the relatively low accuracy of the IMU and the absence of sonar-based ranging and directional information, the trajectories of the two AUVs under Dr increasingly deviate from the true values, with significant heading angle errors, posing challenges in practical applications. The trajectories of the leader and follower AUVs estimated by the Kalman filter generally follow the true trajectory and remain relatively parallel. However, since the Kalman filter relies solely on single-instance ranging and directional measurements with substantial error noise, the estimated trajectory exhibits considerable oscillations.

In contrast, the trajectories estimated by Method 1 also follow the true trajectory and maintain mutual parallelism, with the leader’s trajectory showing smaller oscillations compared to the Kalman filter. Method 2, on the other hand, produces a leader trajectory consistent with Dr. While it partially mitigates oscillations in the follower’s estimated trajectory, the leader’s heading deviation results in an overall bias in the trajectories.

From Figure 10 and

Table 3. Error analysis (RMSE: Root Mean Square Error; MAXE: Maximum Absolute Error).

The Algorithms	Description	Position Error of Leader AUV (m)	Orientation Error of Leader AUV (Rad)	Position Error of Slave AUV (m)	Orientation Error of Slave AUV (Rad)
Dr	RMSE	6.13	0.05	29.05	0.27
	MAXE	15.98	0.12	69.69	0.49
EKF	RMSE	3.51	0.07	3.37	0.06
	MAXE	9.94	0.22	8.46	0.18
Method 1	RMSE	2.67	0.04	2.86	0.04
	MAXE	4.16	0.15	4.64	0.16
Method 2	RMSE	6.31	0.05	6.40	0.07
	MAXE	16.32	0.12	16.15	0.20

, it can be seen that in the case of using two low-precision IMUs in a distributed cluster, Method 1, with the minimum RMSE, should be used for parallel configuration. It can be observed that in terms of the position of the leader AUV, the orientation of the leader AUV, the position of the follower AUV, and the orientation of the follower AUV, the error RMSE of Method 1 is reduced by 24%, 43%, 16%, and 33%, respectively, compared to the EKF, with an average reduction of 29%.

5.3. Simulation Result of Leader-Slave Configuration

Under the second set of simulation conditions, the trajectory of the AUV was estimated using Dr, EKF, Method 1, and Method 2, as shown in Figure 11.

It can be observed that due to the high precision of the leader AUV’s IMU, the trajectory estimated by Dr remains very close to the true value throughout. However, the trajectory of the follower AUV based solely on Dr deviates significantly from the true trajectory. Noises in ranging, bearing, and the follower’s IMU affect the estimated trajectory of the leader, causing both the leader’s and the followers’ estimated paths to deviate from the true values.

In Method 2, the trajectory of the leader AUV is estimated entirely using the attitude angles provided by the high-precision IMU. The ranging and bearing noises do not degrade its heading accuracy. As a result, the leader’s trajectory almost coincides with that of the Dr leader, and the trajectory of the follower AUV is also significantly corrected. As demonstrated in Figure 12 and

Table 4. Error Analysis.

The Algorithms	Description	Position Error of Leader AUV (m)	Orientation Error of Leader AUV (Rad)	Position Error of Slave AUV (m)	Orientation Error of Slave AUV (Rad)
Dr	RMSE	4.9	0.02	15.33	0.15
	MAXE	8.38	0.03	35.60	0.24
EKF	RMSE	7.19	0.08	6.92	0.07
	MAXE	14.51	0.19	14.27	0.23
Method 1	RMSE	4.11	0.04	3.76	0.07
	MAXE	10.48	0.11	10.56	0.21
Method 2	RMSE	4.97	0.02	4.73	0.06
	MAXE	8.53	0.03	8.69	0.21

, the pose errors of the leader AUV in Method 2 are consistent with those of Dr and are smaller than those of the EKF and Method 1. Meanwhile, the pose errors of the follower AUV are noticeably smaller than those of the previous two methods.

It can be observed that in terms of the position of the leader AUV, the orientation of the leader AUV, the position of the follower AUV, and the orientation of the follower AUV, the error RMSE of Method 2 is reduced by 31%, 75%, 32%, and 14%, respectively, compared to the EKF, with an average reduction of 38%.

As stated in Section 4.5, the average optimization time per update was consistently below 50 ms in our simulations. This timing data solidly supports the real-time capability of the proposed method, as it is an order of magnitude faster than the 1 s cycle of acoustic measurements.

6. Conclusions

This paper addressed the cooperative navigation problem for multi-AUV systems under different configurations by proposing a novel optimization algorithm based on factor graph theory and Lie group theory.

The main contributions and conclusions of this study are summarized as follows:

Novel Methodology: This work is the first to utilize the two-dimensional Special Euclidean Group SE(2) to describe the motion state of AUVs. Based on this representation, error functions for both the motion model and the acoustic ranging/bearing model were formulated, effectively transforming the cooperative navigation problem into a nonlinear least-squares optimization problem on the Lie group.

Dual Optimization Strategies: Two distinct optimization methods (Method 1 and Method 2) were proposed to cater to different AUV configurations. Method 1 is designed for parallel configurations, simultaneously optimizing the poses of both the leader and follower AUVs. Method 2 is tailored for leader–follower configurations; it fully trusts the leader’s high-precision attitude sensor, optimizing only its position, thereby preventing the contamination of the leader’s attitude estimate by acoustic measurement noise.

Superior Performance: The effectiveness of the proposed algorithms was validated through parameter-level simulation experiments. The results demonstrate that in parallel configurations, Method 1 reduces the average RMSE of position and orientation errors by 29% compared to the traditional Extended Kalman Filter (EKF). In leader–follower configurations, Method 2 performs even better, achieving a significant 38% average reduction in RMSE, which highlights a substantial improvement in navigation accuracy.

The proposed algorithm is not dependent on a specific AUV formation configuration. Its flexible factor graph model effectively fuses observations from diverse sources, demonstrating strong generality and robustness.

In conclusion, the factor graph and Lie group-based cooperative navigation algorithm proposed in this paper provides an effective and versatile solution for the precise pose estimation of multi-AUV systems. Future work will focus on extending the algorithm to three-dimensional space (SE(3)) and further investigating its performance under more practical challenges, such as communication delays and packet loss. It is important to note that a direct comparative analysis with other state-of-the-art multi-AUV localization algorithms was not conducted. This is due to the current absence of standardized benchmark scenarios and the fact that advanced algorithms are typically validated under specific, non-uniform simulation setups or configurations that differ from this study. Future work will aim to address this gap by engaging in collaborative efforts to establish common benchmarks for the community.