Multi-under-Actuated Unmanned Surface Vessel Coordinated Path Tracking

Multi-under-actuated unmanned surface vehicles (USV) path tracking control is studied and decoupled by virtue of decentralized control. First, an improved integral line-of-sight guidance strategy is put forward and combined with feedback control to design the path tracking controller and realize the single USV path tracking in the horizontal plane. Second, graph theory is utilized to design the decentralized velocity coordination controller for USV formation, so that multiple USVs could consistently realize the specified formation to the position and velocity of the expected path. Third, cascade system theory and Lyapunov stability are used to respectively prove the uniform semi-global exponential stability of single USV path tracking control system and the global asymptotic stability and uniform local exponential stability of coordinated formation system. At last, simulation and field experiment are conducted to analyze and verify the advancement and effectiveness of the proposed algorithms in this paper.


Introduction
Unmanned surface vehicle (USV) is a small intelligent mission platform on the water surface, and has such advantages as small size, low cost, high speed, intelligence, small radar cross section, and no personal injury or death [1]. In the military domain, it could realize flexible deployment and combat. Equipped with different mission modules, it could complete a variety of missions such as information gathering, surveillance and reconnaissance, target strike, minesweeping, anti-submarine, and damage assessment. USV could set out from the port and fulfill its mission as scheduled, or sail with combat warships to complete the mission independently when combat warships are not suitable for the mission or in a dangerous sea area. Therefore, it has got great attention from navies around the world since it could greatly expand the range of naval combat, and become an "amplifier" for own combat effectiveness [2]. In the civil domain, USV could be used for hydro-geological survey, search and rescue, relay communication, etc., and it is a key node for integrating low-altitude unmanned aerial vehicle and underwater robot across network. Therefore, its application has a bright prospect [3].
The study of USV coordinated formation control started quite late, but has great potential for actual application. In the civil domain, it could be performed to not only coordinate the hydrologic and topographic surveys in a large sea area, but also complete the pavement and patrol of subsea pipeline. In the military domain, it could be conducted for large minefield detection and clearing, and tactical formation could be employed to block the enemy warship or port, and enhance the combat efficiency and the success rate of enemy target strike. Therefore, it is of great significance. network was utilized to online approach the uncertain terms caused by external disturbance, and eventually Lyapunov stability theory was employed to prove the uniform semi-global stability of closed-loop control system. However, leader's breakdown may lead to the collapse of the formation so that the leader-follower mode is not applicable to practical engineering applications [13]. In 2016, Shojaei further utilized multi-layer neural network and adaptation technology to perform the real-time estimation and compensation for the model perturbation caused by external disturbance on the basis of Reference [13], so as to guarantee the stability of formation controller. In the end, Lyapunov theory was taken as the basis to prove the uniformly ultimate boundedness for all the states and signals of control system [14]. In 2017, Nair et al. proposed an autonomous robot formation control algorithm based on artificial potential field path planning and quick adaptive gain nonsingular terminal sliding mode control, and introduced the idea of tolerance control to ensure the control stability of the entire formation during the failure of single entity. The algorithm was proved feasible in the simulation experiment. Nevertheless, they considered only the position of members in the formation, and paid no attention to the tracking accuracy of each USV with regard to the expected path [15]. Xiao et al. (2017) proposed a combination of unmanned aerial vehicle (UAV) and USV for maritime search and rescue. In the combination, UAV could provide a wider range of vision to enhance the situation perceptibility of USV. However, their study focused on the applications using both UAV and USV for search and rescue, and did not further explore the USV control method. Therefore, the combination is not applicable to the problem of USV formation control, but it provides a new approach to the future studies on USV formation control [16]. In 2018, Ghommam et al. designed a robust adaptive USV formation controller based on Lyapunov's direct and backstepping methods and following the line-of-sight guidance strategy, to cope with the unknown velocity of leader and the constraint of turning angle on the members in the USV formation control. Based on Lyapunov stability theory and finite time theory, they proved that formation tracking error could converge into a small adjacent domain of approximately 0 within the finite time, while comparative simulation experiment was conducted to verify the effectiveness of the algorithm. However, they did not take into account the problem of collision prevention among formation members and the influence of leader's failure on formation control in the leader-follower mode [17].
Generally, formation control is often achieved using leader-follower method [18], behavior-based method [19], and virtual structure method [20]. In the leader-follower formation control, robots in the formation are appointed to two complementary roles, i.e., leader and follower. A follower tracks the trajectory of the leader. The formation control is realized in this way. This method could simply control the entire formation by specifying the leader's behavior or trajectory, but the leader does not pay attention to other robots. Therefore, there is not any feedback on the formation in the system. In other words, the leader and followers are independent from each other, and it is not easy for the leader to obtain the feedback on tracking error from followers [21][22][23][24][25]. In the behavior-based formation control, each kind of behavior has a specific objective and mission. When there are several different objectives, it is easy to determine the collective behavior control strategy. Each robot responds based on the position of other robots, so that there is specific formation feedback. Additionally, this method allows distributed control in a real-time manner. However, it could not clearly indicate the principles for local control of collective behavior, and not guarantee the stability of formation control [26][27][28]. Virtual structure method utilizes the virtual structure of rigid object and the fixed positions in the structure to realize formation control. This method could easily determine the formation control strategy and give formation feedback. It is quite intelligent, and guarantees the stability of formation to some extent. However, it requires the motion of the formation in a virtual structure, so that it is less flexible and adaptive [29][30][31]. Moreover, there are also other formation control methods. For instance, artificial potential field [32] could effectively shun obstacles and avoid collisions when there are obstacle constraints. It features easy calculation and convenient real-time control, but it is difficult to design a reasonable potential field function, and involves local extreme value points. Graph theory [33] could be used to represent any formation, and there are some well-developed formal theories such as algebraic graph theory. Nevertheless, it is very complicated to implement the method, so that it is only limited to simulation research. Model predictive control [23] is a method with strong theoretical basis and massive computation, so that it is very difficult to implement, and not applicable to practical engineering.
In the abovementioned formation control methods, coordination control law is designed only to realize the relative position or distance of USVs as expected and maintain a specific formation for mission execution, but no restriction is imposed on the motion track of each USV. In most references, the design of formation controller does not include external disturbance and uncertainties in USV mathematical model. Most references describe the verification of algorithm only by virtue of simulation, but rarely put it into the actual cruising test of USV coordinated formation.

Main Contributions
This paper focuses on formation path tracking in the problem of formation control, and addresses the problem of coordinated path tracking for multiple under-actuated USVs subject to external disturbance. With the decentralized control strategy, the problem of multi-USV coordinated path tracking is decoupled into the design of single USV path tracking controller and multi-USV coordination controller. In this way, each USV is controlled to overcome the influence of external disturbance and realize accurate path tracking while maintaining the expected formation. Hence, this approach is suitable for some engineering applications with higher requirements for the USV formation path tracking accuracy.
Based on Reference [34], this paper first builds an asymmetric kinematic and kinetic model with three degrees of freedom for under-actuated USV subject to external disturbance in the horizontal plane. At present, the nonlinear mathematical model of USV with three degrees of freedom is employed nearly in all references available at home and abroad, so that this model could accurately present the motion of USV in the horizontal plane. In many other works, this model has been also used, but it is simplified for controller design to different degrees. In this paper, this model is employed but not simplified.
Second, integral line-of-sight guidance is mainly employed as a strategy to address the problem of single USV control. The strategy is further improved by changing the fixed headway range to the time-variant adaptive headway range. Time-variant headway range allows more flexible motion of USV. When USV is very far away from the expected path, tracking error is large, but headway range is small, making USV approach the expected path fast. When USV is close to the expected path, tracking error is small, but headway range is large, which could effectively reduce the position error overshoot. Meanwhile, the influence of USV under-actuation, uncertainty and external disturbance is taken into account comprehensively to design the heading and velocity controller of single USV based on the idea of feedback control. In this way, the expected heading given by the guidance system is tracked so that USV could maintain the accurate and stable tracking of the expected path, which resolves the problem of lower path tracking accuracy caused by time-variant disturbance in the traditional ILOS algorithm. As for multi-USV coordination control system, this paper utilizes the graph theory to design the decentralized velocity coordination controller for USV formation. Therefore, each single USV could accurately track the expected path, while the velocity of each USV is controlled in a decentralized manner to quickly realize the USV formation and maintain it in a coordinated and consistent way.
Subsequently, Lyapunov theory is employed to verify the uniform semi-global exponential stability of single path tracking control system, while guidance system and heading and velocity control system are constructed into a cascade system. The theory of cascade system is utilized to prove the global asymptotic stability and uniform local exponential stability of multi-USV coordinated formation control system. Compared with the global K-exponential stability described in most references, this approach offers faster convergence and stronger robustness.
At last, comparative simulation experiment and USV maritime test are conducted to analyze and verify the advancement and effectiveness of the proposed algorithm in this paper. This paper is organized as follows: Section 1 analyzes and summarizes the current status and shortcomings of the research on single USV and formation control, and explains the research purpose of this article; Section 2 establishes an asymmetric mathematical model with three degrees of freedom subject to external disturbance for under-actuated USV. In order to avoid controller design difficulties, the model is coordinate transformed, and the USV longitudinal error formula is derived by synthesizing the model and the expected path description; Section 3 decouples the USV formation path tracking problem into a single USV path tracking problem and a multi-USV collaborative formation problem. Through analysis and summary, the control goals of the single USV path tracking and multiple USV collaborative formation are presented, which provides a basis for the controller design below; In order to achieve the control goals proposed in Section 3, Section 4 combined the mathematical model and error formula given in Section 2 and designed single USV path tracking controller and multiple USVs collaborative formation controller based on the improved ILOS guidance strategy, feedback linearization method, and graph theory knowledge; Section 5 proved mathematically the stability of the controller designed by Section 4 through cascade theory and Lyapunov theory, that is, the control target proposed by Section 3 was reachable; Section 6 applies the controller designed in Section 3 to simulation experiments and real ship experiments, and compares it with the single USV control algorithm under traditional ILOS guidance strategy and the formation control algorithm without considering model perturbation and external interference. The result chart proves the effectiveness and advancedness of the algorithm in this paper. The effectiveness of the engineering application of the algorithm in this paper is verified by the test of three USV real ships organized in actual sea area. Section 7 summarizes the work of this paper.

Under-Actuated USV Model
Considering the irrotational flow of seawater in the geodetic coordinate system (hereinafter referred to as {i} system), and the constant velocity V c = [V x , V y , 0] T , there is a resultant velocity Hence, the kinematic and kinetic model of under-actuated USV with three degrees of freedom is expressed in the following way: where η = [x, y, ψ] T is the horizontal position and heading angle of USV under the {i} system; v r = [u r , v r , r] T is the longitudinal and transverse linear velocities and the relative velocity of heading angle when USV is under the hull coordinate system (hereinafter referred to as {b} system). The schematic diagram is shown in Figure 1. In the figure, U = u 2 r + v 2 r is the resultant velocity of USV relative motion under the geodetic coordinate system; heading angle ψ is the angle formed by the bow and the OX axis of inertial coordinate system; track angle χ = ψ + β is the angle formed by the velocity U and the OX axis of inertial coordinate system; drift angle β is the angle formed by the surge velocity u and the velocity U of USV, or the angle formed by the track angle and the heading angle; U c is the resultant velocity of irrotational ocean current under the geodetic coordinate system. J(ψ) is the rotation matrix from the {b} system to the {i} system; M is the inertial parameter matrix of under-actuated USV; C(v) = C RB (v) + C A (v) with C RB (υ r ) and C A (υ r ) as the respective matrixes of Coriolis force and centripetal force applied by rigid body and hydrodynamics, and C RB (v) is unrelated to the velocities u and v; D(υ) is damping parameter matrix; τ = [T u , T r ] T is control input matrix with T u as forward Sensors 2020, 20, 864 6 of 33 thrust and T r as yawing torque; and vector matrix B is control input configuration matrix. They are defined as follows [34]: At present, the nonlinear mathematical model of USV with 3 degrees of freedom (DOF) is employed nearly in all references available at home and abroad, so that this model could accurately present the motion of USV in the horizontal plane. In many other works, this model has been also used, but it is simplified for controller design to different degrees. In this paper, this model is employed but not simplified.
At present, the nonlinear mathematical model of USV with 3 degrees of freedom (DOF) is employed nearly in all references available at home and abroad, so that this model could accurately present the motion of USV in the horizontal plane. In many other works, this model has been also used, but it is simplified for controller design to different degrees. In this paper, this model is employed but not simplified.

Coordinate Transformation
The influence of stock torque on the transverse motion of USV makes it difficult to design the heading and velocity controller. To prevent this influence, the center of USV in the { }

Coordinate Transformation
The influence of stock torque on the transverse motion of USV makes it difficult to design the heading and velocity controller. To prevent this influence, the center of USV in the {b} system is transformed into the position of pivoting point.
where F u r (u r , v r , r) = m 22 (v r − εr)r + m 23 r 2 m 11 − d 11 u r m 11 (14) X(u r ) = After coordinate transformation, Equation (12) shows that . v r has no direct actuation. Hence, the USV studied in this paper is under-actuated.

Problem Description of Path Tracking
The planned path is an expected path formed by a number of track reference points under the geodetic coordinate system P(Ω), where Ω is parameter variable and Ω ≥ 0. It satisfies the parameter update rate point on the parameter path (x p (Ω), y p (Ω)) and the longitudinal axis X p of the {i} system. The angle is positive when it is clockwise. The expression is as follows: The current position of USV in the {i} system is P(t) = (x, y) T , and the expected velocity is u rd . In combination with Figure 2, the longitudinal tracking error is as follows: Sensors 2020, 20, x FOR PEER REVIEW 8 of 34 From Equation (18), we obtain: Based on the geometry in Figure 1, it is learned that: Equations (21) and (22)

Control Objectives
The problem of multi-USV formation control could be broken down into the problem of single Equation (19) is differentiated as follows: .
From Equation (18), we obtain: Based on the geometry in Figure 1, it is learned that: Equations (21) and (22) are substituted into Equation (20) to obtain: . y e (t) = u r sin ψ + v r cos ψ + V y cos(γ p (Ω)) Sensors 2020, 20, 864 where U is the resultant velocity in the {i} system, and β = arctan(v r , u r ) is the lateral sliding angle generated by the transverse velocity of USV and the disturbance in the external environment.

Control Objectives
The problem of multi-USV formation control could be broken down into the problem of single USV path tracking and the problem of multi-USV coordinated formation. First of all, a path tracking controller is designed for single USV to realize the control objective of spatial motion converging to the expected path. After that, the forward motion velocity of each USV is adjusted considering the USV path tracking velocity and position to realize the control objective of multi-USV coordinated formation.

Single USV Path Tracking Control Objective
Considering the above analysis of path and model, this paper employs the integral line-of-sight guidance subsystem and the heading control subsystem cascade. Guidance system could be used to determine the heading of under-actuated USV based on the real-time position, attitude information and planned route of under-actuated USV. Heading control subsystem could determine the expected steering gear manipulation signal based on the deviation between expected heading and actual heading of guidance system, so as to control the under-actuated USV and make its path tracking position and heading converge to the equilibrium position. Heading control subsystem controls the velocity of under-actuated USV converging to the expected velocity.
Therefore, the expected control objective in this paper could be described as follows: where ψ d is the heading reference signal given by guidance subsystem, i.e., expected heading angle.

Coordinated Formation Control Objective
As shown in Figure 3, in order to realize the USV formation coordination control, it is considered that the constant d > 0 represents the expected distance between USVs. A controller is designed to ensure that the USV distance eventually converges to d. Therefore, the problem of formation system control could be described as follows: The USV forward velocity u r converges to the expected velocity u d . In other words, longitudinal velocity error u e converges to 0. Therefore, there is: Above all, the problem of multi-USV formation coordination path tracking could be divided into: 1.
Designing a single USV path tracking controller for USV control to realize the control objective Equations (24) and (25), of position and heading, and move along the x axis at the given velocity u c j ;

2.
Designing a controller for u c j to coordinate USVs for the expected distance d on the x axis, so as to meet the coordinated control objective Equation (26) and control the formation velocity tracking to the expected velocity u d .

Design of Path Tracking Controller Based on Adaptive Integral Line-of-Sight Guidance Algorithm
In the traditional line-of-sight (LOS) algorithm, the current position of USV is projected on the expected route and a tangent is made at the point to define the headway range on the tangent and obtain the virtual target point related to the reference point. The current position of USV and the virtual point are the LOS vectors. The resultant velocity direction of USV is guided to align with the LOS vectors for tracking. The LOS algorithm does not rely on the USV model in principle, so that it is slightly affected by the algorithm. Hence, it does not have very high requirements for parameter design. Additionally, expected heading could be calculated only using the real-time position and expected route of USV. The expected heading could be obtained and output in a real-time manner to the control system. Nevertheless, the traditional LOS algorithm may be affected by time-varying disturbance, so that its accuracy is poor during the tracking process. For this reason, integral term should be introduced to make compensation for disturbance in a real-time manner, so as to realize higher accuracy. The integral LOS (ILOS) guidance strategy under the relative velocity in the traditional algorithm could be expressed as follows: .
The adaptive headway range ∆ is designed to improve the ILOS and define it as the function of longitudinal tracking error: where σ is the constant parameter that is strictly greater than 0 [7]; ∆ min is the minimum headway range, and ∆ χ = ∆ max − ∆ min is the maximum increment of headway range. When USV deviates far away from the planned route, the tracking error y e is high, and lim y e (t)→∞ e −σ(y e (t)) 1/2 ≈ 0. At this time, headway range is ∆ = ∆ min , so that USV approaches the given expected route fast. When USV gets close to the planned route, the tracking error y e is low, and lim y e (t)→0 e −σ(y e (t)) 1/2 ≈ 1. At this time, headway range is ∆ = ∆ max , so that USV could keep the stable tracking on the given track.

Design of Heading and Velocity Controller
In this section, the feedback linearization design control law is followed to make the USV heading and velocity converge to the expected values.
Based on the expected heading ψ d and expected angular velocity . ψ d given in the guidance subsystem [35], Equation (11) is used to obtain the heading control law as follows: where k 1 and k 2 are the constants that are strictly greater than 0. Equation (13) is used to design the velocity control law as follows: here k 3 is a constant that is greater than 0, which ensures that velocity exponent converges to the expected value.

Design of Coordinated Formation Controller
The influence of seawater flow on the design of path tracking controller has been taken into account. Therefore, V x is ignored in the design of coordination controller. Equation (8) is transformed to obtain: where u c is velocity reference command, u e = u r − u c .
Because of ψ = ψ d + ψ e = − tan −1 (y e /∆) y e y e + ψ e , it is substituted into Equation (33) to obtain: where According to the trigonometric Moreover, the exponent of the element in the vector converges to 0 under the path tracking controller designed in the above section. Hence, the velocity u(t) on the direction of the x axis asymptotically converges to the reference command velocity u c (t). By then, the controller u c j , j = 1, . . . , n is designed to make USV formation realize the coordination control objective (27), and satisfy the reference command velocity. Assuming that there is h > 0, the expected velocity u d (t) satisfies: The controller is designed as follows: where d ji = x d j − x d i represents the distance between the jth USV and the ith USV in parallel with the x axis. Γ(·) is the bounded continuous differentiable increasing function and satisfies Γ (0) > 0, Γ(0) = 0 and Γ(x) ∈ (−h, h).

Stability of Guidance System
During the process of USV motion, β varies with time under the change of heading angle ψ and transverse velocity v r . Compared with the motion of USV, such change is very slow. The following assumptions could be given: Assumption 1. β is a constant and varies very slightly, i.e., . β = 0. Therefore, Equation (23) could be written as: .
After defining ψ = ψ − ψ d as the heading angle error of USV, Equation (37) could be rewritten as: .
, Equation (29) is substituted into Equation (49) to obtain: Therefore, guidance subsystem has uniform global asymptotic stability (UGAS) at the equilibrium point y e (t) = 0.
Additionally, there is µ > 0. When y e (t) satisfies D = y e (t) ∈ R y e (t) ≤ µ , . Therefore, guidance subsystem has uniform local exponential stability (ULES) at the equilibrium point y e (t) = 0. UGAS and ULES are equivalent to the global K-exponential stability (GKES), and their stability is conceptually stronger than UGAS but weaker than global exponential stability (GES).

Stability of Heading and Velocity Controller
The formula for dynamic tracking error of heading and velocity control subsystem is as follows: ψ − . ψ d , and u r = u r − u rd are the tracking errors of heading angle, heading angular velocity, and velocity, respectively.
When the system is linear and time-invariant, and k 1 , k 2 , and k 3 in the coefficient matrix are all greater than 0, the real parts of all characteristic roots in the system are negative. The coefficient matrix of the system is Hurwitz matrix. Hence, the equilibrium state of the system ( u r , ψ, r) = (0, 0, 0) is global exponential stable [35,36], i.e., the control objectives lim t→∞ ψ(t) = ψ d , lim t→∞ u(t) − u d (t) = 0 are achieved.

Stability of Path Tracking System
The position error dynamic equation and heading and velocity error dynamic equation are written in the following cascade: 1 : . y e (t) = − U(y e (t)+∆y int ) .
When the system 1 has uniform semi-global exponential stability and the system 2 has uniform local exponential stability, the closed-loop control system 1 − 2 has uniform global asymptotic and uniform semi-global exponential stability at the position (y e , u r , ψ, . ψ) = (0, 0, 0, 0). Verification: As a result of φ(y e , ψ) ≤ 1.73 , the link term g(t, y e , ξ) of the system 1 − 2 satisfies: Since the systems 1 and 2 have been proved to have uniform semi-global exponential stability and uniform local exponential stability in the above section, the system 1 − 2 has uniform semi-global exponential stability at the position (y e , u r , ψ,

Stability of Coordinated Formation Controller
Equation (36) is substituted into Equation (34) to obtain: Let x j = x j − x d j , j = 1, . . . , n, so that Equation (56) could be written as: Thus, Equation (57) could be rewritten as:x where ω = [ T 1 , . . . , T n ] T , Z is block diagonal matrix. The diagonal element is z j , j = 1, . . . , n. Since z j is bounded, Z is bounded. The matrix L is the Laplacian matrix of communication topological graph G, so that L = l ji ∈ R n×n . In the meanwhile, there is: where δ j is the out-degree of the vertex j. As shown in Equation (59) (1), it is assumed that u d (t) is continuously differentiable, and satisfies Equation (35); u cj (t) satisfies u cj (t) ∈ (u min , u max ), j = 1, . . . , n; and the communication topographic graph G between USVs has at least one globally reachable point. In the USV formation, the initial position of each USV is random. Under the effect of controller Equations (30), (31), and (32), USVs move at the expected velocity obtained from Equation (36). Eventually, all exponents could converge to the expected path and the expected formation is realized. In other words, control objective Equations (24) and (25) are fulfilled.
Verification: It is assumed that the communication topographic graph G in Equation (58) contains s globally reachable points, and 1 ≤ s ≤ n. The matrix L could be written as: where L 3 ∈ R s×s is Laplacian matrix, and its element is globally reachable. Hence, G(L 3 ) is strongly connected. L 1 ∈ R (n−s)×(n−s) is semi-Hurwitz, and satisfies: where P is positive definite and a diagonal matrix.
L 3 is semi positive definite, and contains a zero eigen-value. The zero eigen-value corresponds to the eigenvector 1 s = [1, . . . , 1] ∈ R s . Thus L 3 could be divided into L 3 = M 3 M T 3 and M 3 ∈ R s×(s−1) . Therefore, µ = T x could be rewritten as: Therefore, .
Let V = ln(V + 1), V is calculated to obtain: where C 1 > 0, and Γ 1 (µ 1 ) is globally bounded, Equation (61) is verified. In other words, Theorem 2 is verified. Above all, USV formation system could tend to be stable under the control of the controller Equation (36). Hence, Theorem 1 is verified. The verification of its stability fulfils a control objective limx j (t) − x i (t) − d ji = 0, j, i = 1, . . . , n, i.e., USV formation could converge to the expected formation and be maintained under the effect of formation coordination controller.
The verification ends.

Simulation and Experiment
To verify the effectiveness of the proposed control approach in this paper, the USV model in Reference [36] is employed to perform the simulation experiment for single USV and formulation path tracking. It is assumed that each USV in the simulation satisfies the following model parameters of USV, as shown in Table 1:

Single USV Simulation Experiment
A USV path is straight and curved. In this section, simulation experiment is conducted using the path tracking algorithm with the traditional LOS guidance strategy and the control algorithm with the proposed ILOS guidance strategy in this paper to compare straight and curved paths. Among them, the parameters for heading and velocity subsystem are k 1 = 5, k 2 = 3, and k 3 = 5. The parameters for guidance system are selected as follows: under the traditional LOS strategy, there is γ = 0.005 and ∆ = 8 m; under the proposed strategy in this paper, there is γ = 0.0009, δ = 3, ∆ min = 6 m, and ∆ χ = 8 m. The simulation is presented in Figures 4-15. Among them, the control algorithm with the traditional guidance strategy is represented by the subscript 1, and abbreviated as Algorithm 1, while the control algorithm with the proposed strategy is indicated by the superscript 2, and abbreviated as Algorithm 2.                                 As revealed in Figures 4 and 10, Algorithms 1 and 2 could both guarantee the rapid convergence of USV to the straight and curved paths. Compared with Algorithm 1, Algorithm 2 has faster convergence. At the position with the largest curvature variation of the curved path, Algorithm 2 has lower oscillation and overshoot than Algorithm 1.
As shown in Figures 5 and 11, two algorithms could guarantee that the longitudinal position error of USV e y converges to approximately 0. However, Algorithm 2 has shorter oscillation and lower amplitude than Algorithm 1.   As revealed in Figures 4 and 10, Algorithms 1 and 2 could both guarantee the rapid convergence of USV to the straight and curved paths. Compared with Algorithm 1, Algorithm 2 has faster convergence. At the position with the largest curvature variation of the curved path, Algorithm 2 has lower oscillation and overshoot than Algorithm 1.
As shown in Figures 5 and 11, two algorithms could guarantee that the longitudinal position error of USV e y converges to approximately 0. However, Algorithm 2 has shorter oscillation and lower amplitude than Algorithm 1.   As revealed in Figures 4 and 10, Algorithms 1 and 2 could both guarantee the rapid convergence of USV to the straight and curved paths. Compared with Algorithm 1, Algorithm 2 has faster convergence. At the position with the largest curvature variation of the curved path, Algorithm 2 has lower oscillation and overshoot than Algorithm 1.
As shown in Figures 5 and 11, two algorithms could guarantee that the longitudinal position error of USV e y converges to approximately 0. However, Algorithm 2 has shorter oscillation and lower amplitude than Algorithm 1. (1) Straight path The expected route of USV is y = 0, and the expected velocity is u d = 3 m/s. The initial position of USV is set to (30,−100). The initial heading is ψ 0 = π/4 rad, and the initial velocity is u r = 0.5 m/s. The flow rate of seawater is set to U c = 0.2 m/s, and the flow direction is ψ c = 30 • .
As revealed in Figures 4 and 10, Algorithms 1 and 2 could both guarantee the rapid convergence of USV to the straight and curved paths. Compared with Algorithm 1, Algorithm 2 has faster convergence. At the position with the largest curvature variation of the curved path, Algorithm 2 has lower oscillation and overshoot than Algorithm 1.
As shown in Figures 5 and 11, two algorithms could guarantee that the longitudinal position error of USV y e converges to approximately 0. However, Algorithm 2 has shorter oscillation and lower amplitude than Algorithm 1.
After comprehensively analyzing Figure 6, 7, 12, and 13, two algorithms could guarantee the velocity stability of USV approximately u d = 3 m/s in the tracking of both straight and curved paths, but Algorithm 1 has larger oscillation amplitude of the forward thrust in the early section of the track than Algorithm 2. It means that Algorithm 2 has lower initial thrust than Algorithm 1. For this reason, Algorithm 2 could control USV to achieve the expected velocity at the lower expense of fuel.
After comprehensively analyzing Figure 8, 9, 14, and 15, two algorithms could guarantee the satisfying tracking of expected heading angle, but Algorithm 2 gives the expected heading angle in a relatively smoother way and with shorter oscillation, so that the turning torque varies relatively more smoothly. Therefore, Algorithm 2 could control USV to better turn to and maintain the expected heading.

Formation Cruising Simulation
To verify the validity and advancement of the USV formation control algorithm proposed in this part (hereinafter referred to as Algorithm 3), a formation of 5 under-actuated USVs is employed for control. Each USV has the same model parameters and single USV path tracking simulation parameters. The software MATLAB is used for comparative simulation experiment. The control group is taken from Reference [37]. In the reference, time-variant line-of-sight guidance strategy was employed to design the path tracking controller, but it did not include model parameter perturbation and external disturbance (hereinafter referred to as Algorithm 4).
The parameters of simulation environment are selected as follows: the parameters of heading and velocity subsystems are k 1 = 5, k 2 = 3, k 3 = 5; the parameters of guidance system are γ = 0.0005, δ = 3, ∆ min = 5 m, ∆ χ = 7 m. The time-variant disturbance is set to U c = 0.5 m/s; the direction is set to ψ c t) = 1 + 0.2 sin(0.12)+ cos(0.3t + 1 4 π) ; and Γ(·) is selected as Γ(x) = 2/π tan −1 (x) ∈ [−1, 1]. The expected path of USV 1 is y = −x, and its expected velocity is u d = 10 kn. The formation of USV 1 , USV 2 , USV 3 , USV 4 , and USV 5 is shown in Figure 16: After comprehensively analyzing Figures 6, 7, 12, and 13, two algorithms could guarantee the velocity stability of USV approximately 3m/s d u = in the tracking of both straight and curved paths, but Algorithm 1 has larger oscillation amplitude of the forward thrust in the early section of the track than Algorithm 2. It means that Algorithm 2 has lower initial thrust than Algorithm 1. For this reason, Algorithm 2 could control USV to achieve the expected velocity at the lower expense of fuel. After comprehensively analyzing Figures 8, 9, 14, and 15, two algorithms could guarantee the satisfying tracking of expected heading angle, but Algorithm 2 gives the expected heading angle in a relatively smoother way and with shorter oscillation, so that the turning torque varies relatively more smoothly. Therefore, Algorithm 2 could control USV to better turn to and maintain the expected heading.

Formation Cruising Simulation
To verify the validity and advancement of the USV formation control algorithm proposed in this part (hereinafter referred to as Algorithm 3), a formation of 5 under-actuated USVs is employed for control. Each USV has the same model parameters and single USV path tracking simulation parameters. The software MATLAB is used for comparative simulation experiment. The control group is taken from Reference [37]. In the reference, time-variant line-of-sight guidance strategy was employed to design the path tracking controller, but it did not include model parameter perturbation and external disturbance (hereinafter referred to as Algorithm 4).
The parameters of simulation environment are selected as follows: the parameters of heading and velocity subsystems are 1 Table 2: A triangular formation is made by 5 USVs. Among them, USV 3 is at the vertex of the triangle; USV 2 and USV 4 are on the right and left back of USV 3 respectively with the angle of 135 • and the distance of 80 m; USV 1 and USV 5 are on the right and left back of USV 3 , respectively with the angle of 135 • and the distance of 160 m. The initial state of each USV is shown in Table 2: Table 2. Initial parameters of USVs.
The simulation results are given in Figures 17 and 18.
Sensors 2020, 20, x FOR PEER REVIEW 24 of 34  The simulation results are given in Figures 17 and 18.  As shown in Figure 17, under the effect of single USV path tracking controller, each USV enters its expected path from their respective initial state. Under the effect of formation coordination controller, each USV constantly adjusts the path to maintain their formation and overcome the influence of environmental factors. Therefore, their paths are continuously oscillating, but eventually converge to the expected path and maintain the formation.   The simulation results are given in Figures 17 and 18.  As shown in Figure 17, under the effect of single USV path tracking controller, each USV enters its expected path from their respective initial state. Under the effect of formation coordination controller, each USV constantly adjusts the path to maintain their formation and overcome the influence of environmental factors. Therefore, their paths are continuously oscillating, but eventually converge to the expected path and maintain the formation. As shown in Figure 17, under the effect of single USV path tracking controller, each USV enters its expected path from their respective initial state. Under the effect of formation coordination controller, each USV constantly adjusts the path to maintain their formation and overcome the influence of environmental factors. Therefore, their paths are continuously oscillating, but eventually converge to the expected path and maintain the formation.  As revealed in Figure 20, each USV has a different initial heading angle, so that the oscillation amplitude of heading angle error is large at the beginning of the formation path. The heading angle error of each USV gradually converges and decreases in the middle and late section of the path. As shown in Figure 21, the oscillation amplitude of velocity error is large since USVs have different initial positions and velocities while they are making the formation. While maintaining the formation, the velocity error of USVs is low, and eventually converges to approximately 0. As revealed in Figure 20, each USV has a different initial heading angle, so that the oscillation amplitude of heading angle error is large at the beginning of the formation path. The heading angle error of each USV gradually converges and decreases in the middle and late section of the path.  As revealed in Figure 20, each USV has a different initial heading angle, so that the oscillation amplitude of heading angle error is large at the beginning of the formation path. The heading angle error of each USV gradually converges and decreases in the middle and late section of the path. As shown in Figure 21, the oscillation amplitude of velocity error is large since USVs have different initial positions and velocities while they are making the formation. While maintaining the formation, the velocity error of USVs is low, and eventually converges to approximately 0.

USV Formation Cruising Field Experiment
To verify the engineering effectiveness of the proposed USV formation control algorithm, three 'Jellyfish' USVs (As shown in Figures 27,28 and 29) developed by the research group were used in a USV formation cruising control test in a sea area adjacent to Guzhenkou Bay in Qingtao.

USV Formation Cruising Field Experiment
To verify the engineering effectiveness of the proposed USV formation control algorithm, three 'Jellyfish' USVs (As shown in Figures 27,28 and 29) developed by the research group were used in a USV formation cruising control test in a sea area adjacent to Guzhenkou Bay in Qingtao.

USV Formation Cruising Field Experiment
To verify the engineering effectiveness of the proposed USV formation control algorithm, three 'Jellyfish' USVs (As shown in Figures 27, 28 and 29) developed by the research group were used in a USV formation cruising control test in a sea area adjacent to Guzhenkou Bay in Qingtao.

USV Formation Cruising Field Experiment
To verify the engineering effectiveness of the proposed USV formation control algorithm, three 'Jellyfish' USVs (As shown in Figures 27-29) developed by the research group were used in a USV formation cruising control test in a sea area adjacent to Guzhenkou Bay in Qingtao.   ϕ°, and 1,3 =135 ϕ°. In this case, the expected formation is as shown in Figure   30:   ϕ°, and 1,3 =135 ϕ°. In this case, the expected formation is as shown in Figure   30:   ϕ°, and 1,3 =135 ϕ°. In this case, the expected formation is as shown in Figure   30: The expected relative distance and direction among USV 1 , USV 2 , and USV 3 are l 1,2 = 300 m, l 1,3 = 300 m, ϕ 1,2 = −135 • , and ϕ 1,3 = 135 • . In this case, the expected formation is as shown in  .The initial parameters of each USV are shown in Table 3.  The test environment is sunny, temperature 5-12°, freeze southeast (approx. 3.7 m/s), calm sea surface, Level 0-1 sea condition, and wave height approx. 0.6 m.   Table 3.  .The initial parameters of each USV are shown in Table 3.  The test environment is sunny, temperature 5-12°, freeze southeast (approx. 3.7 m/s), calm sea surface, Level 0-1 sea condition, and wave height approx. 0.6 m.  The test environment is sunny, temperature 5-12 • , freeze southeast (approx. 3.7 m/s), calm sea surface, Level 0-1 sea condition, and wave height approx. 0.6 m.
During the field test, the parameters of heading and velocity subsystems are k 1 = 6, k 2 = 4, k 3 = 3. The parameters of guidance system are γ = 0.0054, δ = 2, ∆ min = 15 m, The results of USV formation cruising control are shown in Figures 32-37. Among them, Figure 32 presents the cruising track of USV formation, revealing that the proposed formation cruising control algorithm in this paper enables formation members gradually converge to the expected formation in the setting, and realizes the straight path tracking of the expected formation.
tracking errors in the process of formation cruising. As revealed, the formation control algorithm makes both the transverse and longitudinal errors of each USV converge fast to around 0 with lower oscillation. Meanwhile, each USV has the transverse error of around 3 m and the longitudinal error of around 4 m while maintaining the formation.
As shown in Figure 35, each USV has large oscillation amplitude of heading angle error while making the formation, and could converge fast to the expected heading angle, but smaller heading angle error while maintaining the formation (approximately 2°). Figure 36 shows the velocity error curve of each USV in the process of formation cruising. It is revealed that each USV has large oscillation amplitude of velocity error while making the formation, and could also converge fast to around the expected velocity, but small velocity error while maintaining the formation (approximately 0.5 kn). Figure 37 presents the velocity variation curve of each USV in the process of formation cruising. As we can see in the figure, velocity varies dramatically at the initial stage of making the formation, but each USV could stabilize the velocity around 15 kn to maintain the formation.  tracking errors in the process of formation cruising. As revealed, the formation control algorithm makes both the transverse and longitudinal errors of each USV converge fast to around 0 with lower oscillation. Meanwhile, each USV has the transverse error of around 3 m and the longitudinal error of around 4 m while maintaining the formation.
As shown in Figure 35, each USV has large oscillation amplitude of heading angle error while making the formation, and could converge fast to the expected heading angle, but smaller heading angle error while maintaining the formation (approximately 2°). Figure 36 shows the velocity error curve of each USV in the process of formation cruising. It is revealed that each USV has large oscillation amplitude of velocity error while making the formation, and could also converge fast to around the expected velocity, but small velocity error while maintaining the formation (approximately 0.5 kn). Figure 37 presents the velocity variation curve of each USV in the process of formation cruising. As we can see in the figure, velocity varies dramatically at the initial stage of making the formation, but each USV could stabilize the velocity around 15 kn to maintain the formation.  As shown in Figure 35, each USV has large oscillation amplitude of heading angle error while making the formation, and could converge fast to the expected heading angle, but smaller heading angle error while maintaining the formation (approximately 2 • ).

Conclusions
This paper proposes an improved ILOS guidance strategy while considering the influence of USV under-actuation, uncertainty, and external disturbance. A single USV path tracking controller is designed, while the idea of feedback control is followed to complete the stabilization of position error, heading angle, and expected velocity, so as to realize the single USV path tracking control. Based on the graph theory, a decentralized speed coordination controller is designed to realize USV formation tracking and maintain the formation. At last, cascade system theory and Lyapunov stability are utilized to respectively prove the uniform semi-global exponential stability of path tracking control system and the global asymptotic stability and uniform local exponential stability of coordinated formation system. Simulation and field experiment is conducted and analyzed to prove the advancement and effectiveness of path tracking algorithm and coordinated formation control algorithm based on the improved ILOS guidance strategy.

Conflicts of Interest:
The authors declare no conflict of interest.  Figure 36 shows the velocity error curve of each USV in the process of formation cruising. It is revealed that each USV has large oscillation amplitude of velocity error while making the formation, and could also converge fast to around the expected velocity, but small velocity error while maintaining the formation (approximately 0.5 kn). Figure 37 presents the velocity variation curve of each USV in the process of formation cruising. As we can see in the figure, velocity varies dramatically at the initial stage of making the formation, but each USV could stabilize the velocity around 15 kn to maintain the formation.

Conclusions
This paper proposes an improved ILOS guidance strategy while considering the influence of USV under-actuation, uncertainty, and external disturbance. A single USV path tracking controller is designed, while the idea of feedback control is followed to complete the stabilization of position error, heading angle, and expected velocity, so as to realize the single USV path tracking control. Based on the graph theory, a decentralized speed coordination controller is designed to realize USV formation tracking and maintain the formation. At last, cascade system theory and Lyapunov stability are utilized to respectively prove the uniform semi-global exponential stability of path tracking control system and the global asymptotic stability and uniform local exponential stability of coordinated formation system. Simulation and field experiment is conducted and analyzed to prove the advancement and effectiveness of path tracking algorithm and coordinated formation control algorithm based on the improved ILOS guidance strategy.