Trajectory Tracking Predictive Control for Unmanned Surface Vehicles with Improved Nonlinear Disturbance Observer

Abstract

The motion of unmanned surface vehicles (USVs) is frequently disturbed by ocean wind, waves, and currents. A poorly designed controller will cause failures and safety problems during actual navigation. To obtain a satisfactory motion control performance for the USVs, a model predictive control (MPC) method based on an improved Nonlinear Disturbance Observer (NDO) is proposed. First, the USV model is approximately linearized and MPC is designed for the multivariable system with constraints. To compensate for the influence of disturbances, an improved NDO is designed where the calculation time for MPC is reduced. Finally, comparison simulations are conducted between MPC with the original NDO and MPC with an improved NDO, and the results show that they have similar performances to the USVs. However, the proposed method has fewer parameters that need to be tuned and is much more time-saving compared to MPC with a traditional NDO.

1. Introduction

As technology has developed, USVs have been extensively used in various fields. However, due to disturbances from the sea wind, waves, and current, trajectory tracking control is of widespread concern. There have many studies undertaken on the control of the USV trajectory tracking technologies, including the PID controller [1,2,3,4], sliding mode control [5,6,7,8], backstepping control [9,10,11,12], MPC [13,14,15,16], adaptive control [17,18,19,20] and intelligent control [21,22,23,24].

MPC has been developed as an advanced optimization control algorithm based on the superiorities of feedback correction and rolling optimization. MPC has low requirements for the model, and it can solve constrained and multivariable problems. Although the computational load increases with fractional order, presently the development of microprocessors has made it possible for such computation. Predictive compensator-based event-triggered MPC with an NDO strategy has been proposed, and the trajectory tracking control problem of USV subject to input constraints, external disturbances, and cyber-attacks has been addressed [14]. However, the output constraints are considered in this paper, and the NDO has been discretized with an approximate discretization method. The adaptive line-of-sight algorithm was developed to obtain an expected heading angle. In addition, MPC was applied to reduce the lateral error, where the sideslip angle compensation was considered [25]. In addition, to obtain accurate state variables in real time, a linear extended-state observer was designed to overcome the influence of environmental disturbances and the nonlinearity of the model. However, the linearization still caused certain deviations in model disturbance estimation. To adjust the controller parameters, the MPC controller has been used to carry out both control allocation and trajectory tracking in real time [26]. Furthermore, it has concurrently optimized closed-loop performance with reinforcement learning-based and system-identification methods. To convert chance constraints into deterministic convex constraints, a convex conditional value of risk approximation has been introduced [27]. The converted constraints were further transformed into second-order cone constraints. Then, to account for the external disturbances and fulfill physical constraints, a stochastic model predictive control (SMPC) scheme was used to design the controller. The authors in [28] designed the path planning and control of the USVs simultaneously to overcome the disadvantage of the “first-planning-then-tracking” structure, and the artificial potential field and MPC were combined to solve the planning and tracking problem. The authors in [29] proposed the finite control set model predictive control (FCS-MPC). The more practical control commands formed a limited set of control, namely the thruster propulsion angle and speed of the USV. In addition, a fast and safe collision-avoidance system was designed according to the basics of FCS-MPC, which was applicable to varying environments. The authors in [30], to guarantee precise and stable trajectory tracking performance for AUVs, proposed a novel control architecture based on model-free control principles. The combination of intelligent PID and PD feedforward control had good performance for trajectory tracking accuracy, disturbance rejections, and initial tracking error compensations. The authors in [31], to solve the surge-motion tracking control problem of an autonomous undersea vehicle (AUV) with system constraints, proposed an adaptive backstepping control scheme. Both a state feedback control scheme and an output feedback control scheme were developed for AUVs with deferred output constraints. The authors in [32], to pay more attention to the characteristics of flexibility and accessibility, proposed a fusion framework of field theoretical planning and an MPC algorithm. In addition, the trajectory smoothness and collision-avoidance constraints under a complex environment were considered. The authors in [33], to solve the fault-tolerant trajectory tracking control problem of twin-propeller non-rudder USVs subject to propeller faults, proposed an adaptive fault-tolerant trajectory tracking control scheme by utilizing the excellent nonlinearity approximation performance of neural networks. The authors in [34], to achieve autonomous cooperative formation control of underactuated USVs in a complex ocean environment, adopted a dual MPC approach based on a virtual trajectory. The authors in [35] showed that time-varying external disturbances affected the accuracy of trajectory tracking. To ensure trajectory tracking accuracy, a reduced-order extended-state observer and the super-twisting second-order sliding mode controller were proposed. The authors in [36], to solve the lumped uncertainties caused by input quantization, actuator faults, and dead zones, proposed an adaptive sliding mode tracking controller for USVs with predefined time performance. In the control design, adaptive control gains were established based on barrier functions. In addition, a predefined time-adaptive SMC scheme was adopted by introducing an auxiliary function. The authors in [37], to set the velocity of the USV converge to zero at the berth, adopted an interpolation approach to densify the waypoints at the end of the berthing trajectory. In addition, to improve computational performance during the USV berthing, an event-triggered adaptive horizon MPC approach was adopted.

Controllers are usually poorly tuned to USV motion systems, and the disturbance-rejection performance of controllers is not satisfactory due to disturbances from ocean waves, wind, and currents. Therefore, an improved NDO-based MPC method for trajectory tracking control of USVs is proposed in this paper. First, the MPC is designed for USV trajectory tracking. Then, an NDO is designed to estimate the disturbance of ocean wind, waves, and currents, which has fewer parameters. In addition, Lyapunov stability is analyzed for the overall system. Finally, the proposed method is verified by simulation experiments. The main contributions of this work can be summarized as follows: the output constraints are considered in this paper, and the NDO has been discretized with an approximate discretization method; the disturbances are compensated for by an improved NDO, and better trajectory tracking performances are obtained for USVs; also, with the improved NDO, calculation time is saved compared with the traditional NDO.

The rest of the paper is structured as follows: Section 2 introduces the USV kinematics model and dynamic model. In Section 3, the improved NDO-based MPC is designed for the USV, and Lyapunov stability is analyzed for the overall system. Comparison simulations and discussion of the results is performed in Section 4. Section 5 presents the conclusions.

2. State-Space Model of Unmanned Surface Vehicles

In general, the kinematics modeling of USVs is represented by the North–East–Down coordinate system, while the dynamics model is built in the ship coordinate system. The North–East–Down coordinate system is also called the geodetic coordinate system or the inertial coordinate system. It is usually used as the reference system, and any point on the sea can be used as the origin of the system. The ship coordinate system changes with the motion of the ship and can describe the force, moment, linear velocity, and angular velocity of a USV in various degrees of freedom.

To simplify the model, a USV model is utilized with three degrees of freedom for trajectory tracking control. The motions of yaw, surge, and sway are the most important for the trajectory tracking of the USVs, so the roll, pitch, and heave of the USVs are ignored. Thus, the USV model can be represented in the ship coordinate system and the geodetic coordinate system. This is shown in Figure 1.

In Figure 1, it can be seen the North–East–Down coordinate system is represented by $N{O}_{E}E$, and the coordinate system for the ship is described by $XOY$. The kinematics and dynamics model of the ship can be represented as:

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

(1)

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

(2)

where $\eta ={[x,y,\psi ]}^T$ represents the $x$, $y$ position and heading angle vector of the USV in the inertial coordinate system; $x$ and $y$ represent the position of the ship with regard to the North–East–Down coordinate system, $\psi$ represents the yaw angle information of the USV; $\upsilon ={[u,v,r]}^T$ is the vector of the velocity and angular velocity for the USV in the ship coordinate system; $u$, $v$, and $r$ are the surge velocity, sway velocity, and yaw angular velocity of USV, respectively; ${\tau }_u$, ${\tau }_v$, and ${\tau }_r$ represent the surge thrust, sway thrust, and yaw moment, which are the control inputs of the system; ${\tau }_d={[{\tau }_{ud},{\tau }_{vd},{\tau }_{rd}]}^T$ is the corresponding thrust and moment caused by the time-varying external disturbances. $R(\psi )$ is the rotation matrix with the relationship of $R^{-1}(\psi )=R^T(\psi )$; $M$ represents the inertial matrix of USV, where $M=M^T>0$; $C(\upsilon )$ represents the Coriolis centripetal force matrix, and $C(\upsilon )=-C{(\upsilon )}^T$; and $D(\upsilon )$ is the nonlinear hydrodynamic damping matrix. The detailed information for the matrixes is shown as follows:

$$R(\psi )=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right], M=\left[\begin{array}{ccc}{m}_{11}& 0& 0\\ 0& m_{22}& m_{23}\\ 0& m_{32}& m_{33}\end{array}\right]$$

(3)

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

(4)

According to Equations (3) and (4), the reduced kinematics and dynamics equations can be represented as:

$$\left\{\begin{array}{l}\dot{x}=u\cos \psi -v\sin \psi \\ \dot{y}=u\sin \psi +v\cos \psi \\ \dot{\psi }=r\end{array}\right.$$

(5)

$$\left\{\begin{array}{l}{m}_{11}\dot{u}-m_{22}vr+d_{11}u={\tau }_u+{\tau }_{ud}\\ m_{22}\dot{v}+m_{23}\dot{r}+m_{11}ur+d_{22}v+d_{23}r={\tau }_v+{\tau }_{vd}\\ m_{32}\dot{v}+m_{33}\dot{r}+(m_{22}-m_{11})uv+d_{32}v+d_{33}r={\tau }_r+{\tau }_{rd}\end{array}\right.$$

(6)

For the USVs, there are constraints for the actuators and the outputs. In addition, they are described as follows.

The increment constraints for inputs can be represented as:

$$\begin{array}{c}\Delta u_{\min }(t+k)\le \Delta u(t+k)\le \Delta u_{\max }(t+k)\\ k=0,1,\cdots ,N_c-1\end{array}$$

(7)

where $N_c$ denotes the control horizon.

The upper and lower-limit constraints for inputs can be represented as:

$$\begin{array}{c}{u}_{\min }(t+k)\le u(t+k)\le u_{\max }(t+k)\\ k=0,1,\cdots ,N_c-1\end{array}$$

(8)

The outputs of speed increment constraints can be represented as:

$$\begin{array}{c}\Delta {\upsilon }_{\min }(t+k)\le \Delta \upsilon (t+k)\le \Delta {\upsilon }_{\max }(t+k)\\ k=0,1,\cdots ,N_c-1\end{array}$$

(9)

The outputs of speed upper and lower-limit constraints can be represented as:

$$\begin{array}{c}{\upsilon }_{\min }(t+k)\le \upsilon (t+k)\le {\upsilon }_{\max }(t+k)\\ k=0,1,\cdots ,N_c-1\end{array}$$

(10)

The terminal equality constraint can be represented as:

$${‖\eta \left(k+N\left|t\right.\right)-{\eta }_r\left(k+N\left|t\right.\right)‖}^2=0$$

(11)

where $\eta$ denotes the vector to be controlled, and ${\eta }_r$ denotes the reference trajectory.

3. Nonlinear Disturbance Observer-Based Model Predictive Control

In this section, NDO-based MPC is designed for the three-degrees-of-freedom kinematics and dynamics of a USV with the state-space model. The design schematic diagram of NDO-based MPC is shown in Figure 2.

According to Equations (1) and (2), the state-space equation for the ship can be rewritten as:

$$\left\{\begin{array}{l}\dot{\eta }=R\left(\phi \right)\upsilon \\ \dot{\upsilon }=M^{-1}\left(\tau +{\tau }_d-C\left(\upsilon \right)\upsilon -D\left(\upsilon \right)\upsilon \right)\end{array}\right.$$

(12)

From Equation (12), the matrixes R, C, and D are nonlinear variables, which gives the model strong nonlinearity.

3.1. Model Predictive Control Design of an Unmanned Surface Vehicle

3.1.1. Discrete Linearization of an Unmanned Surface Vehicle Model

USV is a complex system with large inertia and time-delay characteristics. During navigation, it is subjected to various forces such as thrust, hydrodynamic force, hydrostatic force, and external disturbances. Therefore, the USV model has obvious nonlinear characteristics. In this paper, a simplified model of the USV is applied, and the uncertainty is dealt with by the NDO together with the disturbances from the ocean environment. In addition, the model is linearized.

In this paper, the NDO is designed to compensate for the disturbance. Therefore, a linear model of the USV is sufficient for MPC design, where the uncertainty from the linearization of the USV model can be solved with an NDO. Therefore, the model is linearized first and then discretized. Finally, the optimal control sequence is obtained according to the linear model predictive control.

The linearization of a nonlinear system can be divided into approximate linearization and exact linearization. Among them, the approximate linearization method is relatively simple with high applicability but low accuracy. The precision of the accurate linearization method is high, but it should be noted that a special case analysis of a single system is difficult and has poor universality.

The reference trajectory is represented in Equation (13) with environmental disturbances. The first-order Taylor expansion can be obtained at any point $(x_r,u_r)$; then approximate linearization is achieved by leaving the higher-order terms. This can be seen in Equation (14).

$${\dot{x}}_r=f(x_r,u_r)$$

(13)

$$\dot{x}=f\left(xr,ur\right)+{\left.\frac{\partial f}{\partial x}\right|}_{\begin{array}{c}x=xr\\ u=ur\end{array}}\left(x-xr\right)+{\left.\frac{\partial f}{\partial u}\right|}_{\begin{array}{c}x=xr\\ u=ur\end{array}}\left(u-ur\right)$$

(14)

Subtract Equation (13) from (14), and the following equation can be achieved.

$$\dot{\tilde{x}}=A\tilde{x}+B\tilde{u}$$

(15)

and $\tilde{x}=x-x_r$, $\tilde{u}=u-u_r$, $A={\left.\frac{\partial f}{\partial x}\right|}_{\begin{array}{c}x=xr\\ u=ur\end{array}}$, $B={\left.\frac{\partial f}{\partial u}\right|}_{\begin{array}{c}x=xr\\ u=ur\end{array}}$.

The discrete form for Equation (15) is shown as follows:

$$\left\{\begin{array}{l}\tilde{x}\left(k+1\right)=A_d\tilde{x}\left(k\right)+B_d\tilde{u}\left(k\right)\\ \tilde{y}(k)=C\tilde{x}\left(k\right)\end{array}\right.$$

(16)

where $A_d=TA+I$, $B_d=TB$, $\tilde{y}(k)=y(k)-y_r(k)$, $T$ is the discretization step.

3.1.2. Objective Function Design

To ensure that the USV can track the trajectory smoothly and quickly, the cost function shown in Equation (17) is applied, which concerns the increments of the control variables and the errors of the system states.

$$\begin{array}{cc}\min & J=\min \left\{{{\displaystyle \sum _{i=1}^{N_p}‖\eta \left(k+i\right)-{\eta }_r\left(k+i\right)‖}}_Q^2+{{\displaystyle \sum _{i=1}^{N_c-1}‖\Delta u\left(k+i\right)‖}}_R^2\right\}\end{array}$$

(17)

where $Q$ and $R$ are the weight matrixes for tracking errors and increments of the control variables, respectively; and $N_p$ is the prediction horizon. $\Delta u\left(k+i\right)$ is the variable of the increments of the control variables, so it can be obtained as follows:

$$\xi (k)=\left[\begin{array}{c}\tilde{x}(k)\\ \tilde{u}(k-1)\end{array}\right]$$

(18)

The new state-space equation is represented as:

$$\left\{\begin{array}{l}\xi (k+1)=\tilde{A}\xi (k)+\tilde{B}\Delta u(k)\\ \tilde{y}(k)=\tilde{C}\xi (k)\end{array}\right.$$

(19)

where $\tilde{A}=\left[\begin{array}{cc}{A}_d& B_d\\ 0& I_{Nu}\end{array}\right]$, $\tilde{B}=\left[\begin{array}{c}{B}_d\\ I_{Nu}\end{array}\right]$, $\tilde{C}=[\begin{array}{cc}{I}_{Nx}& 0\end{array}]$, $N_u$ denotes the number of control variables, $N_x$ denotes the number of state variables.

Therefore, the system prediction outputs can be calculated as follows:

$$\begin{array}{l}Y=\Psi \xi \left(k\right)+H\tilde{u}\left(k\right)\\ J=\frac{1}{2}\Delta u{\left(k\right)}^T{H}_J\Delta u\left(k\right)+f_J\Delta u\left(k\right)\end{array}$$

(20)

where $H_J=2\left(H^T{Q}_{c}H+R_c\right)$, $f_J=2\Psi \xi \left(k\right)Q_{c}H$, $Q_c={\left[\begin{array}{ccc}Q& & \\ & \ddots & \\ & & Q\end{array}\right]}_{N_p\times N_p}$, $R_c={\left[\begin{array}{ccc}R& & \\ & \ddots & \\ & & R\end{array}\right]}_{N_c\times N_c}$.

3.2. Nonlinear Disturbance Observer Design of Unmanned Surface Vehicles

To make MPC for a USV more applicable, a nonlinear disturbance observer is designed, which can estimate and compensate for the external environmental disturbance received by the USV. Therefore, the stability and anti-disturbance performance is improved for the USV, while capsizing and unnecessary navigation accidents are avoided for the USV.

According to the mathematical model of the USV, the state equation is designed as follows:

$${\dot{\widehat{\tau }}}_d=K_0({\tau }_d-{\widehat{\tau }}_d)=-K_0{\widehat{\tau }}_d+K_0(M\dot{\upsilon }+C(\upsilon )\upsilon +D(\upsilon )\upsilon -\tau )$$

(21)

where $K_0$ is a three-dimensional positive definite matrix; the estimated disturbance values ${\widehat{\tau }}_d$ can be specifically written as ${\widehat{\tau }}_d={[{\widehat{\tau }}_{ud},{\widehat{\tau }}_{vd},{\widehat{\tau }}_{rd}]}^T$, which are the estimated values of surge disturbance, sway disturbance, and yaw direction disturbance.

It can be seen from Equation (12) that $\upsilon$ of a USV can be obtained directly, but the derivative term of the speed state variable $\dot{\upsilon }$ cannot be obtained directly. Therefore, it is necessary to improve the disturbance observer. The variable $\beta$ can be selected as the intermediate assignment variable of the observer, which is expressed as follows:

$$\beta ={\widehat{\tau }}_d-K_{0}M\upsilon$$

(22)

Then,

$$\begin{array}{ll}\dot{\beta }& ={\dot{\widehat{\tau }}}_d-K_{0}M\dot{\upsilon }\\ & =K_0(M\dot{\upsilon }+C(\upsilon )\upsilon +D(\upsilon )\upsilon -\tau )-K_0{\widehat{\tau }}_d-K_{0}M\dot{\upsilon }\\ & =-K_0(\beta +K_{0}M\upsilon )+K_0(C(\upsilon )\upsilon +D(\upsilon )\upsilon -\tau )\\ & =-K_0\beta -K_0(K_{0}M\upsilon -C(\upsilon )\upsilon -D(\upsilon )\upsilon +\tau )\end{array}$$

(23)

Therefore, the new equation of the improved NDO is:

$$\begin{array}{c}{\widehat{\tau }}_d=\beta +K_{0}M\upsilon \\ \dot{\beta }=-K_0\beta -K_0(K_{0}M\upsilon -C(\upsilon )\upsilon -D(\upsilon )\upsilon +\tau )\end{array}$$

(24)

The improved equation avoids the calculation of $\dot{\upsilon }$ and simplifies the calculation process. Therefore, it can improve calculation efficiency and save calculation time.

Equation (24) is in a continuous form, and cannot be directly used for MPC design. Therefore, it is discretized with the approximate discretization method:

$$\begin{array}{c}{\tau }_d\left(k+1\right)={\beta }_d+K_{d1}M\upsilon +{\tau }_d\left(k\right)\\ \beta \left(k+1\right)=-K_{d2}\beta \left(k\right)-K_{d1}\left(K_{0}M\upsilon -C\left(\upsilon \right)\upsilon -D\left(\upsilon \right)\upsilon +\tau \right)\end{array}$$

(25)

and ${\beta }_d=T\beta$, $K_{d1}=T{K}_0$, and $K_{d2}=T{{\rm K}}_0-I$.

3.3. Stability Analysis of Unmanned Surface Vehicles

3.3.1. Stability Analysis of the Model Predictive Control of Unmanned Surface Vehicles

To verify the stability of the USV control system under MPC, the Lyapunov function defined as $V^0(k)$ is selected:

$$V^0(k)=\underset{\Delta v}{\min }{{\displaystyle \sum _{i=1}^{N_p}‖\eta \left(k+i\left|t\right.\right)-{\eta }_d\left(k+i\left|t\right.\right)‖}}_Q^2+{{\displaystyle \sum _{i=1}^{N_c-1}‖\Delta u\left(k+i\left|t\right.\right)‖}}_R^2$$

(26)

If the control horizon $N_{\mathrm{c}}$ is defined to be equal to the prediction horizon $N_p$, the above equation can be simplified as follows:

$$V^0(k)=\underset{\Delta v}{\min }{\displaystyle \sum _{i=1}^N{‖\eta \left(k+i\left|t\right.\right)-{\eta }_d\left(k+i\left|t\right.\right)‖}_Q^2+{‖\Delta u\left(k+i\left|t\right.\right)‖}_R^2}$$

(27)

The quadratic function is always greater than 0, so its positive definiteness is proven:

$$V^0(k)\ge 0$$

(28)

Then, we only need to prove that $V^0(k)$ is decreasing, then its stability is proven.

$$\begin{array}{ll}{V}^0(k+1)\hfill & =\underset{\Delta \upsilon }{min}\left\{{\displaystyle \sum _{i=1}^N}{∥\mathbf{\eta }\left(k+i+1\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+i+1\left|t\right.\right)∥}_{\mathbf{Q}}^2+{∥\Delta \mathbf{u}\left(k+i+1\left|t\right.\right)∥}_{\mathbf{R}}^2\right\}\\ & =\underset{\Delta v}{\underset{\Delta \upsilon }{min}\left\{\begin{array}{c}\sum _{i=1}^N\left({∥\mathbf{\eta }\left(k+i\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+i\left|t\right.\right)∥}_{\mathbf{Q}}^2+{∥\Delta \mathbf{u}\left(k+i\left|t\right.\right)∥}_{\mathbf{R}}^2\right)-\hfill \\ {∥\mathbf{\eta }\left(k+1\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+1\left|t\right.\right)∥}_{\mathbf{Q}}^2-{∥\Delta \mathbf{u}\left(k+1\left|t\right.\right)∥}_{\mathbf{R}}^2+\hfill \\ {∥\mathbf{\eta }\left(k+1+N\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+1+N\left|t\right.\right)∥}_{\mathbf{Q}}^2+{∥\Delta \mathbf{u}\left(k+1+N\left|t\right.\right)∥}_{\mathbf{R}}^2\hfill \end{array}\right\}}\hfill \\ & =-{∥\mathbf{\eta }\left(k+1\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+1\left|t\right.\right)∥}_{\mathbf{Q}}^2-{∥\Delta \mathbf{u}\left(k+1\left|t\right.\right)∥}_{\mathbf{R}}^2+\hfill \\ & \underset{\Delta \upsilon }{min}\left\{\begin{array}{c}\sum _{i=1}^N\left({∥\mathbf{\eta }\left(k+i\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+i\left|t\right.\right)∥}_{\mathbf{Q}}^2+{∥\Delta \mathbf{u}\left(k+i\left|t\right.\right)∥}_{\mathbf{R}}^2\right)+\hfill \\ {∥\mathbf{\eta }\left(k+1+N\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+1+N\left|t\right.\right)∥}_{\mathbf{Q}}^2+{∥\Delta \mathbf{u}\left(k+1+N\left|t\right.\right)∥}_{\mathbf{R}}^2\hfill \end{array}\right\}\hfill \\ & \le -{∥\mathbf{\eta }\left(k+1\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+1\left|t\right.\right)∥}_{\mathbf{Q}}^2-{∥\Delta \mathbf{u}\left(k+1\left|t\right.\right)∥}_{\mathbf{R}}^2+V^0\left(k\right)+\hfill \\ & \underset{\Delta \upsilon }{min}\left\{{∥\mathbf{\eta }\left(k+1+N\left|t\right.\right)-{\mathbf{\eta }}_{\mathbf{d}}\left(k+1+N\left|t\right.\right)∥}_{\mathbf{Q}}^2+{∥\Delta \mathbf{u}\left(k+1+N\left|t\right.\right)∥}_{\mathbf{R}}^2\right\}\hfill \end{array}$$

(29)

The terminal equation constraint is:

$${‖\eta \left(k+1+N\left|t\right.\right)-{\eta }_r\left(k+1+N\left|t\right.\right)‖}_Q^2=0$$

(30)

Furthermore,

$$\underset{\Delta \upsilon }{\min }\left\{{‖\eta \left(k+1+N\left|t\right.\right)-{\eta }_d\left(k+1+N\left|t\right.\right)‖}_Q^2+{‖\Delta u\left(k+1+N\left|t\right.\right)‖}_R^2\right\}=0$$

(31)

$${‖\eta \left(k+i\left|t\right.\right)-{\eta }_r\left(k+i\left|t\right.\right)‖}_Q^2+{‖\Delta u\left(k+i\left|t\right.\right)‖}_R^2=0$$

(32)

Therefore, $V^0(k+1)\le V^0(k)$, and the stability of MPC is proven.

3.3.2. Stability Analysis of the Nonlinear Disturbance Observer of Unmanned Surface Vehicles

To verify the stability of the NDO and ensure that it can be applied to the trajectory tracking control system, it is necessary first to define the variable of the difference between the observer’s estimated value and the actual value of the external disturbance to the USV:

$${\tilde{\tau }}_d={\tau }_d-{\widehat{\tau }}_d$$

(33)

Considering the kinematic Equations (2), (24) and (33) of the USV, then by calculating the derivative of time on both sides of Equation (27), the formula can be represented as follows:

$$\begin{array}{ll}{\dot{\widehat{\tau }}}_d& =\dot{\beta }+K_{0}M\dot{\upsilon }\\ & =-K_0\beta -K_0(K_{0}M\upsilon -C(\upsilon )\upsilon -D(\upsilon )\upsilon +\tau )+K_0(-C(\upsilon )\upsilon -D(\upsilon )\upsilon +\tau +{\tau }_d)\\ & =K_0({\tau }_d-(\beta +K_{0}M\upsilon ))\\ & =K_0{\tilde{\tau }}_d\end{array}$$

(34)

From Equation (34), one can calculate the derivative of both sides of Equation (33) with respect to time, and simplify it to obtain:

$${\dot{\tilde{\tau }}}_d={\dot{\tau }}_d-{\dot{\widehat{\tau }}}_d={\dot{\tau }}_d-K_0{\tilde{\tau }}_d$$

(35)

The Lyapunov method is used to verify the stability of the disturbance observer, and the appropriate Lyapunov function is selected as follows:

$$V_d=\frac{1}{2}{\tilde{\tau }}_d^{\mathrm{T}}{\tilde{\tau }}_d$$

(36)

According to Equation (35), the derivative of both sides of Equation (36) with respect to time can be obtained as follows:

$${\dot{V}}_d={\tilde{\tau }}_d^{\mathrm{T}}{\dot{\tilde{\tau }}}_d=-{\tilde{\tau }}^{\mathrm{T}}{K}_0\tilde{\tau }+{\tilde{\tau }}_d^{\mathrm{T}}{\dot{\tau }}_d$$

(37)

According to Young’s inequality theory,

$${\tilde{\tau }}_d^{\mathrm{T}}{\dot{\tau }}_d\le a_1{\tilde{\tau }}_d^{\mathrm{T}}{\tilde{\tau }}_d+\frac{C_d^2}{4a_1}$$

(38)

and $a_1>0$, $C_d$ is the limit of the disturbance change rate.

From Equations (37) and (38), this can be written as inequality:

$${\dot{V}}_d\le -{\tilde{\tau }}^{\mathrm{T}}{K}_0\tilde{\tau }+a_1{\tilde{\tau }}_d^{\mathrm{T}}{\tilde{\tau }}_d+\frac{C_d^2}{4a_1}\le -2({\lambda }_{\min }(K_0)-a_1)V_d+\frac{C_d^2}{4a_1}$$

(39)

Take

$$\left\{\begin{array}{l}{\mu }_0=2({\lambda }_{\min }(K_0)-a_1)>0\\ C_0=\frac{C_d^2}{4a_1}>0\end{array}\right.$$

(40)

and ${\lambda }_{\min }(K_0)>a_1$. Equation (39) can be abbreviated as follows:

$${\dot{V}}_d\le -{\mu }_0{V}_d+C_0$$

(41)

According to Equation (36), $V_d$ is always greater than 0, and from (41), the result can be obtained as follows:

$$0\le V_d\le \frac{C_0}{{\mu }_0}+(V_d(0)-\frac{C_0}{{\mu }_0})e^{-{\mu }_{0}t}$$

(42)

According to Equation (42), it shows that the Lyapunov function $V_d$ stays in a closed ball of some radius whose origin is the center of the sphere. In addition, it is uniformly ultimately bounded. In addition, the radius of the sphere is $R_{V_d}=\frac{C_0}{{\mu }_0}=\frac{C_d^2}{8a_1({\lambda }_{\min }(K_0)-a_1)}$. According to Equation (36), it can be found that the disturbance estimation error variable ${\tilde{\tau }}_d$ also converges to the sphere radius $R_{{\tilde{\tau }}_d}=\sqrt{2\frac{C_0}{{\mu }_0}}=\frac{C_d}{\sqrt{4a_1({\lambda }_{\min }(K_0)-a_1)}}$ with the origin as the center of the sphere. At the same time, it can also be known that if the external environmental disturbance value ${\tau }_d$ of USV is an arbitrary unknown constant value, then the boundary of the disturbance charge rate is $C_d=0$. According to Equation (39), the observer estimation error value ${\tilde{\tau }}_d$ can converge to the origin.

According to Equation (40), as long as the appropriate observer parameters $a_1$ and $K_0$ can be selected, an arbitrarily small error convergence radius $R_{{\tilde{\tau }}_d}$ can be obtained. In other words, NDO can estimate the external environmental disturbance suffered by the USV according to an arbitrarily small error, and the estimation accuracy depends on the selected parameters.

4. Results and Discussions

To verify the influence of the improved NDO-based MPC, it is applied for trajectory tracking control of a USV called CyberShip II. The tracking errors and performance of the USV are shown in this section. In addition, the computational efficiency of the improved NDO is verified.

4.1. Model Parameters of Unmanned Surface Vehicle

In Equations (3) and (4), $M=\left[\begin{array}{ccc}25.8& 0& 0\\ 0& 33.8& 1\\ 0& 1& 2.8\end{array}\right]$, $D=\left[\begin{array}{ccc}0.72& 0& 0\\ 0& 0.86& -0.11\\ 0& -0.11& 1.90\end{array}\right]$. In (16), $C=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$. In (17), $Q=\left[\begin{array}{cccccc}10& 0& 0& 0& 0& 0\\ 0& 10& 0& 0& 0& 0\\ 0& 0& 10& 0& 0& 0\\ 0& 0& 0& 10& 0& 0\\ 0& 0& 0& 0& 10& 0\\ 0& 0& 0& 0& 0& 10\end{array}\right]$, $R=\left[\begin{array}{ccc}0.01& 0& 0\\ 0& 0.01& 0\\ 0& 0& 0.01\end{array}\right]$.

In the simulation, $\eta ={[0 0{ 0}^{\circ }]}^{\mathrm{T}}$, which is set as the initial state of USV; $\upsilon ={[0 0 0]}^{\mathrm{T}}$, which is set as the initial speed of USV. In addition, the reference trajectories of USV are shown as follows:

$$\left\{\begin{array}{ll}{x}_d=2\sin (0.02t),\hfill & 0\le t\le 500\hfill \\ y_d=2-2\cos (0.02t),\hfill & 0\le t\le 500\hfill \\ {\psi }_d=\arctan ({\dot{x}}_d/{\dot{y}}_d),\hfill & 0\le t\le 500\hfill \end{array}\right.$$

(43)

$$\left\{\begin{array}{ll}{x}_d=8\sin (0.02t),\hfill & 0\le t\le 500\hfill \\ y_d=t,\hfill & 0\le t\le 500\hfill \\ {\psi }_d=\arctan ({\dot{x}}_d/{\dot{y}}_d),\hfill & 0\le t\le 500\hfill \end{array}\right.$$

(44)

$T=0.1$, which is set as the simulation sampling time; $N_x=6$, which is set as the number of states; $N_u=3$, which is set as the number of control variables; $N_p=20$, which is set as the prediction horizon; $N_c=10$, which is set as the control horizon.

4.2. Simulation Results and Analysis

In MATLAB 2021a, NDO-based MPC for trajectory tracking control of a USV is compared with MPC without an observer. Simulations with different disturbances are performed to verify the anti-disturbance and robustness performances. The performances of the improved NDO are discussed.

The composite model of external disturbances meets the requirements of Level 3 sea conditions. The specific external wind, wave, and current disturbances settings are as follows:

$$\left\{\begin{array}{c}{\tau }_{du}=m_{11}h\left(s\right)w_u\left(s\right)\\ {\tau }_{dv}=m_{22}h\left(s\right)w_v\left(s\right)\\ {\tau }_{dr}=m_{33}h\left(s\right)w_r\left(s\right)\end{array}\right.$$

(45)

where wave transfer function $h\left(s\right)=\frac{K_{\omega }s}{s^2+2\zeta {\omega }_{0}s+{\omega }_0^2}$, $K_{\omega }=2\zeta {\omega }_0{\sigma }_0$, ${\omega }_0$, $\zeta$, and ${\sigma }_0$ represent wave frequency, wave strength gain, and damping constant, respectively; $K_{\omega }=0.255$, ${\omega }_0=0.808$. $w_u\left(s\right)$, $w_v\left(s\right)$, and $w_r\left(s\right)$ represent random white-noise disturbances, then the noise power is set to 0.01, 0.005, and 0.1, respectively.

The simulation results of improved NDO-based MPC for trajectory tracking control and traditional MPC with disturbances, when the reference trajectory is a circle, are shown in Figure 3. The comparisons of improved NDO-based MPC for trajectory tracking control and traditional MPC with disturbances, when the reference trajectory is sinusoidal, are shown in Figure 4. The calculation times between the two NDO methods with disturbances are shown in Figure 5. The trajectory tracking errors of the three trajectory tracking methods with different parameters are shown in Figure 6.

Figure 3 shows that the improved NDO-based MPC has better disturbance-rejection performance than MPC without an observer when the reference trajectory is a circle. It also can be seen from the simulations that both methods can track the reference trajectory. However, the former can track the reference trajectory smoothly, while the latter fluctuates a lot. The surge position error range of MPC without an observer for trajectory tracking control is −0.1 to 0.1 m; the sway position error range is −0.1 to 0.1 m; and the yaw angle error range is −0.05 to 0.05. Although the surge position error range of the NDO-based MPC is −0.005 to 0.005 m, the sway position error range is −0.005 to 0.005 m and the yaw angle error range is −0.005 to 0.005. Figure 4 shows the comparisons of the two trajectory tracking methods with disturbances when the reference trajectory is sinusoidal. In addition, the improved NDO-based MPC has lower tracking errors than MPC without an observer. The surge position error range of MPC without an observer for trajectory tracking control is −0.05 to 0.05 m; the sway position error range is −0.02 to 0.02 m; and the yaw angle error range is 0.00 to 0.05. Although the surge position error range of NDO-based MPC is 0.05 to 0.05 m, the sway position error range is 0.02 to 0.06 m, and the yaw angle error range is 0.000 to 0.005. Figure 5 shows that the improved NDO has better performance than the unimproved NDO in terms of the calculation time with disturbances.

We do not have a real ship for this experiment. To overcome this shortage, the ship parameters were changed when we did some comparisons between different methods to ensure the robustness of the proposed method. The USV model has uncertainty, so three methods were used for trajectory tracking for the USV with different model parameters. The three methods are unimproved NDO-based MPC, improved NDO-based MPC, and MPC without NDO. Figure 6 shows that the improved NDO and unimproved NDO effectively reduce the roughness caused by model uncertainty. In addition, the unimproved NDO has similar performance of tracking errors compared to the improved NDO-based MPC.

In addition, a comparison of the calculation time of two NDOs with disturbances is shown in

Table 1. Comparison of the calculation time of the two methods.

	Improved NDO Based MPC	Unimproved NDO Based MPC
Average calculation time(s)	0.0020	0.0024
Maximum single calculation time(s)	0.0046	0.0064

. It shows the average calculation time and maximum single calculation time of the two NDOs. $IAE={\displaystyle {\int }_0^t\left|e(\zeta )\right|}d\zeta$ and $RMSE={(\frac{1}{t}{\displaystyle {\int }_0^t{e}^2(\zeta )d\zeta })}^{1/2}$ are used to evaluate the tracking effect and steady-state performance. The smaller the values of IAE and RMSE, the better the control performance of the scheme applied. In addition, the comprehensive performance comparisons of the position and speed tracking errors of the two methods are shown in

Table 2. Comparison of position and velocity errors of the two methods.

Tracking Error	Computing Method	Improved NDO Based MPC	Non-Observer
$y_e$	$IAE$	9.3209	141.6562
	$RMSE$	0.0072	0.1104
$x_e$	$IAE$	9.2273	135.6914
	$RMSE$	0.0071	0.1061
${\psi }_e$	$IAE$	10.7869	79.6175
	$RMSE$	0.0077	0.0574
$‖u_e‖$	$IAE$	6.3167	55.5531
	$RMSE$	0.0055	0.0435
$‖{\upsilon }_e‖$	$IAE$	3.9132	58.2295
	$RMSE$	0.0030	0.0425
$‖r_e‖$	$IAE$	6.1470	40.2591
	$RMSE$	0.0050	0.0290

. Table 2 shows the $IAE$ and $RMSE$ of the two methods.

From Table 1, the calculation time of the improved NDO is much lower than that of the traditional NDO. The average calculation time of the improved NDO is 0.0020, and it is 0.0024 for the traditional NDO. The maximum single calculation time of the improved NDO is 0.0046, and it is 0.0064 for the traditional NDO. The results show that, compared with the traditional NDO, the average calculation time of the improved NDO is decreased by 16.67%, and the maximum individual calculation time is decreased by 28.13%.

From Table 2, the $IAE$ and $RMSE$ of NDO-based MPC are lower than the MPC without an observer. NDO-based MPC effectively enhances the anti-disturbance performance of the system. MPC without observer trajectory tracking control has the characteristics of the predictive model, rolling optimization, and feedback correction, which can resist external disturbances and model mismatches to some extent.

With the data shown in Table 1 and Table 2, NDO-based MPC is superior to MPC without an observer for trajectory tracking control in terms of position and speed tracking errors.

The NDO is designed to estimate the external disturbances suffered, to improve the anti-disturbance performance of the USV. Therefore, the comparison of estimated values of the NDO and actual disturbances is shown in Figure 7.

Figure 7 shows the relationship between the estimated values of the NDO and the actual values of the disturbance. The NDO has good estimation performance in terms of the disturbances, including surge disturbance force, sway disturbance force, and yaw disturbance force.

5. Conclusions

In this paper, an improved NDO-based MPC for trajectory tracking is proposed to guarantee the stable motion of a USV, which suffers various disturbances from the ocean wind, waves, and currents. MPC is used to optimize the system torque based on the measured position and speed state variables. Then, the NDO is designed to estimate the disturbances, and the estimated torque is compensated for in the controller. Estimation errors can converge to zero in a finite time. The simulation results show that NDO-based MPC can effectively compensate for external disturbances and obtain good tracking and disturbance-rejection performance. The proposed method has a similar tracking performance to the USVs with the MPC based on unimproved NDO, but the improved NDO-based MPC is far quicker. However, due to the linearization of the model of the USV, the method only shows good performance in a near-neighbor area around the operation point. For large-difference operation points, the parameters need to be retuned.