Obstacle Avoidance Tracking Control of Underactuated Surface Vehicles Based on Improved MPC

Abstract

This paper addresses the issue of the poor collision avoidance effect of underactuated surface vehicles (USVs) during local path tracking. A virtual ship group control method is suggested by using Freiner coordinates and a model predictive control (MPC) algorithm. We track the planned path using the MPC algorithm according to the known vessel state and build a hierarchical weighted cost function to handle the state of the virtual vessel, to ensure that the vessel avoids obstacles while tracking the path. In addition, the control system incorporates an Extended Kalman Filter (EKF) algorithm to minimize the state estimation error by continuously updating the ship state and providing more accurate state estimation for the system in a timely manner. In order to validate the anti-interference and robustness of the control system, the simulation experiment is carried out with the “Yukun” as the research object by adding the interference of wind and wave of level 6. The outcome shows that the algorithm suggested in this paper can accurately perform the trajectory-tracking task and make collision avoidance decisions under six levels of external interference. Compared with the original MPC algorithm, the improved MPC algorithm reduces the maximum rudder angle output value by 58%, the integral absolute error by 46%, and the root mean square error value by 46%. The improved control algorithm reduces the maximum rudder angle output value by 42% and the maximum rudder angle output value by 10%. The control method provides a new technical choice for trajectory tracking and collision avoidance of USVs in complex marine environments, with a reliable theoretical basis and practical application value.

1. Introduction

With the growing demand for maritime trade, the marine transportation industry has flourished. The underactuated surface vehicle (USV) plays an indispensable role in the transport industry; therefore, the USV control problem has received widespread attention [1,2]. Among the solutions, trajectory-tracking control is an essential research direction in the area of ship control, which not only helps to promote the development of ship intelligence and automation but also reduces ship operating costs and improves ship navigation safety [3].

For the target-tracking problem of underpowered ships, Wang [4] designed a fixed-time predictor to approximate the side slip caused by interference. And the Line-of-Sight method is utilized to design a fixed-time heading controller to ensure the stability of the heading and achieve accurate path tracking. Yang [5] transformed the path-tracking problem into the stabilization problem of longitudinal velocity and yaw angle by the Line-of-Sight method, and they combined a neural network and the sliding method to design a neural sliding mode robust controller, which realized the tracking control of an underactuated ship. Although neural networks have powerful nonlinear approximation capabilities, their training process may require a large amount of data and time, and there may be problems such as overfitting. Su [6] improved the integral Line-of-Sight method by introducing the concept of a vector field into the curved path, achieving the goal of effectively tracking dynamic objects on the water surface for underactuated surface vehicles. However, the Line-of-Sight method cannot converge the controlled object to the desired path and there is steady-state position error. To address this issue, Zhang [7] proposed an adaptive, hyperbolic tangent, Line-of-Sight method control strategy based on the hyperbolic tangent function. The hyperbolic tangent function was used to correct the expected bow curve flatness, further improving the tracking accuracy and convergence performance. However, none of the above studies took into account the issue of external perturbation.

Since underdriven ships have dynamic instability as well as unknown time-varying perturbation problems, references [8,9] redesigned the interference observer to provide an estimated value of the interference when it is unknown. Combined with dynamic inverse control technology, the robust trajectory-tracking control rate is calculated to enhance the anti-interference capability of underactuated ships. However, the above-mentioned research failed to take into account the frequent update of the controller when considering external disturbances, which may lead to severe wear of the steering gear. To address this issue, Song [10] designed an event-triggered adaptive sliding mode controller. By introducing an event-triggered mechanism, the system’s immunity to interference is significantly improved, while the frequency of controller updates is further minimized. However, in this study, the static thresholds T₁ and T₂ in the event-triggering conditions are fixed values, and the dynamic influence of the ship’s navigation state (such as speed and heading change rate) on the thresholds is not considered. This may lead to a decrease in control accuracy or an excessive number of updates under certain working conditions. Zhou [11] proposed a trajectory-tracking control strategy based on deep reinforcement learning. This method develops a controller based on the mixed-priority dual-delay deep deterministic policy gradient (TD3), integrates the mixed-priority experience replay (HPER) mechanism, and further accelerates the convergence of the network. Judging from its simulation results, this method has s strong anti-interference ability in complex marine environments. The above studies are unable to converge the error state to 0 or a nearby field in a finite amount of time, thus making it difficult to meet the needs of ships for precise tracking and control tasks.

In response to the above issues, Wang [12] proposed an event-triggered, robust, adaptive, finite-time, trajectory-tracking control scheme. While improving the control accuracy, this ensures the safe and efficient completion of the tracking task. Li [13] proposed a trajectory-tracking control strategy for a USV with a disturbance compensation observer, which can accurately compensate for disturbances within a limited time. Zeng [14] proposed a new finite-time control law based on the backstepping method and additive power integration method. The controller ensures that the trajectory-tracking error of the surface vessel converges to the neighborhood near the origin within a finite time through continuous feedback. However, in practical applications, if the system model differs significantly from the actual situation, it may affect the control effect.

The above-mentioned research has solved problems such as handling external interference and error convergence, but the load on its controller is relatively large. Shen [15] proposed a minimum-parameter, adaptive, recursive, sliding-mode control strategy based on the time-varying, asymmetric obstacle Lyapunov function to address this issue. Minimum-parameter method neural network approximation of model uncertainty is utilized to reduce the computational complexity. Meanwhile, the command filter is adopted to constrain the amplitude of the input signal, to avoid the problem of differential explosion. Wang [16] suggested a nonlinear, robust, trajectory-tracking control method by combining the inverse step method as well as adaptive techniques. The dynamic surface method is utilized to directly derive the derivatives of the variables, thus effectively reducing the complexity of virtual variable derivation in the traditional algorithm. However, the real-time obstacle avoidance problem was not considered in the research.

As can be seen from

Table 1. Analysis table.

Control Method	Analysis
Traditional Line-of-Sight (LOS)	The tracking accuracy for nonlinear paths is relatively low, and it is difficult to handle scenarios with large curvature turns.
LOS + PID control	The lack of an active compensation mechanism for disturbances leaves the dynamic tracking performance vulnerable to external factors.
Expanded observer + dynamic inverse control	The performance of the observer depends on the accuracy of the model, and its suppression effect on unmodeled dynamics and strong disturbances is limited.
Event-triggered sliding mode control	Sliding mode control has chattering phenomena, which can easily lead to frequent actions of the actuator.
Conventional model predictive control (MPC)	The computational complexity increases exponentially with the growth of the prediction time domain, and the real-time performance is limited.

below, none of the above studies took into account the real-time obstacle avoidance problem of underactuated ships. Based on the above incentives, this paper makes the following contributions:

(1)

The equal-radius circular-arc steering control methodology proposed in this paper avoids the issue of overshoot or undershoot caused by the large turning radius.

(2)

The dynamic virtual ship group methodology suggested in this paper can update the coordinates and bow angles of virtual ships in real time to plan an optimal obstacle avoidance path.

(3)

The EKF algorithm can continuously correct the state estimates to further reduce the estimation error and ensure the control accuracy of the system.

The other parts of this article are organized as follows. Section 2 presents the ship control strategy, including the equal-radius circular-arc steering maneuver, Cartesian coordinates and Freightliner coordinates, and virtual ship group control methods; Section 3 introduces the model predictive control scheme; Section 4 presents simulation experiments to verify the anti-interference ability and obstacle avoidance function of the system; and Section 5 presents the conclusion and outlook.

2. Ship Control Strategy

2.1. Equal-Radius Circular-Arc Steering Maneuver

In the actual navigation process of the ship, the path is planned according to the route points. However, the planned paths are often not smooth enough at the corners, and they are prone to reducing the efficiency of ship navigation and ship performance [17].

Remark 1. 

In this paper, an equal-radius circular-arc steering maneuver method is proposed to prevent overshooting or undershooting caused by excessive turning angles. This method will redesign the navigation points and re-plan an optimal turning radius, thereby enabling the ship to turn according to the arc of equal radius. And the newly designed turning radius satisfies the “Harbor Master Design Specification” [18], as shown in Figure 1.

In Figure 1, the original path is $\overrightarrow{ABC}$, and $\overrightarrow{ADC}$ is the path planned by the equal-radius circular-arc steering maneuver method. When the ship travels to point A, this method will calculate the optimal turning radius. The specific calculation is shown in Equation (1). It is clear from Figure 1 that the planned path $\overrightarrow{ADC}$ sails a shorter distance than the original path $\overrightarrow{ABC}$, further reducing the voyage time. Meanwhile, the ship can change its course along a smoother path, ensuring that the ship maintains a stable trajectory during the turning process and preventing overshoot caused by an excessive radius. However, due to the large inertia and long-time lag of large ships, a smaller radius may result in too sharp a steering for them.

$$\left\{\begin{array}{l}\omega =\left({\theta }_2-{\theta }_1\right)\\ L_1=L_3=sqrt\left({\left(r_0/\sin \omega \right)}^2-{r_0}^2\right)\\ x_0=x_2+r_0\cos \left({\displaystyle \frac{{\theta }_1+{\theta }_2}{2}}\right)/\sin \omega +r_0\cos {\theta }_4\\ y_0=y_2+r_0\sin \left({\displaystyle \frac{{\theta }_1+{\theta }_2}{2}}\right)/\sin \omega +r_0\sin {\theta }_4\\ \omega \in \left({\theta }_3,{\theta }_5\right)\end{array}\right.$$

(1)

In the formula, ${\theta }_1$ represents the initial turning angle when the ship turns from the original path $\overrightarrow{AB}$ to the circular arc path $\overrightarrow{ADC}$; ${\theta }_2$ is the angle between the ship and the arc path during the turning process; ${\theta }_3$ is the angle between the ship and the arc path during the turning process, which is used to adjust the ship’s heading; ${\theta }_4$ is the heading angle of the ship when it approaches the end point $D$ of the circular arc path; and ${\theta }_5$ is the final angle between the vessel and the target path $\overrightarrow{DC}$ after the steering is completed. Equation (2) is the process of calculating the fixed curvature of the ship when it turns, and only three parameters need to be satisfied when planning the path: the turning radius $R$ and the two azimuth angles ${\theta }_1, {\theta }_2$. The positional coordinates of the waypoint $D$ can be solved by calculating the angular value of ${\theta }_4$. To avoid increasing the computational effort, ${\theta }_4$ is taken in the range $\left(-\pi , \pi \right)$. Based on the positional coordinates of the three waypoints of $A, D, C$, the positions of the straight lines and arcs can be calculated, and then the positions of ${\theta }_4$ can be moved sequentially until the entire route is covered.

2.2. Coordinate Conversion

The Freightliner coordinate system is a coordinate system based on the movement characteristics of ships. It can describe the movement of ships more naturally, including heading and position information, etc. Compared with Cartesian coordinates, this coordinate system can reflect the actual motion state of ships more intuitively in the field of ship control and navigation. Therefore, in order to more intuitively reflect the traveling distance and transverse offset of the ship when it is actually sailing, this paper transforms the ship’s Cartesian coordinates into Freightliner coordinates, and it completes path planning under the Freightliner coordinates. Then, the state information under the corresponding Cartesian coordinate system is obtained according to the displacement, transverse distance, and its derivative information. As shown in Figure 2, the centerline of the channel is set as the $S$ axis in the Freightliner coordinate system to describe the ship displacement on the planned route, and the normal vector is set as the $l$ axis to describe the ship yaw [19].

Here, the $l$ axis travels along the route with an increasing $S$ axis and is perpendicular to the reference curve at all times. Equation (2) shows the mapping relationship between the Cartesian coordinate system and the Freightliner coordinates, where ${\left(\cdot \right)}_x$ stands for variables in Cartesian coordinates; ${\left(\cdot \right)}_r$ stands for variables in Freightliner coordinates; and $k_r$ represents the curvature of its trajectory. Equation (3) is the inverse transformation between the two coordinate systems.

$$\left\{\begin{array}{l}S=S_r\\ S^{\prime }={\displaystyle \frac{v_x\cos \left({\theta }_x-{\theta }_r\right)}{1-k_{r}l}}\\ l=sign\left(\left(y_x-y_r\right)\cos \left({\theta }_r\right)-\left(x_x-x_r\right)\sin \left({\theta }_r\right) · \sqrt{{\left(x_x-x_r\right)}^2+{\left(y_x-y_r\right)}^2}\right)\\ l^{\prime }=\left(1-k_{r}l\right)\tan \left({\theta }_x-{\theta }_r\right)\end{array}\right.$$

(2)

$$\left\{\begin{array}{l}{x}_x=x_r-l\sin \left({\theta }_r\right)\\ y_x=y_r+l\cos \left({\theta }_r\right)\\ {\theta }_x=a\tan \left({\displaystyle \frac{l^{\prime }}{1-k_{r}r}}\right)+{\theta }_r \in \left[-\pi ,\pi \right]\\ v_x=\sqrt{{\left[S^{\prime }\left(1-k_{r}l\right)\right]}^2+{\left({\left(Sl\right)}^{\prime }\right)}^2}\end{array}\right.$$

(3)

2.3. Virtual Ship Group Control Method

In this section, a virtual ship group control method is proposed to further plan the forward distance as well as the lateral offset of the ship. The virtual ship group is mainly composed of the status of actual ships and the position information of the planned virtual ships. Figure 3 shows the schematic diagram of a dynamic virtual ship group.

This method considers the longitudinal and transverse movements relative to time, respectively, based on the Freiner coordinates, and it calculates the position information of the virtual ship according to the state of the actual ship, then plans the path to be tracked. As shown in Equation (4), the path of the planned virtual ship is $l$, and it is described using a cubic polynomial.

$$l=b_1+b_{2}S+b_3{S}^2+b_4{S}^3$$

(4)

By substituting the initial state of the actual ship’s position coordinates and the final position information of the dynamic virtual ship, the following relationship can be obtained:

$$\left[\begin{array}{l}{l}_0\\ {l^{\prime }}_0\\ l_1\\ {l^{\prime }}_1\end{array}\right]=\left[\begin{array}{llll}1& S_0& {S_0}^2& {S_0}^3\\ 0& 1& 2S_0& 3{S_0}^2\\ 1& S_1& {S_1}^2& {S_1}^3\\ 0& 1& 2S_1& 3{S_1}^2\end{array}\right]·\left[\begin{array}{l}{b}_0\\ b_1\\ b_2\\ b_3\end{array}\right]$$

(5)

where $S_k=S_0+k·dS$, $k$ is the number of execution steps, and $\left(S_0, l_0\right)$ and $\left(S_1, l_1\right)$ represent the Freightliner coordinates of the virtual ship at the beginning and end, respectively. Let ${\left[l_0 {l^{\prime }}_0 l_1 {l^{\prime }}_1\right]}^T=G$, $\left[b_0 b_1 b_2 b_3\right]=K$, the intermediate arithmetic process matrix be replaced by $L$, and Equation (5) be simplified as

$$K = L^{-1}·G$$

(6)

where $L^{-1}$ is a pseudo-inverse operation used to prevent singular value problems resulting from special primitive end states. $K$ stands for the coefficient matrix of the polynomial, in which the real ship state is a known condition and the state of the dynamic virtual ship is selected through a continuous optimization process.

In order for the optimized dynamic virtual ship to have the functions of collision avoidance, elimination of transverse offset, and smooth trajectory processing at the same time, and for the collision avoidance function to be prioritized over the other two objectives, this study chooses the dynamic hierarchical weighted function to process the virtual ship state, as shown in Equation (7), where ${\omega }_1>{\omega }_2>{\omega }_3$.

$$J=\zeta {\omega }_1{J}_{obs}+{\omega }_2{J}_l+{\omega }_3{J}_{lS}$$

(7)

The remaining parameters in the above equation are as follows:

$$\left\{\begin{array}{l}{J}_{obs}=\max \left\{\cos \left({\theta }_0\pm {\theta }_p-{\theta }_k\right)\right\}\\ J_l=\left|l_k\right|/\left|l_{\max }-l_s\right|\\ J_K=k_r\\ \zeta =\left(S_{cap}<S_0-S_S<S_{rls}\right)\wedge \left(‖\left(S_0,l_0\right)-\left(S_S,l_S\right)‖<d_{cap}\right)\end{array}\right.$$

(8)

$J_{obs}$, $J_l$, and $J_K$ are regularized loss variables that impose penalties on heading, yaw, and course curvature toward the obstacle, respectively; $\xi$ is a Boolean variable that determines whether to capture obstacle information based on relative distance; $S_{cap}$ stands for capture advance, which refers to the threshold distance at which a ship triggers obstacle avoidance decision preparation when approaching an obstacle, with a set value of 500 m; $S_{rls}$ stands for release advance, which refers to the threshold distance at which a vessel exits the obstacle avoidance state after avoiding an obstacle and moving away from it (the set value is 100 m); ${\theta }_k$ stands for the combined velocity planning direction relative to the obstacle; and ${\theta }_p$ is the adjustment angle that keeps the ship away from obstacles. The system is configured with a safety distance of 150 m and an obstacle detection range of 500 m, ensuring an adequate margin for obstacle avoidance. The significance of the relevant parameters is shown in the figure below, where the obstacle velocity is zero.

The blue circle in Figure 4 represents obstacles, and ${\theta }_k$ is the planning direction of the synthesized velocity. When the ship travels to the $b$ position, the obstacle enters into the capture distance. The system will generate five path branches based on the obstacles, and then we select the path with the lowest cost as the optimal obstacle avoidance path based on the established performance metrics.

The entire control process revolves around the trajectory tracking and collision avoidance of a ship. Initially, the actual position of the ship is acquired and subjected to coordinate transformation, providing a unified reference for subsequent computations. Based on the position information, multiple candidate navigation trajectories are generated, and a closed loop for obstacle collision-avoidance judgment is introduced simultaneously. If a trajectory is deemed unsafe, it will be regenerated to give priority to collision avoidance and ensure the basic safety of navigation. Subsequently, from the perspectives of navigation efficiency, control cost, and path-tracking accuracy, a cost assessment is conducted on the safe trajectories. Ultimately, in accordance with the assessment results, the trajectory with the optimal comprehensive performance is selected as the navigation instruction for the actual execution of the ship, enabling the ship to navigate safely and efficiently in complex environments. The entire control process is illustrated in Figure 5.

3. Model Predictive Control Scheme

The Nomoto model is applicable to the approximate description of the motion characteristics of ships and can be used for the dynamic response simulation of path-tracking control [20]. Therefore, this paper selects the Nomoto ship motion model for further research. The model is constructed as shown in Equation (9):

$$G\left(s\right)={\displaystyle \frac{\phi \left(s\right)}{\delta \left(s\right)}}={\displaystyle \frac{K_0}{s\left(T_{0}s+1\right)}}$$

(9)

where $G\left(s\right)$ stands for the transfer function mathematical model, and $\phi , \delta$ represents the ship range and rudder angle. MPC is a discrete model-based control method, and to reduce the computational effort of subsequent constraintization, Equation (9) needs to be discretized into a differential, integrated, moving-average autoregressive (CARIMA) model, which then captures most of the properties of the transfer function model. This paper takes the “Yukun” ship as the research object, and it discretizes its Nomoto mathematical model to obtain the CARIMA mathematical model as follows:

$$b\left(z\right){\phi }_k=c\left(z\right){\delta }_k+{\displaystyle \frac{T_f\left(z\right)}{\Delta \left(z\right)}}{\xi }_k$$

(10)

in which $b\left(z\right)=b_1{z}^{-1}+\cdots +b_m{z}^{-m}$, $c\left(z\right)=c_1{z}^{-1}+\cdots +c_m{z}^{-m}$, $\phi \left(k\right)$ is the bow output of the $k$ step, ${\delta }_k$ is the rudder angle of the $k$ step, ${\xi }_k$ denotes the unknown zero-mean value of the environmental interference and measurement noise, $\Delta \left(z\right)=1-z^{-1}$ stands for the difference operator, and $T_f\left(z\right)$ is the filter, which is the mechanism for adjusting the sensitivity of the loop about ${\xi }_k$. Its specific value is $T_f\left(z\right)=\left[1, -0.2\right]$.

If the noise and interference are well suppressed by the compensation mechanism, let $A\left(z\right)=a\left(z\right) \cdot \Delta \left(z\right)$, expanding $\left[a\left(z\right)\cdot \Delta \left(z\right)\right] \phi \left(z\right)=b\left(z\right)\cdot \Delta \delta \left(z\right)$ (as an example of expanding by 4 steps), which yields

$$\left\{\begin{array}{l}{\phi }_{k+1}+A_1{\phi }_k+\cdots +A_n{\phi }_{k-n+1}=b_1\Delta {\delta }_k+b_2\Delta {\delta }_{k-1}+\cdots +b_m\Delta {\delta }_{k-m+1}\\ {\phi }_{k+2}+A_1{\phi }_{k+1}+\dots +A_n{\phi }_{k-n+2}=b_1\Delta {\delta }_{k+1}+b_2\Delta {\delta }_k+\cdots +b_m\Delta {\delta }_{k-m+2}\\ {\phi }_{k+3}+A_1{\phi }_{k+2}+\cdots +A_n{\phi }_{k-n+3}=b_1\Delta {\delta }_{k+2}+b_2\Delta {\delta }_{k+1}+\cdots +b_m\Delta {\delta }_{k-m+3}\\ {\phi }_{k+4}+A_1{\phi }_{k+3}+\cdots +A_n{\phi }_{k-n+4}=b_1\Delta {\delta }_{k+3}+b_2\Delta {\delta }_{k+2}+\cdots +b_m\Delta {\delta }_{k-m+4}\\ ⋮\end{array}\right.$$

(11)

The above equation can be organized as

$$C_A{\underrightarrow{\phi }}_{k+1}=\left[C_b{\underrightarrow{\Delta \delta }}_k+H_b{\underleftarrow{\Delta \delta }}_{k-1}-H_A{\underleftarrow{\phi }}_k\right]$$

(12)

in which $\underrightarrow{\left(\cdot \right)}$ and $\underleftarrow{\left(\cdot \right)}$ represent the predicted time-domain variables and past variables, respectively, $C_A$ and $C_B$ represent the $A\left(z\right)$ and $b\left(z\right)$ Toplitz matrices, respectively, and $H_A$ and $H_B$ are the Hankel matrices for $A\left(z\right)$ and $b\left(z\right)$, respectively.

$$\begin{array}{l}\left\{\begin{array}{l}{C}_A=\left[\begin{array}{llll}1& 0& 0& 0\\ A_1& 1& 0& 0\\ A_2& A_1& 1& 0\\ A_3& A_2& A_1& 1\end{array}\right], C_B=\left[\begin{array}{llll}{b}_1& 0& 0& 0\\ b_2& b_1& 0& 0\\ b_3& b_2& b_1& 0\\ b_4& b_3& b_2& b_1\end{array}\right]\\ H_A=\left[\begin{array}{llllllll}{A}_1& A_2& \dots & A_{n-4}& A_{n-3}& \dots & A_{n-1}& A_n\\ A_2& A_3& \dots & A_{n-3}& A_{n-2}& \dots & A_n& 0\\ A_3& A_4& \dots & A_{n-2}& A_{n-1}& \dots 0& 0\\ A_4& A_5& \dots & A_{n-1}& A_{n-3}& \dots & 0& 0\end{array}\right]\\ H_b=\left[\begin{array}{llllllll}{b}_2& b_3& \dots & b_{m-4}& b_{m-3}& \dots & b_{m-1}& b_m\\ b_3& b_4& \dots & b_{m-3}& b_{m-2}& \dots & b_m& 0\\ b_4& b_5& \dots & b_{m-2}& b_{m-1}& \dots & 0& 0\\ b_5& b_6& \dots & b_{m-1}& b_m& \dots & 0& 0\end{array}\right]\end{array}\right.\end{array}$$

(13)

To further enhance the control accuracy of the system, the Extended Kalman Filter (EKF) algorithm is added to the control system. EKF takes the current state estimation of the MPC algorithm as the control input. By constantly updating the state estimation, it predicts the state at the next moment, thereby estimating the state of the ship more accurately. Equation (14) shows the state prediction step of the EKF algorithm at moment $k$:

$$\left\{\begin{array}{l}{x}_{k|k-1}=f\left(x_{k-1} ,u_{k-1}\right)\\ P_{k|k-1}=A_k{P}_{k-1}{A}_k^{\mathrm{T}}+Q_k\end{array}\right.$$

(14)

in which $x_{k|k-1}$ stands for the state prediction value at the moment $k$; $A_k$ is the state transfer Jacobi matrix; $P_{k|k-1}$ is the prediction covariance matrix; and $Q_k$ is the process noise covariance matrix. Among them, the $Q_k$ matrix is shown as Equation (15).

$$Q_k=\left[\begin{array}{lllll}{q}_x& 0& 0& 0& 0\\ 0& q_y& 0& 0& 0\\ 0& 0q_v& 0& 0\\ 0& 0& 0& q_{\theta }& 0\\ 0& 0& 0& 0& q_r\end{array}\right]$$

(15)

where $q_x$, $q_y$, $q_v$, $q_{\theta }$, and $q_r$ denote the process noise variance of position $x$, position $y$, velocity $v$, heading angle $\theta$, and yaw rate $r$, respectively.

The EKF algorithm performs a state update step after predicting the state in the next moment, taking into account the actual measured data as well as the predicted state. Equation (16) shows the state update step of the EKF algorithm at moment $k$:

$$\left\{\begin{array}{l}{y}_k=z_k-h\left(x_{k|k-1}\right)\\ S_k=H_k{P}_{k|k-1}{H}_k^{\mathrm{T}}+R_k\\ K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}\\ x_k=x_{k|k-1}+K_k{y}_k\\ P_k=\left(I-K_k{H}_k\right)P_{k|k-1}\end{array}\right.$$

(16)

in which $y_k$ stands for the measurement residual; $H_k$ stands for the measurement Jacobi matrix; $S_k$ stands for the measurement covariance matrix; $K_k$ stands for the Kalman gain; and $R_k$ stands for the measurement noise covariance matrix, as shown in Equation (17).

$$R_k=\left[\begin{array}{lllll}{r}_x& 0& 0& 0& 0\\ 0& r_y& 0& 0& 0\\ 0& 0& r_v& 0& 0\\ 0& 0& 0& r_{\theta }& 0\\ 0& 0& 0& 0& r_r\end{array}\right]$$

(17)

where $r_x$, $r_y$, $r_v$, $r_{\theta }$, and $r_r$ denote the measurement noise variance of position $x$, position $y$, velocity $v$, heading angle $\theta$, and yaw rate $r$, respectively.

The EKF algorithm feeds the computed state estimates back into the MPC algorithm again to further update the state variables of the MPC algorithm. This step ensures that the state variables of the MPC are always the estimated values corrected by the EKF, thus enhancing the accuracy and robustness of the system. Equation (18) represents the latest bow output at the moment of $k+1$, where $E=C_A^{-1}{C}_b; F=C_A^{-1}{H}_b;$ and $G=C_A^{-1}{H}_A$.

$${\underrightarrow{\phi }}_{k+1}=\left[E\underrightarrow{\Delta {\delta }_k}+F\underleftarrow{\Delta {\delta }_{k-1}}-G\underleftarrow{{\phi }_k}\right]$$

(18)

In the control system, the state error is the error between the current state and the expected state, which is utilized to measure the tracking performance of the system. Its specific calculation is shown in Equation (19), where ${\phi }_{ref, k+1}$ represents the expected output of the system at the $k+1$ moment, and ${\phi }_{k+1}$ represents the predicted output at the $k$ moment.

$$e_{k+1}={\phi }_{ref, k+1}-{\phi }_{k+1}$$

(19)

Establish the following quadratic indicators:

$$\begin{array}{l}J=\underrightarrow{e_{k+1}^{\mathrm{T}}}\underrightarrow{e_{k+1}}+\lambda \underrightarrow{\Delta {\delta }_k^{\mathrm{T}}}\\ =\left(\underrightarrow{{\phi }_{ref, k+1}^{\mathrm{T}}}-\underrightarrow{{\phi }_{k+1}^{\mathrm{T}}}\right)\left(\underrightarrow{{\phi }_{ref, k+1}}-\underrightarrow{{\phi }_{k+1}}\right)+\lambda \underrightarrow{\Delta {\delta }_k^{\mathrm{T}}}\underrightarrow{\Delta {\delta }_k}\end{array}$$

(20)

where $\Delta {\delta }_k$ is the rudder angle change at the $k$ step. Among them, $\lambda$ is a regularization parameter used to balance the state error and control the energy of the input. When substituting ${\underrightarrow{\phi }}_{k+1}$ in Equation (18), the following can be obtained:

$$\begin{array}{l}J=\left[\left[\underrightarrow{{\phi }_{ref, k+1}^{\mathrm{T}}}-{\left(F\underleftarrow{\Delta {\delta }_{k-1}}\right)}^{\mathrm{T}}-{\left(G\underleftarrow{{\phi }_k}\right)}^{\mathrm{T}}\right]\left[\underrightarrow{{\phi }_{ref, k+1}}-F\underleftarrow{\Delta {\delta }_{k-1}}-G\underleftarrow{{\phi }_k}\right]+\left[{\left(E\underrightarrow{\Delta {\delta }_k}\right)}^{\mathrm{T}}\right]\left[E\underrightarrow{\Delta {\delta }_k}\right]\right.\\ \left.-\left[2{\left(E\underrightarrow{\Delta {\delta }_k}\right)}^{\mathrm{T}}\right]\left[\underrightarrow{{\phi }_{ref, k+1}}-F\underleftarrow{\Delta {\delta }_{k-1}}+G\underleftarrow{{\phi }_k}\right]+\lambda \underrightarrow{\Delta {\delta }_k^{\mathrm{T}}}\underrightarrow{\Delta {\delta }_k}\right]\end{array}$$

(21)

By analyzing the above equation, it can be concluded that only the terms related to $\underrightarrow{\Delta {\delta }_k}$ are operable in the control. Therefore, the optimization problem can be established as

$$\left\{\begin{array}{l}f\left(\underrightarrow{\Delta {\delta }_k}\right)=\underrightarrow{\Delta {\delta }_k^T}\left(H^{T}H+\lambda I\right)\underrightarrow{\Delta {\delta }_k}-\left(\underrightarrow{\Delta {\delta }_k^T}\right)2H^T\left[\underrightarrow{R_{k+1}}-F\underleftarrow{\Delta {\delta }_{k-1}}-G\underleftarrow{{\phi }_k}\right]\\ \underrightarrow{\Delta {\delta }_k}=\arg \min f\left(\underrightarrow{\Delta {\delta }_k}\right)\\ s.t. M\underrightarrow{\Delta \delta }\le d_z\end{array}\right.$$

(22)

in which $M$ stands for the selection matrix of the predictor variables, and $d_z$ is the constant matrix containing the upper- and lower-bound constraints $\overline{\Delta \delta }$ and $\underset{¯}{\Delta \delta }$. The above equation is a quadratic programming issue, and the result can be addressed based on the interior point method encapsulated in MATLAB R2023b.

Assume that there exists a feasible optimal control sequence $\left\{{\delta }_0^{\ast },{\delta }_1^{\ast },{\delta }_2^{\ast },\cdots ,{\delta }_{n-2}^{\ast },0\right\}$ in the initial state $\phi \left(0\right)$, corresponding to the state response sequence $\left\{\phi \left(0\right),{\phi }_1,{\phi }_2,\cdots ,{\phi }_n\right\}$. Therefore, the corresponding control term of this control sequence $\left\{{\delta }_0^{\ast },{\delta }_1^{\ast },{\delta }_2^{\ast },\cdots ,{\delta }_{n-2}^{\ast },0\right\}$ continues to be used at $\phi \left(1\right)$, resulting in $e_n=R_n-{\phi }_n=0$. The optimal performance indicators at the initial moment and the optimal performance indicators at the next moment are shown in Equation (23).

$$\left\{\begin{array}{l}{J}^{\ast }\left(0\right)=\underset{{\delta }_0}{\min }{\displaystyle \sum _{i=0}^{n-1}\left(e_i^2+{\lambda }_1\Delta {\delta }_i^2\right)}\\ J^{\ast }\left(1\right)=\underset{{\delta }_0}{\min }{\displaystyle \sum _{i=1}^n\left(e_i^2+{\lambda }_1\Delta {\delta }_i^2\right)}={\displaystyle \sum _{i=0}^{n-1}\left[\left(e_i^2+{\lambda }_1\Delta {\delta }_i^2\right)\right]}-\left(e_0^2+{\lambda }_1\Delta {\delta }_i^{\ast 2}\right)+\left(e_n^2+{\lambda }_1\Delta {\delta }_n^2\right)\end{array}\right.$$

(23)

By comparing these two performance indicators, it can be obtained that

$$J^{\ast }\left(1\right)-J^{\ast }\left(0\right)=-\left(e_0^2+{\lambda }_1\Delta {\delta }_0^{\ast 2}\right)+\left(e_n^2+{\lambda }_1\Delta {\delta }_0^2\right)<0.$$

(24)

From the above formula, it can be seen that the performance indicators of the system monotonically decrease, thereby proving the stability of the system.

Summary of this section: In the actual navigation process of a ship, its real-state information is input into the Extended Kalman Filter (EKF) module. Through state prediction and update steps, environmental disturbances and measurement noises are filtered out, thereby obtaining optimized state estimates. The state estimates corrected by the EKF are used as inputs to the model predictive control (MPC). Combined with the discretized CARIMA ship model, a sequence of future states is generated within the prediction horizon. Subsequently, MPC solves the optimal control sequence based on state errors and a quadratic cost function, and it outputs the first-step rudder angle command to act on the ship. The new state of the ship after executing the control command is fed back to the EKF again, forming a closed-loop data flow of “measurement—filtering—prediction—control”, which ultimately achieves high-precision trajectory tracking and improved robustness. This data flow architecture, through the dynamic coupling of EKF and MPC, not only ensures the accuracy of state estimation but also enhances the adaptability of control decisions to model errors and external disturbances.

Remark 2. 

Incorporating the EKF algorithm into the MPC algorithm can realize the function of fast real-time updating of the ship state estimation, which enables the system to respond to the changes in the ship state in a timely manner, and it ensures the maneuverability of the ship as well as the safety of navigation in the complex environment.

4. Experimental Simulation

To verify the anti-interference of the control method proposed, level 6 wind and wave disturbances are included in the simulation experiments. Figure 6 and Figure 7 are the simulation result diagrams of the wind field and wave field introduced in this experiment [21]. The direction of the blue arrow in Figure 6 indicates the flow direction of the wind. The parameters of this wind farm are set as follows: average wind speed 10 m/s, wind direction angle 30°. The wave field parameters are as follows: The effective wave height is approximately 2.5 m, the wave period is between 6 and 10 s, and the wave direction is random. The wave spectrum adopts the Pierson–Moskowitz model, in which the peak frequency of the spectrum is 0.1 Hz and the spectrum width is 0.05 Hz.

This experiment takes the “Yukun” ship as the research object to carry out simulation experiments. The specific parameters of the model are shown in the following

Table 2. Hydrodynamic parameters of the “Yukun” vessel.

Parameter	Value	Parameter	Value
$X_{\mathit{uu}}^{\prime }$	3.56556	$N_{\mathit{v}}^{\prime }$	−0.0999
$X_{\mathit{vv}}^{\prime }$	−0.0697	$N_{\mathit{r}}^{\prime }$	−0.0455
$X_{\mathit{vr}}^{\prime }$	−0.0481	$N_{\mathit{rr}}^{\prime }$	−0.0339
$X_{\mathit{rr}}^{\prime }$	0.0292	$N_{\mathit{vrr}}^{\prime }$	−0.0561
$Y_{\mathit{v}}^{\prime }$	−0.3032	$N_{\mathit{vvr}}^{\prime }$	−0.1822
$Y_{\mathit{r}}^{\prime }$	0.0832	$N_{\mathit{\phi }}^{\prime }$	−0.0076
$Y_{\mathit{vv}}^{\prime }$	−0.7834	$N_{\mathit{v\phi }}^{\prime }$	−0.1718
$Y_{\mathit{rr}}^{\prime }$	−0.0543	$N_{\mathit{r\phi }}^{\prime }$	0.01073
$Y_{\mathit{vr}}^{\prime }$	−0.2099

[22].

This paper adopts the Legion R7000 ARP8 device for simulation experiments. The processor of this device is “AMD Ryzen 7 7735H with Radeon Graphics 3.20 GHz”. The simulation software used is MATLAB R2023b.

In this experiment, a reference path is generated using the cubic spiral curve function, and static obstacles as well as dynamic obstacles are added to the path to further validate the algorithm’s obstacle avoidance function in complex environments. The obstacle avoidance effect is shown in Figure 8. The star points represent the obstacles, and the star points with moving trajectories represent the dynamic obstacles. The position information of the four obstacles a, b, c, and d in sequence is [810, 2516, 0, 0], [−700, 3800, 0.75, 120 * pi/180], [−2500, 4000, 0.4300 * pi/180], and [−2163, 2000, 0, 0]. The first two elements represent the initial horizontal and vertical coordinates of the obstacle, and the last two elements represent the speed and direction of the obstacle. The blue line represents the original planned route, the red represents the tracking trajectory, and the dashed lines represent the planned path branches. This experiment was set up with a ship speed of 13 knots and a sampling rate of 1 Hz.

The system causes the ship to perform approach behavior and perform tracking functions based on a reference path. While traveling, when the system detects an obstacle entering the capture distance, it can pre-generate five path branches based on the location of the obstacle. And a cost function is calculated for each obstacle with respect to the real-time position of the ship, which is added to the total planning cost. Subsequently, the system selects one of the paths with the smallest total cost as the optimal obstacle avoidance path. And so on until the ship reaches the final position of the target reference path.

From the above figure, it can be seen that the ship model can still safely avoid obstacles and travel normally under the interference of 6 level wind and waves. And it can execute a trajectory-tracking decision quickly after successful obstacle avoidance, close to the original reference path.

As shown in Figure 9, the rudder angle output of the algorithm proposed in this paper is plotted when performing trajectory tracking as well as obstacle avoidance tasks, where the red line represents the rudder angle output value of the original MPC algorithm, the green line represents the rudder angle output value of the nonlinear feedback control algorithm, pink represents the rudder angle output graph of the Line of-Sight method, and the blue line represents the rudder angle output value of the improved MPC. It can be clearly seen from the above figure that the improved model predictive control algorithm, compared with the other two algorithms, not only accurately completes the track tracking and obstacle avoidance tasks but also further reduces the motion amplitude of the ship model, avoiding problems such as energy loss and mechanical wear caused by an excessive output rudder angle.

Table 3. Comparison of performance in class 6 sea state.

	Maximum Rudder Angle Output Value/°	IAE	RMSE
Improved MPC	15	161	4.90
Nonlinear feedback algorithm	36	245	7.63
Improvement rate compared to improved MPC	58%	34%	36%
Original MPC	26	297	9.11
Improvement rate compared to improved MPC	58%	46%	46%
Line of Sight	22	294	9.45
Improvement rate compared to improved MPC	32%	45%	48%

shows a comparison of rudder angle outputs between the original MPC algorithm and the improved MPC algorithm under sea conditions of level 6. Compared with the nonlinear feedback control algorithm, the improved MPC algorithm reduces the maximum rudder angle output value by 58%, the integral absolute error by 34%, and the root mean square error value by 36%. Compared with the Line-of-Sight (LOS) algorithm, the improved MPC algorithm still has a better control performance. The maximum rudder angle output value was reduced by 32%, the integral absolute error was reduced by 45%, and the root mean square error was reduced by 48%. Compared with the original MPC algorithm, the improved MPC algorithm reduces the maximum rudder angle output value by 58%, the integral absolute error by 46%, and the root mean square error value by 46%. Thus, the algorithm suggested in this paper can ensure that the ship accurately performs the trajectory tracking and obstacle avoidance tasks under adverse sea conditions, as well as further reduce mechanical wear and tear.

To further analyze the tracking performance of the algorithm proposed in this paper and the nonlinear feedback control algorithm, Figure 9 presents the convergence curves of position errors under the two control laws. The calculation of the position tracking error is shown in Equation (25).

$$d=\sqrt{{\left(x_{act}-x_{ref}\right)}^2+{\left(y_{act}-y_{ref}\right)}^2}$$

(25)

As can be seen from Figure 10, compared with the nonlinear control algorithm and the LOS algorithm, the position error under the improved MPC control law is smaller and can effectively track the reference path. Meanwhile, the improved MPC control law has a relatively fast convergence speed, which can respond promptly and adjust the control input to reduce offset.

To ensure the statistical reliability of the experimental results, this paper conducted 10 independent repeated tests for the same scenario (including sea conditions of level 6). The final result shows a high degree of consistency. Therefore, the proposed algorithm has a stable performance in complex sea conditions and the results are reproducible.

This subsection verifies the validity of the proposed method through the above simulation experiments, while all the verifications are carried out in the simulation environment. In the future, an experimental platform will be set up to test the hardware and validate the proposed method in real scenarios using the Yukun ship as the experimental object.

5. Conclusions

To solve the overshoot problem caused by an excessive turning radius when ships turn, an equal-radius-arc turning maneuver method is proposed. Based on the model predictive control algorithm, a virtual ship group control method was proposed, and a hierarchical weighting function was established to process the state information of virtual ships, thereby effectively implementing collision avoidance decisions. Meanwhile, the Extended Kalman Filtering algorithm is introduced. By continuously performing the prediction and update steps, the control accuracy of the system is further improved. It can be seen from the simulation results that the method proposed in this paper can still accurately perform the tracking task and safely avoid obstacles under the interference of wind waves of level 6, and it has a strong anti-interference ability and obstacle avoidance function. While ensuring safe operation, we must further reduce the rudder angle output value to avoid excessive energy consumption and mechanical wear problems.

Subsequently, the author will integrate real sensors to verify the performance of complex, dynamic-obstacle recognition and obstacle avoidance, and conduct real-ship experiments on the “Yukun” vessel to verify the stability of the steering control method and the real-time performance of the control strategy, as well as the adaptability of the extended algorithm in high-flow waters.