Multi-under-Actuated Unmanned Surface Vessel Coordinated Path Tracking

Abstract

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.

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

1.1. USV Control

With regard to USV path tracking control, Børhaug et al. (2011) put forward the integral line-of-sight (ILOS) guidance control law as a compensation for the environmental disturbance, and demonstrated the stability of system [4]. John et al. (2013) introduced an input-output feedback linearization control approach based on neural network for anti-lock braking system (ABS) control, which overcame the failure of traditional PID (Proportion Integral Differential), slide mode, and feedback linearization control to cope with the strong nonlinearity of friction between tyre and ground and the uncertainty of vehicle kinetic parameters. Additionally, the differences were verified in a laboratory test. However, the algorithm was coupled with the model, and could not expand into other control fields [5]. In 2014, Caharija et al. designed the global K-exponential straight path tracking controller and ocean current observer based on coordinate transformation, but did not take into account the common problem of curved path tracking control [6]. Lekkas et al. (2014) studied path tracking control while considering the disturbance caused by external factors such as ocean current to the USV kinematic model, but ignored the asymmetry of USV model [7]. Moreover, Fredriksen et al. (2014) designed the path tracking ILOS guidance algorithm based on relative velocity model, and proved the global K-exponential stability of control system, but only straight path tracking was realized [8]. Blažic (2014) put forth the periodic controller based on the Lyapunov theory for two differential drive wheeled robots, and verified its global convergence. However, the study focused on wheeled robots, so that the model was much different from that for USV. Additionally, the study did not take into account the influence of external disturbance [9]. In 2015, Tian et al. designed the straight tracking controller based on the line-of-sight guidance strategy and anti-saturation PID control algorithm, but overlooked the influence of external disturbance [10]. Takács et al. (2015) analyzed and generalized the main components, practical difficulties and challenges of remote surgical system, and proposed some corresponding solutions. Based on their discussion, a remote surgical system control approach was proposed for integrated modelling, but it was designed for surgical robot, so that the control model was limited to certain applications [11]. In 2018, Pozna et al. introduced an economical and effective approach for designing the nonlinear state and space control system, which was general and applicable to LTI (Linear Time-Invariant), LTV (Linear Time-Varying), LPV (Linear Parameter-Varying), and q-LPV (quasi-Linear Parameter-Varying) systems. Through the cart-pendulum system in a laboratory, they verified the effectiveness of the approach. However, the approach depends much on the model, so that it is not applicable to the control applications using the model with uncertainties. In the meanwhile, the approach is quite theoretical, and not meaningful to engineering applications [12].

At present, most USV path tracking algorithms including those used in the above references are designed with simplified system and numerical analysis. In their design, it is assumed that USV is longitudinally and laterally symmetrical. All the models are strictly diagonal, and employ inertial coefficient matrix and damping coefficient matrix. However, USV often has a structure of lateral symmetry but longitudinal asymmetry. Therefore, modeling results in very large errors. Algorithms are designed with known disturbance, so that the disturbance caused by ocean current to the USV kinematic model is overlooked in most references, but regarded as the disturbance to the USV kinetic model together with wind, wave and un-modelled dynamics. As a matter of fact, ocean current exerts only kinematic effects on the USV maneuvering motion, and causes USV drifting to change its velocity and position and make it deviate from the planned route and heading. In the meanwhile, some references rely on much prior knowledge of single structure, so that the models are less adaptive to environment. Error direct feedback disturbance observer is used in the design to observe disturbance in a real-time manner, which sacrifices the controllability of system.

With regard to USV formation control, Shojaei et al. (2015) studied the under-actuated USV formation control under external disturbance and model parameter perturbation. After firstly designing a formation coordination controller based on the leader-follower mode, they utilized state feedback in the design of single USV tracking controller and introduced generalized saturation function to effectively prevent the possible oversaturation caused by actuating mechanism. After that, neural 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.

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

2. Under-Actuated USV Model and Problem Description

2.1. Under-Actuated USV Model

Considering the irrotational flow of seawater in the geodetic coordinate system (hereinafter referred to as $\left\{i\right\}$ system), and the constant velocity $V_c={[V_x,V_y,0]}^{\mathrm{T}}$, there is a resultant velocity $U_c=\sqrt{V_x^2+V_y^2}$. When the constant is $V_{\max }>0$, we let $V_{\max }\ge U_c$ and ${\dot{U}}_c=0$, where $V_c=\Vert {\mathit{V}}_c\Vert$. Hence, the kinematic and kinetic model of under-actuated USV with three degrees of freedom is expressed in the following way:

$$\{\begin{array}{l}\dot{\mathit{\eta }}=J(\psi )v_r+V_c\\ M{\dot{\mathit{\upsilon }}}_r+\mathit{C}({\upsilon }_r){\upsilon }_r+D({\upsilon }_r)=B\tau \end{array}$$

(1)

where $\mathit{\eta }={[x,y,\psi ]}^{\mathrm{T}}$ is the horizontal position and heading angle of USV under the $\left\{i\right\}$ system; $v_r={[u_r,v_r,r]}^{\mathrm{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 $\left\{b\right\}$ system). The schematic diagram is shown in Figure 1. In the figure, $U=\sqrt{u_r^2+v_r^2}$ is the resultant velocity of USV relative motion under the geodetic coordinate system; heading angle $\psi$ is the angle formed by the bow and the $OX$ axis of inertial coordinate system; track angle $\chi =\psi +\beta$ is the angle formed by the velocity $U$ and the $OX$ axis of inertial coordinate system; drift angle $\beta$ 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(\psi )$ is the rotation matrix from the $\left\{b\right\}$ system to the $\left\{i\right\}$ system; $\mathit{M}$ is the inertial parameter matrix of under-actuated USV; $C(v)=C_{RB}(v)+C_A(v)$ with $C_{RB}({\upsilon }_r)$ and $C_A({\upsilon }_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(\upsilon )$ is damping parameter matrix; $\tau ={[T_u,T_r]}^{\mathrm{T}}$ is control input matrix with $T_u$ as forward thrust and $T_r$ as yawing torque; and vector matrix $B$ is control input configuration matrix. They are defined as follows [34]:

$$J(\psi )=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$

(2)

$$M=\left[\begin{array}{ccc}{m}_{11}& 0& 0\\ 0& m_{22}& m_{23}\\ 0& m_{32}& m_{33}\end{array}\right]$$

(3)

$$B=\left[\begin{array}{cc}{b}_{11}& 0\\ 0& b_{22}\\ 0& b_{32}\end{array}\right]$$

(4)

$$C(v)=\left[\begin{array}{ccc}0& 0& -m_{22}v-m_{23}r\\ 0& 0& m_{11}u\\ m_{22}v+m_{23}r& -m_{11}u& 0\end{array}\right]$$

(5)

$$D(\upsilon )=\left[\begin{array}{ccc}{d}_{11}& 0& 0\\ 0& d_{22}& d_{23}\\ 0& d_{32}& d_{33}\end{array}\right]$$

(6)

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.

2.2. 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 $\left\{b\right\}$ system is transformed into the position of pivoting point.

$$\begin{array}{l}\overline{x}=x+\epsilon \cos \psi \\ \overline{y}=y+\epsilon \sin \psi \\ \overline{\psi }=\psi \\ {\overline{u}}_r=u_r\\ {\overline{v}}_r=v_r+\epsilon r\\ \overline{r}=r\end{array}$$

(7)

where $\epsilon =-\frac{(m_{33}{b}_{22}-m_{23}{b}_{32})}{(m_{22}{b}_{32}-m_{23}{b}_{22})}$, and the model is transformed into:

$$\dot{\overline{x}}={\overline{u}}_r\cos \overline{\psi }-{\overline{v}}_r\sin \overline{\psi }+V_x$$

(8)

$$\dot{\overline{y}}={\overline{u}}_r\sin \overline{\psi }+{\overline{v}}_r\cos \overline{\psi }+V_y$$

(9)

$$\dot{\overline{\psi }}=\overline{r}$$

(10)

$${\dot{\overline{u}}}_r=F_{u_r}({\overline{u}}_r,{\overline{v}}_r,\overline{r})+\frac{b_{11}}{m_{11}}{T}_u$$

(11)

$${\dot{\overline{v}}}_r=X({\overline{u}}_r)\overline{r}+Y({\overline{u}}_r)\overline{v}$$

(12)

$$\begin{array}{l}\dot{\overline{r}}=F_r({\overline{u}}_r,{\overline{v}}_r,\overline{r})+\frac{(b_{32}{m}_{22}-b_{32}{m}_{23})}{(m_{22}{m}_{33}-m_{23}{}^2)}{T}_r\\ \end{array}$$

(13)

where

$$F_{u_r}({\overline{u}}_r,{\overline{v}}_r,\overline{r})=\frac{m_{22}({\overline{v}}_r-\epsilon \overline{r})\overline{r}+m_{23}{\overline{r}}^2}{m_{11}}-\frac{d_{11}{\overline{u}}_r}{m_{11}}$$

(14)

$$\begin{array}{l}X({\overline{u}}_r)=\frac{(m_{23}^2-m_{11}{m}_{33}){\overline{u}}_r+(m_{23}{d}_{33}-m_{33}{d}_{23})}{m_{22}{m}_{33}-m_{23}^2}+\\ \frac{(m_{11}{m}_{23}-m_{22}{m}_{23}){\overline{u}}_r+(m_{23}{d}_{23}-m_{22}{d}_{33})}{m_{22}{m}_{33}-m_{23}^2}\epsilon \end{array}$$

(15)

$$Y({\overline{u}}_r)=\frac{(m_{22}{m}_{23}-m_{11}{m}_{23}){\overline{u}}_r+(m_{23}{d}_{32}-m_{33}{d}_{22})}{m_{22}{m}_{33}-m_{23}^2}$$

(16)

$$\begin{array}{l}{F}_r({\overline{u}}_r,{\overline{v}}_r,\overline{r})=\\ \frac{(m_{11}{m}_{23}-m_{22}{m}_{23}){\overline{u}}_r\overline{r}+(m_{23}{d}_{23}-m_{22}{d}_{33})\overline{r}}{m_{22}{m}_{33}-m_{23}^2}+\\ \frac{(m_{11}{m}_{22}-m_{22}^2){\overline{u}}_r+(m_{23}{d}_{22}-m_{22}{d}_{32})}{m_{22}{m}_{33}-m_{23}^2}\ast ({\overline{v}}_r-\epsilon \overline{r})\end{array}$$

(17)

After coordinate transformation, Equation (12) shows that ${\dot{\overline{v}}}_r$ has no direct actuation. Hence, the USV studied in this paper is under-actuated.

2.3. 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(\mathsf{\Omega })$, where $\mathsf{\Omega }$ is parameter variable and $\mathsf{\Omega }\ge 0$. It satisfies the parameter update rate $\dot{\mathsf{\Omega }}=\frac{U}{{({x^{\prime }}_p^2(\mathsf{\Omega })+{y^{\prime }}_p^2(\mathsf{\Omega }))}^{1/2}}>0$, where ${\overline{y^{\prime }}}_p(\mathsf{\Omega })=\frac{d{\overline{y}}_p(\mathsf{\Omega })}{d\mathsf{\Omega }}$, ${\overline{x^{\prime }}}_p(\mathsf{\Omega })=\frac{d{\overline{x}}_p(\mathsf{\Omega })}{d\mathsf{\Omega }}$, and $U$ is the absolute resultant velocity of USV motion. ${\gamma }_p(\mathsf{\Omega })$ is the included angle between the tangent direction at any point on the parameter path $(x_p(\mathsf{\Omega }),y_p(\mathsf{\Omega }))$ and the longitudinal axis $X_p$ of the $\left\{i\right\}$ system. The angle is positive when it is clockwise. The expression is as follows:

$${\gamma }_p(\mathsf{\Omega })=\arctan (\frac{y^{\prime }(\mathsf{\Omega })}{x^{\prime }(\mathsf{\Omega })}),{\gamma }_p(\mathsf{\Omega })\in (-\pi ,\pi )$$

(18)

The current position of USV in the $\left\{i\right\}$ system is $P(t)={(\overline{x},\overline{y})}^{\mathrm{T}}$, and the expected velocity is ${\overline{u}}_{rd}{}_{}$. In combination with Figure 2, the longitudinal tracking error is as follows:

$$\begin{array}{c}{y}_e(t)=-(\overline{x}-{\overline{x}}_p(\mathsf{\Omega }))\sin {\gamma }_p(\mathsf{\Omega })+\\ (\overline{y}-{\overline{y}}_p(\mathsf{\Omega }))\cos {\gamma }_p(\mathsf{\Omega })\end{array}$$

(19)

Equation (19) is differentiated as follows:

$$\begin{array}{lll}{\dot{y}}_e(t)& =& -\sin {\gamma }_p(\mathsf{\Omega })(\dot{\overline{x}}-{\dot{\overline{x}}}_p(\mathsf{\Omega }))\\ & & -{\dot{\gamma }}_p(\mathsf{\Omega })\cos {\gamma }_p(\mathsf{\Omega })(\overline{x}-{\overline{x}}_p(\mathsf{\Omega }))\\ & & +\cos {\gamma }_p(\mathsf{\Omega })(\dot{\overline{y}}-{\dot{\overline{y}}}_p(\mathsf{\Omega }))\\ & & -{\dot{\gamma }}_p(\mathsf{\Omega })\sin {\gamma }_p(\mathsf{\Omega })(\overline{y}-{\overline{y}}_p(\mathsf{\Omega }))\end{array}$$

(20)

From Equation (18), we obtain:

$$\dot{\overline{x}}\sin ({\gamma }_p(\mathsf{\Omega }))-\dot{\overline{y}}\cos ({\gamma }_p(\mathsf{\Omega }))=0$$

(21)

Based on the geometry in Figure 1, it is learned that:

$$\begin{array}{l}(\overline{x}-{\overline{x}}_p(\mathsf{\Omega }))\cos {\gamma }_p(\mathsf{\Omega })\\ -(\overline{y}-{\overline{y}}_p(\mathsf{\Omega }))\sin {\gamma }_p(\mathsf{\Omega })=0\end{array}$$

(22)

Equations (21) and (22) are substituted into Equation (20) to obtain:

$$\begin{array}{ll}{\dot{y}}_e(t)& =({\overline{u}}_r\sin \overline{\psi }+{\overline{v}}_r\cos \overline{\psi }+V_y)\cos ({\gamma }_p(\mathsf{\Omega }))\\ & -({\overline{u}}_r\cos \overline{\psi }-{\overline{v}}_r\sin \overline{\psi }+V_x)\sin ({\gamma }_p(\mathsf{\Omega }))\\ & =U\sin (\overline{\psi }+\beta -{\gamma }_p(\mathsf{\Omega }))\end{array}$$

(23)

where $U$ is the resultant velocity in the $\left\{i\right\}$ system, and $\beta =\arctan ({\overline{v}}_r,{\overline{u}}_r)$ is the lateral sliding angle generated by the transverse velocity of USV and the disturbance in the external environment.

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

3.1. 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:

$$\underset{t\to \infty }{\lim }{y}_e(t)=0$$

(24)

$$\underset{t\to \infty }{\lim }\overline{\psi }(t)={\overline{\psi }}_d$$

(25)

where ${\overline{\psi }}_d$ is the heading reference signal given by guidance subsystem, i.e., expected heading angle.

3.2. 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:

$$\lim {\overline{x}}_j\left(t\right)-{\overline{x}}_i\left(t\right)-d_{ji}=0,j,i=1,\dots ,n$$

(26)

The USV forward velocity ${\overline{u}}_r$ converges to the expected velocity ${\overline{u}}_d$. In other words, longitudinal velocity error ${\overline{u}}_e$ converges to 0. Therefore, there is:

$$\underset{t\to \infty }{\lim }{\overline{u}}_{r_j}\left(t\right)-{\overline{u}}_d\left(t\right)=0,j=1,\dots ,n$$

(27)

Above all, the problem of multi-USV formation coordination path tracking could be divided into:

• 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 ${\overline{u}}_{c_j}$;
• Designing a controller for ${\overline{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 ${\overline{u}}_d$.

4. Controller Design

4.1. 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:

$${\overline{\psi }}_d={\gamma }_p(\mathsf{\Omega })-\beta -\arctan (\frac{1}{\Delta }(y_e(t)+y_{\mathrm{int}}))$$

(28)

$${\dot{y}}_{\mathrm{int}}=\gamma \frac{\Delta U{y}_e(t)}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta y_{\mathrm{int}})}^2}},\gamma >0$$

(29)

The adaptive headway range $\Delta$ is designed to improve the ILOS and define it as the function of longitudinal tracking error:

$$\Delta ={\Delta }_{\min }+{\Delta }_{\chi }{e}^{-\sigma \left(y_e(t)\right)}{}^{{}^{1/2}}$$

(30)

where $\sigma$ is the constant parameter that is strictly greater than 0 [7]; ${\Delta }_{\min }$ is the minimum headway range, and ${\Delta }_{\chi }={\Delta }_{\max }-{\Delta }_{\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 $\underset{y_e(t)\to \infty }{\lim }{e}^{-\sigma \left(y_e(t)\right)}{}^{{}^{1/2}}\approx 0$. At this time, headway range is $\Delta ={\Delta }_{\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 $\underset{y_e(t)\to 0}{\lim }{e}^{-\sigma \left(y_e(t)\right)}{}^{{}^{1/2}}\approx 1$. At this time, headway range is $\Delta ={\Delta }_{\max }$, so that USV could keep the stable tracking on the given track.

4.2. 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 ${\overline{\psi }}_d$ and expected angular velocity ${\dot{\overline{\psi }}}_d$ given in the guidance subsystem [35], Equation (11) is used to obtain the heading control law as follows:

$$T_r=\frac{m_{22}{m}_{33}-m_{23}^2}{b_{32}{m}_{22}-b_{32}{m}_{23}}\left(\begin{array}{l}-F_r({\overline{u}}_r,{\overline{v}}_r,\overline{r})+{\ddot{\overline{\psi }}}_d-\\ k_1(\dot{\overline{\psi }}-{\dot{\overline{\psi }}}_d)-k_2(\overline{\psi }-{\overline{\psi }}_d)\end{array}\right)$$

(31)

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:

$$T_u=\frac{m_{11}}{b_{11}}\left(\begin{array}{c}-F_{u_r}({\overline{u}}_r,{\overline{v}}_r,\overline{r})+\frac{b_{11}}{m_{11}}{\overline{u}}_{rd}+\\ {\dot{\overline{u}}}_{rd}-k_3({\overline{u}}_r-{\overline{u}}_{rd})\end{array}\right)$$

(32)

here $k_3$ is a constant that is greater than 0, which ensures that velocity exponent converges to the expected value.

4.3. 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:

$$\begin{array}{lll}\dot{\overline{x}}& =& {\overline{u}}_r-{\overline{u}}_r\left(1-\cos \overline{\psi }\right)-{\overline{v}}_r\sin \overline{\psi }\\ & =& {\overline{u}}_c+{\overline{u}}_e-{\overline{u}}_r\frac{\left(1-\cos \overline{\psi }\right)}{\overline{\psi }}\overline{\psi }-{\overline{v}}_r\frac{\sin \overline{\psi }}{\overline{\psi }}\overline{\psi }\end{array}$$

(33)

where ${\overline{u}}_c$ is velocity reference command, ${\overline{u}}_e={\overline{u}}_r-{\overline{u}}_c$.

Because of $\overline{\psi }={\overline{\psi }}_d+{\overline{\psi }}_e=\frac{-{\tan }^{-1}\left(y_e/\Delta \right)}{y_e}{y}_e+{\overline{\psi }}_e$, it is substituted into Equation (33) to obtain:

$$\dot{\overline{x}}={\overline{u}}_c+f\left(y_e,\overline{\psi },\overline{u},\overline{v}\right)\varpi$$

(34)

where $f(·)={\left[\begin{array}{c}\frac{{\overline{u}}_r\left(1-\cos \overline{\psi }\right)-{\overline{v}}_r\sin \overline{v}}{\overline{\psi }}\cdot \frac{{\tan }^{-1}\left(y_e/\Delta \right)}{y_e}\\ \frac{{\overline{v}}_r\sin \overline{\psi }-{\overline{u}}_r\left(1-\cos \overline{\psi }\right)}{\overline{\psi }}\end{array}\right]}^{\mathrm{T}}$ and $\varpi ={\left[{\overline{u}}_e,y_e,{\overline{\psi }}_e\right]}^{\mathrm{T}}$. According to the trigonometric function, $\sin x/x,\left(1-\cos x\right)/x,{\tan }^{-1}\left(x\right)/x$ is globally bounded. Moreover, the exponent of the element in the vector $\varpi$ converges to 0 under the path tracking controller designed in the above section. Hence, the velocity $\overline{u}\left(t\right)$ on the direction of the $x$ axis asymptotically converges to the reference command velocity ${\overline{u}}_c\left(t\right)$.

By then, the controller ${\overline{u}}_{c_j},j=1,\dots ,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 ${\overline{u}}_d\left(t\right)$ satisfies:

$${\overline{u}}_d\left(t\right)\in \left[u_{\min }+h,u_{\max }-h\right],\forall t\ge t_0$$

(35)

where $u_{\max }>u_{\min }>0$.

The controller is designed as follows:

$${\overline{u}}_{c_j}={\overline{u}}_d\left(t\right)-\mathsf{\Gamma }\left({\displaystyle \sum _{i=1}^n\left({\overline{x}}_j-{\overline{x}}_i-d_{ji}\right)}\right),j=1,\dots ,n$$

(36)

where $d_{ji}={\overline{x}}_{d_j}-{\overline{x}}_{d_i}$ represents the distance between the $j$th USV and the $i$th USV in parallel with the $x$ axis. $\mathsf{\Gamma }(·)$ is the bounded continuous differentiable increasing function and satisfies ${\mathsf{\Gamma }}^{\prime }\left(0\right)>0$, $\mathsf{\Gamma }\left(0\right)=0$ and $\mathsf{\Gamma }\left(x\right)\in \left(-h,h\right)$.

5. Stability Verification

5.1. Stability of Guidance System

During the process of USV motion, $\beta$ varies with time under the change of heading angle $\overline{\psi }$ and transverse velocity ${\overline{v}}_r$. Compared with the motion of USV, such change is very slow. The following assumptions could be given:

Assumption 1.$\beta$ is a constant and varies very slightly, i.e., $\dot{\beta }=0$.

Therefore, Equation (23) could be written as:

$${\dot{y}}_e(t)=U\sin (\overline{\psi }-{\gamma }_p(\mathsf{\Omega }))+U\cos (\overline{\psi }-{\gamma }_p(\mathsf{\Omega }))\beta$$

(37)

After defining $\tilde{\psi }=\overline{\psi }-{\psi }_d$ as the heading angle error of USV, Equation (37) could be rewritten as:

$$\begin{array}{c}{\dot{y}}_e(t)=U\sin (\tilde{\psi }+{\psi }_d-{\gamma }_p(\mathsf{\Omega }))+\\ U\cos (\tilde{\psi }+{\psi }_d-{\gamma }_p(\mathsf{\Omega }))\beta \end{array}$$

(38)

Equation (38) is expanded to obtain:

$$\begin{array}{c}{\dot{y}}_e(t)=U\sin (\tilde{\psi }+{\psi }_d-{\gamma }_p(\theta ))+\\ U\cos (\tilde{\psi }+{\psi }_d-{\gamma }_p(\theta ))\beta \end{array}$$

(39)

Moreover, there is:

$$\sin \left({\tan }^{-1}\left(-\frac{1}{\Delta }{y}_e(t)-y_{\mathrm{int}}\right)\right)=\frac{y_e(t)+\Delta y_{\mathrm{int}}}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta y_{\mathrm{int}})}^2}}$$

(40)

$$\cos \left({\tan }^{-1}\left(-\frac{1}{\Delta }{y}_e(t)+y_{\mathrm{int}}\right)\right)=\frac{\Delta }{\sqrt{{\Delta }^2+{(y_e(t)+\Delta y_{\mathrm{int}})}^2}}$$

(41)

Equations (40) and (41) are substituted into Equation (38) to obtain:

$$\begin{array}{c}{\dot{y}}_e(t)=-\frac{U(y_e(t)+\Delta y_{\mathrm{int}})}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta y_{\mathrm{int}})}^2}}+\\ \frac{U\Delta }{\sqrt{{\Delta }^2+{(y_e(t)+\Delta y_{\mathrm{int}})}^2}}\beta +\\ U[{\varphi }_1(y_e(t),\tilde{\psi })+\beta {\varphi }_2(y_e(t),\tilde{\psi })]\tilde{\psi }\end{array}$$

(42)

where

$${\varphi }_1(y_e(t),\tilde{\psi })=\frac{\sin \tilde{\psi }}{\tilde{\psi }}\cos ({\psi }_d-{\gamma }_p(\mathsf{\Omega }))+\frac{\cos \tilde{\psi }-1}{\tilde{\psi }}\sin ({\psi }_d-{\gamma }_p(\mathsf{\Omega }))$$

(43)

$${\varphi }_2(y_e(t),\tilde{\psi })=-\frac{\sin \tilde{\psi }}{\tilde{\psi }}\sin ({\psi }_d-{\gamma }_p(\theta ))+\frac{\cos \tilde{\psi }-1}{\tilde{\psi }}\cos ({\psi }_d-{\gamma }_p(\theta ))$$

(44)

Obviously, there is $\left|\sin \tilde{\psi }/\tilde{\psi }\right|\le 1$, $\left|(\cos \tilde{\psi }-1)/\tilde{\psi }\right|\le 0.73$, $\frac{\Delta }{\sqrt{{\Delta }^2+({\mathrm{y}}_{\mathrm{e}}+{\mathrm{y}}_{\mathrm{int}}{)}^2}}\le 1$, and $\frac{{\mathrm{y}}_{\mathrm{e}}+{\mathrm{y}}_{\mathrm{int}}}{\sqrt{{\Delta }^2+({\mathrm{y}}_{\mathrm{e}}+{\mathrm{y}}_{\mathrm{int}}{)}^2}}\le 1$. Thus, ${\varphi }_1(y_e(t),\tilde{\psi })$ and ${\varphi }_2(y_e(t),\tilde{\psi })$ are bounded and satisfy:

$$\left|{\varphi }_1(y_e(t),\tilde{\psi })\right|\le 1.73,\left|{\varphi }_2(y_e(t),\tilde{\psi })\right|\le 1.73$$

(45)

Let $\widehat{\beta }$ be the estimation of drift angle $\beta$ from adaptive observer, and the estimation error is $\tilde{\beta }=\beta -\widehat{\beta }$. When the accurate tracking of heading and velocity is realized, the virtual control input could be $y_{\mathrm{int}}=\widehat{\beta }$ and meets Assumption 3. Thus, Equation (42) may be rewritten as:

$${\dot{y}}_e(t)=-\frac{U(y_e(t)+\Delta \widehat{\beta })}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}+\frac{U\Delta }{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}\beta$$

(46)

In other words, there is:

$${\dot{y}}_e(t)=-\frac{U{y}_e(t)}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}+\frac{U\Delta }{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}\tilde{\beta }$$

(47)

The Lyapunov function $V_1$ is designed and defined as follows:

$$V_1(y_e(t),t,\tilde{\beta })=\frac{1}{2}{y}_e{(t)}^2+\frac{1}{2k}{\tilde{\beta }}^2>0$$

(48)

where $y_e(t)\ne 0$, $\tilde{\beta }\ne 0$ and $k>0$.

It is differentiated to obtain:

$${\dot{V}}_1(y_e(t),t,\tilde{\beta })=y_e(t){\dot{y}}_e(t)+\frac{1}{k}\tilde{\beta }\dot{\tilde{\beta }}=-\frac{U{y}_e{(t)}^2}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}+\left(\frac{U\Delta y_e(t)}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}+\frac{1}{k}\dot{\tilde{\beta }}\right)$$

(49)

As a result of $\dot{\tilde{\beta }}=-\dot{\widehat{\beta }}$, Equation (29) is substituted into Equation (49) to obtain:

$${\dot{V}}_1(y_e(t),t,\tilde{\beta })=-\frac{U{y}_e{(t)}^2}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}\le 0$$

(50)

Therefore, guidance subsystem has uniform global asymptotic stability (UGAS) at the equilibrium point $y_e(t)=0$.

Additionally, there is $\mu >0$. When $y_e(t)$ satisfies $D=\left\{y_e(t)\in R|\left|y_e(t)\right|\le \mu \right\}$, ${\dot{V}}_1(y_e(t),t)$ satisfies:

$${\dot{V}}_1(y_e(t),t,\tilde{\beta })=\frac{-U{y}_e{(t)}^2}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}\le -w{y}_e{(t)}^2$$

(51)

where $0<w<\frac{U}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta \widehat{\beta })}^2}}$. 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).

5.2. Stability of Heading and Velocity Controller

The formula for dynamic tracking error of heading and velocity control subsystem is as follows:

$$\left[\begin{array}{c}\dot{\tilde{\psi }}\\ \ddot{\tilde{\psi }}\\ {\dot{\tilde{u}}}_r\end{array}\right]=\left[\begin{array}{ccc}\begin{array}{c}0\\ -k_2\\ 0\end{array}& \begin{array}{c}1\\ -k_1\\ 0\end{array}& \begin{array}{c}0\\ 0\\ -k_3\end{array}\end{array}\right]\left[\begin{array}{c}\tilde{\psi }\\ \dot{\tilde{\psi }}\\ {\tilde{u}}_r\end{array}\right]$$

(52)

where $\tilde{\psi }=\overline{\psi }-{\overline{\psi }}_d$, $\dot{\tilde{\psi }}=\dot{\overline{\psi }}-{\dot{\overline{\psi }}}_d$, and ${\tilde{u}}_r={\overline{u}}_r-{\overline{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 $({\tilde{u}}_r,\tilde{\psi },\tilde{r})=(0,0,0)$ is global exponential stable [35,36], i.e., the control objectives $\underset{t\to \infty }{\lim }\overline{\psi }(t)={\overline{\psi }}_d$, $\underset{t\to \infty }{\lim }\overline{u}\left(t\right)-{\overline{u}}_d\left(t\right)=0$ are achieved.

5.3. Stability of Path Tracking System

The position error dynamic equation and heading and velocity error dynamic equation are written in the following cascade:

$$\begin{array}{l}{\displaystyle \sum 1}:{\dot{y}}_e(t)=-\frac{U(y_e(t)+\Delta y_{\mathrm{int}})}{\sqrt{{\Delta }^2+{(y_e(t)+\Delta y_{\mathrm{int}})}^2}}+\\ \frac{U\Delta \beta }{\sqrt{{\Delta }^2+{(y_e(t)+\Delta y_{\mathrm{int}})}^2}}+g(t,y_e,\xi )\xi \end{array}$$

(53)

$${\displaystyle \sum 2:}\dot{\xi }=\left[\begin{array}{c}\dot{\tilde{\psi }}\\ \ddot{\tilde{\psi }}\\ {\dot{\tilde{u}}}_r\end{array}\right]=\left[\begin{array}{ccc}\begin{array}{c}0\\ -k_2\\ 0\end{array}& \begin{array}{c}1\\ -k_1\\ 0\end{array}& \begin{array}{c}0\\ 0\\ -k_3\end{array}\end{array}\right]\left[\begin{array}{c}\tilde{\psi }\\ \dot{\tilde{\psi }}\\ {\tilde{u}}_r\end{array}\right]$$

(54)

where $\xi =\left[\begin{array}{c}\tilde{\psi }\\ \dot{\tilde{\psi }}\\ {\tilde{u}}_r\end{array}\right]$, $g(t,y_e,\xi )={\left[\begin{array}{c}{U}_r\varphi (y_e,\tilde{\psi })\\ 0\\ 0\end{array}\right]}^T$.

When the system ${\displaystyle \sum 1}$ has uniform semi-global exponential stability and the system ${\displaystyle \sum 2}$ has uniform local exponential stability, the closed-loop control system ${\displaystyle \sum 1}-{\displaystyle \sum 2}$ has uniform global asymptotic and uniform semi-global exponential stability at the position $(y_e,{\tilde{u}}_r,\tilde{\psi },\dot{\tilde{\psi }})=(0,0,0,0)$.

Verification: As a result of $|\varphi (y_e,\tilde{\psi })|\le 1.73$, the link term $g(t,y_e,\xi )$ of the system ${\displaystyle \sum 1}-{\displaystyle \sum 2}$ satisfies:

$$\Vert g(\mathrm{t},{\mathrm{y}}_{\mathrm{e}},\xi )\Vert \le 1.73U_r$$

(55)

Since the systems ${\displaystyle \sum 1}$ and ${\displaystyle \sum 2}$ have been proved to have uniform semi-global exponential stability and uniform local exponential stability in the above section, the system ${\displaystyle \sum 1}-{\displaystyle \sum 2}$ has uniform semi-global exponential stability at the position $(y_e,{\tilde{u}}_r,\tilde{\psi },\dot{\tilde{\psi }})=(0,0,0,0)$. This fulfils a control objective $\underset{t\to \infty }{\lim }{y}_e(t)=0$, $\underset{t\to \infty }{\lim }\overline{\psi }(t)={\overline{\psi }}_d$, i.e., USV could realize the stable tracking of expected path under the control of path tracking algorithm.

5.4. Stability of Coordinated Formation Controller

Equation (36) is substituted into Equation (34) to obtain:

$${\dot{\overline{x}}}_j={\overline{u}}_d\left(t\right)-\mathsf{\Gamma }\left({\displaystyle \sum _i^n\left({\overline{x}}_j-{\overline{x}}_i-d_{ji}\right)}\right)+z_j{\varpi }_j,j=1,\dots ,n$$

(56)

Let ${\tilde{x}}_j={\overline{x}}_j-{\overline{x}}_{d_j},j=1,\dots ,n$, so that Equation (56) could be written as:

$${\tilde{x}}_j=-\mathsf{\Gamma }\left({\displaystyle \sum _{i=1}^n\left({\tilde{x}}_j-{\tilde{x}}_i\right)}\right)+z_j{\varpi }_j,j=1,\dots ,n$$

(57)

Equation (57) is transformed in the form of vector $\tilde{x}={\left[{\tilde{x}}_1,\dots .{\tilde{x}}_n\right]}^{\mathrm{T}}$, and $\mathsf{\Gamma }\left(\tilde{x}\right)=\left[g\left({\tilde{x}}_1\right),\dots ,g\left({\tilde{x}}_n\right)\right]$. Thus, Equation (57) could be rewritten as:

$$\tilde{x}=-\mathsf{\Gamma }\left(L\tilde{x}\right)+Z\omega$$

(58)

where $\omega ={[{\varpi }_1^{\mathrm{T}},\dots ,{\varpi }_n^{\mathrm{T}}]}^{\mathrm{T}}$, $Z$ is block diagonal matrix. The diagonal element is $z_j,j=1,\dots ,n$. Since $z_j$ is bounded, $Z$ is bounded. The matrix $L$ is the Laplacian matrix of communication topological graph $G$, so that $L=\left\{l_{ji}\right\}\in R^{n\times n}$. In the meanwhile, there is:

$$l_{ji}\{\begin{array}{l}{\delta }_j,j=1\\ -1,j\ne 1,\mathrm{and}\left(j,i\right)\in E,j,i=1,\dots ,n\\ 0,\mathrm{other}\end{array}$$

(59)

where ${\delta }_j$ is the out-degree of the vertex $j$. As shown in Equation (59), $\mathit{L}{\mathit{v}}_1=0$, ${\mathit{v}}_1={\left[1,\dots ,1\right]}^{\mathrm{T}}$ and ${\mathit{v}}_1\in R^n$. According to the graph theory, matrix $L$ has a zero eigen-value, if and only if communication topographic graph $G$ contains a globally reachable point.

Stability analysis is conducted as follows:

Theorem 1.

Considering the kinematic and kinetic model of USV Equation (1), it is assumed that$u_d(t)$is continuously differentiable, and satisfies Equation (35);$u_{cj}(t)$satisfies$u_{cj}(t)\in (u_{\min },u_{\max }),j=1,\dots ,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.

Theorem 2.

Considering that Theorem 1 is justified, and the system in Equation (58) is under the coordination transformation$\mathit{\mu }=\mathit{T}\tilde{\mathit{x}},\mathit{T}\in R^{\left(n-1\right)\times n}$, the following formulas are established:

(1)${\tilde{x}}_1=\dots ={\tilde{x}}_n$, i.e.,$\mathit{\mu }=0$

(2)$\dot{\mathit{\mu }}=\mathit{g}\left(\mathit{\mu }\right)+\mathit{G}\left(\mathit{\omega },{\mathit{u}}_c\right)\mathit{\omega }$, where$\mathit{G}\left(\mathit{\omega },{\mathit{u}}_c\right)$is globally bounded.

(3) There is positive definite and radially unbounded Lyapunov function$V=V\left(\mathit{\mu }\right)$, which satisfies:

$$\frac{\partial V}{\partial \mathit{\mu }}\left(\mathit{\mu }\right)\mathit{g}\left(\mathit{\mu }\right)\le W\left(\mathit{\mu }\right)<0,\forall \mathit{\mu }\in R^{n-1}$$

(60)

$$\Vert \frac{\partial V}{\partial \mathit{\mu }}\left(\mathit{\mu }\right)\Vert \le C_1,\forall \mathit{\mu }\in R^{n-1}$$

(61)

The system$\dot{\mathit{\mu }}=\mathit{g}\left(\mathit{\mu }\right)$has global asymptotic stability and uniform local exponential stability.

Verification: It is assumed that the communication topographic graph $G$ in Equation (58) contains $s$ globally reachable points, and $1\le s\le n$. The matrix $L$ could be written as:

$$\mathit{L}=\left[\begin{array}{cc}{L}_1& L_2\\ 0& L_3\end{array}\right]$$

(62)

where $L_3\in R^{s\times s}$ is Laplacian matrix, and its element is globally reachable. Hence, $G\left(L_3\right)$ is strongly connected. $L_1\in R^{\left(n-s\right)\times \left(n-s\right)}$ is semi-Hurwitz, and satisfies:

$$\mathit{P}{\mathit{L}}_1+{\mathit{L}}_1^{\mathrm{T}}\mathit{P}=\mathit{Q},\mathit{Q}={\mathit{Q}}^{\mathrm{T}}>0$$

(63)

where $\mathit{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=\left[1,\dots ,1\right]\in R^s$. Thus ${\mathit{L}}_3$ could be divided into $L_3=M_3{M}_3^{\mathrm{T}}$ and $M_3\in R^{s\times \left(s-1\right)}$. Therefore, $\mathit{\mu }=\mathit{T}\tilde{\mathit{x}}$ could be rewritten as:

$$\mu =\left[\begin{array}{ll}{L}_1& L_2\\ 0& M_3^{\mathrm{T}}\end{array}\right]\tilde{\mathit{x}}=\mathit{T}\tilde{\mathit{x}}$$

(64)

Verification 1. There is $\mathit{\mu }=0$, so that $\mathit{L}\mathit{\mu }=\left[\begin{array}{ll}I& 0\\ 0& M_3\end{array}\right]=0$, i.e., $\mathit{\mu }=\alpha 1_n,\alpha \in R$. Therefore, ${\tilde{x}}_j={\tilde{x}}_i,j,i=1,\dots ,n$.

Verification 2. Equations (58), (62), and (64), are combined as follows:

$$\begin{array}{ll}\dot{\mathit{\mu }}& =\left[\begin{array}{c}-L_1{\mathsf{\Gamma }}_1({\mu }_1)-L_2{\mathsf{\Gamma }}_2(M_3{\mu }_2)\\ -M_3^{\mathrm{T}}{\mathsf{\Gamma }}_2\left(M_3{\mu }_2\right)\end{array}\right]+\mathit{T}\mathit{Z}\mathit{\omega }\\ & =\mathit{g}\left(\mathit{\mu }\right)+\mathit{G}\mathit{\omega }\end{array}$$

(65)

where $\mathit{\mu }={\left[{\mu }_1^{\mathrm{T}},{\mu }_2^{\mathrm{T}}\right]}^{\mathrm{T}}$, ${\mu }_1\in R^{\left(s-r\right)}$, ${\mu }_2\in R^r$, $\mathit{G}=\mathit{T}\mathit{Z}$, $\mathit{T}$ is constant matrix, and $\mathit{Z}$ is globally bounded, so that $\mathit{G}$ is globally bounded.

Verification 3. Equation (65) is used to obtain:

$$\mathit{\mu }=\mathit{g}\left(\mathit{\mu }\right)=\left[\begin{array}{c}{\dot{\mu }}_1\\ {\dot{\mu }}_2\end{array}\right]=\left[\begin{array}{l}-L\mathsf{\Gamma }\left({\mu }_1\right)-L_2\mathsf{\Gamma }\left(M_3{\mu }_2\right)\\ -M_3^{\mathrm{T}}{\mathsf{\Gamma }}_2\left(M_3{\mu }_2\right)\end{array}\right]$$

(66)

Considering the Lyapunov function:

$$\begin{array}{ll}V& =\frac{\lambda }{2}{\Vert {\mu }_2\Vert }^2+{\displaystyle \sum _{i=1}^n{\displaystyle \underset{0}{\overset{{\mu }_1}{\int }}{p}_i}}\mathsf{\Gamma }\left(y\right)dy\\ & =\frac{\lambda }{2}{\Vert {\mu }_2\Vert }^2+{\displaystyle \sum _{i=1}^n{\displaystyle \underset{0}{\overset{{\mu }_1}{\int }}\mathit{P}{\mathsf{\Gamma }}_1\left(y\right)}}dy\end{array}$$

(67)

where $p_i$ is the diagonal element of the positive definite diagonal matrix $\mathit{P}$. Meanwhile, we select $\lambda >0$ since $\mathsf{\Gamma }\in \left[0,\ell \right],\ell >0$. $V$ is radially unbounded, so that there is:

$$\dot{V}=-\lambda {\left(M_3{\mu }_2\right)}^{\mathrm{T}}{\mathsf{\Gamma }}_2\left(M_3{\mu }_2\right)-\frac{1}{2}{\mathsf{\Gamma }}_1^{\mathrm{T}}\left({\mu }_1\right)\left[P{L}_1+L_1^{\mathrm{T}}P\right]{\mathsf{\Gamma }}_1\left({\mu }_1\right)-{\mathsf{\Gamma }}_1^{\mathrm{T}}\left({\mu }_1\right)P{L}_2{\mathsf{\Gamma }}_2\left(M_3{\mu }_2\right)$$

(68)

Let $\kappa =M_3{\mu }_2$. Equation (68) could be simplified to:

$$\dot{V}\le -\lambda {\kappa }^{\mathrm{T}}{\mathsf{\Gamma }}_2\left(\kappa \right)-\frac{q_m}{2}{\Vert {\mathsf{\Gamma }}_1^{\mathrm{T}}\left({\mu }_1\right)\Vert }^2+c\Vert {\mathsf{\Gamma }}_2\left({\mu }_1\right)\Vert \cdot \Vert {\mathsf{\Gamma }}_2\left(\kappa \right)\Vert$$

(69)

where $q_m$ is the minimum Eigen-value of the positive definite matrix $\mathit{Q}$ in Equation (63), and it is positive, $c\ge \Vert P{L}_2\Vert >0$. Since $\mathsf{\Gamma }\in \left[0,\ell \right],\ell >0$, there is $0<{\mathsf{\Gamma }}^{\prime }\left(x\right)<\ell$ and $\forall x\in R$. Hence, $\frac{x}{\mathsf{\Gamma }\left(x\right)}\ge \frac{1}{\ell }$, $\forall x\in R$. Then Equation (69) could be rewritten as:

$$\dot{V}\le -\frac{\lambda }{\ell }{\Vert {\mathsf{\Gamma }}_2\left(\kappa \right)\Vert }^2-\frac{q_m}{2}{\Vert {\mathsf{\Gamma }}_1^{\mathrm{T}}\left({\mu }_1\right)\Vert }^2+c\Vert {\mathsf{\Gamma }}_1\left({\mu }_1\right)\Vert \cdot \Vert {\mathsf{\Gamma }}_2\left(\kappa \right)\Vert$$

(70)

We select $\lambda \ge \ell \left({\left[\frac{c}{\sqrt{2q_m}}\right]}^2+\alpha \right)$ and $\alpha >0$, so that there is:

$$\begin{array}{ll}\dot{V}& \le -{\left(\frac{c}{\sqrt{2q_m}}\Vert {\mathsf{\Gamma }}_2\left(\kappa \right)\Vert -\sqrt{\frac{q_m}{2}}\Vert {\mathsf{\Gamma }}_1\left({\mathsf{\Gamma }}_1\left({\mu }_1\right)\right)\Vert \right)}^2-\alpha {\Vert {\mathsf{\Gamma }}_2\left(\kappa \right)\Vert }^2\\ & =-\mathit{W}\left(\left({\mathsf{\Gamma }}_1\left({\mu }_1\right),{\mathsf{\Gamma }}_2\left(\kappa \right)\right)\right)\end{array}$$

(71)

where $\mathit{W}=W\left({\mathsf{\Gamma }}_1\left({\mu }_1\right),{\mathsf{\Gamma }}_2\left(\kappa \right)\right)$ is the positive definite function of ${\mathsf{\Gamma }}_1\left({\mu }_1\right)$ and ${\mathsf{\Gamma }}_2\left(\kappa \right)$. $\mathsf{\Gamma }\left(x\right)=0$ if and only if $x=0$. The matrix $M_3$ is a full-rank matrix. If and only if ${\mu }_1={\mu }_2=0$, $W=0$. In other words, time-invariant system Equation (58) has uniform global asymptotic stability at the position $\mu =0$.

After linearization, Equation (66) is changed to:

$$\Delta \dot{\mu }=-{\mathsf{\Gamma }}^{\prime }(0)\left[\begin{array}{ll}{L}_1& L_2{M}_3\\ 0& M_3^{\mathrm{T}}{M}_3\end{array}\right]\Delta \mu$$

(72)

where $-L_1$ is a Hurwitz matrix, $-M_3^{\mathrm{T}}{M}_3$ is a Hurwitz matrix as well. Hence, the system Equation (72) has uniform global asymptotic stability at the position $\mu =0$. Hence, the system Equation (66) has uniform global exponential stability at the position $\mu =0$.

Let $\tilde{V}=\ln \left(V+1\right)$, $\tilde{V}$ is calculated to obtain:

$$\dot{\tilde{V}}\le -\frac{1}{V\left(\mu \right)+1}W\left({\mathsf{\Gamma }}_1\left({\mu }_1\right),{\mathsf{\Gamma }}_2\left(\kappa \right)\right)=-\tilde{W}\left(\mu \right)<0$$

(73)

Meanwhile,

$$\begin{array}{ll}\Vert \frac{\partial \tilde{V}}{\partial \mu }\Vert & \le \frac{1}{V+1}\left(\gamma \Vert {\mu }_2\Vert +\Vert {\mathsf{\Gamma }}_1\left({\mu }_1\right)\Vert \cdot \Vert P\Vert \right)\\ & \le \gamma \frac{\Vert {\mu }_2\Vert }{V+1}+\Vert {\mathsf{\Gamma }}_1\left({\mu }_2\right)\Vert \cdot \Vert P\Vert \\ & \le \gamma \frac{\Vert {\mu }_2\Vert }{\frac{\gamma }{2}{\Vert {\mu }_2\Vert }^2+1}+\Vert {\mathsf{\Gamma }}_1\left({\mu }_1\right)\Vert \cdot \Vert P\Vert \\ & \le C_1\end{array}$$

(74)

where $C_1>0$, and $\Vert {\mathsf{\Gamma }}_1\left({\mu }_1\right)\Vert$ 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 $\lim {\overline{x}}_j\left(t\right)-{\overline{x}}_i\left(t\right)-d_{ji}=0,j,i=1,\dots ,n$, i.e., USV formation could converge to the expected formation and be maintained under the effect of formation coordination controller.

The verification ends.

6. 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. Model parameters of USV.

Model Parameter	Value	Model Parameter	Value
$m_{11}$	25.8	$b_{32}$	1
$m_{22}$	33.8	$d_{11}$	2.0
$m_{23}$	−11.748	$d_{22}$	7.0
$m_{32}$	−11.748	$d_{23}$	−2.5425
$m_{33}$	6.813	$d_{32}$	−2.5425
$b_{11}$	1	$d_{33}$	1.422
$b_{22}$	0

:

6.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 $\gamma =0.005$ and $\Delta =8$ m; under the proposed strategy in this paper, there is $\gamma =0.0009$, $\delta =3$, ${\Delta }_{\min }=6$ m, and ${\Delta }_{\chi }=8$ m. The simulation is presented in Figure 4, Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14 and Figure 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.

(1) Straight path

The expected route of USV is $y=0$, and the expected velocity is $u_d=3\mathrm{m}/\mathrm{s}$. The initial position of USV is set to (30,−100). The initial heading is ${\psi }_0=\mathsf{\pi }/4\mathrm{rad}$, and the initial velocity is $u_r=0.5\mathrm{m}/\mathrm{s}$. The flow rate of seawater is set to $U_c=0.2\mathrm{m}/\mathrm{s}$, and the flow direction is ${\psi }_c={30}^{\circ }$.

(2) Curved path

The expected track is a continuous curve generated through approximation by five polynomials, that is, $(0,0)$, $(100,100)$, $(150,200)$, $(150,300)$, $(200,400)$, and $(250,600)$. The expected velocity is $u_d=3\mathrm{m}/\mathrm{s}$. The initial position of USV is $(-5,-5)$, and the initial velocity is $u_r=0.5\mathrm{m}/\mathrm{s}$. The heading is ${\psi }_0=\mathsf{\pi }/4\mathrm{rad}$. The flow rate of seawater is set to $U_c=0.2\mathrm{m}/\mathrm{s}$, and the flow direction is ${\psi }_c={30}^{\circ }$.

As revealed in Figure 4 and Figure 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 Figure 5 and Figure 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, Figure 7, Figure 12, and Figure 13, two algorithms could guarantee the velocity stability of USV approximately $u_d=3\mathrm{m}/\mathrm{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, Figure 9, Figure 14, and Figure 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.

6.2. 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 $\gamma =0.0005$, $\delta =3$, ${\Delta }_{\min }=5\mathrm{m}$, ${\Delta }_{\chi }=7\mathrm{m}$. The time-variant disturbance is set to $U_c=0.5\mathrm{m}/\mathrm{s}$; the direction is set to ${\psi }_c(t)=1+0.2\sin (0.12)+\cos (0.3t+\frac{1}{4}\pi )$; and $\mathsf{\Gamma }(·)$ is selected as $\mathsf{\Gamma }\left(x\right)=2/\pi {\tan }^{-1}(x)\in [-1,1]$. The expected path of ${\mathrm{USV}}_1$ is $y=-x$, and its expected velocity is $u_d=10\mathrm{k}\mathrm{n}$. The formation of USV₁, ${\mathrm{USV}}_2$, ${\mathrm{USV}}_3$, ${\mathrm{USV}}_4$, and ${\mathrm{USV}}_5$ is shown in Figure 16:

A triangular formation is made by 5 USVs. Among them, ${\mathrm{USV}}_3$ is at the vertex of the triangle; ${\mathrm{USV}}_2$ and ${\mathrm{USV}}_4$ are on the right and left back of ${\mathrm{USV}}_3$ respectively with the angle of 135° and the distance of 80 m; ${\mathrm{USV}}_1$ and ${\mathrm{USV}}_5$ are on the right and left back of ${\mathrm{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. Initial parameters of USVs.

	$\mathit{x}$	$\mathit{y}$	$\mathit{\psi }$	$\mathit{u}$	$\mathit{v}$	$\mathit{r}$
${\mathrm{USV}}_1$	0	120	0°	0	0	0
${\mathrm{USV}}_2$	0	85	45°	0	0	0
${\mathrm{USV}}_3$	0	55	60°	1	0	0
${\mathrm{USV}}_4$	70	0	90°	2	1	0
${\mathrm{USV}}_5$	120	0	0°	2.5	1	0.5

:

The simulation results are given in Figure 17 and Figure 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.

Figure 18 and Figure 19 show the transverse and longitudinal error variations between the actual and expected positions of each USV. Due to different initial states of USVs, the errors are larger while they are making the formation. However, the transverse and longitudinal errors gradually decrease under the effect of path tracking controller and coordination controller in the middle and late section of the path. At last, their errors converge 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 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 shown in Figure 22, Figure 23, Figure 24, Figure 25 and Figure 26, the formation control algorithm for the control group does not take into account the influence of model parameter perturbation and external disturbance. As a result of the set time-variant disturbance, formation control is not achieved satisfactorily. Lateral, longitudinal, heading angle, and velocity errors are all large. The expected formation under straight line condition could not be maintained.

6.3. USV Formation Cruising Field Experiment

To verify the engineering effectiveness of the proposed USV formation control algorithm, three ‘Jellyfish’ USVs (As shown in Figure 27, Figure 28 and Figure 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.

The expected relative distance and direction among ${\mathrm{USV}}_1$, ${\mathrm{USV}}_2$, and ${\mathrm{USV}}_3$ are $l_{1,2}^{}=300\mathrm{m}$, $l_{1,3}^{}=300\mathrm{m}$, ${\phi }_{1,2}=-135°$, and ${\phi }_{1,3}^{}=135°$. In this case, the expected formation is as shown in Figure 30:

Figure 31 shows the setting interface for the expected path of ${\mathrm{USV}}_1$ on the base station with the coordinates $({120.150320}^{\circ }\mathrm{E},{35.7072377}^{\circ }\mathrm{N})$ for the starting point and $({120.2173775}^{\circ }\mathrm{E},{35.7072468}^{\circ }\mathrm{N})$ for the finishing point. In the figure, a red rectangle indicates the electronic security fence in service, while a black line marks the expected path of ${\mathrm{USV}}_1$. The expected velocity is set to $u_d=15\mathrm{kn}$. The initial parameters of each USV are shown in

Table 3. Initial parameters of USVs.

	Longitude	Latitude	$\mathit{\psi }$	$\mathit{u}$	$\mathit{v}$	$\mathit{r}$
${\mathrm{USV}}_1$	${120.144640}^{\circ }\mathrm{E}$	${35.7111377}^{\circ }\mathrm{N}$	137°	0	0	0
${\mathrm{USV}}_2$	${120.145960}^{\circ }\mathrm{E}$	${35.7051077}^{\circ }\mathrm{N}$	52°	0	0	0
${\mathrm{USV}}_3$	${120.145300}^{\circ }\mathrm{E}$	${35.7005077}^{\circ }\mathrm{N}$	43°	0	0	0

.

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 $\gamma =0.0054$, $\delta =2$, ${\Delta }_{\min }=15\mathrm{m}$, ${\Delta }_{\chi }=8\mathrm{m}$. $\mathsf{\Gamma }(·)$ is $\mathsf{\Gamma }\left(x\right)=2/\pi {\tan }^{-1}(x)\in [-1,1]$.

The results of USV formation cruising control are shown in Figure 32, Figure 33, Figure 34, Figure 35, Figure 36 and Figure 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.

Figure 33 and Figure 34 present the variation curves of USV transverse and longitudinal position 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.

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