Ship Path-Tracking with Real Disturbance and Exponential Cost Function

1. Introduction

Intelligent ship technology is widely applied to economic construction or military industry, all the world's maritime powers are actively studying intelligent ship technology, so the study of ship path-tracking problems is of great significance. The target path for a path-tracking problem is fixed, which is different from the trajectory following. There may be differences in the formulation of these two problems in different articles, and we will divide the problems into two categories according to whether the path changes over time. The ship path-tracking problem is a nonlinear control problem, this field has been associated with a lot of research results.

The meaning of trajectory tracking mentioned is the same as path-tracking. [8] described both the trajectory tracking methods and the most commonly used trajectory tracking controllers of autonomous vehicles, besides state-of-the-art research studies of these controllers. [17] they highlighted vessel autonomy, regulatory framework, guidance, navigation and control components, advances in the industry, and previous reviews in the field. In addition, they analyzed the terminology used in the literature and attempt to clarify ambiguities in commonly used terms related to path planning. [1] found the need for a unified platform for evaluating and comparing the performance of algorithms under a large set of possible real-world scenarios. [15] considered autonomous ships are expected to improve the level of safety and efficiency in future maritime navigation. [20] provided a review of recent progress in the context of motion planning and control for Autonomous marine vehicles (AMVs) from the perceptive of model predictive control (MPC).

[16] reviewed the main control schemes, including proportional-integral-derivative (PID)-based, sliding mode control (SMC)-based, adaptive-based, observation-based, model predictive control (MPC)-based combined control techniques, to consolidate the main efforts made by the automatic control community in this field over the last two decades. [12] developed a model predictive controller for a 3 Degrees of Freedom (DOF) unified maneuvering model of the KVLCC2 crude oil tanker. The rudder control was exerted by a nonlinear model predictive controller and the effectiveness of the controller was studied.

[18] employed a nonlinear model predictive control (NMPC) model based on an accurate ship motion model to control the trajectory tracking, efficiently improving the anti-disturbance capability. [21] presented a weight optimization method for a nonlinear MPC (NMPC) based on the genetic algorithm (GA) for ship trajectory tracking. [7] used the model predictive control (MPC) method under constraints to track planned trajectories and optimize motion processes, while ensuring control stability through Lyapunov's theorem. [2] applied a practical nonlinear model predictive control strategy to reduce the computational cost considerably, given the non-negligible computational cost of such a nonlinear predictive guidance strategy.

After learning the research points of above authors, we found that most of the current papers in the field of ship path-tracking only considered the influence of current or wind, and few papers considered the common disturbance of wave, current, and wind together. Therefore, we consider the environmental disturbance and study the application of model predictive control algorithm in the field of ship path-tracking. In particular, we use real meteorological data to construct an environmental disturbance model, use polar diameter and polar angle to construct the path-tracking cost function of underdriven ships, change the fourth-order exponential function to an exponential function, and perform the stability analysis. Finally, Different path-tracking experiment shows that the ship can resist the disturbance of external wave, current, and wind, and track the target path effectively.

Section 1 introduces the significance and status of the path-tracking control problem, as well as the main contents of our research. Section 2 introduces the distribution of wave, current, and wind, and constructs environmental disturbance models. Section 3 introduces the ship's motion model, prediction equations, cost function, improved cost function, and stability analysis. Section 4 introduces the ship anti-disturbance path-tracking control algorithm, and studies the effect of the algorithm in different usage scenarios. Section 5 introduces the conclusions and implications based on the research content of this paper.

2. Environmental disturbance model

Wave, current, and wind loads affect a ship’s movement [4]. To accurately simulate the actual working conditions of the ship, first, the actual wave, current, and wind data of the target sea area were obtained from the major meteorological data platforms, and then, the effective data were used to calculate the change laws of the wave, current, and wind; next, an environment disturbance model were constructed according to the statistical laws, and finally, the environment disturbance model were used as the disturbance item to study the performance of the ship tracking the target path.

2.1. Wave

The actual wave data used in this study were obtained from the National Marine Science Data Center [10]. As shown in Figure 1, from January 1996 to October 2001, the maximum wave height (MH) (m) of the target sea area followed these patterns: The max wave height was concentrated between 0 m and 2.3 m and rarely 2.3 m to 10 m. The distribution of the max wave height was approximately a normal distribution.

2.2. Current

The current dataset from Marine Science Big Data Center [5,9]. As shown in Figure 2, From January 1997 to December 2016, the surface current of 1 square nautical mile in the target sea area had the following characteristics: The maximum combined velocity of the current (MC) (m/s) did not exceed 0.8 m/s, and the distribution of the current combined velocity approximately met the normal distribution.

2.3. Wind

The mixed wind dataset was obtained from the National Environmental Information Center (NCEI) of the National Oceanic and Atmospheric Administration (NOAA) [11]. As shown in Figure 3, From 2002 to 2018, the characteristics of the wind data at 10 m above the target sea area were as follows: The maximum combined velocity of wind (MW) (m/s) was close to 24 m/s, the maximum frequency of combined velocity was about 7.8 m/s, and the statistical histogram of combined velocity frequency distribution approximately met the normal distribution.

2.4. External meteorological model

It is assumed that meteorological conditions remain largely unchanged in the last 30 years. According to the probability density function in Figure 1 , Figure 2, and Figure 3 three groups of arrays MH ～ N (${\mu }_{MH}$, ${\sigma }_{MH}$), MC ～ N (${\mu }_{MC}$,${\sigma }_{MC}$), and MW ～ N (${\mu }_{MW}$, ${\sigma }_{MW}$), be generated that conform to the corresponding normal distribution. After reordering, three groups of arrays (MH0, MC0, MW0) were obtained, this is the environment disturbance model, as shown in Figure 4. The X-axis represents position, and the Y-axis represents the values of wave, current, and wind and the change rate of corresponding values.

The wave, current, and wind values in the figure demonstrate trends from a small value to a medium value, then to a large value, then to a medium value, and finally to a small value. The wave, current, and wind values first increase slowly, then increase rapidly, then decrease rapidly, and finally decrease slowly. The location of the maximum values of wave, current, and wind is the same, and this location produced the maximum disturbance. Therefore, this environment disturbance model was used as the external disturbance of the ship.

3. Model predictive control algorithm

Model predictive control (MPC) is easy to realize multi-objective cooperative control, and has advantages in dealing with multi-input multi-output systems, complex constraint problems, and system nonlinear problems. Therefore, we use the idea of this algorithm to construct a ship anti-disturbance path-tracking method.

Only the 3 DOF of surge, sway, and yaw had to be considered in path-tracking problems. As is shown in Figure 5, the path-tracking problem is to let the ship's position $(\rho \left(k\right),\phi \left(k\right))$ track the target position $({\rho }_r\left(k\right),{\phi }_r\left(k\right))$. The origin of body coordinate frame $O_B$ is fixed at the center of gravity of the ship, $u$ points to the front of the ship, $v$ points to the right of the ship, and $r$ points clockwise. The origin of the global coordinate frame $O_E$ is fixed on the section of the earth's surface, $X_N$ represents to the north direction, $Y_E$ represents to the east direction, and $\phi$ denotes the heading angle between the heading and the north direction. There is a load angle between the environment disturbance and the heading $u$ of the ship.

3.1. Model of ship motion

The Fossen model [13] Equation (1) in the body coordinate frame $O_E$ is given by:

$$\dot{v}+C\left(v\right)v+D\left(v\right)v+g\left(\eta \right)=\tau +w$$

(1)

The parameters corresponding to this expression are as follows:

$$v=[u,v,r{]}^T,\tau =[\tau ,0,\delta {]}^T,w=[w_u,w_v,w_r{]}^T,\eta =[x,y,\phi {]}^T,g\left(\eta \right)=0$$

$$\begin{gathered} M=\left[\begin{array}{ccc}{m}_{11}& 0& 0\\ 0& m_{22}& 0\\ 0& 0& m_{33}\end{array}\right], \\ C\left(v\right)=\left[\begin{array}{ccc}0& 0& -m_{22}v\\ 0& 0& m_{11}u\\ m_{22}v& {-m}_{11}u& 0\end{array}\right], \\ D\left(v\right)=\left[\begin{array}{ccc}{d}_{11}& 0& 0\\ 0& d_{22}& 0\\ 0& 0& d_{33}\end{array}\right]. \\ {m}_{11}=m-X_{\dot{u}},m_{22}=m-Y_{\dot{v}},m_{33}=I_z{-N}_{\dot{r}} \\ {d}_{11}=-X_u-X_{u\left|u\right|}\left|u\right|,d_{22}=-Y_v-Y_{v\left|v\right|}\left|v\right|,d_{33}=-N_r-N_{r\left|r\right|}\left|r\right| \end{gathered}$$

Using the wave load formula in [19], and using the current load formula [3] which is similar to the wind load formula in [14]. The wave load is represented by Equation (3), the current load is represented according to Equation (4), and the wind load is represented by Equation (5).

$$w=w_{wave}+w_{current}+w_{wind}$$

(2)

$$w_{wave}=\frac{1}{2}{\rho }_{water}g{L}_{BP}{{\zeta }_{wave}}^2\left[\begin{array}{c}{C}_{surge1}\left(\lambda \right)\cos \left(\chi \right)\\ C_{sway1}\left(\lambda \right)\sin \left(\chi \right)\\ C_{yaw1}\left(\lambda \right)\sin \left(\chi \right)\end{array}\right]$$

(3)

$$w_{current}=\frac{1}{2}{\rho }_{water}{V_{current}}^2\left[\begin{array}{c}{C}_{surge2}{\left(\beta \right)A}_{surge2}\\ C_{sway2}\left(\beta \right)A_{sway2}\\ C_{yaw2}\left(\beta \right)A_{yaw2}{L}_{BP}\end{array}\right]$$

(4)

$$w_{wind}=\frac{1}{2}{\rho }_{air}{V_{wind}}^2\left[\begin{array}{c}{C}_{surge3}\left(\alpha \right)A_{surge3}\\ C_{sway3}\left(\alpha \right)A_{sway3}\\ C_{yaw3}\left(\alpha \right)A_{yaw3}{L}_{BP}\end{array}\right]$$

(5)

The ship motion model Equation (1) - (5) is regarded as a state-space control system with $v=[u,v,r{]}^T$ as states, $\tau =[\tau ,0,\delta {]}^T$ as inputs, and $w=[w_u,w_v,w_r{]}^T$ as disturbances. The parameters are explained in the above relevant literature, and we set specific parameter values according to the physical conditions of the actual ship, and the value of ${\zeta }_{wave},V_{current},V_{wind}$ is taken from (MH0, MC0, MW0).

The first-order linear equations of the ship's longitudinal, transverse, and yaw directions can be obtained by Taylor Formula. Then, the Euler’s method is used to obtain the discretized ship motion Equation (6). Equation (6) takes the velocities $[u,v,r{]}^T$ of the ship as the states.

$$\tilde{v}\left(\mathrm{k}+1\right)={\mathrm{A}}_{\mathrm{k}}\tilde{v}\left(\mathrm{k}\right)+\mathrm{B}\tilde{\tau }\left(\mathrm{k}\right)+\mathrm{E}\tilde{w}\left(\mathrm{k}\right)$$

(6)

$$\begin{gathered} \tilde{v}=v-v_r,\tilde{\tau }=\tau -{\tau }_r,\tilde{w}=w-w_r \\ v=[u,v,r{]}^T,\tau =[\tau ,0,\delta {]}^T,w=[w_u,w_v,w_r{]}^T \\ {v}_r=[u_r,v_r,r_r{]}^T,{\tau }_r=[{\tau }_r,0,{\delta }_r{]}^T,w_r=[{w_u}_r,{w_v}_r,{w_r}_r{]}^T \\ {\mathrm{A}}_{\mathrm{k}}=\left[\begin{array}{ccc}1-\frac{d_{11}T}{m_{11}}& \frac{m_{22}T}{m_{11}}r\left(k\right)& \frac{m_{22}T}{m_{11}}v\left(k\right)\\ -\frac{m_{11}T}{m_{22}}r\left(k\right)& 1-\frac{d_{22}T}{m_{22}}& -\frac{m_{11}T}{m_{22}}u\left(k\right)\\ \frac{{{(m}_{11}-m}_{22})T}{m_{33}}v\left(k\right)& \frac{{{(m}_{11}-m}_{22})T}{m_{33}}u\left(k\right)& 1-\frac{d_{33}T}{m_{33}}\end{array}\right], \\ \mathrm{B}=\left[\begin{array}{ccc}\frac{\mathrm{T}}{m_{11}}& 0& 0\\ 0& 0& 0\\ 0& 0& \frac{\mathrm{T}}{m_{33}}\end{array}\right],\mathrm{E}=\left[\begin{array}{ccc}\frac{\mathrm{T}}{m_{11}}& 0& 0\\ 0& \frac{\mathrm{T}}{m_{22}}& 0\\ 0& 0& \frac{\mathrm{T}}{m_{33}}\end{array}\right] \end{gathered}$$

Multiply both sides of the Equation (6) by the sampling period $\mathrm{T}$ to get Equation (7). Equation (7) uses the positions $[pu,pv,pr{]}^T$ of the ship as states.

$$\tilde{p}\left(\mathrm{k}+1\right)={\mathrm{A}}_{\mathrm{k}}\ast \tilde{p}\left(\mathrm{k}\right)+\mathrm{B}\ast \mathrm{T}\ast \tilde{\tau }\left(\mathrm{k}\right)+\mathrm{E}\ast \mathrm{T}\ast \tilde{w}\left(\mathrm{k}\right)$$

(7)

$$\tilde{p}=p-p_r,p=[pu,pv,pr{]}^T,p_r=[{pu}_r,{pv}_r,{pr}_r{]}^T$$

3.2. Prediction expression

Using the discretized first-order ship motion Equation (7), a new state-space expression and output expression are constructed, as shown in Equation (8).

$$\left\{\begin{array}{c}\xi \left(\mathrm{k}+1\right)={\mathrm{A}}_{\mathsf{\xi }}\xi \left(\mathrm{k}\right)+{\mathrm{B}}_{\mathsf{\xi }}\Delta \tilde{\tau }\left(\mathrm{k}\right)+{\mathrm{E}}_{\mathsf{\xi }}\tilde{w}\left(\mathrm{k}\right)\\ \zeta \left(\mathrm{k}\right)={\mathrm{H}}_{\mathsf{\xi }}\xi \left(\mathrm{k}\right)\end{array}\right.$$

(8)

Including:

$$\mathsf{\xi }\left(\mathrm{k}+1\right)={\left[\tilde{p}\left(\mathrm{k}+1\right),\tilde{\tau }\left(\mathrm{k}\right)\right]}^T,\Delta \tilde{\tau }\left(\mathrm{k}\right)=\tilde{\tau }\left(\mathrm{k}\right)-\tilde{\tau }\left(\mathrm{k}-1\right),\zeta \left(\mathrm{k}\right)={\left[\tilde{\eta }\left(k\right),\tilde{\tau }\left(\mathrm{k}\right)\right]}^T$$

$$\begin{gathered} \tilde{\eta }=\eta -{\eta }_r,\eta =[x,y,\phi {]}^T,{\eta }_r=[x_r,y_r,{\phi }_r{]}^T \\ {\mathrm{A}}_{\mathsf{\xi }}=\left[\begin{array}{cc}{\mathrm{A}}_{\mathrm{k}}& \mathrm{B}\ast \mathrm{T}\\ 0& \mathrm{I}\end{array}\right],{\mathrm{B}}_{\mathsf{\xi }}=\left[\begin{array}{c}B*T\\ I\end{array}\right],{\mathrm{E}}_{\mathsf{\xi }}=\left[\begin{array}{c}E*T\\ 0\end{array}\right],{\mathrm{H}}_{\mathsf{\xi }}=\left[\begin{array}{cc}\mathrm{H}& 0\\ 0& \mathrm{I}\end{array}\right] \\ \mathrm{H}=\left[\begin{array}{ccc}\cos \left(pr\right)& -\sin \left(pr\right)& 0\\ \sin \left(pr\right)& \cos \left(pr\right)& 0\\ 0& 0& 1\end{array}\right],\mathrm{I}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$

According to the Equation (8), the position of the ship in the global coordinate frame at the next time is obtained:

$$\left\{\begin{array}{c}\zeta \left(\mathrm{k}+1\right)={\mathrm{H}}_{\mathsf{\xi }}\xi \left(\mathrm{k}+1\right)={\mathrm{H}}_{\mathsf{\xi }}{\mathrm{A}}_{\mathsf{\xi }}\xi \left(\mathrm{k}\right)+{\mathrm{H}}_{\mathsf{\xi }}{\mathrm{B}}_{\mathsf{\xi }}\Delta \tilde{\tau }\left(\mathrm{k}\right)+{\mathrm{H}}_{\mathsf{\xi }}{\mathrm{E}}_{\mathsf{\xi }}\tilde{w}\left(\mathrm{k}\right)\\ \zeta \left(\mathrm{k}+2\right)={\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^2\xi \left(\mathrm{k}\right)+H_{\mathsf{\xi }}{A}_{\mathsf{\xi }}{B}_{\mathsf{\xi }}\Delta \tilde{\tau }\left(\mathrm{k}\right)+H_{\mathsf{\xi }}{B}_{\mathsf{\xi }}\Delta \tilde{\tau }\left(\mathrm{k}+1\right)+H_{\mathsf{\xi }}{A}_{\mathsf{\xi }}{E}_{\mathsf{\xi }}\tilde{w}\left(\mathrm{k}\right)+H_{\mathsf{\xi }}{E}_{\mathsf{\xi }}\tilde{w}\left(\mathrm{k}+1\right)\\ \cdots \cdots \\ \zeta \left(\mathrm{k}+N_p\right)={{\mathrm{H}}_{\mathsf{\xi }}A}_{\mathsf{\xi }}^p\xi \left(\mathrm{k}\right)+{\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{\mathrm{p}-1}{\mathrm{B}}_{\mathsf{\xi }}\Delta \tilde{\tau }\left(\mathrm{k}\right)+\cdots +{\mathrm{H}}_{\mathsf{\xi }}{\mathrm{B}}_{\mathsf{\xi }}\Delta \tilde{\tau }\left(k+N_p-1\right)\\ +{\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{\mathrm{p}-1}{\mathrm{E}}_{\mathsf{\xi }}\tilde{w}\left(\mathrm{k}\right)+\cdots +{\mathrm{H}}_{\mathsf{\xi }}{\mathrm{E}}_{\mathsf{\xi }}\tilde{w}\left(k+N_p-1\right)\end{array}\right.$$

(9)

After derivation, the prediction expression of the system is expressed as:

$$Y\left(\mathrm{k}\right)=\Psi \xi \left(\mathrm{k}\right)+\theta \Delta \mathrm{U}\left(\mathrm{k}\right)+\mathsf{\Phi }\mathsf{\Omega }\left(k\right)$$

(10)

$$\begin{gathered} Y\left(\mathrm{k}\right)=\left[\begin{array}{c}\zeta \left(\mathrm{k}+1\right)\\ \zeta \left(\mathrm{k}+2\right)\\ \dots \dots \\ \zeta \left(\mathrm{k}+N_c\right)\\ \dots \dots \\ \zeta \left(\mathrm{k}+N_p\right)\end{array}\right], \\ \Delta \mathrm{U}\left(\mathrm{k}\right)=\left[\begin{array}{c}\Delta \tilde{\tau }\left(\mathrm{k}\right)\\ \Delta \tilde{\tau }\left(\mathrm{k}+1\right)\\ \dots \dots \\ \Delta \tilde{\tau }\left(\mathrm{k}+N_c-1\right)\\ \dots \dots \\ \Delta \tilde{\tau }\left(\mathrm{k}+N_p-1\right)\end{array}\right], \\ \mathsf{\Omega }\left(\mathrm{k}\right)=\left[\begin{array}{c}\tilde{w}\left(\mathrm{k}\right)\\ \tilde{w}(k+1)\\ \dots \dots \\ \tilde{w}(k+N_c-1)\\ \dots \dots \\ \tilde{w}(k+N_p-1)\end{array}\right]. \\ \Psi =\left[\begin{array}{c}{{\mathrm{H}}_{\mathsf{\xi }}A}_{\mathsf{\xi }}\\ {{\mathrm{H}}_{\mathsf{\xi }}A}_{\mathsf{\xi }}^2\\ \dots \dots \\ {{\mathrm{H}}_{\mathsf{\xi }}A}_{\mathsf{\xi }}^{N_c}\\ \dots \dots \\ {{\mathrm{H}}_{\mathsf{\xi }}A}_{\mathsf{\xi }}^{N_p}\end{array}\right], \\ \theta =\left[\begin{array}{cccc}{\mathrm{H}}_{\mathsf{\xi }}{B}_{\mathsf{\xi }}& 0& 0& 0\\ {\mathrm{H}}_{\mathsf{\xi }}{A}_k{B}_{\mathsf{\xi }}& {\mathrm{H}}_{\mathsf{\xi }}{B}_k& 0& 0\\ \dots \dots & \dots \dots & \dots \dots & \dots \dots \\ {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_c-1}{B}_{\mathsf{\xi }}& {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_c-2}{B}_{\mathsf{\xi }}& \dots \dots & {\mathrm{H}}_{\mathsf{\xi }}{B}_{\mathsf{\xi }}\\ {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_c}{B}_{\mathsf{\xi }}& {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_c-1}{B}_{\mathsf{\xi }}& \dots \dots & {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}{B}_{\mathsf{\xi }}\\ \dots \dots & \dots \dots & \dots \dots & \dots \dots \\ {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_p-1}{B}_{\mathsf{\xi }}& {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_p-2}{B}_{\mathsf{\xi }}& \dots \dots & {\mathrm{H}}_{\mathsf{\xi }}{\mathrm{A}}_{\mathsf{\xi }}^{N_p-N_c-1}{B}_{\mathsf{\xi }}\end{array}\right] \\ \mathsf{\Phi }=\left[\begin{array}{cccc}{\mathrm{H}}_{\mathsf{\xi }}{E}_{\mathsf{\xi }}& 0& 0& 0\\ {\mathrm{H}}_{\mathsf{\xi }}{A}_k{E}_{\mathsf{\xi }}& {\mathrm{H}}_{\mathsf{\xi }}{E}_k& 0& 0\\ \dots \dots & \dots \dots & \dots \dots & \dots \dots \\ {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_c-1}{E}_{\mathsf{\xi }}& {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_c-2}{E}_{\mathsf{\xi }}& \dots \dots & {\mathrm{H}}_{\mathsf{\xi }}{E}_{\mathsf{\xi }}\\ {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_c}{E}_{\mathsf{\xi }}& {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_c-1}{E}_{\mathsf{\xi }}& \dots \dots & {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}{E}_{\mathsf{\xi }}\\ \dots \dots & \dots \dots & \dots \dots & \dots \dots \\ {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_p-1}{E}_{\mathsf{\xi }}& {\mathrm{H}}_{\mathsf{\xi }}{A}_{\mathsf{\xi }}^{N_p-2}{E}_{\mathsf{\xi }}& \dots \dots & {\mathrm{H}}_{\mathsf{\xi }}{\mathrm{A}}_{\mathsf{\xi }}^{N_p-N_c-1}{E}_{\mathsf{\xi }}\end{array}\right] \end{gathered}$$

3.3. Cost function

It is shown in Equation (11), that we combine the ship position states $x$ and $y$ in the global polar coordinate system to obtain the global polar diameter position ${\mathsf{\rho }}^2$, and obtain the global polar angle position $\mathsf{\phi }$ according to the heading position $pr$.

$$\left\{\begin{array}{c}{\mathsf{\rho }}^2\left(\mathrm{k}\right)={x\left(\mathrm{k}\right)}^2+{y\left(\mathrm{k}\right)}^2={pu\left(\mathrm{k}\right)}^2+{pv\left(\mathrm{k}\right)}^2\\ \phi \left(\mathrm{k}\right)=pr\left(\mathrm{k}\right)\end{array}\right.$$

(11)

In this way, we can accurately track any target path $TJ({\mathsf{\rho }}_{\mathrm{r}},{\phi }_{\mathrm{r}})$ by controlling the polar diameter ${\mathsf{\rho }}^2$ and polar angle $\mathsf{\phi }$ of the ship separately in the global polar coordinate system. Simultaneously, it is necessary to avoid sudden changes in the control quantity at each time. Accordingly, the cost function of the ship's radial motion and the ship's heading motion is given by Equation (12).

$$J_{original}\left(k\right)=\left\{\begin{array}{c}{\mathrm{J}}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(\mathrm{k}\right)={\displaystyle \sum _{i=1}^{{\mathrm{N}}_{\mathrm{p}}}}{\mathrm{Q}}_{\mathsf{\rho }}{‖{\mathsf{\rho }}^2\left(\mathrm{k}+\mathrm{i}\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(\mathrm{k}+\mathrm{i}\right)‖}^2+{\displaystyle \sum _{\mathrm{i}=1}^{{\mathrm{N}}_{\mathrm{c}}-1}}{\mathrm{R}}_{\mathsf{\rho }}{‖\Delta \mathsf{\tau }\left(\mathrm{k}+\mathrm{i}\right)‖}^2\\ {\mathrm{J}}_{\left(\mathsf{\phi },\Delta \delta \right)}\left(\mathrm{k}\right)={\displaystyle \sum _{\mathrm{i}=1}^{{\mathrm{N}}_{\mathrm{p}}}}{\mathrm{Q}}_{\mathsf{\phi }}{‖\mathsf{\phi }\left(\mathrm{k}+\mathrm{i}\right)-{\phi }_{\mathrm{r}}\left(\mathrm{k}+\mathrm{i}\right)‖}^2+{\displaystyle \sum _{\mathrm{i}=1}^{{\mathrm{N}}_{\mathrm{c}}-1}}{\mathrm{R}}_{\mathsf{\phi }}{‖\Delta \delta \left(\mathrm{k}+\mathrm{i}\right)‖}^2\end{array}\right.,k=\mathrm{1,2},\dots ,$$

(12)

$$Constraints.\left\{\begin{array}{c}{\mathsf{\tau }}_{\min }\left(k\right)\le \tau \left(k\right)\le {\mathsf{\tau }}_{max}\left(k\right)\\ {\delta }_{\min }\left(k\right)\le \delta \left(k\right)\le {\delta }_{max}\left(k\right)\\ \Delta {\mathsf{\tau }}_{\min }\left(k\right)\le \Delta \tau \left(k\right)\le \Delta {\mathsf{\tau }}_{max}\left(k\right)\\ \Delta {\delta }_{\min }\left(k\right)\le \Delta \delta \left(k\right)\le \Delta {\delta }_{max}\left(k\right)\\ \Delta \tau \left(k\right)=\mathsf{\tau }\left(k\right)-\tau \left(k-1\right)\\ \Delta \delta \left(k\right)=\delta \left(k\right)-\delta \left(k-1\right)\end{array}\right.,k=\mathrm{1,2},\dots ,$$

(13)

$$Optimization.\left\{\begin{array}{c}{\mathsf{\tau }}_k^{\ast }={arg}_{{\mathsf{\tau }}_k}=\min {\mathrm{J}}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(\mathrm{k}\right)\\ {\delta }_k^{\ast }={arg}_{{\delta }_k}=\min {\mathrm{J}}_{\left(\mathsf{\phi },\Delta \delta \right)}\left(\mathrm{k}\right)\end{array}\right.,k=\mathrm{1,2},\dots ,$$

(14)

The cost function $J_{original}\left(k\right)$ consist of ${\mathrm{J}}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(\mathrm{k}\right)$ and ${\mathrm{J}}_{\left(\mathsf{\phi },\Delta \delta \right)}\left(\mathrm{k}\right)$, $\mathrm{k}$ is the current moment, ${\mathrm{N}}_{\mathrm{p}}$ is the number of predicted steps, $N_c$ is the number of control steps, ${\mathrm{Q}}_{\mathsf{\rho }},{\mathrm{R}}_{\mathsf{\rho }},{\mathrm{Q}}_{\mathsf{\phi }}{,\mathrm{R}}_{\mathsf{\phi }}$ are weight matrix. The constraint expression of the ship control quantity $\mathsf{\tau }$ and $\delta$ and the constraint expression of the control increment $\Delta \mathsf{\tau }$ and $\Delta \delta$, is shown in Equation (13). The goal of optimization is to find the optimal amount of control for each step, as shown in the Equation (14).

4. Algorithm improve and analysis

4.1. Artificial potential field

The attractive field formula of the artificial potential field method [6] is shown in Equation (15), where $\mu$ is the scale factor, $d\left(q{,q}_{goal}\right)$ represents the distance from the current position $q$ to the target position $q_{goal}$, and $d^{\ast }$ is the distance domain value. According to the Equation (15), we know that within the distance $d^{\ast }$, the greater the distance from the current position to the target position, the greater the attraction.

$$\left\{\begin{array}{c}F\left(d\right)=\frac{1}{2}{\mu d\left(q{,q}_{goal}\right)}^2, d\le d^{\ast }\\ \nabla F\left(\mathrm{d}\right)=\mu d\left(q{,q}_{goal}\right), d\le d^{\ast }\end{array}\right.$$

(15)

From the characteristics of the attractive field formula Equation (15) and the corresponding gradient formula, we know that when ${\mathrm{N}}_{\mathrm{p}}=1$ the gradients in both directions of the original cost function ${\mathrm{J}}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(\mathrm{k}\right)$ are equal. It does not distinguish the magnitude of the respective effects of the two variables ${\mathsf{\rho }}^2$ and $\Delta \mathsf{\tau }$.

4.2. Improved cost function

We replace one power function ${‖{\mathsf{\rho }}^2\left(\mathrm{k}+\mathrm{i}\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(\mathrm{k}+\mathrm{i}\right)‖}^2$ of the original cost function ${\mathrm{J}}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(\mathrm{k}\right)$ with an exponential function $e^{f\left({\mathsf{\rho }}^2{,{\mathsf{\rho }}_{\mathrm{r}}}^2\right)}$ and obtain a new cost function $\mathrm{F}\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)$, as show in the below.

$$\mathrm{F}\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)=\left\{\begin{array}{c}{e}^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2-{\mathsf{\rho }}^2\right)}+{\left(\Delta \mathsf{\tau }-0\right)}^2, {\mathsf{\rho }}^2\le {{\mathsf{\rho }}_{\mathrm{r}}}^2\\ e^{\left({\mathsf{\rho }}^2-{{\mathsf{\rho }}_{\mathrm{r}}}^2\right)}+{\left(\Delta \mathsf{\tau }-0\right)}^2, {\mathsf{\rho }}^2>{{\mathsf{\rho }}_{\mathrm{r}}}^2\end{array},\right.{\mathsf{\tau }}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le \Delta \mathsf{\tau }\le \Delta {\mathsf{\tau }}_{max}$$

(16)

The value of the cost function $\mathrm{F}\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)$ becomes larger as the current variable $({\mathsf{\rho }}^2,\Delta \mathsf{\tau })$ moves away from the target variable $({{\mathsf{\rho }}_{\mathrm{r}}}^2,0)$, and smaller as the current variable $({\mathsf{\rho }}^2,\Delta \mathsf{\tau })$ moves closer to the target variable $({{\mathsf{\rho }}_{\mathrm{r}}}^2,0)$, so the minimum value of the cost function $\mathrm{F}\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)$ is always achieved at $({{\mathsf{\rho }}_{\mathrm{r}}}^2,0)$.

According to the characteristics of the cost function $\mathrm{F}\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)$ the polar diameter ${\mathsf{\rho }}^2$ is taken as the greater descending direction and the control quantity $\Delta \mathsf{\tau }$ is taken as another direction. Then, the improved cost function Equation (17) is obtained.

$$J_{improved}\left(k\right)=\left\{\begin{array}{c}{\mathrm{F}}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(\mathrm{k}\right)={\displaystyle \sum _{i=1}^{{\mathrm{N}}_{\mathrm{p}}}}{Q}_1\left\{\begin{array}{c}‖e^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+i\right)-{\mathsf{\rho }}^2\left(k+i\right)\right)}‖, {\mathsf{\rho }}^2\left(k\right)\le {{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\\ ‖e^{\left({\mathsf{\rho }}^2\left(k+i\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+i\right)\right)}‖, {\mathsf{\rho }}^2\left(k\right)>{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\end{array}\right.+{\displaystyle \sum _{i=1}^{{\mathrm{N}}_{\mathrm{c}}-1}}{R_1‖\Delta \mathsf{\tau }\left(\mathrm{k}+\mathrm{i}\right)‖}^2\\ {\mathrm{J}}_{\left(\mathsf{\phi },\Delta \delta \right)}\left(\mathrm{k}\right)={\mathrm{Q}}_{\mathsf{\phi }}{\displaystyle \sum _{\mathrm{i}=1}^{{\mathrm{N}}_{\mathrm{p}}}}{‖\mathsf{\phi }\left(\mathrm{k}+\mathrm{i}\right)-{\phi }_{\mathrm{r}}\left(\mathrm{k}+\mathrm{i}\right)‖}^2+{\mathrm{R}}_{\mathsf{\phi }}{\displaystyle \sum _{\mathrm{i}=1}^{{\mathrm{N}}_{\mathrm{c}}-1}}{‖\Delta \delta \left(\mathrm{k}+\mathrm{i}\right)‖}^2\end{array}\right.,k=\mathrm{1,2},\dots ,$$

(17)

$$Constraints.\left\{\begin{array}{c}{\mathsf{\tau }}_{\min }\left(k\right)\le \tau \left(k\right)\le {\mathsf{\tau }}_{max}\left(k\right)\\ {\delta }_{\min }\left(k\right)\le \delta \left(k\right)\le {\delta }_{max}\left(k\right)\\ \Delta {\mathsf{\tau }}_{\min }\left(k\right)\le \Delta \tau \left(k\right)\le \Delta {\mathsf{\tau }}_{max}\left(k\right)\\ \Delta {\delta }_{\min }\left(k\right)\le \Delta \delta \left(k\right)\le \Delta {\delta }_{max}\left(k\right)\\ \Delta \tau \left(k\right)=\mathsf{\tau }\left(k\right)-\tau \left(k-1\right)\\ \Delta \delta \left(k\right)=\delta \left(k\right)-\delta \left(k-1\right)\end{array}\right.,k=\mathrm{1,2},\dots ,$$

(18)

$$Optimization.\left\{\begin{array}{c}{\mathsf{\tau }}_k^{\ast }={arg}_{{\mathsf{\tau }}_k}=\min {\mathrm{F}}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(\mathrm{k}\right)\\ {\delta }_k^{\ast }={arg}_{{\delta }_k}=\min {\mathrm{J}}_{\left(\mathsf{\phi },\Delta \delta \right)}\left(\mathrm{k}\right)\end{array}\right.,k=\mathrm{1,2},\dots ,$$

(19)

The improved cost function $J_{improved}\left(k\right)$ consist of ${\mathrm{F}}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(\mathrm{k}\right)$ and ${\mathrm{J}}_{\left(\mathsf{\phi },\Delta \delta \right)}\left(\mathrm{k}\right)$, $\mathrm{k}$ is the current moment, ${\mathrm{N}}_{\mathrm{p}}$ is the number of predicted steps, $N_c$ is the number of control steps, ${\mathrm{Q}}_1,{\mathrm{R}}_1,{\mathrm{Q}}_{\mathsf{\phi }}{,\mathrm{R}}_{\mathsf{\phi }}$ are weight matrix. The constraint expression of the ship control quantity $\mathsf{\tau }$ and $\delta$ and the constraint expression of the control increment $\Delta \mathsf{\tau }$ and $\Delta \delta$, is shown in Equation (18). The goal of optimization is to find the optimal amount of control for each step, as shown in Equation (19).

4.3. Stability Analysis

Lyapunov theorem:

$$\left\{\begin{array}{c}‖x,u‖\to \infty \Rightarrow V\left(x,u\right)\to \infty \\ V\left(x,u\right)>0, \forall \left(x,u\right)\ne \left(\mathrm{0,0}\right)\\ V\left(x,u\right)=0, if and only if \left(x,u\right)=\left(\mathrm{0,0}\right)\\ V\left(x_{k+1},u_{k+1}\right)-V\left(x_k,u_k\right)<-\alpha \left(x_k,u_k\right), \forall \left(x,u\right)\ne \left(\mathrm{0,0}\right),\alpha \left(x_k,u_k\right)>0\\ k\to \infty \Rightarrow V\left(x_k,u_k\right)\to 0\end{array}\right.$$

(20)

$k$ is the time, $\left(\mathrm{0,0}\right)$ is a stable equilibrium point of $V\left(x,u\right)$, and $V\left(x,u\right)$ is asymptotically stable. Such a function $V\left(x,u\right)$ is called Lyapunov function.

Assumed function $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k\right)$ expression is as follows:

$$\begin{gathered} V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k\right)={\sum }_{\mathrm{i}=0}^{N_p-1}{Q}_1\left\{\begin{array}{c}‖e^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+i\right)-{\mathsf{\rho }}^2\left(k+i\right)\right)}‖,{\mathsf{\rho }}^2\left(k\right)\le {{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\\ ‖e^{\left({\mathsf{\rho }}^2\left(k+i\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+i\right)\right)}‖,{\mathsf{\rho }}^2\left(k\right)>{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\end{array}\right. \\ +\sum _{i=0}^{N_c-1}{R}_1{‖\Delta \mathsf{\tau }\left(\mathrm{k}+\mathrm{i}\right)‖}^2,k=\mathrm{1,2},3,\cdots \end{gathered}$$

(21)

In the above expression, the weight matrix $Q_1>0,R_1>0$, the prediction step is $N_p$, and the control step is $N_c$, ${\mathsf{\rho }}^2\left(k\right)$ represents the polar diameter of the ship at time $k$ in the global coordinate system, ${{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)$ represents the target polar diameter value at time $k$ in the global coordinate system, $\Delta \mathsf{\tau }$ represents the increment of polar diameter control quantity.

Set terminal constraints for the states of the system.

$$\forall i\ge N_c,{\mathsf{\rho }}^2\left(k+i\right)\in \mathsf{\Pi },\exists {\mathsf{\tau }}_{\mathsf{\Gamma }}\in \mathsf{\Gamma }\cup {\mathsf{\delta }}_{\mathsf{\Gamma }}\in \mathsf{\Gamma }\Rightarrow {\mathsf{\rho }}^2(k+i)⟶0,\Delta \mathsf{\tau }\left(\mathrm{k}+\mathrm{i}\right)⟶0$$

$\mathsf{\Pi }$ represents terminal constraint set, $\mathsf{\Gamma }$ represents control law set, ${\mathsf{\tau }}_{\mathsf{\Gamma }}$ represents the polar diameter control law, ${\mathsf{\delta }}_{\mathsf{\Gamma }}$ represents the polar angle control law.

When $N_c\le i\le N_p-1$, polar diameter control increment $\Delta \mathsf{\tau }$ becomes 0, and the function $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k\right)$ becomes:

$$V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{cp}\left(k\right)={\displaystyle \sum _{\mathrm{i}=N_c}^{N_p-1}}{Q}_1\left\{\begin{array}{c}‖e^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+i\right)-{\mathsf{\rho }}^2\left(k+i\right)\right)}‖,{\mathsf{\rho }}^2\left(k\right)\le {{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\\ ‖e^{\left({\mathsf{\rho }}^2\left(k+i\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+i\right)\right)}‖,{\mathsf{\rho }}^2\left(k\right)>{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\end{array}\right.$$

(22)

Since the ship control system is stable, there are

$$N_c\le i\le N_p-1\Rightarrow 0\le V_m\le V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{cp}\left(k\right)\le V_M\le {\displaystyle \sum _{\mathrm{i}=N_c}^{N_p}}{Q}_1{e}^{{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+i\right)}$$

Where $V_m$ is the lower limit of $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{cp}\left(k\right)$, $V_M$ is the upper limit of $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{cp}\left(k\right)$, in other words $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{cp}\left(k\right)$ is bounded, moreover the system state has terminal constraints. As long as the appropriate control quantity is selected, the system can always make the bounded value converge to zero.

The process of tracking the path is the process of tracking each target path point with the increase of time $k$. The polar direction uses the $N_p$ step prediction tracking method each time, and each solution results in an optimal solution sequence ${\tau }^{\ast }\left(k\right)=\left\{{\tau }_1^{\ast },{\tau }_2^{\ast },\cdots ,{\tau }_{N_c}^{\ast },\cdots ,{\tau }_{N_p}^{\ast }\right\}$, select the first pole diameter optimal control quantity ${\tau }_1^{\ast }$ to obtain the corresponding function $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{\ast }\left(k\right)$ and $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{\ast }\left(k\right)<V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k\right)$.

In the time of $k+1$

$$\begin{array}{c}{V}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k+1\right)={\displaystyle \sum _{\mathrm{i}=0}^{N_p-1}}{Q}_1\left\{\begin{array}{c}‖e^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1+i\right)-{\mathsf{\rho }}^2\left(k+1+i\right)\right)}‖,{\mathsf{\rho }}^2\left(k+1\right)\le {{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1\right)\\ ‖e^{\left({\mathsf{\rho }}^2\left(k+1+i\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1+i\right)\right)}‖,{\mathsf{\rho }}^2\left(k+1\right)>{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1\right)\end{array}\right.\\ +{\displaystyle \sum _{i=0}^{N_c-1}}{R}_1{‖\Delta \mathsf{\tau }\left(\mathrm{k}+1+\mathrm{i}\right)‖}^2\end{array}$$

(23)

$$\begin{array}{c}{V}_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k+1\right)=V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k\right)+\\ Q_1\left\{\begin{array}{c}‖e^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1+N_p\right)-{\mathsf{\rho }}^2\left(k+1+N_p\right)\right)}‖,{\mathsf{\rho }}^2\left(k+1\right)\le {{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1\right)\\ ‖e^{\left({\mathsf{\rho }}^2\left(k+1+N_p\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1+N_p\right)\right)}‖,{\mathsf{\rho }}^2\left(k+1\right)>{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1\right)\end{array}\right.+R_1{‖\Delta \mathsf{\tau }\left(\mathrm{k}+N_c\right)‖}^2-\\ Q_1\left\{\begin{array}{c}‖e^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)-{\mathsf{\rho }}^2\left(k\right)\right)}‖\\ ‖e^{\left({\mathsf{\rho }}^2\left(k\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\right)}‖\end{array}\right.-R_1{‖\Delta \mathsf{\tau }\left(\mathrm{k}\right)‖}^2\end{array}$$

(24)

Let

$${\alpha }_1=Q_1\left\{\begin{array}{c}‖e^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1+N_p\right)-{\mathsf{\rho }}^2\left(k+1+N_p\right)\right)}‖,{\mathsf{\rho }}^2\left(k+1\right)\le {{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1\right)\\ ‖e^{\left({\mathsf{\rho }}^2\left(k+1+N_p\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1+N_p\right)\right)}‖,{\mathsf{\rho }}^2\left(k+1\right)>{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k+1\right)\end{array}\right.+R_1{‖\Delta \mathsf{\tau }\left(\mathrm{k}+N_c\right)‖}^2$$

(25)

$${\alpha }_2=Q_1\left\{\begin{array}{c}‖e^{\left({{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)-{\mathsf{\rho }}^2\left(k\right)\right)}‖,{\mathsf{\rho }}^2\left(k\right)\le {{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\\ ‖e^{\left({\mathsf{\rho }}^2\left(k\right)-{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\right)}‖,{\mathsf{\rho }}^2\left(k\right)>{{\mathsf{\rho }}_{\mathrm{r}}}^2\left(k\right)\end{array}\right.+R_1{‖\Delta \mathsf{\tau }\left(\mathrm{k}\right)‖}^2$$

(26)

Here

$$\forall {\mathsf{\rho }}^2,\Delta \mathsf{\tau }\to {\mathsf{\alpha }}_1\ge 0,{\mathsf{\alpha }}_2\ge 0$$

(27)

Therefore

$$V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k+1\right)=V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k\right)+{\alpha }_1-{\alpha }_2$$

(28)

At $k+1$, after the optimal solution control quantity ${\tau }_1^{\ast }(k+1)$ was inputed into the system

$$V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{\ast }\left(k+1\right)=V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{\ast }\left(k\right)+{\alpha }_1^{\ast }-{\alpha }_2^{\ast }$$

(29)

Due to the terminal constraints of system states, such that ${\alpha }_1^{\ast }=0$ so

$$V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{\ast }\left(k+1\right)=V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{\ast }\left(k\right)+{\alpha }_1^{\ast }-{\alpha }_2^{\ast }$$

(30)

Here is the derivation

$$V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{\ast }\left(k+1\right)<V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{\ast }\left(k\right)$$

(31)

And we can get:

$$k\to \mathrm{\infty }\Rightarrow V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}^{*}\left(k\right)\to 0$$

(32)

To sum up: The function $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}$ satisfies the stability condition of the Lyapunov function, the zero point is an equilibrium point, and the function $V_{\left({\mathsf{\rho }}^2,\Delta \mathsf{\tau }\right)}\left(k\right)$ is globally asymptotically stable.

5. Path-tracking experiment simulation

5.1. Experimental setup

Different paths were set: 1) Straight line $Y_E=X_N$. 2) cycle line ${(X_N-10)}^2+{Y_E}^2={10}^2$. 3) outline of SMU-Smart Lake. External meteorological disturbance model (MH0, MC0, MW0) was introduced. Nonlinear ship motion model Equation (7) was used. The improved cost function $J_{improved}\left(k\right)$ and the original cost function $J_{original}\left(k\right)$ were adopted, respectively.

5.2. Path-tracking algorithm

The algorithm of path-tracking is shown below.

Path-tracking MPC algorithm

1. Design the target path $\mathbf{T}\mathbf{J}({\mathsf{\phi }}_{\mathbf{r}},{\mathsf{\rho }}_{\mathbf{r}}$ ) according to the two polar parameters and store the target path in discrete form. 2. Set the sampling period $\mathbf{T}$ , predict step size $\mathbf{p}$ , initial control quantity $\mathsf{\tau },\mathsf{\delta },$ initial state variables $\mathbf{u},\mathbf{v},\mathbf{r}$ 3. Set parameters such as the ship's mass, moment of inertia, damping coefficient, and additional mass. 4. Set parameters such as encounter angle, density, speed, drag coefficient, cross-sectional area, and ship length. 5. According to the environment disturbance model, set their respective sequences $\mathbf{M}\mathbf{H}0,\mathbf{M}\mathbf{C}0,\mathbf{M}\mathbf{W}0$ 6. Calculate the load of wave, current, and wind, and obtain the longitudinal disturbance load ${\mathbf{w}}_{\mathbf{u}}$ , the lateral disturbance load ${\mathbf{w}}_{\mathbf{v}}$ , and the yaw direction disturbance load ${\mathbf{w}}_{\mathbf{r}}.$ $\mathbf{f}\mathbf{o}\mathbf{r}\mathbf{k}=1:\mathbf{l}\mathbf{e}\mathbf{n}\mathbf{g}\mathbf{h}\left(\mathbf{T}\mathbf{J}\right)$ Obtain the $\mathbf{k}-\mathbf{t}\mathbf{h}$ longitudinal disturbance ${\mathbf{w}}_{\mathbf{u}}\left(\mathbf{k}\right)$ lateral disturbance ${\mathbf{w}}_{\mathbf{v}}\left(\mathbf{k}\right)$ , and yaw direction disturbance ${\mathbf{w}}_{\mathbf{r}}\left(\mathbf{k}\right)$ Update parameters of ship motion equation and parameters of wave, current, and wind. Update the position equation $\mathbf{p}\mathbf{u}\left(\mathbf{k}\right),\mathbf{p}\mathbf{v}\left(\mathbf{k}\right),\mathbf{p}\mathbf{v}\left(\mathbf{k}\right)$ Calculate the heading prediction state matrix ${\mathbf{C}}_1,$ predictive control matrix ${\mathbf{C}}_2$ From the above prediction matrix, the prediction heading of the future $\mathbf{p}-\mathbf{s}\mathbf{t}\mathbf{e}\mathbf{p}$ is ${\mathsf{\phi }}_{\mathbf{p}}\left(\mathbf{k}\right).$ From $\mathbf{p}\mathbf{u}\left(\mathbf{k}\right),\mathbf{p}\mathbf{v}\left(\mathbf{k}\right)$ the radial motion equation ${\mathsf{\rho }}^2\left(\mathbf{k}\right)$ of the ship is obtained. According to the expression of ${\mathsf{\rho }}^2\left(\mathbf{k}\right),{\mathsf{\phi }}_{\mathbf{p}}\left(\mathbf{k}\right)$ , the cost function coefficient matrix ${\mathbf{Q}}_1,{\mathbf{R}}_1,{\mathbf{Q}}_{\mathsf{\phi }},{\mathbf{R}}_{\mathsf{\phi }}$ is determined. Obtain the $\mathbf{k}-\mathbf{t}\mathbf{h}$ target pole diameter value ${{\mathsf{\rho }}_{\mathbf{r}}\left(\mathbf{k}\right)}^2$ from the target path $\mathbf{T}\mathbf{J}({\mathsf{\phi }}_{\mathbf{r}},{\mathsf{\rho }}_{\mathbf{r}})$. Let ${{\mathsf{\rho }}_{\mathbf{r}}\left(\mathbf{k}\right)}^2-{{\mathsf{\rho }}_{\mathbf{k}}}^2={\mathbf{a}}_{\mathsf{\rho }}{\mathsf{\tau }}^2+{\mathbf{b}}_{\mathsf{\rho }}\mathsf{\tau }+{\mathbf{c}}_{\mathsf{\rho }}$ Cost function ${\mathbf{J}}_{\mathsf{\rho }}={\mathbf{Q}}_{\mathsf{\rho }}{\mathbf{e}}^{({\mathbf{a}}_{\mathsf{\rho }}{\mathsf{\tau }}^2+{\mathbf{b}}_{\mathsf{\rho }}\mathsf{\tau }+{\mathbf{c}}_{\mathsf{\rho }})}+\mathsf{\tau }{\mathbf{R}}_{\mathsf{\rho }}\mathsf{\tau }$ with constraints ${\mathbf{a}}_{\mathsf{\rho }}{\mathsf{\tau }}^2+{\mathbf{b}}_{\mathsf{\rho }}\mathsf{\tau }+{\mathbf{c}}_{\mathsf{\rho }}\ge 0$ Gradient descent algorithm is used to solve the function ${\mathbf{J}}_{\mathsf{\rho }}$ with $\mathsf{\tau }$ as the variable, ${\mathsf{\tau }}_0$ is obtained. If the amount of control for the first few steps is too small, set the amount of control to a large value to speed up tracking. Take ${\mathsf{\tau }}_0$ stored in variables ${\mathsf{\tau }}_{\mathbf{k}}$ Bring ${\mathsf{\tau }}_0$ into the function ${\mathbf{p}\mathbf{u}}_{\mathbf{k}}\left(\mathbf{p}\mathbf{u},\mathsf{\tau },\cdots \right),{\mathbf{p}\mathbf{v}}_{\mathbf{k}}\left(\mathbf{p}\mathbf{v},\cdots \right)$ to get the position $\mathbf{p}\mathbf{u},\mathbf{p}\mathbf{v}$ at the next moment. Radial prediction position at the next moment ${\mathsf{\rho }}_{\mathbf{k}+1}=\mathbf{s}\mathbf{q}\mathbf{r}\mathbf{t}\left({\mathbf{p}\mathbf{u}}^2+{\mathbf{p}\mathbf{v}}^2\right)$ $\mathbf{i}\mathbf{f}\mathbf{k}\le \left(\mathbf{l}\mathbf{e}\mathbf{n}\mathbf{g}\mathbf{t}\mathbf{h}\left(\mathbf{T}\mathbf{J}\right)-\mathbf{p}+1\right)$ $\mathbf{j}=\mathbf{k};$ $\mathbf{e}\mathbf{l}\mathbf{s}\mathbf{e}$ $\mathbf{j}=\left(\mathbf{l}\mathbf{e}\mathbf{n}\mathbf{g}\mathbf{t}\mathbf{h}\left(\mathbf{T}\mathbf{J}\right)-\mathbf{p}+1\right);$ $\mathbf{e}\mathbf{n}\mathbf{d}$ $\mathbf{f}\mathbf{o}\mathbf{r}\mathbf{i}1=\mathbf{j}:(\mathbf{j}+\mathbf{p}-1)$ Starting from the $\mathbf{j}-\mathbf{t}\mathbf{h}$ polar angle value of the target path, take $\mathbf{p}-\mathbf{s}\mathbf{t}\mathbf{e}\mathbf{p}$ continuously to obtain ${\mathbf{T}}_{\mathsf{\phi }}$ $\mathbf{e}\mathbf{n}\mathbf{d}$ Construct the cost function of pole angle ${{\mathbf{J}}_{\mathsf{\phi }}=({\mathsf{\phi }}_{\mathbf{p}}-{\mathbf{T}}_{\mathsf{\phi }})}^{\mathbf{T}}{\mathbf{Q}}_{\mathsf{\phi }}({\mathsf{\phi }}_{\mathbf{p}}-{\mathbf{T}}_{\mathsf{\phi }})+\mathsf{\delta }{\mathbf{R}}_{\mathsf{\phi }}\mathsf{\delta }={\mathbf{J}}_{\mathsf{\phi }}\left({\mathsf{\delta }}^2,\mathsf{\phi },\cdots \right)$ The quadratic programming algorithm is used to solve the quadratic function ${\mathbf{J}}_{\mathsf{\phi }}\left({\mathsf{\delta }}^2,\mathsf{\phi },\cdots \right)$ with $\mathsf{\delta }$ as the variable to obtain the ${\mathsf{\delta }}_0.$ Take ${\mathsf{\delta }}_0$ stored in variables ${\mathsf{\delta }}_{\mathbf{k}}.$ Bring ${\mathsf{\delta }}_0$ into the function ${\mathbf{p}\mathbf{r}}_{\mathbf{k}}\left(\mathbf{p}\mathbf{r},\mathsf{\delta },\cdots \right)$ to get the heading ${\mathbf{p}\mathbf{r}}_{\mathbf{k}+1}$ of the next moment. Subtract the $\mathbf{p}\mathbf{u}\left(\mathbf{k}\right),\mathbf{p}\mathbf{v}\left(\mathbf{k}\right),\mathbf{p}\mathbf{r}\left(\mathbf{k}\right)$ from the $\mathbf{p}\mathbf{u}(\mathbf{k}+1),\mathbf{p}\mathbf{v}(\mathbf{k}+1),\mathbf{p}\mathbf{r}(\mathbf{k}+1)$ , respectively, and divide the difference by $\mathbf{T}$ to get the new $\mathbf{u},\mathbf{v},\mathbf{r}.$ $\mathbf{e}\mathbf{n}\mathbf{d}$ Draw the target path, follow path, control quantity, etc. separately.

5.3. The impact of disturbance

First, the rudder angle control amount was fixed at 0, the longitudinal control amount was set to 100000, and the influence of external disturbance on the ship's motion is shown in Figure 6. Then the rudder angle control amount was set to 10000, the longitudinal control amount was fixed to 0, and the test results of external disturbances affecting the ship's movement are shown in Figure 7.

As observed from Figure 6 and Figure 7, under the influence of wave, current, and wind the ship's path is shifted, when there was no rudder angle with a constant driving force, the ship's motion track is approximately S-shaped, and when there is no driving force with constant rudder angle, the ship movement is negligible, and the path of movement is approximate to a combined spiral.

5.4. Line and circle path-tracking

The linear path-tracking and the circular path-tracking simulation results are shown in Figure 8 and Figure 9, respectively.

The tracking polar diameter curve is slightly smaller than the target polar diameter curve, and the tracking polar angle curve is stable after catching up with the target polar angle curve. The tracking line coincides with the target line, and the tracking line is slightly larger than the target line. With the increase of control steps, the polar diameter control quantity gradually increases according to a certain slope after catching up with the target polar diameter, and the polar angle control quantity is stable at zero after catching up with the target polar angle. As the external wave, current, and wind changed over time, the tracking path offset of the ship's heading was not affected. Overall, the straight-line path-tracking effect is desirable.

The tracking polar diameter value is slightly less than the target polar diameter value before the maximum of the target polar diameter value, the tracking polar diameter value is slightly greater than the target polar diameter value after the maximum of the target polar diameter value, and the tracking polar angle curve is slightly less than the target polar angle curve after catching up with the target polar angle curve; The diameter of the tracking curve is slightly smaller than that of the target circle, and the circular tracking error increases from small to large and then decreases. With the increase of control steps, the pole diameter control value decreases from a large positive value to zero and then changes to a large negative value, and the pole angle control value decreases from a large negative value to zero and then changes to a small positive value and is stable. From the results of the simulation, the influence of the external wave, current, and wind on the path of the ship has been compensated by the control variable. Overall, circular path-tracking works as desired.

5.5. Complex curve path-tracking

Since the outline of SMU-Smart Lake is composed of curves with different curvatures, we use the contour curve of SMU-Smart Lake to perform the path-tracking test. According to the contour curve of SMU-Smart Lake, the target path is drawn, the environmental disturbances model (MH0, MC0, MW0) were added to the path, and the path-tracking simulation test is performed using the original cost function and the improved cost function, respectively. After simulation, the path-tracking results of the improved cost function and the original cost function are shown in Figure 10 and Figure 11, respectively.

It can be seen from the SMU-Smart Lake tracking path-tracking process in Figure 10 that when the curvature of the target curve changes significantly, the deviation between the tracking curve and the target curve is large, this shows that when the curvature of the target path is large, the tracking effect of the improved cost function is limited. If the external environmental disturbance changes abruptly, the tracking curve will change, and the tracking control amount will also change accordingly. Comparing the simulation results of Figure 11 with Figure 10, we can find that the two are almost the same, so it is believed that the improved cost function can also better realize the function of resisting environmental disturbances and tracking the target path.

6. Summary

This work studies the ship anti-disturbance path-tracking from the following aspects: the status of the problem, the wave, current, and wind disturbance model, the ship motion model, prediction expression, cost function, improved cost function and stability analysis, path-tracking experimental analysis. Further, the simulation results of this paper can enable intelligent ships to achieve control objectives more flexible and maneuverable, and improve ship safety in path-tracking. For more rigorous outcomes, the simulation experiment results herein must be further verified by testing the actual system. For the problem of anti-disturbance path-tracking introduced in this paper, solving it demands a set of mathematical equations and control theories. Nevertheless, these equations and theories are complex, abstract, and time-consuming to understand, which hinders the widespread adoption of this technology. Therefore, it is vital that a simpler and more accessible approach be developed in the future. Moreover, Intelligent vehicle companies and research institutes are paying close attention to this technology, which once matures will help promote the development of Intelligent ships.