USV Application Scenario Expansion Based on Motion Control, Path Following and Velocity Planning

Abstract

The ability of unmanned surface vehicles (USV) on motion control and the accurate following of preset paths is the embodiment of its autonomy and intelligence, while there is extensive room for improvement when expanding its application scenarios. In this paper, a model fusion of USV and preset path was carried out through the Serret-Frenet coordinate system. Control strategies were then scrupulously designed with the help of Lyapunov stability theory, including resultant velocity control in the presence of drift angle, course control based on the nonlinear backstepping method, and reference point velocity control as a virtual control variable. Specifically, based on USV resultant velocity control, this paper proposes respective solutions for two common scenarios through velocity planning. In a derailment correction scenario, an adaptive reference velocity was designed according to the position and attitude of USV, which promoted its maneuverability remarkably. In a dynamic obstacle avoidance scenario, an appropriate velocity curve was searched by dynamic programming on ST graph and optimized by quadratic programming, which enabled USV to evade obstacles without changing the original path. Simulation results proved the convergence and reliability of the motion control strategies and path following algorithm. Furthermore, velocity planning was verified to perform effectively in both scenarios.

1. Introduction

The ocean is one of the world’s most valued resource treasury, and ships have been an indispensable tool for human beings to approach and explore the ocean since ancient times. With the gradual increase of our desire to explore the unknown areas of the ocean and the complex water environment, people’s requirements for the performance of ships are also correspondingly strengthened, and the development of automation and intelligence of ships is one of the important areas that have received keen attention [1]. Among them, autonomy is an important attribute that USV can address by adaptively adjusting the output of its execution unit through a closed-loop control to achieve an ideal motion state. Intelligence is reflected in the USV’s ability to perceive and identify the environment, and respond through optimal decisions to adapt to the needs in that scenario. At present, due to its advantages and functions, USVs are gradually playing an irreplaceable role in scenarios such as water sampling and monitoring, anti-submarine warfare, target tracking, disaster management, and maritime search and rescue [2,3,4]. Any application scenario of USV is inseparable from the realization of the underlying motion control, whose goal is to control the movement of the USV in the spatial position. As the representative of USV motion control, path following is a basic function that mostly reflects the autonomy of USV. Undoubtedly, the fundamental research and design of a USV will determine the level of sophistication that can be achieved in such advanced devices integrating unmanned technology and artificial intelligence technology.

Modelling is the first step in USV motion control research, in which the accuracy and complexity of the model need to be taken into account. Mu D et al. [5] uses the Maneuvering Modeling Group (MMG) model to describe the motion characteristics of USV; the Nomoto model is derived to establish a transfer function describing the relationship between rudder angle and heading [6]. Thor I. Fossen has done a lot of remarkable work on USV. A maneuvering model he derived [7,8], which is widely used by scholars, intuitively reflects the maneuvering characteristics between the thrust and torque acting on the hull and the output of the hull motion state and has practical application value. Second, the USV motion control design aiming at the tracking of the preset path mainly revolves around the navigation and control algorithms. Based on the USV kinematic model, there are common guidance strategies such as Pure Pursuit, Line-Of-Sight (LOS) guidance [9] and Vector Field (VF) guidance [10]. Among them, LOS guidance is the most commonly used navigation method, and many researchers have improved it. Su Y et al. [11] considered both the time-varying sideslip angle and the time-varying ocean currents and proposed an Improved Adaptive Integral LOS (IAILOS). The control algorithm is based on the USV kinetic model. Common control strategies include PID control [12] and model predictive control (MPC) [13], which are more applicable in practice. In addition, robust controllers such as backstepping methods [14] and sliding mode control [15] are more accurate for nonlinear systems. Many scholars have comprehensively used the above methods and introduced new methods and ideas to carry out some optimization and improvement work. Li L et al. [16] proposed the extended state observer Line of Sight (ESO-LOS) guidance law, based on the extended state observer (ESO) prediction and compensation of the sideslip angle. Yu Y et al. [17] used finite time observers (FTOB) to predict the sideslip angle and kinetic uncertainties. Liu T et al. [18] established a ship tracking error model in the Serret-Frenet coordinate frame and proposed an improved LOS guidance algorithm using the tangential velocity of the desired path as the control input. Mu D et al. [19] designed an improved adaptive line-of-sight (ALOS) guidance law suitable for curve navigation based on the fuzzy optimization algorithm. It can be deduced that the drift angle of the USV motion is fully considered to make the results more realistic, and the Serret-Frenet coordinate frame can greatly simplify and clarify the path following the problem of the USV.

Based on the realization of motion control and path following, a USV can expand its application scenarios. A common situation is that the USV is likely to encounter moving obstacles such as other ships, offshore equipment and floating objects when navigating along a preset path on a relatively busy and complex water surface. Under certain constraints, it is of great significance to study the avoidance strategy of USV for these dynamic obstacles that violate its movement route. Song Lifei et al. [20] combined the velocity obstacle (VO) algorithm and the improved artificial potential field (APF) method to form a new two-level dynamic obstacle avoidance algorithm, in which emergency and non-emergency obstacle avoidance situations are carefully distinguished according to the standard whether the vehicle should move away from the obstacle immediately or not, so it can react with different situations efficiently. Ren J et al. [21] further considered the obstacle avoidance behaviour and the velocity obstacle region on the traditional velocity obstacle (VO) method, and further improved the method to make it easier for the ship to take avoidance actions. Faced with obstacle avoidance in complex unknown environments, Liu X et al. [22] combined Ant Colony Algorithm (ACA) and the Clustering Algorithm (CA) to propose a new auto-obstacle avoidance method, which can make full use of limited computing resources to automatically adjust and select smooth paths to avoid obstacles. Xia G et al. [23] determined the velocity window and course window of the USV through the dynamic window approach (DWA) and planned the ship’s local path to avoid collision based on modified quantum particle swarm optimization (MQPSO), which satisfies some rules of the International Regulations for Preventing Collisions at Sea (COLREGs). Chen Y et al. [24] proposed an improved ant colony optimization artificial potential field (ACO-APF) algorithm that takes into account both global and local path planning, and plans for dynamic obstacle avoidance during each step length analysis, ensuring that the hull is warned as soon as possible to avoid its action. Xia G et al. [25] proposed the nonlinear finite-time velocity obstacle (NLFVO) method to avoid obstacles whose motion laws cannot be determined. This method will analyze and predict the velocity state of the obstacles and select collision-free velocity for the USV based on the Gaussian mixture model (GMM) and Gaussian process regression (GPR). In the existing research, after evaluating the safe obstacle avoidance speed of the ship through methods such as velocity obstacle (VO), dynamic window approach (DWA) and artificial potential field (APF), the dynamic avoidance is generally completed by re-planning the path. However, this will obviously make the originally optimized preset path lose part of its meaning, and it is kind of inadvisable in some busy or narrow water scenes.

Thanks to a large number of excellent works by scholars, this paper can systematically study and discuss the motion control and path-following problems of USV, as well as the expansion of application scenarios based on velocity planning. Figure 1 is an illustration for the research framework for the concerned issues about USV established in this study. In this paper, the Serret-Frenet coordinate system is used in model processing, and the preset path and the motion of USV are combined into a new path following the error system. Then the system is decoupled into three aspects to carry out control strategies designed with the help of Lyapunov stability theory. Specifically, USV resultant velocity control in the presence of drift angle, USV course control based on nonlinear backstepping controller and LOS guidance law, and reference point velocity control as a virtual control variable are all carefully considered. Based on realizing reliable and comprehensive controls, this paper proposes solutions based on velocity planning about two common scenarios, i.e., derailment correction and dynamic obstacle avoidance. The derailment correction mainly reduces the movement velocity according to the position and attitude of the USV when it deviates from the desired route in order to reduce the deviation state as much as possible. Dynamic obstacle avoidance mainly searches for the optimal velocity curve by performing dynamic programming (DP) on the path-time graph generated by obstacles’ occupation (ST graph) and uses quadratic programming (QP) to fit the result. The optimization finally enables USV to avoid obstacles and keep following the original path only by adjusting its movement velocity. All the research results of this paper have been proven and verified through theoretical derivation and numerical simulation, which can provide some ideas and methods for scientific research and promotion in the field of autonomous unmanned system technology.

2. Mathematical Models

The implementation of a reliable and comprehensive motion control strategy for USV is the basis for realizing a series of autonomous functions such as path following and obstacle avoidance. Therefore, this section firstly establishes a system model that can accurately represent the actual object and is not too complex at the same time, mainly including the USV kinetic model and kinematic model. To facilitate the research, the following hypotheses are given:

• Only 3-DOF movements of USV described by maneuvering theory are considered when modelling, i.e., surge, sway and yaw.
• The USV has no lateral push mechanism, that is, the drive input of the system only includes surge force and yaw moment.
• The USV surge velocity is constantly positive, that is, the parking and reversing of the USV are not considered.

2.1. USV Kinetic Model

The USV kinetic model reflects the maneuvering characteristics between the drive input acting on the hull and the output of the ship’s motion state. The underactuated USV kinetic model on the horizontal plane can be expressed by the simplified Fossen Model [26] that is widely used by scholars [27,28]:

$$M\dot{\upsilon }+C\left(\upsilon \right)\upsilon +D\upsilon =\tau$$

(1)

where $\upsilon =\left(\begin{array}{c}u\\ v\\ r\end{array}\right)$ is the 3-DOF motion state vector of USV in the body coordinate system {B} with $u,v,r$ representing surge velocity, sway velocity and yaw angular velocity respectively. $M=\left(\begin{array}{ccc}{m}_{11}& 0& 0\\ 0& m_{22}& 0\\ 0& 0& m_{33}\end{array}\right)$ is the inertia matrix with $m_{11},m_{22},m_{33}$ representing the inertia coefficient containing the additional mass. $C\left(\upsilon \right)=\left(\begin{array}{ccc}0& 0& -m_{22}v\\ 0& 0& m_{11}u\\ m_{22}v& -m_{11}u& 0\end{array}\right)$ is the Coriolis force-centripetal force matrix. $D=\left(\begin{array}{ccc}{d}_{11}& 0& 0\\ 0& d_{22}& 0\\ 0& 0& d_{33}\end{array}\right)$ is the damping matrix with $d_{11},d_{22},d_{33}$ representing the hydrodynamic damping coefficient. $\tau =\left(\begin{array}{c}{\tau }_u\\ 0\\ {\tau }_r\end{array}\right)$ is the driving input matrix with ${\tau }_u$ and ${\tau }_r$ representing the thrust in the surge direction and the torque in the yaw direction respectively.

2.2. USV Kinematic Model in Serret-Frenet Coordinate Frame

The USV kinematic model describes the motion of USV as a particle in geometric space. In the inertial coordinate system, it is easy to obtain the kinematic equation of a USV moving on a horizontal plane:

$$\dot{\eta }=R\left(\psi \right)\upsilon$$

(2)

where $\eta =\left(\begin{array}{c}x\\ y\\ \psi \end{array}\right)$ is the position and attitude state vector of USV in the Cartesian inertial coordinate system {I} with $x,y,\psi$ representing the coordinates and heading direction of the ship’s centre of gravity respectively. $R\left(\psi \right)=\left(\begin{array}{ccc}\cos \left(\psi \right)& -\sin \left(\psi \right)& 0\\ \sin \left(\psi \right)& \cos \left(\psi \right)& 0\\ 0& 0& 1\end{array}\right)$ is a rotation matrix determined by the heading angle of the ship.

A Serret-Frenet coordinate system is introduced in this study to facilitate and simplify the USV path following the problem. As shown in Figure 2, the reference path is a parameterized planar two-dimensional curve l, and the curve parameter is denoted as p. The Serret-Frenet coordinate frame {SF} was established by selecting a reference point on path l, taking the tangential direction of the reference point as the vertical axis T and the normal direction of the reference point as the horizontal axis N.

According to geometric relations, the kinematic equation of USV in {SF} frame is:

$$\left(\begin{array}{l}{e}_T\\ e_N\\ {\psi }_{SF}\end{array}\right)=\left(\begin{array}{ccc}\cos \left({\psi }_T\right)& \sin \left({\psi }_T\right)& 0\\ -\sin \left({\psi }_T\right)& \cos \left({\psi }_T\right)& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{l}x-x_c\\ y-y_c\\ \psi +\beta -{\psi }_T\end{array}\right)$$

(3)

where $e_T$ and $e_N$ represent the position of the USV centre of gravity relative to the origin of the Serret-Frenet coordinate system. ${\psi }_{SF}$ represents the angle between the direction of USV motion and the T-axis of the SF coordinate system. $x_C$ and $y_C$ represent the position of the path reference point relative to the origin of the inertial coordinate system, and ${\psi }_T$ represents the angle between the T-axis of the SF coordinate system and the North-axis of inertial coordinate system. $\beta$ is the drift angle of USV, indicating the angle between the actual movement direction of USV and the heading direction, which is essentially generated by the velocity of sway movement. It can be written as follows:

$$\beta =\arctan \left(\frac{v}{u}\right)$$

(4)

An error equation of the USV path following in the SF coordinate system can be obtained by differentiating the Equation (3):

$$\{\begin{array}{l}{\dot{e}}_T=C\dot{s}{e}_N-\dot{s}+U\cos \left({\psi }_{SF}\right)\\ {\dot{e}}_N=-C\dot{s}{e}_T+U\sin \left({\psi }_{SF}\right)\\ {\dot{\psi }}_{SF}=r+\dot{\beta }-C\dot{s}\end{array}$$

(5)

where $C$ is the curvature of the desired path at the reference point, $\dot{s}$ is the change rate of the reference point along the path curve, and $U$ is the resultant velocity of USV.

2.3. Problem Formulation

In this section, the kinetic model of USV, as well as the kinematic model under the {SF} frame, are established. Compared with the Cartesian coordinate frame, the data information that can characterize the preset path curve is integrated into the mathematical model of USV, making the target of the path following problem very clear and definite. It can also be noticed suggestively from the error Equation (5) that under the {SF} frame, the coordinate system is constantly changing along the path when the velocity $\dot{s}$ of the reference point is not 0. In fact, the selection of reference points can be completely arbitrary, because there is no concrete physical object to manipulate the reference point. The inspiration brought by this is that the moving velocity $\dot{s}$ of the reference point can be regarded as a virtual control variable, and an appropriate control law can be designed to make the position of the reference point change according to the expectation for the system’s stability. As a result, the original underactuated system model is supplemented into a fully-actuated one with three inputs and three outputs in terms of the quantity of inputs and outputs.

The USV path following problem can then be described as designing control laws for

$${\tau }_u,{\tau }_r,\dot{s}\begin{array}{cc}& \end{array}s.t.\begin{array}{cc}& 0\le {\tau }_u\le {\tau }_{ulimit}\end{array},\left|{\tau }_r\right|\le \left|{\tau }_{rlimit}\right|$$

(6)

to let

$$e_T,e_N,{\psi }_{SF}\to 0\begin{array}{cc}& \end{array}\forall e_{T0},e_{N0},{\psi }_{SF0}$$

(7)

where two control variables ${\tau }_u$ and ${\tau }_r$ from USV actuators have output limits, and $e_{T0}$, $e_{N0}$, ${\psi }_{SF0}$ are the initial state of USV. $e_T$, $e_N$ and ${\psi }_{SF}$ all converging to zero means that USV will finally move to the origin of the coordinate system and keep its forward direction consistent with the vertical axis under the view of {SF} frame.

3. Control Strategies

Based on introducing a virtual control variable, the system model of the USV path following is still a strongly coupled and nonlinear research object. This paper mainly uses Lyapunov stability theory, with the help of the idea of nonlinear backstepping controller design and LOS guidance design, to develop relatively independent control strategies under the {SF} frame: First, the control strategy of USV thrust with fully considering the existence of drift angle is designed, so that the real moving velocity of USV can be controlled; Second, the control strategy of USV torque is designed so that the USV moving course can follow the expected course generated by the guidance law. Thirdly, the control strategy of reference point movement is designed to ensure that the position of USV can converge to the reference point which is precisely on the curve path.

3.1. Resultant Velocity Controller Design

Different from other studies that set the USV velocity as a constant by default, or fixed the thrust output as a constant, or only controlled the surge velocity u, resultant velocity controller (RVC), without ignoring the drift angle, drives the USV according to the desired actual velocity by controlling the thrust force based on the full modelling of the USV system. This also makes it possible to optimize the USV path following and dynamic obstacle avoidance based on velocity planning.

Without ignoring the drift angle, that is, taking full account of the sway velocity, the derivative of $U$ can be achieved:

$$\stackrel{·}{U}=\frac{u\dot{u}+v\dot{v}}{\sqrt{u^2+v^2}}$$

(8)

Substituting the surge acceleration $\dot{u}$ and the sway acceleration $\dot{v}$ from USV kinetic Equation (1) into Equation (8) leads to:

$$\stackrel{·}{U}=\frac{1}{U}\left(\left(\frac{m_{22}}{m_{11}}-\frac{m_{11}}{m_{22}}\right)uvr-\frac{d_{11}}{m_{11}}{u}^2-\frac{d_{22}}{m_{22}}{v}^2+\frac{u}{m_{11}}{\tau }_u\right)$$

(9)

Applying the Lyapunov Direct Method on Equation (9) and the specific derivation process can be seen in Appendix A. Then the control law of RVC is designed as:

$${\tau }_u=\frac{m_{11}{}^2-m_{22}{}^2}{m_{22}}vr+d_{11}u+\frac{m_{11}{d}_{22}}{m_{22}}\frac{v^2}{u}+\frac{m_{11}U}{u}\left({\dot{U}}_{RVC}-K_U\left(U-U_{RVC}\right)\right)$$

(10)

where the positive parameter $K_U$ is the controller gain.

3.2. Nonlinear Backstepping Course Controller Design

The main function of the nonlinear backstepping course controller (NBCC) is to track the expected value by controlling the USV torque. In this study, the LOS algorithm, which is widely used in path-following problems, is used to generate the desired course ${\psi }_{LOS}$:

$${\psi }_{LOS}=\arctan \left(\frac{-e_N}{\Delta }\right)$$

(11)

where $\Delta$ is a parameter representing the forward-looking distance in LOS guidance law. This traditional LOS algorithm is simple and reliable for solving the path following problem with intuitive geometric significance. Its essence is a kind of planning for the convergence process of course tracking, which guides the object to approach the preset path by establishing the constraint between transverse error and course.

The backstepping method is a design idea that realizes the stabilization of the chain of integrator system by constructing Control-Lyapunov Function (CLF) recursively. At the same time, the course control problem of the USV path following can be solved reliably by using feedback linearization control law design. According to the USV mathematical model, yaw angular velocity $r$ can be controlled by changing the torque, which is also the main variable affecting the course ${\psi }_{SF}$ of USV. The specific derivation process can be seen in Appendix B. The course control law for the path following problem is:

$$\begin{array}{c}{\tau }_u=m_{33}\left(1+K_{r1}{K}_{r2}\right)\left({\psi }_{LOS}-{\psi }_{SF}\right)+m_{33}\left(K_{r1}+K_{r2}\right)\left({\dot{\psi }}_{LOS}-{\dot{\psi }}_{SF}\right)\\ +m_{33}{\ddot{\psi }}_{LOS}-\left(m_{11}-m_{22}\right)uv+d_{33}r-m_{33}\ddot{\beta }+m_{33}{\ddot{\psi }}_T\end{array}$$

(12)

where both positive parameters $K_{r1}$ and $K_{r2}$ are the controller gains.

3.3. Path Reference Point Controller Design

The selection of {SF} frame provides an additional virtual control variable for the USV path following system in kinematics, i.e., the moving velocity of path reference point $o_{sf}$. The essential purpose of controlling the moving velocity of the path reference point is to enable USV to select the appropriate reference point on the path in real-time and establish an SF coordinate system based on this point. In this coordinate system, together with USV moving velocity and course control, the established kinematic error equation can converge to zero. Its geometric significance is to clarify the relative position and angle differences between the USV and the preset path and provide a reference for the positioning of the forward-looking distance in LOS guidance.

According to the error Equation (5), with the help of the Lyapunov Direct Method, the control law of the reference point’s movement velocity is:

$$\dot{s}=U\cos \left({\psi }_{SF}\right)+K_s{e}_T$$

(13)

where the positive parameter $K_s$ is the controller gain.

It should be pointed out that the natural parameter equation of curve is often difficult to obtain, but for the two-dimensional curve equation expressed by general parameter $\{\begin{array}{c}{x}_c=x_c\left(p\right)\\ y_c=y_c\left(p\right)\end{array}$, there is an operational relationship between arc length s and parameter p:$dp=\frac{1}{\sqrt{{x^{\prime }}_c{\left(p\right)}^2+{y^{\prime }}_c{\left(p\right)}^2}}ds$. Substitute it into Equation (13) and the control law regarding the velocity of path curve parameter as the control variable can be obtained:

$$\dot{p}=\frac{U\cos \left({\psi }_{SF}\right)+K_s{e}_T}{\sqrt{{x^{\prime }}_c{\left(p\right)}^2+{y^{\prime }}_c{\left(p\right)}^2}}$$

(14)

where ${x^{\prime }}_c$ and ${y^{\prime }}_c$ are the derivatives of the curve equation with respect to the parameter p.

4. Stability Analysis

In Section 3 of this paper, the corresponding control law is designed regarding the thrust and torque of USV and the selection of reference points in the SF coordinate system. This section mainly analyzes the stability of the designed control law based on the Lyapunov stability criterion and finally proves the convergence of USV motion control and path following.

4.1. Stability Analysis of Velocity Control

The error between the resultant velocity of USV and the expected value in RVC is written as $e_U$:

$$e_U=U_{RVC}-U$$

(15)

Differentiating the equation above and substituting the result into Equation (9) and control law (10) leads to:

$${\dot{e}}_U=-K_u{e}_U$$

(16)

It obviously comes to a time-domain solution that decays exponentially to zero when the parameter $K_u$ is positive, which indicates that $U$ will eventually approach $U_{RVC}$. It proves that USV can advance according to the expected velocity value given by the outer loop in actual motion.

4.2. Stability Analysis of Course Control

Based on the derivation process of backstepping presented in Appendix B, a new system was constructed using the error equation of USV motion direction and angular velocity:

$$\left(\begin{array}{l}{\dot{e}}_1\\ {\dot{e}}_2\end{array}\right)=A_{2\times 2}\left(\begin{array}{l}{e}_1\\ e_2\end{array}\right)=\left(\begin{array}{cc}-K_{r1}& 1\\ -1& -K_{r2}\end{array}\right)\left(\begin{array}{l}{e}_1\\ e_2\end{array}\right)$$

(17)

It can be found that the USV course error system becomes a linear system with an equilibrium point of ${\left({\dot{e}}_1,{\dot{e}}_2\right)}^{\mathrm{T}}={\left(e_1,e_2\right)}^{\mathrm{T}}={\left(0,0\right)}^{\mathrm{T}}$ after adopting the backstepping method combined with feedback linearization in controller design. Upon analyzing the eigenvalue of the system matrix $A_{2\times 2}$, there is:

$$\{\begin{array}{c}{\lambda }_1+{\lambda }_2=\mathrm{tr}\left(A_{2\times 2}\right)=-K_{r1}-K_{r2}<0\\ {\lambda }_1{\lambda }_2=\det \left(A_{2\times 2}\right)=K_{r1}{K}_{r2}+1>0\end{array}$$

(18)

When the parameters $K_{r1}$ and $K_{r2}$ are positive, the solutions of the eigenvalues ${\lambda }_1$ and ${\lambda }_2$ of the error system matrix all have negative real parts, which indicates the fact that the linearized USV course error system is a stable system that can converge to zero, which proves that USV can track the desired course in actual motion.

4.3. Stability Analysis of USV Path Following

Based on the algorithm design of USV thrust and torque output, the actual movement velocity and direction are stably controlled at the kinetic level, that is, the states of USV motion are respectively satisfied $U\to U_{RVC}$ and ${\psi }_{SF}\to {\psi }_{LOS}$ with convergence proved. On this basis, designing the control law of the reference point in the SF coordinate system let the error variables in the system (5) converge stably to zero, which enables the USV motion path to track the preset curve from the kinematic level. The proof is as follows:

Define a Lyapunov function $V_{SF}\left(e_T,e_N\right)$:

$$V_{SF}\left(e_T,e_N\right)=\frac{1}{2}{e}_T{}^2+\frac{1}{2}{e}_N{}^2$$

(19)

Differentiating the equation above and substituting the control strategy (13) of reference point velocity leads to:

$${\dot{V}}_{SF}=-K_s{e}_T{}^2+e_N{U}_{RVC}\sin \left({\psi }_{LOS}\right)$$

(20)

And according to the designed LOS guidance law (11), there is:

$$\sin \left({\psi }_{LOS}\right)=\frac{-e_N}{\sqrt{{\Delta }^2+e_N{}^2}}$$

(21)

Substituting the Equation (21) into Equation (20) leads to:

$${\dot{V}}_{SF}=-K_s{e}_T{}^2-\frac{U_{RVC}}{\sqrt{{\Delta }^2+e_N{}^2}}{e}_N{}^2$$

(22)

On the whole, the error Equation (5) of USV path following under {SF} frame has equilibrium point of ${\left({\dot{e}}_T,{\dot{e}}_N,{\dot{\psi }}_{SF}\right)}^{\mathrm{T}}={\left(e_T,e_N,{\psi }_{SF}\right)}^{\mathrm{T}}={\left(0,0,0\right)}^{\mathrm{T}}$. Obviously, when $K_s$ and $U_{RVC}$ both have positive values, $V_{SF}\left(e_T,e_N\right)$ is positive definite and ${\dot{V}}_{SF}\left(e_T,e_N\right)$ is negative definite. Then the USV path following the error system converges stably to the equilibrium point when applying the Lyapunov stability theory, which ultimately proves that USV can move from any initial state to the preset path curve in the inertial coordinate system.

5. Scenario-Oriented Velocity Planning

In this study, the resultant velocity of USV can be controlled to adjust adaptively with the change of given expected value by the design of RVC. Taking advantage of this feature, velocity planning can be carried out for a USV to realize various functions of autonomous movement and meet the needs of different scenarios.

5.1. Velocity Planning for Derailment Correction Based on USV Position and Attitude

Derailment generally refers to the fact that the USV deviates from the preset path to be tracked. There are two common adverse situations: First, the initial position and attitude of the USV are far from the starting point of the path to be tracked; Second, the course or even position of the USV deviates from the original path when the USV encounters sudden external disturbances during driving. For ships with underactuated properties, reducing the velocity appropriately will be beneficial to contract the turning radius under the condition of limited steering torque input, which is helpful to complete derailment correction in a small scale of space. This paper gives a scheme of USV velocity planning for derailment correction based on its position and attitude to enhance the path following performance. The reference velocity value satisfies the following:

$$U_{RVC}=z_1{e}^{-\left(z_2{\psi }_{SF}^2+z_3{e}_N^2\right)}+z_4$$

(23)

where parameter $z_1$ represents the variation range of the reference velocity. $z_2$ and $z_3$ are weight parameters of USV deviate angle and distance respectively. Parameter $z_4$ represents the lowest velocity of USV. All the above parameters are positive. With the design on reference velocity of RVC, the USV’s resultant velocity can be adjusted adaptively to the current position and attitude within a certain range. Specifically, when the deviate angle or distance of the USV increases under {SF} frame, RVC will reduce the reference value of the USV’s moving velocity until the lowest value $z_4$; When the USV position converges to the preset path, the RVC increases the expected velocity to the value $z_1+z_4$, which is also exactly the cruising velocity of the USV along the preset path.

5.2. Velocity Planning for Dynamic Obstacle Avoidance Based on ST Graph Optimization

The ST graph takes time as the horizontal axis and the distance along the preset path curve as the vertical axis. The dynamic obstacle that may overlap with the path is projected onto the path, so as to generate the corresponding ST boundary. The area enclosed is the two-dimensional expression of the path length and time occupied by this dynamic obstacle. All obstacles that may encroach on the path can be in accordance with the rules of geometry to solve their projection into the ST graph, so as to determine the occupied range of time and space of the path. Therefore, as long as a monotone increasing curve that bypasses all obstacle blocks as well as satisfies certain conditions of constraints and optimization can be searched out in the ST graph, obstacle avoidance of unmanned mobile devices can be safely ensured. The derivative of such searched curve is exactly the velocity curve, and the searching process is the velocity planning process facing the dynamic obstacle scenario.

In this study, the projection of the dynamic obstacle on the ST graph is obtained by solving the implicit function:

$$f_{implicit}\left(s,t\right):{\left(Y_{obs,i}-Y_{USV}\right)}^2+{\left(X_{obs,i}-X_{USV}\right)}^2-{\left(R_{obs,i}+R_{USV}\right)}^2=0$$

(24)

where $Y_{obs,i}$ and $X_{obs,i}$ describe the real-time position of the ith obstacle. $Y_{USV}$ and $X_{USV}$ describe the real-time position of USV. $R_{obs,i}$ and $R_{USV}$ represents the collision radius of obstacles and USV, respectively.

The feasible region and obstacle region for searching are divided according to the conservative principle after rasterizing the ST graph with appropriate accuracy. Meanwhile, the upper and lower limits of USV velocity and acceleration were used as constraints to further limit the feasible region for searching. At this point, the route search of the discretized ST graph is transformed into an optimization problem to solve the multi-stage decision process. This paper adopts the dynamic programming (DP) algorithm to solve the problem, and defines the cost function between each connected state according to the following formula:

$$cos{t}_{DP}={\omega }_{DP1}\times ac{c}_{DP}+{\omega }_{DP2}\times jer{k}_{DP}$$

(25)

where $ac{c}_{DP}$ is the acceleration of USV actual velocity, $jer{k}_{QP}$ is the jerk of movement, ${\omega }_{DP1}$ and ${\omega }_{DP2}$ are their weight coefficients, respectively.

The result from DP is optimal under the selected dispersion with respect to the smoothness of velocity variation, but the derivative of the searched curve is often discontinuous, which means that the reference value of USV movement velocity will jump. USV then will go through a transitional stage of breaking away from the desired state in the actual situation, resulting in the risk of being out of control. In this study, a series of quintic polynomial curves are used to smooth the curve searched by DP. In addition to the constraints from position, velocity and acceleration, boundary condition constraints of the start point of the ST curve and the connection points of each stage are also considered. The cost function containing acceleration, jerk and position error is defined as follows, and the corresponding coefficients of the polynomial are solved by quadratic programming (QP) to complete the smooth optimization of velocity planning:

$$cos{t}_{\mathrm{Q}P}={\displaystyle \sum _{i=1}^N\left({\omega }_{QP1}{\displaystyle \underset{t=0}{\overset{1}{\int }}ac{c}_{QP,i}{}^2\left(t\right)}+{\omega }_{QP2}{\displaystyle \underset{t=0}{\overset{1}{\int }}jer{k}_{QP,i}{}^2\left(t\right)}+{\omega }_{QP3}{{\displaystyle \underset{t=0}{\overset{1}{\int }}\left(s_{QPi}\left(t\right)-s_{DPj}\left(t\right)\right)}}^2\right)}$$

(26)

where $N$ is the total number of stages in DP, $ac{c}_{QP,i}$ is the acceleration of USV motion in each stage, and $jer{k}_{QP,i}$ for jerk. Expression $s_{QPi}-s_{DPi}$ represents the difference between the position of the ST curve obtained by DP against the position after QP optimization. ${\omega }_{DP1}$, ${\omega }_{DP2}$ and ${\omega }_{DP3}$ are the weight coefficients of the three respectively.

6. Numerical Simulation Results

The research ideas of USV motion control, path following and obstacle avoidance proposed in this paper have been proved theoretically. This section will further verify the convergence and effectiveness of relevant algorithms through computer numerical simulation with MATLAB&SIMULNK. Specifically, the motion modelling under the framework of USV combined with the SF coordinate system is first carried out. The relative control algorithm for USV resultant velocity, course and path reference point velocity as a virtual control variable is programmed. Meanwhile, LOS guidance law is also introduced. Then, a series of simulation experiments were carried out around the velocity planning schemes applied in the two scenarios to comprehensively verify the control effect of USV autonomous navigation studied in this paper. The USV model used in the simulation experiment is described by the following parameters: $m_{11}=25.8$, $m_{22}=33.8$, $m_{33}=2.76$, $d_{11}=12$, $d_{22}=17$, $d_{0.5}=0.5$.

6.1. Simulation Results of Derailment Correction

Draw up a preset path curve C1 for the lane change operation: $\{\begin{array}{c}{y}_c=p\\ x_c=20\tan \mathrm{h}\left(6p\right)\end{array}$, where $p\in \left[-50,50\right]$. Set an initial state where the USV is deployed deviate significantly from the start of the path: ${\left[Y_{USV0},X_{USV0}\right]}^T={\left[-50,-30\right]}^T$, ${\psi }_0=\pi /4$, $u_0=3$ m/s, and the rest initial values of motion states are all set to zero. There are limits on USV actuators: $0\le {\tau }_u\le 100$, $\left|{\tau }_u\right|\le 40$. The following parameters are used to configure the velocity planning for derailment correction based on USV position and attitude: $U_{RVC}=2.5e^{-\left(2{\psi }_{SF}^2+0.5e_N^2\right)}+0.5$. At the same time, a specific zone crossing the desired path is assumed where there are disturbances caused by external environment, compelling the USV to change its course angle by 30°. The initial value of the parameter of the preset path is $p_0=-50$, and the forward-looking distance in LOS guidance law is $\Delta =0.65$ m. Controller gain of RVC is $K_u=1$. Controller gains of NBCC are $K_{r1}=K_{r2}=0.5$. Controller gain for path reference point is $K_s=1$.

Figure 3 shows the following effect of USV on a lane-changing path in the inertial coordinate system and the Serret-Frenet coordinate system, respectively. Two derailment scenarios under the influence of initial state deviation and external disturbances are also set up. In Figure 3a, the green dotted line is the preset path. The black arrow area shows the zone where disturbance interferes with the course of USV. The blue line shows the path following and derailment correction effects of the USV moving at a constant velocity. The red line shows the effect of the USV moving at an adaptively adjustable velocity based on its position and attitude. In Figure 3b, the cyan circle represents the start point of USV in the Serret-Frenet coordinate system, and the pentacle represents the endpoint (both points of the two groups of comparative experiments are consistent). The blue dotted line indicates the convergence effect of constant velocity moving, and the red dotted lines indicate the convergence effect of variable velocity moving.

Figure 4 shows a series of data related to the underlying motion control of the USV velocity planning based on its position and attitude in path following and derailment correction. Figure 4a records the rate of motion on 3-DOF of surge, sway and yaw, and the real moving velocity. Figure 4b,c shows the control effect towards set values by an outer loop of RVC and NBCC, respectively. Figure 4d shows USV thrust and torque output under finite-amplitude conditions. Figure 4e is the reference point moving velocity along the path curve represented by a general parameter, which also serves as a virtual control variable introduced for the underactuated USV path following problem. Figure 4f is the position of the reference point at the corresponding time.

6.2. Simulation Results of Dynamic Obstacle Avoidance

Draw up a preset path curve C2 in the polynomial form: $\{\begin{array}{l}{y}_c=p\\ x_c=0.004p^3-0.3p+6.3p\end{array}$, where $p\in \left[0,70\right]$. Set the initial position and direction of the USV to be consistent with the start point of the path curve, while the initial velocity is $U_0=3$ m/s. In this scenario, two dynamic obstacles are moving across the preset path straightly. The starting position and velocity vectors of the obstacle are respectively ${\left[Y_{obs1},X_{obs1}\right]}^T={\left[40,0\right]}^T$, ${\stackrel{\rightharpoonup }{V}}_{obs1}={\left[0.5,0.5\right]}^T$ and ${\left[Y_{obs2},X_{obs2}\right]}^T={\left[40,-5\right]}^T$, ${\stackrel{\rightharpoonup }{V}}_{obs2}={\left[0,0.8\right]}^T$. Their collision radius is assumed to be 2 m and 4 m. USV’s collision radius is assumed to be 2 m. The upper and lower moving velocities are respectively assumed to be 3 m/s and 0.1 m/s, while accelerations are 3 m/s² and −2 m/s². The rest configurations of the controller coincide with the ones in Section 6.1.

Figure 5 is the ST graph generated by the projection calculation of obstacles and the velocity curve obtained after DP and QP optimization. In Figure 5a, the yellow and purple blocks represent the Spatio-temporal domain of the preset path occupied by two dynamic obstacles. DP rasterizes the ST graph according to the discretization degrees of 0.5 s and 0.5 m, and then conducts search optimization for the 140 discrete stages according to constraints and cost functions to obtain the path-time curve represented by segment lines, namely the blue dotted line in the figure. The curve derivative of DP search has mutations, which is not conducive to practical application. Next, the curve was smoothed by a series of quintic polynomials, and their coefficients are solved by QP according to constraints and cost functions. Finally, the optimized green curve in the figure was obtained, so the velocity planning of USV for dynamic obstacle avoidance was completed. As can be seen from Figure 5b, the velocity, acceleration and jerk of the USV’s movement are equipped with smooth continuity after QP for optimizing, which can be used as the reference input value of RVC in practical application.

Figure 6 is a time diagram of the USV path following and dynamic obstacle avoidance based on ST graph optimization. The dotted blue line is the preset path to be tracked, and the solid black line is the actual trajectory of the USV. The red arrow in front of the USV mark indicates the direction and velocity of its actual movement. The yellow and purple circles are the two moving obstacles crossing the path, and the tail lines of the corresponding colours are their respective trajectories.

Figure 7 shows a series of data related to the underlying motion control of the USV in this obstacles avoidance scenario. Figure 7a records the rate of motion on 3-DOF of surge, sway and yaw, and the real moving velocity. Figure 7b,c shows the control effect towards set values by an outer loop of RVC and NBCC, respectively. Figure 7d is the time curve of USV thrust and torque output. Figure 7e reflects the actual velocity at different positions on the USV trajectory, ranging from 1 m/s to 3 m/s. The lower the colour temperature is, the higher the USV resultant velocity is.

7. Discussion

From the simulation results of Section 6.1, this paper puts forward the velocity control of USV based on its position and attitude, which enables USV to correct the initial position deviation through a smaller steering radius to reduce the path loss. USV can also correct the derailment caused by the sudden disturbance from the outside environment with a smaller space loss due to such a velocity planning scheme. Furthermore, such a scheme will not produce adverse results of overshoot or even oscillation concerning the same LOS forward-looking distance parameter, which will greatly improve the smoothness and accuracy of transition in correction. Figure 3 shows the convergence effect of the USV motion control strategy and path following from the perspective of the {SF} frame. An analysis of the results of USV underlying motion control shows that the moving velocity and course both track the setpoints and overcome the disturbance remarkably. In particular, a comparison of the data in the 20–50 s time range of Figure 4a–c shows that when USV is derailing (caused by the too-small path curvature at around 24 s and 48 s, disturbance’s occurrence at around 28 s and disturbance’s disappearance at around 36 s), subject to velocity control and the ship’s maneuvering characteristics, USV’s performance is reflected in the decrease of surge velocity and increase of sway velocity, while the resultant velocity decreases and a certain drift angle occurs. Vividly speaking, USV completes the steering process in a “drifting” way with velocity deceleration which is more consistent with reality. This is an effective way to reduce the offset and complete the derailment correction as soon as possible, improving the maneuverability and flexibility of the USV. When USV succeeds to move to the preset path, the velocity set value will be increased to the expected patrol velocity, which can also be customized by the specific operation task. In addition, the output of thrust and torque is saturated, but mainly at the moment of sharp steering and acceleration, which is related to the power exponent form of Equation (23) used in variable velocity correction. Finally, the velocity and position data of reference points are consistent with USV velocity and position, which conforms to the essence of path reference points being used as the base for SF coordinate system construction, and the significance of converging USV’s path following error system by introducing such virtual control variable.

In Section 6.2, we first calculated and plotted the ST graph of a relationship between preset path curve and dynamic obstacle encroachment, which means that the USV cannot appear at the corresponding path distance at the corresponding time, otherwise a collision will be inevitable. The ST graph transfers the complex collision problem in the scale of time and space to the path searching problem in a 2-D image. The use of DP and QP optimization algorithms finally gives a continuous and smooth velocity planning including velocity, acceleration and jerk with respect to the constraints and costs from the actual situation. This ensures the smoothness of USV velocity variation, the continuity of motion trajectory curvature, and the smoothness of actuator outputs. The continuity of motion trajectory curvature means that the preset path in this planning is traceable for the USV because the USV cannot move horizontally or stop. And a mutational curvature of the desired path means that the USV will have a transition process of temporary deviation from the path. Although it is possible to return to the desired path eventually, such temporary deviation should be avoided in scenarios where moving obstacles may be all around. The greatest value of this time-for-space method can be seen from the time diagram of scenario simulation, that is, the USV can evade obstacles by adjusting its velocity but not breaking away from the preset path. The logic behind it lies in the fact that the preset path is usually well planned for many factors, e.g., task requirement, channel limits, navigation rules and so on. In other words, it is very likely to encounter new troubles if a local path is created for obstacle avoidance. Therefore, it is better to let the USV follow the preset path as close as possible, and realizing obstacle avoidance through velocity planning deserves a higher priority than making local path planning to generate a new route away from the preset path, which is the conventional method that most works adopt. Of course, when velocity planning fails, which means that there is no solution from dynamic programming on the ST graph, USV cannot but resort to local planning to avoid an incoming collision. From the perspective of the underlying motion control data, the USV has achieved the tracking of the expected velocity and heading precisely, which is also the basis for applying velocity planning. Figure 7e is a colourmap that directly shows the actual velocity of the USV moving at different positions of its trajectory according to velocity planning for obstacle avoidance.

8. Conclusions

In this paper, the motion control and path-following problems of a USV and the application scenario expansion based on velocity planning are studied and discussed strictly according to the research ideas of control theory and the engineering application field. The process includes mathematical modelling, controller design, stability proof, scenario optimization and simulation verification. The simulation results of USV in two scenarios verify the effectiveness and reliability of the motion control strategy and the path following algorithm proposed in this study. Secondly, the simulation results show that both velocity planning strategies perform well in derailment and obstacle avoidance scenarios. Through the detailed display and analysis of the result data, the feasibility and practicability of the research and design ideas in this paper to solve the problems of the USV path following control, derailment correction and dynamic obstacle avoidance are fully proved, which is also the main contribution and value of this study. Some details not involved in the whole process, such as model parameter perturbation, actuator saturations, obstacle perception and judgement, real ship identification and test, have formed the subject direction for follow-up in-depth research which will motivate the scientific research and promotion of autonomous unmanned system technology together with this study.