Real-Time Emergency Collision Avoidance for Unmanned Surface Vehicles with COLREGS Flexibly Obeyed

Abstract

This paper presents a real-time emergency collision-avoidance method for unmanned surface vehicles (USVs) with the International Regulations for Preventing Collisions at Sea (COLREGS) flexibly obeyed. The pivotal issue is that some traffic vessels may violate the demands of this convention, which would increase the risk of collision if the USV blindly obeys the COLREGS rules. To avoid mandatory compliance with these COLREGS rules, a real-time truncated velocity obstacle (TVO) algorithm is proposed to assign a collision-free velocity vector for the control system to realize. Considering a reasonable trade-off between safety and the COLREGS rules, the proposed collision-avoidance method expands the TVO’s area based on the velocity uncertainties of traffic vessels, which greatly enhance the safety of collision-avoidance operations and encourage the USV to follow the COLREGS rules. To promptly realize an assigned collision-free velocity, this paper also develops a discrete simultaneous planning and executing (SPAE) controller design. The proposed discrete controller is divided into three parts: online polynomial planning to satisfy the constraints of tracking errors, an accurate uncertainty estimation, and an algebraic control law to promptly execute the planned polynomial. Numerical results have validated the reliability and intelligibility of the proposed collision-avoidance method. Furthermore, simulated and experimental results have validated the effectiveness of the proposed controller design.

1. Introduction

In recent decades, different commercial and military communities have designed and fabricated numerous unmanned marine vehicles to perform multiple maneuvering tasks, including path following, trajectory tracking, formation control, and station keeping [1,2,3,4,5,6,7,8,9,10,11]. Collision avoidance among multiple agents has become an essential component of a modern navigation system [12,13,14,15,16,17,18,19,20]. In terms of marine navigation, a reliable capability of collision avoidance is important for unmanned surface vehicles (USVs) to perform task-based autonomous maneuvers. Based on mapping information from an onboard chart server, the mission of a USV path planner is to generate a safe and optimized path that avoids all the obstacles listed on the map. Generally, global path planning provides an optimal path from the starting point to the goal. When some potential obstacles come within the detection range of onboard sensors, the local path replanning will take a response to locally modify the path or even abandon the path planning mission [1].

In particular, all marine surface vehicles are encouraged to obey the International Regulations for Preventing Collisions at Sea (COLREGS) approved by the International Maritime Organization in 1972. These regulations specify certain maneuvers to avoid a collision between two vehicles, such as an overtaking case, a head-on case, a stern-crossing case, and a bow-crossing case. Numerous ship collisions show that most marine accidents occur due to the violation of COLREGS rules. Developing the collision-avoidance approaches on the basis of COLREGS rules is a consensus for the study of the intelligent navigation of USVs [21]. Currently, a variety of approaches based on COLREGS rules have been proposed in the past few decades, such as fuzzy logic [22,23], artificial potential field [24,25,26], evolutionary algorithms [27], reinforcement learning [28,29,30,31], and neural networks [32,33]. However, these methods require real-time computational requirements and are difficult to deal with multiple obstacles and multiple COLREGS rules. Furthermore, the uncertainty of vehicle motion, the violation of COLREGS rules, and the perturbation of obstacle detection may also threaten the navigation’s safety [34]. In this context, more encounter situations and avoidance operations are needed to cover all the potential collision-avoidance cases, which certainly increases the difficulty of application and simultaneously decreases the vehicle intelligence [35]. Therefore, developing a real-time emergency collision-avoidance method considering COLREGS is essential for the USV’s navigation system to safely complete its missions.

The velocity obstacle (VO) is a real-time emergency collision-avoidance approach that can handle multiple obstacles in the detection range of onboard sensors. The VO approach was first proposed by [36] for robot motion planning, and several variations have been developed, including truncated velocity obstacle (TVO), reciprocal velocity obstacle (RVO), hybrid RVO, and optimal RVO [37,38]. Currently, many approaches based on VO and its variations considering the COLREGS rules have been proposed for USVs to avoid multiple obstacles in an emergency. In [39], a cone-shaped VO was adopted, and the velocity space was divided into different conic zones based on the COLREGS rules. Similarly, Zhao et al. [40] proposed a COLREGS-compliant collision-avoidance method with optimal RVO, where a collision-avoidance decision was triggered by an evidential reasoning theory after a potential collision warning. In [34], a two-level dynamic obstacle avoidance algorithm was proposed, i.e., a first-level VO algorithm in a non-emergency situation and a second-level artificial potential method in an emergency situation. In [41], a COLREGS-based VO algorithm was presented by visualizing the changes in course and speed in collisions. However, a practical problem is that some commercial ships may not obey the COLREGS rules to actively avoid USVs since USVs usually have small vehicle sizes and are difficult fr other ships to detect. Given that USVs usually have highly flexible maneuvering capability, for safety reasons, they should have a certain sense of confidence to actively violate the COLREGS rules unless it is safe enough to follow these rules.

In view of the existing shortages, this paper specially develops a TVO-based collision-avoidance method for USVs, which not only strengthens the operation security in the presence of motion uncertainty but also can flexibly obey the COLREGS rules. Firstly, a collision probability between two vehicles is calculated, and a set covering the motion uncertainty of a target ship is then created and expanded based on the collision probability. Importantly, the expansion of this set should encourage the USV to follow the COLREGS rules. As a result, the USV can decide whether to obey the COLREGS rules or violate them after an evaluation of collision risk. Several potential contributions of this paper are summarized as follows:

• A real-time collision-avoidance method is proposed for USVs to efficiently handle multiple obstacles and flexibly consider the COLREGS rules in the determination of TVO. For safety purposes, the motion uncertainty of the target ship is considered in an expandable set. Instead of blindly obeying the COLREGS rules, the USV can take an action that violates the rules in the context of a high collision risk.
• Other than most existing methods that only take passive actions to avoid the moving target, the proposed method has no conflicts to take synchronous active actions for multiple vehicles.
• A discrete simultaneous planning and executing (SPAE) controller design is developed to promptly realize the assigned collision-free velocity. Compared to most conventional controllers, there is no need to fine-tune control gains and it is easy for non-control readers to understand and implement.
• To validate the proposed SPAE controller design, both simulations and experiments are conducted to realize a selected crossing scenario between two vehicles.

This paper is organized as follows. Section 3 presents a VTO-based collision-avoidance algorithm. Section 4 describes an obstacle expansion strategy considering the motion uncertainty and COLREGS rules. Section 5 outlines the proposed control system design to realize the collision-free reference states. Section 6 provides the main results and discussions. Section 7 concludes the paper.

2. Collision Risk Assessment

Collision risk refers to the collision probability and can be classified into two categories, i.e., a numerical representation and a graphical representation [17]. The numerical representation presents the collision risk as a number called the collision risk index (CRI). The graphical representation presents the collision risk as a two-denominational map with warning rings, action lines, etc. In this section, the popular numerical method in practice is adopted to evaluate the collision risk by two indices, i.e., distance to closest point of approach (DCPA) and the time to closest point of approach (TCPA). Then, the CRI can be formulated as a linear combination of DCPA and TCPA.

As shown in Figure 1, let A and B denote the circle configuration space with their centers $P_A$ and $P_B$ fixed at the middle of USV and the moving vehicle, respectively. The configuration radii of A and B are depicted by $r_R$ and $r_o$, and the velocity vectors are represented by $v_R$ and $v_o$, respectively. The bearing angle between the relative speed $v_{A|B}=v_R-v_o$ and position vector $P_{AB}=P_B-P_A$ is written as

$$\chi =\mathrm{atan}2(P_{AB}^y,P_{AB}^x)-\mathrm{atan}2(v_{A|B}^y,v_{A|B}^x).$$

(1)

Hence, the DCPA and TCPA can be calculated as

$$\mathrm{DCPA}=|P_{AB}|sin(\left|\chi \right|),$$

(2)

$$\mathrm{TCPA}=\frac{|P_{AB}|cos(\left|\chi \right|)}{|v_{A|B}|},$$

(3)

where DCPA provides the direct collision probability. $\mathrm{DCPA}\le d_1$ indicates that a collision is inevitable if two vehicles do not take any avoiding actions. Here, $d_1$ can be set to $r_R+r_o$. When $d_1<\mathrm{DCPA}<d_2$, there exists a possibility of collision. Here, $d_2$ is the preset secure collision-avoidance distance, and it can be set to several times that of $d_1$. This means that the vehicle must take avoiding actions when $P_{AB}\le d_2$. TCPA provides the potential collision probability. A larger TCPA indicates a lower collision probability. The negative TCPA with $\left|\chi \right|\in (0,\pi ]$ demonstrates that two vehicles are moving away from each other. To avoid a singularity of TCPA, a small minimum value can be set for $|v_{A|B}|$.

The direct collision probability based on DCPA can be calculated as

$${\mu }_{\mathrm{DCPA}}=\left\{\begin{array}{c}{\displaystyle 1\mathrm{DCPA}\le d_1,}\hfill \\ {\displaystyle p{d}_1<\mathrm{DCPA}<d_2,}\hfill \\ {\displaystyle 0d_2\le \mathrm{DCPA},}\hfill \end{array}\right.$$

(4)

where

$$p=\frac{1}{2}-\frac{1}{2}sin\left[\frac{\pi }{d_2-d_1}\left(\mathrm{DCPA}-\frac{d_1+d_2}{2}\right)\right].$$

The potential collision probability based on TCPA can be calculated as

$${\mu }_{\mathrm{TCPA}}=\left\{\begin{array}{c}{\displaystyle 10<\mathrm{TCPA}\le t_1,}\hfill \\ {\displaystyle {\left(\frac{t_2-\mathrm{TCPA}}{t_2-t_1}\right)}^2{t}_1<\mathrm{TCPA}<t_2,}\hfill \\ {\displaystyle 0\mathrm{TCPA}\le 0\mathrm{or}{t}_2\le \mathrm{TCPA},}\hfill \end{array}\right.$$

(5)

$$t_1=\left\{\begin{array}{c}{\displaystyle \frac{\sqrt{d_1^2-{\mathrm{DCPA}}^2}}{|v_{A|B}|}\mathrm{DCPA}\le d_1,}\hfill \\ {\displaystyle 0\mathrm{DCPA}>d_1,}\hfill \end{array}\right.$$

(6)

$$t_2=\left\{\begin{array}{c}{\displaystyle \frac{\sqrt{d_2^2-{\mathrm{DCPA}}^2}}{|v_{A|B}|}\mathrm{DCPA}\le d_2,}\hfill \\ {\displaystyle 0\mathrm{DCPA}>d_2.}\hfill \end{array}\right.$$

(7)

The comprehensive collision probability ${\mu }_{\mathrm{comp}}\in [0,1]$ can be designed as a combination of the direct collision probability and the potential collision probability, which gives

$${\mu }_{\mathrm{comp}}=\min (w_{\mathrm{DCPA}}{\mu }_{\mathrm{DCPA}}+w_{\mathrm{TCPA}}{\mu }_{\mathrm{TCPA}},1),$$

(8)

where $w_{\mathrm{DCPA}}$ and $w_{\mathrm{TCPA}}$ denote the corresponding weights.

3. TVO-Based Collision Avoidance

In a velocity space, as shown in Figure 2, let $H(P_h,r_h)$ denote an open disc of radius $r_h$ centered at $P_h$, which gives

$$H(P_h,r_h)=\left\{P\right|\left|\right|P-P_h\left|\right|<r_h\},$$

(9)

According to [38], the TVO with its apex at the origin O of the velocity-space coordinate system is defined as

$$TV{O}_{A|B}^{\kappa }\left(O\right)=\left\{v_{A|B}\right|\exists t\in [0,\kappa ],\kappa v_{A|B}\cap H(P_B-P_A,r_R+r_o)\ne ⌀\},$$

(10)

where $\kappa$ is a preset collision-free time period associated with a small $v_{A|B}$. If $v_{A|B}\in TV{O}_{A|B}^{\kappa }\left(O\right)$, a collision between two vehicles will occur at some moment before time $\kappa$, and vice versa. The conventional VO algorithm usually adopts zero time to collision ($\kappa =0$). This means the vehicle needs to immediately take avoiding actions once the VO algorithm is carried out. Sometimes, we do not expect to take avoiding actions when the distance between two vehicles is far enough away. Hence, a nonzero $\kappa$ can be set to truncate the VO.

To avoid a collision between these two vehicles, as shown in Figure 3a, it follows that

$$|\chi +\mathsf{\Delta }\chi |\ge \left|\lambda \right|,$$

(11)

where $\mathsf{\Delta }\chi$ represents the change in the aforementioned bearing angle, and $\lambda$ is the half cone angle. As shown in Figure 3b, a new TVO is defined by shifting the TVO in Figure 3a from its origin to the apex of the velocity $v_o$ of vehicle B. Notice that the arc boundary of $TV{O}_{A|B}^{\kappa }$ in Figure 3a,b is simplified as a straight line. Given that the collision-free time $\kappa$ along this line has a relatively small change, this approximation can facilitate the determination of USV velocity $v_R$ to avoid a collision.

4. Obstacle Expansion Induced by Motion Uncertainties and COLREGS Rules

4.1. COLREGS Rules

As shown in Figure 4, this paper will discuss three scenarios regulated by the COLREGS rules, i.e., overtaking, head-on, and crossing.

• Overtaking: The own ship should be taken as an overtaking ship when it comes up with another ship from a direction more than 22.5 degrees abaft its beam. The own ship can pass the moving ship on its port side or the starboard side.
• Head-on: When two head-on ships encounter each other on the reciprocal or nearly reciprocal courses, they should alter their courses to starboard such that each should pass on the port side of the other.
• Crossing: When two ships are crossing and have a collision risk, the ship with the other on its own starboard side should keep out of the way and shall, if the circumstances of the case admit, avoid crossing ahead of the other ship.

In Case IV of Figure 4, the COLREGS crossing rule requires that the moving vehicle should keep out of the way to avoid the USV. Considering that some commercial ships with larger sizes have insufficient maneuvering capabilities to actively avoid a USV with a small size, the USV should have a sense of confidence to determine whether to obey or violate the COLREGS rules in an emergency.

4.2. Obstacle Expansion Encouraged by COLREGS

To enhance the operating safety, we suggest that a velocity expansion $V_B$ of the moving vehicle can be designed as a monotone increasing function of the comprehensive collision probability, as shown in Figure 5. As depicted by Case III in Figure 4, the USV is encouraged to obey the COLREGS crossing rule from the stern of the target ship. Therefore, the following adaptive expansion strategy is developed to determine the expansion of the moving ship’s velocity $v_o$

$$\mathsf{\Delta }{v}_o={\mathsf{\Lambda }}_1{v}_o(1-{\exp }^{-{\rho }_1{\mu }_{\mathrm{comp}}}),$$

(12)

where ${\mathsf{\Lambda }}_1>0$ denotes the maximum expansion of velocity magnitude, and ${\rho }_1>0$ dominates the decreasing rate. It is worth noting that the above expansion strategy (12) is not unique, and readers can design other kinds of reasonable adaptive laws to replace (12).

As depicted in Figure 5, therefore, a moving vehicle’s velocity $v_o^{\prime }$ considering the measurement uncertainty is formulated within the following range:

$$v_o^{\prime }\in [k_1{v}_o,k_1{v}_o+\mathsf{\Delta }{v}_o],$$

(13)

where $0<k_1<1$ is the parameter that considers the measurement uncertainty of the moving ship’s velocity $v_o$.

To enhance the operating safety of Cases I and II in Figure 4, the expansion direction of the moving ship’s velocity $v_o$ is chosen to be clockwise. This means that the USV is encouraged to pass from the port side of the moving vehicle instead of its starboard side, which has a good agreement with COLREGS rules. An adaptive direction expansion of velocity $v_o$ is developed as

$$\mathsf{\Delta }\varphi =\left\{\begin{array}{c}{\displaystyle {\mathsf{\Lambda }}_2|v_{A|B}|>c\&|\chi |\in [0,b],}\hfill \\ {\displaystyle 0|v_{A|B}|>c\&|\chi |\notin [0,b],}\hfill \\ {\displaystyle {\mathsf{\Lambda }}_2(1+k_2{\mu }_{\mathrm{comp}})\left|v_{A|B}\right|\le c,}\hfill \end{array}\right.$$

(14)

where b denotes a preset maximum bearing angle that is used to distinguish whether to enter a crossing scenario or not. ${\mathsf{\Lambda }}_2$ represents the direction expansion of $\mathsf{\Delta }{v}_o$. $k_2>0$ is an introduced scaling gain. $c>0$ is a preset minimum value of the relative speed $|v_{A|B}|$. $|v_{A|B}|\le c$ demonstrates that the USV and the moving ship have almost the same velocity, i.e., $v_R\approx v_o$. If the collision risk is high with $v_R\approx v_o$, ${\mu }_{\mathrm{comp}}\approx 1$ indicates that a larger direction expansion ${\mathsf{\Lambda }}_2(1+k_2{\mu }_{\mathrm{comp}})$ is generated, which does not encourage the USV to accompany the moving ship on its starboard side. For the overtaking and head-on scenarios, i.e., $|v_{A|B}|>c$ and $\left|\chi \right|\in [0,b]$, a fixed direction expansion ${\mathsf{\Lambda }}_2$ is produced to encourage the USV to take a passing course from the port side of the moving ship. When two vehicles are moving away from each other, i.e., $|v_{A|B}|>c$ and $\left|\chi \right|\notin [0,b]$, no direction expansion is needed.

As shown in Figure 5, an adaptive expansion of velocity $v_o$ is formed as a set $V_B$. By using the Minkowski sum operation between $TV{O}_{A|B}^{\kappa }\left(v_o\right)$ and $V_B$, a new truncated velocity obstacle $TV{O}_{A|B}^{\kappa }\oplus V_B$ is established. Notice that the vehicle’s desired speed vector $v_d$ is generated online by the USV’s guidance system, and a minimum bias speed $\mathsf{\Delta }{v}_R$ can be accordingly determined to avoid the velocity obstacle $TV{O}_{A|B}^{\kappa }\oplus V_B$. Thus, the USV’s collision-free velocity is calculated as

$$v_R=v_d+\mathsf{\Delta }{v}_R,$$

(15)

which will be sent to the USV’s control system for realization. Detailed assignment for the desired speed $v_d$ is given in Section 5.

5. USV Control System Design

5.1. Mathematical Model for Marine Surface Vessels

Without considering the pitch, roll, and heave motions [42], a three-degree-of-freedom nonlinear model for marine surface vessels can be described as

$$\dot{\mathbf{\eta }}=\mathit{J}\left(\psi \right)\mathit{v},$$

(16)

$$\mathit{M}\dot{\mathit{v}}+\mathit{C}\left(\mathit{v}\right)\mathit{v}+\mathit{D}\left(\mathit{v}\right)\mathit{v}=\mathbf{\tau }+{\mathbf{\tau }}_e,$$

(17)

where vector $\mathbf{\eta }={[x,y,\psi ]}^{\mathrm{T}}$ contains the north position x, east position y, and yaw heading $\psi$ in the inertial coordinate system; vector $\mathit{v}={[u,\nu ,r]}^{\mathrm{T}}$ includes the surge velocity u, sway velocity $\nu$, and yaw angular velocity r in the body-fixed coordinate system. Rotation matrix $\mathit{J}\left(\psi \right)$ is expressed as

$$\mathit{J}\left(\psi \right)=\left[\begin{array}{ccc}cos\left(\psi \right)& -sin\left(\psi \right)& 0\\ sin\left(\psi \right)& cos\left(\psi \right)& 0\\ 0& 0& 1\end{array}\right];$$

nonlinear term $\mathit{C}\left(\mathit{v}\right)\mathit{v}$ provides the Coriolis and centripetal surge and sway forces as well as yaw moment; mass matrix $\mathit{M}$ includes the rigid-body mass and the hydrodynamic added mass; nonlinear damping term $\mathit{D}\left(\mathit{v}\right)\mathit{v}$ covers the hydrodynamic damping force and moment; vector $\mathbf{\tau }$ is the control input; vector ${\mathbf{\tau }}_{\mathbf{e}}$ contains the incremental disturbances. The formulations of these matrices can be found in [42,43].

Given that most USVs are underactuated, the sway motion cannot be directly controlled. In this context, expanding from (17) yields the following surge and yaw dynamic models

$$m_u\dot{u}+d_u={\tau }_{surge},$$

(18)

$$I_z\ddot{\psi }+d_{\psi }={\tau }_{yaw},$$

(19)

where $m_u$ and $I_z$ represent the surge mass and the yaw inertial moment, respectively. Nonlinear terms $d_u$ and $d_{\psi }$ lump all uncertainties together [44].

5.2. Desired Surge Speed and Yaw Heading Assignment

To accomplish multiple marine tasks, including path following, trajectory tracking, and formation control, the USV is required to follow a predefined trajectory or track a moving target that moves on the predefined trajectory [45,46,47,48,49,50,51]. This predefined trajectory is formed by several waypoints, and each segment can be formulated by a high-order polynomial as the function of a path parameter $\theta \in [0,j]$. Index j represents the jth segment. When $\theta =0$ and $\theta =j$, the USV arrives at the first waypoint and the final waypoint, respectively. Detailed path formulation can be found in [45,49]. As shown in Figure 6, a right-hand moving coordinate system is established with its $X_p$ axis tangent to the path. Its origin $O_p$ moves along this trajectory and an optimal path parameter $\theta$ exists such that its $Y_p$ axis can pass through the origin of the USV body-fixed coordinate system [49,52]. To obtain this optimal path parameter $\theta$, numerical solutions or iteration mechanism methods can be adopted to figure it out. For the numerical solutions, readers can refer to [52]. For the iteration mechanism methods, detailed implementation processes and their summaries can be found in Section III of the research work [49]. To assign a desired yaw heading ${\psi }_d$ for the vehicle to track the trajectory, a line-of-sight (LOS) guidance system presented is presented as follows:

$${\psi }_d=\delta \left(\theta \right)-\mathrm{atan}\left(\frac{y_e}{\mathsf{\Lambda }}\right),$$

(20)

where $\delta \left(\theta \right)$ denotes the rotated angle of the moving coordinate system; $y_e$ is the cross-track error; and $\mathsf{\Lambda }$ represents the look-ahead distance.

Here, we do not consider the sideslip effect since the sideslip angle $\beta =\mathrm{atan}2(\nu ,u)$ is relatively small in practice (typically lower than 5 degrees) [47,53]. Without considering collision avoidance, therefore, the desired speed vector $v_d$ in Equation (15) can be appropriately assigned as

$$v_d={\left[u_{d}cos\left({\psi }_d\right),u_{d}sin\left({\psi }_d\right)\right]}^{\mathrm{T}}.$$

(21)

where $u_d$ is a predefined surge velocity.

Then, the aforementioned collision-avoidance method in Section 4 is used to provide a collision-free vehicle speed $v_R=v_d+\mathsf{\Delta }{v}_R$, which yields

$$v_R={\left[u_{c}cos\left({\psi }_c\right),u_{c}sin\left({\psi }_c\right)\right]}^{\mathrm{T}}.$$

(22)

where $u_c$ and ${\psi }_c$ are the collision-free surge velocity and yaw heading, respectively. These two collision-free states act as reference inputs for the control system to realize, which will be presented in Section 5.4.

5.3. State Estimation by Using a Three-Order Differentiator

After obtaining a measured state signal by an onboard sensor or a desired state signal by the above guidance system, here denoted as $y_m$, their higher-order derivatives are usually indispensable to the control system design. Generally, we can develop a filtering system or use a high-order differentiator to provide these higher-order states for the control implementation [54]. Some typical high-order differentiator designs can be found in [55,56,57,58,59,60]. Here, a three-order differentiator presented in [59] is introduced to provide the USV’s high-order states. Its discrete formulations can be expressed as follows:

$$x_1\left[i\right]=x_1[i-1]+\mathsf{\Delta }t\left(x_2[i-1]-\frac{{\sigma }_3}{\epsilon }{|x_1[i-1]-y_m|}^{(\alpha +2)/3}\mathrm{sign}(x_1[i-1]-y_m)\right),$$

(23)

$$x_2\left[i\right]=x_2[i-1]+\mathsf{\Delta }t\left(x_3[i-1]-\frac{{\sigma }_2}{{\epsilon }^2}{|x_1[i-1]-y_m|}^{(2\alpha +1)/3}\mathrm{sign}(x_1[i-1]-y_m)\right),$$

(24)

$$x_3\left[i\right]=x_3[i-1]-\mathsf{\Delta }t\frac{{\sigma }_1}{{\epsilon }^3}{|x_1[i-1]-y_m|}^{\alpha }\mathrm{sign}(x_1[i-1]-y_m),$$

(25)

where $\epsilon$ and $\alpha$$\in (0,1)$. The sampling time is defined as $\mathsf{\Delta }t$. Notice that $\left[i\right]$ denotes an index of the present sampling time period $[i\mathsf{\Delta }t,(i+1\left)\mathsf{\Delta }t\right]$. Hence, in this paper, $[i-1]$ and $[i+1]$ (if any) describe the corresponding information in the last and next sampling time periods. These three positive gains ${\sigma }_1$, ${\sigma }_2$, and ${\sigma }_3$ should be selected to fulfill the Hurwitz requirement associated with a polynomial $s^3+{\sigma }_3{s}^2+{\sigma }_{2}s+{\sigma }_1$. As a consequence, $x_1\left[i\right]$, $x_2\left[i\right]$, and $x_3\left[i\right]$ here are the filtered position, velocity, and acceleration, respectively.

5.4. SPAE Controller Design

In kinematics, the variations in position and velocity are not independent, and they are naturally produced via the integral process of acceleration. If the variations of position and velocity are constrained by a planned polynomial, the system’s convergence depends solely on the execution of the planned acceleration (the second-order derivative of the planned polynomial) [61]. In this subsection, the proposed SPAE controller consists of three parts: a planned polynomial to satisfy state constraints, an efficient estimation of uncertainty, and an algebraic control law design to carry out the planned acceleration. Notice that the proposed control framework is a digital implementation, which is widely adopted in practical applications.

Let $e_u\left[i\right]=u\left[i\right]-u_c\left[i\right]$ and $e_{\psi }\left[i\right]=\psi \left[i\right]-{\psi }_c\left[i\right]$ represent the surge velocity-track error and the yaw heading-track error, respectively. To generate the aforementioned planned accelerations for surge and yaw motions, two state-constrained online polynomial planning strategies are proposed as follows:

• First-order polynomial planning for surge velocity-track motion: for a given time period $t_u$, a first-order polynomial $p_u\left(t\right)=e_u\left[i\right]+a_u\left[i\right]t$ can be uniquely determined by satisfying the speed-track constraints of the present velocity-track error $e_u\left[i\right]$ at time $t=0$ and a zero velocity-track error at time $t=t_u$. To execute the acceleration-track error ${\dot{e}}_u\left[i\right]$ by using the planned acceleration $a_u\left[i\right]=-e_u\left[i\right]/t_u$ as a reference, i.e., ${\dot{e}}_u\left[i\right]\to a_u\left[i\right]$, the surge acceleration $\dot{u}\left[i\right]$ can be planned as

  $${\dot{u}}_p\left[i\right]={\dot{u}}_c\left[i\right]-\frac{e_u\left[i\right]}{t_u}$$

  (26)

  where ${\dot{u}}_p\left[i\right]$ is the planned surge acceleration for the surge control execution. The collision-free acceleration ${\dot{u}}_c\left[i\right]$ can be obtained by using the above differentiator with $u_c\left[i\right]$ as the input.
• Third-order polynomial planning for yaw heading-track motion: for a given time period $t_{\psi }$, a third-order polynomial planning $p_{\psi }\left(t\right)=e_{\psi }\left[i\right]+{\dot{e}}_{\psi }\left[i\right]t+a_{\psi }\left[i\right]t^2/2+j_{\psi }\left[i\right]t^3/6$ can be uniquely determined by satisfying the yaw-track constraints of the present states $(e_{\psi }\left[i\right],{\dot{e}}_{\psi }\left[i\right])$ at time $t=0$ and zero states $(0,0)$ at time $t=t_{\psi }$. Here, $a_{\psi }\left[i\right]=-6e_{\psi }\left[i\right]/t_{\psi }^2-4{\dot{e}}_{\psi }\left[i\right]/t_{\psi }$ can be assigned as the planned angular acceleration. To execute the angular acceleration-track error ${\ddot{e}}_{\psi }\left[i\right]$ by using the planned angular acceleration $a_{\psi }\left[i\right]$ as a reference, i.e., ${\ddot{e}}_{\psi }\left[i\right]\to a_{\psi }\left[i\right]$, the angular acceleration $\ddot{\psi }\left[i\right]$ can be planned as

  $${\ddot{\psi }}_p\left[i\right]={\ddot{\psi }}_c\left[i\right]-\frac{6e_{\psi }\left[i\right]}{t_{\psi }^2}-\frac{4{\dot{e}}_{\psi }\left[i\right]}{t_{\psi }}$$

  (27)

  where ${\ddot{\psi }}_p\left[i\right]$ is the planned angular acceleration for the yaw control execution. Similarly, the collision-free angular acceleration ${\ddot{\psi }}_c\left[i\right]$ can be obtained by using the above differentiator with ${\psi }_c\left[i\right]$ as the input.

To accurately execute the planned surge acceleration (26) and the yaw angular acceleration (27), the discrete velocity-track and yaw-track control laws are presented as follows:

$${\tau }_{surge}\left[i\right]={\widehat{m}}_u{\dot{u}}_p\left[i\right]+{\widehat{d}}_u[i-1],$$

(28)

$${\tau }_{yaw}\left[i\right]={\widehat{I}}_z{\ddot{\psi }}_p\left[i\right]+{\widehat{d}}_{\psi }[i-1],$$

(29)

where ${\widehat{m}}_u$ and ${\widehat{I}}_z$ are the estimated mass and inertial moment, respectively. It is worth noting that the proposed control laws adopt the estimated uncertainty terms ${\widehat{d}}_u[i-1]$ and ${\widehat{d}}_{\psi }[i-1]$ in the last sampling time period since ${\widehat{d}}_u\left[i\right]$ and ${\widehat{d}}_{\psi }\left[i\right]$ have not been updated in the present sampling time period. Therefore, ${\widehat{d}}_u\left[i\right]$ and ${\widehat{d}}_{\psi }\left[i\right]$ will be updated for the control executions ${\tau }_{surge}[i+1]$ and ${\tau }_{yaw}[i+1]$ in the next sampling time period.

By substituting the control laws (28) and (29) into the discrete dynamic equations of (18) and (19), respectively, the uncertainty terms used for the control executions ${\tau }_{surge}[i+1]$ and ${\tau }_{yaw}[i+1]$ in the next sampling time period can be updated as

$${\widehat{d}}_u\left[i\right]={\widehat{d}}_u[i-1]+k_u{\widehat{m}}_u({\dot{u}}_p\left[i\right]-\dot{u}\left[i\right]),$$

(30)

$${\widehat{d}}_{\psi }\left[i\right]={\widehat{d}}_{\psi }[i-1]+k_{\psi }{\widehat{I}}_z({\ddot{\psi }}_p\left[i\right]-\ddot{\psi }\left[i\right]),$$

(31)

where $0<k_u\le 1$ and $0<k_{\psi }\le 1$ are two introduced positive gains. The feedback angular accelerations $\ddot{\psi }\left[i\right]$ can also be obtained by using the above differentiator with the yaw measurement as the input. According to the USV kinematic Equation (16), the feedback surge accelerations $\dot{u}\left[i\right]$ can be obtained based on the filtered $\dot{\mathbf{\eta }}\left[i\right]$ and $\ddot{\mathbf{\eta }}\left[i\right]$.

5.5. System Convergence Analysis

5.5.1. Surge Velocity-Track Convergence

Considering that ${\dot{e}}_u\left[i\right]=\dot{u}\left[i\right]-{\dot{u}}_c\left[i\right]$, substituting the control law (28) and the planned acceleration (26) into the discrete dynamic model of (18) gives

$${\widehat{m}}_u\left({\omega }_u{\dot{e}}_u\left[i\right]+\frac{e_u\left[i\right]}{t_u}\right)+{\widehat{m}}_u(1-{\omega }_u){\dot{u}}_c\left[i\right]+d_u\left[i\right]-{\widehat{d}}_u[i-1]=0.$$

(32)

where ${\omega }_u=m_u/{\widehat{m}}_u$ is defined as a scale factor for the surge mass.

In the next sampling time period, we have

$${\widehat{m}}_u\left({\omega }_u{\dot{e}}_u[i+1]+\frac{e_u[i+1]}{t_u}\right)+{\widehat{m}}_u(1-{\omega }_u){\dot{u}}_c[i+1]+d_u[i+1]-{\widehat{d}}_u\left[i\right]=0.$$

(33)

Considering that ${\ddot{e}}_u\left[i\right]\mathsf{\Delta }t={\dot{e}}_u[i+1]-{\dot{e}}_u\left[i\right]$ and ${\dot{e}}_u\left[i\right]\mathsf{\Delta }t=e_u[i+1]-e_u\left[i\right]$, subtracting Equation (32) from Equation (33) yields

$${\widehat{m}}_u\mathsf{\Delta }t\left[{\omega }_u{\ddot{e}}_u\left[i\right]+(\frac{k_u}{\mathsf{\Delta }t}+\frac{1}{t_u}){\dot{e}}_u\left[i\right]+\frac{k_u}{t_u\mathsf{\Delta }t}{e}_u\left[i\right]\right]={\mathsf{\Omega }}_u.$$

(34)

where ${\mathsf{\Omega }}_u={\widehat{m}}_u(1-{\omega }_u)({\dot{u}}_c\left[i\right]-{\dot{u}}_c[i+1])+d_u\left[i\right]-d_u[i+1]$ contains the offsets of uncertainties and collision-avoidance acceleration in two nearby sampling time periods. With the decrease in the sampling loop period $\mathsf{\Delta }t$, the surge uncertainty error ${\mathsf{\Omega }}_u$ will gradually approach zero since $d_u\left[i\right]\to d_u[i+1]$ and ${\dot{u}}_c\left[i\right]\to {\dot{u}}_c[i+1]$. Therefore, it is clear that the speed-track error system (34) converges in a finite time period.

5.5.2. Yaw Heading-Track Convergence

Considering that ${\ddot{e}}_{\psi }\left[i\right]=\ddot{\psi }\left[i\right]-{\ddot{\psi }}_d\left[i\right]$, substituting the yaw control law (29) and the planned angular acceleration (27) into the discrete model of (19) gives

$${\widehat{I}}_z\left({\omega }_{\psi }{\ddot{e}}_{\psi }\left[i\right]+\frac{4{\dot{e}}_{\psi }\left[i\right]}{t_{\psi }}+\frac{6e_{\psi }\left[i\right]}{t_{\psi }^2}\right)+{\widehat{I}}_z(1-{\omega }_{\psi }){\dot{\psi }}_c\left[i\right]+d_{\psi }\left[i\right]-{\widehat{d}}_{\psi }[i-1]=0.$$

(35)

where ${\omega }_{\psi }=I_z/{\widehat{I}}_z$ is defined as a scale factor for the yaw inertial moment.

Similarly, we have

$${\widehat{I}}_z\mathsf{\Delta }t\left[{\omega }_{\psi }{e}_{\psi }^{\left(3\right)}\left[i\right]+\left(\frac{k_{\psi }}{\mathsf{\Delta }t}+\frac{4}{t_{\psi }}\right){\ddot{e}}_{\psi }\left[i\right]+\left(\frac{4k_{\psi }}{t_{\psi }\mathsf{\Delta }t}+\frac{6}{t_{\psi }^2}\right){\dot{e}}_{\psi }\left[i\right]+\frac{6k_{\psi }}{t_{\psi }^2\mathsf{\Delta }t}{e}_{\psi }\left[i\right]\right]={\mathsf{\Omega }}_{\psi }.$$

(36)

where the yaw uncertainty error ${\mathsf{\Omega }}_{\psi }$ is defined as ${\mathsf{\Omega }}_{\psi }={\widehat{I}}_z(1-{\omega }_{\psi })({\ddot{\psi }}_c\left[i\right]-{\ddot{\psi }}_c[i+1])+d_{\psi }\left[i\right]-d_{\psi }[i+1]$. If ${\mathsf{\Omega }}_{\psi }\approx 0$ and ${\omega }_{\psi }\le 1$, it is also not difficult to validate the convergence of the error system (36) since its left-hand formulation satisfies the Routh–Hurwitz criterion.

6. Main Results and Discussions

6.1. System Setup

The vehicle model used in simulations is an underactuated USV powered by two fixed thrusters at the stern. The inertial mass of this vehicle is 40 kg. Its inertial moment is 12.8 $\mathrm{kg}·{\mathrm{m}}^2$. Detailed descriptions of this USV can be found in [10,44,62]. Implementation processes of the proposed methods are summarized in a block diagram, as shown in Figure 7. Based on a number of simulations, the preset parameters for simulated validations were fine-tuned and are listed in

Table 1. Preset relevant parameters.

Items	Specifications	Values
$\kappa$	Truncated value	20
$r_R$	USV configuration radius	2 (m)
$r_o$	Configuration radius of moving vehicles	2, 3.5, 4.5, and 5.5 (m)
$d_1$	Secure encounter distance	$d_1=r_R+r_o$ (m)
$d_2$	The reaction distance to avoid obstacle	$d_2=4d_1$ (m)
${\mathsf{\Lambda }}_1$	Maximum expansion of velocity magnitude	${\mathsf{\Lambda }}_1=1$
$k_1$	Gain of velocity measurement uncertainty	$k_1=0.7$
${\rho }_1$	Expansion rate of magnitude	${\rho }_1=2$
b	Maximum bearing angle	$b=\pi /6$
c	Minimum relative speed	$c=0.2\left(\mathrm{m}/\mathrm{s}\right)$
${\mathsf{\Lambda }}_2$	Maximum expansion of orientation	${\mathsf{\Lambda }}_2=\pi /6$
$k_2$	Scaling gain of orientation	$k_2=2$
$\epsilon$	Differentiator parameter	$\epsilon =0.1$
$\alpha$	Differentiator parameter	$\alpha =0.6$
${\sigma }_1$	Differentiator parameter	${\sigma }_1=6$
${\sigma }_2$	Differentiator parameter	${\sigma }_2=12$
${\sigma }_3$	Differentiator parameter	${\sigma }_3=6$
$t_u$	Surge planning time period	$t_u=2$ s
$k_u$	Surge uncertainty estimation gain	$k_u=0.02$
$t_{\psi }$	Yaw planning time period	$t_{\psi }=2$ s
$k_{\psi }$	Yaw uncertainty estimation gain	$k_{\psi }=0.02$

.

6.2. USV Passive Collision Avoidance

This subsection presents the main results of the passive collision-avoidance scenario where only the USV takes action to avoid collision with other moving vehicles. All the vehicles are represented by configuration circles, and the graphical representation of each circle was recorded every 8 s with a gradient color. The initial configuration circles for all the vehicles are marked in red. When the USV arrives at the goal, the USV, vehicle B, vehicle C, and vehicle D are marked in blue, black, pink, and cyan, respectively.

Firstly, the head-on and overtaking scenarios were selected for simulated validations. As shown in the first plot of Figure 8, both the USV and the moving vehicle B were initially heading toward each other. After about 32 s, the USV altered its heading to the port side of vehicle B to avoid a collision, which obeys the COLREGS head-on rule. After 48 s, the USV returned to its initial path. As shown in the second and third plots of Figure 8, there are two different collision-avoidance actions when the USV overtook vehicle B. The USV speed was 0.5 m/s, and the speed of vehicle B was 0.16 m/s. The initial position of vehicle B was (0 m, 18 m), and the initial positions of USV were (−2 m, −15 m) and (−2 m, −5 m), respectively. When the USV was far away from vehicle B in the overtaking scenario (second plot), Equation (14) indicates that the clockwise orientation expansion of vehicle B was larger, thereby resulting in the overtaking on the port side of vehicle B. In the third plot, the USV had a sense of confidence to pass vehicle B’s starboard side. Notice that both overtaking actions obey the COLREGS rules.

Secondly, different crossing scenarios were selected for simulated validations. As shown in Figure 9, the USV had the moving vehicle B on its own starboard side when the simulations started. Vehicle B started at position (0 m, 50 m) with a constant speed of 0.33 m/s. In the first plot of Figure 9, the USV started at position (−20 m, 35 m) with a lower speed of 0.37 m/s, and it took a collision-avoidance path from vehicle B’s stern, which obeys the COLREGS crossing rule. However, when the USV was far away from vehicle B and had a larger speed of 0.5 m/s, as shown in the second plot of Figure 9, it directly took the crossing action from vehicle B’s bow, which violated the COLREGS crossing rule. As shown in Figure 10, the USV had the moving vehicle B on its own port side when the simulations started. The difference is that vehicle B violated the COLREGS crossing rule in this case, which is expected to occur in reality. It can be observed that this situation can also be safely handled.

Then, the collision-avoidance scenarios with multiple vehicles were selected for simulated validations. As shown in Figure 11, the USV, vehicle B, vehicle C, and vehicle D started at positions (0 m, 0 m), (40 m, 0 m), (20 m, 15 m), and (−25 m, 40 m), respectively. At the beginning, there was a head-on situation between the USV and vehicle B. Then, the USV successfully took the crossing route from the bow of vehicle C and the stern of vehicle D one by one. It can be observed that the proposed real-time collision-avoidance method provided a reasonable balance between safety and compliance with COLREGS rules and had remarkable performances in passively avoiding other vehicles.

For comparative purposes, finally, the passive collision-avoidance scenario in Figure 12 was carried out. The closed-loop trajectories for Vehicles B, C, and D were the edges of a five-pointed star, and these vehicles started their positions from three different apexes. In particular, the velocities of Vehicles B, C, and D were set to time-varying sinusoidal functions with the same average speed of 0.5 m/s, the same period, and different phase angles. The USV moved along a circular trajectory in an anti-clockwise direction with a constant speed of 0.5 m/s. Comparisons among different methods were made and are listed in

Table 2. Comparisons among different methods in passive collision avoidance with multiple vehicles.

Comparative	Indexes
Methods	Number of	Number of	Number of	Successful
	Encounter	Collision	Violated Rules	Percentage
Velocity Obstacle (VO)	100	4	18	96%
VO with COLREGS Blindly Obeyed	100	2	0	98%
Proposed Method	100	0	5	100%

. We can observe that the USV with our proposed method successfully avoided all the obstacles. Instead of blindly obeying the COLREGS rules, the proposed method may violate these rules to safely and successfully avoid other vehicles compared to the other two methods.

6.3. Active Collision Avoidance among Multiple Vehicles

Unlike the aforementioned passive collision avoidance, the active situation allows other vehicles to adopt the same collision-avoidance method. The main results of active collision avoidance are presented in this subsection. The configuration radii for all the vehicles were set to 2 m. As shown in Figure 13, four vehicles started to move towards the same position (0 m, 30 m) from four different directions. It can be observed that these vehicles simultaneously altered their headings to the starboard side to avoid a collision, which is quite similar to entering and exiting the roundabout at the same time. As shown in Figure 14, the USV, vehicle B, vehicle C, and vehicle D started their positions at (0 m, 0 m), (40 m, 0 m), (20 m, 15 m), and (−25 m, 40 m), respectively. At the beginning, there was a head-on situation between the USV and vehicle B. Unlike the complex scenarios presented in Figure 12, it was safer when all vehicles took collision-avoidance actions. Moreover, the USV successfully took the crossing route from the bow of vehicle D, which violated the crossing rule of COLREGS. The main results of active collision avoidance also provide a reasonable balance between safety and compliance with COLREGS rules.

6.4. Simulated and Experimental Validations of the Proposed SPAE Controller

To validate the proposed SPAE controller experimentally, the passive crossing scenario shown in Figure 10 was selected as a collision-avoidance example and carried out to provide the collision-free desired heading ${\psi }_c$ and surge speed $u_c$ for the proposed controller to realize. The experiments were conducted at the offshore seaside of Hong Kong University of Science and Technology (HKUST), as shown in Figure 15. The experimental collision-free desired heading ${\psi }_c$ and surge speed $u_c$ were adopted for the simulated validations of the proposed SPAE controller. In particular, the nonlinear positioning control method presented in [44] was conducted to provide the estimated time-varying environmental disturbances that were considered in the simulations.

The simulated and experimental plane motions are shown in Figure 16. In this case, a virtual moving vehicle was created on the ocean surface. It had a constant moving forward speed of 0.4 m/s and came from the USV’s port side. The experimental horizontal trajectory demonstrated that the USV took a collision-avoidance action from the moving vehicle’s stern. The yaw-track heading errors and surge-track velocity errors are plotted in the right subplots of Figure 16. Simulated and experimental yaw-track results are shown in Figure 17. Before 18 seconds, we can observe that the vehicle yaw heading had chattering responses due to the switches between a potential collision and a collision-free situation. As demonstrated in Figure 5, the decreasing yaw heading indicated that the USV speed was within the TVO area such that an anticlockwise yaw rotation was needed to avoid the obstacle. Conversely, the increasing yaw heading indicated that the USV speed was out of the TVO area. Clockwise yaw rotation was taken to move closer to the preset path-following trajectory. The yaw-track heading error and its angular velocity error were used to calculate the planned angular acceleration ${\ddot{\psi }}_p$ (blue dashed line). The yaw uncertainty term was estimated by using the planned angular acceleration ${\ddot{\psi }}_p$ and the feedback-estimated angular acceleration $\ddot{\psi }$ (black solid line). With the yaw control inputs, we can observe that the feedback-estimated angular acceleration $\ddot{\psi }$, the planned angular acceleration ${\ddot{\psi }}_c$, and the collision-free angular acceleration ${\ddot{\psi }}_c$ (black solid line) had a good agreement with each other, especially in simulations. The north-east state estimations presented in Figure 18 were used to calculate the surge velocity and acceleration on the basis of USV’s kinematic Equation (16). In Figure 19, similarly, the surge-track velocity error was adopted to calculate the planned surge acceleration ${\dot{u}}_p$ (blue dashed line). We can also observe that the feedback-estimated surge acceleration $\dot{u}$, the planned acceleration ${\dot{u}}_p$, and the collision-free acceleration ${\dot{u}}_c$ (black solid line) had a good agreement with each other, especially in simulations. Overall, accurate yaw-track and surge-track control performances were obtained by using the proposed controller since system lower-order states are actually the integration of the system’s acceleration.

7. Conclusions

Firstly, this paper has developed an emergency collision-avoidance method for unmanned surface vehicles with the COLREGS rules flexibly obeyed. The proposed collision-avoidance method inherits the real-time reaction characteristics of the velocity obstacle (TVO) algorithm. Its main feature is to greatly encourage the USV to follow the COLREGS rules based on an expansion of a truncated velocity obstacle (TVO). From a security standpoint, it is not mandatory for the USV to obey the COLREGS rules when it has a high collision risk with other ships. With only the velocity uncertainty of the target ship considered in the VTO’s expansion, uniform collision avoidance tasks can be carried out regardless of the emergency level. In sum, the proposed algorithm can efficiently handle multiple obstacles by providing quantitative solutions in different scenarios, including passive and active avoiding actions.

With the collision-free yaw heading and surge velocity as an input, secondly, a discrete simultaneous planning and executing (SPAE) controller has been developed to promptly realize these assigned objectives. All selected error state constraints are satisfied via an online planned polynomial so that a planned acceleration can be promptly generated for control execution. Based on a well-estimated uncertainty term, the control law that is formulated as an algebraic formulation can accurately execute the system acceleration to track the planned acceleration, which ensures tracking performance. Only a control gain is introduced in the controller design, and there is no need to fine-tune this control gain. The developed control framework demonstrates that it is easy for readers to understand it and carry it out. The main results have shown remarkable collision-avoidance performance under both passive and active collision-avoidance modes. Both simulations and experiments have been carried out to verify the proposed controller design.

In collision risk assignment, the collision risk index (CRI) was calculated as the linear combination of the DCPS and TCPA according to the research work in this field. However, more factors may affect the calculation of CRI, such as a vehicle’s maneuvering capacity and the reliability of the perception system. For further investigation, it would be interesting to develop a collision-avoidance method that can take into account the maneuvering capacity of USV and the reliability of an onboard perception system.