Open AccessArticle

Consensus-Based Formation Control and Gyroscopic Obstacle Avoidance for Multiple Autonomous Underwater Vehicles on SE(3)

National Key Laboratory of Autonomous Marine Vehicle Technology, Harbin Engineering University, Harbin 150001, China

Southern Marine Science and Engineering Guangdong Laboratory (Zhuhai), Zhuhai 519000, China

School of Marine Engineering and Technology, Sun Yat-sen University, Zhuhai 519082, China

Author to whom correspondence should be addressed.

J. Mar. Sci. Eng. 2024, 12(12), 2350; https://doi.org/10.3390/jmse12122350

Submission received: 27 October 2024 / Revised: 16 December 2024 / Accepted: 19 December 2024 / Published: 21 December 2024

(This article belongs to the Section Ocean Engineering)

Download

Browse Figures

Figure 1
The inertial reference frame and body-fixed frame. "> Figure 2
Control block diagram. "> Figure 3
Movement trajectories of three AUVs during the simulation process. "> Figure 4
Relative distance between AUVs in the simulation. "> Figure 5
Translational speed of the AUVs in three directions. "> Figure 6
Thrust of the AUVs in three directions. "> Figure 7
Movement trajectories of the three AUVs during the simulation. "> Figure 8
Relative distance between the AUVs during the simulation. "> Figure 9
Translational speed of the AUVs in three directions. "> Figure 10
Thrust of the AUVs in three directions. "> Figure 11
Distance from the AUVs to the surface of obstacle 1 on the plane <math display="inline"><semantics> <mrow> <mi>Z</mi> <mo>=</mo> <mo>−</mo> <mn>5</mn> </mrow> </semantics></math>. "> Figure 12
Distance from the AUVs to the surface of obstacle 2 on the plane <math display="inline"><semantics> <mrow> <mi>Z</mi> <mo>=</mo> <mo>−</mo> <mn>5</mn> </mrow> </semantics></math>. "> Figure 13
Distance from the AUVs to the surface of obstacle 3 on the plane <math display="inline"><semantics> <mrow> <mi>Z</mi> <mo>=</mo> <mo>−</mo> <mn>5</mn> </mrow> </semantics></math>. "> Figure 14
Movement trajectories of the three AUVs when t = 10 s. "> Figure 15
Movement trajectories of the three AUVs when t = 19 s. "> Figure 16
Movement trajectories of the three AUVs when t = 34 s. "> Figure 17
Movement trajectories of the three AUVs when t = 50 s. "> Figure 18
Relative distance between the AUVs in the final simulation. "> Figure 19
Translational speed of the AUVs in three directions. "> Figure 20
Thrust of the AUVs in three directions. "> Figure 21
Distance from the AUVs to the surface of obstacle on the <math display="inline"><semantics> <mrow> <mi>Z</mi> <mo>=</mo> <mo>−</mo> <mn>5</mn> </mrow> </semantics></math> plane. ">

Versions Notes

Abstract

To address the control challenges posed by increasingly complex mission scenarios, this paper aims to develop an advanced formation control and obstacle avoidance strategy for autonomous underwater vehicles (AUVs) in SE(3). This study establishes a dynamic model for fully actuated AUVs and designs a consensus-based formation control strategy to achieve coordinated movement. Motivated by limitations of existing obstacle avoidance strategies such as local minima issues and mutual interference between formation members in high-density environments, this paper introduces a novel gyroscopic force-based obstacle avoidance method. The proposed approach leverages the principles of rotation and angular momentum conservation to enable effective obstacle avoidance while maintaining formation integrity. Simulation results demonstrate the effectiveness of the proposed methodology in achieving robust formation control and collision avoidance under challenging conditions.

Keywords:

multi-AUV; formation control; consensus control; gyroscopic force obstacle avoidance

1. Introduction

Autonomous underwater vehicle (AUVs) are vehicle that operate independently according to a specified mission, then completes the established mission in oceans, rivers, or other waterways. AUVs play a very important role in marine science research, exploration of marine resources, and water-related rescue. Compared with a single AUV, the coordinated control of multiple AUVs in formation has the advantages of improving safety and work efficiency [1,2,3,4,5,6]. In [1,5], the authors studied the problem of formation control under time delay, while [2,6] studied formation control using virtual leaders and [3] explored the distributed formation control of AUVs. Considering that AUVs may encounter obstacles or need to take emergency avoidance measures during the formation mission, it is necessary to add appropriate and effective obstacle avoidance strategies to the formation control of the AUVs.

The current formation control methods for multiple AUVs are mainly divided into virtual structure methods, leader–follower methods, and artificial potential field methods. The core idea of the virtual structure method is to model AUVs as a virtual structure or entity, where the virtual structure typically represents the desired formation shape, layout, or task configuration. This can include information such as the relative positions, distances, velocities, and angles between the AUVs in the formation. The AUVs are viewed as nodes or elements in the virtual structure. The shape and configuration of the virtual structure can be defined and adjusted based on task requirements.

In [7], the authors proposed an optimal formation control for multiple AUVs based on the virtual structure, evaluating the impact of the formation on cooperative positioning. They also provided a cost function for the optimal formation and discussed the best formation in two different scenarios. In [8], the authors proposed a virtual master–slave control strategy that uses the relative displacement and angle between the master and slave AUVs to generate and maintain the formation. They designed a robust controller for multiple AUVs based on the AUV dynamic model in a constant current disturbance environment, and proved its stability through the Lyapunov function.

The artificial potential field method simulates each AUV’s movement in a virtual potential field to achieve coordinated formation behavior. To establish smooth routes from multiple AUVs to escort positions while flexibly avoiding obstacles, Zhang et al. [9] proposed a cooperative underwater target estimation mechanism (CUTE) that includes a self-organizing map algorithm based on the trust function method, then applied it for task allocation and formation control based on the artificial potential field method. In [10], the authors designed a distributed leader control method combining consensus theory with the artificial potential field method for multi-AUV systems with a leader AUV. The artificial potential field method was used to control the multi-AUV system to form the required circular formation, while consensus theory and velocity communication topology were applied to control the formation to achieve consistency in speed and heading.

The leader–follower method designates one or more AUVs as leaders and the other vehicles as followers, with both types collaborating to complete the task. This method mimics the coordinated behaviors observed in many animal groups in nature, such as fish schools and bird flocks. For the coordination control problem of multiple AUVs under discrete information exchange communication topology, a leader–follower formation structure can be adopted in which the lead AUV sends state information to the followers and communication between the follower AUVs is conducted randomly. A coordination control protocol with and without time delay was proposed in [11], while [12] adopted a fusion control strategy based on a reallocation mechanism (RM) with virtual structure and leader–follower formation characteristics to improve the speed, stability, and accuracy of multi-AUV systems. Their combination of two path update methods utilized a self-organizing map algorithm to enhance the performance of multi-AUV systems.

The artificial potential field (APF) method has become a mainstream strategy in the recent literature on obstacle avoidance for formation control. This approach guides AUVs by constructing attractive and repulsive potential fields, allowing them to avoid obstacles while moving toward a target. Each vehicle in the formation generates a repulsive force based on its perceived obstacle information, while the collective target generates an attractive force. In [13], Hao et al. proposed a vector-based APF method to achieve obstacle avoidance for AUVs. Their method improves the calculation of resultant force direction using the spatial vector method, which increases the computational efficiency of the algorithm. Key path points are used as local targets to guide local obstacle avoidance under the vector APF approach. Simulation results demonstrated the effectiveness of this obstacle avoidance strategy in reducing the cost of avoidance maneuvers for AUVs. In response to the local minima and goal inaccessibility issues encountered when applying APF for underwater AUVs, ref. [14] introduced a distance correction factor in the repulsive potential function to address these problems. They also proposed a hexagonal guiding method to avoid local minima, resulting in an improved APF-based path planning method for AUVs.

The dynamic window approach (DWA) is a real-time obstacle avoidance algorithm primarily used for robotic path planning. It evaluates the feasible window of velocity and acceleration for the current position of a robot in order to select the optimal collision-free velocity combination. For AUV formations, DWA can be applied to individual AUVs for obstacle avoidance while maintaining relative positions with other vehicles. In [15], the authors implemented an improved DWA for autonomous AUV obstacle avoidance with static obstacles by incorporating Q-learning reinforcement learning. Q-learning was used to optimize the weights of DWA’s objective function, allowing for the selection of appropriate weights in different environments and improving DWA’s applicability. Compared to the original DWA method, the improved DWA combined with Q-learning was more effective and suitable in complex obstacle environments. In [16], the authors further adapted DWA for the second-order non-holonomic constraints of AUVs by modifying the algorithm to predict AUV trajectories using linear approximations, thereby reducing prediction errors and improving the accuracy of DWA in underwater AUV obstacle avoidance.

The basic concept of using a virtual structure method for obstacle avoidance in multi-AUV formations is similar to the method for individual AUVs. It involves constructing a virtual structure to represent obstacles in the environment, then planning the paths of multiple AUVs to avoid these obstacles while maintaining formation shape. Applying the virtual structure method for obstacle avoidance allows for effective avoidance of both obstacles and other AUVs, ensuring safe operation of the formation. In [17], a 3D formation control and obstacle avoidance method for multi-AUV formations was proposed on the basis of backstepping control and a biologically-inspired neural network model within a leader–follower formation control structure. The simulation results verified the method’s effectiveness. In [18], the authors provided a fuzzy logic control algorithm for uniform local path planning in underwater AUVs, then converted the multi-AUV obstacle avoidance behavior into path planning for virtual AUVs, designing a fuzzy control strategy to avoid obstacles. A simulation showed that the formation was flexible in terms of its ability to maintain or change shape as needed to avoid obstacles.

Given the limitations of current obstacle avoidance strategies, this paper introduces the concept of gyroscopic force for AUV obstacle avoidance. Gyroscopic force, defined as a force perpendicular to the direction of movement, was first proposed in [19,20]. Compared to other methods, gyroscopic force avoidance has advantages in avoiding local minima and simplifying computations. When applied to multi-AUV formations, gyroscopic force allows for quick obstacle responses without disrupting the formation’s overall structure. Unlike previous works that primarily focused on 2D environments, this paper introduces gyroscopic obstacle avoidance as a novel mechanism for 3D formation control. The SE(3) framework ensures geometric consistency, reducing errors during long-duration simulations.

With the application of Lie group elements in modeling spacecraft, researchers have paid more and more attention to Lie group modeling methods that can preserve structures for a long time and provide more accurate modeling of rigid bodies. To solve the problem of attitude stabilization for satellites in circular orbits, ref. [21] proposed a controller based on the global SO(3) and used the Lyapunov method to analyze the closed-loop characteristics. In [22], the authors proposed a feedback controller based on the framework of the spacecraft on SO(3) to deal with the problem of tracking the attitude and angular velocity of the spacecraft in the presence of gravity and disturbance torque. In [23], the authors proposed a method to solve the problem of forming strategies for multi-agent systems while taking into account the geometry of the state space configuration of the system. In [24], the authors studied the problem of robust formation control of multiple rigid bodies with kinematics and dynamics that evolve on the Lie group SE(3). Using a working example of multiple planar rigid body systems on SE(2), ref. [25] studied the global coordination stability problem of multi-agent systems. Existing research [26,27,28] has shown that modeling AUVs in SE(3) offers advantages such as global description and coordinate independence. The control algorithm used in this paper is classified as a consensus-based control strategy within the SE(3) framework. This method is particularly advantageous for multi-AUV systems, as it enables decentralized coordination, reduces communication overhead, and ensures robust formation maintenance under dynamic environments. Therefore, this paper is based on the fully actuated AUV model in SE(3) and incorporates a gyroscopic force-based obstacle avoidance strategy into the formation control of multiple AUVs, using consensus theory within the SE(3) framework. In addition, we demonstrate how this strategy allows AUVs to avoid various types of obstacles and quickly return to the task-specified formation.

The rest of this paper is structured as follows: Section 2 introduces some useful notation and fundamental knowledge along with the dynamic model of AUVs in SE(3); Section 3 presents the formation strategy of multiple AUVs based on consensus theory in the SE(3) framework; Section 4 explains how the gyroscopic force obstacle avoidance strategy is integrated into AUV formation control; Section 5 presents simulation results regarding obstacle avoidance of AUV formations in various scenarios using the gyroscopic force method; finally, Section 6 presents our conclusions.

2. Background and Model Description

2.1. Notations

The variables related to the theoretical and mathematical representations in this paper are defined in Table 1.

2.2. Dynamics Model

The configuration space of the AUV modeled as a rigid body is defined by the location of the center of mass and the attitude with respect to the the inertial frame. Here,

x \in R^{3}

represents the position vector of the origin of the vehicle body frame

B

with respect to the inertial frame

B

shown in the frame

I

. Let

R \in S O (3)

denote the direction (attitude), defined as the rotation matrix from the frame

B

to the frame

I

. Then, the attitude and position information of the entire AUV can be expressed as a matrix:

g = [\begin{matrix} R & x \\ 0 & 1 \end{matrix}] \in S E (3)

(1)

where

S E (3)

is the six-dimensional rigid body motion Lie group, which is obtained as the semi-direct product of

R^{3}

and

S O (3)

. We choose an inertial reference frame

{\vec{i}}_{1}, {\vec{i}}_{2}, {\vec{i}}_{3}

and a body-fixed frame

{\vec{b}}_{1}, {\vec{b}}_{2}, {\vec{b}}_{3}

. The origin of the body mount is located at the center of mass of the vehicle. The first and second shafts of the body mount are denoted

{\vec{b}}_{1}, {\vec{b}}_{2}

, as shown in Figure 1. The third body-fixed axis

{\vec{b}}_{3}

is perpendicular to the plane.

We assume that the geometric center of the AUV and its center of gravity are almost coincident, allowing us to ignore the torque generated by the resistance. The kinematic equations of the AUV are as follows:

\dot{x} = R v

(2)

\dot{R} = R \hat{Ω}

(3)

where

h a t m a p \hat{\cdot} : R^{3} \to S O (3)

is provided by

\hat{x} = [\begin{matrix} x_{1} \\ x_{2} \\ x_{3} \end{matrix}] = [\begin{matrix} 0 & - x_{3} & x_{2} \\ x_{3} & 0 & - x_{1} \\ - x_{2} & x_{1} & 0 \end{matrix}] \in S O (3) .

(4)

We assume that the AUV is immersed in the viscous fluid and that the origin of the body-fixed frame of the body is at the center of gravity. The total kinetic energy of the system is composed of rigid bodies and fluids, provided by

T = \frac{1}{2} {(\begin{matrix} v \\ Ω \end{matrix})}^{T} (\begin{matrix} m I + M_{f} & O \\ 0 & J_{b} + J_{f} \end{matrix}) (\begin{matrix} v \\ Ω \end{matrix}),

(5)

where I is a

3 \times 3

identity matrix,

M_{f}

is the added mass, and

J_{b}

and

J_{f}

are the body inertia matrix and added moment of inertia, respectively, (for more on hydrodynamics, see [29]).

The dynamic model enables precise trajectory tracking by ensuring that each AUV adjusts its motion based on its neighbors’ states; for instance, the consensus algorithm uses the relative positions and attitudes of AUVs to generate control inputs for maintaining formation integrity. The dynamics which evolve on TSE(3) are trivialized to SE(3)

\times se (3)

. We assume here that the submerged weight of the AUV is equal to the buoyancy force; then, we can obtain the dynamics equations of the AUV on SE(3)

\times se (3)

(for more details, see [26]):

M \dot{v} = M v \times Ω + D_{v} (v) v + u

(6)

J \dot{Ω} = J Ω \times Ω + M v \times v + D_{Ω} (Ω) Ω + τ

(7)

where

M = m I + M_{f}

J = J_{b} + J_{f}

. The above equation can be written in the following compact form:

I \dot{ξ} = {ad}_{ξ}^{*} I ξ + D_{ξ} (ξ) ξ + φ

(8)

where

φ = {[τ^{T} u^{T}]}^{T}

ξ = {[Ω^{T} v^{T}]}^{T}

and

I = [\begin{matrix} J & 0_{3 \times 3} \\ 0_{3 \times 3} & M \end{matrix}],

(9)

where

a d_{ξ}

is a linear adjoint operator on the Lie algebra associated with Lie group SE(3), with

{ad}_{ξ}^{*} = {(a d_{ξ})}^{T}

and

a d_{ξ} = [\begin{matrix} \hat{Ω} & 0_{3 \times 3} \\ \hat{v} & \hat{Ω} \end{matrix}] .

(10)

The dynamic model is built on the assumption of full actuation while leveraging the SE(3) framework to describe the position and attitude of the AUVs. Key equations such as translational and rotational dynamics are derived to capture the physical behavior of AUVs assuming an ideal underwater environment with negligible external disturbances.

3. Formation Control Design for AUVs

3.1. Formation Control Design Based on Consensus

Considering the consensus control of multiple AUVs, we design the control law

φ_{i}

of each AUV such that when

t \to \infty

1 \leq i \leq N, 1 \leq j \leq N, i \neq j

, we obtain

g_{i} \to g_{j}

{\hat{ξ}}_{j}^{B} (t) - A d_{{\tilde{g}}_{i j}^{- 1} (t)} {\hat{ξ}}_{i}^{B} = 0

Let

g_{i j} (t) = g_{i}^{- 1} (t) g_{j} (t)

represent the relative configuration of underwater vehicle i with respect to underwater vehicle j. When the formation task is achieved, the following condition must hold:

\begin{matrix} lim_{t \to \infty} g_{i j} (t) = {\bar{g}}_{i j}, \\ lim_{t \to \infty} {\hat{ξ}}_{j}^{B} (t) - A d_{g_{i j}^{- 1} (t)} {\hat{ξ}}_{i}^{B} (t) = 0 \end{matrix}

(11)

where

{\bar{g}}_{i j} \in S E (3)

denotes the desired relative configuration between the vehicles. This can be expressed as

{\bar{g}}_{i j} = {({\bar{g}}_{1 i})}^{- 1} ({\bar{g}}_{1 j})

. The equation signifies that the velocities of both vehicles are equal as the task progresses, ensuring that the multi-AUV system maintains a rigid formation while executing related tasks.

Given

{\bar{g}}_{i j} = I

, when the entire multi-AUV system reaches the formation of the given formation task, the relative configuration is

g_{i j} = I

{\hat{ξ}}_{j}^{B} (t) \to {\hat{ξ}}_{i}^{B} (t)

, that is, the multi-AUV system reaches consensus at this time. The relative configuration between AUVs follows the relationship shown below:

{\tilde{g}}_{i} (t) = g_{i} (t) {\bar{g}}_{i 1}, i = 1, 2, \dots N

(12)

from which we can derive

{\dot{\tilde{g}}}_{i} = {\dot{g}}_{i} {\bar{g}}_{i 1} = g_{i} {\hat{ξ}}_{i}^{B} {\bar{g}}_{i 1} = {\tilde{g}}_{i} A d_{g_{i 1}^{- 1}} {\hat{ξ}}_{i}^{B} .

(13)

Given the definition

{\tilde{ξ}}_{i}^{B} ≜ A d_{g_{i 1}^{- 1}} {\hat{ξ}}_{i}^{B}

, we obtain

{\dot{\tilde{g}}}_{i} = {\tilde{g}}_{i} {\tilde{ξ}}_{i}^{B}

. By finding the time derivative of

{\tilde{ξ}}_{i}^{B}

, we have

{\dot{\tilde{ξ}}}_{i}^{B} = {\bar{g}}^{- 1}_{i 1} {\dot{\hat{ξ}}}_{i}^{B} {\bar{g}}_{i 1}

. We define the relative configuration of the transformation system of AUV j relative to the transformation system of AUV i as

{\tilde{g}}_{i j} (t) ≜ {\tilde{g}}_{i}^{- 1} (t) {\tilde{g}}_{j} (t) (1 \leq i \leq N, 1 \leq j \leq N, i \neq j)

. Then, when the transformation system of the entire multi-AUV system reaches consensus, we have

{\tilde{g}}_{i j} (t) \to I

{\tilde{ξ}}_{i}^{B} (t) \to {\tilde{ξ}}_{j}^{B} (t)

Lemma 1.

The multi-AUV system achieves the desired formation if and only if the transformation system of the multi-AUV system reaches consensus.

Proof of Lemma 1.

Consensus of the transformation system of the multi-AUV system refers to

\begin{matrix} lim_{t \to \infty} {\tilde{g}}_{i j} \to I, \\ lim_{t \to \infty} {\tilde{ξ}}_{j}^{B} (t) - {\tilde{ξ}}_{i}^{B} (t) = 0 . \end{matrix}

(14)

It can be seen that at this time we have

\begin{matrix} lim_{t \to \infty} {(g_{i} (t) {\bar{g}}_{i 1})}^{- 1} (g_{j} (t) {\bar{g}}_{j 1}) \to I, \\ lim_{t \to \infty} {\bar{g}}_{i 1}^{- 1} g_{i}^{- 1} (t) g_{j} (t) {\bar{g}}_{j 1} \to I, \end{matrix}

(15)

and

lim_{t \to \infty} g_{i j} \to {\bar{g}}_{i 1} {\bar{g}}_{j 1}^{- 1} = {\bar{g}}_{i 1} {\bar{g}}_{j 1} = {\bar{g}}_{i j} .

(16)

For

{\tilde{ξ}}_{i}^{B}

and

{\tilde{ξ}}_{j}^{B}

, we can obtain

\begin{matrix} lim_{t \to \infty} A d_{g_{j 1}^{- 1} (t)} {\hat{ξ}}_{j}^{B} (t) - A d_{g_{i 1}^{- 1} (t)} {\hat{ξ}}_{i}^{B} (t) = 0, \\ lim_{t \to \infty} {\hat{ξ}}_{j}^{B} (t) - A d_{g_{j 1} (t)} A d_{g_{i 1}^{- 1} (t)} {\hat{ξ}}_{i}^{B} (t) = 0, \\ lim_{t \to \infty} {\hat{ξ}}_{j}^{B} (t) - A d_{g_{j i} (t)} {\hat{ξ}}_{i}^{B} (t) = 0 . \end{matrix}

(17)

Finally, we have

lim_{t \to \infty} {\hat{ξ}}_{j}^{B} (t) - A d_{g_{i j}^{- 1} (t)} {\hat{ξ}}_{i}^{B} (t) = 0 .

(18)

□

Thus, we can conclude that the consensus control problem for a multi-AUV formation can be transformed into a problem of designing control laws that ensure the transformation system achieves consensus. Indeed, there is just such a conversion relationship between the multi-AUV system and its transformation system:

{\dot{\tilde{ξ}}}_{i}^{B} = {\bar{g}}_{i 1}^{- 1} {\dot{\hat{ξ}}}_{i}^{B} {\bar{g}}_{i 1}

3.2. Consensus and Formation Control of Multiple AUVs

Considering the consensus control problem of the transformation system of multiple AUVs, the definition is provided as follows:

\begin{matrix} {\tilde{X}}_{i j} ≜ log ({\tilde{g}}_{i j}), \\ {\tilde{ξ}}_{i j}^{B} ≜ {\tilde{ξ}}_{j}^{B} - A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i}^{B} \end{matrix}

(19)

where

{\tilde{X}}_{i j}

is the logarithmic mapping of the relative configuration on

S E (3)

to the corresponding Lie algebra

se (3)

. According to this definition,

{\tilde{X}}_{i i} = {\tilde{X}}_{j j} = 0

; then, we have

\begin{matrix} {\tilde{X}}_{i j} = log ({\tilde{g}}_{i j}) = - log ({\tilde{g}}_{j i}) = - {\tilde{X}}_{j i}, \\ A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{X}}_{i j} = A d_{{\tilde{g}}_{i j}^{- 1}} log ({\tilde{g}}_{i j}) = log ({\tilde{g}}_{i j}) = {\tilde{X}}_{i j}, \end{matrix}

(20)

where

{\tilde{ξ}}_{i j}^{B}

can be understood as the speed difference between AUV j and AUV i in the body-fixed coordinate system of AUV j. Considering

{\tilde{g}}_{i j}

, the differential with respect to time is obtained as follows:

\begin{matrix} {\dot{\tilde{g}}}_{i j} = - {\tilde{g}}_{i}^{- 1} {\dot{\tilde{g}}}_{i} g_{i}^{- 1} {\tilde{g}}_{j} + {\tilde{g}}_{i}^{- 1} {\dot{\tilde{g}}}_{j} \\ = {\tilde{g}}_{i j} ({\tilde{ξ}}_{j}^{B} - A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i}^{B}) . \end{matrix}

(21)

By definition, we have

{\dot{\tilde{g}}}_{i j} = {\tilde{g}}_{i j} {\tilde{ξ}}_{i j}^{B},

(22)

and because

{\tilde{X}}_{i j} = log ({\tilde{g}}_{i j}) \in g

, we can obtain

{\dot{\tilde{X}}}_{i j} = B_{- {\tilde{X}}_{i j}} {\tilde{ξ}}_{i j}^{B} .

(23)

The above equation is the dynamical equation of the exponential mapping of the relative poses between each AUV in the transformation system of the multi-AUV system. It is also known that

{\tilde{X}}_{i j}

takes values in the Lie algebra

se (3)

, which is a linear space isomorphic to

R^{6}

. Therefore, the equation transforms the formation control problem on the Lie group

S E (3)

into a stabilization problem on the Lie algebra

se (3)

. We convert the body-fixed coordinate system to the inertial coordinate system; for

{\hat{ξ}}^{I}_{i}

in the inertial frame, we have

{\dot{\tilde{g}}}_{i j} = {\tilde{ξ}}_{i j}^{I} {\tilde{g}}_{i j}

. Now, we can provide the following lemma.

Lemma 2.

The following equation relationship holds:

\begin{matrix} {\tilde{ξ}}^{I}_{i k} = - {\tilde{ξ}}^{B}_{k i} \\ A d_{{\tilde{g}}_{i j}} {\tilde{ξ}}_{i j}^{B} = - {\tilde{ξ}}_{j i}^{B} \end{matrix}

(24)

and

\begin{matrix} \frac{d}{d t} (A d_{{\tilde{g}}_{i j}}) {\tilde{ξ}}_{k}^{B} = A d_{{\tilde{g}}_{i j}} [{\tilde{ξ}}_{j i}^{B}, {\tilde{ξ}}_{k}^{B}] \\ A d_{{\tilde{g}}_{i j}} [{\tilde{ξ}}_{k}^{B}, {\tilde{ξ}}_{i k}^{B}] = - [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}] \end{matrix}

(25)

from which we have

\frac{d}{d t} (A d_{{\tilde{g}}_{i j}^{- 1}}) {\tilde{ξ}}_{i}^{B} = [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{i j}^{B}] .

(26)

Then, differentiating

{\tilde{ξ}}_{i j}^{B}

with respect to time provides

{\dot{\tilde{ξ}}}_{i j}^{B} = {\dot{\tilde{ξ}}}_{j}^{B} - A d_{{\tilde{g}}_{i j}^{- 1}} {\dot{\tilde{ξ}}}_{i}^{B} - [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{i j}^{B}] .

(27)

It can be seen that in the dynamical equations of the transformation system of multiple AUVs, when we have

{\tilde{g}}_{i j} (t) \to I

, it follows that

{\tilde{ξ}}_{i j}^{B} (t) \to 0

, meaning that

{\tilde{ξ}}_{j}^{B} (t) \to {\tilde{ξ}}_{i}^{B} (t)

. Therefore, when the multi-AUV system is set to form a specific formation, i.e., when the transformation system of multiple AUVs reaches consensus, the state of this transformation system is as follows:

\begin{matrix} lim_{t \to \infty} {\tilde{X}}_{i j} (t) = 0, \\ lim_{t \to \infty} {\tilde{ξ}}_{i j}^{B} (t) = 0 . \end{matrix}

(28)

At this point, the formation control problem for the multi-AUV system becomes the problem of designing a control law that stabilizes the dynamic equations of the entire transformation system with respect to the origin. Based on this, we provide the following lemma.

Lemma 3.

For the system on SE(3), the following control law is provided:

u (g, {\hat{ξ}}^{B}) = - K_{p} log (g) - K_{d} {\hat{ξ}}^{B}

(29)

where

K_{p} = k_{p} I

and

K_{d} = k_{d} I

are positive-definite gain matrices. This control law stabilizes the local exponential state

g \to I \in S E (3)

Furthermore, we have the following relationships for

k_{p}

, the initial angular velocity

ω_{0}

in the rigid body coordinate system, and the initial attitude

R_{0} \in S O (3)

k_{p} > \frac{∥ ω_{0} ∥^{2}}{π^{2} - {∥ R_{0} ∥}^{2}} .

(30)

Therefore, for a given initial value of the system

g_{0}

with

t r (R_{0}) \neq - 1

, the above control law stabilizes the system’s exponential state g from

g_{0} \to I

. In the above equation,

∥ R ∥

denotes the distance between R and the identity element

I_{3 \times 3} \in S O (3)

, which is defined by the following norm:

∥ R ∥ = {〈log (R), log (R)〉}^{0.5}

(31)

where

〈 \cdot, \cdot 〉

denotes the inner product operation on the Lie algebra.

It is noted that the initial attitude

t r (R_{0}) \neq - 1

and the numerical choice for

k_{p}

in the lemma are taken so as to avoid ambiguity regarding the relative attitudes between underwater vehicles in the multi-AUV system being

π

- π

. In practical applications, when the relative attitude is

π

- π

during the evolution of the multi-AUV system over time, this can be determined from the attitudes and derivatives of the attitudes of the underwater vehicles at that time. At the initial moment when the task begins, it is necessary to first define the relative attitude as

π

- π

when in an ambiguous posture.

For the compact form of the continuous dynamic equations of the underwater vehicle provided in this section under the framework of SE(3), we have

I \dot{ξ} = {ad}_{ξ}^{*} I ξ + D_{ξ} (ξ) ξ + φ .

(32)

From the above, we have

{\tilde{ξ}}_{i}^{B} = A d_{g_{i 1}^{- 1}} {\hat{ξ}}_{i}^{B}

and

{\dot{\tilde{ξ}}}_{i}^{B} = A d_{g_{i 1}^{- 1}} {\dot{\hat{ξ}}}_{i}^{B}

; then, for the relative error dynamics equation, the relative error equation of the AUV about the ith node is as follows:

{\dot{\tilde{ξ}}}_{i}^{B} = A d_{g_{i 1}^{- 1}} {[I^{- 1} {(ad}_{ξ_{i O}}^{*} I ξ_{i O} + D_{ξ} (ξ) ξ_{i O} + {\tilde{φ}}_{i})]}^{\land}

(33)

where

ξ_{i O} = {(A d_{g_{i 1}} {\tilde{ξ}}_{i})}^{\lor}

Then, for the transformation system of the multi-AUV system, we can provide the following control law

{\tilde{φ}}_{i}

about consensus:

\begin{matrix} {\tilde{φ}}_{i} = I {(A d_{g_{i 1}} (\sum_{k = 1}^{N} {\bar{a}}_{i k} (k_{1} {\tilde{X}}_{i k} + k_{2} {\tilde{ξ}}_{i k}^{I} + [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}])))}^{\lor} - \\ D_{ξ} (ξ) ξ_{i O} - {ad}_{ξ_{i O}}^{*} I ξ_{i O}, i = 1, 2, \dots, N \end{matrix}

(34)

where

[{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}]

denotes the Lie bracket operation,

{\bar{a}}_{i k} = a_{i k} {(\sum_{j = 1}^{N} a_{i j})}^{- 1}

is the element of the normalized adjacency matrix, and

k_{1} > 0

and

k_{2} > 0

are control gain constant matrices.

Before analyzing the system, we present the following lemma.

Lemma 4.

Based on the relative speed definition

{\tilde{ξ}}_{i j}^{B} ≜ {\tilde{ξ}}_{j}^{B} - A d_{{\tilde{g}}_{i j}^{- 1}} {\hat{ξ}}_{i}^{B}

between AUVs in the multi-AUV system, the following relationship can be defined:

{\tilde{ξ}}_{i j}^{B} = A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i j}^{I}

(35)

{\tilde{ξ}}_{i j}^{B} = - A d_{{\tilde{g}}_{j i}} {\tilde{ξ}}_{j i}^{B}, i \neq j

(36)

\begin{matrix} A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i k}^{I} = A d_{{\tilde{g}}_{i j}^{- 1}} (- {\tilde{ξ}}_{k i}^{B}) \\ = A d_{{\tilde{g}}_{i j}^{- 1}} (- {\tilde{ξ}}_{i}^{B} + A d_{{\tilde{g}}_{k i}^{- 1}} {\tilde{ξ}}_{k}^{B}) \\ = A d_{{\tilde{g}}_{i j}^{- 1}} (- {\tilde{ξ}}_{i}^{B}) + A d_{{\tilde{g}}_{k j}^{- 1}} {\tilde{ξ}}_{k}^{B} \\ = {\tilde{ξ}}_{j}^{B} - A d_{{\tilde{g}}_{i j}^{- 1}} ({\tilde{ξ}}_{i}^{B}) - ({\tilde{ξ}}_{j}^{B} - A d_{{\tilde{g}}_{k j}^{- 1}} {\tilde{ξ}}_{k}^{B}) \\ = {\tilde{ξ}}_{i j}^{B} - {\tilde{ξ}}_{k j}^{B} \end{matrix}

(37)

A d_{\tilde{g}} [{\hat{ξ}}_{i}^{B}, {\hat{ξ}}_{j}^{B}] = [A d_{\tilde{g}} {\hat{ξ}}_{i}^{B}, A d_{\tilde{g}} {\hat{ξ}}_{j}^{B}]

(38)

A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{X}}_{k i} = {\tilde{X}}_{j i} + {\tilde{X}}_{k j} + \dots, k \neq i .

(39)

The higher-order terms are omitted after the second term of the above equation. These higher-order terms have the following form:

\begin{matrix} \frac{1}{2} [{\tilde{X}}_{j i}, {\tilde{X}}_{k j}] + \frac{1}{12} [{\tilde{X}}_{j i}, [{\tilde{X}}_{j i}, {\tilde{X}}_{k j}]] + \frac{1}{12} [{\tilde{X}}_{k j}, [{\tilde{X}}_{k j}, {\tilde{X}}_{j i}]] \\ - \frac{1}{24} [{\tilde{X}}_{j i}, [{\tilde{X}}_{k j}, [{\tilde{X}}_{j i}, {\tilde{X}}_{k j}]]] + \dots \end{matrix}

(40)

For the jth vehicle in the multi-AUV system, the corresponding control law is

\begin{matrix} {\tilde{φ}}_{j} = I {(A d_{g_{j 1}} (\sum_{k = 1}^{N} {\bar{a}}_{j k} (k_{1} {\tilde{X}}_{j k} + k_{2} {\tilde{ξ}}_{j k}^{I} + [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{k j}^{B}])))}^{\lor} - \\ D_{ξ} (ξ) ξ_{j O} - {ad}_{ξ_{j O}}^{*} I ξ_{j O}, i = 1, 2, \dots, N . \end{matrix}

(41)

Then, we substitute the control laws of the ith and jth vehicles in the system into the dynamic equations of the transformation system of the multi-AUV system, after which the system becomes

\{\begin{matrix} {\dot{\tilde{X}}}_{i j} = B_{- {\tilde{X}}_{i j}} {\tilde{ξ}}_{i j}^{B} \\ {\dot{\tilde{ξ}}}_{i j}^{B} = A d_{g_{j 1}^{- 1}} {[I^{- 1} (I {(A d_{g_{j 1}} (\sum_{k = 1}^{N} {\bar{a}}_{j k} (k_{1} {\tilde{X}}_{j k} + k_{2} {\tilde{ξ}}_{j k}^{I} + [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{k j}^{B}])))}^{\lor})]}^{\land} \\ - A d_{{\tilde{g}}_{i j}^{- 1}} (A d_{g_{i 1}^{- 1}} {[I^{- 1} (I {(A d_{g_{i 1}} (\sum_{k = 1}^{N} {\bar{a}}_{i k} (k_{1} {\tilde{X}}_{i k} + k_{2} {\tilde{ξ}}_{i k}^{I} + [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}])))}^{\lor})]}^{\land}) \\ - [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{i j}^{B}] \end{matrix}

(42)

where

i, j = 1, 2, \dots, N

. We can rearrange

{\dot{\tilde{ξ}}}_{i j}^{B}

in the above formula as follows:

\begin{matrix} {\dot{\tilde{ξ}}}_{i j}^{B} = k_{1} \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{X}}_{j k} - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{X}}_{i k}) + k_{2} \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{ξ}}_{j k}^{I} - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i k}^{I}) \\ + (\sum_{k = 1}^{N} ({\bar{a}}_{j k} [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{k j}^{B}] - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}])) - [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{i j}^{B}] . \end{matrix}

(43)

The formation control design relies on the dynamic model to compute control inputs that ensure the AUVs’ trajectories are coordinated. The SE(3) model allows for capturing both position and attitude, enabling accurate adjustments in both translational and rotational dynamics to achieve stable formation control.

Now, we can analyze the stability of the system. We simplify the above formula and discuss it in blocks. First, we have the following definitions:

\begin{matrix} A_{1} ≜ \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{X}}_{j k} - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{X}}_{i k}), \\ A_{2} ≜ \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{ξ}}_{j k}^{I} - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i k}^{I}), \\ A_{3} ≜ (\sum_{k = 1}^{N} ({\bar{a}}_{j k} [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{k j}^{B}] - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}])) - [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{i j}^{B}] . \end{matrix}

(44)

For

A_{1}

, from the previous equation

A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{X}}_{k i} = {\tilde{X}}_{j i} + {\tilde{X}}_{k j} + \dots, k \neq i

we can obtain

\begin{matrix} A_{1} = \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{X}}_{j k} - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{X}}_{i k}), \\ = \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{X}}_{j k} + {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{X}}_{k i}), \\ = \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{X}}_{j k} + {\bar{a}}_{i k} {\tilde{X}}_{j i} + {\bar{a}}_{i k} {\tilde{X}}_{k j} + \dots) . \end{matrix}

(45)

Using the relationship

\sum_{k = 1}^{N} {\bar{a}}_{j k} = 1

, the above formula can be changed to

\begin{matrix} A_{1} = \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{X}}_{j k} + {\bar{a}}_{i k} {\tilde{X}}_{j i} + {\bar{a}}_{i k} {\tilde{X}}_{k j} + \dots), \\ = \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{X}}_{j k} - {\bar{a}}_{i k} {\tilde{X}}_{i j} - {\bar{a}}_{i k} {\tilde{X}}_{j k} + \dots), \\ = - (1 + {\bar{a}}_{i j}) {\tilde{X}}_{i j} + \sum_{k = 1, k \neq i}^{N} ({\bar{a}}_{j k} - {\bar{a}}_{i k}) {\tilde{X}}_{j k} + \dots . \end{matrix}

(46)

For

A_{2}

, by substituting the previously provided equation

A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i k}^{I} = {\tilde{ξ}}_{i j}^{B} - {\tilde{ξ}}_{k j}^{B}

, we can obtain

A_{2} = \sum_{k = 1}^{N} ({\bar{a}}_{j k} {\tilde{ξ}}_{j k}^{I} - {\bar{a}}_{i k} {\tilde{ξ}}_{i j}^{B} + {\bar{a}}_{i k} {\tilde{ξ}}_{k j}^{B}) .

(47)

From the relationship

\sum_{k = 1}^{N} {\bar{a}}_{j k} = 1

{\tilde{ξ}}_{j i}^{I} = - {\tilde{ξ}}_{i j}^{B}

, the above equation can be changed to

\begin{matrix} A_{2} = - {\tilde{ξ}}_{i j}^{B} + {\bar{a}}_{j i} {\tilde{ξ}}_{j i}^{I} + \sum_{k = 1, k \neq i, j}^{N} ({\bar{a}}_{j k} {\tilde{ξ}}_{j k}^{I} + {\bar{a}}_{i k} {\tilde{ξ}}_{k j}^{B}), \\ = - (1 + {\bar{a}}_{j i}) {\tilde{ξ}}_{i j}^{B} + \sum_{k = 1, k \neq i, j}^{N} (({\bar{a}}_{i k} - {\bar{a}}_{j k}) {\tilde{ξ}}_{k j}^{B}) . \end{matrix}

(48)

For

A_{3}

, the previously provided equations

A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i k}^{I} = {\tilde{ξ}}_{i j}^{B} - {\tilde{ξ}}_{k j}^{B}

{\tilde{ξ}}_{j i}^{I} = - {\tilde{ξ}}_{i j}^{B}

\sum_{k = 1}^{N} {\bar{a}}_{j k} = 1

can be rearranged as follows:

\begin{matrix} A_{3} = (\sum_{k = 1}^{N} ({\bar{a}}_{j k} [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{k j}^{B}] - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}])) - [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{i j}^{B}] \\ = \sum_{k = 1}^{N} ({\bar{a}}_{j k} [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{k j}^{B}] - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}] - {\bar{a}}_{j k} [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{i j}^{B}]) \\ = \sum_{k = 1}^{N} ({\bar{a}}_{j k} [{\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{k j}^{B} - {\tilde{ξ}}_{i j}^{B}] - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}]) \\ = \sum_{k = 1}^{N} ({\bar{a}}_{j k} [{\tilde{ξ}}_{j}^{B}, - A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{i k}^{I}] - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}]) \\ = \sum_{k = 1}^{N} ({\bar{a}}_{j k} [{\tilde{ξ}}_{j}^{B}, A d_{{\tilde{g}}_{i j}^{- 1}} {\tilde{ξ}}_{k i}^{B}] - {\bar{a}}_{i k} A d_{{\tilde{g}}_{i j}^{- 1}} [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}]) . \end{matrix}

(49)

Then, from the relationship

A d_{\tilde{g}} [{\hat{ξ}}_{i}^{B}, {\hat{ξ}}_{j}^{B}] = [A d_{\tilde{g}} {\hat{ξ}}_{i}^{B}, A d_{\tilde{g}} {\hat{ξ}}_{j}^{B}]

, we can obtain

\begin{matrix} A_{3} = A d_{{\tilde{g}}_{i j}^{- 1}} \sum_{k = 1}^{N} ({\bar{a}}_{j k} [A d_{{\tilde{g}}_{i j}} {\tilde{ξ}}_{j}^{B}, {\tilde{ξ}}_{k i}^{B}] - {\bar{a}}_{i k} [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}]), \\ = A d_{{\tilde{g}}_{i j}^{- 1}} \sum_{k = 1}^{N} ([{\bar{a}}_{j k} A d_{{\tilde{g}}_{i j}} {\tilde{ξ}}_{j}^{B} - {\bar{a}}_{i k} {\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}]) . \end{matrix}

(50)

In the above equations, ⋯ in

A_{1}

represents the high-order terms of the logarithmic mapping of the relative configurations between AUVs, while

A_{3}

represents the high-order terms about the relative speed. Next, we discuss the impact of the value of

{\bar{a}}_{i j}

on the final formation of multiple AUVs, that is, the impact of the communication topology on the formation.

Theorem 1.

Consider a communication topology that is a complete graph for a multi-AUV system with N AUVs. Under the given control law, the multi-AUV system asymptotically reaches the formation assigned by the task.

Proof of Theorem 1.

The proof process is divided into two parts, first considering

N = 2

and then

N > 2

. When

N = 2

, the communication topology of the two AUVs takes the form of bidirectional communication, expressed as

{\bar{a}}_{12} = {\bar{a}}_{21}

. Regarding the previous analysis of

{\dot{\tilde{ξ}}}_{12}^{B}

, it can be seen that at this time we have

A_{1} = - 2 k_{1} {\tilde{X}}_{i j}

A_{2} = - 2 k_{2} {\tilde{ξ}}_{i j}^{B}

A_{3} = 0

. This means that in the transformation system of multiple AUVs, the relative configuration and relative velocity high-order terms between the AUVs at this time are 0, and the system can be written as

{\dot{\tilde{ξ}}}_{12}^{B} = - 2 k_{1} {\tilde{X}}_{i j} - 2 k_{2} {\tilde{ξ}}_{i j}^{B} .

(51)

This matches the form provided by the lemma, that is, when the initial relative posture of the two AUVs is an ambiguous one, with the relative posture provided by the task, the entire system converges globally and the two AUVs asymptotically reach formation.

When

N > 2

, the complete communication topology graph is expressed as

{\bar{a}}_{i j} = 1 / (N - 1), i, j = 1, 2, \dots, N, i \neq j

. Then, the analysis of

{\dot{\tilde{ξ}}}_{i j}^{B}

is as follows:

\begin{matrix} A_{1} = - (1 + {\bar{a}}_{i j}) {\tilde{X}}_{i j} + \sum_{k = 1, k \neq i}^{N} ({\bar{a}}_{j k} - {\bar{a}}_{i k}) {\tilde{X}}_{j k} + \dots \\ A_{2} = - (1 + {\bar{a}}_{j i}) {\tilde{ξ}}_{i j}^{B} + \sum_{k = 1, k \neq i, j}^{N} (({\bar{a}}_{i k} - {\bar{a}}_{j k}) {\tilde{ξ}}_{k j}^{B}) \\ A_{3} = A d_{{\tilde{g}}_{i j}^{- 1}} \sum_{k = 1}^{N} ([{\bar{a}}_{j k} A d_{{\tilde{g}}_{i j}} {\tilde{ξ}}_{j}^{B} - {\bar{a}}_{i k} {\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}]) . \end{matrix}

(52)

It can be seen that when multiple AUVs approach the given formation, the influence of the high-order terms in the system on the convergence of the system is lower than that of the linear terms, and the influence of the high-order terms can be ignored. At this time, the transformation system of the multiple AUVs becomes

{\dot{\tilde{ξ}}}_{i j}^{B} = - (1 + {\bar{a}}_{i j}) {\tilde{X}}_{i j} - (1 + {\bar{a}}_{j i}) {\tilde{ξ}}_{i j}^{B} .

(53)

□

This is consistent with the form provided by the lemma. At this time, multiple AUVs asymptotically reach the formation assigned by the task, which is non-global for

N > 2

. Although the results provided at this time are non-global, the simulation results after multiple initial values reach the form of asymptotic formation, which indicates that this control law has a wide range of convergence. When the communication topology is a tree diagram, its effectiveness is also proved by correlation. Because the simulation scenario in this article uses a fully communicated topology, the proof process is omitted here.

4. Formation Control Algorithm Design for AUVs with Gyroscopic Force-Based Obstacle Avoidance on SE(3)

4.1. Gyroscopic Force

In the process of avoiding obstacles during travel, most obstacle avoidance strategies of an overall formation system may encounter local minimum problems in certain environments, as well as mutual interference between formation members and obstacles in high-density or crowded environments. To address this situation, in this chapter we consider introducing gyroscopic force into the task of formation obstacle avoidance to achieve obstacle avoidance while retaining formation.

Obstacle avoidance using gyroscopic force is an obstacle avoidance method based on the principle of rotation and angular momentum conservation of objects. It achieves obstacle avoidance by controlling the attitude and forward direction of AUVs.

The gyroscopic force

F_{g}

is defined here as a type of force that does not do any work on an object. In the mathematical expression, if

F_{g} \cdot \dot{q} = 0

is satisfied, where

q = (x, y)

represents the position of the object and

\dot{q}

is the velocity vector of the object, then the force is called the gyroscopic force. Obviously, the magnitude of the object’s velocity will not change due to the gyroscopic force. The gyroscopic force can also be written in the following form:

F_{g} = S (q, \dot{q}) \dot{q}

(54)

where

S (q, \dot{q})

is a skew-symmetric matrix that satisfies

S = [\begin{matrix} 0 & - ω (q, \dot{q}) \\ ω (q, \dot{q}) & 0 \end{matrix}] .

(55)

From this, we can find

F_{g} \cdot \dot{q} = \dot{q} \cdot S (q, \dot{q}) \dot{q} = {\dot{q}}^{T} S (q, \dot{q}) \dot{q} = 0 .

(56)

In this subsection, we use an intelligent agent as a model and use the potential field force, dissipative force, and gyroscopic force to represent the force conditions of the intelligent agent. The potential field force and dissipative force are used to make the intelligent agent converge to the target point, while the gyroscopic force is responsible for obstacle avoidance. For simple representation, this subsection only considers a case with one obstacle on the plane. Because this algorithm uses local information near the intelligent agent, it is also applicable to scenarios with multiple obstacles.

Assume that there is an intelligent agent and an obstacle on the

x y

plane, and assume that the intelligent agent is a point mass. The purpose of the algorithm design is to drive the intelligent agent to the target point

q_{T} = (x_{T}, y_{T})

without colliding with the obstacle. Given that the safe monitoring range of the intelligent agent is a circle with a radius of

r_{s}

, if the obstacle enters the circular area of the safe monitoring range when the intelligent agent is driving, then the intelligent agent performs obstacle avoidance behavior.

The dynamic model of the intelligent agent is provided as follows:

\ddot{q} = u

(57)

where

u = (u_{x}, u_{y})

. u consists of the following four parts:

u = F_{p} + F_{d} + F_{g} + F_{e x}

(58)

where

F_{p}

is a potential force given to the agent by a potential energy function that takes its minimum value at the target point

q_{T}

F_{d}

is the dissipative force,

F_{g}

is the gyroscopic force, and

F_{e x}

is another control force. In general,

F_{e x}

is set to zero, but can be set to take some value in certain special cases requiring additional control, such as zero-speed collisions. We can rewrite this as follows:

\begin{matrix} F_{p} = - \nabla V (q) \\ F_{d} = - D (q, \dot{q}) \dot{q} \\ F_{g} = S (q, \dot{q}) \dot{q} \end{matrix}

(59)

where V represents the potential force, the matrix D is a symmetric and positive-definite matrix, and the matrix S is a skew-symmetric matrix. We select the potential force function and the dissipative force as

\begin{matrix} V (q) = \frac{1}{2} ∥ q - q_{T} ∥^{2}, \\ F_{d} = - 2 \dot{q} . \end{matrix}

(60)

Let

d (q) = (d_{x} (q), d_{y} (q))

be the vector from the agent’s position

q

to the closest point to the obstacle in the case where the obstacle is convex. Let

d (q) = ∥ d (q) ∥

be the distance between the vehicle and the obstacle. From the formula, we choose

ω

in the matrix S as

ω (q, \dot{q}) = \{\begin{matrix} \frac{π V_{max}}{d (q)}, [d (q) \leq r_{det}] \land [d (q) \cdot \dot{q} > 0] \land [det [d (q), \dot{q}] \geq 0] \\ - \frac{π V_{max}}{d (q)}, [d (q) \leq r_{det}] \land [d (q) \cdot \dot{q} > 0] \land [det [d (q), \dot{q}] < 0] \\ 0 . \end{matrix}

(61)

It can be seen that

V_{max} > 0

and is a constant, while the sign of

ω

and the rotation direction of the agent have the following relationship. When the agent detects an obstacle

(d (q) \leq r_{d e c})

within its detection range and drives towards the obstacle

d (q) \cdot \dot{q} > 0

, the agent is affected by the gyroscopic force. The agent only turns if this is the case. The gyroscopic force acts to rotate the velocity vector of the agent. The direction of rotation (i.e., the positive or negative sign of

ω (q, \dot{q}

) depends on the directions of the two vectors

d (q)

and

\dot{q}

, i.e.,

det [d (q), \dot{q}]

is positive or negative.

The energy function of the agent consists of kinetic energy and potential energy functions, expressed as

E (q, \dot{q}) = \frac{1}{2} ∥ \dot{q} ∥^{2} + V (q) .

(62)

Differentiating the energy function of the agent with respect to time, we have

\frac{d}{d t} E (q, \dot{q}) = \dot{q} \cdot F_{d} = - 2 ∥ \dot{q} ∥^{2} \leq 0 .

(63)

From this, it can be found that the energy function does not increase with time.

4.2. Obstacle Avoidance Strategy for Multiple Agents Using Gyroscopic Force

First, we consider the case of two agents.

For the obstacle avoidance strategy with two agents, we first consider the obstacle avoidance between two agents, with both agents on a plane and no other agents or obstacles. Each agent tries not to collide with the other agent in the process of approaching the target point. First, we assume that each agent has a finite volume and two regions surrounding it. The inner region is called the safe region, which completely contains the agent. Its radius is represented by

r_{s}

. The outer region is a detection region with a thickness of

r_{d}

. Each agent can detect another agent only when the other agent enters the detection region. Collisions between agents are represented based on the safe regions of each agent.

For agents numbered 1 and 2, the position of the ith agent is represented by

q^{(i)} = (x^{(i)}, y^{(i)}), i = 1, 2

, and the position of the target point of the ith agent is represented by

q_{D}^{(i)} = (x^{(i)}, y^{(i)})

. The dynamics of the ith agent are represented by

\begin{matrix} {\ddot{q}}^{(i)} = u^{(i)}, u^{(i)} = (u_{x}^{(i)}, u_{y}^{(i)}) . \end{matrix}

The control law u is composed of the potential field force, dissipative force, and gyroscopic force:

u = - \nabla V + F_{d} + F_{g}

For agent 1, we select the potential field function and dissipative force as follows:

\begin{matrix} V^{(1)} (q^{(1)}) = \frac{1}{2} ∥ q^{(1)} - q_{D}^{(1)} ∥^{2} \\ F_{D}^{(1)} = (q^{(1)}, {\dot{q}}^{(1)}) = - 2 {\dot{q}}^{(1)} . \end{matrix}

(64)

The distance between the safety ranges of the two agents is expressed as

d (q^{(1)}, q^{(2)})

. Then, we have

d (q^{(1)}, q^{(2)}) = ∥ q^{(1)} - q^{(2)} ∥ = r_{s}^{(1)} - r_{s}^{(2)} .

(65)

Given a mapping

ϖ (v, w)

from

R^{2} \times R^{2} \to [- π / 2, π / 2]

, defined as the signed angle from vector

v

to vector

w

when

v \cdot w \geq 0

and

∥ v ∥ \cdot ∥ w ∥ \neq 0

, the mapping evaluates to 0 for all other relations from vector

v

to vector

w

. For example,

ϖ ((1, 0), (0, 1)) = π / 2

Next, we provide a mapping

ϑ

from

R^{2} \times R^{2} \to R

, which is defined as

ϑ (q^{(1)}, q^{(2)}) \leq r_{d}^{(1)}

ϑ (q^{(1)}, q^{(2)}) = 1

and as

ϑ (q^{(1)}, q^{(2)}) = 0

in other cases. This mapping is used to determine whether agent 2 is within the detection range of agent 1.

We define

q^{(21)} = q^{(2)} - q^{(1)}

; then, the gyroscopic force of agent 1 is provided by

F_{g}^{1} = ϑ (q^{(1)}, q^{(2)}) [\begin{matrix} 0 & - ω (q^{(1)}, {\dot{q}}^{(1)}, q^{(2)}, {\dot{q}}^{(2)}) \\ ω (q^{(1)}, {\dot{q}}^{(1)}, q^{(2)}, {\dot{q}}^{(2)}) & 0 \end{matrix}] {\dot{q}}^{(1)}_{1},

(66)

where

ω (q^{(1)}, {\dot{q}}^{(1)}, q^{(2)}, {\dot{q}}^{(2)}) = f (q^{(1)}, {\dot{q}}^{(1)}, q^{(2)}, {\dot{q}}^{(2)}) \frac{π V_{max}}{d (q^{(1)}, q^{(2)})} .

(67)

Here,

V_{max}

is a positive constant and the value of the mapping

f (q^{(1)}, {\dot{q}}^{(1)}, q^{(2)}, {\dot{q}}^{(2)})

is determined by the following relative position and velocity points of the two agents:

Case 1: In the case of $[q^{(21)}, {\dot{q}}^{(1)} \geq 0] \land [q^{(21)}, {\dot{q}}^{(2)} \geq 0]$ , if there is a relationship between the two agents, the numerical relationship is $ϖ (q^{(21)}, {\dot{q}}^{(1)}) \geq ϖ (q^{(21)}, {\dot{q}}^{(2)})$ ; then, the value of the mapping f is determined to be 1. Under the condition of case 1, when the values of the two agents are in other relationships, the value of the mapping f is −1.
Case 2: In the case of $[q^{(21)}, {\dot{q}}^{(1)} \geq 0] \land [q^{(21)}, {\dot{q}}^{(2)} < 0]$ , if there is a relationship between the two agents, the numerical relationship is $ϖ (q^{(21)}, {\dot{q}}^{(1)}) \geq - ϖ (q^{(21)}, {\dot{q}}^{(2)})$ ; then, the value of the mapping f is determined to be 1. Under the condition of case 2, when the values of the two agents are in other relationships, the value of the mapping f is −1.
Case 3: In the case of $[q^{(21)}, {\dot{q}}^{(1)} < 0] \land [q^{(21)}, {\dot{q}}^{(2)} < 0]$ , if there is a relationship between the two agents, the relationship between is $ϖ (q^{(12)}, {\dot{q}}^{(1)}) > ϖ ({\dot{q}}^{(12)}, q^{(2)})$ ; then, the value of the mapping f is determined to be 1. Under the conditions of case 3, when the values of the two agents are in other relationships, the value of the mapping f is −1.
Case 4: In all other cases, the mapping f is 0.

The energy function of each agent is provided by the following equation:

E_{i} (q^{(i)}, {\dot{q}}^{(i)}) = \frac{1}{2} ∥ {\dot{q}}^{(i)} ∥^{2} + V_{i} (q^{(i)}), i = 1, 2 .

(68)

In this way, we can find that the energy of each agent is non-increasing over time.

The obstacle avoidance strategy between two agents can also be extended to multiple agents. For a multi-agent system with N agents, we consider agent 1 and define the distance between its safety range and the safety range of the ith agent as

d^{(1, i)}

i = 1, 2 \dots N - 1

. Then, the average position of the rest of the agents in the multi-agent system with respect to agent 1 is

q_{A} = \frac{1}{N - 1} \sum_{i = 1}^{N - 1} q_{i},

(69)

and the corresponding average speed is

{\dot{q}}_{A} = \frac{1}{N - 1} \sum_{i = 1}^{N - 1} {\dot{q}}_{i} .

(70)

The corresponding gyroscopic force is then

F_{A} = (- ω {\dot{y}}_{A}, ω {\dot{x}}_{A})

, where the value of

ω

ω = f \frac{π V_{max}}{min {d^{(i)} ∣ i = 1, \dots N - 1}} .

(71)

Afterwards, we can design the corresponding f based on the relationship between the two agents in the previous chapter. Take

q_{A}

as the equivalent positions of the remaining

N - 1

agents, and take

{\dot{q}}_{A}

as the equivalent speed; then, apply the algorithm process for agent 1 to the remaining agents (special consideration is required if there is overlap between the equivalent positions).

4.3. Consensus-Based Formation Control of AUVs on SE(3) with Gyroscopic Obstacle Avoidance

First, consider the intelligent agent model in three-dimensional Euclidean space. Taking into account the actual situation, if the model of obstacles in three-dimensional Euclidean space is represented as a sphere, the requirements for the intelligent agent are too high if only the obstacle avoidance behavior with the shortest path is taken, and this approach does not meet the scene requirements when actually performing tasks. In order to simplify the obstacle avoidance behavior and bring it more into line with the actual scene, this chapter classifies the obstacle avoidance behavior of an intelligent agent in three-dimensional Euclidean space. For the body-fixed coordinate system of the AUV, the obstacle avoidance behavior is performed in the plane formed by

{\vec{b}}_{1}

and

{\vec{b}}_{2}

represented in the body-fixed coordinate system or in the plane formed by

{\vec{b}}_{1}

and

{\vec{b}}_{3}

, that is, considering avoiding obstacles to the left or right in the direction of the intelligent agent’s forward movement or avoiding obstacles up or down in the direction of forward movement. Considering that the algorithms required for these two situations are basically the same, but are differentiated for agents with different structures in different scenarios, this section presents and demonstrates the algorithm based on the obstacle avoidance behavior of the plane formed by

{\vec{b}}_{1}

and

{\vec{b}}_{2}

in the body-fixed coordinate system. Because the gyroscopic force does not involve the force in the

{\vec{b}}_{3}

direction, the gyroscopic force in three-dimensional space is expressed as

F_{g}^{'} = S^{'} (q, \dot{q}) \dot{q},

(72)

where

S^{'} (q, \dot{q})

is the following skew-symmetric matrix:

S^{'} = [\begin{matrix} 0 & - ω (q, \dot{q}) & 0 \\ ω (q, \dot{q}) & 0 & 0 \\ 0 & 0 & 0 \end{matrix}] .

(73)

In formation control of AUVs, considering that the formation strategy is based on consensus, if one of the AUVs in the formation performs obstacle avoidance behavior, then the final coordinated configuration space of the entire formation cluster changes, and the other AUVs also need to perform obstacle avoidance actions according to the consensus strategy, which causes additional losses. To solve this problem, in this subsection we provide a triggering strategy: when an AUV in the formation system performs obstacle avoidance behavior, a virtual underwater unmanned vehicle with the same configuration space and speed as the other AUVs is generated to participate in the formation control based on consensus with the AUV that is taking the obstacle avoidance action. When that AUV performs obstacle avoidance behavior, the virtual AUV also performs obstacle avoidance actions. Thanks to the consistent formation strategy, when the obstacle avoidance behavior ends, the AUV performing obstacle avoidance returns to its previous formation and speed, allowing the corresponding formation task to be continued. For the dynamic model of a fully driven AUV under the SE(3) framework, the control strategy for N AUVs in a consensus-based formation is designed as shown in Equation (34). After adding the triggered gyroscopic force-based obstacle avoidance strategy to the control strategy, the control strategy is as follows:

\begin{matrix} {\tilde{φ}}_{i} = I {(A d_{g_{i 1}} (\sum_{k = 1}^{N} {\bar{a}}_{i k} (k_{1} {\tilde{X}}_{i k} + k_{2} {\tilde{ξ}}_{i k}^{I} + [{\tilde{ξ}}_{i}^{B}, {\tilde{ξ}}_{k i}^{B}]) + F_{g}^{(i)}))}^{\lor} \\ - D_{ξ} (ξ) ξ_{i O} - {ad}_{ξ_{i O}}^{*} I ξ_{i O}, i = 1, 2, \dots, N . \end{matrix}

(74)

As stated before, this chapter only considers obstacle avoidance in the

{\vec{b}}_{1}

and

{\vec{b}}_{2}

plane when applying gyroscopic force to obstacles in three-dimensional space. Therefore, the gyroscopic force in the

{\vec{b}}_{3}

direction is constant at 0.

F_{g}^{(i)}

in the above equation is expressed as

F_{g}^{(i)} = [\begin{matrix} 0_{4 \times 4} & F_{g}^{'} (i) \\ 0_{1 \times 3} & 0 \end{matrix}] .

(75)

When an obstacle is detected, the gyroscopic obstacle avoidance mechanism is triggered, which adjusts the AUV’s attitude and angular velocity to steer away from the obstacle. This method leverages the conservation of angular momentum, ensuring that the AUV maintains its forward velocity while altering its trajectory to avoid collisions.

Considering that the initial state of the AUVs is assigned by the formation control method, using the gyroscopic force of the AUVs at this time to ensure that they avoid each other will cause disturbances. Considering that the final formation of the AUVs assigned by the mission will normally ensure that they are at a safe distance, introducing the gyroscopic force into mutual avoidance between the AUVs is not considered at this time, and is only applied to the obstacle avoidance behavior encountered during AUV movement.

4.4. Control Block Diagram

The formation control of AUVs based on gyroscopic obstacle avoidance in SE(3) space is provided as Figure 2 in the form of a control block diagram.

In the control diagram, the inputs consist of g and

{\hat{ξ}}^{B}

for the AUV. The formation control module provides the desired control inputs for maintaining the formation. The obstacle detection module and trigger module determine whether to activate obstacle avoidance based on proximity. The obstacle avoidance module adds gyroscopic force-based adjustments to help avoid obstacles. Finally, the dynamics model simulates the AUV’s motion and updates its state. The outputs consist of the updated position, velocity, and attitude of the AUV.

5. Simulation Results

When an obstacle is detected, the AUV computes a temporary avoidance trajectory while maintaining alignment with the formation through the gyroscopic mechanism. After clearing of the obstacle, the AUV realigns with the formation using the consensus algorithm. The relevant parameters of the AUV were selected from [30]. The following vehicle parameters were selected:

\begin{matrix} I = [\begin{matrix} J & 0_{3 \times 3} \\ 0_{3 \times 3} & M \end{matrix}], \\ J = d i a g [2037.999, 13586.983, 13586.9966] kg \cdot m^{2}, \\ M = d i a g [5454.5324, 5454.485, 5454.3] kg, \end{matrix}

D (ξ) = - d i a g [0.011, 0, 0.016, 0, 0.1, 0.3] .

5.1. Formation Control and Obstacle Avoidance Under Multiple Obstacles

We consider a scenario with three AUVs where the communication topology between them is a complete communication topology. The initial configuration spaces of the three AUVs are as follows:

g_{1} (0) = [\begin{matrix} 1 & 0 & 0 & - 1 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1 & - 5 \\ 0 & 0 & 0 & 1 \end{matrix}] g_{2} (0) = [\begin{matrix} 1 & 0 & 0 & 1 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1 & - 5 \\ 0 & 0 & 0 & 1 \end{matrix}] g_{3} (0) = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & - 5 \\ 0 & 0 & 0 & 1 \end{matrix}]

while the initial speed of each AUV is

{\hat{ξ}}_{1}^{B} (0) = [\begin{matrix} 0 & 0 & 0 & 2 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{matrix}] {\hat{ξ}}_{2}^{B} (0) = [\begin{matrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{matrix}] {\hat{ξ}}_{3}^{B} (0) = [\begin{matrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{matrix}] .

Then, the relative configuration space of the final ideal formation provided by the formation task is

{\bar{g}}_{12} = [\begin{matrix} 1 & 0 & 0 & - 10 \\ 0 & 1 & 0 & - 10 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] {\bar{g}}_{13} = [\begin{matrix} 1 & 0 & 0 & - 10 \\ 0 & 1 & 0 & 10 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] .

Thus, when the AUVs move forward without avoiding obstacles, the formation design generated by the consensus-based control law is an isosceles triangle. The simulation scenario of formation control at this time is shown in Figure 3.

It can be seen that the three AUVs quickly converge to the isosceles triangle formation assigned by the mission. Figure 4 also shows that the distance between the AUVs quickly reaches the specified relative distance.

Figure 5 shows the speed of each AUV in three directions during this simulation. It can be seen that the three AUVs make a formation based on consensus, and that their speeds also reach consensus.

Figure 6 shows the propulsion of the AUVs in three directions during this simulation.

In the simulation, obstacles were modeled as stationary spherical regions with fixed radii and known positions. Each AUV was equipped with virtual proximity sensors to detect obstacles within a predefined range. The safe detection distance of the AUVs was

r_{d e c} = 1.5

, and the safe area of the underwater unmanned vehicle was

r_{s} = 0.3

. There were three spherical obstacles in the three-dimensional space of the simulation area: (5, 10, −3.8), (20, 11.5, −3.8), and (15, −9, −3.8), with radius

\sqrt{2}

, referred to as obstacles 1, 2, and 3, respectively. The following simulation results were obtained when the simulation time was 50 s.

Figure 7 shows the trajectories of the three AUVs during the simulation of this formation task. From the trajectories, it can be seen that after the three AUVs reach a formation based on consensus, AUVs 2 and 3 encounter obstacles during the driving process and successfully perform obstacle avoidance behavior. After the obstacle avoidance action ends, they quickly recover to the previous formation reached through the consensus strategy prior to the obstacle avoidance behavior, and continue with the formation task.

Figure 8 shows the distance between each AUV and the others in this simulation. In the first subfigure, showing the distance diagram from AUV 1 to the other two AUVs, it can be clearly seen that the team quickly reaches the relative position of the ideal formation after the simulation starts. After that, AUVs 2 and 3 perform obstacle avoidance actions, causing the relative distance to fluctuate slightly. Following the obstacle avoidance behavior, AUVs 2 and 3 immediately recover to the respective positions assigned by the task, maintaining good rigidity on the part of the entire formation.

Figure 9 shows the speed of each AUV in three directions during this simulation. It can be seen that when the three AUVs reach a formation based on consensus, their speeds also reach consensus. AUVs 2 and 3 quickly recover to their respective speeds reached through consensus after the obstacle avoidance behavior ends.

Figure 10 shows the propulsion of the AUVs in three directions during this simulation.

Figure 11 shows the distance between each AUV and the surface of obstacle 1 on the plane at

Z = - 5

during this simulation. It can be observed that the distance corresponding to AUV 2 fluctuates somewhat as it avoids the obstacle. AUV 3 starts its obstacle avoidance action after its corresponding distance to obstacle 1 becomes smaller than the detection range, and the corresponding distance fluctuates around the detection range during avoidance. After avoiding obstacle 1, AUV 3 encounters other obstacles, causing small fluctuations in its distance to obstacle 1.

Figure 12 shows the distance between each AUV and the surface of obstacle 2 during the simulation on the plane at

Z = - 5

. It can be seen that the distance between AUV 2 and obstacle 2 fluctuates during avoidance. A short while after the simulation begins, the distance between AUV 3 and obstacle 2 also fluctuates due to its own obstacle avoidance. When the corresponding distance becomes smaller than the detection range, AUV 3 initiates its avoidance action, with the corresponding distance fluctuating around the detection range. After the avoidance action is complete, AUV 3 returns to the previous formation.

Figure 13 shows the distance between each AUV and the surface of obstacle 3 during the simulation on the plane at

Z = - 5

. It can be observed that gyroscopic force avoidance is triggered when the corresponding distance between AUV 2 and obstacle 3 first becomes smaller than the detection range, causing the relative distance to fluctuate around the detection range value. After completing the avoidance action, AUV 2 returns to the normal formation. The fluctuation in the distance between AUV 3 and obstacle 3 is due to the avoidance maneuvers carried out by AUV 2.

5.2. Formation Control and Obstacle Avoidance Under Moving Obstacle Scenario

Next, we consider a scenario with a moving obstacle and three AUVs, where the communication topology among the AUVs is fully connected. The initial positions, initial velocities, and final formation of the three AUVs are the same as in the previous simulation. The safety detection range and safety zone of the AUVs also remain consistent with those in previous subsection. In this scenario, a spherical obstacle moves at a constant speed in the 3D space. The radius of the obstacle is

\sqrt{2}

and its center is located at (20, −10, −3.8) at the initial moment. At the start of the simulation, the obstacle moves at a speed of

{[0, 0.65, 0]}^{T}

m/s. With a total simulation time of

t = 50

s, the following simulation results were obtained.

Figure 14 shows the trajectories of each AUV at simulation time

t = 10

s. It can be observed that all AUVs have already formed a formation based on the consensus strategy.

Figure 15 shows the trajectories of each vehicle at simulation time

t = 19

s, where it can be seen that AUV 1 has started its obstacle avoidance action. As the obstacle continues to move, the relationship between the movement direction of AUV 1 and the direction vector of its approach to the obstacle surface in the same plane is determined, and the gyroscopic force is no longer triggered. After this, AUV 1 returns to the formation.

Figure 16 shows the trajectories of each vehicle at simulation time

t = 34

s, where it can be seen that AUV 3 is in the process of avoiding an obstacle. When AUV 3 reaches a distance that is less than the detection range from the obstacle surface in the same plane, the gyroscopic force based on the relationship between the velocity vector directs the vehicle’s movement toward the positive direction of the Y-axis to avoid the obstacle. As the obstacle moves, the gyroscopic force adjusts the direction based on the new relationship between the movement and the obstacle’s direction vector, shifting toward the negative direction of the Y-axis to achieve obstacle avoidance. As the obstacle continues to move, the avoidance action ends when AUV 3 reaches a distance greater than the detection range, and AUV 3 returns to the formation based on the consensus strategy.

Figure 17 shows the trajectories of all the AUVs throughout the entire simulation, with obstacles appearing at simulation times

t = 19

s and

t = 34

s. It can be seen that the vehicle formation successfully completes the assigned task even in the presence of a moving obstacle.

Figure 18 shows the changes in distance between each vehicle and the others. Small fluctuations can be observed in the distances between AUVs 1 and 3 during the gyroscopic obstacle avoidance process when faced with moving obstacles. However, after the obstacle avoidance action ends, the vehicles quickly return to the prescribed formation, demonstrating the rigidity of the formation structure.

Figure 19 shows the translational velocities of each vehicle in three directions during the simulation. It can be seen that AUVs 1 and 3 quickly regain their ideal speeds after completing obstacle avoidance, which reflects the robustness of the consensus formation strategy. Due to the non-work characteristic of the gyroscopic force, the consensus speeds before and after obstacle avoidance remain unchanged, allowing all AUVs to continue the task at the predefined speed.

Figure 20 shows the thrust of each vehicle in three directions.

Figure 21 shows the distance between each vehicle and the surface of obstacle 3 in the

Z = - 5

plane during the simulation. It can be seen that gyroscopic force-based obstacle avoidance is triggered when the distance between AUV 1 and the obstacle becomes less than the AUV’s detection range. As the obstacle gradually moves away from AUV 1, the distance between them increases, and AUV 1 returns to the formation after completing obstacle avoidance. In the case of AUV 3, the initial gyroscopic obstacle avoidance direction is determined by the relationship between the velocity vector and the surface vector in the same plane. As the obstacle continues to move forward, the avoidance direction is rotated. When the distance between AUV 3 and the obstacle exceeds the detection range, the obstacle avoidance action ends and AUV 3 returns to the previous formation.

6. Conclusions

This paper first presents the dynamic model of an AUV on SE(3) and introduces a consensus-based control law for maintaining formation. Then, the gyroscopic force is introduced as an obstacle avoidance method within the formation strategy of multiple AUVs. The application of the gyroscopic obstacle avoidance strategy is extended from mutual avoidance between multiple agents to a fully actuated underwater vehicle model in the SE(3) framework. Given the non-energy-consuming nature of the gyroscopic force, there is excellent compatibility between the gyroscopic force and consensus-based formation control in the SE(3) framework. Because of this characteristic, the velocity of the formation established through consensus remains unchanged before and after the AUVs perform gyroscopic obstacle avoidance. The formation system in the SE(3) framework exhibits strong rigidity, allowing each AUV to quickly return to the pre-avoidance relative configuration and velocity specified by the mission after completing the obstacle avoidance maneuver. Simulations were used to verify the feasibility of the gyroscopic obstacle avoidance behavior for AUVs under a consensus-based formation strategy in the SE(3) framework. In scenarios involving multiple or moving obstacles, the underwater vehicle formation system successfully performed gyroscopic obstacle avoidance and accomplished the formation task under the consensus-based strategy.

The technical contributions of this paper can be classified as follows: (1). Consensus-based formation control in SE(3). This paper extends consensus-based control strategies to the SE(3) framework, which fully captures the coupled translational and rotational dynamics of underactuated AUVs. Unlike traditional methods, which often rely on decoupled or simplified kinematic models, this approach enables precise coordination in 3D environments while preserving geometric consistency and reducing numerical errors over extended operations. (2). Gyroscopic force-based obstacle avoidance. A novel gyroscopic obstacle avoidance mechanism is introduced leveraging angular momentum conservation principles to avoid obstacles without causing abrupt trajectory changes. This method addresses the limitations of conventional potential field-based and reactive avoidance strategies, which often face challenges such as local minima and instability in dense obstacle environments. (3). Integrated framework for formation-keeping and obstacle avoidance. The proposed framework seamlessly integrates formation control and gyroscopic obstacle avoidance within the SE(3) dynamics. This integration allows AUVs to maintain formation stability while avoiding obstacles dynamically, providing robustness in high-density environments. In addition, it bridges the gap between obstacle avoidance and formation control by ensuring that avoidance maneuvers do not compromise the formation integrity. (4). Practical implementation and validation. The feasibility of the proposed methods has been demonstrated through detailed simulation studies, showcasing the ability of AUVs to maintain a triangular formation while effectively avoiding obstacles. The proposed framework offers a practical solution for real-world multi-AUV missions requiring complex maneuvers in constrained environments.

Our future work will focus on selecting more appropriate gyroscopic obstacle avoidance methods for different situations and addressing formation and obstacle avoidance control in more complex scenarios. Other areas for future work could include extending the model to heterogeneous AUVs, integrating machine learning, improving robustness in complex environments, and detailing plans for further experimental validation.

Author Contributions

Conceptualization, Q.Z. and D.J.; methodology, Q.Z. and D.J.; software, Q.Z. and D.J.; validation, Q.Z. and Y.Z.; formal analysis, Q.Z.; investigation, Q.Z. and D.J.; resources, Q.Z. and L.W.; data curation, Q.Z.; writing—original draft preparation, Q.Z.; writing—review and editing, D.J.; visualization, Q.Z.; supervision, L.W., Y.Z. and D.J.; project administration, D.J.; funding acquisition, D.J. All authors have read and agreed to the published version of the manuscript.

Funding

This research was funded by the Southern Marine Science and Engineering Guangdong Laboratory (Zhuhai), China (Grant No. SML2023SP228), National Natural Science Foundation of China (52101379, U22A2012), Opening Research Fund of the National Engineering Laboratory for Test and Experiment Technology of Marine Engineering Equipment (Grant No. 750NEL-2023-04).

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Data are contained within the article.

Conflicts of Interest

The authors declare no conflicts of interest. The funders had no role in the design of the study, in the collection, analysis, or interpretation of data, in the writing of the manuscript, or in the decision to publish the results.

References

Huang, H.; Liao, Y.L.; Shen, H.L.; Sheng, M.W.; Jiang, D.P.; Wan, L.; Pang, Y.J. Adaptive AUV formation strategy under acoustic communication conditions. In Proceedings of the OCEANS 2014-TAIPEI, Taipei, Taiwan, 7–10 April 2014; IEEE: Piscataway, NJ, USA, 2014; pp. 1–6. [Google Scholar]
Hao, L.; Gu, H.; Kang, F.; Yang, H. Virtual-leader based formation control with constant bearing guidance for underactuated AUVs. ICIC Express Lett. 2017, 11, 117–125. [Google Scholar]
Liang, H.; Kang, F.; Li, H. UUV formation system modeling and simulation research based on Multi-Agent Interaction Chain. Int. J. Model. Simul. Sci. Comput. 2015, 6, 1550019. [Google Scholar] [CrossRef]
Qi, X.; Zhang, L.J.; Zhao, J.M. Adaptive path following and coordinated control of Autonomous Underwater Vehicles. In Proceedings of the 33rd Chinese Control Conference, Nanjing, China, 28–30 July 2014; IEEE: Piscataway, NJ, USA, 2014; pp. 2127–2132. [Google Scholar]
Yang, H.; Wang, C.; Zhang, F. Robust geometric formation control of multiple autonomous underwater vehicles with time delays. In Proceedings of the 2013 American Control Conference, Washington, DC, USA, 17–19 June 2013; IEEE: Piscataway, NJ, USA, 2013; pp. 1380–1385. [Google Scholar]
Cui, R.; Ge, S.S.; How, B.V.E.; Choo, Y.S. Leader–follower formation control of underactuated autonomous underwater vehicles. Ocean. Eng. 2010, 37, 1491–1502. [Google Scholar] [CrossRef]
Zhang, L.c.; Wang, J.; Wang, T.; Liu, M.; Gao, J. Optimal formation of multiple AUVs cooperative localization based on virtual structure. In Proceedings of the OCEANS 2016 MTS/IEEE Monterey, Monterey, CA, USA, 19–23 September 2016; IEEE: Piscataway, NJ, USA, 2016; pp. 1–6. [Google Scholar]
Li, J.; Du, X. Underactuated multi-AUV robust formation control based on virtual leader. In Proceedings of the 2018 IEEE International Conference on Mechatronics and Automation (ICMA), Changchun, China, 5–8 August 2018; IEEE: Piscataway, NJ, USA, 2018; pp. 1568–1573. [Google Scholar]
Zhang, J.; Sha, J.; Han, G.; Liu, J.; Qian, Y. A cooperative-control-based underwater target escorting mechanism with multiple autonomous underwater vehicles for underwater Internet of Things. IEEE Internet Things J. 2020, 8, 4403–4416. [Google Scholar] [CrossRef]
Xia, G.; Zhang, Y.; Yang, Y. Control method of multi-AUV circular formation combining consensus theory and artificial potential field method. In Proceedings of the 2020 Chinese Control And Decision Conference (CCDC), Hefei, China, 22–24 August 2020; IEEE: Piscataway, NJ, USA, 2020; pp. 3055–3061. [Google Scholar]
Yan, Z.; Yang, Z.; Yue, L.; Wang, L.; Jia, H.; Zhou, J. Discrete-time coordinated control of leader-following multiple AUVs under switching topologies and communication delays. Ocean. Eng. 2019, 172, 361–372. [Google Scholar] [CrossRef]
Chen, Y.L.; Ma, X.W.; Bai, G.Q.; Sha, Y.; Liu, J. Multi-autonomous underwater vehicle formation control and cluster search using a fusion control strategy at complex underwater environment. Ocean. Eng. 2020, 216, 108048. [Google Scholar] [CrossRef]
Hao, K.; Zhao, J.; Li, Z.; Liu, Y.; Zhao, L. Dynamic path planning of a three-dimensional underwater AUV based on an adaptive genetic algorithm. Ocean. Eng. 2022, 263, 112421. [Google Scholar] [CrossRef]
Fan, X.; Guo, Y.; Liu, H.; Wei, B.; Lyu, W. Improved artificial potential field method applied for AUV path planning. Math. Probl. Eng. 2020, 2020, 6523158. [Google Scholar] [CrossRef]
Shen, Y.; Xu, H.; Wang, D.; Zhang, Y.; Yan, T.; He, B. Real-time obstacle avoidance for AUV based on reinforcement learning and dynamic window approach. In Proceedings of the Global Oceans 2020: Singapore–US Gulf Coast, Biloxi, MS, USA, 5–30 October 2020; IEEE: Piscataway, NJ, USA, 2020; pp. 1–4. [Google Scholar]
Eriksen, B.O.H.; Breivik, M.; Pettersen, K.Y.; Wiig, M.S. A modified dynamic window algorithm for horizontal collision avoidance for AUVs. In Proceedings of the 2016 IEEE Conference on Control Applications (CCA), Buenos Aires, Argentina, 19–22 September 2016; IEEE: Piscataway, NJ, USA, 2016; pp. 499–506. [Google Scholar]
Ding, G.; Zhu, D.; Sun, B. Formation control and obstacle avoidance of Multi-AUV for 3-D underwater environment. In Proceedings of the 33rd Chinese Control Conference, Nanjing, China, 28–30 July 2014; IEEE: Piscataway, NJ, USA, 2014; pp. 8347–8352. [Google Scholar]
Huang, W.; Fang, H.; Li, L. Obstacle avoiding policy of multi-AUV formation based on virtual AUV. In Proceedings of the 2009 Sixth International Conference on Fuzzy Systems and Knowledge Discovery, Tianjin, China, 14–16 August 2009; IEEE: Piscataway, NJ, USA, 2009; Volume 4, pp. 131–135. [Google Scholar]
Chang, D.E.; Shadden, S.C.; Marsden, J.E.; Olfati-Saber, R. Collision avoidance for multiple agent systems. In Proceedings of the 42nd IEEE International Conference on Decision and Control (IEEE Cat. No. 03CH37475), Maui, HI, USA, 9–12 December 2003; IEEE: Piscataway, NJ, USA, 2003; Volume 1, pp. 539–543. [Google Scholar]
Chang, D.E.; Marsden, J.E. Gyroscopic forces and collision avoidance with convex obstacles. In New Trends in Nonlinear Dynamics and Control and Their Applications; Springer: Berlin/Heidelberg, Germany, 2004; pp. 145–159. [Google Scholar]
Chaturvedi, N.A.; McClamroch, N.H. Almost global attitude stabilization of an orbiting satellite including gravity gradient and control saturation effects. In Proceedings of the 2006 American Control Conference, Minneapolis, MN, USA, 14–16 June 2006; IEEE: Piscataway, NJ, USA, 2006; p. 6. [Google Scholar]
Sanyal, A.; Chaturvedi, N. Almost global robust attitude tracking control of spacecraft in gravity. In Proceedings of the AIAA Guidance, Navigation and Control Conference and Exhibit, Honolulu, HI, USA, 18–21 August 2008; p. 6979. [Google Scholar]
Mansourinasab, S.; Sojoodi, M.; Moghadasi, S.R. Convex Optimization of Bearing Formation Control of Rigid bodies on Lie Group. arXiv 2023, arXiv:2309.17150. [Google Scholar]
Ghasemi, K.; Ghaisari, J.; Abdollahi, F. Robust formation control of multiagent systems on the Lie group SE (3). Int. J. Robust Nonlinear Control. 2020, 30, 966–998. [Google Scholar] [CrossRef]
Tong, X.; Ding, Q.; Cheng, S.S. Global Coordinated Stabilization of Multiple Simple Mechanical Control Systems on a Class of Lie Groups. IEEE Trans. Autom. Control. 2024. [Google Scholar] [CrossRef]
Sanyal, A.; Nordkvist, N.; Chyba, M. An almost global tracking control scheme for maneuverable autonomous vehicles and its discretization. IEEE Trans. Autom. Control. 2010, 56, 457–462. [Google Scholar] [CrossRef]
Dong, R.; Geng, Z. Consensus based formation control laws for systems on Lie groups. Syst. Control. Lett. 2013, 62, 104–111. [Google Scholar] [CrossRef]
Sarlette, A.; Bonnabel, S.; Sepulchre, R. Coordinated motion design on Lie groups. IEEE Trans. Autom. Control. 2010, 55, 1047–1058. [Google Scholar] [CrossRef]
Fossen, T.I. Handbook of Marine Craft Hydrodynamics and Motion Control; John Willy & Sons Ltd.: Hoboken, NJ, USA, 2011. [Google Scholar]
Healey, A.J.; Lienard, D. Multivariable sliding mode control for autonomous diving and steering of unmanned underwater vehicles. IEEE J. Ocean. Eng. 1993, 18, 327–339. [Google Scholar] [CrossRef]

Figure 1. The inertial reference frame and body-fixed frame.

Figure 2. Control block diagram.

Figure 3. Movement trajectories of three AUVs during the simulation process.

Figure 4. Relative distance between AUVs in the simulation.

Figure 5. Translational speed of the AUVs in three directions.

Figure 6. Thrust of the AUVs in three directions.

Figure 7. Movement trajectories of the three AUVs during the simulation.

Figure 8. Relative distance between the AUVs during the simulation.

Figure 9. Translational speed of the AUVs in three directions.

Figure 10. Thrust of the AUVs in three directions.

Figure 11. Distance from the AUVs to the surface of obstacle 1 on the plane

Z = - 5

Figure 11. Distance from the AUVs to the surface of obstacle 1 on the plane

Z = - 5

Figure 12. Distance from the AUVs to the surface of obstacle 2 on the plane

Z = - 5

Figure 12. Distance from the AUVs to the surface of obstacle 2 on the plane

Z = - 5

Figure 13. Distance from the AUVs to the surface of obstacle 3 on the plane

Z = - 5

Figure 13. Distance from the AUVs to the surface of obstacle 3 on the plane

Z = - 5

Figure 14. Movement trajectories of the three AUVs when t = 10 s.

Figure 15. Movement trajectories of the three AUVs when t = 19 s.

Figure 16. Movement trajectories of the three AUVs when t = 34 s.

Figure 17. Movement trajectories of the three AUVs when t = 50 s.

Figure 18. Relative distance between the AUVs in the final simulation.

Figure 19. Translational speed of the AUVs in three directions.

Figure 20. Thrust of the AUVs in three directions.

Figure 21. Distance from the AUVs to the surface of obstacle on the

Z = - 5

plane.

Figure 21. Distance from the AUVs to the surface of obstacle on the

Z = - 5

plane.

Table 1. Notation.

Symbol	Description
$m \in R$	Mass
$J \in R^{3 * 3}$	Inertia matrix with respect to the body-fixed frame
$R \in S O (3)$	Rotation matrix from the body-fixed frame to the inertial frame
$Ω \in R^{3}$	Angular velocity in the body-fixed frame
$x \in R^{3}$	Location of the center of mass in the inertial frame
$v \in R^{3}$	Velocity of the center of mass in the inertial frame
$u \in R^{3}$	Total thrust
$τ \in R^{3}$	Thrust generated by the propeller along the $b_{1}$ axis
$M \in R^{3}$	Total moment in the body-fixed frame
${\hat{ξ}}^{B} \in S E (3)$	Velocity in the body-fixed frame
${\hat{ξ}}^{I} \in S E (3)$	Velocity in the inertial frame

Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

© 2024 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Zhen, Q.; Wan, L.; Zhang, Y.; Jiang, D. Consensus-Based Formation Control and Gyroscopic Obstacle Avoidance for Multiple Autonomous Underwater Vehicles on SE(3). J. Mar. Sci. Eng. 2024, 12, 2350. https://doi.org/10.3390/jmse12122350

AMA Style

Zhen Q, Wan L, Zhang Y, Jiang D. Consensus-Based Formation Control and Gyroscopic Obstacle Avoidance for Multiple Autonomous Underwater Vehicles on SE(3). Journal of Marine Science and Engineering. 2024; 12(12):2350. https://doi.org/10.3390/jmse12122350

Chicago/Turabian Style

Zhen, Qingzhe, Lei Wan, Yuansheng Zhang, and Dapeng Jiang. 2024. "Consensus-Based Formation Control and Gyroscopic Obstacle Avoidance for Multiple Autonomous Underwater Vehicles on SE(3)" Journal of Marine Science and Engineering 12, no. 12: 2350. https://doi.org/10.3390/jmse12122350

APA Style

Zhen, Q., Wan, L., Zhang, Y., & Jiang, D. (2024). Consensus-Based Formation Control and Gyroscopic Obstacle Avoidance for Multiple Autonomous Underwater Vehicles on SE(3). Journal of Marine Science and Engineering, 12(12), 2350. https://doi.org/10.3390/jmse12122350

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Menu

Consensus-Based Formation Control and Gyroscopic Obstacle Avoidance for Multiple Autonomous Underwater Vehicles on SE(3)

Abstract

1. Introduction

2. Background and Model Description

2.1. Notations

2.2. Dynamics Model

3. Formation Control Design for AUVs

3.1. Formation Control Design Based on Consensus

3.2. Consensus and Formation Control of Multiple AUVs

4. Formation Control Algorithm Design for AUVs with Gyroscopic Force-Based Obstacle Avoidance on SE(3)

4.1. Gyroscopic Force

4.2. Obstacle Avoidance Strategy for Multiple Agents Using Gyroscopic Force

4.3. Consensus-Based Formation Control of AUVs on SE(3) with Gyroscopic Obstacle Avoidance

4.4. Control Block Diagram

5. Simulation Results

5.1. Formation Control and Obstacle Avoidance Under Multiple Obstacles

5.2. Formation Control and Obstacle Avoidance Under Moving Obstacle Scenario

6. Conclusions

Author Contributions

Funding

Institutional Review Board Statement

Informed Consent Statement

Data Availability Statement

Conflicts of Interest

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI