Distributed Tracking Control for Connectivity-Preserving and Collision-Avoiding Formation Tracking of Underactuated Surface Vessels with Input Saturation

: This paper investigates the formation tracking control problem of a group of underactuated surface vessels (USVs) in the presence of model uncertainties and environmental disturbances. Additional constraints, such as collision avoidance, heterogeneous limited communication range and input saturation are also considered. A modiﬁed barrier Lyapunov function (BLF) is introduced to achieve the connectivity preservation, the collision avoidance and the distributed formation tracking. Extended state observer (ESO) is employed to estimate total disturbances consisting of environmental disturbances and model uncertainties. Auxiliary variables are introduced to deal with the underactuated problem and input saturation. A distributed controller is developed for each USV. Using the Lyapunov method analyze the stability of the system, it is proven that all signals are bounded and tracking errors converge to a neighborhood of the origin. Simulation results show that the proposed controller is practicable and effective.


Introduction
Formation tracking control of multiple underactuated surface vessels (USVs) has aroused great interest in recent years, owing to the fact that a team of USVs working together can accomplish more challenging missions than a single USV, such as surveillance, autonomous exploration, reconnaissance and perimeter security. A significant amount of research efforts has been focused on the control of multiple USVs. There are some challenges in USVs formation control, which are still worth mentioning. The first challenge is the amount of information being exchanged among the USVs in formation tracking control. In the beginning, USVs could sense their own positions, which are presented in a global coordinate system. Every USV controls its own positions to achieve the desired formation, which is prescribed by the desired positions in the global coordinate system [1][2][3]. The desired trajectory is available for all USVs. In this case, interactions are not necessarily needed because the desired formation can be achieved by position control of individual USVs [4]. This means that every USV should be equipped with advanced sensors, but this may not be practical. Considering the limitation of sensors, a distributed control law was proposed based on graph theory in [5][6][7][8]; it requires interactions to extract information from neighbors. The leader-following consensus problem of networked Lagrangian systems was investigated in [9]; both unknown control directions and uncertain dynamics were taken into consideration. Two types of distributed control protocols were proposed base on undirected graphs and directed graphs.
Output constraint is a challenge to USVs. A coordination strategy for multi-agent formation control based on a constraint function was proposed in [10]; this method could stabilize the formation error under a bounded tracking error assumption. A cooperative controller for a group of N USVs with limited sensing ranges was proposed in [11]; a novel potential function was used to solve the collision avoidance problem. Output-feedback cooperative controllers for mobile robots were designed in [12], where limited sensing was considered, and a control system based on potential functions incorporated with jump functions was designed. In [13], a group of USVs with a leader was considered, in order to make sure USVs function under asymmetric range and bearing constraints, the control design incorporated an asymmetric barrier Lyapunov function (BLF), and a fast convergent observer was designed to estimate the velocity of the leader. A nonlinearly transformed formation error without considering input saturation was developed in [14]; collision-avoiding, connectivity-preserving and limited communication ranges were considered simultaneously, and a distributed controller using the transformed error was designed for each USV.
In practice, designing a controller without considering the input saturation factor may lead closed-loop systems to instability. In [15], a basic controller base on two feedback functions was proposed, and the functions ensure the realization of the expected formation and input constraint. In [16], an output feedback controller was proposed by using additional controllers that are able to deal with the input saturation and underactuated problems simultaneously. In [17], an adaptive steering control method for uncertain ship dynamics with input constraint was designed; the method ensures the performance of the system under changing environmental conditions. In [18], uncertain strict-feedback nonlinear systems with input saturation and unknown disturbances were investigated, a dynamic surface control (DSC) combined with a backstepping method was proposed, and the effect of input saturation was approximated using a radial basis function neural network (RBFNN).
The model uncertainties and environmental disturbances are an important challenge. To overcome these difficulties, a hub motion estimation algorithm was designed in [19], where sensor fusion was employed. A controller that forces a USV to track arbitrary reference trajectories was proposed in [20]; a disturbance observer was presented to estimate environmental disturbances. In [21], an adaptive fuzzy controller for USV exposed to ocean currents and time-varying sideslip angle was designed, the dynamic uncertainties and environmental disturbances could be compensated by the fuzzy logic system. A practical adaptive sliding mode controller for an USV was proposed in [22], where an RBFNN combined with minimum the learning parameter method was designed to approximate the uncertain system dynamics online. In [23], neural network (NN)-based tracking control of underactuated systems was surveyed; unknown parameters and matched and mismatched disturbances were considered, and an adaptive control scheme incorporating multi-layer NNs was proposed.
In most of the above papers, the controller is designed by using the backstepping method, and Lyapunov function is used to analyze the stability of the system. In backstepping design, the computer explosion problem is universal. Tracking differentiators were used to solve the problem in [14].
Inspired by the above, in this paper we simultaneously consider model uncertainties, environmental disturbances, input saturation, collision avoidance and the limitations of communication distance. An extended state observer (ESO) is used for observing model uncertainties and environmental disturbances. A nonlinearly error transformation is provided for achieving the connectivity preservation, the collision avoidance and the distributed formation tracking. The USVs are interconnected through a directed communication network. Auxiliary variables are introduced to solve input saturation and the underactuated problem. Tracking differentiators are employed to calculating derivatives of virtual control variables. Finally, a distributed control law for each USV is constructed. The stability of the total closed-loop system is analyzed via Lyapunov theory.
The main contributions of this paper are summarized as follows. First, compared with [13], unknown model dynamics and environmental disturbances are estimated by ESO, and graph theory is combined with a distributed controller, which makes the controller suitable to be readily applied to formation control. Second, compared with [14], BLF is introduced into nonlinearly transformed formation error. In order to cope with input saturation, auxiliary variables are introduced into controller design.
The rest of this paper is organized as follows. The models of USVs, ESO and graph theory are introduced and the USV formation control problem is formulated in Section II. Section III proposes a distributed controller and presents the stability analysis. Simulation results are shown and discussed in Section IV. Section V summarizes.

Notion
The following notations will be used throughout this paper. | · | is the absolute value. λ min (.) and λ max (.) represent minimum and maximum eigenvalue of a square matrix, respectively. · represents the Euclidean norm. diag{·} is diagonal matrix. R m×n represents the m × n dimensional Euclidean Space. I n represents the n × n dimensional identity matrix. i is used as the index of the USVs, i.e., i = 1, . . . , n.

Graph Theory
Graph theory is used to describe the communication topology of n + 1 USVs. A directed graph describes that information of the jth USV is available to the ith USV. The ith USV and the jth USV are said to be neighbors if ρ ij < L j , where j = 0, . . . , n, i = j and ρ ij is the distance between the ith USV and the jth USV. The neighbors of the ith USV are described by N i (j) = {j ∈ V G , (i, j) ∈ Θ}. Assumption 1. The total graph G is directed at t = 0 and G has a directed spanning tree with the root node being the leader node.

Model of USVs
A group of USVs consisting of a leader and n followers are considered. Assume that the ith USV has an x i z i -plane of symmetry. Heave, pitch and roll motions are neglected. The body-fixed frame coordinate origin is set in the center-line of the USV. The mathematical model of the ith USV is defined as [24]:η where η i = [x i , y i , ψ i ] T is the vector denoting the ship position (x i , y i ) and yaw angle ψ i with coordinates in the earth-fixed frame, and ν i = [u i , v i , r i ] T is the vector denoting surge, sway and yaw velocities of the ith USV in the body-fixed frame.
T is the control vector of the ith USV, which consists of the surge force τ i,u and yaw moment τ i,r . The matrices J i (ψ i ) are given by is the inertia matrix of the ith USV. Here, we assume that the inertia matrices are diagonal. C i (ν i ) = −C T i (ν i ) ∈ R 3×3 represents a skew-symmetric matrix of Coriolis and centripetal term. D i (ν i ) ∈ R 3×3 is a nonlinear damping matrix.
The model of leader is defined as follows: a subscript "0" denotes the leader whose position Assumption 2. η 0 , u 0 , v 0 , r 0 are bounded, and the data are available only for the ith USV satisfying 0 ∈ N i (0). Assumption 4. C i (ν i ) and D i (ν i ) are assumed unknown.

Input Saturation
Considering input saturation, control vector τ i is defined as follows: where τ i,max and τ i,min ∈ R 3 are the maximum and minimum control force and moment, respectively. Define the mismatch function between input without saturation and with saturation as T with τ i,uc and τ i,rc are surge force and yaw moment calculated by the distributed controller, respectively. The saturated control in (1) is given by

Extended State Observer
In this section, an ESO is used for estimating total disturbances consisting of the unknown term of the system matrix C i (ν i ), D i (ν i ) and environmental disturbances d i [25]. The ith USV dynamic (1) is rewritten asη where The following assumption is made during ESO design.
The velocity v i is bounded, and control inputs to drive USVs are bounded, and thus the derivative of v i is bounded. According to Assumption 3 and the disturbances d i are bounded, the derivative of d i is bounded. Then, Assumption 5 is reasonable.
An ESO is used for estimating the total disturbances as follows:η are gain matrices. From (5) and (6), the error dynamics of the observer can be expressed aṡη (7) can be rewritten aṡX and Theorem 1. Consider the system (5) under Assumptions 2-5, the proposed observer (6) guarantees estimation error is bounded.
Proof. Consider the following Lyapunov function candidate as differentiating V io with respect to time and using (8), using Young's inequality and Assumption 5, Select the appropriate parameters K i,o1 , K i,o2 and K i,o3 to make sure λ min (A i ) > 1 2 . Equation (12) shows that the observer (6) ensures that the estimation error is bounded.

Barrier Lyapunov Function
Definition 1 ([26]). BLF is a scalar function, defined with respect to a systemẋ = f (x) on D, which is continuous, positive definite, and an open region containing the origin. BLF has continuous first-order partial derivatives at every point of D and the property V(x) → ∞ as x approaches the boundary of D, and satisfies V(x(t)) ≤ b, ∀t ≥ 0 along the solution ofẋ = f (x) for x(0) ∈ D and some positive constant b.
To deal with the output constraint, a BLF is introduced as where k > 0 is a constant and z is variable of error.

Problem Formulation
As shown in Figure 1, ρ ij is defined as the relative distance between the ith USV and the jth USV; its equation and differential equation are given as follows: λ ij is defined as the relative angle between the ith USV and the jth USV; its equation and differential equation are given below: The relative information ρ ij and λ ij can be measured by using local sensors, such as lidar and the gimbaled camera. If the ith USV and the jth USV are said to be neighbors, the ith USV is able to obtain the data η j and ν j directly.
The control objective is to design a distributed controller for the ith USV to track the leader with model uncertainties, input saturation and limited communication ranges. Specifically, it is to achieve the following two objectives.
where R = max{R i , R j }, R i and R j are minimum avoidance ranges of the ith USV and the jth USV, respectively. L = min{L i , L j }, L i and L j are maximum communication ranges of the ith USV and the jth USV, respectively. θ and θ are the minimum and maximum bearing angle of the ith USV.
where c 1 and c 2 are positive constants that can be made small enough.

Remark 2.
Objective (1) means that the connectivity preservation and the collision avoidance are considered if the ith USV is a neighbor of the jth USV. Objective (2) represents the formation tracking problem.

Controller Design
In this section, the distributed controller is designed to meet the requirements of connectivity-preserving and collision avoidance. To satisfy these requirements, nonlinearly transformed error surfaces are introduced as follows: where c ij = 1 if the ith USV and the jth USV are neighbors, otherwise c ij = 0. e ij,1 and e ij,2 will be explained later. ψ ij,a is the approach angles expressed as: where γ ij is a positive constant, β i,h , h = 1, 2, 3, is virtual control andβ i,h , h = 1, 2, 3, is the signal derived from the following first-order low-pass filters l i,hβi,h +β i,h = β i,h , and l i,h > 0 is constant. α i,h , h = 1, 2, 3, is a time-varying and bounded auxiliary variable derived to deal with the underactuated problem and input saturation.
Inspired by the proposed asymmetric BLF method in [26], a nonlinearly transformed formation error combined with modified BLF was developed to achieve the connectivity preservation and the collision avoidance. Error surfaces e ij,1 and e ij,2 are defined as follows: where

Remark 3.
The error surfaces e ij,1 and e ij,2 are introduced to realize the connectivity preservation and the collision avoidance. According to definition of objective (1) From definition of a ij,1 = R − ρ ij,d and a ij,2 = L − ρ ij,d , such that a ij,1 < ρ ij,e < a ij,2 . One notes that a ij,1 < 0 and a ij,2 > 0, thus ρ ij,e > a ij,1 if ρ ij,e < 0 and ρ ij,e < a ij,2 if ρ ij,e > 0. It holds that the connectivity preservation and the collision avoidance are achieved as ρ ij,e > a ij,1 if ρ ij,e < 0 and ρ ij,e < a ij,2 if ρ ij,e > 0. From Equation (32), the definition of e ij.1 implies −a ij < ρ ij,e < a ij . According to the definition of a ij , ρ ij,e < a ij,2 if ρ ij,e > 0, a ij,1 < ρ ij,e if ρ ij,e < 0. Then the distance constraint R < ρ ij < L can be satisfied. The angle constraint θ < λ ij < θ is similar to the distance constraint.
A distributed controller using the nonlinearly transformed error is presented.
Step 1: Differentiating q i,e along (17) and (19) yieldṡ where Then,q i,e is rewritten as follows: where and  In order to stabilizeq i,e , the desired virtual control β i is given as: where h i,1 , . . . , h i,B are the elements of the set h i = {j|c ij = 0}, P i,h i = [ρ ih i ,e , λ ih i ,e ],f i,2 = [0, tanh( f i,2 / i,1 )] T , and i,1 > 0 is a constant. Notice that the matrix B i is invertible owing to ρ ij = 0. Notice that based on Lemma 1, we have ln k 2 k 2 −z 2 < z 2 k 2 −z 2 and define k i,1 = λ min (K i,1 ), then −A i K i,1 P i < −2k i,1 q i,e . Step 2: Differentiating ψ i,e along (21) yieldsψ where ϑ i,1 = ∑ n j=0,j =i c ij and ϑ i,2 = ∑ n j=0,j =i c ijψij,a .
The stabilizing function β i,3 is chosen as: where k i,2 is a positive constant.

Remark 5.
The virtual controllers (46) and (49) are composed of the error surfaces (20) and (21), the distributed vectors P i , and matrices Ξ i,1 and Ξ i,2 . The link weights c ij of the distributed error surfaces (20) and (21) depict the directed graph among USVs that satisfies Assumption 1. Thus, the proposed formation approach is in a fully distributed formation manner.
From (74), q e = [q 1.e , . . . , q N,e ] T exponentially converges to the compact set ∏ = {q e | q e ≤ 2µ 2 /µ 0 } that can be made arbitrarily small by adjusting u 0 . From (20), e ij,1 and e ij,2 can be reduced to be arbitrarily small; this leads to the conclusion that ρ ij,e and λ ij,e can be also reduced to be arbitrarily small. Then, it holds that lim x→+∞ |ρ ij (t) − ρ ij,d | ≤ c 1 and lim x→+∞ |λ ij (t) − λ ij,d | ≤ c 2 , and the proof of (2) is completed.
The distributed controller is composed of the local virtual control laws (46) and (49) and the auxiliary dynamics and the actual control laws (56) and (57). In (1), there is no control input on the sway dynamics, and this causes difficulty in the control of sway. To make sure of the stability of the sway dynamics, auxiliary variable α i,2 is introduced to solve the problem, and auxiliary variables α i,1 and α i,3 are introduced to solve the problem of input saturation. The tanh(.) function guarantees that the introduced variables tanh(α i,h ), h = 1, 2, 3 are smooth and differentiable. By designingα i,2 in (54) and substituting it into (59), the boundedness and convergence of v i,e can be achieved.
According to (72), the following conditions determine the stability of the entire system: k i,1 > 0, k i,2 > 1 2 , k i,3 > 1 2 , k i,4 > 1 2 , k i,5 > 1 2 , and λ min (A i ) > 1 2 +m. According to (13), the size of the eigenvalues of parameters K i,o1 , K i,o2 and K i,o3 determines the convergence speed of estimation error. Note that λ min (A i ) affects both the stability of the observer and of the whole system.

Simulation Results and Discussion
In this section, the formation is composed of one leader and four followers. From the defined heterogeneous communication and avoidance ranges, the directed communication graph is given in Figure 2. The model of USV is taken from [28], and the main parameters are M i = diag{23.8, 33.8, 2.764}, Environmental disturbances are chosen as a superposition of zero mean white noise and constant interference. Standard deviation of white noise is chosen as 0.4, constant interference is chosen as 0.1. The parameters for ESO are set to K i,o1 = 7 · I 3 , K i,o2 = 45 · I 3 , and K i,o3 = 80 · I 3 . The control forces and moment are constrained as τ i,max = [τ i,u max , 0, τ i,r max ] T , τ i,min = [τ i,u min , 0, τ i,r min ] T , τ i,u max = −τ i,u min = 2N, and τ i,r max = −τ i,r min = 1.5Nm. The design parameters of control laws are chosen The formation tracking result is shown in Figure 3. Figures 4a,c, 5 and 6 show the the distance errors ρ ij − ρ ij,d , and Figures 4b,d, 7 and 8 show the angle errors λ ij − λ ij,d . As shown in Figure 3, one can see that all the USVs are able to track the formation. Figures 4-8 show trends of relative distance and angle, and the connectivity-preserving and collision-avoiding is achieved. Figures 9-12 depict the control inputs of the follower. In the beginning, since the initial heading angles of followers are 0 rad, all followers turn around and travel in the opposite direction for a certain period of time, then the followers take a turn to avoid collision, and the control inputs τ i are saturated and suffer from sudden jumps. The control inputs τ i become smaller and unsaturated when the follower catches up with the leader.            Comparison with the controller proposed in [14] is given though Matlab simulation in Figures 13-16; the simulation result is provided to validate the effectiveness and feasibility of the proposed controller. Comparison is designed under the same environmental disturbances, and the standard deviation of white noise is chosen as 0.4 while the constant interference is chosen as 0.1, with the same initial position η 1 (0) = [0 m, −5 m, 0 rad] T and the same objective ρ 10,d = 5 and λ 10,d = −25π/180 rad. Figure 13 shows that USV 1 could track the leader accurately in a different controller. Figure 14 shows that the controller proposed in [14] accomplishes faster convergence and tracing speed. Not taking input saturation into consideration, the result is a more aggressive tracking trajectory, which is difficult to implement. In Figure 15, control inputs under different controllers are given. A local enlarged drawing of the control input is given in Figure 16. In the beginning, the input signal in this paper is saturated, and the position of the USV changes relatively slowly. At about 60 s, the follower catches up with the leader. The input signal in [14] is huge, and the position of the USV changes rapidly. At about 10 s, the follower has caught up with the leader. Unlike the controller proposed in this paper, it is extremely difficult to avoid a huge input signal and a drastic change of input signal.  By using the modified barrier Lyapunov function, the connectivity preservation and the collision avoidance are achieved. By using ESO and the proposed controller, the followers can track the leader accurately. By using an auxiliary variable, the input saturation is solved. Positive definitions of matrix K io1 , K io2 and K io3 make sure that convergence of estimation error is achieved. By adjusting u 0 , the track error can be made arbitrarily small.
Both controllers are designed based on Assumption 2, and η 0 , u 0 , v 0 , r 0 are bounded, meaning that the formation control problem is solved only when the follower starts in a certain neighborhood of the leader. In [29], the path following the control problem of USVs was investigated and the system could provide global asymptotic stability. How to design a controller which could makes the system have global asymptotic stability is an interesting challenge. Both controllers are designed without considering the saturation rate of the actuator, meaning that the conclusion is relatively radical. How to design a controller with the rate saturation factor for formation control will be considered in future work.

Conclusions
This paper proposed an error-transformation-based design strategy for tracking control for multiple USVs with limited communication ranges and input saturation. ESO was used for estimating model uncertainties and unknown disturbances. The distributed tracker for each follower was designed by using a modified BLF. Auxiliary variables were used to solve input saturation and underactuated problems. From the Lyapunov stability sense, all error signals in the closed-loop were bounded. A simulation verified the effectiveness of the proposed distributed controller.
Author Contributions: Conceptualization, G.X. and B.Z.; Investigation, X.X.; Methodology, X.X. and X.S.; Software, C.S. All authors have read and agreed to the published version of the manuscript.