Coordinated Obstacle Avoidance of Multi-AUV Based on Improved Artificial Potential Field Method and Consistency Protocol

1. Introduction

Autonomous Underwater Vehicle (AUV) has recently become a new marine development tool for marine resource exploitation and environmental exploration operations [1]. It has the advantages of autonomy, independence, a tiny target, and high applicability. However, it is difficult for a single underwater autonomous vehicle to meet the increasing mission requirements in the complex and broad ocean environment. Single unmanned underwater autonomous vehicles have limited energy load, small range, low task performance efficiency, and instability. With the Multi Autonomous Underwater Vehicles (MAUVs) system [2] coming into operation, the mutual coordination between single AUVs [3] more rationally compensates for the shortcomings of single AUVs.

In recent years, many researchers have investigated formation avoidance algorithms in complex environments [4]. Obstacle avoidance can be divided into path planning for global obstacle avoidance and path planning for local obstacle avoidance. If the global environment information is known, international path planning can be seen as a nonlinear optimization problem to find the optimal solution for global variables. Many scholars have proposed many standard global path-planning algorithms at home and abroad. For example, the A* algorithm（Dechter, R and Pearl，1985）[5], the genetic algorithm（Cobb and Grefenstette, 1993）[6], fast extended random tree (Cui, RX and Li, Y, 2016)[7], particle swarm algorithm （Eberhart and Kennedy，1995）[8], ant colony algorithm [9] and so on. And the local obstacle avoidance aspects are more applied such as the artificial potential field method [10], fuzzy decision making (Smith, SM and Ganesan, K, 1998)[11], neural network[12] and reinforcement learning methods[13], etc.

Due to the rapid change of the ocean environment and many uncertainties, although global path planning can plan a better path, the path cannot be used as the actual reference path of the AUV. Cheng, Chun Lei et al. [14] considered the ocean current’s force. They synthesized the current’s velocity with the AUV’s velocity, combined with the artificial potential field method to overcome the influence of the ocean current on path planning. Song [15] proposed an improved artificial potential field method by improving the parameters such as angle limit to solve the problem that the artificial potential field algorithm can easily fall into particular terrain and improve the algorithm’s stability. Zhao, ZY [16] proposed a hybrid adaptive preference method based on the improved artificial potential field (HAP-IAPF). Adaptive weight control units are used to adjust the preference strategy. Cooperative multi-AUV hunting in dynamic underwater obstacle environments is achieved under weakly connected conditions. Wang SM et al. [17] combined the H-infinity controller with the artificial potential field method (APFM) for the subsea navigation of autonomous underwater vehicles (AUVs). Depth control and altitude control prevent AUVs from colliding with the seafloor or obstacles, and simulations and laboratory tests of various seafloor profiles show that the combination of the H infinity controller and APFM is feasible and effective. Zhen, QZ [18] confronted with the attitude adjustment problem in AUV deployment and the detection of seafloor topographic undulations, proposed a finite-time position and attitude tracking control method combining artificial potential fields and virtual structures. This control method ensures that the AUVs avoid colliding with each other during the dive and form and change formations after reaching a set depth. Orozco-Rosas et al.[19] proposed a membrane-evolutionary artificial potential field method for solving the path planning problem of mobile robots, which combines membrane computation with genetic algorithms (membrane-inspired evolutionary algorithms with a first-class membrane structure) and artificial potential field methods to find the parameters that generate feasible and safe paths.

The rest of this paper is organised as follows. Section 2 gives the AUV dynamic and kinetics model and the problem description. Section 3 presents the improved artificial potential field method and the consistency protocol. The flow and stability proof of the cooperative obstacle avoidance algorithm are given in Section 4. Subsequently, the simulation experiments are given in Section 5. Finally, the conclusions are obtained in Section 6.

2. Problem Statement and Model Description

2.1. Problem Description

In unknown underwater environments, AUVs may encounter complex terrains such as reefs, islands, trenches and valleys, and the obstacles they encounter vary in shape, complexity and number. To ensure the AUV’s safety and the mission’s successful completion, the AUV must be capable of avoiding these complex obstacles [20].

1. Movement limitation: In the actual motion state, AUVs are limited by the performance of their equipment and so on. Therefore, it is necessary to consider the impact of the actual model of AUV in the motion limitation function on the track planning during the AUV navigation.

2. Obstacle types: When AUVs perform missions in unknown underwater environments, they will encounter multiple obstacle types. In this paper, the threat values of each point of the obstacles to AUVs are represented uniformly for modelling the probabilistic threat environment. Grouping different types of obstacles under the same map is helpful in improving the speed and stability of the algorithm.

2.2. AUV Movement Model

Based on the characteristics of AUVs, the dynamics and dynamics of AUVs are usually modelled by a six-degree-of-freedom (DOF) model. However, since the AUV needs an actuator for roll control, the roll motion is usually not considered. In the inertial coordinate system and its own fixed coordinate system. According to (Fossen, 2011), the kinematic and dynamical model of the AUV kinematic and dynamical model of the AUV can be represented by five degrees of freedom as follows [22].

$$\left\{\begin{array}{l}\dot{\mathit{\eta }}=\mathit{R}(\psi )\mathit{v}\\ \mathit{M}\dot{\mathit{v}}+\mathit{C}(\mathit{v})\mathit{v}+\mathit{D}(\mathit{v})\mathit{v}+\mathit{g}(\mathit{\eta })=\mathit{T}\end{array}\right.$$

(1)

where, $\mathit{\eta }={\left[x,y,z,\theta ,\psi \right]}^{\mathrm{T}},\mathit{v}={\left[u,v,w,q,r\right]}^{\mathrm{T}}$, $\mathit{R}(\psi )$ is the rotational transformation matrix from the body-fixed coordinate frame to the inertial coordinate frame $\mathit{M}$ is the inertia matrix, $\mathit{C}(\mathit{v})$ is the hydrodynamic Coriolis and centripetal force matrix, $\mathit{D}(\mathit{v})$ is the nonlinear fluid hydrodynamic damping matrix, $\mathit{g}(\mathit{\eta })$ is the restoring force (moment) vector, and $\mathit{T}$ is the external force and external moment vector applied to the vehicle.

The navigator system itself is strongly coupled and vulnerable to the influence of external waves and currents. The nonlinear characteristics of the navigator cause some difficulties in engineering processing, so this paper linearizes the model of the navigator with feedback.

First, the AUV dynamics can be expressed as a general nonlinear system.

$$\begin{array}{l}\dot{\mathit{x}}(t)=\mathit{f}\left(\mathit{x}(t)\right)+\mathit{g}\left(\mathit{x}(t)\right)\mathit{u}(t)\\ \mathit{y}(t)=\mathit{h}\left(\mathit{x}(t)\right)\end{array}$$

(2)

where $\mathit{x}\in {ℝ}^m$ is the system state vector, $\mathit{u}\in {ℝ}^p$ is the system control input vector, and $\mathit{y}\in {ℝ}^q$ is the system output vector. $\mathit{f}(\mathit{x})$, $\mathit{g}(\mathit{x})$ and $\mathit{h}(\mathit{x})$ are the corresponding vector coefficients.

Define the matrix ${\mathit{M}}_c$ and Damping matrix ${\mathit{M}}_{\mathit{x}}$

$${\mathit{M}}_c=\left[\begin{array}{ccccc}m-\frac{1}{2}\rho L^3{X}_{\dot{u}}^{\prime }& 0& 0& 0& 0\\ 0& m-\frac{1}{2}\rho L^3{Y}_{\dot{v}}^{\prime }& 0& 0& -\frac{1}{2}\rho L^4{Y}_{\dot{r}}^{\prime }\\ 0& 0& m-\frac{1}{2}\rho L^3{Z}_{\dot{w}}^{\prime }& -\frac{1}{2}\rho L^4{Z}_{\dot{q}}^{\prime }& 0\\ 0& 0& -\frac{1}{2}\rho L^4{M}_{\dot{w}}^{\prime }& I_y-\frac{1}{2}\rho L^5{M}_{\dot{q}}^{\prime }& 0\\ 0& -\frac{1}{2}\rho L^4{N}_{\dot{v}}^{\prime }& 0& 0& I_z-\frac{1}{2}\rho L^5{N}_{\dot{r}}^{\prime }\end{array}\right]$$

(3)

$${\mathit{M}}_{\mathit{x}}=\left[\begin{array}{cc}\mathit{I}& 0\\ 0& {\mathit{M}}_c\end{array}\right]$$

(4)

${\mathit{M}}_{\mathit{x}}$ must be a non-singular matrix and the system function $\mathit{f}(\mathit{x})$ and $\mathit{g}(\mathit{x})$ can be expressed as:

$$\left\{\begin{array}{l}\mathit{f}\left(\mathit{x}\right)={\mathit{M}}_{\mathit{x}}^{-1}{\mathit{f}}^{\prime }\left(\mathit{x}\right)\\ \mathit{g}\left(\mathit{x}\right)={\mathit{M}}_{\mathit{x}}^{-1}{\mathit{g}}^{\prime }\left(\mathit{x}\right)\end{array}\right.$$

(5)

Define the output equation of the system:

$$\mathit{h}(\mathit{x})={\left[\begin{array}{lllll}x\hfill & y\hfill & z\hfill & \theta \hfill & \psi \hfill \end{array}\right]}^{\mathrm{T}}$$

(6)

Based on the nonlinear model of AUV and the principle of feedback linearization, the feedback control rate and the set of coordinate transformations are found to obtain the feedback linearization model of the unmanned aerial vehicle.

The system function vector $\mathit{f}(\mathit{x})$, $\mathit{h}(\mathit{x})$, $\mathit{g}(\mathit{x})$ can be obtained from the previous presentation.

$$\mathit{f}(\mathit{x})={\mathit{M}}_{\mathit{x}}^{-1}{\mathit{f}}^{\prime }(\mathit{x})=\left[\begin{array}{cc}\mathit{I}& 0\\ 0& {\mathit{M}}_c^{-1}\end{array}\right]\left[\begin{array}{c}\mathit{R}(\mathit{\eta })\mathit{v}\\ \mathit{N}(\mathit{\eta },\mathit{v})\end{array}\right]$$

(7)

$$\mathit{h}(\mathit{x})={\left[h_1(x),h_2(x),h_3(x),h_4(x),h_5(x)\right]}^{\mathrm{T}}={\left[x,y,z,\theta ,\psi \right]}^{\mathrm{T}}$$

(8)

$$\mathit{g}(\mathit{x})=\left[{\mathit{g}}_1(\mathit{x}),{\mathit{g}}_2(\mathit{x}),\cdots ,{\mathit{g}}_5(\mathit{x})\right]=\left[\begin{array}{c}0\\ {\mathit{M}}^{-1}{\mathit{g}}^{\prime }(\mathit{x})\end{array}\right]$$

(9)

where, 0 is a $5\times 5$ zero matrix.

Based on the Lee derivative [23] and the definition of $\mathit{g}(\mathit{x})$, for any $1\le i\le 10$, $1\le j\le 10$, it follows that:

$${\mathit{L}}_{g_i}{h}_j(x)=0$$

(10)

By calculating the above equation we can obtain the matrix $\mathbf{\Gamma }(\mathit{x})=[{\mathbf{\Gamma }}_{ij}(\mathit{x})]$,${\mathbf{\Gamma }}_{ij}(\mathit{x})={\mathit{L}}_{g_j}{\mathit{L}}_f{h}_i(x)$, $1\le i\le p,1\le j\le p$.

$$\mathbf{\Gamma }(\mathit{x})=\left[\begin{array}{ccccc}{\tilde{g}}_{11}\cos \psi \cos \theta & -{\tilde{g}}_{22}\sin \psi & {\tilde{g}}_{33}\cos \psi \sin \theta & {\tilde{g}}_{34}\cos \psi \sin \theta & -{\tilde{g}}_{25}\sin \psi \\ {\tilde{g}}_{11}\sin \psi \cos \theta & {\tilde{g}}_{22}\cos \psi & {\tilde{g}}_{33}\sin \psi \sin \theta & {\tilde{g}}_{34}\sin \psi \sin \theta & {\tilde{g}}_{25}\cos \psi \\ -{\tilde{g}}_{11}\sin \theta & 0& {\tilde{g}}_{33}\cos \theta & {\tilde{g}}_{34}\cos \theta & 0\\ 0& 0& {\tilde{g}}_{43}& {\tilde{g}}_{44}& 0\\ 0& \frac{{\tilde{g}}_{52}}{\cos \theta }& 0& 0& \frac{{\tilde{g}}_{55}}{\cos \theta }\end{array}\right]$$

(11)

where matrix $\mathbf{\Gamma }(\mathit{x})$a is a non-singular matrix, and${\rho }_1+{\rho }_2+{\rho }_3+{\rho }_4+{\rho }_5=10$. The relative order is the same as the system order, i.e., the nonlinear model of the navigator can be linearized by feedback. By transforming $z=\phi (x)$ by coordinates, it follows that:

$$\begin{array}{l}{z}_1={\left[\begin{array}{lllll}{h}_1(x)\hfill & h_2(x)\hfill & h_3(x)\hfill & h_4(x)\hfill & h_5(x)\hfill \end{array}\right]}^{\mathrm{T}}\\ z_2={\left[\begin{array}{lllll}{\mathit{L}}_f{h}_1(x)\hfill & {\mathit{L}}_f{h}_2(x)\hfill & {\mathit{L}}_f{h}_3(x)\hfill & {\mathit{L}}_f{h}_4(x)\hfill & {\mathit{L}}_f{h}_5(x)\hfill \end{array}\right]}^{\mathrm{T}}\end{array}$$

(12)

Based on the definition of the Lee derivative:

$$\begin{array}{l}{\dot{z}}_1=z_2\\ {\dot{z}}_2={\mathit{L}}_{\mathit{f}}^2\mathit{h}(\mathit{x})+{\mathit{L}}_{\mathit{g}}{\mathit{L}}_{\mathit{f}}\mathit{h}(\mathit{x})\mathit{u}\end{array}$$

(13)

Control inputs in the new coordinate system:

$$\tilde{\mathit{u}}=\mathit{B}(\mathit{x})+\mathit{\Gamma }(\mathit{x})\mathit{u}={\mathit{L}}_{\mathit{f}}^2\mathit{h}(\mathit{x})+{\mathit{L}}_{\mathit{g}}{\mathit{L}}_{\mathit{f}}\mathit{h}(\mathit{x})\mathit{u}$$

(14)

In the presence of a transformed coordinate system and feedback control inputs:

$$\mathit{u}={\mathit{\Gamma }}^{-1}(\mathit{x})(\mathit{u}-\mathit{B}(\mathit{x}))$$

(15)

The model feedback linearization of the navigator can be expressed as:

$$\begin{array}{l}{\dot{z}}_1^i=z_2^i\\ {\dot{z}}_2^l={\mathit{u}}_i\end{array}$$

(16)

where, ${\dot{z}}_1^i\in {ℝ}^5$, $z_2^i\in {ℝ}^5$,${\mathit{u}}_i\in {ℝ}^5$.

2.3. Probabilistic Threat Environment Map

To represent the threats to AUVs in complex subsea environments, probabilistic threat environment maps are defined in this paper to model them. The probabilistic threat map is a function of location and describes the threat probability of the threat sources at different locations to which the AUV is exposed [24]. It is an environment that shows the distribution of threats in the environment in a probabilistic form. Each location coordinate corresponds to a probability density value as an indication of the risk of AUV exposure to a threat source at that coordinate point.

Suppose there are $m$ suspicious locations in the region, the threat probability for any location $r$in the region can be expressed as a multidimensional Gaussian probability function:

$$t\mathit{h}reat(r)={\displaystyle \sum _{i=1}^m\frac{1}{2\pi }}\exp [-\frac{1}{2}{(r-r_i)}^{\mathit{T}}{\mathit{K}}_i(r-r_i)]$$

(17)

Since this paper conducts the study of local obstacle avoidance problem of AUV formation, only local threat environment modeling is performed when designing the environment.

Figure 1. Probabilistic Threat Environment Map.

Figure 1. Probabilistic Threat Environment Map.

3. Improvement of artificial potential field method

3.1. Auxiliary potential field repulsion field design for obstacles

The artificial potential field method is widely used in the scenario of local obstacle avoidance, which has the advantages of good real-time and does not rely on global information [25]. But it is prone to disadvantages such as zero combined force falling into local minima and overlapping potential fields of neighboring obstacles that prevent the optimal path from being obtained. In the actual AUV control process, it also generates insufficient time to complete the obstacle avoidance operation command due to the longitudinal speed of the AUV. Based on the shortcomings of the traditional artificial potential field method, this chapter introduces an obstacle-assisted potential field on the traditional artificial potential field method to avoid falling into local minima due to zero combined forces.

Figure 2. AUV formation obstacle recognition.

Figure 2. AUV formation obstacle recognition.

Suppose $p_t(t)=(\mathit{x}(t),\mathit{y}(t),z(t))$ is an obstacle reference point in the same plane as the AUV. ${\mathit{x}}_{tar}({\mathit{x}}_{tar},{\mathit{y}}_{tar},z_{tar})$ is the coordinate of the target point. The target point and the obstacle reference point exert gravitational force $F_t(t)$ and repulsive force $F_o(t)$ on the AUV, respectively. The repulsive force generated between the other internal AUVs is $F_{in}(t)$. The current AUV position A is denoted as $p_l(t)=[{\mathit{x}}_l(t),{\mathit{y}}_l(t),z_l(t)]$.

In the probabilistic threat environment, the probabilistic threat value of the obstacle also affects the potential energy function of the AUV. The potential energy function varies with the transformation of the probabilistic threat value at the current location. Suppose the distance vector between the AUV and the boundary barrier reference point is$△l=p_l(t)-p_t(t)$. With a time-varying distance vector, the potential energy function at the current position of AUV at time can be expressed as:

$$U=t\mathit{h}reat(r)={\displaystyle \sum _{i=1}^m\frac{1}{2\pi }}\exp [-\frac{1}{2}{(r_i)}^{\mathit{T}}{\mathit{K}}_i(r_i)]$$

(18)

Potential energy function with reference to the barrier point.

$$U_{req}(p_t(t))=\left\{\begin{array}{cc}{\gamma }^2\exp \left\{-\frac{1}{2}{(p_l(t)-p_t(t))}^{\mathit{T}}{\mathit{K}}_i(p_l(t)-p_t{(t)}_i)\right\}& △l\le d\\ 0& △l>d\end{array}\right.$$

(19)

where $\gamma$ denotes the potential energy function coefficient, ${\mathit{K}}_i$ is the diagonal matrix indicating the threat intensity at the obstacle reference point, and $d$ is the radius of the potential field action.

The potential energy function based on the reference barrier point yields the gradient function:

$$\nabla U_{req1}(p_l(t))=\left\{\begin{array}{cc}\left[\begin{array}{cc}\frac{\partial U_{req}(p_l(t))}{\partial △\mathit{x}}& \frac{\partial U_{req}(p_l(t))}{\partial △\mathit{y}}\end{array}\right]& △l\le d\\ 0& △l>d\end{array}\right.$$

(20)

In order to avoid the AUV from falling into a local optimal solution, the artificial potential field method is improved here by constructing an obstacle-assisted potential field [26]. As shown in Figure 3: the direction of the potential field force of the auxiliary obstacle is perpendicular to the direction of travel of the AUV. The angle formed with the AUV is ${\theta }_1$.

Obstacle-assisted potential energy function $U_{req2}(p_t(t))$ is designed as:

$$U_{req2}(p_t(t))=\frac{1}{2}{\mathit{g}}_{\mathit{v}}{(1+\mathit{v})}^2{\mathit{R}}_{req2}^{\mathit{v}}(\beta )$$

(21)

where, ${\mathit{g}}_{\mathit{v}}=k_o^2(1+r_n)$ is the random adjustment gain of the auxiliary potential field function. $k_o^2$ is the gain adjustment factor. $0\le r_n\le 1$, $r_n$ indicates a random number between 0 and 1. ${\mathit{R}}_{req2}^{\mathit{v}}(\beta )={[\begin{array}{cc}{\mathit{R}}_1& {\mathit{R}}_2\end{array}]}^{\mathit{T}}$ is expressed as the potential field transformation angle matrix. where, ${\mathit{R}}_1={[\begin{array}{cc}\cos (\beta )& \sin (\beta )\end{array}]}^{\mathit{T}}$, ${\mathit{R}}_2={[-\sin \begin{array}{cc}(\beta )& \cos (\beta )\end{array}]}^{\mathit{T}}$.

$\beta$ indicates the rotation angle, which is related to the speed of the AUV itself and the relative angle of the obstacle. As shown in Figure 3, at this point$0<{\theta }_1-{\theta }_2<\pi /2$. The direction of the potential field force is $\pi /2$ clockwise rotation of the current bow of AUV in the direction of velocity.

$$\beta =\left\{\begin{array}{cc}{\theta }_1+\pi /2& 0<{\theta }_1-{\theta }_2\le \pi /2\\ {\theta }_1-\pi /2& -\pi /2<{\theta }_1-{\theta }_2\le 0\\ 0& \mathrm{else}\end{array}\right.$$

(22)

The potential energy function based on the auxiliary potential field yields the gradient function:

$$\nabla U_{req2}(p_t(t))=-{\mathit{g}}_{\mathit{v}}{(1+\mathit{v})}^2{\mathit{R}}_{req2}^{\mathit{v}}(\beta ){\overrightarrow{n}}_{\mathit{v}}$$

(23)

where the unit vector of the current velocity direction of the navigator is denoted as ${\overrightarrow{n}}_{\mathit{v}}$.

The repulsion control input B${\mathit{u}}_o(t)$ is denoted as:

$${\mathit{u}}_o(t)=-\nabla U_{req1}(p_l(t))-\nabla U_{req2}(p_t(t))$$

(24)

3.2. Gravitational field design for target points.

Based on the artificial potential field method, it is known that the target point will exert a gravitational effect on the AUV formation, and the potential energy function of the target point on the current AUV is denoted as:

$$U_{att}(p_l(t))=\frac{\kappa }{{\lambda }^2}\rho (p_l(t),{\mathit{x}}_{tar})+\frac{\kappa }{\rho (p_l(t),{\mathit{x}}_{tar})+\lambda }$$

(25)

where, $\lambda ,\kappa$ denotes the positive coefficient of the potential energy function.

Gradient of the potential energy function based on the target point:

$$\nabla U_{att}(p_l(t))=\frac{\partial U_{att}(p_l(t))}{\partial \rho }{\overrightarrow{n}}_{att}$$

(26)

where，${\overrightarrow{n}}_{att}$ is the unit vector from the navigator to the target point

$$\frac{\partial U_{att}(p_l(t))}{\partial \rho }=\frac{\kappa }{{\lambda }^2}-\frac{\kappa }{{(\rho (p_l(t),{\mathit{x}}_{tar})+\lambda )}^2}$$

(27)

$${\overrightarrow{n}}_{att}=\frac{p_l(t)-{\mathit{x}}_{tar}}{‖p_l(t)-{\mathit{x}}_{tar}‖}$$

(28)

Gravity control input ${\mathit{u}}_t(t)$ is denoted as:

$${\mathit{u}}_t(t)=-\nabla U_{att}(p_l(t))$$

(29)

The potential energy gradient function of the target point can be regarded as a constant value because the target point is at a location far from the AUV, and the potential energy gradient function of the target point remains essentially constant.

3.3. Coordination and control within the formation

In order to ensure the overall robustness of the formation, make it less likely to have obvious formation changes due to the disturbance of external environment and obstacle avoidance behavior, reduce the risk of AUVs colliding with each other within the formation, and ensure the AUVs can converge stably, based on the communication structure in the formation AUVs, design the coordination control force within the formation based on the position and velocity state information of each AUV [27].

$${\mathit{u}}_{in}(t)=-\omega {\mathit{x}}_i(t)+{\mathit{K}}_{in}\left[{\mathit{K}}_p{\displaystyle \sum _{j\in {\mathit{N}}_i^{\mathit{x}}}{a}_{ij}(t)({\mathit{x}}_j-{\mathit{x}}_i)}+{\mathit{K}}_{\mathit{v}}{\displaystyle \sum _{j\in {\mathit{N}}_i^{\mathit{v}}}{b}_{ij}(t)}({\mathit{v}}_j-{\mathit{v}}_i)\right]$$

(30)

where, ${\mathit{K}}_{in}$ and $\omega$ denote the control gain of the formation coordination control input. $a_{ij}(t)$ and $b_{ij}(t)$ are the communication weights within the system. ${\mathit{K}}_p$ and ${\mathit{K}}_{\mathit{v}}$ denote the control gains in the position and velocity communication topologies, respectively.

Based on the forces on the AUV in the potential field, the control input to the AUV can be expressed as:

$$\mathit{u}(t)={\mathit{u}}_t(t)+{\mathit{u}}_o(t)+{\mathit{u}}_{in}(t)$$

(31)

3.3. Algorithm flow of cooperative obstacle avoidance

The AUV formation is controlled by the formation clustering algorithm to complete the overall movement of the formation before the threat is perceived [28]. When a threatening obstacle is encountered, the above-mentioned obstacle avoidance algorithm is used to successfully avoid the high-threatening obstacle by the multi-beam sonar sensing obstacle and the improved artificial potential field algorithm to return to the cluster formation control after getting rid of the obstacle.

The second-order integral model of AUV after feedback linearization can be expressed as:

$$\begin{array}{l}{\dot{\mathit{x}}}_i(t)={\mathit{v}}_i(t)\\ {\dot{\mathit{v}}}_i(t)={\tilde{\mathit{u}}}_i(t)\end{array}$$

(32)

The actual position and velocity state information during the AUV voyage can be expressed as ${\mathit{D}}_i(\mathit{x})={[{\mathit{x}}_i(t),{\mathit{v}}_i(t)]}^{\mathit{T}}$. The desired time-varying formation state is expressed as ${\overline{\mathit{D}}}_i(\mathit{x})={[{\overline{\mathit{x}}}_i(t),{\overline{\mathit{v}}}_i(t)]}^{\mathit{T}}$.There exists a vector $\mathit{C}(t)$ is called the central function, such that ${\lim }_{t\to \infty }({\mathit{D}}_i(\mathit{x})-{\overline{\mathit{D}}}_i(\mathit{x})-\mathit{C}(t))=0$.

The pre-command generates the initial desired formation, and the desired formation is modified by the modified artificial potential field method above when the AUV cluster system senses an obstacle. The corrected desired formation is denoted as

$$\overline{H}(\mathit{x})=\overline{\mathit{D}}(\mathit{x})+F(\mathit{x}).$$

where, $F(\mathit{x})=-\nabla U_{req}({\mathit{x}}_i(t))-\nabla U_{att}({\mathit{x}}_i(t))$.

The consistency protocol at this point is denoted as:

$${\tilde{\mathit{u}}}_i(t)=F(\mathit{x})+{\mathit{K}}_1({\mathit{D}}_i(\mathit{x})-{\overline{H}}_i(t))+{\mathit{K}}_2{\displaystyle \sum _{j\in {\mathit{N}}_i}{w}_{ij}(({\mathit{D}}_j(\mathit{x})-{\overline{H}}_j(t))-({\mathit{D}}_i(\mathit{x})-{\overline{H}}_i(t))}$$

(33)

where, ${\mathit{K}}_1={\mathit{K}}_{in}{\mathit{K}}_p$, ${\mathit{K}}_2={\mathit{K}}_{in}{\mathit{K}}_{\mathit{v}}$, $w$ indicates the coefficient of the effect of the current position on its motion state.

At this point, the AUV formation cooperative obstacle avoidance algorithm flow can be shown in Figure 4.

4. Stability analysis

The different potential field effects due to the obstacle space produce different directional forces in the formation system. It causes the AUV to produce obstacle avoidance routes in different directions [29]. In this paper, the AUV formation system will form two small subsystems. The total number of AUVs in the two subsystems is$m$，The number of AUVs in the two subsystems are $m_1$ and $m_2$ respectively, and $m_1+m_2=m$.

The stability of the system can be proved by constructing an energy function. The energy functions of the two systems are defined as $W_1(V(t),X(t))$ and $W_2(V(t),X(t))$ respectively. $V(t)$ is denoted as the velocity energy function, $X(t)$ is denoted as the position energy primary function, and $\mathit{\eta }(V(t))$ is denoted as the sum of the energies of the subsystem potential fields.

$$W_1(V_i(t),X_i(t))=V_i(t)+X_i(t)+2{\mathit{\eta }}_1(V_i(t))$$

(34)

$$W_2(V_j(t),X_j(t))=V_j(t)+X_j(t)+2{\mathit{\eta }}_2(V_j(t))$$

(35)

where, $V_i(t)={\displaystyle \sum _{i=1}^{m_1}{\mathit{v}}_i^{\mathit{T}}}{\mathit{v}}_i(t)$,$X_i(t)=\omega {\displaystyle \sum _{i=1}^{m_1}{\mathit{x}}_i^{\mathit{T}}}{\mathit{x}}_i(t)$,${\mathit{\eta }}_1(V_i(t))={\displaystyle \sum _{i=1}^{m_1}(U_{req}^i(\mathit{x}(t))+U_{att}^i(\mathit{x}(t)))}$, $V_j(t)={\displaystyle \sum _{j=1}^{m_2}{\mathit{v}}_j^{\mathit{T}}}{\mathit{v}}_j(t)$,$X_j(t)=\omega {\displaystyle \sum _{j=1}^{m_2}{\mathit{x}}_i^{\mathit{T}}}{\mathit{x}}_i(t)$,${\mathit{\eta }}_2(V_j(t))={\displaystyle \sum _{j=1}^{m_2}(U_{req}^j(\mathit{x}(t))+U_{att}^j(\mathit{x}(t)))}$.

The two subsystems are similar in the formation process and the stability proof process. In this section, only one subsystem stability proof is carried out and the conclusion is given.

Theorem 4.1: The initial energy function of the system includes the velocity energy function, the position energy function, and the sum of the energy of the potential field. Then, the initial energy function A of the system is bounded, then the formation can be transformed by autonomous formation and the following conclusion holds: in the process of formation avoidance, the splitting transformation of the formation is generated and the subsystem always keeps converging.

$${\dot{W}}_1(V(t),X(t))<0,{\dot{W}}_2(V(t),X(t))<0$$

(36)

PROOF: Derive the subsystem energy function as follows:

$${\dot{W}}_1(V(t),X(t))=2{\displaystyle \sum _{i=1}^{m_1}{\mathit{v}}_i^{\mathit{T}}(t){\dot{\mathit{v}}}_i(t)}+2\omega {\displaystyle \sum _{i=1}^{m_1}{\mathit{x}}_i^{\mathit{T}}(t){\dot{\mathit{x}}}_i(t)}+2{\dot{\mathit{\eta }}}_1(\zeta (t))$$

(37)

Substitute equation (17) into (21):

$$\begin{array}{ll}{\dot{W}}_1(V(t),X(t))=& 2{\displaystyle \sum _{i=1}^{m_1}{\mathit{v}}_i^{\mathit{T}}(t)(-{\nabla }_l{\psi }_t-{\nabla }_l{\psi }_o-\omega {\mathit{x}}_i(t))}\\ & +2{\displaystyle \sum _{i=1}^{m_1}{\mathit{v}}_i^{\mathit{T}}(t)\left({\mathit{K}}_{in}^p{\displaystyle \sum _{j\in {\mathit{N}}_i^{\mathit{x}}}{a}_{ij}}(t)({\mathit{x}}_j-{\mathit{x}}_i)+{\mathit{K}}_{in}^{\mathit{v}}{\displaystyle \sum _{j\in {\mathit{N}}_i^{\mathit{x}}}{b}_{ij}}(t)({\mathit{v}}_j-{\mathit{v}}_i)\right)}\\ & +2{\dot{\mathit{\eta }}}_1(V(t))+2\omega {\displaystyle \sum _{i=1}^{m_1}{\mathit{x}}_i^{\mathit{T}}(t){\mathit{v}}_i(t)}\end{array}$$

(38)

Since ${\mathit{v}}_i(t)$ and ${\mathit{x}}_i(t)$ are column vectors of the same dimension, there exists ${\mathit{x}}_i^{\mathit{T}}(t){\mathit{v}}_i(t)={\mathit{v}}_i^{\mathit{T}}(t){\mathit{x}}_i(t)$.

$$\begin{array}{ll}{\dot{W}}_1(V(t),X(t))=& -2{\displaystyle \sum _{i=1}^{m_1}{\mathit{v}}_i^{\mathit{T}}(t)({\nabla }_l{\psi }_t+{\nabla }_l{\psi }_o)}\\ & +2{\displaystyle \sum _{i=1}^{m_1}{\mathit{v}}_i^{\mathit{T}}(t){\mathit{K}}_{in}^p{\displaystyle \sum _{j\in {\mathit{N}}_i^{\mathit{x}}}{a}_{ij}}(t)({\mathit{x}}_j-{\mathit{x}}_i)}\\ & +2{\displaystyle \sum _{i=1}^{m_1}{\mathit{K}}_{in}^{\mathit{v}}{\displaystyle \sum _{j\in {\mathit{N}}_i^{\mathit{x}}}{b}_{ij}}(t)({\mathit{v}}_j-{\mathit{v}}_i)+2{\dot{\mathit{\eta }}}_1(\mathit{x}(t))}\end{array}$$

(39)

The derivative of the total potential energy function of the subsystem can be obtained from its definition.

$${\dot{\mathit{\eta }}}_1(V(t))={\left({\displaystyle \sum _{i=1}^{m_1}({\psi }_o^i(△l)+{\psi }_t^i}(△l))\right)}^{\prime }=X(t){\displaystyle \sum _{i=1}^{m_1}({\nabla }_l{\psi }_o^i)}+X(t){\displaystyle \sum _{i=1}^{m_1}({\nabla }_l{\psi }_t^i(△l))}$$

(40)

Substitute equation (25) into (22):

$${\dot{W}}_1(V(t),X(t))=-2X^{\mathit{T}}(t)\left({\mathit{L}}_p\otimes {\mathit{K}}_{in}^p\right)V(t)--2X^{\mathit{T}}(t)\left({\mathit{L}}_p\otimes {\mathit{K}}_{in}^{\mathit{v}}\right)X(t)$$

(41)

Lemma 4.1: When$\mathit{x}\in {ℝ}^n$,$\mathit{y}\in {ℝ}^n$,$\beta >0$, there exists the following inequality that holds.

$${\mathit{x}}^{\mathit{T}}\mathit{y}+{\mathit{y}}^{\mathit{T}}\mathit{x}\le \beta {\mathit{x}}^{\mathit{T}}\mathit{x}+{\beta }^{-1}{\mathit{y}}^{\mathit{T}}\mathit{y}$$

(42)

Based on the above lemma, the following inequalities exist

$$-2X^{\mathit{T}}(t)\left({\mathit{L}}_p\otimes {\mathit{K}}_{in}^p\right)V(t)\le -\delta X^{\mathit{T}}(t)-V^{\mathit{T}}(t){\left({\mathit{L}}_p\otimes {\mathit{K}}_{in}^p\right)}^{\mathit{T}}\left({\mathit{L}}_p\otimes {\mathit{K}}_{in}^p\right)V(t)$$

(43)

Substitute equation (26) into (28):

$${\dot{W}}_1(V(t),V(t))\le -\delta X^{\mathit{T}}(t)X(t)-V^{\mathit{T}}(t)\mathsf{\Theta }V(t)$$

(44)

where, $\mathsf{\Theta }={\left({\mathit{L}}_p\otimes {\mathit{K}}_{in}^p\right)}^{\mathit{T}}\left({\mathit{L}}_p\otimes {\mathit{K}}_{in}^p\right)+2\left({\mathit{L}}_p\otimes {\mathit{K}}_{in}^{\mathit{v}}\right)$.

According to the properties of the Laplacian matrix, it must be a vivacious semi definite matrix. From this, it can be deduced that the formation must be able to converge stably.

5. Simulation verification and analysis

The following simulation example verifies the effectiveness of the research on the multi-AUV coordinated obstacle avoidance method based on the improved artificial potential field method and the consistency protocol algorithm.

The number of AUVs in the formation is 4. The initial position status information of each AUV are${\mathit{x}}_i\in [0,50]$、${\mathit{y}}_i\in [0,140]$、$i\in \left\{1,2,3,4\right\}$.The initial velocity of AUV are 0. Probabilistic Threat Intensity Diagonal Matrix${\mathit{K}}_i$is $0.002{\mathit{I}}_{3\times 3}$. Potential field coefficient$\gamma =1,\kappa =0.1,\lambda =1$. Obstacles are randomly distributed in the map. The target points for the cooperative operation of the AUV formation are$[300,{\mathit{y}}_i,-5]$.After reaching consistency, the distance between the formation AUVs is maintained at 10m.

Figure 5 gives the paths of the formation AUVs in the process of coordinated obstacle avoidance. The formation AUVs feel different threat levels and decelerate differently after entering the potential field area, but each keeps relative distance between the formations and no collision behavior occurs, and finally the formation finally achieves consistency according to the consistency protocol.

Figure 6, Figure 7 and Figure 8 show the changes of position attitude and each velocity state during the formation dissolution, formation coordinated obstacle avoidance process and formation reconfiguration process of multi-AUV in the face of obstacles, respectively. From Figure 6, it can be seen that the spatial position relationship of the AUV formation in the formation system can still converge after passing through the potential field region and generating the separation of obstacle avoidance actions. In Figure 7 and Figure 8, the motion states of each AUV similarly converge-separate-converge as the potential field appears.

Comprehensive simulation results above can be judged that the algorithm based on the improved artificial potential field method and consistency protocol proposed in this chapter can effectively solve the problem of multi-AUV coordinated obstacle avoidance, and ensure the convergence and stability of the formation while ensuring the formation avoidance of obstacles and formation change.

6. Conclusions

This paper focuses on the local obstacle avoidance problem of AUV formation and proposes a cooperative obstacle avoidance algorithm based on an improved artificial potential field method and a consistency protocol. Firstly, for the disadvantage that the traditional artificial potential field method is easy to fall into local minima, an auxiliary potential field perpendicular to the AUV moving direction is designed to solve the problem that AUVs are easy to have zero combined force in the potential field and local minima. Secondly, the control law of AUV formation that keeps the speed and position consistent is designed for the problem that the formation will change during the local obstacle avoidance of the formation system. The control conflict problem of the combined algorithm of the artificial potential field law and the consistency protocol is solved by adjusting the desired formation of the consistency protocol through the potential field force. Finally, the stability of system convergence is demonstrated by bounded energy function. Through simulation verification, it is confirmed that the AUV formation can achieve convergence of the formation state under local obstacle avoidance. The effectiveness of the proposed algorithm is confirmed.