Adaptive Formation Control of Multiple Underactuated Autonomous Underwater Vehicles

Abstract

In this paper, we present a 3D formation control scheme for a group of torpedo-type underactuated autonomous underwater vehicles (AUVs). These multiple AUVs combined with an unmanned surface vessel (USV) construct a sort of star-topology acoustic communication network where the USV is at the center point. Due to this kind of topological feature, this paper applies a virtual school concept. This is a geometric graph where each node is taken as a virtual leader for each specific AUV and assigned its own reference trajectory. For each individual vehicle, its formation strategy is simple: just follow the trajectory of its corresponding virtual leader so as for multiple AUVs to compose the given formation. As for the formation subject, this paper mainly focuses on the formation tracking problem rather than the formation producing. For the torpedo-type vehicle considered in this paper, there are only three control inputs (surge force, pitch, and yaw moments) available for its underwater 3D motion and therefore this is a typical underactuated system. For the following vehicle’s trajectory, a sort of potential field method is used for obstacle avoidance, and a neural network-based adaptive scheme is applied to on-line approximate the vehicle’s unknown nonlinear dynamics, and the uncertainty terms including modeling errors, measurement noises, and external disturbances are handled by the properly designed robust scheme. The proposed formation method can guarantee the uniform ultimate boundedness (UUB) of the closed-loop system. Numerical studies are also carried out to verify the effectiveness of the proposed scheme.

1. Introduction

Because of their huge potential in emerging practical applications, the multi-agent system has become one of the most interesting research issues in the past decades. Among the various subjects within the realm of these multi-agent systems, such as consensus, task assignment, estimation, control, etc. [1], formation control has been one of the most intensively studied topics [2,3].

Formation control refers to the behavior that forces the agents to form certain prescribed geometrical configurations. According to the fundamental ideas of the coordination of multiple agents, formation control can be roughly classified into leader-following, behavioral, and virtual structure approaches [4,5]; whether there is a group reference or not, it also can be grouped into formation producing and formation tracking problems [1]; depending on the sensing capability and the interaction topology of agents, formation control problems also can be categorized as position-, displacement-, and distance-based schemes [3].

For underwater vehicles, the most crucial technical issues are underwater communication and positioning. Recently, thanks to the evolution of underwater acoustic signal processing technology, various commercial acoustic modems, which can provide both tracking and communication functions simultaneously, are available in the market. However, for both of transceiver and transponder, the signal transmission is still considered to be directional even in their omnidirectional modes (180 degrees of beam angle). Therefore, these modems are not suitable for communications between underwater vehicles. Instead, they are optimized for communication links between the underwater vehicles and the surface vessel, where the transceiver is mounted on the bottom of the vessel and the transponder on the top of the vehicle. Under this consideration, for the schooling of multiple underwater vehicles, it is better to introduce a surface vessel and construct a sort of star-topology network, where the vessel is located in the center point, as seen in Figure 1. In this case, there are no direct communication channels between vehicles. Indeed, from the technical point of view, it is difficult to construct a seamless acoustic communication network directly between the vehicles. Due to this kind of star-topology communication network feature, it is convenient to apply the virtual structure concept [4] to construct the formation of multiple underwater vehicles. Under this consideration, this paper proposes a virtual school concept which is similar to the virtual structure [4] except that each node in virtual school is taken as a virtual leader for each of specific vehicle and assigned its own reference trajectory. This virtual school is designed on the support vessel according to the given missions and, through USV which is in this case a communication hub, the virtual leaders’ reference trajectories are transmitted to the each of corresponding underwater vehicles. Coverage path planning (CPP) algorithm [6] is one of the most common methods to design this kind of virtual structure. Here the details of how to design this CPP algorithm are out of the scope of this paper. For each vehicle, the formation strategy becomes simple: just follow its corresponding reference trajectory so as to realize the underwater formation or school.

As for the formation subject, in this paper we mainly focus on the formation tracking problem rather than the formation producing [1]. Considering the related previous works, at first, each dynamic agent was modeled as a particle system [7,8], and this kind of simple linear model was gradually relaxed and extended to the nonlinear cases [9,10]. Especially, formation control for multiple underactuated nonlinear agents has been one of the most intense research areas in the past decade [11,12,13,14,15,16]. In these works, however, only 2D formation problems were considered; even in the case of underwater vehicles, the authors only considered 2D horizontal cases. In [17], the authors proposed a 3D formation scheme for multiple underactuated underwater vehicles under the assumption that a certain control gain matrix was always invertible. However, in practice, it is difficult to guarantee the consistency of this assumption. In [18], two spherical coordinates transformations were applied to transform each vehicle’s trajectory following the model into a three-input-three-output strict-feedback form, for which general backstepping method [19] can be used to solve the control problem. Unfortunately, none of these works [17,18] considered the uncertainty terms in the vehicle’s dynamics. In this paper, we extend the authors’ previous work [18] to the case where the vehicles’ dynamics include both structured and unstructured uncertainty terms [20]. For the structured uncertainties, a sort of neural network-based adaptive scheme is applied for their online approximations; and the unstructured terms including modeling errors, measurement noises, and external disturbances are handled through properly designed robust schemes. In addition, the formation rules including the obstacles detection and modeling in [18] are upgraded in this paper. The proposed formation scheme in this paper can guarantee the UUB of the closed-loop system.

The remainder of this paper is organized as follows. Some preliminaries including the spherical coordinate transformation and nonlinear function approximation problems are discussed in Section 2, while the formation rules are described in Section 3. Section 4 presents the proposed formation control method. In order to verify the effectiveness of the proposed scheme, some numerical studies are carried out in Section 5, and finally, a brief conclusion and future works are presented in Section 6.

2. Preliminaries

2.1. Vehicles Kinematics and Dynamics

In practice, most underwater vehicles are designed so as for their gravity centers to be (much) lower than the buoyancy centers, and this kind of structural feature can guarantee the vehicles’ roll dynamics stability in their relatively slow speed motions. For this reason, in this paper each vehicle’s kinematics and dynamics are presented as the following 5DOF form [18,21,22],

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\dot{x}}_i\\ {\dot{y}}_i\\ {\dot{z}}_i\\ {\dot{\theta }}_i\\ {\dot{\psi }}_i\end{array}\right]& =\left[\begin{array}{ccccc}cos{\theta }_{i}cos{\psi }_i& -sin{\psi }_i& sin{\theta }_{i}cos{\psi }_i& 0& 0\\ cos{\theta }_{i}sin{\psi }_i& cos{\psi }_i& sin{\theta }_{i}sin{\psi }_i& 0& 0\\ -sin{\theta }_i& 0& cos{\theta }_i& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1/cos{\theta }_i\end{array}\right]\left[\begin{array}{c}{u}_i\\ v_i\\ w_i\\ q_i\\ r_i\end{array}\right],\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\dot{u}}_i\\ {\dot{v}}_i\\ {\dot{w}}_i\\ {\dot{q}}_i\\ {\dot{r}}_i\end{array}\right]& =\left[\begin{array}{c}{f}_{ui}\\ f_{vi}\\ f_{wi}\\ f_{qi}\\ f_{ri}\end{array}\right]+\left[\begin{array}{ccc}{g}_{ui}& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& g_{qi}& 0\\ 0& 0& q_{ri}\end{array}\right]\left[\begin{array}{c}{\tau }_{ui}\\ {\tau }_{qi}\\ {\tau }_{ri}\end{array}\right]+\left[\begin{array}{c}{d}_{ui}^{\prime }\\ d_{vi}^{\prime }\\ d_{wi}^{\prime }\\ d_{qi}^{\prime }\\ d_{ri}^{\prime }\end{array}\right],\hfill \end{array}$$

(2)

where $(x_i,y_i,z_i)$ is the ith vehicle’s position in the navigation frame and ${\theta }_i$ and ${\psi }_i$ are corresponding pitch and yaw angles; $u_i,v_i$, and $w_i$ are linear velocities in the body-fixed frame, and $q_i$ and $r_i$ are pitch and yaw angular rates in the inertial frame; $f_{ai}(·)\in C^2$ with $a\in \{u,v,w,q,r\}$ indicates the ith vehicle’s nonlinear hydrodynamics including damping, inertial, Coriolis, and gravitational terms each in the surge, sway, heave, pitch, and yaw directions; surge force ${\tau }_{ui}$, pitch moment ${\tau }_{qi}$, and yaw moment ${\tau }_{ri}$ are three only available control inputs with $g_{ui}$, $g_{qi}$, and $g_{ri}$ the known nonzero control gains; $d_{ai}^{\prime }$ indicate the uncertainty terms including measurements noise and external disturbances.

For torpedo-type vehicles, to fully excite their pitch and yaw dynamics, the vehicles have to possess considerable forward speed. Also, from (1), it is easy to see that if ${\theta }_i=\pi /2$, then a certain singularity problem might occur. Under these considerations, this paper makes the following assumption about the vehicles’ motions.

Assumption 1.

In (1) and (2), $u_i>0$ and ${\theta }_i\in (-\pi /2,\pi /2)$.

2.2. Spherical Coordinates $(u_{li},{\theta }_{li},{\psi }_{li})$

Here we introduce a new velocity parameter $u_{li}=\sqrt{u_i^2+v_i^2+w_i^2}$ by which the vehicle’s linear velocity dynamics can be integrated as one as following

$$\begin{array}{cc}\hfill {\dot{u}}_{li}& ={\dot{u}}_{i}cos{\theta }_{ai}cos{\psi }_{ai}+{\dot{v}}_{i}sin{\psi }_{ai}+{\dot{w}}_{i}sin{\theta }_{ai}cos{\psi }_{ai}\hfill \\ \hfill & =f_{uli}+g_{aui}{\tau }_{ui}+d_{uli},\hfill \end{array}$$

(3)

where

$$\begin{array}{cc}\hfill {\theta }_{ai}& =atan(-w_i/u_i),{\psi }_{ai}=atan\left(v_i/\sqrt{u_i^2+w_i^2}\right),\hfill \\ \hfill f_{uli}& =f_{ui}cos{\theta }_{ai}cos{\psi }_{ai}+f_{vi}sin{\psi }_{ai}+f_{wi}sin{\theta }_{ai}cos{\psi }_{ai},\hfill \\ \hfill g_{aui}& =g_{ui}cos{\theta }_{ai}cos{\psi }_{ai},d_{uli}^{\prime }=d_{ui}^{\prime }cos{\theta }_{ai}cos{\psi }_{ai}+d_{vi}^{\prime }sin{\psi }_{ai}+d_{wi}^{\prime }sin{\theta }_{ai}cos{\psi }_{ai}.\hfill \end{array}$$

Remark 1.

Since $u_i>0$, both of ${\theta }_{ai}$ and ${\psi }_{ai}$ are well defined in $(-\pi /2,\pi /2)$ and $g_{aui}>0$.

Corresponding to the new velocity parameter $u_{li}$, its polar and azimuth angles ${\theta }_{li}$ and ${\psi }_{li}$ can be expressed as follows:

$$\begin{array}{cc}\hfill {\theta }_{li}& =asin\left[cos{\psi }_{ai}sin({\theta }_i-{\theta }_{ai})\right],\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill {\psi }_{li}& ={\psi }_i+atan\left[tan{\psi }_{ai}sec({\theta }_i-{\theta }_{ai})\right].\hfill \end{array}$$

(5)

Remark 2.

The mathematical definitions of ${\theta }_{li}$ and ${\psi }_{li}$ as in the authors’ previous works (Equation (3) in [18] and as Equations (3) and (4) in [22]) should be corrected as (4) and (5).

Using the spherical coordinates $(u_{li},{\theta }_{li},{\psi }_{li})$, the ith vehicle’s kinematics and dynamics can be rewritten as following

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\dot{x}}_i\\ {\dot{y}}_i\\ {\dot{z}}_i\\ {\dot{\theta }}_{li}\\ {\dot{\psi }}_{li}\end{array}\right]& =\left[\begin{array}{c}0\\ 0\\ 0\\ f_{{\theta }_{li}}\\ f_{{\psi }_{li}}\end{array}\right]+\left[\begin{array}{ccc}cos{\theta }_{li}cos{\psi }_{li}& 0& 0\\ cos{\theta }_{li}sin{\psi }_{li}& 0& 0\\ -sin{\theta }_{li}& 0& 0\\ 0& g_{{\theta }_{li}}& 0\\ 0& 0& g_{{\psi }_{li}}\end{array}\right]\left[\begin{array}{c}{u}_{li}\\ q_i\\ r_i\end{array}\right],\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\dot{u}}_{li}\\ {\dot{q}}_i\\ {\dot{r}}_i\end{array}\right]& =\left[\begin{array}{c}{f}_{uli}\\ f_{qi}\\ f_{ri}\end{array}\right]+\left[\begin{array}{ccc}{g}_{aui}& 0& 0\\ 0& g_{qi}& 0\\ 0& 0& g_{ri}\end{array}\right]\left[\begin{array}{c}{\tau }_{ui}\\ {\tau }_{qi}\\ {\tau }_{ri}\end{array}\right]+\left[\begin{array}{c}{d}_{uli}^{\prime }\\ d_{qi}^{\prime }\\ d_{ri}^{\prime }\end{array}\right],\hfill \end{array}$$

(7)

where

$$\begin{array}{cc}\hfill f_{{\theta }_{li}}& =-\frac{{\dot{\theta }}_{ai}cos{\psi }_{ai}cos({\theta }_i-{\theta }_{ai})+{\dot{\psi }}_{ai}sin{\psi }_{ai}sin({\theta }_i-{\theta }_{ai})}{\sqrt{1-co{s}^2{\psi }_{ai}si{n}^2({\theta }_i-{\theta }_{ai})}},\hfill \\ \hfill g_{{\theta }_{li}}& =\frac{cos{\psi }_{ai}cos({\theta }_i-{\theta }_{ai})}{\sqrt{1-co{s}^2{\psi }_{ai}si{n}^2({\theta }_i-{\theta }_{ai})}},\hfill \\ \hfill f_{{\psi }_{li}}& =\frac{{\dot{\psi }}_{ai}cos({\theta }_i-{\theta }_{ai})+(q_i-{\dot{\theta }}_{ai})sin{\psi }_{ai}cos{\psi }_{ai}sin({\theta }_i-{\theta }_{ai})}{1-co{s}^2{\psi }_{ai}si{n}^2({\theta }_i-{\theta }_{ai})},\hfill \\ \hfill g_{{\psi }_{li}}& =sec{\theta }_i.\hfill \end{array}$$

Remark 3.

Consider $g_{{\theta }_{li}}$ in (6). If ${\psi }_{ai}=0$, then we have $g_{{\theta }_{li}}=1$; else, combined with ${\psi }_{ai}\in (-\pi /2,\pi /2)$, we can get $co{s}^2{\psi }_{ai}si{n}^2({\theta }_i-{\theta }_{ai})<1$. Consequently, we have $g_{{\theta }_{li}}\ne 0$. In other words, $g_{{\theta }_{li}}$ is always invertible. On the other hand, according to Assumption 1, we can get that $g_{{\psi }_{li}}$ is also nonzero and therefore is always invertible.

2.3. Nonlinear Dynamics Approximation

For any given continuous function $f\left(x\right)$, it always can be expressed in the following parametric form [23,24]

$$f\left(x\right)=W^{*T}{\varphi }^{*}\left(x\right),$$

(8)

where $W^{*}\in {\Re }^{N^{*}}$ is a constant vector and ${\varphi }^{*}\left(x\right)\in {\Re }^{N^{*}}$ is the basis function vector of $f\left(x\right)$. If ${\varphi }^{*}\left(x\right)$ can be exactly known in advance, then the functional approximation problem can be transferred to a well-known parameters estimation problem. However in practice, especially for the highly nonlinear dynamics as in (2), it is difficult, if not impossible, to exactly know their basis function vectors in advance. Therefore, in practice, it is reasonable for (8) to be rewritten as the following form [23,24]

$$f\left(x\right)=W^T\varphi \left(x\right)+\epsilon \left(x\right),$$

(9)

where $\varphi \left(x\right)\in {\Re }^N$ is the constructed basis function vector and $W\in {\Re }^N$ is the corresponding constant coefficients vector with $N\le N^{*}$.

Using (9), the vehicle’s dynamics (7) can be rewritten as

$$\left[\begin{array}{c}{\dot{u}}_{li}\\ {\dot{q}}_i\\ {\dot{r}}_i\end{array}\right]=\left[\begin{array}{c}{W}_{uli}^T{\varphi }_{uli}\\ W_{qi}^T{\varphi }_{qi}\\ W_{ri}^T{\varphi }_{ri}\end{array}\right]+\left[\begin{array}{ccc}{g}_{aui}& 0& 0\\ 0& g_{qi}& 0\\ 0& 0& q_{ri}\end{array}\right]\left[\begin{array}{c}{\tau }_{ui}\\ {\tau }_{qi}\\ {\tau }_{ri}\end{array}\right]+\left[\begin{array}{c}{d}_{uli}\\ d_{qi}\\ d_{ri}\end{array}\right],$$

(10)

where ${\varphi }_{ai}\in {\Re }^{N_a}$ is the constructed basis function vector for $f_{ai}$ and $W_{ai}\in {\Re }^{N_a}$ is the corresponding coefficients vector, and $d_{ai}=d_{ai}^{\prime }+{\epsilon }_{ai}$. Here $a\in \{u_l,q,r\}$.

In practice it is difficult to exactly derive the constant vectors $W_{ai}$ with $a\in \{u_l,,q,r\}$ in advance. Therefore, in this paper, we make the following assumption.

Assumption 2.

In (10), the constructed basis function vectors ${\varphi }_{ai}$ are known in advance and $W_{ai}$ are unknown constant vectors with $a\in \{u_l,q,r\}$.

Assumption 3.

The uncertainty terms in (10) are bounded such that $|d_{uli}|\le d_{uM}$, $|d_{qi}|\le d_{qM}$, and $|d_{ri}|\le d_{rM}$ with $d_{uM},d_{qM},d_{rM}>0$ known constants.

3. Formation Rules

As mentioned before, in this paper we consider the formation control problem for a group of n torpedo-type underwater vehicles, all of which have the same kinematics and dynamics as (1) and (2). A surface vessel, which is equipped with a fully integrated underwater multiple vehicles simultaneous tracking and communication modem, is introduced so as to construct a sort of star-topological acoustic communication network with the vessel at the center point. According to this kind of network’s structural feature, as mentioned before in this paper we introduce a virtual school concept that is similar to the virtual structure [4] except that each node in this virtual school is taken as a virtual leader for each of specific underwater vehicle and assigned its own reference trajectory. Usually, in practice, the geometrical structure and the reference trajectories for each of the nodes in the virtual school are designed on the support vessel for the purpose to accomplish the given missions. To construct the designed formation, the strategy in this paper is simple, in that each vehicle is simply steered to follow the reference trajectory assigned to its virtual leader while avoiding possible obstacles.

3.1. Virtual School

In practice, given a mission—for example, to complete the survey on a given region using multiple underwater vehicles with the shortest search time, it is convenient to apply CPP (coverage path planning) algorithms to carry out the mission. In this case, each reference trajectory for each of the nodes (virtual leaders) in the virtual school can be designed through a properly designed CPP algorithm [6,25] with appropriate criteria such as minimizing the search time or energy consumption. The geometric structure of the virtual school can be determined by the specifications of the survey devices such as the side scan sonar or multi-beam sonar as in [26]. As mentioned before, the details of how to design this virtual school are out of the scope of this paper and could be considered in our future works.

After the design procedure is completed on the surface vessel, the virtual school information, indeed the exact reference trajectories $p_{di}\left(t\right)=(x_{di}\left(t\right),y_{di}\left(t\right),z_{di}\left(t\right)),i=1,\cdots ,n$ for each of virtual leaders, is transmitted to each of the corresponding underwater vehicles through the acoustic communication modem mounted on the USV. Here $p_{di}\left(t\right)$ can be a smooth function or a series of waypoints which can be considered as a piecewise smooth function. For each vehicle, the formation strategy is simple: just to follow its reference trajectory while avoiding possible obstacles, and in doing so the underwater formation can be constructed, see Figure 2.

3.2. Trajectory Following

For the ith vehicle, the formation control objective as mentioned before is just to follow its reference trajectory $p_{di}\left(t\right)$. Considering two kinematics (1) and (6), it is easy to see that for the ith vehicle’s any given trajectory $p_i\left(t\right)=(x_i\left(t\right),y_i\left(t\right),z_i\left(t\right))$, it can be realized by $(u_i\left(t\right),v_i\left(t\right),w_i\left(t\right),{\theta }_i\left(t\right),{\psi }_i\left(t\right))$ through the position kinematics in (1), or also simply by the spherical coordinates $(u_{li}\left(t\right),{\theta }_{li}\left(t\right),{\psi }_{li}\left(t\right))$ using (6). In this paper, we apply the latter method.

For the ith vehicle’s given reference trajectory $p_{di}\left(t\right)=(x_{di}\left(t\right),y_{di}\left(t\right),z_{di}\left(t\right))$ combined with $({\theta }_{ldi},{\psi }_{ldi})$ where the kinematics is taken as (6), the position error is defined as following

$$p_{ei}\left(t\right)=\left|\right|p_{di}-p_i{\left|\right|}_2=\sqrt{{(x_{di}\left(t\right)-x_i\left(t\right))}^2+{(y_{di}\left(t\right)-y_i\left(t\right))}^2+{(z_{di}\left(t\right)-z_i\left(t\right))}^2},$$

(11)

whose time derivative further can be expanded as

$$\begin{array}{cc}\hfill {\dot{p}}_{ei}& =u_{ldi}\left[cos{\theta }_{ldi}cos{\theta }_{bi}cos({\psi }_{ldi}-{\psi }_{bi})+sin{\theta }_{ldi}sin{\theta }_{bi}\right]-u_{li}\left[cos{\theta }_{li}cos{\theta }_{bi}cos({\psi }_{li}-{\psi }_{bi})+sin{\theta }_{li}sin{\theta }_{bi}\right]\hfill \\ \hfill & =u_{ldi}{A}_{ldi}-u_{li}{A}_{li},\hfill \end{array}$$

(12)

where ${\theta }_{bi}$ and ${\psi }_{bi}$ denote the polar and azimuth angles for the position error vector ${\overrightarrow{p}}_{ei}$ and are defined as follows

$$\begin{array}{cc}\hfill {\theta }_{bi}& =atan\left(\frac{-(z_{di}-z_i)}{\sqrt{{(x_{di}-x_i)}^2+{(y_{di}-y_i)}^2}}\right),\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {\psi }_{bi}& =atan2(y_{di}-y_i,x_{di}-x_i).\hfill \end{array}$$

(14)

For the ith vehicle, the control objective of trajectory following in this paper is defined as follows.

Trajectory following objective: For the ith vehicle’s given reference trajectory $(p_{di}\left(t\right),{\theta }_{ldi}\left(t\right),{\psi }_{ldi}\left(t\right))$, the control objective is to steer the vehicle so as for $(p_{ei}\left(t\right),{\theta }_{li}\left(t\right),{\psi }_{li}\left(t\right))\to (c_i,{\theta }_{bi}\left(t\right),{\psi }_{bi}\left(t\right))$ where $c_i>0$ is a constant design parameter, see Figure 2.

Remark 4.

For the ith vehicle, since $({\theta }_{li}\left(t\right),{\psi }_{li}\left(t\right))\to ({\theta }_{bi}\left(t\right),{\psi }_{bi}\left(t\right))$, its control strategy is clear that to force the vehicle to always face to the target point $(x_{di}\left(t\right),y_{di}\left(t\right),z_{di}\left(t\right))$ while keeping $p_{ei}\left(t\right)\to c_i$. This is why we call it trajectory following, which is different from the traditional trajectory tracking where the control objective is to $(p_i\left(t\right),{\theta }_{li}\left(t\right),{\psi }_{li}\left(t\right))\to (p_{di}\left(t\right),{\theta }_{ldi}\left(t\right),{\psi }_{ldi}\left(t\right))$ where ${\theta }_{ldi}\left(t\right)$ and ${\psi }_{ldi}\left(t\right)$ are predefined.

Consequently, using the spherical coordinates $(u_{li},{\theta }_{li},{\psi }_{li})$ and $(p_{ei},{\theta }_{bi},{\psi }_{bi})$, the ith vehicle’s trajectory following model can be expressed as the following three-input-three-output strict-feedback form,

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\dot{p}}_{ei}\\ {\dot{\theta }}_{lei}\\ {\dot{\psi }}_{lei}\end{array}\right]& =\left[\begin{array}{c}{u}_{ldi}{A}_{ldi}\\ {\dot{\theta }}_{bi}-f_{{\theta }_{li}}\\ {\dot{\psi }}_{bi}-f_{{\psi }_{li}}\end{array}\right]-\left[\begin{array}{ccc}{A}_{li}& 0& 0\\ 0& g_{{\theta }_{li}}& 0\\ 0& 0& g_{{\psi }_{li}}\end{array}\right]\left[\begin{array}{c}{u}_{li}\\ q_i\\ r_i\end{array}\right],\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\dot{u}}_{li}\\ {\dot{q}}_i\\ {\dot{r}}_i\end{array}\right]& =\left[\begin{array}{c}{W}_{uli}^T{\varphi }_{uli}\\ W_{qi}^T{\varphi }_{qi}\\ W_{ri}^T{\varphi }_{ri}\end{array}\right]+\left[\begin{array}{ccc}{g}_{aui}& 0& 0\\ 0& g_{qi}& 0\\ 0& 0& g_{ri}\end{array}\right]\left[\begin{array}{c}{\tau }_{ui}\\ {\tau }_{qi}\\ {\tau }_{ri}\end{array}\right]+\left[\begin{array}{c}{d}_{uli}\\ d_{qi}\\ d_{ri}\end{array}\right],\hfill \end{array}$$

(16)

where ${\theta }_{lei}={\theta }_{bi}-{\theta }_{li}$ and ${\psi }_{lei}={\psi }_{bi}-{\psi }_{li}$.

In the case ${\theta }_{lei}={\psi }_{lei}=0$, (12) becomes ${\dot{p}}_{ei}=u_{ldi}-u_{li}$ for which we have the following proposition.

Proposition 1

([18]). If the stabilizing function [19] for virtual input $u_{li}$ is taken as

$$u_{li}=u_{li}^R=u_{ldi}{A}_{ldi}+k_p(p_{ei}-c_i),$$

(17)

where $k_p>0$ is design parameter, then it can guarantee the exponential stability of $p_{ei}\to c_i$.

Proof. 

Refer to [18]. □

3.3. Obstacle Detection and Avoidance

For each vehicle, there is a forward-looking sonar mounted in front of it and used to detect obstacles. In [11,18], each detected obstacle block is modeled as the nearest point from the sonar head. Though it is simple, this kind of obstacle modeling method might cause some practical problems such as infinite wall following [27]. For this reason, this paper applies the occupancy grid map [28] to model the detected obstacles. At each time $t>0$, using the sonar measurements (multi-beam range measurements or cloud points corresponding to its current ping), we can update the related grids in the map as seen in Figure 3, and consequently, with the series of sonar ping measurements, we can construct the occupancy grid map. In the case of the occupancy grid map, it is convenient to apply the potential function method [29] to guide the vehicle to avoid obstacles. In this paper, we apply the following smooth potential function [8,11]

$$f_p(\zeta ,a)=c_o{\int }_a^{\zeta }\left(\frac{1}{\tau }-\frac{a}{{\tau }^2}\right)\rho \left(\frac{\tau }{a}\right)d\tau ,$$

(18)

where $c_o>0$ is a constant and $\rho (·)$ is a smooth bump function taken as following form

$$\rho \left(\xi \right)=\left\{\begin{array}{cc}1,\hfill & \xi \in [0,h)\hfill \\ e^{-\frac{{(\xi -h)}^2}{{(\xi -1)}^2}},\hfill & \xi \in [h,1]\hfill \\ 0,\hfill & otherwise\hfill \end{array}\right.$$

(19)

where $h\in (0,1)$ is a design parameter. It is easy to see that $f_p(\zeta ,a)$ is a monotone decreasing function and if $\zeta \ge a$, then $f_p(\zeta ,a)=0$.

Remark 5.

In practice, each vehicle can have its own occupancy grid map to avoid obstacles, and the grid size (or map accuracy), as well as the map size, can be adjusted according to the different mission tasks and situations.

Remark 6.

It is well known that the potential function method cannot always guarantee the global minimum in the path search [29]. In the case where the vehicle is trapped in a complicated obstacle environment, how to escape from this local minimum has been a challenging task in the past decades [30] and is also out of the scope of this paper. For the convenience of discussion, in this paper we only consider the case where the potential function method can guarantee the global minimum of path searching.

4. Formation Controller Design

Now consider the ith vehicle’s trajectory following kinematics and dynamics of (15) and (16). For this three-input-three-output strict-feedback form, it is convenient to apply the general backstepping method [19] combined with the Lyapunov direct method [20] to solve the formation control problem.

Step 1. The formation strategy in this paper is to steer the vehicle $(p_i\left(t\right),{\theta }_{li}\left(t\right),{\psi }_{li}\left(t\right))$ to follow its own virtual leader $(p_{di}\left(t\right),{\theta }_{ldi}\left(t\right),{\psi }_{ldi}\left(t\right))$ so as for $p_{ei}\left(t\right)\to c_i$ and $({\theta }_{li}\left(t\right),{\psi }_{li}\left(t\right))\to ({\theta }_{bi}\left(t\right),{\psi }_{bi}\left(t\right))$. For this reason, in this step, we consider the following Lyapunov function candidate

$$V_1=\sum _{i=1}^n\left[{(p_{ei}-c_i)}^2+{\gamma }_{\theta }{\theta }_{lei}^2+{\gamma }_{\psi }{\psi }_{lei}^2+{\gamma }_{OB}\sum _{k=1}^m{f}_p(p_{eik},a)\right],$$

(20)

where ${\gamma }_{\theta },{\gamma }_{\psi },{\gamma }_{OB}>0$ are weighting factors, and the smooth potential function $f_p(·)$ is defined as (18) with (19), and $p_{eik}=\left|\right|p_i-p_{ik}{\left|\right|}_2$ where $\left|\right|·{\left|\right|}_2$ denotes the Euclidean norm and $p_{ik}$ indicates the center point of the kth grid in the ith vehicle’s occupancy grid map where the assigned value to kth grid is larger than certain pre-defined threshold.

By differentiating the above equation and substituting (15) into it, we can get

$$\begin{array}{cc}\hfill {\dot{V}}_1& =\sum _{i=1}^n\left\{(p_{ei}-c_i)(u_{ldi}{A}_{ldi}-A_{li}{u}_{li})+{\gamma }_{\theta }{\theta }_{lei}({\dot{\theta }}_{bi}-f_{{\theta }_{li}}-g_{{\theta }_{li}}{q}_i)+{\gamma }_{\psi }{\psi }_{lei}({\dot{\psi }}_{li}-f_{{\psi }_{li}}-g_{{\psi }_{li}}{r}_i)\right.\hfill \\ \hfill & \left.+{\gamma }_{OB}\sum _{k=1}^m\frac{\partial f_p(p_{eik},a)}{\partial p_{eik}}\left[\frac{x_{ik}-x_i}{p_{eik}}({\dot{x}}_{ik}-{\dot{x}}_i)+\frac{y_{ik}-y_i}{p_{eik}}({\dot{y}}_{ik}-{\dot{y}}_i)+\frac{z_{ik}-z_i}{p_{eik}}({\dot{z}}_{ik}-{\dot{z}}_i)\right]\right\}\hfill \\ \hfill & =\sum _{i=1}^n\left\{(p_{ei}-c_i)\left\{u_{ldi}{A}_{ldi}-u_{li}^R+u_{li}^R-u_{li}-u_{li}\left[cos{\theta }_{li}cos{\theta }_{bi}(cos{\psi }_{lei}-1)+(cos{\theta }_{lei}-1)\right]\right\}\right.\hfill \\ \hfill & +{\gamma }_{\theta }{\theta }_{lei}({\dot{\theta }}_{bi}-f_{{\theta }_{li}}-g_{{\theta }_{li}}{q}_i)+{\gamma }_{\psi }{\psi }_{lei}({\dot{\psi }}_{li}-f_{{\psi }_{li}}-g_{{\psi }_{li}}{r}_i)\hfill \\ \hfill & +{\gamma }_{OB}\left[B_{xi}\left(u_{li}^{R}cos{\theta }_{bi}cos{\psi }_{bi}-u_{li}cos{\theta }_{li}cos{\psi }_{li}\right)+B_{yi}\left(u_{li}^{R}cos{\theta }_{bi}sin{\psi }_{bi}-u_{li}cos{\theta }_{li}sin{\psi }_{li}\right)\right.\hfill \\ \hfill & \left.\left.-B_{zi}\left(u_{li}^{R}sin{\theta }_{bi}-u_{li}sin{\theta }_{li}\right)\right]\right\}+{\mathsf{\Lambda }}_{OB},\hfill \end{array}$$

(21)

where

$$\begin{array}{cc}\hfill B_{\zeta i}& =\sum _{k=1}^m\frac{\partial f_p}{\partial p_{eik}}\frac{{\zeta }_{eik}}{p_{eik}},\mathrm{with}\zeta \in \{x,y,z\}\hfill \\ \hfill {\mathsf{\Lambda }}_{OB}& ={\gamma }_{OB}\sum _{i=1}^n\left[B_{xi}\left({\dot{x}}_{ik}-u_{li}^{R}cos{\theta }_{bi}cos{\psi }_{li}\right)+B_{yi}\left({\dot{y}}_{ik}-u_{li}^{R}cos{\theta }_{bi}sin{\psi }_{li}\right)+B_{zi}\left({\dot{z}}_{ik}+u_{li}^{R}sin{\theta }_{bi}\right)\right].\hfill \end{array}$$

As aforementioned, if we choose the stabilization function $u_{li}^R$ for virtual input $u_{li}$ as (17), and let ${\alpha }_{qi}$ and ${\alpha }_{ri}$ denote the stabilization functions for each of virtual inputs $q_i$ and $r_i$ with $e_{qi}={\alpha }_{qi}-q_i$ and $e_{ri}={\alpha }_{ri}-r_i$, then (21) can be further expanding as following

$$\begin{array}{cc}\hfill {\dot{V}}_1& =\sum _{i=1}^n\left\{-k_p{(p_{ei}-c_i)}^2+(p_{ei}-c_i)\left[u_{lei}+2u_{li}\left(cos{\theta }_{li}cos{\theta }_{bi}si{n}^2\frac{{\psi }_{lei}}{2}+si{n}^2\frac{{\theta }_{lei}}{2}\right)\right]\right.\hfill \\ \hfill & +{\gamma }_{\theta }{\theta }_{lei}({\dot{\theta }}_{bi}-f_{{\theta }_{li}}-g_{{\theta }_{li}}{\alpha }_{qi}+g_{{\theta }_{li}}{e}_{qi})+{\gamma }_{\psi }{\psi }_{lei}({\dot{\psi }}_{bi}-f_{{\psi }_{li}}-g_{{\psi }_{li}}{\alpha }_{ri}+g_{{\psi }_{li}}{e}_{ri})\hfill \\ \hfill & +{\gamma }_{OB}\left\{B_{xi}\left[u_{lei}cos{\theta }_{bi}cos{\psi }_{bi}-2u_{li}\left(cos{\psi }_{bi}sin\frac{{\theta }_{bi}+{\theta }_{li}}{2}sin\frac{{\theta }_{lei}}{2}+cos{\theta }_{li}sin\frac{{\psi }_{bi}+{\psi }_{li}}{2}sin\frac{{\psi }_{lei}}{2}\right)\right]\right.\hfill \\ \hfill & +B_{yi}\left[u_{lei}cos{\theta }_{bi}sin{\psi }_{bi}-2u_{li}\left(sin{\psi }_{bi}sin\frac{{\theta }_{bi}+{\theta }_{li}}{2}sin\frac{{\theta }_{lei}}{2}-cos{\theta }_{li}cos\frac{{\psi }_{bi}+{\psi }_{li}}{2}sin\frac{{\psi }_{lei}}{2}\right)\right]\hfill \\ \hfill & \left.-B_{zi}\left(u_{lei}sin{\theta }_{bi}+2u_{li}cos\frac{{\theta }_{bi}+{\theta }_{li}}{2}sin\frac{{\theta }_{lei}}{2}\right)\right\}+{\mathsf{\Lambda }}_{OB}.\hfill \end{array}$$

(22)

According to (22), the control laws for ${\alpha }_{qi}$ and ${\alpha }_{ri}$ are designed as follows.

$$\begin{array}{cc}\hfill {\alpha }_{qi}& =g_{{\theta }_{li}}^{-1}\left[{\dot{\theta }}_{bi}-f_{{\theta }_{li}}+{\gamma }_{\theta }^{-1}\left({\mathsf{\Lambda }}_{{\theta }_{lei}}+k_{\theta }{\theta }_{lei}\right)\right],\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill {\alpha }_{ri}& =g_{{\psi }_{li}}^{-1}\left[{\dot{\psi }}_{bi}-f_{{\psi }_{li}}+{\gamma }_{\psi }^{-1}\left({\mathsf{\Lambda }}_{{\psi }_{lei}}+k_{\psi }{\psi }_{lei}\right)\right],\hfill \end{array}$$

(24)

where $k_{\theta },k_{\psi }>0$ are design parameters and

$$\begin{array}{cc}\hfill {\mathsf{\Lambda }}_{{\theta }_{lei}}& =u_{li}\frac{sin\frac{{\theta }_{lei}}{2}}{\frac{{\theta }_{lei}}{2}}\left\{(p_{ei}-c_i)sin\frac{{\theta }_{lei}}{2}-{\gamma }_{OB}\left[B_{zi}cos\frac{{\theta }_{bi}+{\theta }_{li}}{2}+sin\frac{{\theta }_{bi}+{\theta }_{li}}{2}\left(B_{xi}cos{\psi }_{bi}+B_{yi}sin{\psi }_{bi}\right)\right]\right\},\hfill \\ \hfill {\mathsf{\Lambda }}_{{\psi }_{lei}}& =u_{li}cos{\theta }_{li}\frac{sin\frac{{\psi }_{lei}}{2}}{\frac{{\psi }_{lei}}{2}}\left[(p_{ei}-c_i)cos{\theta }_{bi}sin\frac{{\psi }_{lei}}{2}+{\gamma }_{OB}\left(B_{yi}cos\frac{{\psi }_{bi}+{\psi }_{li}}{2}-B_{xi}sin\frac{{\psi }_{bi}+{\psi }_{li}}{2}\right)\right].\hfill \end{array}$$

Applying the virtual control laws (23) and (24) into (22), it has

$${\dot{V}}_1=\sum _{i=1}^n\left[-k_p{(p_{ei}-c_i)}^2-k_{\theta }{\theta }_{lei}^2-k_{\psi }{\psi }_{lei}^2+{\mathsf{\Lambda }}_{u_{lei}}{u}_{lei}+{\gamma }_{\theta }{\theta }_{lei}{g}_{{\theta }_{li}}{e}_{qi}+{\gamma }_{\psi }{\psi }_{lei}{g}_{{\psi }_{li}}{e}_{ri}\right]+{\mathsf{\Lambda }}_{OB},$$

(25)

where ${\mathsf{\Lambda }}_{u_{lei}}$ is defined as follows

$${\mathsf{\Lambda }}_{u_{lei}}=p_{ei}-c_i+{\gamma }_{OB}\left(B_{xi}cos{\theta }_{bi}cos{\psi }_{bi}+B_{yi}cos{\theta }_{bi}sin{\psi }_{bi}-B_{zi}sin{\theta }_{bi}\right).$$

Step 2. Recall the ith vehicle’s dynamics (16) and rewriting it in the error terms as following

$$\left[\begin{array}{c}{\dot{u}}_{lei}\\ {\dot{e}}_{qi}\\ {\dot{e}}_{ri}\end{array}\right]=\left[\begin{array}{c}{\dot{u}}_{li}^R-W_{uli}^T{\varphi }_{uli}\\ {\dot{\alpha }}_{qi}-W_{qi}^T{\varphi }_{qi}\\ {\dot{\alpha }}_{ri}-W_{ri}^T{\varphi }_{ri}\end{array}\right]-\left[\begin{array}{ccc}{g}_{aui}& 0& 0\\ 0& g_{qi}& 0\\ 0& 0& g_{ri}\end{array}\right]\left[\begin{array}{c}{\tau }_{ui}\\ {\tau }_{qi}\\ {\tau }_{ri}\end{array}\right]-\left[\begin{array}{c}{d}_{uli}\\ d_{qi}\\ d_{ri}\end{array}\right].$$

(26)

The following Lyapunov function candidate is applied in this step,

$$V_2=V_1+\frac{1}{2}\sum _{i=1}^n\left({\gamma }_u{u}_{lei}^2+{\gamma }_q{e}_{qi}^2+{\gamma }_r{e}_{ri}^2\right),$$

(27)

where ${\gamma }_u,{\gamma }_q,{\gamma }_r>0$ are weighting factors.

By differentiating (27) and further substituting (25) and (26) into it, we can get the following expansion

$$\begin{array}{cc}\hfill {\dot{V}}_2& =\sum _{i=1}^n\left[-k_p{(p_{ei}-c_i)}^2-k_{\theta }{\theta }_{lei}^2-k_{\psi }{\psi }_{lei}^2+{\mathsf{\Lambda }}_{u_{lei}}{u}_{lei}+{\gamma }_{\theta }{\theta }_{lei}{g}_{{\theta }_{li}}{e}_{qi}+{\gamma }_{\psi }{\psi }_{lei}{g}_{{\psi }_{li}}{e}_{ri}\right.\hfill \\ \hfill & +{\gamma }_u{u}_{lei}\left({\dot{u}}_{li}^R-W_{uli}^T{\varphi }_{uli}-g_{aui}{\tau }_{ui}-d_{uli}\right)+{\gamma }_q{e}_{qi}\left({\dot{\alpha }}_{qi}-W_{qi}^T{\varphi }_{qi}-g_{qi}{\tau }_{qi}-d_{qi}\right)\hfill \\ \hfill & \left.+{\gamma }_r{e}_{ri}\left({\dot{\alpha }}_{ri}-W_{ri}^T{\varphi }_{ri}-g_{ri}{\tau }_{ri}-d_{ri}\right)\right]+{\mathsf{\Lambda }}_{OB}.\hfill \end{array}$$

(28)

According to the above equation, the control laws in Step 2 are chosen as following

$$\begin{array}{cc}\hfill {\tau }_{ui}& =g_{aui}^{-1}\left[{\dot{u}}_{li}^R-{\widehat{W}}_{uli}^T{\varphi }_{uli}+d_{uM}\eta \left({\gamma }_u{u}_{lei}{d}_{uM}\right)+{\gamma }_u^{-1}(k_u{u}_{lei}+{\mathsf{\Lambda }}_{u_{lei}})\right],\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill {\tau }_{qi}& =g_{qi}^{-1}\left[{\dot{\alpha }}_{qi}-{\widehat{W}}_{qi}^T{\varphi }_{qi}+d_{qM}\eta \left({\gamma }_q{e}_{qi}{d}_{qM}\right)+{\gamma }_q^{-1}(k_q{e}_{qi}+{\gamma }_{\theta }{\theta }_{lei}{g}_{{\theta }_{li}})\right],\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill {\tau }_{ri}& =g_{ri}^{-1}\left[{\dot{\alpha }}_{ri}-{\widehat{W}}_{ri}^T{\varphi }_{ri}+d_{rM}\eta \left({\gamma }_r{e}_{ri}{d}_{rM}\right)+{\gamma }_r^{-1}(k_r{e}_{ri}+{\gamma }_{\psi }{\psi }_{lei}{g}_{{\psi }_{li}})\right],\hfill \end{array}$$

(31)

where ${\widehat{W}}_{ai}$ with $a\in \{u_l,q,r\}$ is the estimation of $W_{ai}$, $k_u,k_q,k_r>0$ are design parameters, and function $\eta (·)$ denotes a smooth function and satisfies the following proposition.

Proposition 2

([22]). $\forall ϵ>0$, there is a smooth function $\eta (·)$ such that $\eta \left(0\right)=0$ and

$$\left|\xi \right|\le \xi \eta \left(\xi \right)+ϵ,\forall \xi \in \Re .$$

(32)

Remark 7

([22]). One of the example functions satisfying Proposition 2 is $\eta \left(\xi \right)=tanh(\kappa \xi /ϵ)$ with $\kappa =e^{-(\kappa +1)}$.

By applying the above control laws into (28), we can get

$$\begin{array}{cc}\hfill {\dot{V}}_2& \le \sum _{i=1}^n\left[-k_p{(p_{ei}-c_i)}^2-k_{\theta }{\theta }_{lei}^2-k_{\psi }{\psi }_{lei}^2-k_u{u}_{lei}^2-k_q{e}_{qi}^2-k_r{e}_{ri}^2\right.\hfill \\ \hfill & -{\gamma }_u{u}_{lei}{\tilde{W}}_{uli}^T{\varphi }_{uli}+{\gamma }_u|u_{lei}{d}_{uli}|-{\gamma }_u{u}_{lei}{d}_{uM}\eta \left({\gamma }_u{u}_{lei}{d}_{uM}\right)-{\gamma }_q{e}_{qi}{\tilde{W}}_{qi}^T{\varphi }_{qi}+{\gamma }_q\left|e_{qi}{d}_{qi}\right|\hfill \\ \hfill & \left.-{\gamma }_q{e}_{qi}{d}_{qM}\eta \left({\gamma }_q{e}_{qi}{d}_{qM}\right)-{\gamma }_r{e}_{ri}{\tilde{W}}_{ri}^T{\varphi }_{ri}+{\gamma }_r\left|e_{ri}{d}_{ri}\right|-{\gamma }_r{e}_{ri}{d}_{rM}\eta \left({\gamma }_r{e}_{ri}{d}_{rM}\right)\right]+{\mathsf{\Lambda }}_{OB},\hfill \end{array}$$

(33)

where ${\tilde{W}}_{ai}=W_{ai}-{\widehat{W}}_{ai}$ with $a\in \{u_l,q,r\}$ denote the estimation errors.

Applying Proposition 2, (33) can be expanded as following

$$\begin{array}{cc}\hfill {\dot{V}}_2& \le \sum _{i=1}^n\left[-k_p{(p_{ei}-c_i)}^2-k_{\theta }{\theta }_{lei}^2-k_{\psi }{\psi }_{lei}^2-k_u{u}_{lei}^2-k_q{e}_{qi}^2-k_r{e}_{ri}^2-{\gamma }_u{u}_{lei}{\tilde{W}}_{uli}^T{\varphi }_{uli}-{\gamma }_q{e}_{qi}{\tilde{W}}_{qi}^T{\varphi }_{qi}\right.\hfill \\ \hfill & \left.-{\gamma }_r{e}_{ri}{\tilde{W}}_{ri}^T{\varphi }_{ri}+{ϵ}_{ui}+{ϵ}_{qi}+{ϵ}_{ri}\right]+{\mathsf{\Lambda }}_{OB}.\hfill \end{array}$$

(34)

Now we consider the following final step Lyapunov function candidate

$$V_3=V_2+\frac{1}{2}\sum _{i=1}^n\left({\tilde{W}}_{uli}^T{\mathsf{\Gamma }}_{ul}{\tilde{W}}_{uli}+{\tilde{W}}_{qi}^T{\mathsf{\Gamma }}_q{\tilde{W}}_{qi}+{\tilde{W}}_{ri}^T{\mathsf{\Gamma }}_r{\tilde{W}}_{ri}\right),$$

(35)

where ${\mathsf{\Gamma }}_a\in {\Re }^{N_a\times N_a}$ with $a\in \{u_l,q,r\}$ are strictly positive definite matrix.

By differentiating (35) and substituting (34) into it, we can get

$$\begin{array}{cc}\hfill {\dot{V}}_3& \le \sum _{i=1}^n\left[-k_p{(p_{ei}-c_i)}^2-k_{\theta }{\theta }_{lei}^2-k_{\psi }{\psi }_{lei}^2-k_u{u}_{lei}^2-k_q{e}_{qi}^2-k_r{e}_{ri}^2+{ϵ}_{ui}+{ϵ}_{qi}+{ϵ}_{ri}-{\gamma }_u{u}_{lei}{\tilde{W}}_{uli}^T{\varphi }_{uli}\right.\hfill \\ \hfill & \left.-{\tilde{W}}_{uli}^T{\mathsf{\Gamma }}_{ul}{\dot{\widehat{W}}}_{uli}-{\gamma }_q{e}_{qi}{\tilde{W}}_{qi}^T{\varphi }_{qi}-{\tilde{W}}_{qi}^T{\mathsf{\Gamma }}_q{\dot{\widehat{W}}}_{qi}-{\gamma }_r{e}_{ri}{\tilde{W}}_{ri}^T{\varphi }_{ri}-{\tilde{W}}_{ri}^T{\mathsf{\Gamma }}_r{\dot{\widehat{W}}}_{ri}\right]+{\mathsf{\Lambda }}_{OB}.\hfill \end{array}$$

(36)

Here we choose the online adaptation laws for the ith vehicle’s hydrodynamic coefficients as follows

$$\begin{array}{cc}\hfill {\dot{\widehat{W}}}_{uli}& =-{\mathsf{\Gamma }}_{ul}^{-1}\left[{\gamma }_u{u}_{lei}{\varphi }_{uli}+k_{Wu}\left({\widehat{W}}_{uli}-W_{ulo}\right)\right],\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill {\dot{\widehat{W}}}_{qi}& =-{\mathsf{\Gamma }}_q^{-1}\left[{\gamma }_q{e}_{qi}{\varphi }_{qi}+k_{Wq}\left({\widehat{W}}_{qi}-W_{qo}\right)\right],\hfill \end{array}$$

(38)

$$\begin{array}{cc}\hfill {\dot{\widehat{W}}}_{ri}& =-{\mathsf{\Gamma }}_r^{-1}\left[{\gamma }_r{e}_{ri}{\varphi }_{ri}+k_{Wr}\left({\widehat{W}}_{ri}-W_{ro}\right)\right],\hfill \end{array}$$

(39)

where $k_{Wu},k_{Wq},k_{Wr}>0$ are design parameters, and $W_{ao}$ with $a\in \{u_l,q,r\}$ are the initial values of ${\widehat{W}}_{ai}$ at $t=0$.

Substituting (37)–(39) into (36), finally we can get

$$\begin{array}{cc}\hfill {\dot{V}}_3& \le \sum _{i=1}^n\left[-k_p{(p_{ei}-c_i)}^2-k_{\theta }{\theta }_{lei}^2-k_{\psi }{\psi }_{lei}^2-k_u{u}_{lei}^2-k_q{e}_{qi}^2-k_r{e}_{ri}^2-\frac{1}{2}{k}_{Wu}{\tilde{W}}_{uli}^T{\tilde{W}}_{uli}\right.\hfill \\ \hfill & \left.-\frac{1}{2}{k}_{Wq}{\tilde{W}}_{qi}^T{\tilde{W}}_{qi}-\frac{1}{2}{k}_{Wr}{\tilde{W}}_{ri}^T{\tilde{W}}_{ri}+{ϵ}_{ui}+{ϵ}_{qi}+{ϵ}_{ri}+E_{Wi}\right]+{\mathsf{\Lambda }}_{OB},\hfill \end{array}$$

(40)

where $E_{Wi}=\left(k_{Wu}\left|\right|W_{uli}-W_{ulo}{\left|\right|}_2^2+k_{Wq}\left|\right|W_{qi}-W_{qo}{\left|\right|}_2^2+k_{Wr}\left|\right|W_{ri}-W_{ro}{\left|\right|}_2^2\right)/2$.

As mentioned in Remark 6, in this paper we only consider the case where the potential function method of obstacle avoidance can guarantee the global minimum of path searching. Therefore, after a certain period of time, the vehicles always escape from the obstacles. Let us recall the potential function defined as (18). In the case after the vehicles are all escaped from the obstacles, we have $\forall i,k$, $\left|\right|p_{eik}{\left|\right|}_2\ge a$, therefore $f_p(p_{eik},a)=0$, and $\partial f_p(p_{eik},a)/\partial p_{eik}=0$. This means that $B_{\zeta i}=0$ with $\zeta \in \{x,y,z\}$. Consequently we get ${\mathsf{\Lambda }}_{OB}=0$ and (40) becomes

$${\dot{V}}_3\le -\lambda V_3+\rho ,$$

(41)

with $\lambda$ and $\rho$ defined as following

$$\begin{array}{cc}\lambda \hfill & =min\left\{2k_p,2k_{\theta }{\gamma }_{\theta }^{-1},2k_{\psi }{\gamma }_{\psi }^{-1},2k_u{\gamma }_u^{-1},2k_q{\gamma }_q^{-1},2k_r{\gamma }_r^{-1},k_{Wu}{\lambda }_{min}^{-1}\left({\mathsf{\Gamma }}_{ul}\right),k_{Wq}{\lambda }_{min}^{-1}\left({\mathsf{\Gamma }}_q\right),\right.\\ \hspace{1em}& \left.k_{Wr}{\lambda }_{min}^{-1}\left({\mathsf{\Gamma }}_r\right)\right\},\hfill \end{array}$$

(42)

$$\begin{array}{cc}\hfill \rho & ={ϵ}_{ui}+{ϵ}_{qi}+{ϵ}_{ri}+E_{Wi},\hfill \end{array}$$

(43)

where ${\lambda }_{min}\left(\mathsf{\Gamma }\right)$ indicates the minimum singular value of matrix $\mathsf{\Gamma }$.

Theorem 1.

Considering the formation control of multiple underactuated underwater vehicles whose kinematics and dynamics are taken as (1) and (2) with Assumption 1–3, if we choose the formation control laws as (29)–(31) and the vehicles’ hydrodynamics coefficients adaptation laws as (37)–(39), then we can guarantee the uniform ultimate boundedness (UUB) of closed-loop system in terms of spherical coordinates.

Proof. 

From (41), it is easy to verify that

$$0\le V_3\left(t\right)\le \left[V_3\left(0\right)-\frac{\rho }{\lambda }\right]e^{-\lambda t}+\frac{\rho }{\lambda },$$

(44)

which indicates that $V_3\left(t\right)$ will exponentially converge to $\rho /\lambda$ and this concludes the proof. □

Remark 8.

In the case where the basis function vectors ${\varphi }_{uli},{\varphi }_{qi}$, and ${\varphi }_{ri}$ satisfy to be persistency excitation, the adaptation laws (37)–(39) with $k_{Wu}=k_{Wq}=k_{Wr}=0$ can guarantee the exact estimation of $W_{uli},W_{qi}$, and $W_{ri}$. Unfortunately, this kind of PE conditions are difficult to be satisfied in most of practical applications, and in this case, properly designed parameters $k_{Wu}=k_{Wq}=k_{Wr}>0$ can prevent the divergence of ${\widehat{W}}_{uli},{\widehat{W}}_{qi}$, and ${\widehat{W}}_{ri}$ [22,24].

Remark 9.

Consider the definition of the polar and azimuth angles ${\theta }_{bi}$ and ${\psi }_{bi}$ as in (13) and (14). It is easy to see that if $p_{ei}=0$, then both of ${\theta }_{bi}$ and ${\psi }_{bi}$ are undefined. Here the properly designed parameters $c_i>0$ in practice so as for $p_{ei}\to c_i$ can prevent the above mentioned singularity problem.

5. Numerical Studies

To verify the effectiveness of the proposed formation scheme, in this section we carry out some simulation studies using the 6-DOF of REMUS AUV simulation model [31]. In the simulation, both the rudder and stern plane angles are saturated by 15 degrees.

There was a total of three vehicles with the same dynamics considered in the simulation, and the trajectories for each virtual leader are designed as follows. For Virtual Leader 1, if $t<120$ s, then $u_{ld1}=1.5$ m/s, and ${\dot{\theta }}_{ld1}={\dot{\psi }}_{ld1}=0$; else if $t<2000$ s, then $u_{ld1}=1.5$ m/s, ${\dot{\theta }}_{ld1}=-0.1337$, and ${\dot{\psi }}_{ld1}=0.005$; else, $u_{ld1}=1.5$ m/s, and ${\dot{\theta }}_{ld1}={\dot{\psi }}_{ld1}=0$. For both of Virtual Leader 2 and 3, if $t<212$ s, then $u_{ldi}=1.5$ m/s, and ${\dot{\theta }}_{ldi}={\dot{\psi }}_{ldi}=0$; else if $t<2100$ s, then $u_{ldi}=1.5$ m/s, ${\theta }_{ldi}=-0.1337$ rad, and ${\dot{\psi }}_{ldi}=0.005$ rad/s; else, $u_{ldi}=1.5$ m/s, and ${\dot{\theta }}_{ldi}={\dot{\psi }}_{ldi}=0$ with $i=2,3$. And the initial conditions are set as: $p_{d1}\left(0\right)=(180\mathrm{m},(1200-\sqrt{3})\mathrm{m},200\mathrm{m})$, $p_{d2}\left(0\right)=(100\mathrm{m},1200\mathrm{m},200\mathrm{m})$, $p_{d3}\left(0\right)=(260\mathrm{m},1200\mathrm{m},200\mathrm{m})$, ${\psi }_{ldi}=-90\mathrm{degrees},i=1,2,3$. Three vehicles initial conditions are chosen as: $X_1\left(0\right)=(150\mathrm{m},1130\mathrm{m},220\mathrm{m},0,0,-100\mathrm{deg},0.5\mathrm{m}/\mathrm{s},0,0,0,0,0)$, $X_2\left(0\right)=(60\mathrm{m},1280\mathrm{m},200\mathrm{m},0,0,-80\mathrm{deg},0.5\mathrm{m}/\mathrm{s},0,0,0,0,0)$, and $X_3\left(0\right)=(300\mathrm{m},1270\mathrm{m},180\mathrm{m},0,0$, $-70\mathrm{deg},0,0,0,0,0)$ where $X=(x,y,z,\varphi ,\theta ,\psi ,u,v,w,p,q,r)$.

The controller design parameters are chosen as: $c=15$; $k_p=1$, $k_u=15$, $k_{\theta }=15000$, $k_{\psi }=6000$, $k_q=120$, $k_r=2000$; ${\gamma }_u=14$, ${\gamma }_{\theta }=200$, ${\gamma }_{\psi }=200$, ${\gamma }_{OB}=20$, ${\gamma }_q=60$, ${\gamma }_r=10$; ${\mathsf{\Gamma }}_{ul}=5\ast diag\left(ones(25,1)\right)$, ${\mathsf{\Gamma }}_q=diag\left(ones(11,1)\right)$, ${\mathsf{\Gamma }}_r=diag\left(ones(7,1)\right)$, $k_{Wu}=8$, $k_{Wq}=k_{Wr}=0.1$, $W_{ulo}=0.5W_{uli}$, $W_{qo}=0.5W_{qi}$, $W_{ro}=0.5W_{ri}$. Corresponding to the smooth potential function (18) and bump function (19), we set $c_o=3$, $a=200$, and $h=0.9$. And the disturbance terms are selected as $d_{ui}^{\prime }=1.5(1-2rand)$, $d_{vi}^{\prime }=0.5(1-2rand)$, $d_{wi}^{\prime }=0.5(1-2rand)$, $d_{qi}^{\prime }=0.3(1-2rand)$, and $d_{ri}^{\prime }=0.3(1-2rand)$. Finally, the obstacle is modeled as follows

$$z=-600-100cos\left(\pi -0.01\sqrt{{(x-x_o)}^2+{(y-y_o)}^2}\right),$$

(45)

where $(x_o,y_o)=(800\mathrm{m},1200\mathrm{m})$.

Simulation results are shown in Figure 4, Figure 5, Figure 6 and Figure 7. Figure 4 and Figure 5 show the virtual leaders’ designed trajectories and the vehicles’ corresponding trajectory following results while avoiding the given obstacle, from which we can see that the proposed formation scheme can provide satisfactory trajectory following performance while properly avoiding the obstacles. Figure 6 shows the corresponding trajectory following errors in terms of spherical coordinates, and we can see that all the error terms are bounded except during the obstacle avoidance and this is coincident with the main result of this paper. Considering the adaptation laws (37)–(39), it might be interesting to investigate their coefficients estimations performance, and Figure 7 shows the result, from which we can see that the adaptation laws cannot guarantee the exact estimation of coefficients under this simulation conditions. However, the adaptation laws still can prevent the divergence of estimations.

6. Conclusions

An adaptive formation control scheme for a group of torpedo-type underactuated AUVs has been presented in this paper by introducing a sort of virtual school concept. This virtual school, which is similar to the virtual structure, is a geometric graph where each node is taken as a virtual leader for each specific AUV and assigned its own reference trajectory. The formation strategy is that each vehicle is forced to follow its virtual leader so as to construct the pre-defined formation. Since the individual vehicle considered in this paper is a torpedo-type underactuated system, another main focus in this paper has been on the trajectory following the controller design problem for each vehicle. The proposed adaptive formation control scheme can guarantee the UUB of a closed-loop system in terms of spherical coordinates. Numerical simulations also have been carried out and presented the effectiveness of the proposed method.

As mentioned before, the main focus of this paper is formation tracking rather than formation producing [1]. Therefore, how to properly design the formation structure for multiple underactuated underwater vehicles might be one of our most interesting future works. On the other hand, during the simulation studies, we have found that the potential functions shown in (18) seem not very efficient in obstacle avoidance. So, finding a more efficient potential function for obstacle avoidance also requires further study.