Adaptive Formation Control of Multiple Underactuated Autonomous Underwater Vehicles

: 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 speciﬁc 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 ﬁeld 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.


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-threeoutput 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.

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], where (x i , y i , z i ) is the ith vehicle's position in the navigation frame and θ i and ψ 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 (•) ∈ C 2 with a ∈ {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 τ ui , pitch moment τ qi , and yaw moment τ ri are three only available control inputs with g ui , g qi , and g ri the known nonzero control gains; d ai 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 θ i = π/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 θ i ∈ (−π/2, π/2).

Spherical Coordinates
Here we introduce a new velocity parameter u li = u 2 i + v 2 i + w 2 i by which the vehicle's linear velocity dynamics can be integrated as one as following uli = ui cosθ ai cosψ ai + vi sinψ ai + ẇi sinθ ai cosψ ai where f uli = f ui cosθ ai cosψ ai + f vi sinψ ai + f wi sinθ ai cosψ ai , g aui = g ui cosθ ai cosψ ai , d uli = d ui cosθ ai cosψ ai + d vi sinψ ai + d wi sinθ ai cosψ ai .
Corresponding to the new velocity parameter u li , its polar and azimuth angles θ li and ψ li can be expressed as follows: Remark 2. The mathematical definitions of θ li and ψ 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).
In other words, g θ li is always invertible.On the other hand, according to Assumption 1, we can get that g ψ li is also nonzero and therefore is always invertible.

Nonlinear Dynamics Approximation
For any given continuous function f (x), it always can be expressed in the following parametric form [23,24] f where W * ∈ N * is a constant vector and φ * (x) ∈ N * is the basis function vector of f (x).
If φ * (x) 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] where φ(x) ∈ N is the constructed basis function vector and W ∈ N is the corresponding constant coefficients vector with N ≤ N * .Using ( 9), the vehicle's dynamics ( 7) can be rewritten as where φ ai ∈ N a is the constructed basis function vector for f ai and W ai ∈ N a is the corresponding coefficients vector, and Here a ∈ {u l , q, r}.
In practice it is difficult to exactly derive the constant vectors W ai with a ∈ {u l , , q, r} in advance.Therefore, in this paper, we make the following assumption.Assumption 2. In (10), the constructed basis function vectors φ ai are known in advance and W ai are unknown constant vectors with a ∈ {u l , q, r}.

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.

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 (t) = (x di (t), y di (t), z di (t)), i = 1, • • • , 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 (t) 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.

Trajectory Following
For the ith vehicle, the formation control objective as mentioned before is just to follow its reference trajectory p di (t).Considering two kinematics (1) and ( 6), it is easy to see that for the ith vehicle's any given trajectory p i (t) = (x i (t), y i (t), z i (t)), it can be realized by (u i (t), v i (t), w i (t), θ i (t), ψ i (t)) through the position kinematics in (1), or also simply by the spherical coordinates (u li (t), θ li (t), ψ li (t)) using (6).In this paper, we apply the latter method.
For the ith vehicle's given reference trajectory p di (t) = (x di (t), y di (t), z di (t)) combined with (θ ldi , ψ ldi ) where the kinematics is taken as (6), the position error is defined as following (11) whose time derivative further can be expanded as where θ bi and ψ bi denote the polar and azimuth angles for the position error vector p ei and are defined as follows For the ith vehicle, the control objective of trajectory following in this paper is defined as follows.
Remark 4. For the ith vehicle, since (θ li (t), ψ li (t)) → (θ bi (t), ψ bi (t)), its control strategy is clear that to force the vehicle to always face to the target point (x di (t), y di (t), z di (t)) while keeping p ei (t) → 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 (t), θ li (t), ψ li (t)) → (p di (t), θ ldi (t), ψ ldi (t)) where θ ldi (t) and ψ ldi (t) are predefined.
In the case θ lei = ψ lei = 0, ( 12) becomes ṗ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 where k p > 0 is design parameter, then it can guarantee the exponential stability of p ei → c i .

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] where c o > 0 is a constant and ρ(•) is a smooth bump function taken as following form where h ∈ (0, 1) is a design parameter.It is easy to see that f p (ζ, a) is a monotone decreasing function and if ζ ≥ a, then f p (ζ, 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.

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 (t), θ li (t), ψ li (t)) to follow its own virtual leader (p di (t), θ ldi (t), ψ ldi (t)) so as for p ei (t) → c i and (θ li (t), ψ li (t)) → (θ bi (t), ψ bi (t)).For this reason, in this step, we consider the following Lyapunov function candidate where γ θ , γ ψ , γ OB > 0 are weighting factors, and the smooth potential function f p (•) is defined as (18) where As aforementioned, if we choose the stabilization function u R li for virtual input u li as (17), and let α qi and α ri denote the stabilization functions for each of virtual inputs q i and r i with e qi = α qi − q i and e ri = α ri − r i , then ( 21) can be further expanding as following According to (22), the control laws for α qi and α ri are designed as follows.
where k θ , k ψ > 0 are design parameters and Applying the virtual control laws ( 23) and ( 24) into (22), it has where Λ u lei is defined as follows Step 2. Recall the ith vehicle's dynamics (16) and rewriting it in the error terms as following The following Lyapunov function candidate is applied in this step, where γ u , γ q , γ r > 0 are weighting factors.By differentiating (27) and further substituting ( 25) and ( 26) into it, we can get the following expansion According to the above equation, the control laws in Step 2 are chosen as following where Ŵai with a ∈ {u l , q, r} is the estimation of W ai , k u , k q , k r > 0 are design parameters, and function η(•) denotes a smooth function and satisfies the following proposition.
By applying the above control laws into (28), we can get where Wai = W ai − Ŵai with a ∈ {u l , q, r} denote the estimation errors.Applying Proposition 2, (33) can be expanded as following Now we consider the following final step Lyapunov function candidate where Γ a ∈ N a ×N a with a ∈ {u l , q, r} are strictly positive definite matrix.By differentiating (35) and substituting (34) into it, we can get Here we choose the online adaptation laws for the ith vehicle's hydrodynamic coefficients as follows where k Wu , k Wq , k Wr > 0 are design parameters, and W ao with a ∈ {u l , q, r} are the initial values of Ŵai at t = 0. Substituting (37)-( 39) into (36), finally we can get where 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 ∀i, k, ||p eik || 2 ≥ a, therefore f p (p eik , a) = 0, and ∂ f p (p eik , a)/∂p eik = 0.This means that B ζi = 0 with ζ ∈ {x, y, z}.Consequently we get Λ OB = 0 and (40) becomes V3 ≤ −λV 3 + ρ, ( with λ and ρ defined as following where λ min (Γ) indicates the minimum singular value of matrix Γ.
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 which indicates that V 3 (t) will exponentially converge to ρ/λ and this concludes the proof.
Remark 8.In the case where the basis function vectors φ uli , φ qi , and φ 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 Ŵuli , Ŵqi , and Ŵri [22,24].
Remark 9. Consider the definition of the polar and azimuth angles θ bi and ψ bi as in ( 13) and ( 14).
It is easy to see that if p ei = 0, then both of θ bi and ψ bi are undefined.Here the properly designed parameters c i > 0 in practice so as for p ei → c i can prevent the above mentioned singularity problem.

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.
The controller design parameters are chosen as: c = 15; Simulation results are shown in Figures 4-7.Figures 4 and 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.

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.

Figure 1 .
Figure 1.Star-topological acoustic communication network for a group of AUVs and a USV.

Assumption 3 .
The uncertainty terms in(10) are bounded such that |d uli | ≤ d uM , |d qi | ≤ d qM , and |d ri | ≤ d rM with d uM , d qM , d rM > 0 known constants.

Figure 3 .
Figure 3. Obstacle detection and its modeling using occupancy grid map.

Figure 4 .
Figure 4. Virtual leaders trajectories and their trajectory with obstacle avoidance using the proposed formation scheme.

Figure 5 .
Figure 5.Each vehicle's trajectory following with obstacle avoidance on the horizontal plane.

Figure 6 .
Figure 6.UUB stability of proposed formation scheme in terms of spherical coordinate frame.