*Article* **Modified Vector Field Path-Following Control System for an Underactuated Autonomous Surface Ship Model in the Presence of Static Obstacles**

**Haitong Xu \*, Miguel A. Hinostroza and C. Guedes Soares**

Centre for Marine Technology and Ocean Engineering (CENTEC), Instituto Superior Técnico, Universidade de Lisboa, Av. Rovisco Pais, 1049-001 Lisboa, Portugal; miguel.hinostroza@centec.tecnico.ulisboa.pt (M.A.H.); c.guedes.soares@centec.tecnico.ulisboa.pt (C.G.S.)

**\*** Correspondence: haitng.xu@centec.tecnico.ulisboa.pt; Tel.: +351-218-417-607

**Abstract:** A modified path-following control system using the vector field method for an underactuated autonomous surface ship model is proposed in the presence of static obstacles. With this integrated system, autonomous ships are capable of following the predefined path, while avoiding the obstacles automatically. It is different from the methods in most published papers, which usually study path-following and obstacle collision avoidance, separately. This paper considers the coupled path following and collision avoidance task as a whole. Meanwhile, the paper also shows the heading control design method in the presence of static obstacles. To obtain a strong stability property, a nonlinear autopilot is designed based on the manoeuvring tests of the free-running ship model. The equilibrium point of the controller is globally exponentially stable. For the guidance system, a novel vector field method was proposed, and the proof shows the coupled guidance and control system is uniform semi-global exponentially stable (USGES). To prevent the obstacles near the predefined path, the proposed guidance law is augmented by integrating the repelling field of obstacles so that it can control the ship travel toward the predefined path through the obstacles safely. The repelling field function is given considering the obstacle shape and collision risk using the velocity obstacle (VO) algorithm. The simulations and ship model test were performed to validate the integrated system of autonomous ships.

**Keywords:** path-following; vector field; obstacle avoidance; velocity obstacle algorithm; nonlinear autopilot; underactuated surface ship model

## **1. Introduction**

Autonomous ships have been drawing significant attention recently. The most important reasons for the rapid development of autonomous ships are safety and economic benefit. The risk of maritime transportation is quantified based on various types of total ship losses [1], but as reported by Allianz Global Corporate and Specialty [2], about 75% to 96% of marine accidents can be ultimately attributed to human error. Autonomous shipping can significantly improve safety by reducing human factors.

For autonomous vehicles, the guidance system and control system are two basic low-level systems. They are closely related to transient motion behaviour, such as path following [3], path tracking [4], and path manoeuvring [5]. Therefore, there are two fundamental systems that determine the overall performance [6]. The fundamental requirement of autonomous ships is that they can follow the predefined path fully autonomously or remotely. Guidance systems calculate the desired course or heading angles for the autonomous ships. Their objective is to guide the ships approaching the desired path.

One of the most widely used guidance laws for autonomous ships is line of sight (LOS). It has been reported in many papers, as can be seen in [7–13]. The classical LOS is a typical three-points geometry method [6]. A constant look-ahead distance usually needs

**Citation:** Xu, H.; Hinostroza, M.A.; Guedes Soares, C. Modified Vector Field Path-Following Control System for an Underactuated Autonomous Surface Ship Model in the Presence of Static Obstacles. *J. Mar. Sci. Eng.* **2021**, *9*, 652. https://doi.org/10.3390/ jmse9060652

Academic Editor: Michele Viviani

Received: 5 May 2021 Accepted: 3 June 2021 Published: 12 June 2021

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

to be previously defined for LOS. To improve the robustness, it can also be defined as a function of the cross-track error [14]. Follow-up work can be found in [15]. The LOS was also used for the positioning control of the over-actuated autonomous underwater vehicle (AUV) under the effects of ocean current and model uncertainties in [16,17]. A revised version, integral LOS, was proposed to compensate for the environmental disturbance in [16–19], and its stability was also proved. Considering the varying environmental disturbance, Fossen and Lekkas [20] proposed an adaptive ILOS for the path-following control of marine ships.

The vector field guidance law is a mathematical method. The main idea is to build a vector space, where all the vectors point to the path smoothly. Therefore, if the ship follows the vectors in the space, it will converge to the predefined path finally. Compared with the LOS guidance law, the vector field is a mathematical method with a flexible structure. For the vector field methods, only a vector function needs to be defined, meanwhile, it can also be designed with the specific tasks, for example, collision or desired direction. It was widely used for unmanned aerial vehicles [21–24]. For example, Lawrence et al. [25] proposed a Lyapunov-based vector field and proved the global asymptotic stability. Global uniform bounded stability of vector field guidance law of an unmanned aerial vehicle (UAV) following arbitrary curves was proved by Wang et al. [26]. Recently, it was modified and employed for the path-following control of marine ships and underwater vehicles [27–29].

For a closed-loop control system, the global exponential stable (GES) is the strongest property [30,31], because it can guarantee additional robustness and performance properties of the control system. However, it cannot be achieved for marine ships, because the error dynamic function is local [32–34]. Fossen and Pettersen [32] presented a uniform semi-global exponential stability (USGES) proof for the line-of-sight (LOS) guidance law, and the proof extended the previous results that only guarantee global–exponential stability [34]. In this paper, a time-varying vector field guidance law is proposed, and the equilibrium point is uniform semi-global exponential stable.

For marine surface ships, it is inevitable to encounter obstacles at sea. A collision avoidance system is one of the basic systems for autonomous ships because it makes the ship capable of taking action to local sensor information, [35,36]. It also guarantees that the ship sails safely in unknown or dynamic environments. To improve the safety of autonomous ships, an intelligent decision-making system using fuzzy logic was proposed by Perera et al. [37–39]. Statheros et al. [40] summarized the recent works on collision avoidance for autonomous vehicles. The velocity obstacle (VO) algorithm was employed to prevent the collision of marine ships at seas by Huang et al., [41,42]. Kuwata et al. [43] extended the VO for the ship's navigation by considering the International Regulations for Preventing Collisions (COLREGs). Mou et al. [44] proposed a collision-avoidance system based on the collected AIS data.

The contribution of this paper is to extend the vector field path-following control system for an underactuated autonomous surface ship model in the presence of static obstacles. The proposed system considers the coupled path-following and collision avoidance task as a whole. It is different from the most well-established methods in the literature, where the path-following control and collision avoidance control are usually treated separately. Classical collision avoidance usually emphasized on the minimize the collision risk by assuming the ship are fully controlled. However, few papers explore the autopilot design for autonomous ships in the presence of static obstacles. In this paper, a nonlinear heading controller was designed considering the manoeuvrability of the underactuated surface ship. While different forms of sliding mode controllers have been used [26], a classical sliding mode controller with global exponential stability (GES) is employed here. For the guidance system, a time-varying vector field guidance law was proposed and proved to be uniform semi-global exponential stable (USGES). This guidance law was extended using a risk-based repelling field method. The resulted guidance laws can control the ship to avoid obstacles near the path. The proposed system generates the repelling vectors around the obstacle, which guide the ship to travel away from the obstacles. The repelling

field function is given considering the obstacle shape and collision risk using the velocity obstacle (VO) algorithm. field function is given considering the obstacle shape and collision risk using the velocity obstacle (VO) algorithm. field function is given considering the obstacle shape and collision risk using the velocity obstacle (VO) algorithm.

to avoid obstacles near the path. The proposed system generates the repelling vectors around the obstacle, which guide the ship to travel away from the obstacles. The repelling

to avoid obstacles near the path. The proposed system generates the repelling vectors around the obstacle, which guide the ship to travel away from the obstacles. The repelling

#### **2. Path-Following Control System 2. Path-Following Control System 2. Path-Following Control System**

This section will briefly describe the kinematics and control objects of the pathfollowing of marine ships. As presented in Figure 1, a typical control system includes the guidance law and autopilot. The guidance law provides the desired angle for the autopilot, and the autopilot will steer the rudder of the ship to track the path. This section will briefly describe the kinematics and control objects of the path-following of marine ships. As presented in Figure 1, a typical control system includes the guidance law and autopilot. The guidance law provides the desired angle for the autopilot, and the autopilot will steer the rudder of the ship to track the path. This section will briefly describe the kinematics and control objects of the path-following of marine ships. As presented in Figure 1, a typical control system includes the guidance law and autopilot. The guidance law provides the desired angle for the autopilot, and the autopilot will steer the rudder of the ship to track the path.

**Figure 1.** The block diagrams of path-following control for marine ships. **Figure 1.** The block diagrams of path-following control for marine ships. **Figure 1.** The block diagrams of path-following control for marine ships.

*J. Mar. Sci. Eng.* **2021**, *9*, 652 3 of 20

*J. Mar. Sci. Eng.* **2021**, *9*, 652 3 of 20

Without a loss of generality, a straight path-following control system for marine autonomous surface ships is considered, as presented in Figure 2. In order to simplify the problem, some assumptions and physical constraints were made: Without a loss of generality, a straight path-following control system for marine autonomous surface ships is considered, as presented in Figure 2. In order to simplify the problem, some assumptions and physical constraints were made: Without a loss of generality, a straight path-following control system for marine autonomous surface ships is considered, as presented in Figure 2. In order to simplify the

**Assumption 1**. The motion of the ship is described in three degrees of freedom: surge, sway and yaw. **Assumption 1.** *The motion of the ship is described in three degrees of freedom: surge, sway and yaw.* problem, some assumptions and physical constraints were made: **Assumption 1**. The motion of the ship is described in three degrees of freedom: surge, sway and yaw.

**Assumption 2**. The ship is underactuated in its configuration space. **Assumption 3**. The ship is treated as a rigid body and the maximum rudder angle is 35 **Assumption 2.** *The ship is underactuated in its configuration space.* **Assumption 2**. The ship is underactuated in its configuration space.

degrees. **Assumption 3.** *The ship is treated as a rigid body and the maximum rudder angle is 35 degrees.* **Assumption 3**. The ship is treated as a rigid body and the maximum rudder angle is 35 degrees.

**Figure 2.** Geometry description of path-following control. **Figure 2.** Geometry description of path-following control. **Figure 2.** Geometry description of path-following control.

From Figure 2, the cross-track error can be calculated: From Figure 2, the cross-track error can be calculated: From Figure 2, the cross-track error can be calculated:

$$
\mathbb{E}\begin{bmatrix} 0\\ y\_{\varepsilon} \end{bmatrix} = \mathbf{R}^T(\gamma\_p) \begin{bmatrix} \varkappa(t) - \varkappa\_p(t) \\ y(t) - y\_p(t) \end{bmatrix} \tag{1}
$$

where ( ) *xt yt* ( ), ( ) is the ship's position at time *t*. ( ) ( ), ( ) *p p xtyt* is the orthogonal projection of the ship's position on the predefined path. The ( ) *<sup>p</sup> R* γ is the rotation matrix [45], given: where ( ) *xt yt* ( ), ( ) is the ship's position at time *t*. ( ) ( ), ( ) *p p xtyt* is the orthogonal projection of the ship's position on the predefined path. The ( ) *<sup>p</sup> R* is the rotation matrix [45], given: where (*x*(*t*), *y*(*t*)) is the ship's position at time *t*. *xp*(*t*), *yp*(*t*) is the orthogonal projection of the ship's position on the predefined path. The *R*(*γp*) is the rotation matrix [45], given:

$$\mathbf{R}(\gamma\_p) = \begin{bmatrix} \cos(\gamma\_p) & -\sin(\gamma\_p) \\ \sin(\gamma\_p) & \cos(\gamma\_p) \end{bmatrix} \in \text{SO} \tag{2}$$

γ

The cross-track error ye can be obtained from Equation (1):

$$y\_{\varepsilon}(t) = -\left(\mathbf{x}(t) - \mathbf{x}\_{p}(t)\right)\sin(\gamma\_{p}) + \left(y(t) - y\_{p}(t)\right)\cos(\gamma\_{p})\tag{3}$$

Obviously, the control object is to make the cross-track error converge towards zero. It is given as follows:

$$\lim\_{t \to +\infty} y\_\varepsilon(t) = 0 \tag{4}$$

The kinematic equation of the motion of ships is given as

$$\begin{array}{l}\dot{x} = u\cos(\psi) - v\sin(\psi) \\ \dot{y} = u\sin(\psi) + v\cos(\psi) \\ \dot{\psi} = r \end{array} \tag{5}$$

where *ψ* is the yaw angle. Differentiation of (3), and further simplified by substituting (5), results as .

$$
\dot{y}\_e = \mathcal{U}\sin\left(\psi - \gamma\_p + \beta\right) \tag{6}
$$

where the phase *β* is the drift angle [6]. U is the ground speed of a ship, (*U* = √ *u* <sup>2</sup> + *v* 2 ). For notational simplicity, the time *t* is omitted.

To obtain strong stability properties, a nonlinear sliding mode controller based on the first-order nonlinear Nomoto model was used for the heading control. The Nomoto model with bounded bias term is given [46]:

$$
\dot{r}\,T\dot{r} + n\_3r^3 + n\_1r = K\delta + b\_0 \tag{7}
$$

where *b*<sup>0</sup> ≤ *bmax* is a bounded bias term. *K*, *T*, *n*<sup>3</sup> and *n*<sup>1</sup> are the Nomoto constants. *δ* is the rudder angle. Notice that, *n*<sup>1</sup> = 1 for a stable ship. The parameters can be obtained using the free-running model test in real-time [47]. With the Nomoto model, the sliding surface is defined as

$$s := \left(\frac{d}{dt} + \lambda\right)^2 \left(\int\_0^t \widetilde{\psi}(\tau)d\tau\right) = \dot{\widetilde{\psi}} + 2\lambda\widetilde{\psi} + \lambda^2 \int\_0^t \widetilde{\psi}(\tau)d\tau := \dot{s}\_0 + \lambda s\_0 \tag{8}$$

where *s*<sup>0</sup> = *ψ*e + *λ* R *t* 0 *ψ*e(*τ*)*dτ*. *ψ*e is the heading error. *λ* is a constant [6]. Assume that σ:= r−s, and substitute it into (7) gives:

$$T\dot{s} = K\delta - T\dot{\sigma} - (n\_3r^2 + n\_1)(\sigma + s) + b\_0 \tag{9}$$

Then, the control law can be obtained as

$$\delta = \frac{1}{K} \left( T \dot{\sigma} + \left( n\_3 r^2 + n\_1 \right) \sigma - \mathcal{K}\_d \mathbf{s} - \eta \text{sgn}(\mathbf{s}) \right) \tag{10}$$

where *K<sup>d</sup>* > 0 is the feedback control gain. *η* ≥ *bmax* is a positive design gain [30]. The Lyapunov function can be used to prove that the equilibrium point is globally exponentially stable (GES) (Theorem 4.10 in [29]). The detailed proof can be found in [6,22,48].

In this part, Nomoto parameters will be estimated using system identification based on the manoeuvring tests. The free-running manoeuvring tests were carried out using a scaled ship model (1/65.7) with one propeller and one rudder, as presented in Figure 3. The ship is 2.58 m in length, and 0.43 m in breadth. The designed draft is 0.14 m.

To measure the motion of the ship, various sensors and actuators were used and synchronized using the LabView platform. LabView is a graphical programming environment widely used for data acquisition and control application. It includes the software platform and hardware.

*J. Mar. Sci. Eng.* **2021**, *9*, 652 5 of 20

**Figure 3.** A scaled free-running chemical tanker. **Figure 3.** A scaled free-running chemical tanker. ment widely used for data acquisition and control application. It includes the software

platform and hardware.

To measure the motion of the ship, various sensors and actuators were used and synchronized using the LabView platform. LabView is a graphical programming environment widely used for data acquisition and control application. It includes the software Here, a compactRIO with various modules are used in the free-running ship model. The typical sensors, such as an internal measurement unit, yaw rate sensor, electrical motors, server motor industrial Wi-Fi unit, are given in Figure 4. platform and hardware. Here, a compactRIO with various modules are used in the free-running ship model. The typical sensors, such as an internal measurement unit, yaw rate sensor, electrical motors, server motor industrial Wi-Fi unit, are given in Figure 4.

**Figure 4.** Sensors and actuators used in the free-running ship model. **Figure 4.** Sensors and actuators used in the free-running ship model.

**Figure 4.** Sensors and actuators used in the free-running ship model. The 20 20 − zigzag manoeuvring tests, as suggested by ITTC [49], were conducted in the tank of Laboratory National de Civil Engineering (LNEC). The results are presented in Figure 5. Then, the least square support vector machine (LS-SVM), [50,51], was employed to identify the parameters. More details can be found in [46]. One test is used to The 20 20 − zigzag manoeuvring tests, as suggested by ITTC [49], were conducted in the tank of Laboratory National de Civil Engineering (LNEC). The results are presented in Figure 5. Then, the least square support vector machine (LS-SVM), [50,51], was employed to identify the parameters. More details can be found in [46]. One test is used to The 20◦ − 20◦ zigzag manoeuvring tests, as suggested by ITTC [49], were conducted in the tank of Laboratory National de Civil Engineering (LNEC). The results are presented in Figure 5. Then, the least square support vector machine (LS-SVM), [50,51], was employed to identify the parameters. More details can be found in [46]. One test is used to estimate the parameters, as shown in Figure 5a. The results agree well with the training test. The obtained values of the Nomoto parameters are given: *T* = 7.7515, n<sup>3</sup> = 0.0669, *K* = 0.1129. To validate the results, a new 20◦ − 20◦ zigzag manoeuvring test was chosen as a test set. This test was not used for the training. The validation result is presented in Figure 5b. The

tion.

prediction agrees well with the tests. In the training and validation process, the heading angle is the integration of the yaw rate, which is the prediction of the obtained Nomoto model. Therefore, in order to eliminate the accumulated error due to the integration, one step prediction is adopted when calculating the yaw heading. set. This test was not used for the training. The validation result is presented in Figure 5b. The prediction agrees well with the tests. In the training and validation process, the heading angle is the integration of the yaw rate, which is the prediction of the obtained Nomoto model. Therefore, in order to eliminate the accumulated error due to the integration, one step prediction is adopted when calculating the yaw heading.

estimate the parameters, as shown in Figure 5a. The results agree well with the training test. The obtained values of the Nomoto parameters are given: *T* = 7.7515, n3 = 0.0669, *K* = 0.1129. To validate the results, a new20 20 − zigzag manoeuvring test was chosen as a test

*J. Mar. Sci. Eng.* **2021**, *9*, 652 6 of 20

**Figure 5.** LS-SVM for parameter estimation using 20 20 − zigzag manoeuvring tests: (**a**) training result; and (**b**) valida-**Figure 5.** LS-SVM for parameter estimation using 20◦−20◦ zigzag manoeuvring tests: (**a**) training result; and (**b**) validation.

#### **3. Time-Varying Vector Field Guidance Law**

**3. Time-Varying Vector Field Guidance Law**  As discussed above, the guidance system plays an important role in autonomous ships. It calculates the desired heading or course angle based on the orthogonal distance between the ship and the path. In a simple word, the guidance law plays the same role as an experienced sailor. In this section, a vector field guidance law is used for the pathfollowing control of the underactuated marine surface ship. The vector field method is a novel guidance law for marine ships. Its main principle is to generate vector space around the path to be followed, and all the vectors point to the path smoothly, as presented in Figure 6. The vectors usually denote the desired travelling direction (course angle) for the autonomous vessels. If the ship follows the vectors, it will ultimately converge to the predefined path. As discussed above, the guidance system plays an important role in autonomous ships. It calculates the desired heading or course angle based on the orthogonal distance between the ship and the path. In a simple word, the guidance law plays the same role as an experienced sailor. In this section, a vector field guidance law is used for the pathfollowing control of the underactuated marine surface ship. The vector field method is a novel guidance law for marine ships. Its main principle is to generate vector space around the path to be followed, and all the vectors point to the path smoothly, as presented in Figure 6. The vectors usually denote the desired travelling direction (course angle) for the autonomous vessels. If the ship follows the vectors, it will ultimately converge to the predefined path. *J. Mar. Sci. Eng.* **2021**, *9*, 652 7 of 20

(11)

**Figure 6.** Vector space around the predefined path (red line). **Figure 6.** Vector space around the predefined path (red line).

As discussed above, the low-level controller is GES and can track the desired heading angle perfectly. Then, substituting (11) into (6) gives: The function for the generation of vectors is important because it determines the quality of the vectors, such as the distribution and strength of vectors, the convergence

( ) ,

*<sup>e</sup> t y*

θ

(12)

*e*

( )

*e*

,

*t y e*

( ) ( )

*y*

*e e*

θ

, ,1

*e e*

*ty ty e e*

*e e e e*

 θ

Φ = is positive and lower-bounded, then the

Φ = ≥≈ > is positive and lower-bounded. The

*y y*

*e e*

(13)

*V t e e y y* = (14)

( )

 θ

( ) ( )

 θ

θ

θ

*ty y* = +

*t y* , must

+

*e*

2 , 2 ,

θ

−

*U y*

Δ +

( )

*e e*

<sup>1</sup> sgn sin tan

*<sup>y</sup> y yU*

( )

1

*Uy Uy V ty y y*

θ

, *<sup>e</sup> t y e e ty y*

equilibrium point is uniformly stable. As discussed in [48,52], the function, (, )*<sup>e</sup>*

θ−

equilibrium point ye = 0 is uniform semi-global exponential stable (USGES) (Definition 2.7 by Loría and Panteley [31]). In this paper, the function is defined as ( , ) 0.4 1 *e e*

cross-track error, *<sup>e</sup> y* , depends on the initial error and then decreases exponentially with

*e e t y t y*

θ

To prove the stability of the guidance law, the Lyapunov method is used and the

( ) <sup>2</sup>

<sup>1</sup> , <sup>2</sup>

( )

, sgn 0

θ

=− =− ≤ Δ+ Δ+ (15)

( ) ( )

*e ee ty ty ty ty*

Since 1(, ) 0 *Vtye* > and 1(, ) 0 *Vty <sup>e</sup>* <sup>≤</sup> , according to Theorem 4.8 by Khalil [30], the

<sup>1</sup> 2, 2, 2, 2,

θ

sgn

*y y*

= −

It can be further simplified as

Lyapunov candidate is:

time.

The time derivative is:

( ) ()

guarantee that the function ( ) ( ) , 1

and the function ( ) <sup>2</sup> 0.4 <sup>5</sup> , 0.86 0 *ey <sup>e</sup> e e ty y e*<sup>−</sup>

rate, etc. Meanwhile, it also indirectly determines the performance of the guidance system of underactuated autonomous ships. Here, a vector field is given:

$$\psi\_d = \gamma\_p - \text{sgn}(y\_\epsilon) \tan^{-1} \left( \left( \frac{|y\_\epsilon|}{\Delta} \right)^{\theta(t, y\_\epsilon)} \right) - \beta \tag{11}$$

where *θ*(*t*, *ye*) is a time-varying function, ∆ > 0 is a constant, and ∆min ≤ ∆ ≤ ∆max.

As discussed above, the low-level controller is GES and can track the desired heading angle perfectly. Then, substituting (11) into (6) gives:

$$\dot{y}\_{\varepsilon} = -\text{sgn}(y\_{\varepsilon})\mathcal{U}\sin\left(\tan^{-1}\left(\left(\frac{|y\_{\varepsilon}|}{\Delta}\right)^{\theta(t,y\_{\varepsilon})}\right)\right) \tag{12}$$

It can be further simplified as

$$\dot{y}\_{\varepsilon} = -\text{sgn}(y\_{\varepsilon}) \frac{\mathcal{U}|y\_{\varepsilon}|^{\theta(t, y\_{\varepsilon})}}{\sqrt{\Delta^{2\theta(t, y\_{\varepsilon})} + |y\_{\varepsilon}|^{2\theta(t, y\_{\varepsilon})}}} \tag{13}$$

To prove the stability of the guidance law, the Lyapunov method is used and the Lyapunov candidate is:

$$V\_1(t, y\_\varepsilon) = \frac{1}{2} y\_\varepsilon^2 \tag{14}$$

The time derivative is:

$$\dot{V}\_1(t, y\_\varepsilon) = -\text{sgn}(y\_\varepsilon) y\_\varepsilon \frac{\mathcal{U}|y\_\varepsilon|^{\theta(t, y\_\varepsilon)}}{\sqrt{\Delta^{2\theta(t, y\_\varepsilon)} + |y\_\varepsilon|^{2\theta(t, y\_\varepsilon)}}} = -\frac{\mathcal{U}|y\_\varepsilon|^{\theta(t, y\_\varepsilon) + 1}}{\sqrt{\Delta^{2\theta(t, y\_\varepsilon)} + |y\_\varepsilon|^{2\theta(t, y\_\varepsilon)}}} \le 0 \tag{15}$$

Since *V*1(*t*, *ye*) > 0 and . *V*1(*t*, *ye*) ≤ 0, according to Theorem 4.8 by Khalil [30], the equilibrium point is uniformly stable. As discussed in [48,52], the function,*θ*(*t*, *ye*), must guarantee that the function Φ(*t*, *ye*) = |*ye*| *θ*(*t*,*ye*)−1 is positive and lower-bounded, then the equilibrium point y<sup>e</sup> = 0 is uniform semi-global exponential stable (USGES) (Definition 2.7 by Loría and Panteley [31]). In this paper, the function is defined as *θ*(*t*, *ye*) = 0.4|*ye*| + 1 and the function Φ(*t*, *ye*) = |*ye*| 0.4|*ye*<sup>|</sup> <sup>≥</sup> *<sup>e</sup>* − 2 <sup>5</sup>*<sup>e</sup>* ≈ 0.86 > 0 is positive and lower-bounded. The cross-track error, *y<sup>e</sup>* , depends on the initial error and then decreases exponentially with time.

#### **4. Risk-Based Obstacle Collision Avoidance System**

An obstacle avoidance control system is proposed for autonomous surface ships. Usually, the desired path should be planned with a global world map, so the ship can travel safely. However, it cannot neglect the obstacle near the path, which was not displayed on the map, for example, large sea animals or floating marine structure, etc. The obstacle avoidance system can make the autonomous ship respond to the dynamic local sensor information and guide the ship safely to avoid the obstacles.

Assuming that there is one obstacle near the path, to avoid the obstacle, the directions of the vector need to be changed. Here, a repelling field function is used to generate the vector with the angle, *ψ<sup>r</sup>* . When the ship is near the obstacle, the vector will control the ship travel away from the obstacle with the aid of the heading controller.

Figure 7 shows the principle of the collision avoidance system for autonomous surface ships. A repelling field function is used to generate a heading angle *ψ<sup>r</sup>* . If neglecting the path-following task, the ship tracks this heading angle *ψ<sup>r</sup>* , which will guide the ship to travel away from the obstacle. Meanwhile, the vector field guidance law will generate the heading angle *ψ*<sup>v</sup> which attracts the ship moving toward the path. The final resulted heading angle, *ψ*<sup>d</sup> = *ψ<sup>r</sup>* + *ψv*, is given in Figure 7. Obviously, the repelling function needs to be defined carefully, so the ship can travel toward the path smoothly, and meanwhile, avoiding the obstacle. The repelling function is given as

$$\psi\_r = \tan^{-1} \left( \frac{a(p\_0)(y)}{a(p\_0)(x)} \right) \tag{16}$$

where *p*<sup>0</sup> = [*x*0, *y*0] is the location of the obstacle. The function *a*(*p*<sup>0</sup> ) is defined as

$$\mathfrak{a}(\mathfrak{p}\_0) = \left( A \cdot \exp\left( -\left( a(\mathbf{x} - \mathbf{x}\_0)^2 + 2b(\mathbf{x} - \mathbf{x}\_0)(\mathbf{y} - \mathbf{y}\_0) + c(\mathbf{y} - \mathbf{y}\_0)^2 \right) \right) \right) \frac{\mathfrak{p} - \mathfrak{p}\_0}{\|\mathbf{p} - \mathbf{p}\_0\|} \tag{17}$$

where, *p* = [*x*, *y*] is the location of the ship. *a*, *b* and *c* the parameters defined according to the orientation of the obstacle, *γo*, they are given as

$$\begin{cases} a = \frac{\cos^2(\gamma\_o)}{2\sigma\_x^2} + \frac{\sin^2(\gamma\_o)}{2\sigma\_y^2} \\ b = -\frac{\cos(2\gamma\_o)}{4\sigma\_x^2} + \frac{\sin(2\gamma\_o)}{4\sigma\_y^2} \\ c = \frac{\sin^2(\gamma\_o)}{2\sigma\_x^2} + \frac{\cos^2(\gamma\_o)}{2\sigma\_y^2} \end{cases} \tag{18}$$

where *σ<sup>x</sup>* and *σ<sup>y</sup>* are deviations in the *x* and *y* directions. It can be observed that the repelling function in Equation (17) defines a collision area around the obstacle. Once the ship enters the areas, the repelling field will work, but there is a disadvantage of this method: that the repelling field method will push the ship away from the obstacle, even though the ship will not collide with the obstacles. To solve this problem, a collision risk will be introduced in the following part. Collision risk is introduced by employing the principle of the VO algorithm, where the velocity of the obstacle is zero. The general definition of the velocity obstacle for a ship in the presence of a static obstacle is given:

**Definition 1.** *The velocity obstacle (VO) for a ship in the presence of the obstacles is the set of all relative speed of the ship to obstacles that will result in a collision.*

In other words, if the ship chooses a velocity from the VO set, the ship collides with the obstacle eventually. To define the collision in mathematical terms, let a ray starting from the ship, located at *p*, go in the direction of *v* which is defined as

$$
\lambda(\mathfrak{p}, \mathfrak{v}) = \{\mathfrak{p} + \mathfrak{v}t | t \ge 0\} \tag{19}
$$

Usually, the collision position is defined using the Minkowski addition [41,44]. Here, to simplify the problem, ship safety domains [53] can be used for reference to defined a conflict position (ConfP), as presented in Figure 8. The conflict position (ConfP) is the area surrounded by a red elliptical line. The velocity obstacle (VO) can be defined as

$$VO = \{\mathfrak{v}|\lambda(\mathfrak{p}, \mathfrak{v}) \in \operatorname{Conf} fP\} \tag{20}$$

As can be observed, the VO region has the geometric shape of a cone, and the Equation (20) can be represented as

$$VO = \left\{ \mathbf{v} \, \middle| \, \mathbf{v} \cdot \mathbf{P}\_{\text{Left}} \ge \mathbf{0} \cap \mathbf{v} \cdot \mathbf{P}\_{\text{Right}} \ge \mathbf{0} \right\} \tag{21}$$

where (·) is the vector dot product. PLeft and PRight are vectors perpendicular to the left and right edges of the cone, respectively. As presented in Figure 9, the velocity obstacle cone splits the space into four regions [43]. These are region V1, to avoid the obstacle while seeing it on the right, region V2, to avoid the obstacle while seeing it on the left, and region V3 which is where the ship moves away from the obstacles. The vectors, PLeft and PRight, are given [54]: In other words, if the ship chooses a velocity from the VO set, the ship collides with the obstacle eventually. To define the collision in mathematical terms, let a ray starting

PLeft = *R*(−*α* + *π* 2 ) *P* k*P*k , PRight = *R*(*α* − *π* 2 ) *P* k*P*k (22) from the ship, located at *p* , go in the direction of *v* which is defined as λ(,) 0 *pv p v* =+ ≥ { } *t t* (19)

where *α* is the angle between the centre line and the cone edges, which is given as

$$a = \arcsin\left(\frac{e}{\|P\|}\right) \tag{23}$$

where *e* is the distance of the centre of ConfP to the edge of the VO region. *R*(*θ*) is the rotation matrix. surrounded by a red elliptical line. The velocity obstacle (VO) can be defined as *VO ConfP* = ∈ { } *v pv* λ(,) (20)

**Figure 7.** The proposed collision avoidance system for autonomous surface ships ((**a**): geometry chart; and (**b**): the flowchart). **Figure 7.** The proposed collision avoidance system for autonomous surface ships ((**a**): geometry chart; and (**b**): the flowchart).

As discussed above, a collision risk-based obstacle avoidance guidance law can be defined as

$$\psi\_{\mathbf{r}} = \sum\_{i=1}^{N} f(p\_i) \cdot \tan^{-1}(a\_i(p\_i)) \tag{24}$$

where *N* is the number of obstacles. *f*(*pi*) is the collision risk and defined as

$$f(p\_i) = \begin{cases} 1 & \text{if } \mathfrak{v} \in V\mathcal{O} \\ 0 & \text{else} \end{cases} \tag{25}$$

chart; and (**b**): the flowchart).

tion (20) can be represented as

**Figure 8.** The velocity obstacle (VO) with a static obstacle. The green area represents the obstacle. The area surrounded by a dotted elliptical line is the obstacle domain. The area surrounded by a red elliptical line is the conflict position (ConfP). <sup>=</sup> where *e* is the distance of the centre of ConfP to the edge of the VO region. *R*( ) θ is the rotation matrix.

P ( ) , P ( ) Left Right 2 2 π

α

α

= −+ = −

*P P R R*

arcsin *<sup>e</sup> P*

(**a**) (**b**)

*VO*= ⋅ ≥ ∩⋅ ≥ { } P 0 P 0 Left Right *vv v* (21)

 π

 α

*P P*

,

(22)

(23)

As can be observed, the VO region has the geometric shape of a cone, and the Equa-

**Figure 7.** The proposed collision avoidance system for autonomous surface ships ((**a**): geometry

**Figure 8.** The velocity obstacle (VO) with a static obstacle. The green area represents the obstacle. The area surrounded by a dotted elliptical line is the obstacle domain. The area surrounded by a

*J. Mar. Sci. Eng.* **2021**, *9*, 652 10 of 20

red elliptical line is the conflict position (ConfP).

In other words, if the ship chooses a velocity from the VO set, the ship collides with the obstacle eventually. To define the collision in mathematical terms, let a ray starting

Usually, the collision position is defined using the Minkowski addition [41,44]. Here, to simplify the problem, ship safety domains [53] can be used for reference to defined a conflict position (ConfP), as presented in Figure 8. The conflict position (ConfP) is the area

> *VO ConfP* = ∈ { } *v pv* λ

(,) 0 *pv p v* =+ ≥ { } *t t* (19)

(,) (20)

from the ship, located at *p* , go in the direction of *v* which is defined as

surrounded by a red elliptical line. The velocity obstacle (VO) can be defined as

λ

**Figure 9.** Illustration of the velocity obstacle (VO) regions. **Figure 9.** Illustration of the velocity obstacle (VO) regions.

#### As discussed above, a collision risk-based obstacle avoidance guidance law can be **5. Case Study**

defined as ( ) <sup>1</sup> 1 ( ) tan ( ) *N r i ii i* ψ *f p* <sup>−</sup> = = ⋅ *a p* (24) where *N* is the number of obstacles. ( )*<sup>i</sup> f p* is the collision risk and defined as 1 ( ) <sup>0</sup>*<sup>i</sup> if VO f p else* <sup>∈</sup> <sup>=</sup> *<sup>v</sup>* (25) To validate the proposed system, the simulation and model tests were carried out in this section. The simulation tests were carried out in Matlab software platform using a laptop with i7-6700HQ CPU and 16G RAM. In the simulations, static obstacles avoidance was considered, and the obstacles are near the predefined path. The information about the position and size of obstacles are assumed to be available in advance. In the real application, these data can be obtained from marine Radar. The model test was carried out in a swimming pool using the ship model introduced in the previous section.

#### *5.1. Nonlinear Manoeuvring Model*

A nonlinear manoeuvring model was used in the simulation, which is a modified version of Abkowitz model, and was validated with the manoeuvring tests [55]. The values of the hydrodynamic coefficients can be found in [55]. The time derivatives of *u*, *v* and *r* are given:

$$\begin{cases} \dot{u} = \frac{f\_1}{m - X\_{\dot{u}\_r}} \\ \dot{v} = \frac{1}{f\_4} [(I\_z - N\_{\dot{r}})f\_2 - (m\chi\_G - Y\_{\dot{r}})f\_3] \\ \dot{r} = \frac{1}{f\_4} \left[ (m - Y\_{\dot{v}\_r})f\_3 - (m\chi\_G - N\_{\dot{v}\_r})f\_2 \right] \end{cases} \tag{26}$$

where the nondimensionalized forces and moments are given:

$$\begin{cases} \begin{aligned} f'\_{1} &= \eta'\_{1}u'^2 + \eta'\_{2}u'u' + \eta'\_{3}u'^2 - \mathcal{C}'\_{R} + \mathcal{X}'^{2}\_{v}v'^2 + \mathcal{X}'\_{\mathcal{C}}e^2 + \left(\mathcal{X}'\_{\mathcal{C}} + m'\mathcal{X}\_{\mathcal{G}}\right)r'^2 + (\mathcal{X}'\_{vv} + m')v'r' + \mathcal{X}'\_{\mathcal{Z}^2v^2}r'^2\\ f'\_{2} &= \mathcal{Y}'\_{0} + \left\{\mathcal{Y}'\_{v}v' + \mathcal{Y}'\_{\mathcal{S}}(c - c\_{0})r'\right\} + \left\{(\mathcal{Y}'\_{r} - m'u')r - \frac{\mathcal{Y}'\_{\mathcal{C}}}{2}(c - c\_{0})r'\right\} + \mathcal{Y}'\_{\mathcal{S}}\delta + \mathcal{Y}'\_{\mathcal{V}^2v'}r' + \mathcal{Y}'\_{\mathcal{C}}e^2\\ f'\_{3} &= \mathcal{N}'\_{0} + \left\{\mathcal{N}'\_{v}v' - \mathcal{N}'\_{\delta}(c - c\_{0})r'\right\} + \left\{(\mathcal{N}'\_{r} - m'\mathcal{X}\_{\mathcal{C}}v')r + \frac{1}{2}\mathcal{N}'\_{\delta}(c - c\_{0})r'\right\} + \mathcal{N}'\_{\delta}\delta + \mathcal{N}'\_{\mathcal{V}^2v'}r' + \mathcal{N}'\_{\mathcal{C}}e^2\\ f'\_{4} &= \left(m' - Y'\_{v}\right)\left(\mathcal{I}'\_{z} - \mathcal{N}'\_{r}\right) - \left(m'\mathbf{x}'\_{G} - \mathcal{N}'\_{v}\right)\left(m'\mathbf{x}'\_{G} - \mathcal{Y}'\_{r}\right) \end{aligned} \tag{27}$$

For the autopilot, the parameters are defined as: *K<sup>d</sup>* = 0.4, *η* = 1 and *λ* = 0.1. The timevarying function is chosen as *θ*(*t*, *ye*) = 0.4|*ye*| + 1, which renders the system equilibrium point of the guidance subsystem as USGES.

#### *5.2. Single Static Obstacle*

In the simulation, one static obstacle near the path is considered. When the obstacle is round, as presented in Figure 10a,b, the deviations are set as, *σ<sup>x</sup>* = *σ<sup>y</sup>* = 1. When the obstacle is rectangular, an asymmetric repelling field can be generated by setting the different deviations in the *x* and *y* directions, as presented in Figure 10c,d. The deviations of the repelling field can be set as *σ<sup>x</sup>* = 2*Lpp*, *σ<sup>y</sup>* = 3*B*, where *L* is the length of the ship and B is the width. The obstacle locates at the same position in both case, *p*<sup>0</sup> = [10, 5].

The trajectory of the ship in the simulation is given in Figure 10. The blue vectors show the desired heading angle, generated using the repelling function. In Figure 10a, a simulation around static obstacle is given, where the collision risk switches on. The trajectory of the ship is more practical compared with the trajectory in Figure 10b, where the collision risk control was switched off. The obstacle collision avoidance system switches off when the collision risk is zero, which can be observed from the partial enlargement in Figure 10a. When the collision risk control is switched off, as shown in Figure 10b, the obstacle collision avoidance system will push the ship away from the obstacle until it arrives at a safe distance, which is defined by the Gaussian function, as defined in Equations (17) and (18). From Figure 10b, the collision avoidance system without considering the collision risk will inevitably result in some overshoots, which increases fuel consumption. The computational cost and path length are given in Table 1. Obviously, the path length is shorter when considering the collision risk.

The simulation with a rectangular static obstacle near the path is presented in Figure 10c,d. An asymmetric repelling field is used in the simulation. In Figure 10c, the collision risk control is switched on. The resulted trajectory is more reasonable, and the heading angle generated using the repelling vector field is zero when the ship is located outside the velocity obstacle area. The trajectory with a large oscillation is presented in Figure 10d. It results from alternative actions of repelling field and vector field, where repelling field generated the heading angle that pull the ship away from the obstacle, while the vector field provides the opposite effect. As can be observed, the proposed method can control the ship to avoid the obstacles, meanwhile, the path-following task is also an important factor. The traditional obstacle avoidance system usually only emphasizes how to minimize the collision risk [56,57]. Therefore, the collision avoidance methods will take the ship to travel away from obstacles and neglect the path-following task. The proposed method considers both tasks, path-following and obstacle avoidance, at the same time.

In the beginning, the ship travels at a high speed, then the ship will reduce its speed to avoid the obstacles. During the collision avoidance, the ship travels at a constant speed. Figure 11 shows the heading angle and surge speed (desired versus true) in four cases. The ship tracks the desired heading angle, as presented in Figure 11. It demonstrates that the autopilot works well in the simulations. When the collision risk control is switched off, the underactuated surface ship takes a long time to converge to the desired heading angle, which is due to the alternative actions of repelling field and vector field guidance laws. This phenomenon is more obvious in Figure 11d.

obstacle collision avoidance system will push the ship away from the obstacle until it arrives at a safe distance, which is defined by the Gaussian function, as defined in Equations (17) and (18). From Figure 10b, the collision avoidance system without considering the collision risk will inevitably result in some overshoots, which increases fuel consumption. The computational cost and path length are given in Table 1. Obviously, the path length

**Table 1.** The computational cost and path length in the simulations with a single obstacle.

 **Test a Test b Test c Test d** 

Path length (m) 29.142 23.648 26.950 48.820 Computational cost (s) 1.738 1.310 1.324 2.167

is shorter when considering the collision risk.

**Figure 10.** Trajectory of the underactuated ship: (**a**) round obstacle with collision risk; (**b**) round obstacle without collision risk; (**c**) rectangular obstacle with collision risk; and (**d**) rectangular obstacle without collision risk. **Figure 10.** Trajectory of the underactuated ship: (**a**) round obstacle with collision risk; (**b**) round obstacle without collision risk; (**c**) rectangular obstacle with collision risk; and (**d**) rectangular obstacle without collision risk.

The simulation with a rectangular static obstacle near the path is presented in Figure 10c,d. An asymmetric repelling field is used in the simulation. In Figure 10c, the collision **Table 1.** The computational cost and path length in the simulations with a single obstacle.


Figure 12 presents the rudder angles, cross-track errors in the simulations. The chattering due to the sliding mode control is diminished using the saturation function. The collision risk can reduce the rudder oscillations and have smaller cross-track errors, as presented in Figure 12a,c. In Figure 12b,d, the cross-track errors increase a bit when the ship approaches the obstacle. This is because that the repelling vector field plays a major role in the guidance system, and almost completely cancels the effect of the vector field guidance law.

the velocity obstacle area. The trajectory with a large oscillation is presented in Figure 10d. It results from alternative actions of repelling field and vector field, where repelling field generated the heading angle that pull the ship away from the obstacle, while the vector field provides the opposite effect. As can be observed, the proposed method can control the ship to avoid the obstacles, meanwhile, the path-following task is also an important factor. The traditional obstacle avoidance system usually only emphasizes how to minimize the collision risk [56,57]. Therefore, the collision avoidance methods will take the ship to travel away from obstacles and neglect the path-following task. The proposed method considers both tasks, path-following and obstacle avoidance, at the same time.

In the beginning, the ship travels at a high speed, then the ship will reduce its speed to avoid the obstacles. During the collision avoidance, the ship travels at a constant speed. Figure 11 shows the heading angle and surge speed (desired versus true) in four cases. The ship tracks the desired heading angle, as presented in Figure 11. It demonstrates that the autopilot works well in the simulations. When the collision risk control is switched off, the underactuated surface ship takes a long time to converge to the desired heading angle, which is due to the alternative actions of repelling field and vector field guidance

laws. This phenomenon is more obvious in Figure 11d.

**Figure 11.** Heading angle, surge speed (desired versus true) from the simulations: (**a**) round obstacle with collision risk; (**b**) round obstacle without collision risk; (**c**) rectangular obstacle with collision risk; and (**d**) rectangular obstacle without collision risk. **Figure 11.** Heading angle, surge speed (desired versus true) from the simulations: (**a**) round obstacle with collision risk; (**b**) round obstacle without collision risk; (**c**) rectangular obstacle with collision risk; and (**d**) rectangular obstacle without collision risk.

#### Figure 12 presents the rudder angles, cross-track errors in the simulations. The chat-*5.3. Multi Static Obstacles*

tering due to the sliding mode control is diminished using the saturation function. The The simulations on path-following and obstacle avoidance control are carried out in the presence of two static obstacles. Both round and rectangular obstacles are considered in the simulations.

The deviations of the repelling function can be chosen from the same values. The positions of the two obstacles in the round case are *p*<sup>0</sup> = [10, 5] and *p*<sup>1</sup> = [15, 10]. The positions of two rectangular obstacles are *p*<sup>0</sup> = [10, 5] and *p*<sup>1</sup> = [25, 10], respectively. According to the above discussion, collision risk plays an important role in the obstacle collision avoidance system. Therefore, collision risk control was switched on in the following simulation.

Figure 13 shows the trajectories of the ship in the simulations. In Figure 13a, the simulation with two round obstacles is studied. From the partial enlargement of Figure 13, the desired heading angle due to the repelling field is zero when the ship is out of the VO area. In other words, the collision risk of the ship is zero. Only the vector field guidance law will work and the ship will follow the desired heading angle generated by the guidance system. It will converge to the path. When the ship enters the VO area of the second obstacle, the collision risk is nonzero, as presented in Figure 13. The simulation with two rectangular obstacles is presented in Figure 13b. From this Figure, the proposed system can control the ship travelling towards the predefined path and avoid the static obstacles. The computational cost and path length during the simulations are given in Table 2.

guidance law.

collision risk can reduce the rudder oscillations and have smaller cross-track errors, as presented in Figure 12a,c. In Figure 12b,d, the cross-track errors increase a bit when the ship approaches the obstacle. This is because that the repelling vector field plays a major role in the guidance system, and almost completely cancels the effect of the vector field

**Figure 12.** Rudder angle, cross-track error from the simulations: (**a**) round obstacle with collision risk; (**b**): round obstacle without collision risk; and (**c**) rectangular obstacle with collision risk; (**d**): rectangular obstacle without collision risk. **Figure 12.** Rudder angle, cross-track error from the simulations: (**a**) round obstacle with collision risk; (**b**): round obstacle without collision risk; and (**c**) rectangular obstacle with collision risk; (**d**): rectangular obstacle without collision risk. rectangular obstacles is presented in Figure 13b. From this Figure, the proposed system can control the ship travelling towards the predefined path and avoid the static obstacles. The computational cost and path length during the simulations are given in Table 2.

**Figure 13.** Path-following simulation with multi static obstacles: (**a**) round obstacles; and (**b**) rectangular obstacles. **Figure 13.** Path-following simulation with multi static obstacles: (**a**) round obstacles; and (**b**) rectangular obstacles.

**Table 2.** The computational cost and path length in the simulations with multi static obstacles.

Computational cost (s) 1.474 1.516

PsiC PsiD

> u uD

Figure 14 shows the heading angle and surge speed in the simulations. The crosstack error and rudder angle are presented in Figure 15. As can be observed, the rudder angle changes significantly in the second case due to the different shape of the obstacles.

0.2 0.4 0.6 0.8

u vs. uD (knots)

Psi vs. PsiD (deg)

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> <sup>0</sup>

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> <sup>0</sup>

Time(s)

Psi PsiD

> u uD

(**a**) (**b**)

The cross-tack errors converge to zero in both cases.

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -100

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> <sup>0</sup>

Time(s)

0.1 0.2 0.3 0.4 0.5

u vs. uD (m/s)

PsiC vs. PsiD (deg)

tangular obstacles.

0

5

10

Y (m)

15

**8 8.5 9 9.5 10 10.5 11**

**8.5 9 9.5 10 10.5 11 11.5**

20

25

30

<sup>0</sup> <sup>5</sup> <sup>10</sup> <sup>15</sup> <sup>20</sup> <sup>25</sup> <sup>30</sup> -5

**2.5 3 3.5 4 4.5**

**10.5 11 11.5 12 12.5**

X (m)


**Table 2.** The computational cost and path length in the simulations with multi static obstacles. **Table 2.** The computational cost and path length in the simulations with multi static obstacles.

**Figure 13.** Path-following simulation with multi static obstacles: (**a**) round obstacles; and (**b**) rec-

(**a**) (**b**)

0

5

10

Y (m)

15

20

**3 3.5 4 4.5 5 5.5**

**12.5 13 13.5 14 14.5 15**

25

30

<sup>0</sup> <sup>5</sup> <sup>10</sup> <sup>15</sup> <sup>20</sup> <sup>25</sup> <sup>30</sup> -5

**0.5 1 1.5 2** Ship trajectory Desired path Waypoints

**10.5 11 11.5**

X (m)

*J. Mar. Sci. Eng.* **2021**, *9*, 652 15 of 20

area. In other words, the collision risk of the ship is zero. Only the vector field guidance law will work and the ship will follow the desired heading angle generated by the guidance system. It will converge to the path. When the ship enters the VO area of the second obstacle, the collision risk is nonzero, as presented in Figure 13. The simulation with two rectangular obstacles is presented in Figure 13b. From this Figure, the proposed system can control the ship travelling towards the predefined path and avoid the static obstacles. The computational cost and path length during the simulations are given in Table 2.

> Collision space Obtacles Ship trajectory Desired path Waypoints

Figure 14 shows the heading angle and surge speed in the simulations. The cross-tack error and rudder angle are presented in Figure 15. As can be observed, the rudder angle changes significantly in the second case due to the different shape of the obstacles. The cross-tack errors converge to zero in both cases. Figure 14 shows the heading angle and surge speed in the simulations. The crosstack error and rudder angle are presented in Figure 15. As can be observed, the rudder angle changes significantly in the second case due to the different shape of the obstacles. The cross-tack errors converge to zero in both cases.

**Figure 14.** Heading angle, surge speed (desired versus true): (**a**) multi round obstacle; and (**b**) multi rectangular obstacle. **Figure 14.** Heading angle, surge speed (desired versus true): (**a**) multi round obstacle; and (**b**) multi rectangular obstacle.

**Figure 15.** Rudder angle, cross-track errors: (**a**) multi round obstacle; and (**b**) multi rectangular **Figure 15.** Rudder angle, cross-track errors: (**a**) multi round obstacle; and (**b**) multi rectangular obstacle.

#### *5.4. Collision Avoidance Test Using Ship Model*

obstacle.

*5.4. Collision Avoidance Test Using Ship Model*  The collision avoidance test is carried out using a free-running ship model, which was described in Section 2. The sensors and actuators are installed on the ship model, as presented in Figure 4. The control system is programmed in Labview platform. The test was carried out in a swimming pool, as described in Figure 16. The maximum length is 50 The collision avoidance test is carried out using a free-running ship model, which was described in Section 2. The sensors and actuators are installed on the ship model, as presented in Figure 4. The control system is programmed in Labview platform. The test was carried out in a swimming pool, as described in Figure 16. The maximum length is 50 m and the width is 20 m, the depth is 1.2–1.8 m.

m and the width is 20 m, the depth is 1.2–1.8 m. The path-following and collision avoidance were carried out using a scaled marine surface ship model. Here, only one obstacle was considered due to the limited geometry dimension of the swimming pool. The obstacle was assumed to be located in the middle of the swimming pool. The ship will travel from the northeast to the southwest corner of the swimming pool. Before tests, the battery was charged fully, and the draft of the ship The path-following and collision avoidance were carried out using a scaled marine surface ship model. Here, only one obstacle was considered due to the limited geometry dimension of the swimming pool. The obstacle was assumed to be located in the middle of the swimming pool. The ship will travel from the northeast to the southwest corner of the swimming pool. Before tests, the battery was charged fully, and the draft of the ship model was adjusted to the designed value. The rudder and propeller were checked and

model was adjusted to the designed value. The rudder and propeller were checked and tested in manual operation. The sensors were initiated and were calibrated to zero, such

and wind sensor. During the model tests, the revolutions per minute (RPM) was set as constant. At the beginning of the tests, the ship model was released with zero rudder angle and constant RPM. If the ship cannot go straight, it is necessary to change the position of

the weights in the ship model.

tested in manual operation. The sensors were initiated and were calibrated to zero, such as the Differential Global Positioning System (DGPS), inertial measurement unit (IMU), and wind sensor. During the model tests, the revolutions per minute (RPM) was set as constant. At the beginning of the tests, the ship model was released with zero rudder angle and constant RPM. If the ship cannot go straight, it is necessary to change the position of the weights in the ship model. model was adjusted to the designed value. The rudder and propeller were checked and tested in manual operation. The sensors were initiated and were calibrated to zero, such as the Differential Global Positioning System (DGPS), inertial measurement unit (IMU), and wind sensor. During the model tests, the revolutions per minute (RPM) was set as constant. At the beginning of the tests, the ship model was released with zero rudder angle and constant RPM. If the ship cannot go straight, it is necessary to change the position of the weights in the ship model.

*J. Mar. Sci. Eng.* **2021**, *9*, 652 16 of 20

multi rectangular obstacle.

obstacle.


Cross-track error [m]

Rudder angle [deg]

*5.4. Collision Avoidance Test Using Ship Model* 

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -40

Time (sec)

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -15

Time (sec)

m and the width is 20 m, the depth is 1.2–1.8 m.

**Figure 14.** Heading angle, surge speed (desired versus true): (**a**) multi round obstacle; and (**b**)

Rudder angle, δ

Cross-track error, ye

(**a**) (**b**)


Cross-track error [m]

Rudder angle [deg]

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -40

Rudder angle, δ

Cross-track error, ye

Time (sec)

<sup>0</sup> <sup>20</sup> <sup>40</sup> <sup>60</sup> <sup>80</sup> <sup>100</sup> <sup>120</sup> <sup>140</sup> <sup>160</sup> -15

Time (sec)

The collision avoidance test is carried out using a free-running ship model, which was described in Section 2. The sensors and actuators are installed on the ship model, as presented in Figure 4. The control system is programmed in Labview platform. The test was carried out in a swimming pool, as described in Figure 16. The maximum length is 50

The path-following and collision avoidance were carried out using a scaled marine surface ship model. Here, only one obstacle was considered due to the limited geometry dimension of the swimming pool. The obstacle was assumed to be located in the middle of the swimming pool. The ship will travel from the northeast to the southwest corner of the swimming pool. Before tests, the battery was charged fully, and the draft of the ship

**Figure 15.** Rudder angle, cross-track errors: (**a**) multi round obstacle; and (**b**) multi rectangular

**Figure 16.** Location of the collision avoidance test. **Figure 16.** Location of the collision avoidance test.

The static obstacle is located at *p*<sup>0</sup> = [20, −10], as indicated in black colour. The diameter is 6 m and the deviations are set as *σ<sup>x</sup>* = *σ<sup>y</sup>* = 4 ∗ *Lpp*. Figure 17 presents the trajectories of the ship model in the test. Considering the safety, it is better to set a larger variance of the repelling function, because of the environmental disturbance, for example, the wind and wave are large for a scaled ship model. As shown in the Figure, the collision avoidance method can control the underactuated ship model to avoid the collision. Figure 18 shows the heading angle and rudder angle during the test. The initial heading angle is set to zero. From the figure, the heading angle approaches zero when the ship passes the obstacle. The static obstacle is located at *p*<sup>0</sup> = − [20, 10] , as indicated in black colour. The diameter is 6 m and the deviations are set as 4\* σ σ *<sup>x</sup>* = =*y pp L* . Figure 17 presents the trajectories of the ship model in the test. Considering the safety, it is better to set a larger variance of the repelling function, because of the environmental disturbance, for example, the wind and wave are large for a scaled ship model. As shown in the Figure, the collision avoidance method can control the underactuated ship model to avoid the collision. Figure 18 shows the heading angle and rudder angle during the test. The initial heading angle is set to zero. From the figure, the heading angle approaches zero when the ship passes the obstacle.

**Figure 17.** The path following and collision avoidance test using a free-running ship model. **Figure 17.** The path following and collision avoidance test using a free-running ship model.

Heading angle, ψ

Rudder angle, δ

0 20 40 60 80 100 120

0 20 40 60 80 100 120

Time (sec)

Time (s)

**Figure 18.** Heading angle and rudder angle during the test.


Rudder angle [deg]

Yaw [deg]

**Figure 17.** The path following and collision avoidance test using a free-running ship model.

0 5 10 15 20 25 30 35 40 45

X (m)

**Figure 18.** Heading angle and rudder angle during the test. **Figure 18.** Heading angle and rudder angle during the test.

**Figure 16.** Location of the collision avoidance test.

obstacle.






Y (m)


0

5

10

eter is 6 m and the deviations are set as 4\*

The static obstacle is located at *p*<sup>0</sup> = − [20, 10] , as indicated in black colour. The diam-

of the ship model in the test. Considering the safety, it is better to set a larger variance of the repelling function, because of the environmental disturbance, for example, the wind and wave are large for a scaled ship model. As shown in the Figure, the collision avoidance method can control the underactuated ship model to avoid the collision. Figure 18 shows the heading angle and rudder angle during the test. The initial heading angle is set to zero. From the figure, the heading angle approaches zero when the ship passes the

*<sup>x</sup>* = =*y pp L* . Figure 17 presents the trajectories

Collision space Obtacles Land

Swimming pool Desired path Trajectory of ship

σ σ

#### **6. Conclusions**

This paper proposes an integrated path-following and risk-based obstacle collision avoidance system using the vector field method for underactuated autonomous surface vehicles. It is different from the obstacle avoidance methods in most published papers, which usually treat the path-following and obstacle collision avoidance separately. This paper considers the coupled path following and collision avoidance task. Meanwhile, the stability of the guidance and control system was analysed using the Lyapunov stability theory. For the control system, a sliding mode control was designed based on a nonlinear steering model. To obtain the steering model, the manoeuvring tests were carried out using a free-running ship model, and the collected data were used to estimate the values of the parameters using the LS-SVM. The equilibrium point of the heading error dynamic equations is GES. For the guidance system, the vector field guidance law was used and the stability proof of USGES was given. To avoid the obstacle near the path, the proposed guidance law was extended using a repelling function. The resulting heading angle can control the ship's travel away from the obstacles, which meanwhile, is converging to the predefined path when the collision risk is zero. Simulations and model tests were carried out to test the integrated system. From the simulation tests, it can be concluded that the collision risk plays an important role in the system. It can avoid the overshoot in collision avoidance, and the resulting trajectory of the underactuated ship model is more practical. Considering the expensive cost of testing on a full-scale ship, the proposed system was only validated using a scaled ship model. The test shows that the ship model can follow the predefined path and avoid colliding with the obstacles. In the future, the parameters of the proposed system can be optimized based on the specified task or environmental disturbance. More model tests in large areas with more complex environmental disturbance, such as dynamic obstacle, wind, waves, can be carried out for validation.

**Author Contributions:** Conceptualization, H.X. and C.G.S.; methodology, H.X. and C.G.S.; software, H.X. and M.A.H.; validation, H.X. and M.A.H.; formal analysis, H.X.; investigation, H.X.; resources, H.X.; data curation, H.X. and M.A.H.; writing—original draft preparation, H.X.; writing—review and editing, H.X., M.A.H. and C.G.S.; visualization, H.X.; supervision, C.G.S.; project administration, C.G.S.; funding acquisition, C.G.S. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was supported by the Portuguese Foundation for Science and Technology (Fundação para a Ciência e Tecnologia–FCT) under grants for the ROUTING research project (MARTERA-1/ROUTING/3/2018) in the ERA-NET COFUND MarTERA-1 programme (2018–2021). This work was performed within the Strategic Research Plan of the Centre for Marine Technology and Ocean Engineering, financed by the Portuguese Foundation for Science and Technology (Fundação para a Ciência e Tecnologia-FCT) under contract UIDB/UIDP/00134/2020.

**Institutional Review Board Statement:** This study does not involve humans or animals.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Not applicable.

**Conflicts of Interest:** The authors declare that they have no conflict of interest.

#### **References**


**Jiucai Jin 1,2,\* , Deqing Liu 1,2, Dong Wang 1,2 and Yi Ma 1,2**

	- **\*** Correspondence: jinjiucai@fio.org.cn; Tel.: +86-159-5338-0245

**Abstract:** Trajectory tracking is a basis of motion control for Unmanned Surface Vehicles (USVs), which has been researched well for common USVs. The twin-propeller and twin-hull USV (TPTH-USV) is a special vehicle for applications due to its good stability and high load. We propose a three-layered architecture of trajectory tracking for the TPTH-USV which explicitly decomposes into trajectory guidance, a motion limitator and controller. The trajectory guidance transforms an expected trajectory into an expected speed and expected course in a kinematic layer. The motion limitator describes some restriction for motion features of the USV in the restriction layer, such as the maximum speed and maximum yaw rate. The controller is to control the speed and course of the USV in the kinetic layer. In the first layer, an adaptive line-of-sight guidance law is designed by regulating the speed and course to track a curved line considering the sideslip angle. In the second layer, the motion features are extracted from an identified speed and course coupled model. In the last layer, the course and speed controller are designed based on a twin-PID controller. The feasibility and practicability of the proposed trajectory tracking scheme is validated in sea experiments by a USV called 'Jiuhang 490'.

**Keywords:** trajectory tracking; unmanned surface vehicle; model identification; line-of-sight

## **1. Introduction**

An Unmanned Surface Vehicle (USV) is a novel kind of multifunctional surface platform, which has been applied in many oceanic fields in recent years, such as ocean surveying, hydrology measurements, underwater acoustic communication, target tracking [1–3], etc. The motion control of USVs is a basic and essential part for autonomous operation, which has usually been inspired by conventional vehicles' control. In general, there are three issues in the motion control of vehicles, which contain point stabilization, path following and trajectory tracking. Point stabilization is used to stabilize the vehicle around an expected position, and path following is used to follow a predefined path for the vehicle, while trajectory tracking is used to track a predefined path with a time constraint. Path following and trajectory tracking have, recently, received considerable attention from the control communities, and many control methods have been applied, such as PID, fuzzy, backstepping, sliding mode control, evolutionary algorithms [4–6], etc.

The trajectory tracking of USVs can be departed into two categories, which are called direct and indirect control [7], and the first one is that the control issues are deemed as the zeroing of position errors, and the other is that the control issues are decomposed into guidance in the kinematic level and control in the kinetic level. In the direct control, the trajectory tracking is seen as a whole issue, and the stabilization control for tracking errors is designed based on a dynamic model of the USV, and lots of theories and methods have been developed [8–10]. Many control laws have been designed based on backstepping technology, and the stabilization is usually given out perfectly. However, the direct control

**Citation:** Jin, J.; Liu, D.; Wang, D.; Ma, Y. A Practical Trajectory Tracking Scheme for a Twin-Propeller Twin-Hull Unmanned Surface Vehicle. *J. Mar. Sci. Eng.* **2021**, *9*, 1070. https://doi.org/10.3390/jmse9101070

Academic Editor: Alessandro Ridolfi

Received: 3 September 2021 Accepted: 20 September 2021 Published: 30 September 2021

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

emerges mainly in theoretical research and is not convenient to be applied in the actual USVs due to their complexity [11].

#### *1.1. Related Works*

In the indirect control, the control issue is decomposed into guidance in the kinematic level and control in the kinetic level. In the kinematic level, the guidance law is designed by the speed and course control variables, while the speed and course control variables are deemed as expected values in the kinetic level. The kinematic control is equivalent to a work space control [12,13], where the work space (also known as the operational space) represents the physical space (environment) in which a vehicle moves. The kinematic level considers the geometrical aspects of motions purely, without reference to the forces and moments that generate such motions. The kinetic controllers consider how forces and moments generate the vehicle's motion, which are typically designed based on modelbased methods.

Since the indirect control has an obvious physical meaning in path following and trajectory tracking, lots of works have been published and applied. The course and speed control for USVs is usually seen as the basic controller for indirect control, which has been researched broadly [14–16]. The line-of-sight guidance law is used broadly in a ship's trajectory tracking [17,18], and a time-varying look-ahead distance and integral LOS technology has been developed [19], which is used to solve the sideslip angle problem. Lots of LOS technologies have been applied in USVs' kinematic control [20,21]. A trajectory tracking controller for an underactuated USV with multiple uncertainties and input constraints has been designed based on indirect control, and the design process of the controller is simplified and easy to implement due to the guidance law in the kinematic level [7]. Defining a set of guidance laws at the kinematic level for an underactuated USV in a twodimensional space, a nonlinear Lyapunov-based control law has been designed to yield the convergence of the path-following error coordinates to zero [11]. A modified LOS guidance algorithm has been proposed for the path following control of the underactuated USV, which can adaptively change the guidance law to respond to the longitudinal and lateral path following error [22]. Moreover, many algorithms have been derived by combining the traditional LOS technology and nonlinear control methods [23,24]. In addition, some novelty methods have been applied in the guidance law, such as bioinspired neural [25], deep reinforcement learning methods [26] and vector field [27]. The twin-propeller and twin-hull USV (TPTH-USV) is a usual vehicle for applications due to its good stability and high load [28], such as 'Springer' [29], 'JiuHang-490' [30].

Although many schemes of the trajectory tracking have been developed in the above works, most of the control laws cannot be directly or easily applied in universal USVs, and there are three reasons in view of practicability. Firstly, the control laws are too complicated to be used in actual engineering, also due to their high calculate costs. Secondly, the engineers could not understand the control laws well due to the complexity of the algorithms, and it is difficult to transfer the algorithms to executable procedures. Thirdly, most of the control laws are based on the dynamic models which are usually simplified for the actual systems, so the parameters and application condition of the controllers may not be suitable for common USVs. In summary, the control laws are usually designed for different vehicles and systems, and the bad-transplantation of the controllers appears in actual engineering due to their strong pertinence. In order to improve the disadvantages of the above trajectory tracking control, such as bad transplantation, compatibility for trajectory tracking and path following, a three-layered architecture of trajectory tracking for TPTH-USVs is proposed, and it is nearly suitable for the type of TPTH-USVs. The proposed scheme focuses on the design of guidance law for curved lines, and it is suitable for trajectory tracking and path following simultaneously by considering the speed variable.

#### *1.2. Scheme Design and Paper Structure 1.2. Scheme Design and Paper Structure*  Considering the above advantages and disadvantages of the indirect control, a

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 3 of 18

Considering the above advantages and disadvantages of the indirect control, a threelayered architecture scheme for trajectory tracking for the TPTH-USV is designed which contains the kinematic layer, restriction layer and kinetic layer, which are shown in Figure 1: three-layered architecture scheme for trajectory tracking for the TPTH-USV is designed which contains the kinematic layer, restriction layer and kinetic layer, which are shown in Figure 1:


**Figure 1.** The two categories of the trajectory tracking and the proposed three-layered architecture scheme for trajectory tracking of the USV. **Figure 1.** The two categories of the trajectory tracking and the proposed three-layered architecture scheme for trajectory tracking of the USV.

The advantages of the proposed algorithm can be illustrated as four aspects: The advantages of the proposed algorithm can be illustrated as four aspects:


This paper is organized as follows. Section 2 describes the proposed three-layered architecture scheme for trajectory tracking and our TPTH USV called 'Jiuhang 490′ USV. Section 3 gives out the implement of the proposed scheme in the three layers. The results of the sea experiments are shown in Section 4 and the conclusions are given in Section 5. This paper is organized as follows. Section 2 describes the proposed three-layered architecture scheme for trajectory tracking and our TPTH USV called 'Jiuhang 490' USV. Section 3 gives out the implement of the proposed scheme in the three layers. The results of the sea experiments are shown in Section 4 and the conclusions are given in Section 5.

#### **2. Three-Layered Architecture Scheme for Trajectory Tracking and 'Jiuhang 490′ USV 2. Three-Layered Architecture Scheme for Trajectory Tracking and 'Jiuhang 490' USV**

*2.1. Three-Layered Architecture Scheme 2.1. Three-Layered Architecture Scheme*

According to the above three-layer architecture, the trajectory tracking for USVs could be explicitly divided into the trajectory guidance, motion limitator and controller. In the trajectory guidance, an improved LOS was proposed based on an adaptive look-ahead distance which would give the system the desired course and the speed of According to the above three-layer architecture, the trajectory tracking for USVs could be explicitly divided into the trajectory guidance, motion limitator and controller. In the trajectory guidance, an improved LOS was proposed based on an adaptive look-ahead distance which would give the system the desired course and the speed of the USV. In the model limitator, the coupled speed and yaw motion limitator of the USV was acquired based an identification model of the 'Jiuhang490' USV. In the controller, the twin-PID

controller was designed for the course and speed control. The proposed practical trajectory tracking's flow diagram under the three-layered architecture is shown in Figure 2. practical trajectory tracking's flow diagram under the three-layered architecture is shown in Figure 2. shown in Figure 2.

**Speed and yaw coupled motion limitator**

the USV. In the model limitator, the coupled speed and yaw motion limitator of the USV was acquired based an identification model of the 'Jiuhang490' USV. In the controller, the twin-PID controller was designed for the course and speed control. The proposed

the USV. In the model limitator, the coupled speed and yaw motion limitator of the USV was acquired based an identification model of the 'Jiuhang490' USV. In the controller, the twin-PID controller was designed for the course and speed control. The proposed practical trajectory tracking's flow diagram under the three-layered architecture is

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 4 of 18

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 4 of 18

**ALOS trajectory guidance law**

**Figure 2.** The proposed practical trajectory tracking of the USV under three-layered architecture. **Figure 2.** The proposed practical trajectory tracking of the USV under three-layered architecture.

#### *2.2. 'Jiuhang 490′ USV 2.2. 'Jiuhang 490' USV 2.2. 'Jiuhang 490′ USV*

A TPTH USV called 'Jiuhang 490' was developed in the First Institute of Oceanology, Ministry of Natural Resources in China, in 2017, which is shown in Figure 3. The USV was applied for the offshore emergent observation of nuclear radiation and route monitoring of thermal discharge for national nuclear power stations in our project. The 'Jiuhang 490′ USV was 4.9 m in length, 2.5 m in width, 500 kg in weight, the maximal speed was about 5.5 kn, the endurance of the voyage was about 60~80 km and the maximal communication distance was about 10 km. In order to lower the gravity center of the vehicle and to enhance the stability of navigating, the lithium batteries and main control unit was embedded in the fiberglass hulls of the catamaran. Two propellers was used for the stern propulsion, which was controlled by two brushless DC motor actuators separately. Based on an embedded microcomputer, the aboard main control unit was integrated, and a Honeywell HMR3000 digital compass and a Hemisphere VS330 GNSS (Global Navigation Satellite System) compass were adopted for the attitude and position measurements respectively. The planner and controller for trajectory tracking ran in the main control unit. The other integrated sensors contained a gamma detector for nuclear radiation observation, a CTD (Conductivity, Temperature and Depth), a A TPTH USV called 'Jiuhang 490' was developed in the First Institute of Oceanology, Ministry of Natural Resources in China, in 2017, which is shown in Figure 3. The USV was applied for the offshore emergent observation of nuclear radiation and route monitoring of thermal discharge for national nuclear power stations in our project. The 'Jiuhang 490' USV was 4.9 m in length, 2.5 m in width, 500 kg in weight, the maximal speed was about 5.5 kn, the endurance of the voyage was about 60~80 km and the maximal communication distance was about 10 km. In order to lower the gravity center of the vehicle and to enhance the stability of navigating, the lithium batteries and main control unit was embedded in the fiberglass hulls of the catamaran. Two propellers was used for the stern propulsion, which was controlled by two brushless DC motor actuators separately. Based on an embedded microcomputer, the aboard main control unit was integrated, and a Honeywell HMR3000 digital compass and a Hemisphere VS330 GNSS (Global Navigation Satellite System) compass were adopted for the attitude and position measurements respectively. The planner and controller for trajectory tracking ran in the main control unit. The other integrated sensors contained a gamma detector for nuclear radiation observation, a CTD (Conductivity, Temperature and Depth), a camera and an ultrasonic weather station. More details can be seen in [30]. A TPTH USV called 'Jiuhang 490' was developed in the First Institute of Oceanology, Ministry of Natural Resources in China, in 2017, which is shown in Figure 3. The USV was applied for the offshore emergent observation of nuclear radiation and route monitoring of thermal discharge for national nuclear power stations in our project. The 'Jiuhang 490′ USV was 4.9 m in length, 2.5 m in width, 500 kg in weight, the maximal speed was about 5.5 kn, the endurance of the voyage was about 60~80 km and the maximal communication distance was about 10 km. In order to lower the gravity center of the vehicle and to enhance the stability of navigating, the lithium batteries and main control unit was embedded in the fiberglass hulls of the catamaran. Two propellers was used for the stern propulsion, which was controlled by two brushless DC motor actuators separately. Based on an embedded microcomputer, the aboard main control unit was integrated, and a Honeywell HMR3000 digital compass and a Hemisphere VS330 GNSS (Global Navigation Satellite System) compass were adopted for the attitude and position measurements respectively. The planner and controller for trajectory tracking ran in the main control unit. The other integrated sensors contained a gamma detector for nuclear radiation observation, a CTD (Conductivity, Temperature and Depth), a camera and an ultrasonic weather station. More details can be seen in [30].

camera and an ultrasonic weather station. More details can be seen in [30].

**Figure 3.** The 'Jiu Hang 490' USV and sensors.

#### **3. Implement of Trajectory Tracking**

#### *3.1. Assumptions*

To simplify the problem, the motion of the USV in the horizontal plane was considered in this paper. Some assumptions were given out as follows [31]:


#### *3.2. Trajectory Guidance Law for Curved Line*

In the kinematics layer, the trajectory guidance law for the USV was designed to steer the course and to regulate the speed, which could force the USV to follow the expected trajectory with the temporal constraint, i.e., the tracking position errors *x*error and *y*error had to tend to zero in desired moments. In the section, an adaptive look-ahead-based LOS (ALOS) guidance law was designed for the guidance law of curved lines, which would give the expected speed and course for the kinetic layer.

The guidance law for USVs is usually expressed in the body-fixed reference frame o-XbY<sup>b</sup> {*BF*} and the north-east reference frame O-NE {*NE*}. The look-ahead-based LOS guidance algorithm has usually been used for straight-line path following. In the path following for curved paths without a temporal constraint, there are two solutions, i.e., the first one is that the curved line is divided into some straight-lines, and the other is to minimize the cross-track error in the Serret–Frenet reference frame. The origin of the Serret–Frenet reference frame was set at the position for the shortest distance between the vehicle and the expected curved path. However, the situation was different for the trajectory tracking; for example, the expected waypoint at the certain moment was not coincident with the shortest point between the curved line and the vehicle. Therefore, a reference frame called the Expected Trajectory reference frame {*ET*} was proposed with the origin at the expected waypoint (*x*k, *y*k) at the k-th moment, where its Yet axis was along the tangential direction for the expected trajectory, and the Xet axis was the normal direction. Therefore, it was convenient to calculate the tracking errors *x*<sup>e</sup> and *y*<sup>e</sup> between the USV and the expected trajectory in the reference frame {*ET*}. The reference frame {*ET*} was different from the Serret–Frenet reference frame, where the origin of {*ET*} fixed at the expected waypoint with a temporal constraint and the origin of the Serret–Frenet reference frame changed with the trajectory. The three reference frames and the relationship diagram of trajectory tracking are shown in Figure 4. The points (*x*k−<sup>1</sup> ,*y*k−<sup>1</sup> ), (*x*k, *y*k) and (*x*k+1, *y*k+1) are the three successive expected waypoints of the trajectory at the moment of k − 1, k and k + 1. The point (*x*los, *y*los) is a virtual expected point calculated by the ALOS algorithm.

#### 3.2.1. Selection of Expected Waypoints

Since a curved trajectory tracking is not different from a straight line tracking, how to select the expected waypoints on the trajectory is an essential step. In order to decrease the calculate cost, there is no need for guidance in every moment in the actual engineering, and the selection of expected waypoints (*x*k, *y*k) depends on the precision demand of trajectory tracking. The expected curved trajectory is defined as follows,

$$\begin{aligned} \mathfrak{x} &= \mathfrak{x}(t), \\ \mathfrak{y} &= \mathfrak{y}(t) \end{aligned} \tag{1}$$

**Figure 4.** The reference frames of USV's trajectory tracking.

**Figure 4.** The reference frames of USV's trajectory tracking. 3.2.1. Selection of Expected Waypoints The first point of the expected trajectory was set as the first expected waypoint (*x*0, *y*0) = (*x*(1), *y*(1)), and the subsequent expected waypoints were selected as follows,

$$(\mathbf{x}\_{\mathbf{k}\prime} y\_{\mathbf{k}}) = (\mathbf{x}(t), y(t)), \text{ if } \mathbf{R}(t) < \mathbf{R}\_0 \text{ or } \dot{\mathbf{U}}(t) > \mathbf{U}\_{0\prime} \tag{2}$$

crease the calculate cost, there is no need for guidance in every moment in the actual engineering, and the selection of expected waypoints (*x*k,*y*k) depends on the precision demand of trajectory tracking. The expected curved trajectory is defined as follows, where *R*(*t*) is the radius of the curvature for the curved trajectory, *R*(*t*) = . *x* 2+ . *y* 2 3/2 . *x*· .. *y*− .. *x*· . *y* and *U*(*t*) = q *u*(*t*) <sup>2</sup> + *v*(*t*) 2 , and *u*(*t*) = . *x*(*t*), *v*(*t*) = . *y*(*t*). The selected rule of the expected

=(), =() (1) The first point of the expected trajectory was set as the first expected waypoint waypoints was simple, i.e., when the radius of curvature is smaller than a threshold R<sup>0</sup> or the derivative of the expected speed is larger than a threshold U0. The rule assures that the more mutations of the curved trajectory occurring in the space and moment, the more expected waypoints generate.

#### (*x*0,*y*0) = (*x*(1),*y*(1)), and the subsequent expected waypoints were selected as follows, 3.2.2. Adaptive LOS Law

(୩, ୩) = ൫(), ()൯, if () < R or ሶ() > U, (2) where *R*(*t*) is the radius of the curvature for the curved trajectory, () = (ೣሶ మశሶ మ)య/మ ೣ∙ሶ ሷషೣሷ∙ሶ and () = ඥ()ଶ + ()ଶ, and () = ሶ(), () = ሶ(). The selected rule of the expected According to the transformation relationship between the reference frame {*ET*} and the reference frame {*NE*} in Figure 4, the tracking errors between the USV and the expected curves in {*ET*} are as follows,

$$
\begin{bmatrix} \mathbf{x\_e} \\ \mathbf{y\_e} \end{bmatrix} = \mathbf{R}^T (\gamma\_\mathbf{k}) \begin{bmatrix} \mathbf{x} - \mathbf{x\_k} \\ \mathbf{y} - \mathbf{y\_k} \end{bmatrix}' \tag{3}
$$

more expected waypoints generate. 3.2.2. Adaptive LOS Law According to the transformation relationship between the reference frame {*ET*} and where the transformation matrix R(*γ*k) = cos *γ*<sup>k</sup> − sin *γ*<sup>k</sup> sin *γ*<sup>k</sup> cos *γ*<sup>k</sup> , *γ*<sup>k</sup> = atan2 *y* 0 k (*θ*), *x* 0 k (*θ*) ∈ [−π, π] is the rotated angle between {*NE*} and {*ET*}, *x*<sup>e</sup> and *y*<sup>e</sup> are the errors of the current position(*x*, *y*) and expected waypoint(*x*k, *y*k) in the reference frame {*ET*}.

the reference frame {*NE*} in Figure 4, the tracking errors between the USV and the expected curves in {*ET*} are as follows, The guidance law of the trajectory tracking was to calculate the expected speed *U*<sup>d</sup> and expected course *χ*d, which could be designed as according to the conventional LOS guidance algorithm [19],

[−π, π] is the rotated angle between {*NE*} and {*ET*}, ୣ and ୣ are the errors of the cur-

sin୩ cos୩

 $\mathbf{g}\_{\text{numunc}}$  иврсили  $\left(1\,\text{V}\right)\_{\text{M}}$   $\chi\_{\text{d}} = \gamma\_{\text{k}} + \arctan\left(-\frac{\mathcal{Y}\_{\text{e}}}{\Delta}\right). \tag{4}$ 

൨, ୩ = atan2൫୩

<sup>ᇱ</sup> (), ୩

<sup>ᇱ</sup> ()൯ ∈

rent position(*x*,*y*) and expected waypoint(*x*k,*y*k) in the reference frame{*ET*}.

In the USV's running or turning in the environment disturbance, such as a wave, there exists a sideslip angle *β* between the heading *χ* and the course *ψ* of the USV in Figure 4. Therefore, the expected heading of the USV for the expected trajectory is as follows,

$$
\psi\_{\rm d} = \chi\_{\rm d} - \beta = \gamma\_{\rm k} + \arctan\left(-\frac{y\_{\rm e}}{\Delta}\right) - \beta\_{\prime} \tag{5}
$$

where ∆ is the look-ahead distance, and the sideslip angle *β* = *χ* − *ψ* = atan2(*v*, *u*).

In order to improve the tracking performance, the adaptive look-ahead distance ∆ was designed as follows,

$$
\Delta = \mathbf{m}(1 + 1/|y\_{\mathbf{e}}|) \mathbf{L} \tag{6}
$$

where m is a gain constant, and L is the length of the USV. It is obvious that when *y*<sup>e</sup> is very small, ∆ is very large. According to the stability proof [18], the larger ∆ is, the more limited the region where the system ULES (Uniform Local Exponential Stability) is becomes. Therefore, ∆ should be restricted when the vehicle is close to the path, and the moderated ∆ was set as n·L, where n is larger than m. If ∆ > n·L, then ∆ = n·L.

In the aspect of the expected speed, it was designed as follows [7],

$$\mathcal{U}\_{\rm d} = \frac{(\mathcal{U} - \mathcal{P} \cdot \mathbf{x}\_{\rm e})\sqrt{y\_{\rm e}^2 + \Delta^2}}{\Delta},\tag{7}$$

where P is the control gain and U is the cruising speed for the USV. In the restriction level, the speed *U*<sup>d</sup> should be reasonable, so it is moderated, i.e., if *U*<sup>d</sup> < Umin, *U*<sup>d</sup> = Umin, and if *U*<sup>d</sup> > Umax, *U*<sup>d</sup> = Umax.

The equilibrium points of the cross-track were proven to be globally k exponentially stable [7,18]. It was obvious that the expected speed was a proportional controller in Equation (7), so we adopted a PD controller for the speed term as follows,

$$\mathcal{U}\_{\rm d} = \frac{\left(\mathbf{U} - \left(\mathbf{P} \cdot \mathbf{x}\_{\rm e}(\mathbf{t} + 1) + \mathbf{D} \cdot \left(\mathbf{x}\_{\rm e}(\mathbf{t} + 1) - \mathbf{x}\_{\rm e}(\mathbf{t})\right)\right)\right) \sqrt{y\_{\rm e}^2 + \Delta^2}}{\Delta}. \tag{8}$$

#### *3.3. Motion Limitator*

Since the motion of the USV was considered in the horizontal plane, the speed and yaw rate restrictions were used in the motion limitator corresponding to the two outputs of the trajectory guidance law based on an identified motion model of the USV.

#### 3.3.1. Motion Model

The USV's motion model can be described in a plane by three-degrees-of-freedom equations, i.e., the surge, sway and yaw. The transformation relationships between positions and velocities were expressed as follows,

$$\begin{array}{l} \dot{x} = u \cdot \cos(\psi) - v \cdot \sin(\psi) \\ \dot{y} = u \cdot \sin(\psi) + v \cdot \cos(\psi), \\ \dot{\psi} = r \end{array} \tag{9}$$

where *x*, *y*, and *ψ* represent the position and orientation in {*NE*}, and *u*, *v* and *r* represent the surge speed, sway speed and yaw rate, respectively, in {*BF*}.

A general dynamic model was adopted as follows [17],

$$\mathbf{M}\dot{\boldsymbol{v}} + \mathbf{C}(\boldsymbol{v})\boldsymbol{v} + \mathbf{D}(\boldsymbol{v})\boldsymbol{v} = \boldsymbol{\tau},\tag{10}$$

where M represents the inertia matrix, C represents the Coriolis and centripetal matrix, D represents the hydrodynamic drag matrix, and *v* represents the linear and angular velocity vectors, *τ* represents the driven force and the moment of the thrusters. The

above hydrodynamic matrices were given as follows: M = m<sup>11</sup> 0 0 0 m<sup>22</sup> 0 0 0 m<sup>33</sup> , C(*v*) = 0 0 −m22*v* 0 0 m11*u* m22*v* −m11*u* 0 , and D(v) = d<sup>11</sup> 0 0 0 d<sup>22</sup> 0 0 0 d<sup>33</sup> = X<sup>u</sup> 0 0 0 Y<sup>v</sup> 0 0 0 N<sup>r</sup> . Therefore, the dynamic model of the USV could be described by, where M represents the inertia matrix, C represents the Coriolis and centripetal matrix, D represents the hydrodynamic drag matrix, and *v* represents the linear and angular velocity vectors, *τ* represents the driven force and the moment of the thrusters. The above hydrodynamic matrices were given as follows: M=mଵଵ 0 0 0 mଶଶ 0 0 0mଷଷ ൩, C() = 0 0 −mଶଶ 0 0mଵଵ mଶଶ −mଵଵ 0 <sup>൩</sup>, and D(v) = dଵଵ 0 0 0 dଶଶ 0 ൩=X୳ 0 0 0 Y୴ 0 ൩.

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 8 of 18

$$\text{Surface}: \ i = \frac{\text{m}\_{22}}{\text{m}\_{11}} v \cdot r - \frac{\text{d}\_{11}}{\text{m}\_{11}} u + \frac{1}{\text{m}\_{11}} \tau\_{1\prime} \tag{11}$$

$$\text{Sway}: \dot{v} = -\frac{\mathbf{m}\_{11}}{\mathbf{m}\_{22}} u \cdot r - \frac{\mathbf{d}\_{22}}{\mathbf{m}\_{22}} v \,\tag{12}$$

Mሶ + C()+D() = , (10)

$$\text{Yaw}: \,\ddot{\psi} = \frac{\mathbf{m}\_{11} - \mathbf{m}\_{22}}{\mathbf{m}\_{33}} \boldsymbol{\mu} \cdot \boldsymbol{v} - \frac{\mathbf{d}\_{33}}{\mathbf{m}\_{33}} \dot{\psi} + \frac{1}{\mathbf{m}\_{33}} \mathbf{r}\_{3\prime} \tag{13}$$

where m11, m<sup>22</sup> and m<sup>33</sup> represent the inertia mass, d11, d<sup>22</sup> and d<sup>33</sup> represent the drag coefficients, *τ*<sup>1</sup> and *τ*<sup>2</sup> represent the thrusts in the X<sup>b</sup> and Y<sup>b</sup> axes, respectively, and *τ*<sup>3</sup> represents the thrust moment. It was noted that the value of *τ*<sup>2</sup> for the TPTH USV equaled to zero, since there was not a propeller or a rudder for the USV in the Y<sup>b</sup> axis. where m11, m22 and m33 represent the inertia mass, d11, d22 and d33 represent the drag coefficients, *τ*1 and *τ*2 represent the thrusts in the Xb and Yb axes, respectively, and *τ*3 represents the thrust moment. It was noted that the value of *τ*2 for the TPTH USV equaled to zero, since there was not a propeller or a rudder for the USV in the Yb axis.

Since the speed *u* and yaw rate *r* were the main factors in the model limitator, the model for the surge and yaw motion were identified based on Equations (11) and (13) using the data from a lake trial of the 'Jiuhang 490' USV (Figure 5) on 14–18 September 2017 at a lake in Qingdao city, Shandong Province, China. It is noted that the yaw model was coupled with the speed of the TPTH USV. Since the speed *u* and yaw rate *r* were the main factors in the model limitator, the model for the surge and yaw motion were identified based on Equations (11) and (13) using the data from a lake trial of the 'Jiuhang 490′ USV(Figure 5) on 14–18 September 2017 at a lake in Qingdao city, Shandong Province, China. It is noted that the yaw model was coupled with the speed of the TPTH USV.

**Figure 5.** The 'JiuHang 490' USV's lake trial in 2017. **Figure 5.** The 'JiuHang 490' USV's lake trial in 2017.

3.3.2. Model Identification for Surge Motion 3.3.2. Model Identification for Surge Motion

The model identification for the surge motion of the USV could be acquired by using the steady state data for a straight-line based on Equation (11), The model identification for the surge motion of the USV could be acquired by using the steady state data for a straight-line based on Equation (11),

$$
\mu \mathbf{m}\_{11} \cdot \dot{\boldsymbol{\mu}} + \boldsymbol{\chi}\_{\mathbf{u}} \cdot \boldsymbol{\mu} = \boldsymbol{\tau}\_{1\prime} \tag{14}
$$

where ଵ = ଵ + ଶ,ଵ and ଶ are the thrusts of the left and right propellers, respectively, which ware shown in Figure 6. The relationship between the thrust ଵ and the basis control variable ୳ was fitted linearly by the data from the lake trial which is shown in Figure 7. In Figure 7, the circle represents the measure data, and the solid line represents the linear fitting result. The differential thrust mode was chosen for the TPTH USV as follows, where *τ*<sup>1</sup> = *F*<sup>1</sup> + *F*2, *F*<sup>1</sup> and *F*<sup>2</sup> are the thrusts of the left and right propellers, respectively, which ware shown in Figure 6. The relationship between the thrust *τ*<sup>1</sup> and the basis control variable *C*<sup>u</sup> was fitted linearly by the data from the lake trial which is shown in Figure 7. In Figure 7, the circle represents the measure data, and the solid line represents the linear fitting result. The differential thrust mode was chosen for the TPTH USV as follows,

$$F\_1 = \mathbf{k}\_\mathbf{u} \cdot (\mathbf{C}\_\mathbf{u} + \mathbf{C}\_\mathbf{h})\_\prime \tag{15}$$

$$F\_2 = \mathbf{k}\_\mathbf{u} \cdot (\mathbf{C}\_\mathbf{u} - \mathbf{C}\_\mathbf{h}) \,. \tag{16}$$

ଵ = k୳ ∙ (୳ + ୦), (15)

ଵ = k୳ ∙ (୳ + ୦), (15)

where *C*<sup>u</sup> is the basis control variable and *C*<sup>h</sup> is a differential control variable. Therefore, the linear model for the thrusts was as follows, where ୳ is the basis control variable and ୦ is a differential control variable. Therefore, the linear model for the thrusts was as follows, fore, the linear model for the thrusts was as follows, ଵ =2∙k୳ ∙ ୳, (17)

$$
\tau\_1 = 2 \cdot \mathbf{k}\_\mathbf{u} \cdot \mathbf{C}\_\mathbf{u} \tag{17}
$$

where k<sup>u</sup> is the thrust coefficient for a singular propeller and k<sup>u</sup> = 2.48 in Figure 7. In the lake trial, the thrust was measured by an ergometer, and the basis control variable *C*<sup>u</sup> ∈ [0, 200]. lake trial, the thrust was measured by an ergometer, and the basis control variable ୳ ∈ [0,200]. lake trial, the thrust was measured by an ergometer, and the basis control variable ୳ ∈ [0,200].

*v*

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 9 of 18

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 9 of 18

**Figure 6.** The diagram for speed and course regulation of the USV. **Figure 6.** The diagram for speed and course regulation of the USV. **Figure 6.** The diagram for speed and course regulation of the USV.

**Figure 7.** The relationship between thrust ଵ and the basis control variable ୳. **Figure 7.** The relationship between thrust ଵ and the basis control variable ୳. **Figure 7.** The relationship between thrust *τ*<sup>1</sup> and the basis control variable *C*u.

The transfer function for the speed *u* and the basis control variable ୳, were acquired by the Laplace transformation based on Equation (14), The transfer function for the speed *u* and the basis control variable ୳, were acquired by the Laplace transformation based on Equation (14), The transfer function for the speed *u* and the basis control variable *C*u, were acquired by the Laplace transformation based on Equation (14),

$$\mathbf{G}\_{\mathbf{u}}(\mathbf{s}) = \frac{\mathbf{u}(\mathbf{s})}{\mathbf{C}\_{\mathbf{u}}(\mathbf{s})} = \frac{2 \cdot \mathbf{k}\_{\mathbf{u}}}{\mathbf{m}\_{11}\mathbf{s} + \mathbf{X}\_{\mathbf{u}}}.\tag{18}$$

Let Kଵ <sup>=</sup> ଶ∙୩౫ ଡ଼౫ , Tଵ <sup>=</sup> ୫భభ ଡ଼౫ , so the transfer function became, Let Kଵ <sup>=</sup> ଶ∙୩౫ ଡ଼౫ , Tଵ <sup>=</sup> ୫భభ ଡ଼౫ , so the transfer function became, G୳(s) <sup>=</sup> భ Let K<sup>1</sup> = 2·k<sup>u</sup> Xu , T<sup>1</sup> = m<sup>11</sup> Xu , so the transfer function became,

$$\mathbf{G}\_{\mathbf{u}}(\mathbf{s}) = \frac{\mathbf{K}\_{\mathbf{l}}}{1 + \mathbf{T}\_{\mathbf{l}}\mathbf{s}} \tag{19}$$

According to the steady state data from the lake trial, the drag coefficient X<sup>u</sup> = *τ*1 *u* = 359.78. The inertial mass and added mass were estimated empirically by m<sup>11</sup> = m + 0.1·m = 550. Therefore, the transfer function for the speed *u* and the basis control variable *C*<sup>u</sup> were as follows,

$$\mathbf{G}\_{\mathbf{u}}(\mathbf{s}) = \frac{0.014}{1 + 1.53\mathbf{s}} \tag{20}$$

3.3.3. Model Identification for Yaw Motion

The yaw model of the USV could be simplified in the condition of the quasistatic course changing based on Equation (13),

$$
\mathbf{m}\_{33}\ddot{\boldsymbol{\psi}} = -\mathbf{d}\_{33}\dot{\boldsymbol{\psi}} + \mathbf{r}\_3. \tag{21}
$$

In the TPTH USV, the steering moment, *τ*<sup>3</sup> = (*F*<sup>1</sup> − *F*2)·d, and the arm of force d were the perpendicular distance between the propeller and the central line of the USV; then, the steering moment was simply expressed by:

$$
\pi\_3 = (F\_1 - F\_2) \cdot \mathbf{d} = \mathbf{k}\_{\mathbf{h}} \cdot \mathbf{C}\_{\mathbf{h}}.\tag{22}
$$

If the nonlinear feature of the propeller was not considered, the thrust of the propeller was simply expressed as:

$$F = \mathbf{k}\_1 \cdot \mathbf{n}^2 \,, \tag{23}$$

where *n* is the speed of the revolution for the propeller.

The relationship between the speed of revolution and drive voltage for the propeller was as follows,

$$\mathbf{T}\frac{\mathbf{d}n(t)}{\mathbf{d}t} + n(t) = \mathbf{k}\_2 \cdot v\_1(t). \tag{24}$$

Since the propellers for the USV were two small DC motors, the resistance of the armature and moment of inertia were very small, so the temporal parameter T could be neglected; then, the speed of the revolution was as follows,

$$m(t) = \mathbf{k}\_2 \cdot v\_1(t). \tag{25}$$

The relationship between the drive voltage and control voltage of the actuator was simply described,

$$
v\_1 = \mathbf{k}\_3 \cdot v\_2\tag{26}$$

and the left and right propellers' control voltages for the actuators were:

$$
v\_2 = \mathbf{k}\_{\mathbf{4}} \cdot (\mathbf{C}\_{\mathbf{u}} \pm \mathbf{C}\_{\mathbf{h}}).\tag{27}$$

According to the above relationships, the steering moment was:

$$\mathbf{r\_3 = k\_1 \left(n\_1^2 - n\_2^2\right)} \cdot \mathbf{d} = \mathbf{k\_0} \cdot \mathbf{C\_u} \cdot \mathbf{C\_h} \tag{28}$$

where the parameter k<sup>0</sup> = 4·k1·k 2 2 ·k 2 3 ·k 2 4 ·d.

Therefore, the steering moment depended not only on a differential variable *C*h, but also on the basis of the control variable *C*u, which was more coincidental with the actual situation than that in Equation (22).

In the actual course control, the speed control variable *C*<sup>u</sup> was usually fixed as a constant, so the course control became a single control input issue with a differential control variable *C*h, which was the same as Equation (22).

Substituting Equation (28) into Equation (21), the relationship between the course *ψ* and course control variables was,

$$
\mathbf{m}\_{33} \cdot \ddot{\boldsymbol{\psi}} = -\mathbf{d}\_{33} \cdot \dot{\boldsymbol{\psi}} + \mathbf{k}\_{0} \cdot \mathbf{C}\_{\mathbf{u}} \cdot \mathbf{C}\_{\mathbf{h}}.\tag{29}
$$

Let K<sup>2</sup> = k0·*C*<sup>u</sup> d<sup>33</sup> and T<sup>2</sup> = m<sup>33</sup> d<sup>33</sup> , and the transfer function for the course control was acquired by the Laplace transformation, ୢయయ ୢయయ acquired by the Laplace transformation,

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 11 of 18

and Tଶ <sup>=</sup> ୫యయ

Let Kଶ <sup>=</sup> ୩బ∙౫

$$\mathbf{G}\_{\rm h}(\mathbf{s}) = \frac{\boldsymbol{\Psi}(\mathbf{s})}{\mathbf{C}\_{\rm h}(\mathbf{s})} = \frac{\mathbf{K}\_{\rm 2}}{\mathbf{s}(1 + \mathbf{T}\_{2}\mathbf{s})}.\tag{30}$$

, and the transfer function for the course control was

Remark: The gain K<sup>2</sup> = k0·*C*<sup>u</sup> d<sup>33</sup> was proportional to motor coefficients k<sup>0</sup> and the speed control variable *C*u, but, inversely, proportional to the rotation drag coefficient d33. Therefore, the course control would be affected by the speed control variable. This could be simply understood, because the speed control would affect the course control of the TPTH USV. If the USV ran at a fixed speed, the equation with a fixed parameter could describe the yaw motion. Otherwise, if the USV ran by a variable speed, the course's variance ratio would be proportional to the speed of the USV. The relationship in Equation (30) is similar to the Nomoto model for a conventional ship's steering, and the difference is that the course control is the double-thrust, not a rudder in Nomoto model, and Equation (30) introduces the speed term for the TPTH USV. speed control variable ୳, but, inversely, proportional to the rotation drag coefficient dଷଷ. Therefore, the course control would be affected by the speed control variable. This could be simply understood, because the speed control would affect the course control of the TPTH USV. If the USV ran at a fixed speed, the equation with a fixed parameter could describe the yaw motion. Otherwise, if the USV ran by a variable speed, the course's variance ratio would be proportional to the speed of the USV. The relationship in Equation (30) is similar to the Nomoto model for a conventional ship's steering, and the difference is that the course control is the double-thrust, not a rudder in Nomoto model, and Equation (30) introduces the speed term for the TPTH USV.

It is obvious that Equation (30) is a transformation function with one pole and an integrator. Using the steering data in the lake trial (Figure 8), the transformation function for the course was identified by the System Identification Toolbox (MATLAB) with a gain coefficient K<sup>2</sup> = 0.14 and temporal coefficient T<sup>2</sup> = 0.77 as follows, It is obvious that Equation (30) is a transformation function with one pole and an integrator. Using the steering data in the lake trial (Figure 8), the transformation function for the course was identified by the System Identification Toolbox (MATLAB) with a gain coefficient Kଶ = 0.14 and temporal coefficient Tଶ = 0.77 as follows,

**Figure 8.** Course of the USV with control variable *C*u = 70 and differential control variable *C*h (0~50) (lake data in 2017). (**a**) Course and differential control variable; (**b**) course with control variable. **Figure 8.** Course *ψ* of the USV with control variable *C*<sup>u</sup> = 70 and differential control variable *C*<sup>h</sup> (0~50) (lake data in 2017). (**a**) Course and differential control variable; (**b**) course with control variable.

$$\mathbf{G}\_{\rm h}(\mathbf{s}) = \frac{0.14}{\mathbf{s}(1 + 0.77\mathbf{s})}.\tag{31}$$

Therefore, the transformation function of the yaw rate could simply be approximated by: Therefore, the transformation function of the yaw rate could simply be approximated by:

$$\mathbf{G}\_{\text{yaw}}(\mathbf{s}) = \frac{0.14}{1 + 0.77\mathbf{s}} = \mathbf{C}\_{\text{u}} \frac{0.002}{1 + 0.77\mathbf{s}}.\tag{32}$$

Equation (32) is a coupling transformation function of the yaw rate with the speed, which was different from the situation for the speed and course control separately. Even though in the situation under a fixed speed, the speed of the USV must change before the speed reaches the fixed value, so the yaw model had to be changed, and it would result in a bad control of course. Therefore, the motion limitator for the yaw rate of the USV can be evaluated by Equation (32), which is related to the speed of the USV. When the expected speed was given, the basic control variable could be evaluated. It was noted that the restriction of the yaw rate varied with the basic control variable for an expected speed. The basis control variable ୳ ∈ [0,200], so ୦ ∈ [0, 200 − ୳] with a re- Equation (32) is a coupling transformation function of the yaw rate with the speed, which was different from the situation for the speed and course control separately. Even though in the situation under a fixed speed, the speed of the USV must change before the speed reaches the fixed value, so the yaw model had to be changed, and it would result in a bad control of course. Therefore, the motion limitator for the yaw rate of the USV can be evaluated by Equation (32), which is related to the speed of the USV. When the expected speed was given, the basic control variable could be evaluated. It was noted that the restriction of the yaw rate varied with the basic control variable for an expected speed. The basis control variable *C*<sup>u</sup> ∈ [0, 200], so *C*<sup>h</sup> ∈ [0, 200 − *C*u] with a restriction condition

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 12 of 18

for *C*<sup>h</sup> = min(200 − *C*u, *C*u). Therefore, we could acquire the restriction of the yaw rate based Equation (32), which is shown in Figure 9. striction condition for ୦ = min (200 − ୳, ୳) . Therefore, we could acquire the restriction of the yaw rate based Equation (32), which is shown in Figure 9.

striction condition for ୦ = min (200 − ୳, ୳) . Therefore, we could acquire the re-

striction of the yaw rate based Equation (32), which is shown in Figure 9.

**Figure 9.** The restriction of the yaw rate for the USV. **Figure 9.** The restriction of the yaw rate for the USV. *3.4. Controllers* 

#### *3.4. Controllers* The course and speed regulation of the 'Jiuhang 490′ USV was achieved by the two

defined in Figure 6 by:

*3.4. Controllers*  The course and speed regulation of the 'Jiuhang 490′ USV was achieved by the two brushless DC motor actuators. The speed of the USV depended on the total thrust from the two propellers, and the course of the USV was adjusted by the thrust difference between the left and right propellers. It is seen that the left and right propellers' control is The course and speed regulation of the 'Jiuhang 490' USV was achieved by the two brushless DC motor actuators. The speed of the USV depended on the total thrust from the two propellers, and the course of the USV was adjusted by the thrust difference between the left and right propellers. It is seen that the left and right propellers' control is defined in Figure 6 by: brushless DC motor actuators. The speed of the USV depended on the total thrust from the two propellers, and the course of the USV was adjusted by the thrust difference between the left and right propellers. It is seen that the left and right propellers' control is defined in Figure 6 by:

$$\mathbf{C}\_{\mathbf{l},\mathbf{r}} = \mathbf{C}\_{\mathbf{u}} \pm \mathbf{C}\_{\mathbf{h}}.\tag{33}$$

୪,୰ = ୳ ± ୦. (33) In Equation (30), at Section 3.3.3, the transformation function of the course was couple with the speed of the USV, and the USV could be seen as a cascade system. Since In Equation (30), at Section 3.3.3, the transformation function of the course was couple with the speed of the USV, and the USV could be seen as a cascade system. Since their relationship is linear, the controllers could be designed by a twin-PID controller, and the diagram for the autonomous control of the USV is shown in Figure 10. In Equation (30), at Section 3.3.3, the transformation function of the course was couple with the speed of the USV, and the USV could be seen as a cascade system. Since their relationship is linear, the controllers could be designed by a twin-PID controller, and the diagram for the autonomous control of the USV is shown in Figure 10.

their relationship is linear, the controllers could be designed by a twin-PID controller,

**Figure 10.** The speed and course PID controllers. **Figure 10.** The speed and course PID controllers.

**Figure 10.** The speed and course PID controllers. The speed controller of the USV was designed as a traditional PID, The speed controller of the USV was designed as a traditional PID,

$$\mathbf{C\_{u}}(k) = \mathbf{P\_{1}} \cdot \mathbf{e\_{u}}(k) + \mathbf{I\_{1}} \cdot \sum\_{j=1}^{k-1} \mathbf{e\_{u}}(j) + \mathbf{D\_{1}} \cdot (\mathbf{e\_{u}}(k) - \mathbf{e\_{u}}(k-1)),\tag{34}$$

$$\dots, \quad \dots \quad \dots \quad \dots \quad \dots \quad \dots \quad \dots \quad \dots \quad \dots$$

୳() = Pଵ ∙ ୳() + Iଵ ∙ ∑ ୳() ିଵ ୨ୀଵ + Dଵ ∙ (୳() − ୳(−1)), (34) where ୳() is the error between the expected speed and the current speed at the *k* moment. The course controller of the USV was designed as an incremental PID, where *e*u(*k*) is the error between the expected speed and the current speed at the *k* moment. The course controller of the USV was designed as an incremental PID,

$$\mathbf{C\_h} = \mathbf{P\_2} \cdot \left( \mathbf{e\_h}(k) - e\_\mathbf{h}(k-1) \right) + \mathbf{I\_2} \cdot e\_\mathbf{h}(k) + \mathbf{D\_2} \cdot \left( e\_\mathbf{h}(k) - 2e\_\mathbf{h}(k-1) + e\_\mathbf{h}(k-2) \right), \tag{35}$$

where *e*h(*k*) is the error between the expected course and the current course at the *k* moment. Therefore, the uncoupling course PID controller was, ୰(k) = ୳ − Pଶ ∙ (୦() − ୦(−1))+Iଶ ∙ ୦() + Dଶ ∙ (୦(k) − 2୦(−1) + ୦(−2)). (36) Based on the transformation function of the speed in Equation (20), the parameter

where ୦() is the error between the expected course and the current course at the *k*

୦ = Pଶ ∙ ൫୦() − ୦(−1)൯+Iଶ ∙ ୦() + Dଶ ∙ ൫୦() − 2୦(−1) + ୦(−2)൯, (35)

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 13 of 18

Therefore, the uncoupling course PID controller was, ୪() = ୳ + Pଶ ∙ (୦() − ୦(−1))+Iଶ ∙ ୦() + Dଶ ∙ (୦(k) − 2୦(−1) + ୦(−2)),

$$\begin{array}{c} \mathsf{C}\_{\mathrm{l}}(\mathrm{k}) = \mathsf{C}\_{\mathrm{u}} + \mathsf{P}\_{\mathrm{2}} \cdot \left( \mathsf{e}\_{\mathrm{h}}(\mathrm{k}) - \mathsf{e}\_{\mathrm{h}}(\mathrm{k} - 1) \right) + \mathsf{I}\_{\mathrm{2}} \cdot \mathsf{e}\_{\mathrm{h}}(\mathrm{k}) + \mathsf{D}\_{\mathrm{2}} \cdot \left( \mathsf{e}\_{\mathrm{h}}(\mathrm{k}) - 2\mathsf{e}\_{\mathrm{h}}(\mathrm{k} - 1) + \mathsf{e}\_{\mathrm{h}}(\mathrm{k} - 2) \right), \\ \mathsf{C}\_{\mathrm{r}}(\mathrm{k}) = \mathsf{C}\_{\mathrm{u}} - \mathsf{P}\_{\mathrm{2}} \cdot \left( \mathsf{e}\_{\mathrm{h}}(\mathrm{k}) - \mathsf{e}\_{\mathrm{h}}(\mathrm{k} - 1) \right) + \mathsf{I}\_{\mathrm{2}} \cdot \mathsf{e}\_{\mathrm{h}}(\mathrm{k}) + \mathsf{D}\_{\mathrm{2}} \cdot \left( \mathsf{e}\_{\mathrm{h}}(\mathrm{k}) - 2\mathsf{e}\_{\mathrm{h}}(\mathrm{k} - 1) + \mathsf{e}\_{\mathrm{h}}(\mathrm{k} - 2) \right). \end{array} \tag{36}$$

Based on the transformation function of the speed in Equation (20), the parameter tuning was executed by the cut-and-trial method, and the parameters were acquired as P<sup>1</sup> = 30.0, I<sup>1</sup> = 30.0 and D<sup>1</sup> = 3.0. Based on the transformation function of the course control Equation (31), the parameter tuning was executed by the Ziegler–Nichols frequency response method, and the parameters were acquired as P<sup>2</sup> = 66.7, I<sup>2</sup> = 24.1 and D<sup>2</sup> = 46.2. In the low level control of the actuators for the USV's propellers, the control voltage for the actuators was described according to the intrinsic performance of the propeller in the sea trials as follows, Equation (31), the parameter tuning was executed by the Ziegler–Nichols frequency response method, and the parameters were acquired as P2 = 66.7, I2 = 24.1 and D2 = 46.2. In the low level control of the actuators for the USV's propellers, the control voltage for the actuators was described according to the intrinsic performance of the propeller in the sea trials as follows, V୪,୰ = ቐ1−ቀ .଼ ଶቁ∙୪,୰, when the propellor is corotation

$$\mathbf{V}\_{\mathbf{l},\mathbf{r}} = \begin{cases} 1 - \left(\frac{0.8}{200}\right) \cdot \mathbb{C}\_{\mathbf{l},\mathbf{r}} \text{, when the propeller is rotation} \\\ 1.4 + \left(\frac{0.8}{200}\right) \cdot \mathbb{C}\_{\mathbf{l},\mathbf{r}'} \text{ when the propeller is reverse} \end{cases}, \mathbf{C}\_{\mathbf{l},\mathbf{r}} \in [0, 200],\tag{37}$$

where the stop voltage of the actuators is 1.0 Volt and the control dead zone of the actuators is about 0.2 Volt. ators is about 0.2 Volt. **4. Result of Sea Experiments** 

#### **4. Result of Sea Experiments** The proposed trajectory tracking scheme was tested in sea experiments using our

The proposed trajectory tracking scheme was tested in sea experiments using our 'Jiuhang490' USV. The sea experiments were executed at Nanjiang dock in Qingdao City, China, on 16–31 July 2018. During the sea experiments, the hardware system, autonomous control and data acquisition for nuclear radiation were tested [30], and the sea experiments for the USV are shown in Figure 11. 'Jiuhang490' USV. The sea experiments were executed at Nanjiang dock in Qingdao City, China, on 16-31 July, 2018. During the sea experiments, the hardware system, autonomous control and data acquisition for nuclear radiation were tested [30], and the sea experiments for the USV are shown in Figure 11.

moment.

**Figure 11.** Sea experiments at Qingdao in 2018. (**a**) The 'Jiuhang' USV during the sea experiments; (**b**) location of the sea experiments. **Figure 11.** Sea experiments at Qingdao in 2018. (**a**) The 'Jiuhang' USV during the sea experiments; (**b**) location of the sea experiments.

#### *4.1. Dynamics Control Results 4.1. Dynamics Control Results*

The autonomous control for the expected course and speed is shown in Figure 12. In Figure 12a, the course and speed of the USV followed well with the expected course of 220° and the expected speed of 4 kn. The initial course was about 269°, and the initial speed was zero. The trial result denoted that the coupled controllers for the course and speed were effective; however, there existed a fluctuation in some tracking errors. There were three reasons for the fluctuation; the first one was that the variable attitude of the USV caused by waves led to a fluctuation in the course's measurement by the digital The autonomous control for the expected course and speed is shown in Figure 12. In Figure 12a, the course and speed of the USV followed well with the expected course of 220◦ and the expected speed of 4 kn. The initial course was about 269◦ , and the initial speed was zero. The trial result denoted that the coupled controllers for the course and speed were effective; however, there existed a fluctuation in some tracking errors. There were three reasons for the fluctuation; the first one was that the variable attitude of the USV caused by waves led to a fluctuation in the course's measurement by the digital compass and speed's measurement by the GPS, the second one was that the circumstance compensation for the controllers was not considered, and the third one was that the precision of the speed was about 0.1 kn. Though there were some fluctuations in the following error, the following result for the USV was stable in the corresponding trajectory in Figure 12b, where the circle and the plus denote the initial position and the terminal position of the USV, respectively. Course(°)

Speed(kn)

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 14 of 18

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 14 of 18

The performance of the coupled controllers for the expected course of 330◦ and expected speed of 5 kn was good, which can be seen in the following results in Figure 13. the expected course of 330° and expected speed of 5 kn was good, which can be seen in the following results in Figure 13. the expected course of 330° and expected speed of 5 kn was good, which can be seen in the following results in Figure 13.

compass and speed's measurement by the GPS, the second one was that the circumstance compensation for the controllers was not considered, and the third one was that the precision of the speed was about 0.1 kn. Though there were some fluctuations in the following error, the following result for the USV was stable in the corresponding trajectory in Figure 12b, where the circle and the plus denote the initial position and the terminal position of the USV, respectively. The performance of the coupled controllers for

compass and speed's measurement by the GPS, the second one was that the circumstance compensation for the controllers was not considered, and the third one was that the precision of the speed was about 0.1 kn. Though there were some fluctuations in the following error, the following result for the USV was stable in the corresponding trajectory in Figure 12b, where the circle and the plus denote the initial position and the terminal position of the USV, respectively. The performance of the coupled controllers for

**Figure 12.** The course and speed control of the USV with expected course (220°) and speed (4 kn). (**a**) The coupled control for course and speed; (**b**) the corresponding trajectory of the USV. **Figure 12.** The course and speed control of the USV with expected course (220◦ ) and speed (4 kn). (**a**) The coupled control for course and speed; (**b**) the corresponding trajectory of the USV. **Figure 12.** The course and speed control of the USV with expected course (220°) and speed (4 kn). (**a**) The coupled control for course and speed; (**b**) the corresponding trajectory of the USV.

**Figure 13.** The course and speed control of the USV with expected course (330°) and speed (5 kn). (**a**) The coupled control for course and speed; (**b**) the corresponding trajectory of the USV. **Figure 13.** The course and speed control of the USV with expected course (330°) and speed (5 kn). (**a**) The coupled control for course and speed; (**b**) the corresponding trajectory of the USV. **Figure 13.** The course and speed control of the USV with expected course (330◦ ) and speed (5 kn). (**a**) The coupled control for course and speed; (**b**) the corresponding trajectory of the USV.

In order to test the course and speed coupled controllers, seven autonomous courses of running of the USV were performed in the sea trials. Without the loss of generality, the seven expected courses of the USV were designed in four quadrants, and the corresponding expected velocities were set between 1kn and 5kn. The following errors of the course and speed in the stability running of the USV are shown, respectively, in Table 1. In order to reduce the control frequency for the propellers, the control precisions for the course and speed following were set as 0.5° and 0.1 kn, respectively, which equaled to the measurement precision of the course by the HMR 3000 digital compass and to the measurement precision of the speed by Hemisphere VS330 GPS onboard the USV, respectively. When the course and speed of the USV reached the control precision, con-In order to test the course and speed coupled controllers, seven autonomous courses of running of the USV were performed in the sea trials. Without the loss of generality, the seven expected courses of the USV were designed in four quadrants, and the corresponding expected velocities were set between 1kn and 5kn. The following errors of the course and speed in the stability running of the USV are shown, respectively, in Table 1. In order to reduce the control frequency for the propellers, the control precisions for the course and speed following were set as 0.5° and 0.1 kn, respectively, which equaled to the measurement precision of the course by the HMR 3000 digital compass and to the measurement precision of the speed by Hemisphere VS330 GPS onboard the USV, respectively. When the course and speed of the USV reached the control precision, controls for the course and speed were stopped. In order to test the course and speed coupled controllers, seven autonomous courses of running of the USV were performed in the sea trials. Without the loss of generality, the seven expected courses of the USV were designed in four quadrants, and the corresponding expected velocities were set between 1 kn and 5 kn. The following errors of the course and speed in the stability running of the USV are shown, respectively, in Table 1. In order to reduce the control frequency for the propellers, the control precisions for the course and speed following were set as 0.5◦ and 0.1 kn, respectively, which equaled to the measurement precision of the course by the HMR 3000 digital compass and to the measurement precision of the speed by Hemisphere VS330 GPS onboard the USV, respectively. When the course and speed of the USV reached the control precision, controls for the course and speed were stopped.

trols for the course and speed were stopped. In Table 1, the RMSEs of course tracking were between 3.5◦ and 7.3◦ and the RMSEs of speed tracking were between 0.4 kn and 1.1 kn, except for case seven with the lowest expected speed of 1 kn. Though the experiment was carried out in the port, there always existed a disturbance of the ocean environment, such as wind, current and wave, so the RMSEs of the course and speed tracking were accepted. The tracking performance of case seven for the course was very bad, because the USV was very difficult to steer at a low speed.


In Table 1, the RMSEs of course tracking were between 3.5° and 7.3° and the RMSEs of speed tracking were between 0.4 kn and 1.1 kn, except for case seven with the lowest expected speed of 1 kn. Though the experiment was carried out in the port, there always existed a disturbance of the ocean environment, such as wind, current and wave, so the RMSEs of the course and speed tracking were accepted. The tracking performance of case seven for the course was very bad, because the USV was very difficult to steer at a

**Table 1.** RMSE of course and speed coupled control during the sea experiments. **No. Expected Course/Speed RMSE of Course Control RMSE of Speed Control** 

**Table 1.** RMSE of course and speed coupled control during the sea experiments.

*J. Mar. Sci. Eng.* **2021**, *9*, x FOR PEER REVIEW 15 of 18

\* Cases 1 and 2 were results of the course and speed control in Figures 12 and 13.

#### *4.2. Trajectory Tracking Results 4.2. Trajectory Tracking Results*

low speed.

In order to test the trajectory tracking scheme of the USV, the line and rectangle trajectories tracking were achieved by the 'Jiuhang' USV in the sea experiments, and the typical results are shown in Figures 14 and 15. In order to test the trajectory tracking scheme of the USV, the line and rectangle trajectories tracking were achieved by the 'Jiuhang' USV in the sea experiments, and the typical results are shown in Figures 14 and 15.

**Figure 14.** Line tracking by the USV in the sea experiment. **Figure 14.** Line tracking by the USV in the sea experiment.

**Figure 15.** Rectangular trajectory tracking of the USV in the sea experiment. **Figure 15.** Rectangular trajectory tracking of the USV in the sea experiment.

son was that the USV's control was disturbed by the wind and waves.

In Figure 14, trajectory tracking for a line was performed, where the red plusses In Figure 14, trajectory tracking for a line was performed, where the red plusses represent the two waypoints of the expected line, and the black diamond represents the

represent the two waypoints of the expected line, and the black diamond represents the initial position of the USV, and the black line is the trajectory of the USV. It was shown

expected direction in Figure 14, so the USV could track the line well by a large angle turning. It seemed that the USV did not reach the end point, thanks to an arriving radius around the end point being set. The speed was about 3.3 kn when trajectory tracking of the line was stable. The voyage distance of the USV was about 290.0 m, and the length of

A rectangular trajectory for four waypoints was tracked by the USV, which is shown in Figure 15, where the red plusses represent the four waypoints of the expected rectangle, the black diamond represents the initial position of the USV, and the black line is the trajectory of the USV. The achieved range between the USV and the current expected waypoint is set as 5 m, i.e., when the USV reach to the range, the tracking for the current waypoint was finished and the USV turned to the next waypoint. It was shown that the performance of trajectory tracking was good except for some draft in the vertexes of the rectangle due to no special disposing for plan trajectory around the vertexes. In Figure 15, the rectangle was about 249.6 m × 308.8 m, and the voyage distance of the USV was about 1200.0 m. There was some offset between the expected trajectory and the actual trajectory, and the one reason was that the precision of the GPS was about 2.5 m and the orientation precision of the digital compass was about 0.5°; the other rea-

In view of practical engineering, a three-layered architecture for TPTH-USV's trajectory tracking was proposed and validated using the 'Jiuhang' USV in the sea experiments. Besides the conventional kinematic and kinetic layer, a motion restriction layer was added in the three-layered architecture. The proposed guidance law and controllers in the first and third layers were properly suitable for the type of TPTH USVs, which could be applied directly without considering the motion model's variety. The ALOS law can force the USV to track a curved line with a time constraint and give out speed and course variables which are taken as the expected value in the third layer. The twin PID controller can justly solve the speed and course coupled issue of the TPTH USV. The identified model of the USV was used to restrict the basis control variable and differen-

the expected line was about 249.6 m.

**5. Conclusions** 

initial position of the USV, and the black line is the trajectory of the USV. It was shown that the initial course of the USV was about 266.1◦ , which was almost opposite to the expected direction in Figure 14, so the USV could track the line well by a large angle turning. It seemed that the USV did not reach the end point, thanks to an arriving radius around the end point being set. The speed was about 3.3 kn when trajectory tracking of the line was stable. The voyage distance of the USV was about 290.0 m, and the length of the expected line was about 249.6 m.

A rectangular trajectory for four waypoints was tracked by the USV, which is shown in Figure 15, where the red plusses represent the four waypoints of the expected rectangle, the black diamond represents the initial position of the USV, and the black line is the trajectory of the USV. The achieved range between the USV and the current expected waypoint is set as 5 m, i.e., when the USV reach to the range, the tracking for the current waypoint was finished and the USV turned to the next waypoint. It was shown that the performance of trajectory tracking was good except for some draft in the vertexes of the rectangle due to no special disposing for plan trajectory around the vertexes. In Figure 15, the rectangle was about 249.6 m × 308.8 m, and the voyage distance of the USV was about 1200.0 m. There was some offset between the expected trajectory and the actual trajectory, and the one reason was that the precision of the GPS was about 2.5 m and the orientation precision of the digital compass was about 0.5◦ ; the other reason was that the USV's control was disturbed by the wind and waves.

#### **5. Conclusions**

In view of practical engineering, a three-layered architecture for TPTH-USV's trajectory tracking was proposed and validated using the 'Jiuhang' USV in the sea experiments. Besides the conventional kinematic and kinetic layer, a motion restriction layer was added in the three-layered architecture. The proposed guidance law and controllers in the first and third layers were properly suitable for the type of TPTH USVs, which could be applied directly without considering the motion model's variety. The ALOS law can force the USV to track a curved line with a time constraint and give out speed and course variables which are taken as the expected value in the third layer. The twin PID controller can justly solve the speed and course coupled issue of the TPTH USV. The identified model of the USV was used to restrict the basis control variable and differential control variable simply in the motion limitator. In the future, the three-layered architecture of the TPTH-USV will be improved considering sea disturbances, such as waves and current.

**Author Contributions:** Conceptualization, J.J.; methodology, J.J.; software, D.L.; validation, D.W.; formal analysis, D.L.; investigation, D.L.; resources, Y.M.; data curation, D.L.; writing—original draft preparation, J.J.; writing—review and editing, J.J.; visualization, D.W.; supervision, Y.M.; project administration, Y.M.; funding acquisition, Y.M. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by the National Key Research and Development Program of China, grant number 2017YFC14052.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Not applicable.

**Acknowledgments:** The authors would like to thank Feng Shao and Junnan Shi at FIO for experimental help.

**Conflicts of Interest:** The authors declare no conflict of interest.

## **References**


**Mingjiu Zuo <sup>1</sup> , Guandao Wang <sup>2</sup> , Yongxin Xiao <sup>2</sup> and Gong Xiang 2,3,4,\***


<sup>4</sup> Hubei Key Laboratory of Naval Architecture and Ocean Engineering Hydrodynamics (HUST), Wuhan 430074, China

**\*** Correspondence: gongxiang@hust.edu.cn

**Abstract:** During the implementation of time-consuming tasks such as underwater observation or detection, AUV has to face a difficult and urgent problem that its working duration is greatly shortened by the limited energy stored in the battery device. To solve the power problem, a docking station is installed underwater for AUV charging its battery. However, to realize the automatic underwater charging of AUV via a docking station, the accurate and efficient completion of underwater homing and docking is required for AUV. Underwater automatic homing and docking system is of great significance to improve work efficiency and prolong the endurance of AUV save cost. In this paper, a unified approach that involves such as task planning, guidance and control design, thrust allocation has been proposed to provide a complete solution to the problem of homing and docking of an overactuated AUV. The task-based hybrid target point/line planning and following strategy are proposed for AUV homing and docking. At the beginning of homing, AUV is planned to follow a straight line via the line of sight (LoS) method. Afterward, AUV starts to follow multiple predefined target points until reaching the docking station. At the final stage of docking (within 10 m), a dedicated computer vision algorithm is applied to detect a newly designed LED light array fixed on the docking station to provide accurate guidance for the AUV to dock. The sliding mode control technique is used for the motion control of the AUV allowing robustness. As the AUV configured with eight thrusters is over-actuated, the problem of the thrust allocation is very important and successfully solved using the quadratic programming (QP) optimization method. Finally, the simulations of homing and docking tasks using the AUV are accomplished to verify the proposed approach.

**Keywords:** AUV; homing and docking; vision-based guidance; target point/line planning and following; thrust allocation

## **1. Introduction**

Intelligent Underwater Unmanned Vehicles such as Autonomous Underwater Vehicles (AUVs) have a wide range of applications, which can realize marine environment monitoring, seabed topography survey, underwater resource exploration, marine resource sampling, etc. However, the drawback that the duration of AUV's underwater activities is greatly limited by its own energy stands out during the implementation of time-consuming tasks such as underwater observation. Therefore, to realize an underwater automatic docking system for the AUV is of great significance to improve work efficiency, save manpower and cost. After completing a certain task, it needs to search for the docking station (DS) deployed undersea, gradually navigate to it which are called homing, and finally get clamped by it which is called docking. Once docking is completed successfully, AUV can start the process of charging energy, exchanging data, downloading new tasks, and so on. At last, the fully charged AUV then begins a new mission that has just been uploaded to it.

**Citation:** Zuo, M.; Wang, G.; Xiao, Y.; Xiang, G. A Unified Approach for Underwater Homing and Docking of over-Actuated AUV. *J. Mar. Sci. Eng.* **2021**, *9*, 884. https://doi.org/ 10.3390/jmse9080884

Academic Editor: Rosemary Norman

Received: 7 July 2021 Accepted: 9 August 2021 Published: 17 August 2021

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

Recently, the topics on AUV homing and docking have drawn broad attention from researchers. Many researchers only focused on one single aspect which may be important to realize the autonomous homing and docking process such as hydrodynamic modeling [1–4], planning, decision and following strategy, controller design, etc. Xiang et al. [5] applied the maximum membership and threshold principles into the intelligent decision-making process which guides AUV to take critical operations and ensure safety. Xiong et al. [6] present elite group-based evolutionary algorithms (EGEA) for adaptive ocean sampling using multiple unmanned marine vehicles (UMVs). Peng et al. [7] investigated the distributed time-varying formation control for a fleet of under-actuated autonomous surface vehicles subject to unknown input gains based on a consensus approach, a path-following design, artificial potential functions, and an auxiliary variable approach. Xu and Guedes Soares et al. [8] presented a 2D path following control system for autonomous surface vehicles through a vector field-based way-point guidance scheme. Qin et al. [9] proposed a trajectory tracking control strategy for solving the saturation and full-state constraints problem of the unmanned surface vessels based on the anti-windup compensator and the barrier Lyapunov function. Xu et al. [10] adopted an L1 adaptive backstepping controller where the control law is derived using the Lyapunov control function to realize the path-following control of an underactuated ship. According to the flight characteristics of the parafoil, Tao et al. [11] designed a multiphase homing path. Based on the active disturbance rejection control (ADRC), a homing controller is designed to track the horizontal and vertical trajectory. Simulations show that the planned trajectory can successfully accomplish the target of fixed point homing and flare landing. The ADRC can track the homing path more rapidly, steadily, and get better control performances than the PID controller. Martinsen et al. [12] proposed an all-encompassing procedure method for performing both docking, maneuvering, dynamic positioning, and control allocation of marine vessels using numerical optimal control. The method is found capable of being implemented as a real-time MPC-based algorithm on a supply vessel. Li et al. [13] proposed a robust adaptive neural network control for the dynamic positioning of marine vessels with prescribed performance under model uncertainties and input saturation. Anderlini et al. [14] realized the control for the docking of an AUV onto a fixed station via reinforcement learning strategies. Two reinforcement learning schemes: DDPG and DQN were investigated and compared with optimal control techniques. The authors found that reinforcement learning achieves a performance similar to optimal control at a much lower computational cost at deployment, whilst also presenting a more general framework. Zhang et al. [15] proposed a virtual submerged floating operational system (VSFOS) based on parallel and serial robotic platforms. The data collected by the inertial sensor is received by the designed control system architecture, software to communicate and send instructions. Uchihori et al. [16] developed a control system for driving an AUV performing docking operations in presence of tidal current disturbances is proposed. The Linear Parameter-Varying (LPV) model was used for a Model Predictive Control (MPC) design for computing the set of forces and moments driving the nonlinear vehicle model. The LPV-MPC control action is mapped into the reference signals for the actuators by using a Thrust Allocation (TA) algorithm. The structural decomposition of MPC and TA reduces the computational burden involved in computing the control law online on an embedded control board. The proposed control system has been tested and validated in the range of control scenarios with various tidal current disturbances. Wang et al. [17] adopted a fuzzy adaptive linear active disturbance rejection control (LADRC) for precise control of the underwater glider's trajectory. Besides AUV, the control design of the autonomous surface vehicle (ASV) and other operational mechanical systems is also investigated. Bitar et al. [18] studied automatic docking of a small autonomous surface vehicle (ASV) in confined waters in Trondheim, Norway by interconnecting an optimization-based trajectory planner which provides collision-free trajectories facing static obstacles and a dynamic positioning (DP) controller which can track the planned time-parametrized position, velocity, and acceleration. Wang et al. [19] managed to carry out cloud-based mission control of the USV fleet.

Peng et al. [20,21] discussed the recent development of trajectory-guided, path-guided, and target-guided coordinated control of multiple ASVs in detail. Roman et al. [22] combined model-free adaptive control with the fuzzy component by virtual reference feedback tuning. The new proposed algorithm is validated using experimental results to the arm angular position of the nonlinear tower crane system. Turnip and Panggabean [23] developed a combination of skyhook and ground hook control-based magnetorheological lookup table technique called hybrid control for a quarter car. The simulation of the semi-active suspension design indicates that the proposed Hybrid control lookup table provides better vibration isolation capability than the skyhook controller and hybrid conventional methods. Precup et al. [24] propose a set of evolving Takagi–Sugeno–Kang (TSK) fuzzy models of the nonlinear dynamic mechanisms occurring in the myoelectric-based control of prosthetic hand fingers. By comparing with the experimental data and two recurrent neural network architectures, the proposed controlling method is found reliable and convincing.

When it comes to navigation or guidance design, the navigation aids applied for the AUV docking mainly include acoustic, optical, and electromagnetic ways, etc. The AUV guidance systems are classified into three methods: point-point method, graph search method, and optimization-based method [25]. Anderson et al. [26] simulated the docking process of the Martain AUV using a USBL system and a verified dynamic model. Sans-Muntadas et al. [27] applied an array of underwater navigation aids: ultra-short baseline (USBL), Doppler velocity log (DVL), etc., for the AUV homing and docking tasks. Meanwhile, a modular and cascaded Kalman filter (KF) approach which can estimate the navigation covariance and judge the situation of the DS is used to predict the consequence of the docking procedure. The proposed method can improve the autonomy level of the AUV and adjust measures as required. Vandavasi et al. [28] applied the bio-inspired differential magnetometry-based electromagnetic homing guidance system (EMHGS) into the docking process of a prototype twin thruster AUV operable in two degrees of freedom (DOF) with an underwater DS. By sensing a magnetic field strength, the EMHGS is found to affect AUV orientation correction by measuring the bearing angle. The authors found that higher dock magnetic field strengths could increase the guidance distance via the validated finite element model. Yahya et al. [29] develop a computer vision-based tracking system for AUV to dock underwater by recognizing the light sources placed on the DS. What is more, this study also found that the successful recognition decreases with the camera approaching the target faster. Therefore, a slow and stable movement of the AUV is necessary to complete a successful docking operation. To accomplish the target of autonomous docking of an industry-standard work-class ROV to both static and dynamic DS TMS (Tether Management System—TMS), Trslic et al. [30] present a machine vision-based docking system developed around subsea camera pose estimation. The relative position between the ROV and the DS can be estimated using a single camera and a known light marker pattern. The vision-based docking system has been tested in a real-world environment in the North Atlantic Ocean and showed comparative capability with a commercial stateof-the-art underwater navigation system. Li et al. [31] provided reliable underwater navigation and vision positioning methods using two cameras for AUV docking. Four green LED lights are fixed around the DS and two cameras are installed in AUV's head part. Vallicrosa et al. [32] proposed a method for homing and docking an AUV to a subsea DS by combining acoustic and optical sensing. Within acoustic ranging distance to the DS, AUV can estimate the DS location using a Sum of Gaussian (SoG) filter. Once the DS position becomes known, AUV gradually approaches it till reaching within visual reach of the DS. At this time, visual information obtained from a light beacon navigation system is used to update to a Simultaneous Localization And Mapping (SLAM) filter providing an AUV-pose estimate with the required accuracy. Yang et al. [33] applied a pursuit guidance algorithm with current compensation into USBL and optical sensing-based navigating and docking hybrid underwater glider (HUG) into a rotatable DS. The comparison of the performance of the guidance algorithm with other existing guidance algorithms, such as pure pursuit guidance and proportional navigation guidance by simulations and experiment validate the

feasibility of the docking system and the effectiveness of the proposed guidance algorithm. Breivik et al. [34] proposed an underway docking procedure that includes two phases to help realize the safe docking of a small unmanned surface vehicle (USV) with a larger mother ship moving in transit at sea. A safety circle together with a virtual target point that can move on the circle toward the assigned docking point is defined around the mother ship for the kinematic-controlled USV to approach. Recently, Wang et al. [35] wrote a review on deep learning techniques for marine object recognition. However, only a few researchers like Park et al. [36], Li et al. [37], Palomeras et al. [38], Matsuda et al. [39], Wang et al. [40], Ferreira et al. [41], Thomas et al. [42], and Page et al. [43] proposed complete solutions or approaches which may involve almost all the technical aspects to deal with the whole underwater homing and docking problem.

In this paper, the main contribution of this paper is the new unified approach which involves task planning, guidance and control design, and thrust allocation, for example, which proposed to provide a complete solution to the problem of homing and docking of an over-actuated AUV. Task planning for AUV is introduced in the homing and docking strategy. The path is mainly planned via a predefined target point or straight line to be followed by AUV via the line of sight (LoS) method. However, at the final stage of docking (within 10 m), a higher temporal and spatial accuracy is required for efficient docking, thus, considering the relative inaccuracy of USBL, vision-based guidance is used for the docking process and providing guidance for the AUV to dock. The sliding mode control technique is applied to the motion control of the AUV allowing robustness. Since the AUV is an over-actuated system, the problem of the thrust allocation is successfully solved using the QP optimization method. When dealing with thrust allocation for 8 thrusters, the proposed quadratic programming optimization technique has the advantages of considering power consumption and deviation at the same time, much smaller deviation under saturation compared with the conventional pseudo inverse allocation method. Finally, the simulations of the whole homing and docking tasks using the AUV are accomplished to verify the proposed approach.

#### **2. Problem Formulation**

In the present study, AUV is an over-actuated one that can implement specific underwater tasks. Therefore, in addition to the conventional inertial motion unit (IMU), DVL, camera, and other sensing devices, AUV is also equipped with an acoustic positioning device (USBL) to determine the relative position with the underwater DS, as well as radio frequency communication device to ensure the reliable communication between AUV and DS at different distances. The DS is a bottom-mounted platform, which is equipped with many important devices such as corresponding acoustic positioning and communication devices, multiple preset light sources and cameras, AUV locking device, underwater charging, and data exchange equipment for docking.

To solve the limitations of the onboard energy storage and increase the long-endurance operational capability of this AUV, a means of enabling persistence to realize an underwater automatic homing and docking system is the critical solution. The typical homing and docking process of the AUV are shown in Figure 1a. The homing process starts once AUV finishes a task and needs to search and move towards the DS from a very far position like 1000 m away from the DS. With the AUV entering a small distance like 10 m to DS, AUV comes to the docking stage. The AUV starts to adjust the pose with reference to the DS and finally gets clamped on it. The proposed homing and docking approach concentrates on the AUV moving plan and associated technical issues including task planning, navigation, and guidance scheme, motion modeling, and control of the underwater AUV as shown in Figure 1b.

**Figure 1.** Sketch of homing and docking approach.

#### *2.1. Description of Task Planning*

During homing and docking process, the distance between AUV and DS is gradually reduced from far-field to nearby and to zero namely absolute docking into the DS finally. When AUV is located at different distances, the sensing devices used for communication by AUV are changed according to the required accuracy and the range of use of the sensing devices. Therefore, different task planning should be designed for homing and docking stages.

#### *2.2. Description of Guidance Design*

The homing process starts at a far-field point where only USBL will work. USBL guidance is used for homing. When AUV is reaching within a small distance to the DS, a higher temporal and spatial accuracy is required for the final docking. Thus, vision-based guidance is used for the docking process.

#### *2.3. Description of Controller Design*

To study the underwater motion of an AUV, a 4-DOF dynamic model is supposed to be established via the thorough analysis of forces acting on the AUV and the propulsion performance of the propeller. Then, based on the dynamic model of the AUV, the design

of a sliding mode controller for AUV motion and thrust allocation controller for AUV propulsion are required.

#### **3. Homing and Docking Strategy**

#### *3.1. Task Planning*

To complete the AUV homing and docking process, reasonable task planning is required before AUV moving. The whole process is divided into homing and docking processes. During each process, the path for AUV to follow including target waypoints and target lines needs to be predefined.

#### 3.1.1. Homing Task

Generally, AUV switching from normal work such as underwater observations to homing and docking tasks may happen at a far-field point from the dock station, so it is necessary to navigate to the preset DS area first. Even if the precise location of the DS is known, AUV cannot directly complete docking with the DS due to the accumulated positioning error of AUV for a long time. Only after the USBL of AUV and the USBL installed on the DS have established mutual communications, AUV starts to calculate and know the relative location of the DS, and then it can move closer towards DS.

As shown in Figure 1a, after AUV gets the position and orientation of the DS, first, a target point A which has the same height as AUV is planned near the DS. Taking the current position and attitude of AUV as the initial conditions, a target line is planned going through the current position and the planned point A. After AUV tracks to the target point A with the range of about 200 m away from the DS following the planned target line, another target point B is planned to be located 10 m directly above the DS using USBL. To move from target point A to B, AUV switches from target line tracking to target point tracking approach, multiple target points need to be updated to make AUV finally located at target point B above the DS with a certain posture as shown in Figure 1a.

#### 3.1.2. Docking Task

When the AUV manages to reach the target point B directly above the DS, the camera can completely distinguish the outline of each LED light source in the light array on the DS. Correspondingly, a vision-based guidance algorithm can be used to obtain the relative position and attitude of the AUV with reference to the DS. Vision-based guidance can also calculate the attitude error of AUV through the visual information, and plan the target point C at the DS position. Initially, the line between BC points is roughly vertical downward. During the descent process, the position and attitude information of target point C are constantly updated; AUV will keep descending slowly with its attitude constantly adjusted to keep consistent with the DS, and finally ensure smooth docking.

In a nutshell, the homing and docking task can be simplified as the task to firstly track the target line and then track multiple target points.

#### *3.2. Guidance Design*

Quipped with different sensors and communication devices, AUVs can move underwater with proper guidance. However, due to the different accuracy of the required relative position information at different distances towards the DS, the guidance is mainly designed by two modes: USBL guidance, and vision-based guidance. USBL guidance is mainly used for homing to help AUV approach towards DS from a very far point, and vision-based guidance is used for AUV clamping into the DS which requires much more accurate near field guidance.

#### 3.2.1. Guidance for Homing

USBL system is equipped with an acoustic transponder array under the AUV. In the homing process, the USBL serves as the navigation aid of the AUV to establish communications with the acoustic equipment of the DS. USBL positioning system localizes the

underwater target, DS mainly by measuring the arrival azimuth and distance of the signal. The acoustic transmitter regularly emits acoustic pulse signals and uses the response time of the DS to calculate the slant distance between the AUV and the DS. USBL uses the phase difference of each response signal received by multiple hydrophones to calculate the azimuth of the DS. It is assumed that the transponder array composed of four hydrophones is located on baselines that are mutually perpendicular to each other as shown in Figure 2.

**Figure 2.** Positioning of AUV using USBL.

In Figure 2, four red points represent four hydrophones, the blue point stands for the position of DS. *θ<sup>x</sup>* and *θ<sup>y</sup>* are the relative pose vector with reference to the *x*-axis and *y*-axis, respectively. The coordinate of AUV relative to the DS can be represented by (*xa*, *ya*, *za*) which has the following relation:

$$\begin{array}{l} R = \mathbf{c} \cdot \frac{T}{2} = \sqrt{x\_a^2 + y\_a^2 + z\_a^2} \\ x\_a = R \cos \theta\_x \\ y\_a = R \cos \theta\_y \end{array} \tag{1}$$

where, c is the velocity of sound traveling in the water, *T* is the roundtrip period, *θ<sup>x</sup>* and *θ<sup>y</sup>* can be calculated by the phase difference of the received signals.

#### 3.2.2. Guidance for Docking

In the final stage of 10 m away from the DS, the relative pose of AUV is calculated and controlled through the real-time image recognition of the LED light array captured by the bottom camera of AUV. Among them, the vision system part mainly includes two parts: the input is the real-time image and the output is the spatial position of AUV.

**Design of the LED light array on the DS.** The three LEDs are arranged on the same line which is the longitudinal axis of the DS. There is one LED namely LED2 located at the center of the DS with its left LED 1 and right LED3 symmetrically installed. The fourth light, LED4 is on the same side as LED3, whose connecting line is perpendicular to the longitudinal axis of DS. According to the configuration of the DS shown in Figure 3, the longitudinal axis of DS passes through the connecting line between the two charging pile centers on the right side and the center of the charging pile on the left side. As shown in Figure 3, the LED light array is designed in detail as follows: 1. LED2 is located at the center of the DS. LED1, LED2, and LED3 are arranged on the longitudinal axis of the DS. The central location and the longitudinal axis of the DS can be determined by the location of three LEDs captured in the image; 2. LED1, LED2, and LED3 consist of one axis while LED3 and LED4 make the other axis. Hence, the asymmetry makes it convenient to distinguish the axis and the heading angle of the AUV by the number of lights when the image is rotated;

**Figure 3.** Arrangement of the LED light array on the Docking Station.

**Image preprocessing**. After the image is obtained by the camera under the AUV, there are often different kinds of noise in the image because of many impurities in the water. The important information in the image can be selectively extracted from a specific application environment. To precisely extract the required information from the image, it is necessary to preprocess the image first. Image filtering, that is, to remove the noise of the target image while retaining the details of the image, is an indispensable operation in image preprocessing. The quality of the preprocess will directly affect the effectiveness and reliability of the subsequent image processing and analysis. Filtering can remove the noise in the image, extract useful visual features, resample the image, and so on. As a kind of spatial filtering, median filtering can not only eliminate image noise but also make up for the shortcomings of the neighborhood average method to blur the edge, better retain the edge of the image, ensure relatively clear image contour. Because the edge of the image mainly includes the details and high-frequency characteristics of the image, the median filter is adopted for extracting the edge of the spot in the spot recognition. The basic idea of median filtering is to sort the gray levels of all pixels in a window and take the median value of the sorting result as the gray level value of the pixel at the center of the original window. Median filtering using the selected window is similar to the method of moving the operator on the image in the template matching operation. The process can be described into the following steps: (1) Determine the coincidence mode of the pixel at the center of the window according to the shape of the selected window on the original image; (2) Move the window pixel by pixel on the image; (3) Sort the corresponding pixels in the window according to their gray value, and find the median value of the sorting results; (4) Assign the found median value to the pixel in the resulting image corresponding to the center of the window.

The median filter is very effective for eliminating random noise and salt and pepper noise in the image. The main advantage of planting filter is a simple operation, which can filter out the noise and protect the detailed information of signals, such as edge and acute angle. In addition, the planting filter is easy to be adaptive, which can further improve its filtering characteristics.

**Position recognition of LED light array.** After the AUV reaches 10 m above the DS, the real-time position of the AUV is obtained via the position of the LED light array relative to the AUV. The image obtained by AUV contains many bright spots which are the position of the LEDs in the light array. In the previous step, after preprocessing the image such as filtering, edge extraction is carried out to obtain the position of the edge of the bright spot

in the image, the center of each bright spot, that is, the position of each LED is calculated by the gray barycenter method.

For edge detection, the Canny operator is used in this paper. The operator is a multi-stage optimization operator with filtering, enhancement, and detection. The Canny operator is not easy to be disturbed by noise and can detect real edges, especially weak edges. The advantage of this method is that two different thresholds are used to detect the strong edge and the weak edge respectively. The weak edge is included in the output image only when connected with the strong edge. Therefore, this method is not easy to be affected by noise and is easier to detect the real weak edge. Before processing, the Canny operator first uses a Gaussian smoothing filter to smooth the image, remove noise, and then use the finite-difference of first-order partial derivative to calculate the gradient amplitude and direction. Finally, non-maximum suppression, edge detection, and connection with double threshold algorithm are conducted. Gaussian filtering is a common filtering algorithm at present. Its principle is weighted average according to the gray value of the pixel to be filtered and its neighboring points according to the parameter rules generated by the Gaussian formula, which can effectively filter the high-frequency noise superimposed in the ideal image. The transfer function of two-dimensional Gaussian filter is defined as

$$h(\mathbf{x}, y) = \frac{1}{2\pi\sigma^2} e^{-\frac{\mathbf{x}^2 + y^2}{2\sigma^2}} \tag{2}$$

Apply the *h*(*x*, *y*) into the filtering of image *f*(*x*, *y*) to get

$$g(\mathbf{x}, y) = h(\mathbf{x}, y) \* f(\mathbf{x}, y) \tag{3}$$

where "∗" represents convolution.

The gradient of smoothed can be calculated by using the 2 × 2 first-order finite difference approximation to calculate the two arrays GX and GY of partial derivatives of *x* and *y*. The mean value of the finite difference can be calculated in this 2 × 2 square to calculate the gradient of partial derivatives of *x* and *y* at the same point in the image. Only the global gradient is not enough to determine the edge, so to determine the edge, we must use the gradient direction to retain the point with the maximum local gradient, and suppress the non-maximum value. When the gradient angle is discretized into one of the four sectors of the circle, the window of the circle is used for suppression operation. The four sectors are numbered from 0 to 3, corresponding to four possible combinations of neighborhoods. At each point, the center pixel of the neighborhood is compared with the two pixels along the gradient line. If the gradient value of the center pixel is not greater than the gradient value of the two adjacent pixels along the gradient line, then = 0. After the above three steps, the edge quality is very high, but there are still many false edges. Therefore, the algorithm used in the Canny algorithm is the double threshold method. The specific idea is to select two thresholds. The points less than the low threshold are considered as false edges set to 0, the points greater than the high threshold are considered as strong edges set to 1, and the pixels in the middle need to be further checked.

According to the high threshold image, the edge is linked into a contour. When it reaches the end of the contour, the algorithm will find the point satisfying the low threshold in the 8 neighborhood points of the breakpoint, and then collect new edges according to this point until the whole image is closed. After obtaining the edge information of the bright spot, to obtain the center coordinates of the LED lights, it is necessary to determine the center of the bright spot. In this paper, the gray centroid method is used. For the target with uneven brightness, the gray centroid method can calculate the light power centroid coordinates according to the target light intensity distribution as the tracking point, also known as the density centroid algorithm. For an image of size *m* × *n*, if the gray value of a pixel exceeds the threshold *T*, it will participate in the barycenter processing. The gray center (*x*0, *y*0) is calculated as

$$\mathbf{x}\_0 = \frac{\sum\_{i=1}^{m} \sum\_{j=1}^{n} \mathbf{x}\_i f\_{ij}}{\sum\_{i=1}^{m} \sum\_{j=1}^{n} f\_{ij}} \tag{4}$$

$$y\_0 = \frac{\sum\_{i=1}^{m} \sum\_{j=1}^{n} y\_j f\_{ij}}{\sum\_{i=1}^{m} \sum\_{j=1}^{n} f\_{ij}} \tag{5}$$

$$f\_{\hat{l}\hat{j}} = \begin{cases} \begin{array}{c} 0, \text{gray value} < T \\ f\_{\hat{l}\hat{j}\prime} \text{gray value} \ge T \end{array} \end{cases} \tag{6}$$

where represents the pixel value of the *i*-th row and *j*-th column.

**Positioning of AUV.** According to the position of LED in the image, the relative position (*x*, *y*) and deviation angle θ between AUV and DS center are calculated by using the proportional relationship between camera field of view and image size. The plan of the LED light array is as follows:

1. Determine the relative position(*x*, *y*) between the DS center and the AUV bottom center (where the camera is). In determining the relative position(*x*, *y*) of the DS center, it is first necessary to determine the position of the DS center light, that is, LED2 in the light array in Figure 4. After the center position of each spot is calculated by the gray centroid method, four pixels are obtained, and the sum of the distances between each pixel and the other three pixels is calculated. The least sum of the distances is LED2, which is also the position of the DS center in the image. After determining the representative spot at the center of the DS, the azimuth angle can be calculated by using the geometric relationship (the azimuth of the target on the left side of the axis is negative, and the azimuth on the right side of the axis is positive) as shown in Figure 5. The angle between the projection of the line between the center of the DS and the camera on the horizontal plane with the camera axis and the camera axis is called the horizontal azimuth *θ*Lenx as shown in Figure 6a, then the horizontal azimuth of the DS center has the following relationship with the abscissa of the DS center in the image:

$$l\_{Bx} = \mathfrak{u}\_{obj} - \mathfrak{c}\_x \tag{7}$$

$$\psi\_{\text{obj\\_x}} = l\_{Bx} \frac{\theta\_{\text{Lenz}}}{W\_{\text{imgx}}} = \frac{\left(u\_{\text{obj}} - c\_x\right)\theta\_{\text{Lenz}}}{W\_{\text{imgx}}} \tag{8}$$

**Figure 4.** Arrangement of the LED light array on the DS.

**Figure 5.** Calculation of Azimuth angle.

**Figure 6.** Calculation of Azimuth angles.

Similarly, from Figure 6b, the relationship between *θ*Leny 's vertical azimuth and the longitudinal coordinate of the DS center in the image is as follows:

$$l\_{By} = \upsilon\_{obj} - \mathfrak{c}\_{y} \tag{9}$$

$$\psi\_{\rm obj} = l\_{By} \frac{\theta\_{\rm Leny}}{W\_{\rm impy}} = \frac{\left(v\_{\rm obj} - c\_y\right)\theta\_{\rm Leny}}{W\_{\rm impy}} \tag{10}$$

The relative position (*x*, *y*) between the center of the DS and the center of AUV bottom can be obtained from the geometric relationship.

$$\mathfrak{x} = \mathfrak{h} \times \tan \psi\_{obj\text{x}} \tag{11}$$

$$y = h \times \tan \psi\_{obj} \tag{12}$$

2. Determine the AUV yaw angle *θ*(Counterclockwise is positive, clockwise is negative). The deviation angle of the DS in the image needs to be determined. To calculate the location of the DS axis via the location of LED1, LED2, and LED3 in the light array, it is necessary to calculate the slope of the line between the pixel coordinates of three LED lights except for LED2 and LED2 in the image coordinate system, and then compare the three slopes. Then, the two lines with the closest slope will pass

through LED1 and LED3 respectively, and the mean value of the slopes of the two lines will be treated as the slope of the DS axis in the image. The angle between the DS axis and the horizontal direction can be determined by the slope of the line. Then LED4 is used to determine the driving direction of AUV relative to the DS. The first is to determine the position of LED3 and calculate the distance between the three points on the line and the center of LED4. The closest point is the center of LED3. Here *X*<sup>2</sup> and *X*<sup>3</sup> are the pixel values of the abscissa of LED2 and LED3 in the center of the image respectively. If *X*<sup>2</sup> > *X*3, the yaw angle of AUV *θ* = *θ*0, otherwise *θ* = 180◦ − *θ*0.

## **4. Controller Design**

#### *4.1. Dynamic Model*

Generally, the dynamic model of the AUV or other ocean vehicles is in 6 degrees of freedom [2,4]. Considering the symmetry of the AUV, the two degrees of freedom of roll and pitch are ignored, that is to say, *θ* = Ψ = 0. According to the motion control requirements of AUV, it is necessary to control the pitch, sway, heave, and yaw. Therefore, the general 6-DOF robot model is simplified to form a 4-DOF model as expressed by Equation (13). Only the following state vectors need to be considered, *η* = [ *x y z ψ* ] *T* , *v* = [ *u v w r* ] *T* , *τ* = [ *X Y Z N* ] *T* , It is noted that the coefficient of reference model ignores the small nonlinear terms. The moments produced by each propeller to roll and pitch are ignored.

$$M\dot{v} + \mathcal{C}(v)v + D(v)v + \mathcal{g}(\eta) = \tau \tag{13}$$

where the above hydrodynamic force matrix can be also obtained via the CFD method [44–47], the inertial matrix *M* is

$$M = \begin{bmatrix} m - X\_{\dot{u}} & 0 & 0 & 0 \\ 0 & m - Y\_{\dot{v}} & 0 & 0 \\ 0 & 0 & m - Z\_{\dot{w}} & 0 \\ 0 & 0 & 0 & I\_{zz} - N\_{\dot{r}} \end{bmatrix} \tag{14}$$

The Coriolis force matrix *C*(*v*) is

$$\mathbf{C}(v) = \begin{bmatrix} 0 & 0 & 0 & -(m - Y\_{\bar{v}})v \\ 0 & 0 & 0 & (m - X\_{\bar{u}})u \\ 0 & 0 & 0 & 0 \\ (m - Y\_{\bar{v}})v & -(m - X\_{\bar{u}})u & 0 & 0 \end{bmatrix} \tag{15}$$

The damping force matrix *D*(*v*) is

$$D(v) = -\begin{bmatrix} \mathbf{X}\_{\boldsymbol{\varPi}} + \mathbf{X}\_{\boldsymbol{\varPi}\_{[\boldsymbol{\varPi}]}} \Big| \boldsymbol{u} \Big| & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{Y}\_{\boldsymbol{\varPi}} + \mathbf{Y}\_{\boldsymbol{[v}\_{[\boldsymbol{v}]}} \Big| \boldsymbol{v} \Big| & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{Z}\_{\boldsymbol{W}} + \mathbf{Z}\_{\boldsymbol{\varPi}\_{[\boldsymbol{v}]}} \Big| \boldsymbol{w} \Big| & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{N}\_{\boldsymbol{r}} + \mathbf{N}\_{\boldsymbol{r}\_{[\boldsymbol{r}]}} \Big| \boldsymbol{r} \Big| \end{bmatrix} \tag{16}$$

The resulting force of gravity and buoyancy force is

$$\log(\eta) = \begin{bmatrix} 0, 0, -(\mathcal{W} - B), 0 \end{bmatrix}^T \tag{17}$$

The controlling force acting on the AUV is

$$
\pi = \mathbb{K}u \tag{18}
$$

where *K* is the thrust allocation matrix shown as in Equation (34):

#### *4.2. Sliding Mode Based Homing and Docking Control Design*

To realize various underwater tasks, AUV needs to maintain a certain attitude, such as a certain depth, altitude, and heading, which can be solved by a motion controller. Moreover, AUV moving underwater will be inevitably affected by various external interference forces. In addition, many difficulties are obtaining hydrodynamic coefficients of the AUV in different events. The existence of these factors makes motion control of AUV more challenging. For the purpose of motion control, the sliding mode control method has the advantages of fast response, insensitive to corresponding parameter changes and disturbances, and simple subsequent physical implementation compared with the conventional control method like PID. Considering sliding mode control is very robust to the internal perturbation and external environmental disturbance and easily realized in engineering problems, the sliding mode control method is adopted to study the motion control of AUV.

#### 4.2.1. Target Point Tracking

The four degrees of freedom of AUV need to be controlled separately to realize target point tracking. Taking heave motion as an example, the controller is deduced as such: according to the principle of the sliding mode VSC, the error of target controlling parameters, like depth, must be constructed to calculate the first and second-order derivatives respectively. The depth channel error and its derivatives are as follows

$$e\_z = z - z\_d,\\
\dot{e}\_z = \dot{z} - \dot{z}\_d,\\
\ddot{e}\_z = \ddot{z} - \ddot{z}\_d = -\frac{d\_3 w}{m\_3} + \frac{\tau\_3}{m\_3} - \ddot{z}\_d \tag{19}$$

The sliding mode surface function for the depth channel is defined as follows:

$$\mathcal{S}\_z = \dot{e}\_z + c\_z e\_z \dot{\mathcal{S}}\_z = \ddot{e}\_z + c\_z \dot{e}\_z \tag{20}$$

where, *c<sup>z</sup>* > 0, the depth channel sliding mode function, Lyapunov function is designed as

$$V\_z = \frac{1}{2} \mathcal{S}\_z^2 \tag{21}$$

Substitute Equations (19) and (20) into the derivative of Equation (21):

$$\dot{V}\_{z} = \mathcal{S}\_{z}\dot{\mathcal{S}}\_{z} = \mathcal{S}\_{z}(\ddot{e}\_{z} + c\_{z}\dot{e}\_{z}) = \mathcal{S}\_{z}(\ddot{e}\_{z} + c\_{z}\dot{e}\_{z}) = \mathcal{S}\_{z}(-\frac{d\_{3}w}{m\_{3}} + \frac{\tau\_{3}}{m\_{3}} - \ddot{z}\_{d} + c\_{z}\dot{e}\_{z}) \tag{22}$$

Power reaching law is applied on Equation (22):

$$-\frac{d\_3 w}{m\_3} + \frac{\tau\_3}{m\_3} - \ddot{z}\_d + c\_z \dot{e}\_z = -k\_z |\mathbf{S}\_z|^{d\_z} \text{sgn}(\mathbf{S}\_z) \tag{23}$$

By solving Equation (23), the heaving force *τ*<sup>3</sup> can be calculated as follows:

$$\tau\_3 = m\_3 \left( \frac{d\_3 w}{m\_3} + \ddot{z}\_d - c\_z \dot{e}\_z - k\_z |\mathbf{S}\_z|^{a\_z} \text{sgn}(\mathbf{S}\_z) \right) \tag{24}$$

Similarly, the pitching *τ*1, surging forces *τ*2, and rolling moment *τ*<sup>4</sup> can be derived as

$$\begin{cases} \tau\_1 = m\_1 \left( -\frac{m\_2 v r}{m\_1} + \frac{d\_1 u}{m\_1} + \ddot{\mathbf{x}}\_d - c\_x \dot{e}\_x - k\_x |\mathbb{S}\_x|^{a\_X} \text{sgn}(\mathcal{S}\_x) \right) \\ \tau\_2 = m\_2 \left( -\frac{m\_1 u r}{m\_2} + \frac{d\_2 v}{m\_2} + \ddot{\mathbf{y}}\_d - c\_y \dot{e}\_y - k\_y |\mathbb{S}\_y|^{a\_Y} \text{sgn}(\mathcal{S}\_y) \right) \\ \tau\_4 = m\_4 \left( -\frac{(m\_1 - m\_2) u v}{m\_4} + \frac{d\_4 w}{m\_4} + \ddot{r}\_d - c\_r \dot{e}\_r - k\_r |\mathbb{S}\_x|^{a\_r} \text{sgn}(\mathcal{S}\_r) \right) \end{cases} \tag{25}$$

Theoretically, the discontinuous sign function will cause "jittering" of the system, reducing the validity of the control. To suppress the jittering, the boundary layer method is

applied for improving the designed control law. Specifically, replace the *sgn*(*S*) by *sat*(*S*), and the redesigned power approaching law for Equation (24) is shown as:

$$-\frac{d\_3 w}{m\_3} + \frac{\tau\_3}{m\_3} - \ddot{z}\_d + c\_z \dot{e}\_z = -k\_z |\mathbf{S}\_z|^{a\_z} \text{sat}(\mathbf{S}\_z) \tag{26}$$

where,

$$sat(S) = \begin{cases} \begin{array}{ll} 1, & S > \Delta \\ \ S/\Delta, & |S| \le \Delta \\ -1, & S < -\Delta \end{array} \end{cases} \tag{27}$$

*k<sup>z</sup>* > 0, *α<sup>z</sup>* > 0, ∆ is the thickness of the boundary layer. Substitute Equation (26) into Equation (22):

$$\dot{V}\_z = \mathcal{S}\_z(-k\_z |\mathcal{S}\_z|^{a\_z} \text{sat}(\mathcal{S}\_z)) \tag{28}$$

When |*Sz*| > ∆

$$\dot{V}\_z = \mathcal{S}\_z(-k\_z |\mathcal{S}\_z|^{a\_z} \text{sat}(\mathcal{S}\_z)) = -k\_z |\mathcal{S}\_z|^{a\_z + 1} \le 0 \tag{29}$$

When |*Sz*| ≤ ∆

$$\dot{V}\_z = S\_z(-k\_z|S\_z|^{\underline{a}\_z}S\_z/\Delta) = -k\_z/\Delta|S\_z|^{\underline{a}\_z+2} \le 0\tag{30}$$

At this time, the input forces for all 4 degrees of freedom are calculated as

$$\begin{aligned} \tau\_1 &= m\_1 \Big( -\frac{m\_2 v r}{m\_1} + \frac{d\_1 u}{m\_1} + \ddot{\mathbf{x}}\_d - c\_x \dot{e}\_x - k\_x |\mathbf{S}\_x|^{a\_x} \text{sat}(\mathbf{S}\_x) \Big) \\ \tau\_2 &= m\_2 \Big( -\frac{m\_1 u r}{m\_2} + \frac{d\_2 v}{m\_2} + \ddot{y}\_d - c\_y \dot{e}\_y - k\_y |\mathbf{S}\_y|^{a\_y} \text{sat}(\mathbf{S}\_y) \Big) \\ \tau\_3 &= m\_3 \Big( \frac{d\_3 w}{m\_3} + \ddot{z}\_d - c\_z \dot{e}\_z - k\_z |\mathbf{S}\_z|^{a\_z} \text{sat}(\mathbf{S}\_z) \Big) \\ \tau\_4 &= m\_4 \Big( -\frac{(m\_1 - m\_2) uv}{m\_4} + \frac{d\_4 w}{m\_4} + \ddot{r}\_d - c\_r \dot{e}\_r - k\_r |\mathbf{S}\_r|^{a\_r} \text{sat}(\mathbf{S}\_r) \Big) \end{aligned} \tag{31}$$

## 4.2.2. Target Line Tracking

To track the target line, the Line of Sight (LoS)-based guidance approach is used for calculating the heading angles of the AUV as shown in Figure 7:

**Figure 7.** The sketch for LoS guidance.

In the area far away from the DS, the underwater AUV is generally controlled by following a straight line. The design process of the target line controller is basically the same as that of the target point controller, but the method of obtaining the desired heading angle is different, which is obtained by the line of sight angle. In the process of following the target straight line, the forward thrust of the propeller is set to a fixed value to make AUV sail at a relatively stable forward speed, and the lateral thrust is set to 0. Therefore, the controller is designed only for the heading angle and depth in the motion process to make AUV realize the fixed depth direct motion.

In accordance with the previous section, the following control rates can be obtained by using the saturation function-based power reaching law sliding mode control method.

$$\begin{aligned} \tau\_1 &= a\tau\_2 = 0\\ \tau\_4 &= 0 \tau\_3 = m\_3 \left( \frac{d\_3 w}{m\_3} + \ddot{z}\_d - c\_z \dot{e}\_z - k\_z |\mathcal{S}\_z|^{\alpha\_z} \text{sat}(\mathcal{S}\_z) \right) \\ \tau\_4 &= m\_4 \left( -\frac{(m\_1 - m\_2) \mu v}{m\_4} + \frac{d\_4 w}{m\_4} + \ddot{r}\_d - c\_r \dot{e}\_r - k\_r |\mathcal{S}\_r|^{\alpha\_r} \text{sat}(\mathcal{S}\_r) \right) \end{aligned} \tag{32}$$

where *a* is a constant, *r<sup>d</sup>* is the desired heading angle, namely *ψ<sup>d</sup>* .

#### **5. Thrust Allocation**

Usually, the motion of an AUV mainly depends on the driving force generated by its actuator propeller, rudder, and wing. However, the AUV studied in this paper is not equipped with a rudder and wing mechanism. Therefore, the thrust generated by the propeller has become the main driving force and the main parameter of motion control. AUV is equipped with 8 thrusters, which are over-actuated and have the characteristics of vector arrangement. So, it needs to adopt an appropriate thrust control allocation strategy to obtain the optimal control performance index. The model of the thrusters and thrust control allocation will be discussed separately in this chapter.

#### *5.1. The Model of the Thrusters*

The layout of the 8 thrusters of the AUV is shown in Figure 8 and Table 1. No. 1–4 thrusters are vertically arranged to provide the maximum heave force and No. 5–8 thrusters are arranged at a certain angle with the ox axis to provide greater translation force and yaw moment. This redundant thrust configuration design can greatly increase the reliability of the system. If it is defined as such that *α* = - *α*<sup>1</sup> . . . *α<sup>p</sup>* T <sup>∈</sup> <sup>R</sup>*<sup>p</sup>* represents the angle of azimuth thruster, and the designed underwater UUV has no azimuth thruster, so the thrust allocation matrix *K*(*α*) = *K* is a constant.

**Figure 8.** The vectoral configuration of thrusters of AUV.

For the 4 DOF dynamic model constructed in Equation (13), the calculating formula of thrust allocation matrix is given in Equation (36) where *θ* Represents the rotation angle of the propeller around the Z-axis of the appendage coordinate system, *φ* It represents the angle of thruster thrust direction relative to XY plane of appendage coordinate system, and X, Y, and Z represent the position of thruster relative to origin, respectively.


**Table 1.** Location and azimuth of the thrusters.

$$\begin{aligned} \mathbf{K}\_{l} = \begin{bmatrix} \text{Surge} \\ \text{Sway} \\ \text{Heave} \\ \text{Yaw} \end{bmatrix} = \begin{bmatrix} \cos(\theta)\cos(\phi) \\ \sin(\theta)\cos(\phi) \\ \sin(\phi) \\ -\mathbf{Y}\cos(\theta)\cos(\phi) + \mathbf{X}\sin(\theta)\cos(\phi) \end{bmatrix} \end{aligned} \tag{33}$$

The thrust allocation vector of each propeller can be calculated by Equation (33) combined with the installation position and angle of the propeller. The installation position and angle of the propeller are shown in the table below, where *L*1 = 0.144 m, *L*2 = 0.106 m, *L*3 = 0.1 m, *L*4 = 0.1 m, *Z*1 = 0.1 m, *Z*2 = 0.1 m, in addition, *L* = q *L* 2 <sup>1</sup> + *L* 2 2 ,*α*= 10.4◦ . It is used in the calculation of yaw moment. The thrust allocation matrix obtained by combining the thrust allocation vectors of eight propellers is shown in Equation (34).

$$\mathbf{K} = \begin{bmatrix} \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 & 0 & 0\\ \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & -1 & -1 & 1\\ L \times \cos \mathfrak{a} & -L \times \cos \mathfrak{a} & L \times \cos \mathfrak{a} & -L \times \cos \mathfrak{a} & 0 & 0 & 0 & 0 \end{bmatrix} \tag{34}$$

## *5.2. Thrust Allocation Optimization*

The AUV aimed for autonomous homing and docking is an over-actuated underwater vehicle. How to allocate thrust for each thruster to achieve a certain objective such as minimum energy or power requirement becomes a realistic optimization problem. So the thrust allocation scheme for the desired control quantity of the controller output is not unique. This section mainly focuses on the thrust allocation problem of only four thrusters with a horizontal arrangement and how to achieve the optimal thrust allocation strategy.

#### 5.2.1. Problem Statement

For the thrust allocation problem, the most common method is the pseudo-inverse allocation method, which has the greatest advantage of fast calculation speed and high real-time performance. However, because its basic idea is the least square optimization problem, it does not consider the problem of thrust saturation. Under some extreme control conditions, the thrust saturation happens and cannot meet the thrust demand. As a result, the final motion control effect may be changed.

Compared with the pseudo-inverse method, the quadratic programming(QP) method can effectively consider that the thrust generated by each thruster even if it is bounded, and introduce the command error term while considering the power consumption so that AUV can still make the corresponding response and guarantee the output of the thruster meet the command of the controller as much as possible when it cannot meet the command of the motion controller to achieve positioning and tracking.

As introduced in Xiang and Xiang [4], the objective function needs to be defined first. The objective function of thrust allocation optimization can be established as the following formula:

$$\begin{array}{ll}\min & \frac{1}{2} f^T H f + s^T W s\\ & s.t. \ \ \tau\_d - Kf = s\\ & f\_{\min} \le f \le f\_{\max} \end{array} \tag{35}$$

In Equation (35), the first part is the objective function, in which the first term is to achieve the minimum power, and the second term is to achieve the minimum error between thrust output and expected control command. *H* is the weighted coefficient matrix of the power consumption term, and *W* is the weighted coefficient matrix of the error term. The command error is as small as possible, so the value of W is much larger than H. The second part is the limit function which provides the upper and lower limits of command error calculation and thrust forces.

#### 5.2.2. QP Optimization

According to the definition of a quadratic programming problem, the optimization objective function established in the previous section belongs to a linear convex optimization problem, so the active set method is used to solve it. Equation (35) is then transformed into standard secondary planning as follows:

$$\begin{array}{l} \min \frac{1}{2} f^T \boldsymbol{\Lambda} f + \boldsymbol{c}^T f \\ \text{s.t.} \quad f\_{\text{min}} \le f \le f\_{\text{max}} \end{array} \tag{36}$$

where, **Λ** = *H* + 2*K <sup>T</sup>WK*, *<sup>c</sup>* <sup>=</sup> <sup>−</sup>2*<sup>K</sup> <sup>T</sup>Wτ<sup>d</sup>*

The active set method is an iterative method. After each iteration, the active set containing the optimal solution is predicted in the next step. The active set is some inequality constraints that contain the optimal solution. After each iteration, the information of the active set are used to calculate a new iteration direction and find the optimal solution in this direction or give a new iteration direction to the optimal solution through the calculation in this direction until the optimal solution is given.

.

#### **6. Simulation and Analysis**

It can be seen from the task planning scheme in Section 3.1 that the path planning for AUV homing and docking tasks mainly involves the planning of the target point and target line in the AUV path. To verify the effect of the two tracking methods, the simulation experiments are carried out from the point tracking and the line tracking. In the simulation, the model parameters M = diag (9.91, 25.8, 20.61, 0.28), D = diag (34.69, 103.25, 74.23, 0.43) were selected. The continuous process of target line tracking and target point tracking in the homing and docking tasks is simulated based on the proposed sliding mode controller, QP thrust allocation optimization method, LOS, and vision-based guidance algorithm.

Assumed simulation environment: initial position and attitude of underwater AUV P0 (0 m, 0 m, −10 m, 0◦ ), In the process, because of the different sensors and sensors in different stages, simultaneous interpreting of multiple target points is carried out in the simulation process. Set the points obtained during the movement as follows:


 <sup>3</sup> P5 (640.75 m, 848.68 m, −30 m, −7.03◦ ), This point is the location information of the DS after the AUV arrives at P4 and performs fine target resolution by vision.

**Figure 9.** The original image was captured by the camera at P4 10 m above the DS and the extraction image of the LED light array.

**Table 2.** Visually processed data (=10 m).


Through the two points of P0 and P1, a straight line is planned in the horizontal plane to track. After judging that it enters a certain range of P1 points, it switches to target point tracking, to track P2, P3, P4, and P5 points one by one. In this process, it also judges that it switches to the next target point to track when it reaches a certain range of the current target point.

In the simulations, the sliding mode controller parameters are small parameters, which gradually increase the control effect from divergence to final convergence to obtain a set of more appropriate parameters. For the weight parameters in the thrust allocation problem, a set of parameters is selected empirically under the condition that the weight of the error term is much greater than that of the power consumption term. The time step is set to 0.2 s and settings for parameters required by sliding mode controller design are listed:

<sup>1</sup> Parameters for target line tracking

$$k\_z = 1.4; k\_r = 0.5;$$

$$c\_2 = 1.2; c\_r = 2.2;$$

*α<sup>z</sup>* = 0.8; *α<sup>r</sup>* = 0.8; ∆ = 0.1; *ke* = 0.2;


## *H* = 1; *W* = 10, 000;

$$\begin{array}{l l l} f\_{\text{min}} = \begin{bmatrix} & -40 & -40 & -40 & -40 & \end{bmatrix}^T; \\ f\_{\text{max}} = \begin{bmatrix} & 40 & 40 & 40 & 40 & \end{bmatrix}^T; \end{array}$$

#### *6.1. Simulation Results*

In the visual guidance process, the simulation obtains the image of the LED light array with a certain posture and carries on the visual processing, and the result is shown in Figure 9 and Table 2. The simulation results of control are shown from Figures 10–14:

**Figure 10.** Homing and docking trajectory of the AUV.

**Figure 11.** Time history of the position of the AUV.

**Figure 12.** Time history of the velocity of the AUV.

**Figure 13.** Time history of input force to control the AUV.

**Figure 14.** Time history of the in-plane thrust generated by each thruster of the AUV.

#### *6.2. Discussion and Analysis*

In the visual guidance strategy, the image processing effect is shown in Figure 9. This processing can accurately obtain the center position of the light array, and then calculate the relative position and UUV yaw angle between the docking station and the UUV. The absolute errors of the relative position and yaw angle calculated according to the obtained light array position and the real position of the light array respectively are less than 5%.

Figure 10 shows the space navigation trajectory of the whole process of AUV homing and docking. From the figure, it can be seen that AUV can stably track the target line and target point through different controller methods: sliding mode controller and PID controller.

Figures 11 and 12 show the specific change process of AUV's position, attitude, and speed with four degrees of freedom controlled. Compared with the tracking effect using the PID controller, the sliding mode controller does not present a significant difference at the stage of tracking the straight-line, but in the later point tracking stage, the sliding mode method can achieve faster and smoother tracking to the target endpoint. By using the sliding mode control method, AUV can reach the endpoint at 900 s which is taking much less time than 1400 s by PID controller. The sliding mode method can also achieve a much more smooth tracking effect in the process of switching from tracking target line to tracking target points and tracking between different target points, and finally, reach the end position with the expected heading angle more accurately. At the end of 1500 s 'simulations, AUV controlled by the sliding mode method can achieve X-direction error less than 0.01 m, Y-direction error less than 0.01 m, Z-direction error less than 0.01 m, and heading angle psi error less than 0.1◦ . However, in the PID controller simulations, the x-direction error of AUV is less than 1 m, the y-direction error is less than 1.5 m, the zdirection error is less than 0.3 m, and the heading angle psi error is less than 0.5◦ . Although the simulation time continues to increase, the tracking error of the control method may be further reduced, but more control time is obviously not desirable in practical application.

Figure 13 shows the change process of the expected control quantity of four degrees of freedom. It can be clearly seen that the jump of the control force is caused by the sudden change of tracking error during the switching between target lines to points. However, the final control force is 0 stable at the end position. Compared with PID controller simulation tests, the output torque of yaw freedom of the sliding mode method is significantly lower, resulting in more effective heading control. Figure 14 shows the thrust change process of four thrusters in the horizontal plane. The four thrusters distribute the three degrees of freedom control quantities in the horizontal plane. After the final stability, the thruster output is 0.

Based on the above comparative analysis, it can be concluded that the proposed sliding mode control strategy has the advantages of fast response and strong adaptability to different state tracking parameters. What is more, the sliding mode control strategy can realize the switching process of tracking target line to target point much faster and more accurately. In a word, the simulated results presented in Figures 9–14 demonstrate that the whole homing and docking tasks can be well completed using the proposed unified approach. So, this will be the motivation for the wide practical use of the proposed approach in the future.

#### **7. Conclusions**

This paper mainly focuses on providing a complete solution to the problem of autonomous homing and docking of an over-actuated AUV using a unified approach that involves task planning, guidance and control design, thrust allocation, etc.

Firstly, the AUV is simplified into a four degree of freedom dynamic model according to the actual task needs. The appropriate dynamic model can effectively reflect the actual motion state of AUV, provide convenience for motion control design, and effectively guarantee the handling performance in practical application.

To provide reliable information for the docking of AUV, a vision-based algorithm is used for the guidance of AUV. The LED light array is designed and arranged on the docking station, and a complete set of visual information processing flow including image preprocessing, LED light array position extraction and relative pose analysis is established.

In the motion process of AUV, the target line or target point tracking mode can be adopted based on tasks. The sliding mode control method based on power approaching and saturated boundary layer together with the quadratic programming based thrust allocation method are designed to output the command of each thruster, to ensure the stability of the AUV tracking process.

Finally, to prove the feasibility of the algorithm, the visual algorithm and control algorithm are simulated and verified.

**Author Contributions:** Conceptualization, G.X. and M.Z.; methodology, M.Z. and G.W.; software, G.W. and Y.X.; investigation, G.X. and G.W.; resources, G.X.; writing—original draft preparation, G.X.; writing—review and editing, M.Z. and Y.X. All authors have read and agreed to the published version of the manuscript.

**Funding:** This work was funded by the Open Foundation of the State Key Laboratory of Fluid Power and Mechatronic Systems, grant number GZKF-202001 and the Fundamental Research Funds for the Central Universities, grant number 2021XJJS016.

**Institutional Review Board Statement:** Not applicable.

**Informed Consent Statement:** Not applicable.

**Data Availability Statement:** Not applicable.

**Conflicts of Interest:** The authors declare no conflict of interest.

#### **References**

