Trajectory-tracking control of an underactuated unmanned surface vehicle based on quasi-infinite horizon model predictive control algorithm

Abstract

The method of a designed trajectory tracking for an underactuated unmanned surface vehicle (USV) in the presence of ocean disturbances is addressed in this paper; the differential flatness theory is applied to get the reference inputs and speed states at the reference position trajectory. Second, a transition process is arranged for the reference trajectory to reduce the overshoot of the actuator, which caused by the large deviation in the initial tracking. Third, the nonlinear disturbance observer is designed to obtain the estimated values of unknown disturbances in the ocean. Then, a controller-based model predictive control (MPC) and terminal cost function is designed for the nominal system. The inherent robustness of the controller and estimates of the observer are used to resist and compensate disturbances. Finally, the simulation experiments of linear trajectory and sinusoidal trajectories are carried out to prove the effectiveness and reliability of the control algorithm designed.

Keywords

Model predictive control trajectory tracking unmanned surface vessel differential flatness nonlinear disturbance observer

Introduction

A huge amount of research in the field of unmanned surface vessel (USV) motion control has been witnessed in the recent decades. Trajectory tracking is a crucial part of USV motion control, and the meaning is to follow a time-varying trajectory. There are two types of tracking methods generally used by researchers.

One is to build a virtual ship with the same dynamic model and achieve the purpose of trajectory tracking by tracking the virtual ship. In the last few years, considerable research efforts have been devoted to this strategy. A trajectory-tracking controller for the USV (Li et al., 2021) is present subjected to the full-state constraints. A novel of dynamic surface control-based trajectory-tracking control scheme (Wang et al., 2018b) is proposed for an USV. And a strategy (Wang et al., 2017) is proposed with finite-time trajectory-tracking control scheme for an USV. A semi-global output-feedback controller (Park, 2017) for USVs with actuator saturation is proposed. Besides, a controller for USV trajectory tracking based on backstepping and dynamic slide model control (Liao et al., 2016) is proposed, and a novel model-based backstepping controller (Dong et al., 2015) with relax persistent exciting (PE) conditions of yaw velocity for trajectory tracking is designed. The other is to control the USV to track the trajectory directly. There are some research works have been done. The tracking methods based on sliding mode are used (Qiu et al., 2019; Sun et al., 2020). Besides, an approach including nonlinear tracking differentiators and a new guidance law (Huang et al., 2019) is proposed. An adaptive trajectory-tracking control scheme (Wang et al., 2018a) for a fully actuated USV is proposed.

MPC can deal with two kinds of tasks by generate two reference trajectories. Moreover, MPC can solve the constraints of states that cannot be solved by some algorithms such as sliding mode control and backstepping method, so combining with MPC and trajectory tracking has a remarkable application research prospect. Quasi-infinite horizon MPC (Chen and Allgöwer, 1998) is first proposed with terminal penalty which is designed to ensure the stability of controller. Inherent robustness properties of quasi-infinite horizon MPC (Yu et al., 2014) are certified; thus, the algorithm is valuable to engineering application. Stability of finite horizon MPC with incremental input constraints (Yu et al., 2017) is proved, which is more practical for the gently inputs requirement in the field of motion control.

Thus, MPC have received great attention in the field of ship motion control. A novel disturbance compensating MPC algorithm (Li and Sun, 2012) has been proposed. An approach of linear matrix inequality (LMI)-based MPC to stabilization for USV (Liu et al., 2014) is proposed. An ideal method with line-of-sight (LOS) path generation and path following using MPC (Oh and Sun, 2010) is proposed. And a computationally efficient observer-based MPC controller (Liu et al., 2020) is proposed. Besides, a control method combining MPC and neural network to track trajectory by tracking the virtual ship (Yan and Wang, 2012) is proposed. A novel Lyapunov-based MPC algorithm (Shen et al., 2018) for the trajectory tracking of an autonomous underwater vehicle is presented. In addition, an USV trajectory-tracking controller based on MPC is proposed (Liu et al., 2015), and the controller can tolerate small disturbance. Two different MPC approaches are compared and analyzed (Zheng et al., 2014) to deal with the tracking problem of the USV. For the case of actuators’ fault scenarios, a Fault Tolerant MPC policy (Luca and Gianluca, 2018) has been proposed. And an approach is proposed (Shen and Shi, 2020) to alleviate the computational burden for the trajectory tracking of an autonomous underwater vehicle.

By analyzing the existing research results described above, we can found that, at present, when the controller is directly designed to approximate the USV to a given position trajectory, the absence of corresponding reference velocity states and reference input states may result in convergence of only position states and non-convergence of other states. In addition, the distance is usually existed between the initial position of the USV and reference trajectory, and the overshoot of actuators will be large. When the learning model predictive control (LMPC) algorithm is applied to the trajectory tracking of USV, the stability of the controller cannot be guaranteed. Because the optimality of LMPC algorithm is not equal to stability. Moreover, the problem of trajectory tracking with underactuation USV against 3-degree-of-freedom (3-DOF) disturbances is still a difficult question. The contribution of this paper is as follows. (1) A method based on differential flatness is proposed to generate the reference velocity and reference inputs of the corresponding position states, and then the convergence of velocity and position states can be guaranteed. (2) A switching strategy-based differential tracker aims to reduce the overshot oscillations of the actuators in the initial tracking. (3) A controller with inherent robustness of trajectory tracking for USV-based quasi-infinite horizon MPC is proposed and the stability of the controller is guaranteed. In addition, the convergence rate of the controller is faster and the robustness is stronger. (4) A way associating disturbance observer compensation and inherent robustness of the controller is pointed out to resist ocean disturbances. Simulation results show that the error caused by lateral disturbance is smaller.

This paper is organized as follows. The USV modeling and the trajectory-tracking problem-based MPC are formulated in section “Problem formulation,” the method of reference trajectory preprocessing is presented in section “Reference trajectory preprocessing,” the control laws are designed in section “Controller design,” while stability of the control laws designed is proved in section “Stability analysis,” and some simulation experiments are carried out to verify the effectiveness of the controller in section “Simulation experiments.” Finally, in section “Conclusion,” some conclusions are summarized.

Problem formulation

In this section, the USV modeling and the trajectory-tracking problem of an USV are formulated, meanwhile the trajectory-tracking control system is transformed into the stabilization of the trajectory-tracking error system.

USV modeling

The problem of trajectory tracking of an USV in the presence of ocean disturbances can be formulated as follows: assuming that trajectory $(x_{R}, y_{R})$ is a planned trajectory in Figure 1, then the control law can be designed for the USV to make the USV converge to the predetermined trajectory as soon as possible.

In Figure 1, $x_{n} o_{n} y_{n}$ is the inertial coordinate system, and $x_{b} o_{b} y_{b}$ is the body-fixed coordinate system of the USV, and the origin $o_{b}$ is coincided with the USV’s center of gravity. In addition, $u$ is the longitudinal velocity of USV, while $v$ is the lateral velocity. $φ$ is the heading angle and $r$ is the yaw rate. Besides $τ_{u}$ and $τ_{r}$ are the surge force and yaw moment of USV that drive the USV to track a given trajectory while resisting disturbance including wind, waves and currents.

The state-space model of USV (Perez and Fossen, 2007) is used, and the model of the USV in the presence of ocean disturbance can be described as

{\begin{matrix} \overset{\cdot}{η} = J (η) υ \\ M \overset{\cdot}{υ} + C (υ) υ + D (υ) υ = τ + τ_{d} \end{matrix}

(1)

with

\begin{matrix} η = [\begin{matrix} x \\ y \\ φ \end{matrix}], υ = [\begin{matrix} u \\ v \\ r \end{matrix}], τ = [\begin{matrix} τ_{u} \\ 0 \\ τ_{r} \end{matrix}] \\ τ_{d} = [\begin{matrix} τ_{du} \\ τ_{dv} \\ τ_{dr} \end{matrix}], J (η) = [\begin{matrix} \cos φ & - \sin φ & 0 \\ \sin φ & \cos φ & 0 \\ 0 & 0 & 1 \end{matrix}] \\ M = [\begin{matrix} m_{11} & 0 & 0 \\ 0 & m_{22} & 0 \\ 0 & 0 & m_{33} \end{matrix}] \\ C (υ) = [\begin{matrix} 0 & 0 & - m_{22} v \\ 0 & 0 & m_{11} u \\ m_{22} v & - m_{11} u & 0 \end{matrix}] \\ D (υ) = [\begin{matrix} d_{11} & 0 & 0 \\ 0 & d_{22} & 0 \\ 0 & 0 & d_{33} \end{matrix}] \end{matrix}

where $x$ and $y$ present USV’s position; $τ_{du}$ , $τ_{dv}$ , and $τ_{dr}$ , respectively, represent the longitudinal, lateral, and heading disturbance forces caused by ocean disturbances; $m_{11}$ , $m_{22}$ , and $m_{33}$ are USV’s inertia coefficients including added mass effects; and $d_{11}$ , $d_{22}$ , and $d_{33}$ are the hydrodynamic damping coefficients.

Trajectory-tracking error system

In trajectory tracking, a reference trajectory is usually preset. It is assumed that the USV has passed the reference trajectory and obtained the corresponding reference states and inputs. The trajectory-tracking errors system can be described as

\overset{\cdot}{\tilde{χ}} = f (\tilde{χ}, \tilde{τ})

(2)

with $\tilde{χ} = χ - χ_{R}$ , $\tilde{τ} = τ - τ_{R}$ , $χ = [x, y, φ, u, v, r]^{T}$ , $χ_{R} = [x_{R}, y_{R}, φ_{R}, u_{R}, v_{R}, r_{R}]^{T}$ , and $τ_{R} = [{τ_{u}}_{R}, 0, {τ_{r}}_{R}]^{T}$

When the $\tilde{χ} \to 0$ and $\tilde{τ} \to 0$ at the same time, the task of trajectory tracking is finished. However, the corresponding position $(x_{R}, y_{R})$ is usually given, and other reference states and inputs are unknown. In order to solve this question, differential flatness theory is used in section “Reference trajectory preprocessing.”

Reference trajectory preprocessing

In this section, the differential flatness theory is applied to represent the remaining reference states and inputs with reference position $(x_{R}, y_{R})$ . Besides, a transition process is arranged for the position trajectory in order to prevent the overshoot and oscillation for actuators.

Differential flatness

When $(x_{R}, y_{R})$ is given, other reference states and inputs cannot be obtained directly. When $(x_{R}, y_{R})$ is a trajectory with time-varying inputs such as sinusoidal trajectory, the inputs $τ_{R}$ of the USV are time-varying. In order to obtain them, the differential flatness theory is used.

Lemma 1

If exists a certain function $α$ meet $z = α (x, u, \overset{\cdot}{u}, \dots, u^{(p)})$ , and the states and inputs of the nonlinear system (3) can be expressed in terms of $z$ and a finite number of its higher-order derivatives as equation (4)

\overset{\cdot}{x} (t) = f (x (t), u (t))

(3)

{\begin{matrix} x = β (z, \overset{\cdot}{z} \dots z^{(q)}) \\ u = γ (z, \overset{\cdot}{z} \dots z^{(q)}) \end{matrix}

(4)

Then, the system is differentially flat (Fliess et al., 1995).

The reference position $(x_{R}, y_{R})$ is chosen as the flat output $(z_{1}, z_{2})$ for the trajectory tracking of USV to express other states and inputs.

Assumption 1

During sailing, the longitudinal velocity of the USV is one or more orders of magnitude higher than the lateral velocity, so the effect of drift angle can be ignored when describing the heading angle.

Therefore, for the nominal system (5), the remaining states and inputs can be expressed as equation (6)

{\begin{matrix} \overset{\cdot}{η} = J (η) υ \\ M \overset{\cdot}{υ} + C (υ) υ + D (υ) υ = τ \end{matrix}

(5)

{\begin{matrix} φ_{R} = a \tan 2 (\frac{{\overset{\cdot}{y}}_{R}}{{\overset{\cdot}{x}}_{R}}) \\ υ_{R} = J {(η_{R})}^{- 1} {\overset{\cdot}{η}}_{R} \\ {\overset{\cdot}{u}}_{R} = {\overset{\cdot\cdot}{x}}_{R} \cos (φ_{R}) - {\overset{\cdot}{x}}_{R} \sin (φ_{R}) r_{R} + {\overset{\cdot\cdot}{y}}_{R} \sin (φ_{R}) + {\overset{\cdot}{y}}_{R} \cos (φ_{R}) r_{R} \\ {\overset{\cdot}{r}}_{R} = \frac{({\overset{\dots}{y}}_{R} {\overset{\cdot}{x}}_{R} - {\overset{\dots}{x}}_{R} {\overset{\cdot}{y}}_{R}) ({\overset{\cdot}{x}}_{R}^{2} + {\overset{\cdot}{y}}_{R}^{2}) - (2 {\overset{\cdot}{x}}_{R} {\overset{\cdot\cdot}{x}}_{R} + 2 {\overset{\cdot}{y}}_{R} {\overset{\cdot\cdot}{y}}_{R}) ({\overset{\cdot\cdot}{y}}_{R} {\overset{\cdot}{x}}_{R} - {\overset{\cdot\cdot}{x}}_{R} {\overset{\cdot}{y}}_{R})}{{({\overset{\cdot}{x}}_{R}^{2} + {\overset{\cdot}{y}}_{R}^{2})}^{2}} \\ τ_{u_{R}} = m_{11} {\overset{\cdot}{u}}_{R} - m_{22} v_{R} r_{R} + d_{11} u_{R} \\ τ_{r_{R}} = m_{33} {\overset{\cdot}{r}}_{R} - (m_{11} - m_{22}) u_{R} v_{R} + d_{33} r_{R} \end{matrix}

(6)

with $η_{R} = {[x_{R}, y_{R}, φ_{R}]}^{T}$ and $υ_{R} = [u_{R}, v_{R}, r_{R}]^{T}$ , then for any given $(x_{R}, y_{R})$ , the corresponding reference states and inputs of USV can be obtained.

Transition process

The deviations of USV initial position and the reference trajectory often exist in actual project application, while the reference velocities of the USV are got according to the difference of the reference trajectory. Moreover, the deviation of the position and velocity will lead to the overshoot shock for the actuators of USV.

Therefore, a transition process is used at the initial moment to reduce the overshoot of actuators. Differential tracker (Han, 2009) is used for the system to fulfill this requirement. The transition process for common second-order system is designed as

{\begin{matrix} x_{1} (t + 1) = x_{1} (t) + dt \cdot x_{2} (t) \\ x_{2} (t + 1) = x_{2} (t) + dt \cdot u_{f} \\ u_{f} = fhan (x_{1}, x_{2}, k_{r}, dt) \end{matrix}

(7)

where $x_{1}$ is the variable to be tracked, and $x_{2}$ is the differential variable of $x_{1}$ . $fhan (x_{1}, x_{2}, k_{r}, dt)$ is

{\begin{matrix} a_{1} = \sqrt{k_{r} {(dt)}^{2} [k_{r} {(dt)}^{2} + 8 | x_{1} + dt \cdot x_{2} |]} \\ a_{2} = dt \cdot x_{2} + sign (x_{1} + dt \cdot x_{2}) [a_{1} - k_{r} {(dt)}^{2}] / 2 \\ a = (x_{1} + 2 dt \cdot x_{2}) fsg [x_{1} + dt \cdot x_{2}, k_{r} {(dt)}^{2}] \\ + a_{2} {1 - fsg [x_{1} + dt \cdot x_{2}, k_{r} {(dt)}^{2}]} \\ fhan = - k_{r} (\frac{a}{k_{r} {(dt)}^{2}}) fsg [a, k_{r} {(dt)}^{2}] - k_{r} sign (a) {1 - fsg [a, k_{r} {(dt)}^{2}]} \end{matrix}

(8)

where $f s g (x_{1} + d t \cdot x_{2}, k_{r} {(d t)}^{2}) = [s i g n (x_{1} + d t \cdot x_{2} + k_{r} {(d t)}^{2}) - s i g n (x_{1} + d t \cdot x_{2} - k_{r} {(d t)}^{2})] / 2$ , and $k_{r}$ is the set tracking parameter. $d t$ is the simulation time step.

Since the trajectory is time-varying, the differential tracker will lead to a lag for tracking. Therefore, the switching strategy is adopted. The position information after the transition is used to design the controller in the early tracking. Besides, when the distance between the USV and the reference trajectory is small and trajectory can be tracked stably, the real reference position information is used to reduce the error caused by lag. The specific process is designed as

{\begin{matrix} dis = \sqrt{{(x - x_{R})}^{2} + {(y - y_{R})}^{2}} \\ {η_{R}}^{'} = {\begin{matrix} η_{R} dis \leq 1 \\ x_{1} dis > 1 \end{matrix} \\ x_{1} (t + 1) = x_{1} (t) + dt \cdot x_{2} (t) \\ x_{2} (t + 1) = x_{2} (t) + dt \cdot u_{f} \\ u_{f} = fhan (x_{1} - η_{R}, x_{2}, k_{r}, dt) \end{matrix}

(9)

where $(x, y)$ is the position of USV, and $(x_{R}, y_{R})$ is the reference trajectory. $dis$ is the distance between USV and the reference trajectory point. ${η_{R}}^{'} = [{x_{R}}^{'}, {y_{R}}^{'}, {φ_{R}}^{'}]^{T}$ are the reference position states.

Controller design

The controller is designed to solve the problem of USV trajectory tracking with ocean disturbances. According to the preprocessed reference states and inputs of USV, the quasi-infinite horizon MPC controller for the nominal system is designed. And the ocean disturbance is observed by the nonlinear disturbance observer (NDO) and compensated by actuators and the inherent robustness of the controller. The specific process is shown in Figure 2.

In this section, the controller design process would be divided into two steps: one is to design the NDO and the other is to design the quasi-infinite horizon MPC controller for the nominal system of USV.

Nonlinear disturbance observer

At the real ocean circumstance, USV is subject to unknown marine disturbances during navigation. Therefore, the NDO is designed to observe the unknown disturbances. System dynamics equation of USV can be written as

\overset{\cdot}{υ} = - M^{- 1} C (υ) υ - M^{- 1} D (υ) υ + M^{- 1} τ + M^{- 1} τ_{d}

(10)

The NDO (Chen, 2004) can be designed for USV as

{\begin{matrix} {\hat{τ}}_{d} = z + lM υ \\ \overset{\cdot}{z} = - l {\hat{τ}}_{d} - lM (M^{- 1} C (υ) υ - M^{- 1} D (υ) υ + M^{- 1} τ) \end{matrix}

(11)

where $l = [l_{u}, l_{v}, l_{r}]^{T}$ is the bandwidth parameters of the observer, and ${\hat{τ}}_{d} = [{\hat{τ}}_{du}, {\hat{τ}}_{dv}, {\hat{τ}}_{dr}]^{T}$ are the estimates of ocean disturbances.

Quasi-infinite horizon MPC

When the NDO is designed to observe the environmental disturbances, the controller can only be carried out for the nominal system (5), and the estimated disturbances can be directly compensated by the actuators in the longitudinal and heading direction. Besides, the inherent robustness of the controller is used to resist the lateral disturbance.

Linearization and discretization of the USV model

The 3-DOF motion model of USV is a nonlinear system, which needs to be approximately linearized into a linear time-varying system to design the MPC controller. Since this paper aims to track any directly given trajectory, the approximate method (Kuhne et al., 2004) is used to design the controller with the deviation between the USV and the reference system. The reference system has been obtained through reference preprocessing in section “Reference trajectory preprocessing,” and the reference system is shown as

{\overset{\cdot}{χ}}_{R} = f (χ_{R}, τ_{R})

(12)

where $χ_{R} = [{x_{R}}^{'}, {y_{R}}^{'}, {φ_{R}}^{'}, u_{R}, v_{R}, r_{R}]^{T}$ are the states of reference system, and $τ_{R} = [τ_{uR}, 0, τ_{rR}]^{T}$ are the inputs of reference system.

The Taylor expansion of the USV system is carried out at the stable point of the error system, that is, at each point $(χ_{R}, u_{R})$ of the reference system, and the first-order term is retained as

\overset{\cdot}{χ} = f (χ_{R}, τ_{R}) + {\frac{\partial f}{\partial χ} |}_{\begin{matrix} χ = χ_{R} \\ τ = τ_{R} \end{matrix}} (χ - χ_{R}) + {\frac{\partial f}{\partial τ} |}_{\begin{matrix} χ = χ_{R} \\ τ = τ_{R} \end{matrix}} (τ - τ_{R})

(13)

Combining equations (12) and (13), the deviation system can be obtained as

\overset{\cdot}{\tilde{χ}} = {\frac{\partial f}{\partial χ} |}_{\begin{matrix} χ = χ_{R} \\ τ = τ_{R} \end{matrix}} (χ - χ_{R}) + {\frac{\partial f}{\partial τ} |}_{\begin{matrix} χ = χ_{R} \\ τ = τ_{R} \end{matrix}} (τ - τ_{R}) = A \tilde{χ} + B \tilde{τ}

(14)

where $A$ is the Jacobian of $f$ with respect to $χ_{R}$ , and $B$ is the Jacobian of $f$ with respect to $τ_{R}$ .

Euler’s method is adopted to discretize equation (14), and discretization system can be obtained as

{\begin{matrix} A_{k} = A \cdot dt + I \\ B_{k} = B \cdot dt \end{matrix}

(15)

\tilde{χ} (k + 1) = A_{k} \tilde{χ} (k) + B_{k} \tilde{τ} (k)

(16)

where $I$ is the unit matrix, $A_{k}$ is the discrete Jacobian of $f$ with respect to $χ_{R}$ , and $B_{k}$ is the discrete Jacobian of $f$ with respect to $τ_{R}$ .

Cost function design

Cost function is a minimum function to achieve the control aim. The purpose of the designed controller is to track the designed trajectory gently, and to ensure the stability and robustness of the controller, so as to meet the requirements of practical projects. Therefore, the cost function includes indicators such as states errors, control increment, and terminal penalty function. So, the cost function is designed as

\begin{matrix} J (k) = \sum_{i = 1}^{\infty} {‖ \tilde{χ} (k + i) ‖}_{Q}^{2} + \sum_{i = 0}^{\infty} {‖ Δ \tilde{τ} (k + i) ‖}_{R}^{2} \\ \leq \sum_{i = 1}^{N_{P} - 1} {‖ \tilde{χ} (k + i) ‖}_{Q}^{2} + \sum_{i = 0}^{N_{C} - 1} {‖ Δ \tilde{τ} (k + i) ‖}_{R}^{2} + F (\tilde{χ} (k + N_{P})) \end{matrix}

(17)

with $Δ \tilde{τ} (k + i) = \tilde{τ} (k + i) - \tilde{τ} (k + i - 1)$ , $Q$ and $R$ are the appropriate weighting matrices. $N_{P}$ is the prediction horizon, and $N_{C}$ is the control horizon. $F (\tilde{χ} (k + N_{P}))$ is the terminal cost function.

Taking $Δ \tilde{τ} (k + i)$ as a parameter, the states can be written as

{\begin{matrix} ξ (k) = [\begin{matrix} \tilde{χ} (k) \\ \tilde{τ} (k - 1) \end{matrix}] \\ {\tilde{A}}_{k} = [\begin{matrix} A_{k} & B_{k} \\ 0_{m \times n} & I_{m} \end{matrix}], {\tilde{B}}_{k} = [\begin{matrix} B_{k} \\ I_{m} \end{matrix}] \end{matrix}

(18)

where $m = 3$ is the USV control dimension, and $n = 6$ is the state dimension.

Substituting equation (18) into error system (16) yields to

ξ (k + 1) = {\tilde{A}}_{k} ξ (k) + {\tilde{B}}_{k} Δ \tilde{τ} (k)

(19)

To simplify the operation, it is assumed that in the prediction horizon

A_{k + i} = A_{k}, B_{k + i} = B_{k}, i = 1, 2, \dots, N_{P}

(20)

The terminal cost function in equation (17) is designed (Yu et al., 2017), and the specific process is as follows:

1. Combining with equation (16), find a linear state feedback gain $K$ can be determined such that ${\hat{A}}_{K} = A + B \cdot K$ is asymptotically stable. $K$ can be designed with discrete linear quadratic regulator by solving the Riccati equation.

2. Pick a parameter $κ$ with $κ > 1$ .

3. Then, solve the Lyapunov equation

κ^{2} {\hat{A}}_{K}^{T} P {\hat{A}}_{K} - P + (Q + K^{T} RK) = 0

(21)

a unique positive definite solution $P$ can be carried out.

4. There exists $α > 0$ , which specifies an ellipsoid

Θ = {\tilde{χ} (k) \in \bar{X} | \tilde{χ} {(k)}^{T} P \tilde{χ} (k) \leq α}

(22)

where $\bar{X}$ is the predicted sequence of $χ (k)$ , and $α$ is the terminal set.

5. The terminal cost function can be designed as

F (\tilde{χ} (k + N_{P})) = \tilde{χ} {(k + N_{P})}^{T} P \tilde{χ} (k + N_{P})

(23)

Then, combined with equations (17) and (23), the cost function can be rewritten as

J (k) : = X {(k)}^{T} \bar{QX} (k) + Δ U {(k)}^{T} \bar{R} Δ U (k)

(24)

with $X (k) = (\begin{matrix} ξ (k + 1), & \dots, & ξ (k + N_{P}) \end{matrix})^{T}$ , $Δ U = (\begin{matrix} Δ \tilde{τ} * (k), \dots & , Δ \tilde{τ} (k + N_{C} - 1) \end{matrix})^{T}$ , $\bar{R} = diag (\begin{matrix} R, & \dots & , R \end{matrix})_{N_{c} \times N_{c}}$ , $\hat{Q} = diag (\begin{matrix} Q, & 0_{m \times m} \end{matrix})$ , and $\bar{Q} = diag (\begin{matrix} \hat{Q}, & \dots, & \hat{Q}, & P \end{matrix})_{N_{p} \times N_{p}}$ .

The model prediction expression of the system can be written as

X (k) = Ω ξ (k) + Γ Δ U

(25)

with

\begin{matrix} Ω = {[\begin{matrix} {\tilde{A}}_{k} & {\tilde{A}}_{k}^{2} & \dots & {\tilde{A}}_{k}^{N_{P}} \end{matrix}]}^{T} \\ Γ = [\begin{matrix} {\tilde{B}}_{k} & 0 & 0 & 0 \\ {\tilde{A}}_{k} {\tilde{B}}_{k} & {\tilde{B}}_{k} & 0 & 0 \\ ⋮ & ⋮ & ⋱ & ⋮ \\ {\tilde{A}}_{k}^{N_{C} - 1} {\tilde{B}}_{k} & {\tilde{A}}_{k}^{N_{C} - 2} \tilde{B} & \dots & {\tilde{B}}_{k} \\ {\tilde{A}}_{k}^{N_{C}} {\tilde{B}}_{k} & {\tilde{A}}_{k}^{N_{C} - 1} {\tilde{B}}_{k} & \dots & {\tilde{A}}_{k} {\tilde{B}}_{k} \\ ⋮ & ⋮ & ⋱ & ⋮ \\ {\tilde{A}}_{k}^{N_{P} - 1} {\tilde{B}}_{k} & {\tilde{A}}_{k}^{N_{P} - 2} {\tilde{B}}_{k} & \dots & {\tilde{A}}_{k}^{N_{P} - N_{C}} {\tilde{B}}_{k} \end{matrix}] \end{matrix}

Substituting equation (25) into equation (24) and ignore the constant term which is irrelevant to the minimization function

J (k) = \frac{1}{2} Δ U {(k)}^{T} H Δ U (k) + ξ {(k)}^{T} F Δ U (k)

(26)

where $H = 2 (\bar{R} + Γ^{T} \bar{Q} Γ)$ , and $F = 2 Ω^{T} \bar{Q} Γ$ .

Constraint design

Considering the safety in navigation and the loss of actuator such as paddles and rudders, the states and inputs of USV should be restricted. According to the characteristics of MPC, the constraint of states can be converted into the constraint of inputs. And the constraints on the inputs include the extreme and increment constraints for the surge force and yaw moment. The specific constraints can be designed as

{\begin{matrix} Δ τ_{min} \leq Δ \tilde{τ} (k + i) \leq Δ τ_{max} i = 0, 1, 2, \dots, N_{C} - 1 \\ τ_{min} \leq \tilde{τ} (k + i) \leq τ_{max} i = 0, 1, 2, \dots, N_{C} - 1 \\ {\tilde{χ}}_{min} \leq \tilde{χ} (k + i) \leq {\tilde{χ}}_{max} i = 1, 2, \dots, N_{P} \end{matrix}

(27)

where $Δ τ_{min}$ and $Δ τ_{max}$ are the extreme value of the error system increment, $τ_{min}$ and $τ_{max}$ are the extreme value of error system inputs. ${\tilde{χ}}_{min}$ and ${\tilde{χ}}_{max}$ are the error extreme values of the USV states.

Combined with equation (26), the control problem can be transformed into the optimization problem as

\begin{matrix} min J (k) = min {\frac{1}{2} Δ U {(k)}^{T} H Δ U (k) + ξ {(k)}^{T} F Δ U (k)} \\ s . t . Δ U_{min} \leq Δ U (k) \leq Δ U_{max} \\ U_{min} \leq C \cdot Δ U (k) + U (k) \leq U_{max} \\ ζ_{min} \leq ζ (k) \leq ζ_{max} \end{matrix}

(28)

with

\begin{matrix} Δ U_{min} = 1_{N_{C}} \otimes Δ τ_{min}, Δ U_{max} = 1_{N_{C}} \otimes Δ τ_{max} \\ U_{min} = 1_{N_{C}} \otimes τ_{min}, U_{max} = 1_{N_{C}} \otimes τ_{max} \\ ζ_{min} = 1_{N_{C}} \otimes {\tilde{χ}}_{min}, ζ_{max} = 1_{N_{C}} \otimes {\tilde{χ}}_{max} \\ ζ (k) = [\tilde{χ} (k + 1), \tilde{χ} (k + 2), \dots, \tilde{χ} (k + N_{P})] \\ C = {[\begin{matrix} 1 & 0 & \dots & \dots & 0 \\ 1 & 1 & 0 & \dots & 0 \\ 1 & 1 & 1 & ⋱ & 0 \\ ⋮ & ⋮ & ⋱ & ⋱ & 0 \\ 1 & 1 & \dots & 1 & 1 \end{matrix}]}_{N_{C} \times N_{C}} \otimes I_{N_{u}} \end{matrix}

where $1_{N_{C}}$ represents the unit column vector of the number of $N_{C}$ rows, $I_{N_{u}}$ represents the unit matrix of the $N_{u}$ dimension, and ⊗ represents the Kronecker product.

To sum up, the optimal control increment $Δ \tilde{τ}$ can be obtained by solving the optimal problem (28). According to equation (18), the longitudinal and heading disturbance obtained from the observer can be compensated, and the control law can be expressed as

τ_{k} = τ_{R_{k}} + {\tilde{τ}}_{k - 1} + Δ {\tilde{τ}}_{k} + {\hat{τ}'}_{d_{k}}

(29)

where $τ_{R_{k}}$ are the inputs for the reference system at the current moment, ${\tilde{τ}}_{k - 1}$ are the errors between the USV and the reference inputs at the previous moment, and ${\hat{τ}'}_{d_{k}}$ are the estimated values of the disturbances in longitudinal and heading direction.

Stability analysis

In this section, the stability of the designed NDO is proved. And the stability of the controller for nominal system is analyzed and proved under the condition of no disturbances. At the same time, the robustness of the designed controller is analyzed to resist the lateral disturbance, which cannot be directly compensated due to the underactuation of the USV.

Stability of the NDO

The errors between the actual disturbances and the estimated value are defined as

τ_{de} = τ_{d} - {\hat{τ}}_{d}

(30)

Derivation of equation (30) and simultaneous equation (10) can obtain

\begin{matrix} {\overset{\cdot}{τ}}_{de} = {\overset{\cdot}{τ}}_{d} - {\overset{\cdot}{\hat{τ}}}_{d} \\ = {\overset{\cdot}{τ}}_{d} - \overset{\cdot}{z} - l M \overset{\cdot}{υ} \\ = {\overset{\cdot}{τ}}_{d} + l {\hat{τ}}_{d} + l M (M^{- 1} C (υ) υ - M^{- 1} D (υ) υ + M^{- 1} τ) \\ - lM (M^{- 1} C (υ) υ - M^{- 1} D (υ) υ + M^{- 1} τ + M^{- 1} τ_{d}) \\ = {\overset{\cdot}{τ}}_{d} - l τ_{d_{e}} \end{matrix}

(31)

It is assumed that the actual disturbances are slow-varying which mean ${\overset{\cdot}{τ}}_{d} \to 0$ . Then, the Lyapunov function $V = τ_{de} \cdot {τ_{de}}^{T} / 2$ is designed, and $\overset{\cdot}{V} = - l τ_{de} \cdot {τ_{de}}^{T} < 0$ . Thus, the error of observer is asymptotically stable and the observed estimates will converge to the actual disturbances.

Stability and inherent robustness of controller

The designed controller of quasi-infinite horizon MPC (Yu et al., 2014) is stable as long as the corresponding assumptions are met. Combined with the controller designed in this paper, then analyzing the controller whether it satisfies the expectation or not to ensure the stability.

For the error system equation (14), $(0, 0)$ represents that USV has tracked the reference trajectory, which is an equilibrium of the system and satisfies the assumption (Yu et al., 2014). At the same time, the constraints of $\tilde{χ}$ , $\tilde{τ}$ , and $Δ \tilde{τ}$ of the system are set in section “Cost function design” as continuous closed interval, which satisfies Assumption 2 (Yu et al., 2014) that the range of states, inputs, and control increment are required to be compact. And the constraint range of control increment $Δ \tilde{τ}$ is a continuous bounded closed interval, $\exists P_{r} \in Δ U$ , such that $‖ θ ‖ < ε$ for $\forall ε \in P_{r}$ , where $ε > 0$ is a constant which satisfies Assumption 3 (Yu et al., 2014).

Therefore, it can be proved that the controller of quasi-infinite horizon MPC designed in this paper is stable without disturbances. For ocean disturbances existing in USV navigation, the lateral disturbance cannot be compensated directly due to the underactuation of the USV. The theory (Yu et al., 2017) is certified that the quasi-infinite horizon MPC has inherent robustness and could ensure the system would not diverge with the continuous bounded disturbances. Thus, the bounded lateral disturbance can be resisted by the inherent robustness of the controller. So, the USV can fulfill the task of trajectory tracking with high precision.

Simulation experiments

In order to verify the effectiveness and reliability of the trajectory-tracking controller proposed in this paper, simulation experiments of linear trajectory and sinusoidal trajectory are carried out in a marine vehicle model (Do and Pan, 2006). Specific model and controller parameters are selected as shown in Table 1.

Table 1.

Model and controller parameters.

Symbols	Value
$m_{11}$	25.8
$m_{22}$	33.8
$m_{33}$	2.76
$k_{r}$	0.5
$N_{p}$	10
$Q$	$100 I_{6}$
$d_{t}$	0.5
${\tilde{χ}}_{min}$	$- {[10, 8, π / 2, 2, 2, 2]}^{T}$
$Δ τ_{min}$	$- {[10, 10]}^{T}$
$Δ τ_{max}$	${[10, 10]}^{T}$
$d_{11}$	12
$d_{22}$	17
$d_{33}$	0.5
$l$	${[3, 3, 3]}^{T}$
$N_{C}$	10
$R$	$I_{3}$
${\tilde{χ}}_{max}$	${[10, 8, π / 2, 2, 2, 2]}^{T}$
$τ_{min}$	$- {[48, 48]}^{T}$
$τ_{max}$	${[48, 48]}^{T}$

In order to prove the superiority of the proposed controller in this paper, it is compared with a method of quasi-infinite horizon MPC without disturbance observer called Method I, a method of quasi-infinite horizon MPC without reference trajectory preprocessing called Method II, and a method of LMPC with reference trajectory preprocessing and disturbance observer compensation called Method III. Two sets of simulations are performed as follows:

Case I: in this case of simulation experiment, the initial positions, velocities, and inputs of the USV are chosen as $x_{0} = - 1$ , $y_{0} = 0$ , $φ_{0} = 0$ , $v_{0} = 0$ , $r_{0} = 0$ , $τ_{u 0} = 0$ , and $τ_{r 0} = 0$ . The reference linear trajectory design as: $x_{R} = 0.6 t$ and $y_{R} = 0.3 x_{R} + 4$ , and according to the actual ocean environment, the combination of current disturbance which described as constant (Shen et al., 2018) and wave disturbance which described as sine function (Li and Sun, 2012) is adopted to simulate disturbances in the ocean; the values are chosen as: $τ_{du} = 5 + 5 \sin (0.2 t)$ , $τ_{dv} = 1 + 1 \sin (0.2 t)$ , and $τ_{dr} = 5 + 5 \sin (0.2 t)$ . The simulation results are as shown in Figures 3 –6.

Case II: in this case of simulation experiment, the initial positions, velocities, inputs of the USV, and the ocean disturbances are chosen as same as Case I. The reference sinusoidal trajectory design as: $x_{R} = 0.5 t$ and $y_{R} = 10 \sin (0.1 x_{R}) + 2$ , and the simulation results are as shown in Figures 7 –10.

Simulation results in Figures 3 and 7 show that, compared with the algorithm proposed in this paper, the controller-based Method I which only relies on the inherent robustness to resist ocean disturbance can still track the trajectories; moreover, the system is not diverging with the larger biases. Besides, controller-based Method II has larger overshoot and oscillation in the initial tracking. Especially the method proposed in this paper can track the trajectory accurately with little inevitable lateral error due to the underactuation of USV. In addition, compared with the strategy of Method III, the controller can track the trajectory quickly with less overshoot and less lateral error due to the inherent robustness.

From Figures 4 and 8, it is obvious that compared with the algorithms-based Method I and Method II, the velocities’ overshoot of the algorithm proposed in the paper is smaller and gentler, which is conducive to navigation safety. Simulation results in Figures 5 and 9 show that, the algorithm proposed in this paper can ensure the safety of the actuator, and has strikingly application value with the less actuator overshoots in the initial tracking. And the situation NDO can observe the variable disturbance is proved in Figures 7 and 10.

Conclusion

In this paper, a trajectory-tracking method of USV based on quasi-infinite horizon MPC with ocean disturbances is proposed. The reference trajectory preprocessing proposed can get the reference states and inputs with any given position trajectory and reduce the overshoot of USV’s actuators in the initial tracking. Besides, the methods of combing the disturbance observer compensation and inherent robustness of controller is claimed to improve accuracy. According to the simulation results from the straight and sinusoidal trajectory, the effectiveness and superiority of the strategy are certified. In addition, this strategy can also be applied to other fields, such as the trajectory tracking of unmanned ground vehicles and autonomous underwater vehicle. Future work will include sea trials to further validate the control algorithm.

Footnotes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work is supported by the Natural Science Foundation of China (grant numbers 51709214, 51779052, 51809203 and 51879210).

ORCID iDs

Hao Wang

Zaopeng Dong

References

Chen

Allgöwer

(1998) A quasi-infinite horizon nonlinear model predictive control scheme with guaranteed stability. Automatica 34(10): 1205–1217.

Chen

(2004) Disturbance observer based control for nonlinear systems. IEEE-ASME Transactions on Mechatronics 9(4): 706–710.

Pan

(2006) Global robust adaptive path following of underactuated ships. Automatica 42(10): 1713–1172.

Dong

Wan

, et al. (2015) Trajectory tracking control of underactuated USV based on modified backstepping approach. International Journal of Naval Architecture and Ocean Engineering 7(5): 817–832.

Fliess

Levine

Martin

, et al. (1995) Flatness and defect of non-linear systems: Introductory theory and examples. International Journal of Control 61(6): 1327–1361.

Han

(2009) From PID to active disturbance rejection control. IEEE Transactions on Industrial Electronics 56(3): 900–906.

Huang

Gong

Zhuang

, et al. (2019) A new guidance law for trajectory tracking of an underactuated unmanned surface vehicle with parameter perturbations. Ocean Engineering 175: 217–222.

Kuhne

Lages

Gomes

, et al. (2004) Model predictive control of a mobile robot using linearization. In: Proceedings of mechatronics and robotics 2004, Aachen, pp. 525–530. Available at: http://www.ece.ufrgs.br/~fetter/mechrob04_553.pdf

Dong

Guo

(2021) Trajectory tracking control of underactuated surface vessel with full state constraints. Asian Journal of Control 23: 1762–1771.

10.

Sun

(2012) Disturbance compensating model predictive control with application to ship heading control. IEEE Transactions on Control Systems Technology 20(1): 257–265.

11.

Liao

Zhang

Wan

, et al. (2016) Trajectory tracking control for underactuated unmanned surface vehicles with dynamic uncertainties. Journal of Central South University 23(2): 370–378.

12.

Liu

(2020) Computationally efficient MPC for path following of underactuated marine vessels using projection neural network. Neural Computing and Applications 32: 7455–7464.

13.

Liu

Chu

Wang

, et al. (2015) Trajectory tracking controller for underactuated surface vessels based on model predictive control. Journal of Shanghai Jiaotong University 49(12): 1842–1854.

14.

Liu

Zhang

(2014) LMI-based model predictive control for underactuated surface vessels with input constraints. Abstract and Applied Analysis 2014: 673256.

15.

Luca

Gianluca

(2018) Fault tolerant model predictive control for an over-actuated vessel. Ocean Engineering 160: 1–9.

16.

Sun

(2010) Path following of underactuated marine surface vessels using line-of-sight based model predictive control. Ocean Engineering 37(2): 289–295.

17.

Park

(2017) A simple output-feedback control for trajectory tracking of underactuated surface vessels. Ocean Engineering 143: 133–139.

18.

Perez

Fossen

(2007) Kinematic models for maneuvering and sea keeping of marine vessels. Modeling, Identification and Control 28(1): 19–30.

19.

Qiu

Wang

Fan

, et al. (2019) Adaptive sliding mode trajectory tracking control for unmanned surface vehicle with modeling uncertainties and input saturation. Applied Sciences-Basel 9(6): 1240.

20.

Shen

Shi

(2020) Distributed implementation of nonlinear model predictive control for AUV trajectory tracking. Automatica 115: 108863.

21.

Shen

Shi

Buckham

(2018) Trajectory tracking control of an autonomous underwater vehicle using Lyapunov-based model predictive control. IEEE Transactions on Industrial Electronics 65: 5796–5805.

22.

Sun

Wang

Fan

(2020) Model identification and trajectory tracking control for vector propulsion unmanned surface vehicles. Electronics 9(1): 1–13.

23.

Wang

Liu

, et al. (2018a) Adaptive non-strict trajectory tracking control scheme for a fully actuated unmanned surface vehicle. Applied Sciences-Basel 8(4): 598.

24.

Wang

Gao

, et al. (2017) Integral sliding mode based finite-time trajectory tracking control of unmanned surface vehicles with input saturations. Indian Journal of Geomarine Sciences 46(12): 2493–2501.

25.

Wang

Liu

Zheng

, et al. (2018b) Global exponential trajectory tracking control of underactuated surface vehicles using dynamic surface control approach. In: International conference on intelligent autonomous systems, Singapore, 1–3 March, pp. 221–226. New York: IEEE.

26.

Yan

Wang

(2012) Model predictive control for tracking of underactuated vessels based on recurrent neural networks. IEEE Journal of Oceanic Engineering 37(4): 717–726.

27.

, et al. (2017) Stability of finite horizon model predictive control with incremental input constraints. Automatica 79: 265–272.

28.

Reble

Chen

, et al. (2014) Inherent robustness properties of quasi-infinite horizon NMPC. Automatica 50(9): 2269–2280.

29.

Zheng

Negenborn

Lodewijks

(2014) Trajectory tracking of autonomous vessels using model predictive control. IFAC Proceedings Volumes 47(3): 8812–8817.