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

1. Introduction

Recently, as the continuous development of technology, USVs have been extensive used in various fields. However, due to the disturbances from the sea wind, wave and current, the trajectory tracking control is one of the widespread concern topics. There are many researches about the control for the trajectory tracking technologies of USVs, including 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. Therefore, MPC can solve constrained and multivariable problems. Adaptive line-of-sight algorithm was developed to get the expected heading angle. And MPC was applied to reduce the lateral error, where the sideslip angle compensation was considered in [25]. In addition, in order to get accurate state variables in real time, a linear extended state observer was designed to overcome the influence of environment disturbances and the nonlinearity of the model. However, the linearization still caused certain deviation to the model disturbance estimation. In [26], to adjust the controller parameters, the MPC controller was used to carry out both control allocation and trajectory tracking in real-time. In addition, it concurrently optimized the closed loop performance with reinforcement learning-based and system identification methods. In [27], to convert the chance constraints into deterministic convex constraints, a convex conditional value at risk approximation was introduced. And the converted constraints were further transformed into the second order cone constraints. Then, to decline the external disturbances and fulfill the physical constraints, a stochastic model predictive control (SMPC) scheme was used to design the controller. In [28], the path planning and controller of the USVs was designed simultaneously to overcome the disadvantage of the “first planning then tracking” structure, and the artificial potential field and MPC was combined to solve the planning and tracking problem. In [29], finite control set model predictive control (FCS-MPC) was proposed. The more practical control commands formed a limited set of control: the thruster propulsion angle and speed about the USV. And a fast and safe collision avoidance system was designed according to the basic of FCS-MPC, which was applicable for varying environment.

The controllers are usually poorly tuned for the USV motion system and the disturbance rejection performance of controllers is not satisfactory due to the disturbances from the ocean wave, wind and current. Therefore, an improved NDO based MPC method for trajectory tracking control of USV is proposed in this paper. Firstly, the model predictive controller is designed for the USV trajectory tracking. Then, NDO is designed to estimate the disturbance of ocean wind, wave and current, which has less parameters. And, the 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 disturbances are compensated with an improved NDO, and better trajectory tracking performance are obtained for the USVs; also, with the improved NDO, the calculation time is saved compared with the traditional NDO.

The following 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 experiments and results discussion are performed in Section 4. Section 5 presents the conclusions.

2. State Space Model of Unmanned Surface Vehicle

In general, the kinematics modeling of USV is represented in the North East Down coordinate system, while the dynamics model is built in the ship coordinate system. Among them, 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 geodetic coordinate system. The ship coordinate system changes with the motion of the ship and can describe the force, moment, linear velocity, and angular velocity of the unmanned ship in various degrees of freedom.

In order 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, and it 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 obtained 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 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 USV; $\upsilon ={[u,v,r]}^T$ is the vector of the velocity and angular velocity for 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 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$; $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]\text{，}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]\text{，}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 equation (3) and (4), the reduced kinematics and dynamics equations can be obtained 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. And 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 limits 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)‖}_Q^2=0$$

(11)

where $\eta$ denotes the vector to be controlled, ${\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 degree of freedom kinematics and dynamics of USV with state space model. The design schematic diagram of NDO based MPC is shown in Figure 2.

According to equation (1) and (2), the state space equation for the ship is rewritten as 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)

3.1. Model Predictive Control Design of Unmanned Surface Vehicle

3.1.1. Discrete Linearization of Unmanned Surface Vehicle Model

In this paper, the NDO is designed to compensate the disturbance. Hence, a linear model of the USV is sufficient for the MPC design, where the uncertainty from the linearization of the USV model can be solved with NDO. So, the model is linearized firstly 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 lofty applicability, however low accuracy. The precision of the accurate linearization method is lofty, but it is essential that a special case analysis of a single system, which is tough 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, it 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), the 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

In order to ensure that the surface unmanned ship can track the trajectory smoothly and quickly. The cost function shown in equation (17) is applied, which is about the increments of the control variables and the errors of the system states.

$$\min J={{\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$$

(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 about the increments of the control variables, so it can be obtained as:

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

Hence, the system prediction outputs can be calculated as:

$$\begin{array}{c}Y=\Psi \xi \left(k\right)+H\tilde{u}\left(k\right)\\ J=\frac{1}{2}△u{\left(k\right)}^T{H}_J△u\left(k\right)+f_J△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 Vehicle

In order to make the MPC for the USV much more applicable, a nonlinear disturbance observer is designed. It can estimate and compensate the external environmental disturbance received by USV, so the stability and anti-disturbance performance is improved for the USV, while the rollover and unnecessary navigation accidents are avoided for the USV.

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

$${\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 USV can be obtained directly, but the derivative term of the speed state variable $\dot{\upsilon }$ can not 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:

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

(22)

Then,

$$\begin{array}{c}\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)

So, 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 new equation avoids the calculation of $\dot{\upsilon }$ and simplifies the calculation process. Therefore, it can improve calculation efficiency and save calculation time.

The equation (24) is in a continuous form, and it can not be directly used for the MPC design, hence it is discretized with 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 Vehicle

3.3.1. Stability Analysis of Model Predictive Control of Unmanned Surface Vehicle

In order 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 is defined to be equal the prediction horizon, the above equation can be simplified as:

$$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 proved:

$${‖\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=0$$

(28)

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

$$\begin{array}{c}{V}^0(k+1)=\underset{\Delta \upsilon }{\min }\left\{{\displaystyle \sum _{i=1}^N{‖\eta \left(k+i+1\left|t\right.\right)-{\eta }_d\left(k+i+1\left|t\right.\right)‖}_Q^2+{‖\Delta u\left(k+i+1\left|t\right.\right)‖}_R^2}\right\}\\ =\underset{\Delta v}{\underset{\Delta \upsilon }{\min }\left\{\begin{array}{c}{\displaystyle \sum _{i=1}^N\left({‖\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\right)}-\\ {‖\eta \left(k+1\left|t\right.\right)-{\eta }_d\left(k+1\left|t\right.\right)‖}_Q^2-{‖\Delta u\left(k+1\left|t\right.\right)‖}_R^2+\\ {‖\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\end{array}\right\}}\\ =-{‖\eta \left(k+1\left|t\right.\right)-{\eta }_d\left(k+1\left|t\right.\right)‖}_Q^2-{‖\Delta u\left(k+1\left|t\right.\right)‖}_R^2+\\ \underset{\Delta \upsilon }{\min }\left\{\begin{array}{l}{\displaystyle \sum _{i=1}^N\left({‖\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\right)}+\\ {‖\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\end{array}\right\}\\ \le -{‖\eta \left(k+1\left|t\right.\right)-{\eta }_d\left(k+1\left|t\right.\right)‖}_Q^2-{‖\Delta u\left(k+1\left|t\right.\right)‖}_R^2+V^0(k)+\\ \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\}\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)

And,

$$\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 proved.

3.3.2. Stability Analysis of Nonlinear Disturbance Observer of Unmanned Surface Vehicle

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

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

(33)

Considering the kinematic equation (2), (24) and (33) of USV, then calculating the derivative of time on both sides of equation (27), the formula is represented as:

$$\begin{array}{c}{\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 the equation (34), calculate the derivative of both sides of equation (33) with respect to time, and simplify it to get:

$${\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:

$$V_d=\frac{1}{2}{\tilde{\tau }}_d^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:

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

(37)

According to Young's inequality theory, so:

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

(38)

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

From equation (37) and (38), it can be written as inequality:

$${\dot{V}}_d\le -{\tilde{\tau }}^T{K}_0\tilde{\tau }+a_1{\tilde{\tau }}^T{\dot{\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)

Equation (39) can be abbreviated as:

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

(41)

The result can be obtained as:

$$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. And 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 obtained 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, the constant limit $C_d$ is zero. 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 unmanned ship according to an arbitrarily small error, and the estimation accuracy depends on the selected parameters.

4. Results and Discussions

In order to verify the influence of the improved NDO based MPC, it is applied for trajectory tracking control of USV called CyberShip II. The tracking errors and performance of USV is shown in this part. 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 ={[000^{\circ }]}^T$ which is set as the initial state of USV; $\upsilon ={[000]}^T$ which is set as the initial speed of USV. And 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. Experiments results and Analysis

NDO based MPC for trajectory tracking control of USV is compared with MPC without observer. Experiments with different disturbances are performed to verify the anti-disturbance and robustness performance. The performance of the improved NDO is 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:

$$\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 experiments result of improved NDO based MPC for trajectory tracking control and traditional MPC with disturbances are shown in Figure 3. The tracking errors of the two control methods are shown in Figure 4. The calculation time between the two NDO methods with disturbances are shown in Figure 5. The trajectory tracking errors of three methods trajectory tracking with different parameters are shown in Figure 6.

In Figure 3, it shows that improved NDO based MPC has better disturbance rejection performance than MPC without observer. It also can be seen from the experiment that both methods can track the reference trajectory. But the former can track the reference trajectory stably, while the latter fluctuates a lot. In Figure 4, it shows the comparisons of the two trajectory tracking errors with disturbances. And the improved NDO based MPC has lower tracking errors than MPC without observer. The surge position error range of MPC without observer for trajectory tracking control is -0.1 to 0.1 meters, the sway position error range is -0.1 to 0.1 meters and the yaw angle error range is -0.05 to 0.05. While the surge position error range of NDO based MPC is -0.005 to 0.005 meters, the sway position error range is -0.005 to 0.005 meters and the yaw angle error range is -0.005 to 0.005. In Figure 5, it shows that the improved NDO has better performance than unimproved NDO about the calculation time with disturbances.

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. In Figure 6, it shows that the improved NDO and unimproved NDO effectively reduce the instability caused by model uncertainty. And the unimproved NDO has similar perfomance of tracking errors with improved NDO based MPC.

In addition, the comparison of the calculation time of two NDOs with disturbances are shown in Table 1. 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 values of IAE and RMSE, the better control performance of the scheme applied. And the comprehensive performance comparisons of the position and speed tracking errors of two methods are shown in Table 2. It shows the $IAE$ and $RMSE$ of the two methods.

From Table 1, the calculation time of improved NDO is much lower than that of traditional NDO. The average calculation time of improved NDO is 0.0020, while it is 0.0024 of traditional NDO. The maximum single calculation time of improved NDO is 0.0046, while it is 0.0064 of the traditional NDO. The results show that compared with traditional NDO, the average calculation time of 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 observer. NDO based MPC effectively enhances the anti-disturbances performance of the system. MPC without observer trajectory tracking control has the characteristics of predictive model, rolling optimization and feedback correction, which can resist external disturbances and model mismatch to some extent.

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

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

In Figure 7, it shows the relationship between the estimated values of NDO and the actual values of the disturbances. NDO has good estimate performance about the disturbances, including surge disturbance force, sway disturbing force and yaw disturbance force.

5. Conclusions

An improved NDO based MPC for trajectory tracking is proposed to guarantee the stable motion of USV in this paper, which suffers various disturbances from the ocean wind, wave and current. The MPC is used to optimize the system torque based the measured position and speed state variables. Then, the NDO is designed to estimate the disturbances, and the estimated torque is compensated in the controller. The estimation errors can converge to zero in finite time. The experimental results show that NDO based MPC can effectively compensate external disturbances and obtain good tracking and disturbance rejection performance. The proposed method has similar tracking performance of the USVs with the MPC based on unimproved NDO, however, the improved NDO based MPC is much more time-saving.