Consensus Path-following Control of Multiple Underactuated Unmanned Underwater Vehicles

This work mainly studies the problem of how to steer a group of underactuated unmanned underwater vehicles UUVs to specified paths coordinately. The algorithm proposed consists of a single path-following strategy and a path parameter consensus tracking strategy. In the context of single path following, we describe the path to be followed by an arbitrary scalar, then by using Lyapunov and backstepping theories, a single path-following strategy was derived to drive each UUV move to the predefined path asymptotically. In the coordinated level, we focus on the coordination for the scalar parameters. In particular, we show that all the path parameters can track with a virtual reference leader who is a neighbor of only a subset of following UUVs with local interactions. The stability of the closed system was proved and analyzed theoretically. The validity of the algorithm proposed is supported by simulation results.


Introduction
With the development of artificial intelligence, robots have been widely used for complex tasks such as handling searching and rescuing in a dangerous environment.In [1], the teleoperated robots working in dynamic and unstructured environments to perform complex tasks were considered.In this paper, neural networks methods were used to deal with the uncertainties of the telerobot model to ensure a guaranteed performance; moreover, extreme learning machine-based control scheme and combined radial basis function with neural networks techniques were developed to compensate for unknown nonlinearity in the manipulator dynamics and communication delays in [2,3], respectively.Compared to telerobots considered in the aforementioned works, unmanned underwater vehicle (UUV) works in a more complex environment and with more uncertainties.In order to fulfill these applications, we always need to maneuver an UUV along a desired geometric path.Typically, there are two ways to define the expected geometric path for an UUV should move to [4].These two methods are called trajectory tracking and path following, respectively.In the context of trajectory tracking, it always requires the UUV to converge to the expected path determined by specific times.However, we usually need one UUV to be driven to spatial positions as the main goal rather than dynamic behaviors described by time, and this is the main characteristic of the concept of path following.
The key idea of path following is how to define the path to be followed and assign the according reference position for each vehicle.In [5], the reference point is selected as the closest point to the current position of the vehicle.This method only needs to control the error in a normal direction relative to the path and consequently allowing a quick convergence to the path; however, this method demands that the initial position of the vehicle must be located in a circle with a radius of curvature centered at this closest point.To relax this conservative and necessary condition, Lapierre and Jouvencel [6] took an arbitrary point as the reference point and describe the path by the curvilinear abscissa of this point; the algorithm proposed in this paper took this point as a virtual leader and derived an updating law for the curvilinear abscissa of this point which enables following errors to converge to zero asymptotically.References [7,8] considered the single path-following problem of the underactuated autonomous underwater vehicle in 3D case, respectively.In [7], external disturbances and vehicle's model uncertainty are both considered using Lyapunov direct method, backstepping, and parameter projection techniques.The results show that path-following errors can be made arbitrarily small.In [8], the problem was decoupled to a 3D guidance law and a heuristic fuzzy control law; both internal and external uncertainties are considered in this work.
Sometimes, we need a group of vehicles to work together to complete complex tasks coordinately, which is called cooperative control [9] and has received more and more attention from the researchers.An important theory and tool to analyze the cooperative behavior is a consensus [10], which means to reach consent on their common states via information communications between the members in the group.Motivated by the idea of consensus, [11] considered the cooperative path-following problem in the field of ground robots.In the field of marine vehicles, authors in [12] proposed a synchronized path-following controller for fully actuated surface ships by using the passivity theory.We also proposed a formation control law for full actuated autonomous underwater vehicles in [13] by using passivity techniques.A decentralized formation controller was derived in [14], which deals with the cooperative problem of fully actuated surface vehicles with consideration of the influence of the sea currents and model uncertainty.Although the aforementioned works have considered about the cooperative path following of marine vehicles, all the vehicles are fully actuated rather than underactuated ones.Considering the fact that underactuated vehicles have fewer actuators than degrees of the freedom, there exist more challenging problems for controller designing because of the nonholonomic constraints.Reference [15] considered the coordinated straight path-following problem for underactuated unmanned surface vehicles.All the vehicles are controlled to converge to the accordingly assigned straight line path by the path-following controller based on line of sight guidance.To move synchronously, the surge speeds of each vehicle are adjusted by the distance along paths.By using nonlinear cascaded systems theory, the stability of the closed system was proved and analyzed.References [16,17] also executed researches on the coordinated path following of underactuated autonomous vessels.In the coordination level, the time delay between them to exchange the coordination vectors is considered and discussed.Even all the results proposed in the above works are decentralized because each member in the group only needs to exchange the necessary information under local interactions, all of them assumed that there is a global reference speed for the whole group that should be known to all the team members.In this sense, the controller proposed is not distributed because there is a global vector that should be known to all of them.
Motivated by the ideas of consensus tracking and aforementioned works, we consider the truly distributed path-following problem of underactuated unmanned vehicles in this proposal.Firstly, we take an arbitrary variable rather than the curvilinear abscissa like [6][7][8] to describe the desired path; followed by Lyapunov and backstepping techniques, the path-following errors were driven to zero asymptotically, and this can be regarded as the first contribution of our work.Furthermore, to ensure cooperative path-following behaviors of the vehicles, we proposed a consensus tracking law for the speed of each UUV, by which the reference point of each vehicle was driven coordinately.Compared with [12,[14][15][16][17], in our consensus tracking level, we only need the group reference speed to be available to one subset of the following UUVs rather than all of them.So the strategy proposed is truly distributed, and this can be regarded as the second contribution of this work.
The remainder of this paper was organized as: in Section 2, the problems to be studied were introduced and formulated.In Section 3, the kinematic and dynamic model of UUV was presented, and the single path-following controller for each UUV was designed using Lyapunov and backstepping methods; in the coordinated level, a path parameters consensus tracking strategy based was derived for multiple vehicles.To validate the performance of the method proposed, simulation examples are executed and discussed in Section 4. Finally, a brief conclusion is given in Section 5.

Problem Formulation
The main objective of path following is to enable the vehicle to converge to and move along the specified path with a dynamic behavior.In this paper, we mainly talked about this problem in planar 2D.The position and velocity of the ith vehicle can be denoted by p i = x i y i T ∈ R 2 and p i = x i y i T ∈ R 2 , where i = 1, … , N is the index of the vehicle number, and N denotes the total numbers of vehicles in the group.The size and orientation of the velocity vector can be characterized by U i = p i 2 = x i 2 + y i 2 and χ ip = arctan Let the desired path be represented as Then the consensus path-following problem considered in this paper can be formulated as where ( 1) and ( 2) can be regarded as the single pathfollowing objective which requires each individual vehicle to converge to and move along the desired path with a dynamic speed v 0 .Equation ( 3) is the coordinated objective which ensures all the path parameters achieve consensus, then the reference points determined by these parameters can be updated synchronously.

Consensus Path-following Controller Design
3.1.Path following for Single UUV.Firstly, we consider path following for one single UUV.Before going on, we give the dynamic model of the UUV in the horizontal plane borrowed from [18].
where  5), we can find that there are two control inputs τ u i , τ r i , but there are 3 freedoms to determine the specific spatial location of one UUV.So the UUVs considered in this paper are underactuated systems.Before moving on, we make some common assumptions to specify the model of the UUVs considered in this paper: (1) We do not consider the influence of the environmental disturbance, and we also assume that all model parameters are determined without any uncertainty (2) The position and heading states of each UUV can be captured by inertial navigation sensors and compass system, and the velocity can be measured by Doppler sensor (3) All the UUVs are equipped with sonar systems, which enable UUV to have the capabilities to communicate with each other Let the desired path for the ith vehicle be described by a given variable ϖ i as we have introduced in Section 2, define a path frame with p d ϖ i being the origin, specifically, define x axis along with the tangent and y axis along with normal direction, and name this frame as f .Then, the relative position between the vehicle and the reference point p d ϖ i can be depicted as shown Figure 1.
By using the geometric relations between the vehicle and the path to be followed, the velocity of the vehicle in inertial frame I can be described as where e i = s ie , y ie T denotes the coordinates of Q described in f , which is equivalent to the path-following error from Q to p d ϖ i .r ip is the angular velocity of the reference point, and R I f means transformation from frame f to I with the form of where ψ pi = arctan y pi ′ ϖ i /x pi ′ ϖ i with y pi ′ ϖ i = dy p ϖ i /d ϖ i .Multiplying the tranformation matrix from I to f by both sides of Equation ( 6), we can get the velocity of the vehicle described in f as where   9), ( 10), (11), and ( 12) into (8) and by simple calculations, we will have where U i = u 2 i + v 2 i is the total speed of the ith vehicle and β i = arctan v i /u i is the according side slip angle.From the path-following error dynamics (13), we can find that the tangential error s ie and normal error y ie can be stabilized by U ip , and the course tracking error ψ ie = ψ i + β i − ψ pi can be stabilized by the angular velocity r i .Motivated by this, define the desired course angle as where k δ > 0 and 0 < θ a < π/2.Define the following Lyapunov function and by calculating the differential of V 1i will yield Choose the desired yaw angular velocity for the ith vehicle as Then (16) goes From ( 15) and ( 18), we know lim t→∞ V 1i exists and is also bounded; moreover, V 1i = −2K 1 V 1i .So V 1i is bounded and V 1i is continuous all the time with lim t→∞ V 1i = 0 by using Barbalat lemma.So the course error ψ ie will converge to the desired angle (14).Next, we will design the virtual control input for U ip to stabilize path-following errors; to do this, define the following candidate Lyapunov function as Differentiate (19) with respect to time to obtain Choose U ip as where K 2 > 0, then (20) goes By using the characteristic of ( 14), we will always have y ie sin ψ ie ≤ 0 and y ie sin ψ ie = 0 iff y ie = 0.So if the virtual velocity U ip was defined as (21) and the yaw angular velocity was defined by (17), then the path-following errors s ie , y ie , and ψ ie will converge to zero, respectively.Next, we will design the control force and torque for each vehicle in dynamic level.
Firstly, define the speed-tracking error u ie = u i − u id and r ie = r i − r id .By considering the characteristic of backstepping techniques, we will propose the following control inputs for the ith vehicle as

23
where τ ui and τ ri are the control inputs for the surge force and yaw torque of each UUV.K 3 > 0, K 4 > 0, K 5 > 0 are the control gains to be derived.Bring (23) into (5) will yield Consequently, define the following candidate Lyapunov function Then by differentiating (25) along (24) will get From ( 25) and ( 26), we know lim t→∞ V 3i exists and is also bounded; moreover, V 3i is bounded and V 3i is continuous all the time with lim t→∞ V 3i = 0 by using Barbalat lemma.So, the path-following errors of each vehicle will be global asymptotically stable by using (23).

Consensus Path-following Design for Multiple UUVs.
Considering the virtual input derived as (21) and the fact of pi ϖ i + y 2 pi ϖ i with x pi ϖ i = x pi ′ϖ i , y pi ϖ i = y pi ′ϖ i , we can get the dynamic law for the path parameter by From ( 27), we can find that the velocity U i for the ith vehicle can be used to synchronize the path parameters ϖ i .To do this, we rewrite (27) as From the above subsection, we can find that if the ith vehicle tends to its desired path, then ψ ie and s ie will go to zero, respectively, which means d i also goes to zero.Then the dynamics of each path parameter will go as ϖ i = U i / x pi ′ 2 + y pi ′ 2 .If we assumed that all the parameters of each individual path will be updated by a common synchronized velocity ϖ 0 = v 0 ϖ 0 , then we will have Here, ϖ 0 = v 0 ϖ 0 can be regarded as the virtual group reference velocity to determine the coordinate behavior for the whole group.Then we can find that the desired speed of each vehicle should be Physically motivated by this, we define the following speed coordinate tracking error as where N i denotes the neighbor's sets of the ith vehicle.The first item in (30) characterizes the coordinated velocity error between the vehicles, and the second item, we denote b i > 0 when the ith vehicle can reach the virtual reference velocity v 0 and b i = 0 otherwise, and this item could be regarded as the speed-tracking error of the ith vehicle.In this paper, we will take the error defined in (30) as a virtual assistant variable and design the according dynamic law to stabilize it.Motivated by this, we denote ζ i = τ i and proposed the following law:

31
where a i > 0 is the control gain.The first item in (31) could be seen as the negative feedback to stabilize the error defined in (30), and the second item could be used to stabilize the coordinated errors between vehicles, while the last item aims to control the tracking error for the group reference path.Moreover, from (31), we could find that the group reference information only needs to be known to one subset of the whole group, so the strategy we proposed is totally distributed.We can rewrite (31) in a vector form as where C = diag 1/ R i ϖ i , A = diag a i , and L is the Laplacian matrix with the associated graph of the vehicles.The remaining work is to prove that under the control of (32), the objective of ϖ 1 = ⋯ = ϖ n = ϖ 0 will be reached.According to [19], the graph Laplacian matrix can be partitioned as L = M 1 Y 2 M T 1 for some Y > 0 and M 1 is the incidence matrix of the associated topology.Define the following vector: Considering the characteristic of the incidence matrix, we can find that ξ = 0 is equivalent to ϖ 1 = ⋯ = ϖ n = ϖ 0 ; consequently, the coordination errors ϖ i − ϖ j , ∀i, j will be driven to zero asymptotically.Considering the coordination dynamics ζ = τ, and by using the control law derived in (32), the closed dynamics in terms of ζ, ξ can be represented as Yξ and define a candidate Lyapunov function as By simple operations will yield which is negative and equals to zero only at the equilibrium point, since M 1 is full rank and Y > 0. Therefore, 5 Complexity (36) is negative definite and resorting the Lyapunov stability theorem ζ, ξ = 0, 0 is a uniformly global exponentially stable equilibrium.

Simulation Results
To illustrate the efficiency of the proposed consensus pathfollowing algorithm, we present simulation examples in this section.Considering a fleet of 3 UUVs, the associated communication topology was shown in Figure 2. Assuming that the desired path is parameterized as p d ϖ 0 = ϖ 0 , 50 * sin π * ϖ 0 /50 T and ϖ 0 = 5/ x p ′ 2 ϖ 0 + y p ′ 2 ϖ 0 , specifically, the group reference information can only be available to vehicle 1.The initial positions and velocities of the vehicles are assumed to be η 1 = 40, 80, π T , η 2 = 30, 0, π/4 T , η 3 = −40, −80, −π/2 T and ν 1 = 0 1,0, 0 T , ν 2 = 0 1,0, 0 T , ν 3 = 0 1,0, 0 T .The control gains are set as , and θ a = π/4, respectively.The simulation results were shown by Figures 3-6.From 3, we can find that all of the three UUVs can converge to the reference path and keep moving on them coordinately.Figure 4 shows the process of the dynamic speedsâ€™ responding, obviously, the vehicles can move with synchronized behaviors.Figures 5 and 6 indicate the path parameters' evolution and the errors between them.Results reveal that all the path parameters were driven to a synchronized state.
In order to show that the algorithm proposed can be used to arbitrary paths parameterized by a scalar variable, we took another simulation for a circle case.Suppose all the conditions and gains are the same as the previous example, and the reference path is set as p d ϖ 0 = 20 * cos π * ϖ 0 /200 + 3 * π/4 , 20 * sin π * ϖ 0 /200 + 3 * π/4 T .The evolution of the 3 UUVs' paths and the coordination error of the path parameters are depicted by Figures 7 and 8.We can also find that all the UUVs can follow the reference path coordinately.

Conclusions
This paper addressed the coordination path-following control problem of UUVs with underactuated dynamics by 6 Complexity proposing a consensus path-following strategy.The proposed algorithm was combined by single path-following law and a path parameter consensus tracking law.As for the single path following, a virtual reference point was defined and the according path errors were driven to zero asymptotically.At the meantime, the dynamics of the reference point was determined by the speed of each vehicle, which enabled us to have the freedom to design expected velocity for each UUV to realize the cooperative behaviors, and this can be regarded as the main idea of the algorithm proposed in this paper.We did not consider the influence of the external disturbance and the uncertainty of the model parameters; moreover, we assumed the topology between the UUVs is fixed and there are no communication delays.All the topics about the complex dynamic model for the UUV and the issues about multiple vehicles, such as dynamically changing topologies, time delays, and data dropouts will warrant our future research.
the position and heading of UUV in earth-fixed frame I ; ν i = u i , v i , r i T ∈ R 3 represents the speed in a body-fixed frame b ; R ψ i means the transformation matrix from frame b to I ; m 11 , m 22 , m 33 are mass and hydrodynamic added mass terms, and d 11 , d 22 , d 33 denote hydrodynamic terms.From (

Figure 1 :
Figure 1: Path-following frames and error variables.

3
Complexitywhere U ip represents the speed of the reference point in f along axis x.Considering the fact of r ip × e i

Figure 2 :Figure 3 :Figure 4 : 3 Figure 5 :
Figure 2: Topology of three UUVs with the reference information only being available to vehicle 1.

Figure 6 :
Figure 6: System coordinate path parameters error evolution.