2.1 Architecture of USVs GNC systems

The local COLAV, mid-level decision-making and global path-planning subsystems are hierarchical algorithms in a USV's guidance system (Larson et al., Reference Larson, Bruch and Ebken2006; Eriksen, Reference Eriksen2019; Yang, Reference Yang2019; Loe, Reference Loe2008). The local COLAV subsystem receives the planed trajectories which are produced by upper algorithms and the information of the USV and environment from the navigation system, which consists of information fusion, environmental perception, state estimation and situation awareness technologies. The architecture of the USV GNC system is shown in Figure 1.

Figure 1. Architecture of the USV GNC system

This paper focuses on the COLAV algorithm, so the following assumptions are made.

Assumption 1: The planned trajectories are known, denoted as

(1)\begin{equation}{\boldsymbol{T}_{\boldsymbol{d}}}(k )= \left[ {\begin{array}{@{}c@{}} {{\boldsymbol{\eta }_{\boldsymbol{d}}}(k )}\\ {{\boldsymbol{\upsilon }_{\boldsymbol{d}}}(k )} \end{array}} \right]\end{equation}

where k is the index of the waypoints.

Assumption 2: The positions and velocities of obstacles are known when they enter the detection range ${r_D}$. The positions of static obstacles are denoted as $({{x_i},\; {y_i}} )$ and the positions and velocities of target ships are denoted as ${\left[ {\begin{array}{*{20}{c}} {{\boldsymbol{\eta }_j}}& {{\boldsymbol{\upsilon }_j}} \end{array}} \right]^{\mathrm{\intercal}}}$, where i and j are the indexes of static obstacles and target ships, respectively.

Assumption 3: The trajectories of target ships are known, denoted as

(2)\begin{equation}{\boldsymbol{T}_{\boldsymbol{j}}}(t )= \left[ {\begin{array}{@{}c@{}} {{\boldsymbol{\eta }_j}(t )}\\ {{\boldsymbol{\upsilon }_j}(t )} \end{array}} \right]\end{equation}

where $\boldsymbol{\eta } = {\left[ {\begin{array}{*{20}{c}} x& y& \psi \end{array}} \right]^\mathrm{\intercal}}$, x and y are the position coordinates, and $\psi$ is the orientation in north-east-down-fixed inertial frame $\{n \}$. Here, $\boldsymbol{\upsilon }$ is the velocity vector in body-fixed frame $\{b \}$, $\boldsymbol{\upsilon } = {\left[ {\begin{array}{*{20}{c}} u& v& r \end{array}} \right]^\mathrm{\intercal}}$, where u, v and r are surge velocity, sway velocity and yaw rate.

2.2 USVs motion model

To accurately express the underactuations and nonlinearities of USVs’ motion, a 3DOF mathematical motion model of the USV (Fossen, Reference Fossen2011) is used:

(3)\begin{gather}\dot{\boldsymbol{\eta }} = \boldsymbol{R} \cdot \boldsymbol{\upsilon }\end{gather}

(4)\begin{gather}\boldsymbol{M\dot{\upsilon }} = \boldsymbol{\tau \upsilon } - \boldsymbol{C}(\boldsymbol{\upsilon } )\boldsymbol{\upsilon } - \boldsymbol{D}(\boldsymbol{\upsilon } )\boldsymbol{\upsilon }\end{gather}

where $\boldsymbol{M}$, $\boldsymbol{C}(\boldsymbol{\upsilon } )$ and $\boldsymbol{D}(\boldsymbol{\upsilon } )$ are inertia, Coriolis and damping matrixes. The transformation matrix from $\{b \}$ to $\{n \}$ is given as

(5)\begin{align}\boldsymbol{R}(\psi )= \left[ {\begin{array}{@{}ccc@{}} {\cos \psi }& { - \sin \psi }& 0\\ {\sin \psi }& {\cos \psi }& 0\\ 0& 0& 1 \end{array}} \right]\end{align}

The vessel of interest in this study is the Viknes830 USV. The parameters of Viknes 830 are shown in Table 2.

Table 2. Viknes 830 parameters

2.3 Velocity controller

The COLAV algorithm outputs the desired velocities to the control system which produces switching signals to actuators by controllers and control allocators based on the position and velocity signals. By following the desired velocities, USVs can avoid obstacles and track the planned trajectories.

In this paper, a simple feedback proportional controller is used as an example, and better velocity controllers could be used to improve the performance of the COLAV algorithm in real practice. Considering the input saturation, the control outputs ${\tau _z}$, $z = 1,\;2,\;3$ are defined as

(6)\begin{gather}{\tau _z} = \left\{ {\begin{array}{@{}ll@{}} {{\tau_{z,c}}}& {\textrm{if}\;{\tau_{z,\textrm{min}}} < {\tau_{z,c}} < {\tau_{z,\textrm{max}}}}\\ {{\tau_{z,\textrm{min}}}}& {\textrm{if}\;{\tau_{z,c}} \le {\tau_{z,\textrm{min}}}}\\ {{\tau_{z,\textrm{max}}}}& {\textrm{if}\;{\tau_{z,c}} \ge {\tau_{z,\textrm{max}}}} \end{array}} \right.\end{gather}

(7)\begin{gather}{\tau _{z,\textrm{c}}} = \left\{ {\begin{array}{@{}ll@{}} { - {X_u}u - {X_{uu}}u - {X_{uuu}}u - mrv + {K_{px}}({{u_d} - u} ),}& {when \quad z = 1}\\ {\dfrac{{{K_{p\psi }}{I_z}{T_{d\psi }}({{r_d} - r} )}}{{{l_r}}},}& {when \quad z = 2}\\ {{K_{p\psi }}{I_z}{T_{d\psi }}({{r_d} - r} ),}& {when \quad z = 3} \end{array}} \right.\end{gather}

The variables and coefficients above are collected in Table 3.

Table 3. Variables and coefficients of the controller

2.4 COLREGs

To avoid traditional vessels at sea, USVs must comply with the COLREGs.

A collision situation is first identified by computing the closest point of approach (CPA), given the current pose of the USV. The vessel is deemed to be in a collision situation if the distance to the closest point of approach (DCPA) ${d_{\textrm{CPA}}}$ and the time to the closest point of approach (TCPA) ${t_{\textrm{CPA}}}$ are less than the safe threshold ${d_{\textrm{SAFE}}}$ and ${t_{\textrm{SAFE}}}$,

(8)\begin{gather}0 \le {t_{\textrm{CPA}}} \le {t_{\textrm{SAFE}}} \; \; \; \;\wedge \; \; \; \; {d_{\textrm{CPA}}} \le {d_{\textrm{SAFE}}}\end{gather}

(9)\begin{gather}{t_{\textrm{CPA}}} = \frac{{({{\boldsymbol{p}_B} - {\boldsymbol{p}_\textrm{A}}} )\cdot ({{\textbf{v}_\textrm{A}} - {\textbf{v}_B}} )}}{{||{{\textbf{v}_\textrm{A}} - {\textbf{v}_B}} ||}}\end{gather}

(10)\begin{gather}{d_{\textrm{CPA}}} = ||{({{\boldsymbol{p}_\textrm{A}} + {\textbf{v}_\textrm{A}}{t_{\textrm{CPA}}}} )- ({{\boldsymbol{p}_B} + {\textbf{v}_B}{t_{\textrm{CPA}}}} )} ||\end{gather}

where ${\boldsymbol{p}_\textrm{A}} = \; ({{x_\textrm{A}},\; {y_A}} )$ is the position of the USV's geometry centre, ${\boldsymbol{p}_B} = \; ({{x_B},\; {y_B}} )$ is the position of the obstacle's geometry centre, and ${\textbf{v}_\textrm{A}}$ and ${\textbf{v}_\textrm{B}}$ are the velocities of the USV and the obstacle. Thus, when a possible collision situation is present, it remains to identify the applicable COLREGs rules.

Rules 13–17 in COLREGs stipulate the responsibilities of the give-way vessel and the stand-on vessel in the three encounter situations, namely head-on situation, crossing situation, and overtaking, as shown in Figure 2.

Figure 2. Encounter situations and COLAV manoeuvre

These three encounter situations contain five different scenarios determined by the relative bearing of the target vessel to the USV.

(11)\begin{equation}{\beta _{BA}} = \arctan 2({{y_A} - {y_B},{x_A} - {x_B}\; } )- \; {\psi _B}\end{equation}

The USV acts as the give-way vessel in three scenarios:

1. (1) head-on, ${\beta _{BA}} \in [{ { - 15^\circ ,15^\circ } )}$;

2. (2) the target vessel crossing from starboard side, ${\beta _{BA}} \in [{ {15^\circ ,112.5^\circ } )}$;

3. (3) the target vessel being overtaken by the USV, ${\beta _{AB}} \in [{ {112.5^\circ ,180^\circ } )} \cup [{ { - 180^\circ , - 112.5^\circ } )}$.

Moreover, the USV acts as the stand-on vessel in two scenarios:

1. (1) the target vessel crossing from port side, ${\beta _{BA}} \in [{ { - 112.5^\circ ,15^\circ } )}$;

2. (2) the target vessel overtaking the USV, ${\beta _{BA}} \in [{ {112.5^\circ ,180^\circ } )} \cup [{ { - 180^\circ , - 112.5^\circ } )}$.

As target ships sometimes may violate their responsibilities of the give-way vessel for different practical reasons, the USV should also be able to avoid collisions alone. Therefore, communications between the USV and target ships are not necessary in this paper.

Remark 1: In two-vessel encounter situations, Rule 17 of COLREGs does not require that the stand-on vessel ‘must’ or ‘have to’ always keep its course and speed. The stand-on vessel can also take necessary and understandable manoeuvres to change its course or speed. Since USVs cannot communicate with the target vessel, it is necessary for them to take active, effective and understandable collision avoidance manoeuvres alone.

3.1 Search space

The CS-MPC algorithm makes optimal COLAV decisions based on the hydrodynamic characteristics of USVs, which can be represented as the search space ${\boldsymbol{V}_s}$. It is a set of discrete velocities with which the USV can arrive at within the prediction time. To obtain an accurate search space, the USV motion model in Equations (3)–(5) is used, which retains the underactuated characteristics and the nonlinear relationship between velocities and rudder forces. By setting the initial value ${\boldsymbol{v}_0} = \left[ {\begin{array}{*{20}{c}} 0& 0& 0 \end{array}} \right]$, time step ${t_s} = 0.1\;\textrm{s}$ and discretising ${\tau _i} \in [{{\tau_{i,\textrm{min}}},{\tau_{i,\textrm{max}}}} ]$ averagely, after a sufficient number of iterations (in this paper, 100 steps), the numerical solutions of Equations (3)–(5) are obtained with the fourth-order Runge–Kutta method. These solutions constitute the search space of the USV ${\boldsymbol{V}_s} = \, {\left[ {\begin{array}{*{20}{c}} \boldsymbol{u}& \boldsymbol{r} \end{array}} \right]^\mathrm{\intercal}}$. Putting it more intuitively, the search space can be accurately represented as an irregular pattern, as shown in Figure 4, rather than a simple rectangular space.

Figure 4. Search space ${\boldsymbol{V}_s}$ of the USV

Unfortunately, due to the limits of the prediction time ${T_p}$, the current velocity ${\boldsymbol{\upsilon }_{\boldsymbol{a}}} = ({{u_a},{v_a},{r_a}} )$ and the input saturations of the propeller and rudder, the USV can only arrive at a finite set of velocities ${\boldsymbol{V}_d}$, which can be defined as follows:

(12)\begin{gather}{\dot{\boldsymbol{\upsilon }}_k} = {\boldsymbol{M}^{ - 1}}({{\boldsymbol{\tau }_k} - \boldsymbol{C}({{\boldsymbol{\upsilon }_{\boldsymbol{a}}}} ){\boldsymbol{\upsilon }_{\boldsymbol{a}}} - \boldsymbol{D}({{\boldsymbol{\upsilon }_{\boldsymbol{a}}}} ){\boldsymbol{\upsilon }_{\boldsymbol{a}}}} )\end{gather}

(13)\begin{gather}{\boldsymbol{V}_d} = \left\{ {\begin{array}{@{}c@{}} {({u,r} )\in R \times R|{u \in [{{u_a} + {{\dot{u}}_{\textrm{min}}}{T_p},\; {u_a} + {{\dot{u}}_{\textrm{max}}}{T_p}\; } ]} \; }\\ { \wedge r \in [{{r_a} + {{\dot{r}}_{\textrm{min}}}{T_p},\; {r_a} + {{\dot{r}}_{\textrm{max}}}{T_p}} ]} \end{array}} \right\}\end{gather}

where $k \in \{{\textrm{min},\, \textrm{max}} \}$. Therefore, the search space is reduced to a subset of ${\boldsymbol{V}_s}$, called feasible velocities ${\boldsymbol{V}_f}$,

(14)\begin{equation}{\boldsymbol{V}_f} = \{{{\boldsymbol{V}_d} \cap {\boldsymbol{V}_s}} \}\end{equation}

For example, in the scenario that the current states of USV are ${\boldsymbol{\eta }_{\boldsymbol{a}}} = \left( {\begin{array}{*{20}{c}} 0& 0& 0 \end{array}^\circ } \right)$, ${\boldsymbol{\upsilon }_{\boldsymbol{a}}} = \left( {\begin{array}{*{20}{c}} {7\;\textrm{m/s}}& 0& 0 \end{array}} \right)$, and the states of target ship are ${\boldsymbol{\eta }_{\boldsymbol{b}}} = \left( {\begin{array}{*{20}{c}} {800\;\textrm{m}}& {800\;\textrm{m}}& { - 90^\circ } \end{array}} \right)$, ${\boldsymbol{\upsilon }_{\boldsymbol{b}}} = \left( {\begin{array}{*{20}{c}} {7\;\textrm{m/s}}& 0& 0 \end{array}} \right)$, the prediction time is ${T_p} = 2.5\;\textrm{s}$, and the resolution of surge speed and yaw rate are 0.2 m/s and 0.01 rad/s. The reduced search space is the intersection area of the blue and red dotted lines shown in Figure 6.

3.2 Collision cone

Another important advantage of the CS-MPC algorithm is the short runtime, which is directly influenced by the scale of the search space. To reduce the runtime, the search space is further reduced by introducing collision cones for static or dynamic obstacles. Given a USV A and an obstacle B, the collision cone takes the position of the USV's geometric centre as its vertex ${\boldsymbol{p}_\textrm{A}}$, and the two tangent lines of a circle from the vertex as its edges, denoted as ${\boldsymbol{p}_{AB}}_{\textrm{left}}^ \bot$ and ${\boldsymbol{p}_{AB}}_{\textrm{right}}^ \bot$. The circle's centre is the position of the obstacle's geometric centre ${\boldsymbol{p}_\textrm{B}}$, and its radius is the sum of the safe range of the USV ${r_\textrm{A}}$ and the obstacle ${r_\textrm{B}}$.

Assumption 4: The state of the USV changes immediately. Therefore, the predicted heading of the USV can be calculated by Equations (15), (20) and (21).

(15)\begin{equation}\tilde{\psi } = \psi + r \cdot {T_p}\end{equation}

Based on Assumption 4, the collision cone can be expressed as follows:

(16)\begin{gather}\mathrm{\alpha } = \textrm{arcsin}\left( {\frac{{{r_\textrm{A}} + {r_\textrm{B}}}}{{||{{\textbf{p}_{\textrm{AB}}}} ||}}} \right)\end{gather}

(17)\begin{gather}{\boldsymbol{p}_{\textrm{AB}}}_{\textrm{left}}^ \bot = \boldsymbol{J}({ - \mathrm{\alpha } + \mathrm{\pi }/2} )\frac{{{\boldsymbol{p}_{\textrm{AB}}}}}{{||{{\boldsymbol{p}_{\textrm{AB}}}} ||}}\end{gather}

(18)\begin{gather}{\boldsymbol{p}_{\textrm{AB}}}_{\textrm{left}}^ \bot = \boldsymbol{J}({\mathrm{\alpha } - \mathrm{\pi }/2} )\frac{{{\boldsymbol{p}_{\textrm{AB}}}}}{{||{{\boldsymbol{p}_{\textrm{AB}}}} ||}}\end{gather}

(19)\begin{gather}\boldsymbol{V}{\boldsymbol{O}_{\boldsymbol{A}|\boldsymbol{B}}} = \textrm{ }\{{{\textbf{v}_\textrm{A}}|{\textbf{v}_{\boldsymbol{BA}}} \cdot {\boldsymbol{p}_{\textrm{AB}}}_{\textrm{left}}^ \bot \ge 0 \wedge {\textbf{v}_{\boldsymbol{BA}}} \cdot {\boldsymbol{p}_{\textrm{AB}}}_{\textrm{right}}^ \bot \ge 0} \}\end{gather}

where ${\boldsymbol{p}_{\textrm{AB}}}$ is the relative position of the obstacle with respect to the USV ${\boldsymbol{p}_{\textrm{AB}}} = {\boldsymbol{p}_\textrm{B}} - {\boldsymbol{p}_\textrm{A}}$, likewise, their relative velocity is ${\textbf{v}_{\boldsymbol{BA}}} = {\textbf{v}_\textrm{A}} - {\textbf{v}_\textrm{B}}$, where $\textbf{v}$ is the velocity vector in the north-east-down-fixed inertial frame $\{n \}$, and u and v are surge velocity and sway velocity as described in Section 2.1. Here, $\; \boldsymbol{J}({\tilde{\psi }} )$ is the rotation matrix rotating a matrix with the angle of $\tilde{\psi }$,

(20)\begin{align}\textbf{v} & = \boldsymbol{J}({\tilde{\psi }} ){\left[ {\begin{array}{@{}cc@{}} u& v \end{array}} \right]^\mathrm{\intercal }}\end{align}

(21)\begin{align}\boldsymbol{J}({\tilde{\psi }} )& = \left[ {\begin{array}{@{}cc@{}} {\cos \tilde{\psi }}& { - \sin \tilde{\psi }}\\ {\sin \tilde{\psi }}& {\cos \tilde{\psi }} \end{array}} \right]\end{align}

As the velocities ${\textbf{v}_\textrm{A}}$ in $\boldsymbol{V}{\boldsymbol{O}_{\boldsymbol{A}|\boldsymbol{B}}}$ pointing to the collision cone lead the USV to obstacles or target vessels, it should never be chosen. Consequently, these velocities could be shielded from the search space. The runtime is shortened, because the velocities in the shielded search space will not be evaluated in the following steps. For the example in Section 3.1, the collision cone can be visually represented by Figure 5, and the velocities in the final restricted search space are shown in Figure 6 as blue stars. To prevent too many feasible velocities from being shielded, the collision cone should only be calculated when the distance between the USV and an obstacle is less than the safe range, called the collision cone range ${r_C}$ in this paper.

Figure 5. Schematic diagram of collision cone

Figure 6. Search space of CS-MPC

3.3 Trajectory prediction

From the final restricted search space, the optimal solution can be selected based on the situation in a prediction time. Therefore, the trajectory prediction should be as accurate as possible. Meanwhile, to reduce the computation complexity, the small change assumption is made as follows.

Assumption 5: If the length of the iteration step is short enough, the Coriolis matrixes$\boldsymbol{\; C}(\boldsymbol{\upsilon } )$ and damping matrixes $\boldsymbol{D}(\boldsymbol{\upsilon } )$ are constants within the iteration step.

Then the predicted trajectories within a prediction time ${T_p}$ are obtained by solving the 3DOF mathematic motion model of USVs using Equations (3)–(5) numerically with the improved Euler method,

(22)\begin{gather}{\mathbf{\upsilon }_p} = {\boldsymbol{\upsilon }_n} + h{\boldsymbol{M}^{ - 1}}({\boldsymbol{\tau } - \boldsymbol{C}({{\boldsymbol{\upsilon }_n}} ){\boldsymbol{\upsilon }_n} - \boldsymbol{D}({{\boldsymbol{\upsilon }_n}} ){\boldsymbol{\upsilon }_n}} )\end{gather}

(23)\begin{gather}{\mathbf{\upsilon }_c} = {\boldsymbol{\upsilon }_n} + h{\boldsymbol{M}^{ - 1}}({\boldsymbol{\tau } - \boldsymbol{C}({{\boldsymbol{\upsilon }_n}} ){\boldsymbol{\upsilon }_p} - \boldsymbol{D}({{\boldsymbol{\upsilon }_n}} ){\boldsymbol{\upsilon }_p}} )\end{gather}

(24)\begin{gather}{\mathbf{\upsilon }_{n + 1}} = \frac{1}{2}({{\mathbf{\upsilon }_c} + {\mathbf{\upsilon }_p}} )\end{gather}

(25)\begin{gather}{\boldsymbol{\eta }_{n + 1}} = {\boldsymbol{\eta }_n} + \frac{h}{2}\boldsymbol{R}({{\mathbf{\upsilon }_n} + {\mathbf{\upsilon }_{n + 1}}} )\end{gather}

where n is the index of iteration steps, h is the length of the prediction time step, and $\boldsymbol{\tau } = ({\tau _z}|z = 1, \;2, \; 3)$ is the current control force calculated from Equations (6) and (7).

The predicted trajectories for the example in Section 3.1 are shown in Figure 7(a).

Figure 7. (a) predicted trajectories of ${\boldsymbol{V}_r}$. (b) Objective function value of ${\boldsymbol{V}_r}$

3.4 Selecting the desired velocities

Based on the predicted trajectories, the candidate velocities in the final restricted search space can be evaluated by a criterion. Assuming that in the time step k there are N candidate velocities in ${\boldsymbol{V}_r}$ and M obstacles around the USV, the criterion can be represented as the objective function, shown as

(26)\begin{gather}({{u_r},{r_r}} )= \arg \max {\mathrm{{\cal H}}^k}\end{gather}

(27)\begin{gather}{\mathrm{{\cal H}}^k} = \left\{ {\begin{array}{@{}ll@{}} {{q_\chi } \cdot \dfrac{{|{\chi - {\chi_{\textrm{goal}}}} |}}{{\mathop {\textrm{max}}\limits_{i \in N} |{\chi - {\chi_{\textrm{goal}}}} |}} + {q_d} \cdot \dfrac{{{d_{OA}}}}{{\mathop {\textrm{max}}\limits_{i \in N} \, {d_{OA}}}} + {q_u} \cdot \dfrac{{|u |}}{{\mathop {\textrm{max}}\limits_{i \in N} |u |}} + {q_\mathrm{{\cal T}}} \cdot T,}& {d_{i,n}^k > {d_{\textrm{SAFE}}}}\\ { - 1,}& {d_{i,n}^k \le {d_{\textrm{SAFE}}}} \end{array}} \right.\end{gather}

(28)\begin{gather}d_{i,n}^k = \mathop {\min }\limits_{m \in M} ||{\boldsymbol{p}_{\boldsymbol{A},\boldsymbol{n}}^{\boldsymbol{k}} - \boldsymbol{p}_{\boldsymbol{B},\boldsymbol{m}}^{\boldsymbol{k}}} ||\end{gather}

where$\; ({{u_r},{r_r}} )$ is regarded as the optimal solution, ${q_\chi } < 0$, ${q_u} > 0$, ${q_d} > 0$, ${q_\mathrm{{\cal T}}} < 0$ are coefficients tuned in different missions, and $d_{i,n}^k$ is the minimum distance between the $n$th discrete position in the $i$th predicted trajectory of the USV $\boldsymbol{p}_{\boldsymbol{A},\boldsymbol{n}}^{\boldsymbol{k}}$ and the predicted positions of the $m$th obstacles $\boldsymbol{p}_{\boldsymbol{B},\boldsymbol{m}}^{\boldsymbol{k}}$. All other variables take the values at the end of each trajectory. The first term ensures the course directing to the destination, where $\chi$, ${\chi _{goal}}$ are the predicted course and the course toward the goal. The second term keeps the USV far away from obstacles, where ${d_{OA}}$ represents the sum of the distances between the USV and obstacles. The third term is to make the USV maintain a high surge speed u. The last term presents the COLREGs penalty, evaluates whether the avoidance manoeuvre complies with the COLREGs and maintains a consistent avoidance manoeuvre. Here, $\mathrm{{\cal T}}$ is the Boolean variable, whose value is true (value ‘1’) if the following conditions are all met.

1. (1) The USV is in a collision situation with another vessel. The collision situation is determined in Section 2.4.

2. (2) The COLAV decision violates COLREGs.

Examining whether the USV will pass the other vessel on its port or starboard side is an efficient technique to determine whether the COLAV decision violates COLREGs. As shown in Figure 2, the USV shall turn to starboard passing the other vessel on its port side in scenarios where the target ship is coming from head or crossing from the starboard side. Moreover, the USV shall not turn to port when it has to take action as will best aid to avoid collision. According to the above, turning to port violates COLREGs in these scenarios. It can be described as

(29)\begin{equation}{\boldsymbol{p}_{\textbf{AB}}} \times {\textbf{v}_{\boldsymbol{BA}}} < 0\end{equation}

1. (3) The COLAV decision is different from the previous avoidance manoeuvre.

As shown in Figure 2, when the USV is overtaking the other vessel, the USV should keep out of the way of the other vessel and could pass it from either side. Similarly, when the USV is being overtaken by another vessel, it could let the other vessel pass from either side. In the above two scenarios, the USV will chatter when it is close to the other vessel, because it could choose to pass the vessel from different sides at time step $\textrm{k}$ and $\textrm{k} + 1$. To solve this problem, the COLAV decision shall be consistent by adding the penalty when

(30)\begin{equation}({{\boldsymbol{p}_{\boldsymbol{AB},\boldsymbol{k}}} \times {\boldsymbol{v}_{\boldsymbol{BA},\boldsymbol{k}}}} )\cdot ({{\boldsymbol{p}_{\boldsymbol{AB},\boldsymbol{k} + 1}} \times {\boldsymbol{v}_{\boldsymbol{BA},\boldsymbol{k} + 1}}} )< 0\end{equation}

1. (4) The collision situation has not cleared yet.

To prevent the second-time risk of collision, the COLREGs rule 8 requires that the effectiveness of the action shall be carefully checked until the other vessel is finally past and clear. As a result, the penalty still should be added when the COLAV decision violates COLREGs, even though the action to avoid collision makes ${t_{\textrm{CPA}}} \ge \; {t_{\textrm{SAFE; }}}$ or ${d_{\textrm{CPA}}} \ge {d_{\textrm{SAFE}}}$, until the bearing $\beta$ of the other vessel crosses 22.5 degrees abaft the beam of the USV. At time steps k and $k + 1$, this condition can be described as

(31)\begin{gather}\begin{gathered} [{\beta - ({{\psi_k} + 112^\circ } )} ]\cdot [{\beta - ({{\psi_{k + 1}} + 112^\circ } )} ]> 0\; \\ \wedge \; [{\beta - ({{\psi_k} - 112^\circ } )} ]\cdot [{\beta - ({{\psi_{k + 1}} - 112^\circ } )} ]> 0 \end{gathered}\end{gather}

The objective function values for the above example are shown in Figure 7(b). The RGB colour represents the values of the first three terms in Equation (6). Red represents the values of the course term. Green represents the values of the distance term. Blue represents the values of the surge speed term. In addition, as turning port side violates the COLREGs, all the objective function values of these solutions are lower.

Remark 2: The VO method needs to meet the assumption that USVs move at any constant velocity throughout the avoidance manoeuvre. Therefore, the resulting trajectory is linear (straight line movement). However, this assumption cannot be met when predicting the trajectory using the dynamic motion model of USVs. As a result, there are deviations between the two types of trajectories. The comparison between these two types of trajectories of velocities in the collision cone of Figure 6 is illustrated in Figure 8(a). For the above reasons, two issues arise. First, a small number of shielded velocities may move outside the collision cone, generating the optimal collision avoidance trajectory. Second, a small number of velocities from the final restricted search area may fall into the collision cone along nonlinear trajectories.

Figure 8. (a) Comparison between linear and nonlinear trajectories. (b) Deviation between the two types of trajectories

For the issue of shielding the optimal trajectories, it should be accepted that in some cases, the solution may be suboptimal. However, for the robustness of the MPC algorithm, suboptimal solutions can still be used and be efficient to avoid collisions.

1. (1) The deviation between the optimum and suboptimal solutions is acceptable and can be ignored. The distance between the end positions of linear and nonlinear trajectories are shown in Figure 8(b). Therefore, if the optimal solution is in a collision cone, it will be near the boundary of the collision cone. In addition, as the local COLAV time step is very short, it will barely affect the COLAV effectiveness.

2. (2) Because the CS-MPC uses a rolling optimisation strategy, the low-level controller will follow the desired velocity only at the next time step. In most velocity controllers, such as the one used in this paper, the control forces based on optimal and suboptimal solutions for COLAV are almost the same in the early period when the error appears.

3. (3) If the optimal solution is in the collision cone, the suboptimal solution will be located on the same side of the collision cone. Additionally, the position of the USV predicted by the suboptimal solution is further away from the collision cone. It means the suboptimal solution has a larger DCPA than the optimal solution. Therefore, if the optimal solution is shielded in a collision cone, the chosen suboptimal solution will be safe but more conservative.

For the issue of some velocities outside the collision cone still being dangerous, the objective function values of these velocities are set to ‘−1’ as in Equations (26)–(28). If there are other feasible velocities, these dangerous velocities will not be chosen.

The pseudo-code of the CS-MPC COLAV algorithm is shown as follows.

4.1 Encountering a single vessel

Encountering a single vessel is a common situation for ships' COLAV, especially on ocean voyages. As a typical simulation experiment, it can verify the effectiveness of the COLAV algorithm. Assume that the USV is sailing from one waypoint (the initial state) to another waypoint (the destination) in the planned trajectory without disturbance of wind, current and wave.

1. (1) In the first group of simulations, the USV needs to avoid collision with the target vessel, which does not require coordinated manoeuvres. The simulations are set based on the five scenarios described in Section 2.4. The experimental conditions are shown in Table 5 and the results are shown in Figure 9.

Analysing the experimental results, the runtimes of all iterative steps were within 460 ms, much shorter than the sample time. Therefore, the CS-MPC COLAV algorithm can meet the real-time calculation requirements. In addition, the USV avoided the target vessel, complying with the COLREGs. The USV was able to avoid other vessels even if they did not comply with the COLREGs. Meanwhile, the USV did not oscillate during the COLAV procedures.

1. (2) In the second group of simulations, both the USV and the target vessel manoeuvre coordinately with the CS-MPC COLAV algorithm. So, both vessels are regarded as USVs. The simulations are set based on the three encounter situations shown in Figure 2. The experimental conditions are shown in Table 6 and the results are shown in Figure 10.

Analysing the experimental results, even though there is no communication between the two USVs, both vessels manoeuvre coordinately to avoid collision, complying with the COLREGs. In addition, each COLAV manoeuvre is slighter than the one in uncoordinated manoeuvre scenarios.

Figure 9. Simulation results of COLAV with a single vessel in uncoordinated manoeuvre scenarios

Figure 10. Simulation results of COLAV with a single vessel in coordinated manoeuvre situations

Table 5. Simulation conditions of COLAV with single vessel in uncoordinatedly manoeuvre scenarios

Table 6. Simulation conditions of COLAV with single vessel in coordinated manoeuvre situations

4.2 Encountering multiple vessels

In coastal navigation, USVs are often in multi-vessel encounter situations, requiring the COLAV algorithm to make intelligent decisions.

1. (1) Taking the USV encounter with two target vessels as an example, simulation experiments were carried out. The simulation conditions in Table 5 were combined to form six groups of encounter situations. The results are shown in Figure 11.

From the results, it can be observed that the USV selected effective COLAV decisions in the two-vessel encounter situations. When the COLREGs were impossible to comply with by both vessels [such as the results in Figure 11(c)], the USV even deviated from the COLREGs rules, so as to select the safest manoeuvre.

1. (2) It is difficult for a USV to avoid collision when it encounters more than two target vessels. IMAZU from Tokyo University of Marine Science and Technology raised many encounter situations that were difficult for multi-vessel COLAV (called the IMAZU problem). To further verify the effectiveness of the CS-MPC algorithm, 11 relatively difficult situations of the IMAZU problem are simulated, where the USV encounters three target vessels at the same time. Considering the difficulty of the IMAZU problem, every vessel should manoeuvre to help avoid collision based on the CS-MPC algorithm. Therefore, all the four vessels in the simulation were regarded as USVs. The simulation conditions are shown in Table 7, and the results are shown in Figure 12.

The simulation results show that the USVs can avoid collision when they encounter with three other target vessels. As there is a collision risk between either of the vessels, the prerequisite for the success of the COLAV is that every vessel should manoeuvre to avoid collision. However, in some scenarios, although the COLAV is achieved, the trajectories of the USVs may be very winding, such as in the experiments shown in Figures 12(g), 12(j) and 12(k).

Figure 11. Simulation results of encounter in multi-vessel situations

Figure 12. COLAV simulation results for four vessels in the IMAZU problem

Table 7. Simulation conditions of four vessels encounter situations of IMAZU problem

4.3 Complicated situation

To further verify the superiority in agility of the CS-MPC algorithm, simulations were carried out in a more complicated situation, where the USV should avoid several static obstacles and two target ships. The CS-MPC algorithm was compared with the SB-MPC algorithm. The experiment conditions and some parameters of the SB-MPC algorithm are shown in Table 8. Other parameters of the SB-MPC are the same as the CS-MPC shown in Table 4. The simulation results are shown in Figure 13.

Figure 13. Experiment results in the complicated situation

Table 8. Simulation conditions of the complicated situation and parameters of the SB-MPC algorithm

The USV can avoid the static obstacles and target vessels, guided by either the CS-MPC or SB-MPC algorithm.

From Figure 13(c), the CS-MPS mean runtime is 442.2 ms, which is 9.21% less than the SB-MPC mean runtime of 487.1 ms. Especially in period , the CS-MPC mean runtime is approximately 250 ms, because parts of the search space were shielded by both the static obstacle and the target vessel. Therefore, the USV reacted more quickly, guided by the CS-MPC.

From Figure 13(b), the minimum distance from the USV to obstacles (including target vessels) of the CS-MPC was 143.9 m, which was 35.88% larger than that of the SB-MPC at 105.9 m. Therefore, the USV can avoid obstacles by making safer COLAV decisions guided by the CS-MPC.

From Figure 13(a), the USV guided by the CS-MPC took action earlier in the procedures of both COLAV and returning to the planned trajectory. In addition, the COLAV manoeuvre of the USV guided by the CS-MPC is more obvious. Above all, the COLAV decision of the CS-MPC is better according to the COLREGs rules.