Model Predictive Control Based on State Space and Risk Augmentation for Unmanned Surface Vessel Trajectory Tracking

Abstract

The underactuated unmanned surface vessel (USV) has been identified as a promising solution for future maritime transport. However, the challenges of precise trajectory tracking and obstacle avoidance remain unresolved for USVs. To this end, this paper models the problem of path tracking through the first-order Nomoto model in the Serret–Frenet coordinate system. A novel risk model has been developed to depict the association between USVs and obstacles based on SFC. Combined with an artificial potential field that accounts for environmental obstacles, model predictive control (MPC) based on state space is employed to achieve the optimal control sequence. The stability of the designed controller is demonstrated by means of the Lyapunov method and zero-pole analysis. Through simulation, it has been demonstrated that the controller is asymptotically stable concerning track error deviation, heading angle deviation, and heading angle speed, and its good stability and robustness in the presence of multiple risks are verified.

1. Introduction

Between 2014 and 2019, out of a total analysis of 5100 maritime accidents, 68.2% were attributed to human fault, and port areas accounted for 18.5% followed by coastal areas at 13.3% [1]. Autonomous ships, capable of performing tasks with little or no human interaction, could help lower the high rate of accidents. The realization of complete autonomy plays a crucial role in the advancement of autonomous vessels, enabling unmanned surface vehicles (USVs) to operate effectively in complex environments independently of human intervention [2]. Unmanned navigation is a significant topic and needs to be approached from various angles, including ship maneuverability, environmental loads, obstacles, and adherence to maritime regulations. Research in this area holds great promise for future development in unmanned navigation regarding efficiency and security. However, limited research has been conducted on reducing risks and obstacles and improving safety during maneuvering. Therefore, researching the engineering and academic interest of achieving safe, reliable, and optimal ship maneuvering during trajectory tracking is of great importance. USVs, which benefit from advancements in robotics and artificial intelligence, have attracted considerable attention in ship and ocean engineering. In comparison with conventional ships, USVs present reduced costs, increased efficiency, and superior security. It is imperative to develop a robust and risk-free steering system that meets the different operational prerequisites [3].

Extensive investigations have been undertaken into the control of USVs, including studies on the back-stepping method [4], the linear quadratic regulator algorithm [5], adaptive controller [6], sliding mode controller (SMC) [7], and fuzzy logic control [8]. References [9,10,11] focused on the trajectory tracking of USVs, which is a crucial performance criterion for maneuvering and navigation. In detail, a new guidance and control system was created for a USV which takes environmental factors into account in [9], and robust intelligent path planning has been developed by integrating functional modules. A controller has been presented in the literature [10] for the purpose of tracking the trajectory of USVs while accounting for uncertainties and input constraints. The planning and control of trajectories, oriented towards berthing, were discussed in [11] using an advanced fuzzy PID approach. This approach considers the dynamic alterations of the USV during berthing tasks. The authors’ (Wei Li) previous work concentrated on the design of controllers and disturbance observers in the field of USV trajectory tracking [12], in which the time-varying disturbances such as wind, waves, and currents must be compensated and controlled while achieving path tracking. In addition, the authors have some research results in the field of model predictive control algorithms as well as high-speed multihull robust control. Specifically, a sliding mode predictive anti-pitching control considering appendage constraints is proposed in [13] (Jun Zhang), in which a prediction model and an objective function are established with the sliding mode states as variables, and the sliding mode terminal stability constraint set is adopted to improve the stability of the closed-loop system. Considering the excessive vertical motion amplitude of high-speed multihulls, and the difficulty of measuring heave velocity and pitch angular velocity directly, a robust anti-pitching control method without velocity measurements is proposed in [14]. Nevertheless, the collision avoidance issue has been overlooked in the above trajectory tracking investigations.

Since obstacle avoidance is one of the most important capabilities for USVs to perform safe navigation, the researchers in [15] employed a reinforcement learning-based, decision-making algorithm for path planning of surface vehicles to prevent collisions during maneuvering. However, the time-consuming training of the network and the uncertainty limit the effectiveness of the proposed approach. Wang et al. in [16] presented a path planning algorithm named the local normal distribution-based trajectory that is designed to avoid static obstacles based on the convention of international regulations. The algorithm was tested in a simulation environment and verified to be effective for single obstacles. Li et al. in [17] developed a deep reinforcement learning algorithm for optimal collision avoidance path planning utilizing simulated real-time sensor data during practical navigation. The collision avoidance problem in a marine environment can be effectively addressed with local dynamic path planning based on local sensor data. In actual navigation, USVs primarily rely on perception sensors to obtain surrounding environment information, such as light detection and ranging (LiDAR), radar and vision sensors.

But as stated in [17], the USV cannot obtain complete information of the environment but only obtain environmental information within a certain range centered on itself through various sensors, in practice. Niu et al. in [18] proposed an energy-efficient path planning algorithm integrating multiple algorithms to optimize path planning under spatially and temporally varying environmental loads. Song et al. in [19] presented a two-level dynamic obstacle avoidance algorithm. The initial stage involves global planning through the implementation of the velocity obstacle algorithm, followed by local obstacle avoidance methods. Nevertheless, the literature [15,16,17,18,19] ignores the effect of USV kinematics and manipulation performance on obstacle avoidance effectiveness.

To address this issue, the kinematic model is used to predict the path whilst disregarding the vessel’s intricate mechanics in [20] when conducting collision avoidance studies. Reference [21] utilized a modified version of the artificial potential field method (APFM), in conjunction with the Dijkstra algorithm. To further optimize the APFM, the global path planning technique was improved in [22], with the implementation of the artificial potential field-ant colony optimization obstacle avoidance algorithm. As for the path planning, [23] used a Voronoi diagram and Fermat’s spiral to generate a real-time path, which subsequently navigates the ship through an adaptive line-of-sight (LOS) guidance algorithm. Campbell et al. implemented processes for identifying obstacles, evaluating risks, and devising routes to circumvent them in [24]. The detection mechanism is based on vision-LiDAR, and the path mapping employs the Rule-based Repairing A-Star (R-RA*) algorithm. Furthermore, Kuwata et al. [25] attained dependable and autonomous sea navigation by utilizing the velocity obstacles approach. However, the collision avoidance method of [25] needs the use of satellite imagery, which excludes imperative information such as reefs and ocean depth. Moreover, the capacity for cameras and sonar to observe can be impeded by light and jolts, leading to a decreased range and accuracy, as mentioned in [21,22,23,24].

References [26,27,28,29,30,31,32,33,34,35] are more relevant to this paper. The collision avoidance system strives to carry out vessel tasks efficiently, steadily, and promptly [26]. To circumvent localized reactions in high-speed USVs, Ref. [27] determined that the application of the collision avoidance system involves both path planning and a control system, and the authors of [27] stated that integrating control and planning within an algorithm significantly enhances real-time performance and better meets the requirements for collision avoidance of USVs. Zhang et al. developed a sturdy approach to model predictive control (MPC) for surface vessel path-following, as described in their publication [28], which considered roll motion during optimization. In the study [29], the researchers utilized MPC to navigate obstacles in restricted areas. Since the adoption of an MPC controller, it has been utilized to address the issues of trajectory tracking and obstacle avoidance in both aerial drones and ground vehicles. It has become increasingly common in addressing USV’s navigation challenges [30,31]. Path planning is no exception, with MPC enabling online stability and robustness [32]. This is executed by incessantly re-planning the path process at each iteration and implementing the initial control action linked with the chosen trajectory to the ship [33]. The optimization of a finite control set is a widely recognized concept in robust MPC literature [34]. Furthermore, as stated by [35], MPC has been utilized for collision prevention in maritime operations. However, it is essential to improve the response of the collision avoidance system mentioned [30,31,32,33,34,35], given the relatively small size and higher speed of USVs compared to conventional ships.

In this paper, we assume that the USV has obtained information on the size and shape of obstacles through multiple sensors. With this detection result, our research concentrates on the obstacle avoidance path planning and tracking trajectory controller design process, aiming to ensure successful trajectory tracking with obstacle avoidance. On the other hand, to ensure the accuracy of USV trajectory tracking while avoiding obstacles, this paper proposes a risk enhanced MPC method for USV trajectory tracking. Compared with existing studies and our previous work, the main contributions of this paper are as follows:

(1)

It establishes a state space control model with risk augmentation for underactuated USV path tracking, where the state space control model, including track error deviation, heading angle deviation, heading angle velocity, and risk force augmentation term, plays an important role.

(2)

This paper presents a MPC method combined with an artificial potential field that accounts for multiple environmental obstacles, in which the risk model depicting the correlation between the USV and obstacles has been established based on the Serret–Frenet coordinate system, distinguishing itself from existing techniques.

(3)

It demonstrates that a reasonable design of the weighted gain matrix and risk avoidance parameters ensure convergence of the proposed algorithm. The method developed in this work, based on Lyapunov stability and zero-pole analysis, holds significant value in obtaining optimal control action in an explicit manner.

This paper is organized as follows. In Section 2, the system model is presented. In Section 3, the MPC controller with risk augmentation is designed and the convergence of the algorithm is presented mathematically. Numerical simulations are presented in Section 4. Section 5 concludes this paper and states future works.

2. System Modeling

2.1. Trajectory Tracking Description of USV

Two different research methods are successively applied to the path tracking control of the surface ship. One is to design the controller directly based on the dynamic equation of the path tracking error in the geodetic coordinate system. The other is to study the problem under the Serret–Frenet coordinate (SFC) [36] as shown in Figure 1. The origin of the frame {SF} is located at the closest point on the given path from the origin of the body-fixed frame {B}. $\Omega$ denotes the given path, $T$ is the tangent direction of the given path, $M$ is the USV orientation, $\psi$ is the heading angle formed by the counterclockwise rotation of the USV direction to the $X$ axis, ${\psi }_{SF}$ is the desired heading angle formed by the counterclockwise rotation of the tangent direction of the given path to the $X$ axis, $e$ is the tracking error, and $X_d$ is the desired point.

In Figure 1, the error dynamics based on SFC are given by [37]:

$$\{\begin{array}{l}\dot{e}=\omega \sin (\overline{\psi })+v\cos (\overline{\psi })\\ \dot{\overline{\psi }}=\frac{\kappa }{1-e\kappa }(\omega \sin (\overline{\psi })-v\cos (\overline{\psi }))+r\\ \overline{\psi }=\psi -{\psi }_{SF}\end{array}$$

(1)

where $\omega ,v,r$ represents the surge, sway, and yaw velocities, respectively. $\kappa$ denotes the curvature of the given path $\Omega$. The control objective of the path following in Serret–Frenet frame is to drive $e$ and $\overline{\psi }$ to zero [37]. When environmental obstacles exist, the path following $e$ and $\overline{\psi }$ cannot be eliminated at the same time. In such circumstances, the primary objective is to maintain a near-zero cross-track error $e$, while keeping a certain necessary heading error $\overline{\psi }$ to avoid current environmental obstacles.

For most of the path following, the given path is a straight line or a way-point path, which consists of piecewise straight lines with the curvature $\kappa$ being zero. Even though the curvature is not zero, we can also segment it into lots of piecewise straight lines. Additionally, $\sin (\overline{\psi })\to \overline{\psi }$ can be derived within a small control input. As the ship’s motion change is near the equilibrium point and the drift angle can be disregarded, it can be deduced that that $\omega \gg 0$ and $v\to 0$. From the above analysis, the error dynamics based on SFC (1) evolves into a more concise form as follows:

$$\{\begin{array}{l}\dot{e}=\omega ·\overline{\psi }\\ \dot{\overline{\psi }}=r\\ \overline{\psi }=\psi -{\psi }_{SF}\end{array}$$

(2)

where $e$ is the tracking error, $\overline{\psi }$ is the tracking heading error, $\omega$ is the USV speed, and $r$ is the yaw velocity.

2.2. State Space Model for Trajectory Tracking

This paper utilizes the Nomoto model [38] to analyze ship maneuverability, as it enables the interpretation of ship motion problems through manipulation of performance parameters that are easily gained. The Nomoto model in [38] is presented in detail below:

$$T\ddot{\psi }+\dot{\psi }=K\delta$$

(3)

where $\psi$ denotes the heading and $\delta$ is the constrained rudder angle as the control input. $K$ and $T$ are the operational parameters, which can be obtained by the designated rotary and Z-type experiments. Equation (3) can be rewritten into a concise form as follow:

$$\ddot{\psi }=-\frac{1}{T}\dot{\psi }+\frac{K}{T}\delta$$

(4)

Following the definition of $\overline{\psi }=\psi -{\psi }_{SF}$, if the desired path is a straight line, then ${\dot{\psi }}_{SF}=0$. Then one derives that $\dot{\overline{\psi }}=\dot{\psi }$. Therefore, Equation (4) evolves into the following:

$$\ddot{\overline{\psi }}=-\frac{1}{T}\dot{\overline{\psi }}+\frac{K}{T}\delta$$

(5)

Combined with the error dynamics for path tracking in Equation (2), one follows that

$$\{\begin{array}{l}\dot{r}=-\frac{1}{T}r+\frac{K}{T}\delta \\ \dot{e}=\omega ·\overline{\psi }\\ \dot{\overline{\psi }}=r\\ \overline{\psi }=\psi -{\psi }_{SF}\end{array}$$

(6)

Equation (6) can be rewritten into the form of a state space equation as follow:

$$\left[\begin{array}{c}\dot{r}\\ \dot{\overline{\psi }}\\ \dot{e}\end{array}\right]=\left[\begin{array}{ccc}-\frac{1}{T}& 0& 0\\ 1& 0& 0\\ 0& \omega & 0\end{array}\right]\left[\begin{array}{c}r\\ \overline{\psi }\\ e\end{array}\right]+\left[\begin{array}{c}\frac{K}{T}\\ 0\\ 0\end{array}\right]\delta$$

(7)

From the state space model for trajectory tracking of USV Equation (7), one concludes that it captures the dominant ship maneuvering dynamics and path following error dynamics with a control variable. Based on the state space model for trajectory tracking of the USV in Equation (7), this paper concerns the goal of tracking a trajectory, specifically how to gradually reduce and eventually eliminate the position deviation and direction angle deviation from the current trajectory of a ship to follow a target trajectory. This is achieved by adjusting the rudder angle control quantity to finally converge the position deviation and direction angle deviation to zero by using the following control method.

2.3. MPC Algorithm Framework

Model predictive control (MPC) is becoming increasingly prevalent in advanced control due to its direct formulation and powerful optimization capabilities to handle explicit models and constraints effectively. As shown in Figure 2, the MPC utilizes a closed-loop control structure, comprising the controller module equipped with the dynamic optimizer, plant model, cost function, and constraints [39]. Furthermore, Figure 3 demonstrates the open-loop control in the MPC controller. The plant model serves as a simplified estimate of the actual plant, aiding in the optimization process. The control system’s initial inputs operate within a predetermined range and are supplied to the plant model. The initial output of the model is suboptimal. Moreover, the MPC controller aims to optimize the control inputs based on the appointed cost function. Over various iterations, the control inputs gradually converge to the optimum solution. The improved control inputs should be installed in the physical installation to bring it up to date. The optimization cycle repeats for each subsequent prediction time span, within the new redirection time span. This process continues until the time span concludes. The MPC controller design is achieved by model predictive controller toolbox. The user outlines the core components of the numerical model, including the cost function, state space, and constraints. The remaining unspecified modules are predetermined by default settings, such as the dynamic optimizer.

This paper applies MPC with space state and risk augmentation to the problem of tracking the trajectory of a USV. State space representation and cost function are crucial components of MPC and will be expounded in the following sections.

3. Model Predictive Controller

In the marine environment, USVs will encounter static or dynamic obstacles during trajectory tracking, including sailing ships and reefs, that impede their ability to follow the predetermined trajectory. To facilitate the avoidance of obstacles during trajectory tracking by the USV, an artificial potential field-based risk model is introduced in this section. Additionally, a model prediction controller utilizing the risk augmentation term is designed to minimize the objectives of the optimization problem.

3.1. Obstacle Description under the SFC for USV Trajectory Tracking

After the USV starts moving, the design of the obstacle avoidance controller based on the augmented risk term is only performed if the USV enters the risk area. If the ship does not encounter any obstacles within the risk range, there is no need to account for their influence on its trajectory tracking. In this case, only the trajectory tracking controller needs to be designed [12], which utilizes a model prediction controller that is not affected by obstacles. Then when the USV enters the obstacle’s range of influence, the sensor provides detailed obstacle information relating to location, shape, and size to the control system. Then the controller provides real-time control signals for the USV to achieve trajectory tracking with obstacle avoidance.

This paper assumes that the sensors have identified the position, size, and configuration of obstacles. As illustrated in Figure 4, the risk model depicting the correlation between the USV and the obstacles has been established based on the SFC. The mass center of the USV is denoted by $X$, also serving as the origin of the {B} coordinate, coinciding with the desired track point $X_d$, i.e., the origin of the SFC. This coordinate system reflects the orthogonal projection of $X$ on the target track $\Omega$ on the system and shifts alongside the motion of the USV. $e$ is the distance between $X$ and $X_d$, ${\psi }_{SF}$ is the target heading, and $\psi$ is the heading angle of the USV. $d_0={\lambda }_{0}a$ is the maximum distance at which the risk can affect the movement of the USV, where $a$ denotes the radius of a circular obstacle (i.e., the red obstacle region). If the obstacle is irregular, we approximate it as a circular obstacle. ${\lambda }_0$ is the safety coefficient which depends on the operating performance of the USV and the shape of the risk and is generally taken as 5~10, $d$ is the distance between the mass center of the USV and the center of the blue risk area $C_{obs}$.

Based on the artificial potential field method, the gravitational field of the USV subjected to the desired track point can be expressed as follows:

$$U_{att}(e)=\frac{1}{2}\xi e^2$$

(8)

where $\xi$ is the proportional position gain coefficient. The gravitational force on the USV from the desired track point is as follows:

$$F_{att}(e)=-{\dot{U}}_{att}(e)=-\xi e$$

(9)

where the negative sign signifies that the direction of the force is from the mass center of the USV to the desired trajectory point. Based on the definitions of $d_0$ and $d$, it is evident that $d_0$ represents the scope of a stationary obstacle that is intended to enhance the size of the obstacle, while $d$ denotes the distance between the USV and the obstacle’s center of mass, which changes in real-time and determines the level of the USV’s potential field for the risk obstacle as follows:

$$U_{rep}(d)=\{\begin{array}{l}\frac{1}{2}\eta {\left(\frac{1}{d-a}-\frac{1}{d_0}\right)}^2,d-a\le d_0\\ 0,d-a>d_0\end{array}$$

(10)

The USV’s repulsive force from the risk is defined as follows:

$$F_{rep}(d)={\dot{U}}_{rep}(d)=\{\begin{array}{l}\frac{\eta }{{(d-a)}^2}\left(\frac{1}{d-a}-\frac{1}{d_0}\right)\frac{X-X_c}{\Vert X-X_c\Vert },d-a\le d_0\\ 0,d-a>d_0\end{array}$$

(11)

where $\eta$ is the repulsive potential field parameter, $X_c$ is the closest point to the mass center of the USV in the risk area $C_{obs}$, $X-X_c/\Vert X-X_c\Vert$ denotes the unit vector pointing from $X_c$ to $X$, and the direction of the force $F_{rep}(d)$ is also from $X_c$ to $X$.

3.2. Controller Design with Risk Augmentation

When the USV enters the area affected by the risk, it experiences a repulsive force, while simultaneously experiencing the gravitational force of the intended trajectory. As a result, by adding the combined force field to the control model as the fourth state of the system, we obtain the USV tracking control model based on the risk augmentation as

$$\left[\begin{array}{c}\dot{r}\\ \dot{\overline{\psi }}\\ \dot{e}\\ {\dot{U}}_{sum}\end{array}\right]=\left[\begin{array}{cccc}-\frac{1}{T}& 0& 0& 0\\ 1& 0& 0& 0\\ 0& \omega & 0& 0\\ 0& 0& -\xi & 0\end{array}\right]\left[\begin{array}{c}r\\ \overline{\psi }\\ e\\ U_{sum}\end{array}\right]+\left[\begin{array}{c}\frac{K}{T}\\ 0\\ 0\\ 0\end{array}\right]\delta +\left[\begin{array}{c}0\\ 0\\ 0\\ g(e)\end{array}\right]\rho (d)$$

(12)

where $\rho (d)=\frac{\eta }{{(d-a)}^2}\left(\frac{1}{d-a}-\frac{1}{d_0}\right)\frac{X-X_c}{\Vert X-X_c\Vert }$, $g(e)=\{\begin{array}{l}1,\left|e\right|-a\le d_0\\ 0,\left|e\right|-a>d_0\end{array}$. Equation (12) can be rewritten as a state space expression of continuous systems:

$$\{\begin{array}{l}\dot{x}=Ax+Bu+D\rho \\ y=Cx\end{array}$$

(13)

where $x={\left[\begin{array}{cccc}r& \overline{\psi }& e& U_{sum}\end{array}\right]}^{\mathrm{T}}$ and $y$ is the system output. $C$ is the system output matrix with appropriate form. $A$,$B$, and $D$ are system matrices with

$$A=\left[\begin{array}{cccc}-\frac{1}{T}& 0& 0& 0\\ 1& 0& 0& 0\\ 0& \omega & 0& 0\\ 0& 0& -\xi & 0\end{array}\right], B={\left[\begin{array}{cccc}\frac{K}{T}& 0& 0& 0\end{array}\right]}^{\mathrm{T}}, D={\left[\begin{array}{cccc}0& 0& 0& g(e)\end{array}\right]}^{\mathrm{T}}.$$

Then we perform the first order Eulerian discretization on Equation (13) to obtain a discrete control state space model as follows:

$$\{\begin{array}{l}x(k+1)=A_{k}x(k)+B_{k}u(k)+D_k\rho (k)\\ y(k)=C_{k}x(k)\end{array}$$

(14)

where $A_k$,$B_k$,$C_k$, and $D_k$ are system matrices with discretization. Using Equation (14) as a prediction model, the following prediction states are obtained by iteration:

$$\{\begin{array}{l}x(k+1|k)=A_{k}x(k)+B_{k}u(k)+D_k\rho (k)\\ x(k+2|k)=A_k{}^{2}x(k)+A_k{B}_{k}u(k)+B_{k}u(k+1)+A_k{D}_k\rho (k)+D_k\rho (k+1)\\ \begin{array}{ccc}& ⋮& \begin{array}{ccc}& ⋮& \begin{array}{ccc}& ⋮& \end{array}\end{array}\end{array}\\ x(k+N_p|k)=A_k{}^{N_P}x(k)+A_k{}^{N_P-1}{B}_{k}u(k)+\cdots +A_k{}^{N_P-N_C}{B}_{k}u(k+N_c-1)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} \hspace{0.17em}+A_k{}^{N_P-1}{D}_k\rho (k)+\cdots +A_k{}^{N_P-N_C}{D}_k\rho (k+N_c-1)\end{array}$$

(15)

We denote $x(k+i|k)$, $u(k+i|k)$, and $y(k+i|k)$ the corresponding predictive state vector, control vector, and output vector at the future sample instant $k+i$ based on the current sample instant $k$, where $i=1,2,\cdots ,N_P$, $k=1,2,\cdots .$.$N_P$ is the predictive time domain and $N_c$ is the control time domain. Generally, $N_C\le N_P$. The predicted output vectors for the next $N_P$ instants can be written in the compact form:

$$Y=Fx(k)+\Phi U+TP$$

(16)

where

$$\{\begin{array}{l}Y={[\begin{array}{cccc}{y}^{\mathrm{T}}(k+1)& y^{\mathrm{T}}(k+2)& \cdots & y^{\mathrm{T}}(k+N_P)\end{array}]}^{\mathrm{T}}\\ U={[\begin{array}{cccc}{u}^{\mathrm{T}}(k)& u^{\mathrm{T}}(k+1)& \cdots & u^{\mathrm{T}}(k+N_c-1)\end{array}]}^{\mathrm{T}}\\ P={[\begin{array}{cccc}{\rho }^{\mathrm{T}}(k)& {\rho }^{\mathrm{T}}(k+1)& \cdots & {\rho }^{\mathrm{T}}(k+N_c-1)\end{array}]}^{\mathrm{T}}\\ F=[\begin{array}{cccc}{(C_k{A}_k)}^{\mathrm{T}}& {(C_k{A}_k^2)}^{\mathrm{T}}& \cdots & {(C_k{A}_k^{N_P})}^{\mathrm{T}}\end{array}]{]}^{\mathrm{T}}\\ \Phi =\left[\begin{array}{cccc}{B}_k& 0& \cdots & 0\\ C_k{A}_k{B}_k& C_k{B}_k& \cdots & 0\\ ⋮& ⋮& \ddots & 0\\ C_k{A}_k{}^{N_P-1}{B}_k& C_k{A}_k{}^{N_P-2}{B}_k& \cdots & C_k{A}_k{}^{N_P-N_C}{B}_k\end{array}\right]\\ T=\left[\begin{array}{cccc}{D}_k& 0& \cdots & 0\\ C_k{A}_k{D}_k& C_k{D}_k& \cdots & 0\\ ⋮& ⋮& \ddots & 0\\ C_k{A}_k{}^{N_P-1}{D}_k& C_k{A}_k{}^{N_P-2}{D}_k& \cdots & C_k{A}_k{}^{N_P-N_C}{D}_k\end{array}\right]\end{array}$$

(17)

The cost function used for the MPC design has the structure with

$$\begin{array}{l}J={\displaystyle \sum _{i=1}^{N_p}{[y_d(k+i)-y(k+i|k)]}^{\mathrm{T}}\overline{Q} [y_d(k+i)-y(k+i|k)]+}{\displaystyle \sum _{i=1}^{N_c}u{(k+i-1|k)}^{\mathrm{T}}\overline{R}u(k+i-1|k)} \\ +{\displaystyle \sum _{i=1}^{N_c}P{(k+i-1|k)}^{\mathrm{T}}H P(k+i-1|k)}\end{array}$$

(18)

where $y_d$ is the desired output of the system containing a weighted combination of the states. $\overline{Q}$, $\overline{R}$, and $H$ are weighing matrices. For the cost function (18), the first term of the quadratic objective function depicts the error between the actual output and predicted output based on the weighing matrix $\overline{Q}$, such that it represents the state of the system and the desired state to be as close as possible at the next $N_p$ time steps. The second term of quadratic objective function prevents excessive controller output based on the weighing matrix $\overline{R}$ at the next $N_c$ time steps. The third quadratic objective function prevents excessive repulsive force based on the weighing matrix $H$ at the next $N_c$ time steps. The aim of model predictive control in this paper is to minimize the system’s output error, controller output, and obstacle repulsive force, thus obtaining the optimal control law for USV trajectory tracking.

As a result, the model prediction control can be described as follows: at each sampling instant $k$, the aim is to find the optimal control sequence $\{u^{*}(k),u^{*}(k+1),\cdots ,u^{*}(k+N_c-1)\}$ to minimize the objective function. For notational convenience, we rewrite the cost function as the following impact form:

$$J={(Y_d-Y)}^{\mathrm{T}}\overline{Q}(Y_d-Y)+U^{\mathrm{T}}\overline{R}U+P^{\mathrm{T}}HP$$

(19)

Under the receding horizon principle, the control vectors for the next $N_c$ sampling instants are obtained by minimizing the cost function (19), but only the first of these is applied to the system. Therefore, for unconstrained optimization problems, let $\frac{\partial J}{\partial U}=0$, and solving this equation gives the global optimal control sequence as follows:

$$U={({\Phi }^T\overline{Q}\Phi +\overline{R})}^{-1}[{\Phi }^{\mathrm{T}}\overline{Q}(Y_d-Fx(k)-TP)]$$

(20)

During the path tracking for USV, the control system has certain limitations. In this paper, we investigate the scenario where the control rudder angle amplitude is constrained. For predictive control with a prediction time domain of $N_p$, the control volume amplitude in future $j$ steps are restricted as follows:

$$u_{\min }\le u(k+j-1)\le u_{\max },j=1,\cdots ,N_p.$$

(21)

Equation (21) can be rewritten as a matrix form:

$$\{\begin{array}{l}IU\le U_{\max }\\ -IU\le -U_{\min }\end{array}$$

(22)

where $I$ is a unit matrix with $N_p\times N_p$ dimensions. After considering the amplitude constraint on the rudder angle, the objective function is evolved into the following:

$$\begin{array}{c}\underset{x,u,\rho }{\min }J={(Y_d-Y)}^{\mathrm{T}}\overline{Q}(Y_d-Y)+U^{\mathrm{T}}\overline{R}U+P^{\mathrm{T}}HP\\ \mathrm{s}.\mathrm{t}. \{\begin{array}{l}x(k+1)=A_{k}x(k)+B_{k}u(k)+D_k\rho (k)\\ x(0)=x_0\\ WU\le \Delta \end{array}\end{array}$$

(23)

where $x_0$ is the initial state of state vector $x$ of the system; $W={\left[\begin{array}{cc}I& -I\end{array}\right]}^{\mathrm{T}}$, $\Delta ={[\begin{array}{cc}{U}_{\max }& U_{\min }\end{array}]}^{\mathrm{T}}$, $U_{\max }$, and $U_{\min }$ are vector with elements $u_{\max }$ and $u_{\mathrm{m}\mathrm{in}}$ with $1\times N_P$ dimensions. The trajectory tracking problem of the USV with risk augmentation can be summarized as Equation (1), in which the cost function is a combination of minimization for output error, control inputs, and repulsive force regarding the state model of the USV trajectory tracking and linear inequality control constraints. This type of problem is commonly known as a quadratic programming problem. To minimize online computation, the optimization problem with constraints can be solved by the Lagrange multiplier method [40].

The Lagrange function is defined as

$$L={(Y_d-Y)}^{\mathrm{T}}\overline{Q}(Y_d-Y)+U^{\mathrm{T}}\overline{R}U+P^{\mathrm{T}}HP+{\gamma }^{\mathrm{T}}(WU-\Delta )$$

(24)

where $\gamma$ is the multiplier vector and the inequality constraints are transformed into equational constraints by introducing relaxation variables. Let $\partial L/\partial U=0$ and $\partial L/\partial \gamma =0$, it follows that the optimal controller output with constraints is as follows:

$$U^{*}={({\Phi }^{\mathrm{T}}\overline{Q}\Phi +\overline{R})}^{-1}[{\Phi }^{\mathrm{T}}\overline{Q}(Y_d-Fx(k)-TP)-\frac{1}{2}{W}^{\mathrm{T}}{\gamma }^{*}]$$

(25)

Taking the first control variable of the control sequence to perform on the controller, the corresponding final control law for the USV trajectory tracking is obtained as follows:

$$u^{*}(k)=[I 0 0 \dots 0]U^{*}=[I 0 0 \dots 0]{({\Phi }^{\mathrm{T}}\overline{Q}\Phi +\overline{R})}^{-1}[{\Phi }^{\mathrm{T}}\overline{Q}(Y_d-Fx(k)-TP)-\frac{1}{2}{W}^{\mathrm{T}}{\gamma }^{*}]$$

(26)

where $x={[r \overline{\psi } e U_{sum}]}^{\mathrm{T}}$ is the state vector, $\overline{Q}$ and $\overline{R}$ are positive definite symmetric weighting matrices, and $W={\left[\begin{array}{cc}I& -I\end{array}\right]}^{\mathrm{T}}$, ${\gamma }^{*}={[{\gamma }_1^{*},\cdots ,{\gamma }_{N_P}^{*},{\gamma }_{N_P+1}^{*},\cdots ,{\gamma }_{2N_P}^{*}]}^{\mathrm{T}}$ is the optimal Lagrange multiplier vector. But based on the principle of the rolling optimization mechanism of model predictive control, only the first optimal controller law is acted on in the system. As a result, we denote the first optimal controller law as $u^{*}(k)$. As the optimization time frame advances, $u^{*}(k)$ undergoes considerable modification as well. In other words, the system modifies the time domain length of the control sequence from $[0,N_P-1]$ to $[1,N_P]$, while still implementing the current first control variable as the control time domain progresses. Then the stable state variable convergence is attained by implementing control law Equation (26), enabling the surface unmanned vessel to avoid risks while accomplishing accurate tracking of the intended trajectory.

3.3. Convergence Analysis

While the potential field method can be briefly explained using mathematics, it has inherent limitations. Evidence suggests that the system will lose stability when the trajectory oscillates around obstacles, resulting in a jagged movement of the USV. Therefore, it is essential to present the stability analysis of the USV tracking control system based on the risk enhanced MPC method.

For the real system Equation (1), we transform it to a general form of the controlled plant model as follows:

$$x(k+1)=f(x(k),u(k),\rho (k))$$

(27)

Since the control action at each period is obtained by solving the optimization problem Equation (23). For the sake of presentation, we rewrite the objective function of Equation (23) as follows:

$$V(k)=\underset{x,u,\rho }{\min }{\displaystyle \sum _{i=1}^{N}l(x(k+i|k),u(k+i-1|k),\rho (k+i-1|k))}$$

(28)

where $l(x,u,\rho )\ge 0$ and $l(x,u,\rho )=0$ if and only if $x=0$, $u=0$, and $\rho =0$. For simplification, let the predictive time domain and the control time domain both equal $N$. Then we present the following two assumptions that are easily satisfied based on the proposed MPC method.

Assumption  1.

$x=0$, $u=0$, and $\rho =0$ is an equilibrium condition of the system (27).

Assumption  2.

The optimization problem (28) for each period has a feasible solution and can be obtained using the global optimal solution. Moreover, only the first optimal controller action $u^{*}(k)$ is imposed on the system for the optimal control sequence $\{u^{*}(k+i|k):i=0,\cdots ,N-1\}$.

Then we present Theorem 1 about the convergence result for the proposed MPC method as follows.

Theorem  1.

If Assumption 1 and Assumption 2 hold, then the system (27) is stable at  $x=0$, $u=0$, and $\rho =0$.

Proof  of  Theorem  1.

Based on the Lyapunov stability theory, it is necessary to determine a Lyapunov function of the system that is positively definite, and its differentiation is negatively definite (i.e., the Lyapunov function is decreasing). We define the Lyapunov function as $V(k)$. It is obvious that $V(k)$ is positively definite due to the fact that the form of $V(k)$ is a quadratic objective function. Then,

$$\begin{array}{l}V(k+1)=\underset{x,u,\rho }{\min }{\displaystyle \sum _{i=1}^{N}l(x(k+i+1),u(k+i),\rho (k+i))}\\ =\underset{x,u,\rho }{\min }\left(\begin{array}{l}{\displaystyle \sum _{i=1}^{N}l(x(k+i),u(k+i-1),\rho (k+i-1))-l(x(k+1),u(k),\rho (k))}\\ \hspace{1em}+l(x(k+1+N),u(k+N),\rho (k+N))\end{array}\right)\\ =-l(x(k+1),u(k),\rho (k))+\underset{x,u,\rho }{\min }\left(\begin{array}{l}{\displaystyle \sum _{i=1}^{N}l(x(k+i),u(k+i-1),\rho (k+i-1))}\\ \hspace{1em}+l(x(k+1+N),u(k+N),\rho (k+N))\end{array}\right)\\ \le -l(x(k+1),u^{*}(k),\rho (k))+V(k)+\underset{x,u,\rho }{\min }\{l(x(k+1+N),u(k+N),\rho (k+N))\}\end{array}$$

(29)

Since, for the end of optimization, we have the following endpoint state constraint as $x(k+N|k)=0$. Then one derives that $\underset{x,u,\rho }{\min }\{l(x(k+1+N),u(k+N),\rho (k+N))\}=0$. At the same time, we have $l(x(k+1),u(k),\rho (k))\ge 0$. Then it follows that

$$V(k+1)\le V(k)$$

(30)

Based on the Lyapunov stability theory, the system (27) is stable at $x=0$, $u=0$, and $\rho =0$. □

To ensure the system converges quickly, it is important to consider the impact of the output matrix on the system’s zeros since their position affects convergence speed. Therefore, it is necessary to further analyze the impact of the output matrix on the system convergence performance. A reasonable output matrix can be designed to ensure stability as the dynamic characteristics of the system derived from the rudder angle to the rolling is a kind of non-minimum phase system by dynamics’ analyses. Specifically, if $C=[\begin{array}{cccc}1& 1& 1& 1\end{array}]$, i.e., taking all state variables of the system as output, the root trajectory of the transfer function of the system is shown in Figure 5.

The system depicted in Figure 6 features a zero-pole positioned on the right half-plane, signifying a non-minimum phase system. To prevent divergence owing to the predictive control law, the output matrix must be redefined, ensuring that the system is a minimum phase system with the zero-pole located on the left half-plane. Let $C=[\begin{array}{cccc}1& 1& 1& 0\end{array}]$. The root trajectory of the system can be observed in Figure 6.

The trajectory of the redefined system’s roots displayed in Figure 6 lies entirely within the left half-plane. Stability of the system is guaranteed by modifying the output matrix, and utilizing the designed predictive control law, Equation (26) stabilizes the system state, thus rendering the system asymptotically stable.

4. Simulations

The model predictive controller with risk augmentation is verified through MATLAB simulations. The proposed algorithms are tested on the computer with Intel Core i7-6700 processor (Intel Corporation, Santa Clara, CA, USA). The USV hull parameters are listed in

Table 1. USV SL-20Y hull parameters.

Index	Parameter
Size	1.05 m (Length) $\times$ 0.55 m (Width)
Weight	15 kg
Load Capacity	10 kg
Maximum Speed	5 m/s
Communication Distance	Remote control: 1 km Base station: 2 km
Turning Radius	227 mm
Mode of advancement	Jet Pump
Wave Resistance	Force 3 winds Waves 0.5 m
Maneuverability Index (K)	0.6463
Followability Index (T)	1.0674

. As depicted in Figure 7a, the USV system is presented. In this paper, we establish a kinematic KT model for the vessel type, which relies upon the assumption of the attained precise detection outcomes for obstacles. To explain exactly how this equipment or its characteristics are used in the studies, we have added the control system block diagram of the USV SL-20Y to help understand the system components and controller principles in detail as shown in Figure 7b.

4.1. Scenario 1: Target Point Following

The simulation outcome for target point following from an initial position is shown in Figure 8. The USV departs from point $p_s=[35.35,-35.35,90]$ (m, m, deg) with an initial velocity of $v_0=0$ and aims to reach the final destination at point $p_e=[140,140,45]$. The sampling time for optimization parameters and prediction horizon steps are set to $\tau =0.08 \mathrm{s}$ and $N_p=20$, respectively, resulting in a prediction time of 1.6 s. The weight matrix is set as $\overline{Q}=\mathrm{diag}\{100,100,100\}$, $\overline{R}=0.05$. The controller input is limited to $[-30,30]$ (degrees, degrees).

It can be seen from Figure 8 that (i) the USV starts from the initial position and successfully reaches the target point soon; (ii) the track error deviation and heading angle deviation converge to zero gradually indicating that the USV is converging towards the assigned waypoints; and (iii) the controller input has a maximum value of 25°, suggesting the existence of control limitations while tracking.

4.2. Scenario 2: Trajectory Tracking with a Single Obstacle

In this scenario, the trajectory tracking problem is implemented by the proposed MPC with a single obstacle. The USV starts from the initial pose $p_s=[5,5,90]$ (m, m, deg). The parameters of the MPC controller are set as $\overline{Q}=\mathrm{diag}\{200,200,200\}$,$\overline{R}=1$, $H=1$, $N_P=N_c=10$, and the sampling period $T_s=0.08s$, leading to a prediction time of $T_{mpc}=1 s$.

The controller input is limited to $[-30,30]$ (degrees, degrees). The obstacle position is ${\eta }_{ob}=[30,30]$, the length of the obstacle (i.e., the diameter) is 5 m, and the safety coefficient parameter ${\lambda }_0=8$. The desired heading ${\psi }_d={45}^{\circ }$. The gravitational potential field parameter is set to $\xi =0.001$ and the repulsive potential field parameter is set to $\eta =12$. The simulation result of trajectory tracking is given in Figure 9.

It can be seen from Figure 9 that the USV integrated with the MPC controller, combined with a risk avoidance scheme, proactively steers, adjusts its heading before approaching the obstacle, and subsequently returns to its desired path.

4.3. Scenario 3: Trajectory Tracking with Multiple Obstacles

In this scenario, the trajectory tracking problem is implemented by the proposed MPC under multiple obstacles with different positions and lengths. The USV starts from the initial pose $p_s=[-10,-10,0]$ (m, m, deg). The parameters of the MPC controller and the desired path are set as the same as scenario 2. The first obstacle position is ${\eta }_{ob1}=[30,30]$, and its length is 3m. The second obstacle position is ${\eta }_{ob2}=[50,50]$, and its length is 6m. The safety coefficient parameters are both set as ${\lambda }_{01}={\lambda }_{02}=5$. The gravitational potential field parameters are set to ${\xi }_1={\xi }_2=0.002$, and the repulsive potential field parameters are set to ${\eta }_1=10$ and ${\eta }_2=15$. The result of trajectory tracking is given in Figure 10.

It is evident from Figure 10 that the USV fitted with the model predictive control controller utilizing the risk-enhanced approach adjusts its direction prior to encountering obstacles of varying sizes. Thereafter, it expeditiously resumes its target path, which validates the effectiveness of the proposed algorithm.

5. Conclusions

This paper presents a novel model predictive control based on state space and risk augmentation for USV trajectory tracking. In detail, a state space control model with risk augmentation is proposed for underactuated USV path tracking. The model includes track error deviation, heading angle deviation, heading angle velocity, and a risk force augmentation term and is key to this investigation. Then this paper introduces a method for MPC that combines an artificial potential field to account for multiple environmental obstacles. A risk model has been established based on the SFC system to depict the correlation between the USV and the obstacles. Finally, the study demonstrates that the algorithm can achieve convergence with a reasonable design of the weighted gain matrix and risk avoidance parameters. The technique presented in this study, which utilizes Lyapunov stability and zero-pole analysis, has considerable merit in achieving optimal control response in a straightforward manner. Through simulation, it has been demonstrated that the controller is asymptotically stable concerning track error deviation, heading angle deviation and heading angle speed, and its good stability and robustness in the presence of multiple risks are verified.

6. Future Works

Future works can be carried out in the following aspects: (i) Obstacles detection. The sensors (such as light detection and ranging (LiDAR), radar, and vision sensors) detect obstacles and provide feedback to alert the control system of their presence on the intended trajectory. If obstacle detection through sensors is being considered, it is essential to combine the detection process of multiple sensors and fuse the results before delivering them to the controller; (ii) real-time marine environment experiments using the proposed method, including multiple obstacles, random disturbances, and a robust observer design. In addition, the effect of the controller weight matrix on the convergence performance should also be investigated with the proposed MPC controller with risk augmentation.