Autonomous Trajectory Tracking Integrated Control of Unmanned Surface Vessel

Abstract

Trajectory tracking control of unmanned surface vessels (USVs) has become a popular topic. Regarding the problem of ship collision avoidance encountered in trajectory tracking, more attention needs to be paid to the algorithm application, namely the characteristics of flexibility and accessibility. Thus, a fusion framework of field theoretical planning and a model predictive control (MPC) algorithm is proposed in this paper to obtain a realizable collision-free tracking trajectory, where the trajectory smoothness and collision avoidance constraints under a complex environment need to be considered. Through the designed fast matching (FM) method based on the electric field model, the algorithm gains the direction trend of collision avoidance planning and then combines it with a flexible distance to reconstruct the architecture of the MPC and constraint system, generating the optimal trajectory tracking controller. The new algorithm was tested and validated for several situations, and it can potentially be developed to advance collision-free trajectory tracking navigation in multivessel situations.

1. Introduction

The tracking control of an unmanned surface vessel (USV) is the key factor in determining the effect of autonomous navigation, mainly measured by the trajectory smoothness, angle realizability, attitude fluctuation, tracking time, etc., affecting the complexity and accessibility of the controller. In addition, in an actual sea area, it is necessary to consider collision avoidance regarding surrounding ships and to dynamically adjust the trajectory to adapt to a complex environment of multiple vessels. Therefore, the control of a USV needs to achieve trajectory tracking and collision avoidance [1,2].

In the existing literature, it is customary to divide collision avoidance trajectory planning and tracking control into two parts that take global or local path planning as tracking parameters combined with an appropriate control algorithm to design a control rate to realize trajectory tracking and navigation. Therefore, classical trajectory planning algorithms based on geometric models [3], potential field models [4], velocity obstacle models [5], environmental dynamic interaction models [6], and classical algorithms for control, such as adaptive control [7], model predictive control (MPC) [8], sliding mode control [9], anti-disturbance control [10], and intelligent heuristic control [11], can be used in conjunction with each other according to different requirements. For example, Shen et al. [12] formulated the path planning problem into a receding horizon optimization framework with spline path templates to obtain the local optimal path and then used a nonlinear MPC to achieve the tracking goal. Tor et al. [13] planned route compliance with the Convention on the International Regulations for Preventing Collisions at Sea (COLREGs) and hazard evaluation criterion on a finite horizon to select the optimal behavior for an MPC. Namgung [14] further analyzed various encounter scenarios in COLREGs to put forward a local route planning method using a fuzzy inference system based on near collision, ship domain, and velocity obstacle models. Zheng et al. [15] used the Kalman filter and collision risk index to predict a ship’s trajectory and then sought the steering angle via an improved cultural particle swarm optimization algorithm. Huang et al. [16] constructed a framework of a human–machine interaction-oriented collision avoidance system to gain the decision-making trajectory, cooperating with a PD controller to complete automatic navigation. Husain et al. [17] discussed cooperative path-planning and tracking controller for autonomous vehicles using a distributed MPC approach. Xu et al. [18] considered the direction of resultant velocity under a Frenet frame to solve the problem about static or slow time-varying obstacles and used an MPC to track the planned motion.

The above distribution scheme of planning and control is easier to use and has a large scale for algorithm transplantation and improvement. Nevertheless, the weak coupling and correlation between control parameters and planning states, especially for real-time collision avoidance scenarios, increase the target complexity and reduce the control accessibility. Therefore, some research literature on the fusion scheme of planning and control has been published accordingly. Miele et al. [19] discussed the strategy for the ship collision avoidance of optimal control that maximizes the timewise minimum distance based on the multiple-subarc sequential gradient-restoration algorithm with respect to the controls. Tomas et al. [20] proposed a decentralized collision avoidance system that directly mixes the control problem for multi-UAV scenarios. Zhou et al. [21] listed four steps of the collision avoidance process and determined the optimal maneuvers of each step to obtain a suitable control state. All of the above are basically planning designs integrated in the control algorithm, which are also referred to in this paper, but they ignore the practicality of the control algorithm.

To realize the application of the algorithm, it needs to be based on a control algorithm suitable for the integration control of the fusion framework. It is well known that MPC specializes in solving engineering control problems and has excellent iterative, optimization, and prediction performance, also known as receding horizon control [22,23]. Therefore, the MPC architecture is considered for the tracking control of USVs. Currently, there is a single type of research on MPC collision avoidance, mostly designed to improve the cost function [24] or to add fixed distance constraints [25] to obstacle avoidance. To optimize the collision avoidance constraints, Mohamed [26] employed the COLREGs to give the constraint restrictions in different modes of USVs for circular ship domains and elliptical ship domains, which are more coherent with actual ship handling specifications. Inger [27] argued for the hazard evaluation criterion of collision avoidance on the basis of the COLREG constraints. In addition, Eriksen et al. [28] introduced the pseudo-Huber cost function into the MPC method, and Julien et al. [29] designed a trade-off cost function incorporating obstacle avoidance, reference trajectory tracking, and control energy. All of them set a fixed range of collision avoidance areas, lacked a detailed collision avoidance planning strategy, and used only a single distance dimension. Moreover, Icaro et al. [30] introduced a dynamical collision cost function into the cost function of MPC, but it used a hierarchical scheme with the planning layer and tracking controller layer. Salim et al. [31] proposed soft and hard constraints for the MPC problem to address collision and obstacle avoidance about the formation control of a multiple quadcopter. Both of them provide a trend of development. Therefore, the flexible cost function and multi-dimensional collision avoidance constraints need to be considered and integrated into MPC tracking control. Here, the field method [32,33], which is mathematically elegant and incorporates simple path planning, is integrated into MPC control to compensate for the information of the direction dimension for collision avoidance constraints, and a soft margin is set for the distance cost function in this paper, which achieves the goal of balancing the path redundancies and energy costs.

In summary, to address the difficulty of handling the mutual incoordination of collision avoidance planning and control execution, a fusion framework of field theoretical planning and an MPC algorithm was built that can carry out trajectory tracking and real-time collision avoidance under engineering constraints to realize autonomous navigation in a multivessel environment. This approach has the following advantages over the existing solutions: (1) The fusion framework enhances the coupling relationship between trajectory planning and control action, reduces the redundant influence of ship maneuvering parameters, and executes uniform tracking control regarding the relevant state parameters of ship motion, not just the location state. (2) The collocation of MPC and the field model helps form a tracking path with a reachable angle and smooth trajectory, taking into account the trend of collision avoidance, without the exact collision avoidance path. (3) The flexibility and accessibility of control algorithms encountered in trajectory tracking is improved, which combines the manipulation constraints and trend of avoidance control, strengthening the potential relationship between collision avoidance planning and control. (4) The integrated control of multiobjective dimensions is proposed that is beneficial to coordinate and regulate in line with the trend of collision avoidance. The main novelties are that the direction trend judgment of collision avoidance planning is obtained through the design of a new fast matching (FM) method based on an electric field model and that the direction trend and elastic distance are introduced into the collision avoidance constraint of MPC for the first time rather than a fixed distance range, which broadens the state dimension, obtaining better performance. The position change is dynamically described by the backpropagation of the ship motion when considering the collision avoidance constraint; moreover, the more simplified trajectory tracking control algorithm combined with collision avoidance action is designed to balance the complexity and accuracy to obtain a reliable route. In short, it provides a new idea for the coordinated implementation of trajectory control and collision avoidance strategy.

The rest of this paper is organized as follows. In Section 2, the problem formulation is given. Section 3 presents the detailed methodologies of controller design. The simulation experiments under different complex scenes are presented in Section 4. Finally, the conclusion of this paper is in Section 5.

2. Problem Formulation

Ship tracking navigation is the key phase in ship autonomous navigation systems, which includes navigation, guidance, and control [34]. It is necessary not only to ensure the consistency of the tracking preset trajectory but also to consider maneuverability and to choose the optimal tracking route with low redundancy and energy loss. Therefore, from the point of view of ship kinematics and dynamics models [35], the state is controlled to sail to the expected trajectory, and an underactuated surface ship with three degrees of freedom is considered here, whose functions are determined by

$$\begin{array}{cccc}\left[\begin{array}{c}\dot{\mathit{s}}\\ \dot{\mathit{\eta }}\end{array}\right]=& \left[\begin{array}{cccccc}\frac{d_{11}}{m_{11}}& 0& \frac{m_{22}}{m_{11}}v& 0& 0& 0\\ 0& \frac{d_{22}}{m_{22}}& -\frac{m_{11}}{m_{22}}u& 0& 0& 0\\ -\frac{m_{22}}{m_{33}}v& \frac{m_{11}}{m_{33}}u& \frac{d_{33}}{m_{33}}& 0& 0& 0\\ \cos \varphi & -\sin \varphi & 0& 0& 0& 0\\ \sin \varphi & \cos \varphi & 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]& \left[\begin{array}{c}\mathit{s}\\ \mathit{\eta }\end{array}\right]+& \left[\begin{array}{ccc}\frac{1}{m_{11}}& 0& 0\\ 0& 0& 0\\ 0& 0& \frac{1}{m_{33}}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\mathit{\tau }\end{array}$$

(1)

where $\mathit{s}=[u,v,r{]}^T, \mathit{\eta }=[x,y,\varphi {]}^T, \mathit{\tau }={[{\tau }_u,0,{\tau }_r]}^T$, in which u, v, and r are the surge, sway, and yaw velocity, respectively, in the body-fixed frame; x and y are the positions in the earth-fixed frame; $\varphi$ is the yaw angle; ${\tau }_u$ is the force in the surge; and ${\tau }_r$ is the moment of the yaw. The inertia components are m11, m22, and m33, and d11, d22, and d33 are the damping components. (as shown in Figure 1).

Under the given expected trajectory ${\mathit{\eta }}_{\mathit{r}}={[x_r,y_r,{\varphi }_r]}^T$, trajectory tracking is realized by finding a control law $\tau$ to make

$$\underset{t\to \infty }{lim}\left|\mathit{\eta }-{\mathit{\eta }}_{\mathit{r}}\right|=0$$

(2)

However, for the actual dynamic environment, to prevent collision, the tracking trajectory is adjusted, and the following two requirements should be focused on: (1) the path loss of the adjusted trajectory should be reduced, and the expected tracking status should be restored as soon as possible; (2) the smoothness of the adjusted track should have control accessibility, and the adjustment should be realized normally under the physical constraints of the USV.

Therefore, defining state $\mathit{x}={\left[\mathit{s},\mathit{\eta }\right]}^T$ and control $\mathit{u}=\tau$, Formula (1) is transformed into:

$$\dot{\mathit{x}}=f\left(\mathit{x},\mathit{u}\right)$$

(3)

The aim is to design a real-time trajectory tracking controller based on MPC and field theory for the above system and to avoid obstacle ships.

3. Controller Design

3.1. Model Predictive Control Scheme

MPC has been maturely applied in engineering control to find the optimal target based on a finite-horizon cost, where a receding horizon strategy is used to continuously iterate and update, and the handling of state and control variables are constrained to make the algorithm model more relevant to the actual scenario. This necessitates a model describing the dynamic behavior of a plant whose function is to predict the future dynamics of the system, emphasizing the predictive effect of the model, rather than the form, so that it is suitable for objective online optimization and extreme value programming [36].

The continuous-time model (3) needs to be discretized and described as $\mathit{x}\left(k+1\right)=f_d\left(\mathit{x}\left(k\right),\mathit{u}\left(k\right)\right)$. For the optimization problem of tracking control under a given expected condition, the cost function can be obtained by

$$\begin{gathered} J\left(k\right)={{\displaystyle \sum }}_{i=0}^{N-1}(\parallel \widehat{\mathit{x}}\left(k+i|k\right)-{\mathit{x}}_r{\left(k+i\right)\parallel }_Q^2+\widehat{\mathit{u}}\left(k+i|k\right)-{\mathit{u}}_r{\left(k+i\right)\parallel }_R^2) \\ +{(\widehat{\mathit{x}}(k+N|k)-{\mathit{x}}_r\left(k+N\right))}^{T}Q\left(\widehat{\mathit{x}}\left(k+N|k\right)-{\mathit{x}}_r\left(k+N\right)\right) \end{gathered}$$

(4)

Here, $\widehat{\mathit{x}}(k+i|k)$ represents the forward i-step prediction of the state at time instant k, and ${\mathit{x}}_r\left(k+i\right)$ represents the forward i-step of the expected tracking state at time instant k. Analogously, $\widehat{\mathit{u}}(k+i|k)$ is the forward i-step prediction of the control input at time instant k, and ${\mathit{u}}_r\left(k+i\right)$ is the forward i-step of the expected control input at time instant k. Step N is the terminal horizon.

Ignoring the avoidance problem, at time instant k, the optimization problem can be formulated as

$$\begin{array}{l}\underset{u}{\min }J\left(k\right)\\ s.t.\widehat{\mathit{x}}(k+i+1|k)=f_d(\widehat{\mathit{x}}(k+i|k),\widehat{\mathit{u}}(k+i|k))\\ \widehat{\mathit{x}}(k+i|k)\in \chi , \widehat{\mathit{u}}(k+i|k)\in \upsilon \\ i=0,1,\dots ,N-1\end{array}$$

(5)

$\chi ,\upsilon$ are the state and control input constraints, whose scopes are set according to the specific conditions of ship navigation. Solving the optimal control input of the MPC scheme, the ship tracks and optimizes according to the set trajectory and finally maintains the state of Formula (2).

3.2. Collision Avoidance

(1)

Fast matching method based on the electric field model

Facing the situation of uncertainty and complexity in collision avoidance, field theory is used for avoidance planning. Currently, usually the artificial virtual field method is used to construct the target surrounding field state, but this method cannot describe the intership effect and cannot fully represent the sailing change of the ship, so the use of the electric field model for ship state simulation, where the superposition effect can be expressed well, was proposed [37]. Through the spatial electric field mode, the field energy state of the ship itself and the influence of the ship-to-ship effect are established. To describe each ship as a point charge, the collision level around the ship is represented using the equipotential surface formed by the field energy generated by the electric field model. With increasing distance, the collision avoidance level decreases and so does the collision risk; the equivalent model can be expressed as:

$$E=k_0\frac{Q_s}{r^2}$$

(6)

where $E$ is the field energy, Q_S is the quality of the ship, $k_0$ is a constant parameter, and r is the distance between the surrounding point and the center of the ship.

With the advantage of the superimposed effect of field energy, this is a good representation of the vector field distributed in the space waters of multiple vessels, which is also the base vector field of the FM method carrying out collision avoidance planning.

FM is an energy-efficient path planning method that sets a target point and combines the gradient descent algorithm. This method has the features of a global minimum and no local minima problem, decreasing the computational burden. Its aim is to generate an arrival time map that satisfies the Eikonal equation, which describes a wave front propagation scenario in the base vector field E. The Eikonal equation can be written as:

$$\parallel \nabla U\left(\overrightarrow{p}\right)\parallel \tau \left(\overrightarrow{p}\right)=1$$

(7)

where $U\left(\overrightarrow{p}\right)$ is the arrival time of wave propagation at position $\overrightarrow{p}$, and $\tau \left(\overrightarrow{p}\right)$ is the speed of wave propagation at position $\overrightarrow{p}$. Taking the energy cost into account, the minimum arrival time $U$ is chosen as the best path trend [38]. The planned path of the algorithm is shown below.

In Figure 2, the path is planned with the current position of the ship as the starting point (red point) in the presence of two obstacle ships. The color bar shows the level of the energy field. It is obvious that the trajectory is affected by the energy field generated by the two obstacle ships, and the trajectory is updated with the movement of the ships’ positions.

However, the collision avoidance path obtained by the above method is not the final path that the USV needs to follow, and it is necessary to readjust the original tracking trajectory combined with the MPC controller in Section 3.1.

(2)

Integration of collision avoidance planning into MPC

According to the MPC architecture, the collision avoidance strategy is integrated from the two aspects of the cost function and constraint conditions. The constraint conditions are considered first. The direction dimension is considered in the constraint rather than just using a fixed range for the distance constraint. The novel collision avoidance constraint includes two points: the direction trend from the FM method based on the electric field model indirectly gives the possible adjustment direction of the predicted trajectory and the distance limit between the ship and obstacle ship at the same time, as depicted in Figure 3.

Figure 3 reveals the distance limitation between the obstacle ship and the estimated position of the ship, which is affected by the trend selection of collision avoidance planning through a correlation of $\theta$. The collision avoidance trajectory obtained from Section 3.1 is divided according to the prediction horizon N, and the value of the trajectory point corresponding to each sampling interval is found, and $\tan (\theta )$, namely, is the orthogonal ratio of the adjacent trajectory points. The estimated position of the ship at the current time is indicated by backpropagating the estimated position at the next moment.

Therefore, the collision avoidance inequality constraint is as follows for each obstacle ship j ($j=1,\dots M$):

$$\begin{gathered} \sqrt{\begin{array}{l}{(\widehat{x}(k+i+1|k)-\widehat{u}(k+i|k)T\cos (\theta (k+i|k))+\widehat{v}(k+i|k)T\sin (\theta (k+i|k))-\mathrm{xta}(k+i,j|k))}^2+\\ {(\widehat{y}(k+i+1|k)-\widehat{u}(k+i|k)T\sin (\theta (k+i|k))-\widehat{v}(k+i|k)T\cos (\theta (k+i|k))-yta(k+i,j|k))}^2\end{array}} \\ \ge D{(j)}^{2}i=0,1,\dots N-1 \end{gathered}$$

(8)

where $T$ is the sample time; $\widehat{x}(k+i+1|k)$, $\widehat{y}(k+i+1|k)$, $\widehat{u}(k+i|k)$, and $\widehat{v}(k+i|k)$ correspond to x, y, u, and v of the estimated ship state, respectively; xta and yta are the positions of obstacle ships; M is the total number of obstacle ships; and D is the safe distance of collision avoidance. This constraint focuses on applying the trend of collision avoidance planning to state optimization, which guides the direction of the estimated trajectory and limits the position by setting a safe distance.

Harsh constraints result in an increase in computational complexity, and there exist some path loss and redundancy when the predicted states of N steps are all required to meet the limitation of D; therefore, an additional slack variable is added into the cost function, which also expands the solution-seeking space. The new cost function is listed as:

$$\begin{array}{l}{J}_S\left(k\right)=J\left(k\right)+{\displaystyle {\displaystyle \sum }_{i=0}^{\mathrm{N}-1}}dis\left(k+i\right),\\ dis\left(k+i\right)\\ =\alpha (\sqrt{{(\widehat{x}(k+i|k)-\mathrm{xta}(k+i,j|k))}^2+{(\widehat{y}(k+i|k)-yta(k+i,j|k))}^2}\\ -D\left(j{)}^2\right) j=1,\dots M\end{array}$$

(9)

Here, $dis\left(k+i\right)\ge 0$, $\alpha$ is a segmented constant, which varies with the positive or negative value of the distance difference in dis(k + i). If the distance difference is negative, it is negative; otherwise, it is positive. The new component indicates a soft margin regarding distance setting, which advantageously introduces the elastic factor to balance the hard conditions brought by the constraints.

Furthermore, the variable factor is considered in the safe distance D, as opposed to a fixed value based on experience, where the empirical formula for the minimum safe encounter distance is introduced as an elastic variable, drawing on the report of Li [39].

$$D=D_0\ast \{\begin{array}{cc}1.1-0.2B/{180}^{\circ }& 0^{\circ }\le B<{112.5}^{\circ }\\ 1.0-0.4B/{180}^{\circ }& {112.5}^{\circ }\le B<{180}^{\circ }\\ 1.0-0.4\left({360}^{\circ }-B\right)/{180}^{\circ }& {180}^{\circ }\le B<{247.5}^{\circ }\\ 1.1-0.2\left({360}^{\circ }-B\right)/{180}^{\circ }& {247.5}^{\circ }\le B\le {360}^{\circ }\end{array}$$

(10)

where D₀ is the distance constant, and $B$ is the relative azimuth, which stands for the angle between the ship velocity and the distance from the ship to the obstacle ship.

Thus, Formulas (8) and (9) integrate collision avoidance planning into the MPC method to solve the problem of obstacle ship avoidance during autonomous trajectory tracking. After collision avoidance is complete, the ship trajectory returns to the previously expected tracking route.

3.3. Optimization Problem and Algorithm

The important factors for the optimal control problem are parameter selection and computational efficiency. An increasing number of constraints and objectives increase the computational burden of the traditional algorithm. Therefore, we transform the optimal control problem into a nonlinear programming problem, whose core idea is to use the system model as the state constraint of each optimization step. To achieve better computing efficiency and a faster running time, the CasADi tool proposed for nonlinear optimization and algorithmic differentiation is chosen to solve this optimization problem, which has features of rapid and efficient nonlinear MPC.

The whole structure of USV trajectory tracking is as follows in Figure 4:

The algorithm flow of tracking control is given in Algorithm 1.

Algorithm 1:	The algorithm process
Inputs:	The reference state x of tracked trajectory
Outputs:	The estimated state x of ship trajectory tracking
Procedure:	if no obstacle ships $\mathrm{construct}\mathrm{the}\cos \mathrm{t}\mathrm{function}J\left(k\right)$ with constraints as Formula (5) in current time k. else calculate the direction trend $\theta$ about FM based on electric field model. bring into the formula (8) and combine the Formula (9) to reconstruct the cost function $J_s\left(k\right)$ with the collision avoidance constraint in current time k. end if find the optimal solution of input control u using CasADi. compare the next state x with the reference state xr. At next time, instant repeat procedure

3.4. Stability Analysis

Here, the stability of the proposed MPC structure is verified by Lyapunov functions, which satisfies the following theorem.

Theorem 1.

If there exists a continuously differentiable scalar function V(x) such that:

(1) 

$V\left(x\right)$is positive definite

(2) 

$V\left(f\left(x\right)\right)-V\left(x\right)\le 0$

We consider the minimization of the predicted infinite horizon cost [40]:

$$J_s{}^{*}\left(k\right)={{\displaystyle \sum }}_{i=0}^{\infty } [{\mathit{x}}^{\mathit{T}}(k+i|k)Q\mathit{x}(k+i|k)+{\mathit{u}}^T(k+i|k)R\mathit{u}(k+i|k)+dis\left(k+i\right)]$$

(11)

where $Q>0,R>0$.

Assumption 1 $\left(A,Q^{1/2}\right)$ is observable in the sense that

$W_0={{\displaystyle \sum }}_{i=0}^{n-1}{\left({{\displaystyle \prod }}_{j=0}^i{A}_j\right)}^{T}Q{{\displaystyle \prod }}_{j=0}^i{A}_j\succ 0$ for some finite $n>0$, $Q^{1/2}$ is any matrix

with $Q^{1/2}{}^T{Q}^{1/2}=Q$

$$\mathrm{Let}{J}_s{}^{*}\left(k\right)=V\left(x\left(k\right)\right)$$

(12)

According to Assumption 1, it is known that $J_s{}^{*}\left(k\right)\ge 0$ for getting $x\left(k\right)$, meaning that $J_s{}^{*}\left(k\right)$ is positive definite.

Considering an infinite horizon, the ${\tilde{J}}_s\left(k+1\right)$ is followed:

$$\begin{array}{l}{\tilde{J}}_s\left(k+1\right)={\displaystyle {\displaystyle \sum }_{i=1}^N}\left[{\mathit{x}}^T\left(k+i|k\right)Q\mathit{x}\left(k+i|k\right)+{\tilde{\mathit{u}}}^T\left(k+i|k\right)R\tilde{\mathit{u}}\left(k+i|k\right)+dis\left(k+i\right)\right]\\ +{\mathit{x}}^T\left(k+N+1|k\right)Qx\left(k+N+1|k\right)\\ ={\displaystyle {\displaystyle \sum }_{i=1}^{\infty }}\left[{\mathit{x}}^T\left(k+i|k\right)Q\mathit{x}\left(k+i|k\right)+{\tilde{\mathit{u}}}^T\left(k+i|k\right)R\tilde{\mathit{u}}\left(k+i|k\right)+dis\left(k+i\right)\right]\\ ={\displaystyle {\displaystyle \sum }_{i=0}^{\infty }}\left[{\mathit{x}}^T\left(k+i|k\right)Q\mathit{x}\left(k+i|k\right)+{\tilde{\mathit{u}}}^T\left(k+i|k\right)R\tilde{\mathit{u}}\left(k+i|k\right)\right]\\ -\left[{\mathit{x}}^{\mathit{T}}\left(k\right)Q\mathit{x}\left(k\right)+{\mathit{u}}^T\left(k\right)R\mathit{u}\left(k\right)+dis\left(k\right)\right]\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}=J_s{}^{*}\left(k\right)-\left[{\mathit{x}}^T\left(k\right)Q\mathit{x}\left(k\right)+{\mathit{u}}^T\left(k\right)R\mathit{u}\left(k\right)+dis\left(k\right)\right]\end{array}$$

(13)

Then, $\tilde{u}\left(k+1\right)$ is suboptimal at time k + 1; meanwhile, the obstacle ship is detected in a certain distance range D_d, so it has

$$\begin{gathered} {\tilde{J}}_s\left(k+1\right)=J_s{}^{*}\left(k\right)-\left[{\mathit{x}}^T\left(k\right)Qx\left(k\right)+{\mathit{u}}^T\left(k\right)R\mathit{u}\left(k\right)+dis\left(k\right)\right]\ge J_s{}^{*}\left(k+1\right) \\ {J}_s{}^{*}\left(k+1\right)-J_s{}^{*}\left(k\right)\le -\left[{\mathit{x}}^T\left(k\right)Q\mathit{x}\left(k\right)+{\mathit{u}}^T\left(k\right)R\mathit{u}\left(k\right)+dis\left(k\right)\right]\le 0 \end{gathered}$$

(14)

Therefore, the stability of the algorithm is guaranteed.

4. Simulation

4.1. Simulation Parameters

The damping components of the ship system are $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|$, and other hydrodynamic parameters are selected depending on Cui et al. [41]. The parameters involved in the MPC method are selected as follows: the sample step in simulation unit is T = 0.1($\approx 0.32s$), prediction horizon N = 8, $Q=diag\left(\left[{10}^6,{10}^6,{10}^3,{10}^7,{10}^7,{10}^3\right]\right)$, $R=diag\left(\left[{10}^{-3},{10}^{-3},{10}^{-3}\right]\right)$, and $\alpha =\pm 0.5$. The thrust is within the interval of [−400, 400], and the moment is within the interval of [−100, 100]. The initial state of the ship is $\mathit{x}=\left[0.5;0;0.5;-1;1;{10}^{\circ }\right]$, and the expected tracking trajectory is set to $\{\begin{array}{c}{x}_r\left(t\right)=2\cos ({60}^{\circ })t\\ y_r\left(t\right)=2\sin ({60}^{\circ })t\end{array},x_r\left(0\right)=0,y_r\left(0\right)=0$. Then, USV tracking control for different multivessel scenarios conforming to Algorithm 1 is simulated. Here, we use two algorithms for comparison with the proposed algorithms. The influence on the trajectory when using the fixed distance as the obstacle avoidance constraint shown in Formula (15), called the original algorithm, is observed, and the multiobjective programming model of the distance at closest point of approach (DCPA) and time at closest point of approach (TCPA), with reference to Li et al. [37], are used to replace the collision avoidance trend angle $\theta$, called the compared algorithm, and are compared.

$$\sqrt{{(\widehat{x}(k+i|k)-\mathrm{xta}(k+i,j|k))}^2+{(\widehat{y}(k+i|k)-yta(k+i,j|k))}^2}\ge D_0{}^2$$

(15)

where $D_0$ = 1.5.

4.2. Results

(1)

Single obstacle ship

The trajectory of the obstacle ship is set to ${\mathit{x}}_{\mathit{o}\mathit{b}}=\left[2{.5;0;0;11;12;-140}^{\circ }\right]$, and the results of the three algorithms are obtained.

The superiority of the new algorithm is expressed in Figure 5a, where the course and trajectory adjustment occur at the time range of t30–t50, but for the new algorithm, the change is obviously better than those of the original algorithm and the compared algorithm, indicating that the direction trend and elastic strategy have a certain effect. The comparison of the input control, that is, the thrust, is given in Figure 5b, which shows that the fluctuations caused by tracking the expected trajectory are relatively frequent in the initial stage, where the effects of the three algorithms are exactly the same, and fluctuations produce differences when collision avoidance is required. The new algorithm reaches the expected trajectory state immediately after a small adjustment, and the changes of other states with the new algorithm are shown in Figure 5c, all of which achieve the expected state very well.

(2)

Multivessel environment

The following focuses on collision avoidance when multiple vessels meet at the same time. The tracking situations of the two algorithms are given in Figure 6a–c, where two obstacle ships with states ${\mathit{x}}_{\mathit{o}\mathit{b}\mathbf{1}}=\left[2;0;0;0.7;-{1;65}^{\circ }\right]$ and ${\mathit{x}}_{\mathit{o}\mathit{b}\mathbf{2}}=\left[2;0;0;0.5;9;-{60}^{\circ }\right]$ are encountered simultaneously. The collision avoidance adjustment is mainly concentrated in the range between t10 and t80; in this stage, the new algorithm needs to avoid two obstacle ships at the same time and triggers larger fluctuations in the trajectory, corresponding to the changes in u, v, r, and states reflected in (c). The fluctuations are smaller in the late t80 period, which is mainly affected by one obstacle ship. The compared and original algorithms have more oscillations than the new algorithm throughout the whole period and are still adjusting when the new algorithm has tracked the expected trajectory, as shown in (b), and have worse smoothness than the new algorithm.

The displayed trajectory and state changes with an obstacle ship with state ${\mathit{x}}_{\mathit{o}\mathit{b}\mathbf{3}}=\left[\mathbf{1.5};0;0;{6.5;2;160}^{\circ }\right]$, in addition to the above two obstacle ships, are depicted in Figure 7a,c. The adjustment of the new algorithm is concentrated between t10 and t60, where the amplitude and smoothness of fluctuations are better than those of the original algorithm, and the adjustment range of the thrust is smaller than that of the original algorithm (see (b)). However, the difference between the comparison algorithm and the new algorithm is not obvious; the new algorithm is adjusted ahead of the comparison algorithm, and the thrust is weaker in the later stage.

Furthermore, the performance when a fourth obstacle ship with state ${\mathit{x}}_{\mathit{o}\mathit{b}\mathbf{4}}=\left[\mathbf{1.5}{;0;0;9;7;-170}^{\circ }\right]$ rendezvous with the others at the same time is described in Figure 8. With the increase in the number of obstacle ships, there is a greater impact on the collision avoidance complexity, and the adjustment range is obviously larger than that in Figure 7, but the overall status is prior to the original algorithm, although the adjustment time increases. The fluctuations of the compared algorithm are greater than those of the new ones. Although the collision avoidance planning in the compared algorithm is more accurate than that of the new ones, it is not conducive to the overall tracking of the integrated controller, and the new algorithm has certain advantages in trajectory trend and state tracking, which has a better trajectory performance for slow fluctuations in the overall situation.

In brief, considering the direction dimension of the collision avoidance constraint and the elastic distance of the cost function, the new algorithm is an improvement of the MPC method and has lower complexity, easier angle realizability, less attitude fluctuations, and shorter tracking time, especially in the case of multiple vessels meeting simultaneously, obtaining a better balance between the complexity and accessibility of the controller.

5. Conclusions

A novel fusion algorithm for trajectory tracking was proposed that creates a controller based on MPC-integrated collision avoidance planning whose structure uses the direction trend generated by the FM method of the electric field model and flexible distance to design the collision avoidance constraints and the cost function of the MPC controller. Then, the CasADi method was used for fast optimization to realize the trajectory tracking target during real-time collision avoidance adjustment. A collision-free trajectory that balances efficiency and accuracy was obtained through the fusion framework proposed in this study, which focused on coordinating control execution and collision avoidance planning while considering manipulation constraints. The framework met the parameter requirements for algorithm flexibility and accessibility and exhibits excellent performance under harsh environments where multiple vessels meet simultaneously.

The proposed algorithm provides a solution to the fusion control process for considering the multidimensional trend of collision avoidance, and cross-fuses the collision avoidance decision-making and trajectory control modules in intelligent navigation rather than just providing control input parameters. At the same time, the field model was used to avoid the complex structure of collision avoidance decision-making, but it also brings deficiencies in decision accuracy and trajectory redundancy. Other elements of collision avoidance decision need to be added to optimize this structure, such as energy cost not being satisfactory; and some practical factors still need to be given in the future, such as the COLREGs and collision avoidance actions of obstacle ships, which can be solved by introducing game theory and other methods. In addition, some environmental interferences can be added to the field model to form a more comprehensive control impact. Therefore, further enhancement is necessary to improve this fusion algorithm.