USV Dynamic Accurate Obstacle Avoidance Based on Improved Velocity Obstacle Method

Abstract

Unmanned surface vehicle (USV) path planning is a crucial technology for achieving USV autonomous navigation. Under global path planning, dynamic local obstacle avoidance has become the primary focus for safe USV navigation. In this study, a USV autonomous dynamic obstacle avoidance method based on the enhanced velocity obstacle method is proposed in order to achieve path replanning. Through further analysis of obstacles, the obstacle geometric model set in the conventional velocity obstacle method was redefined. A special triangular obstacle geometric model was proposed to reconstruct the velocity obstacle region. The collision time was predicted by fitting the previously gathered data to the detected obstacle’s distance, azimuth, and other relevant data. Then, it is combined with the collision risk to determine when obstacle avoidance should begin and end. In order to ensure safe driving between path points, the international maritime collision avoidance rules (COLREGs) are incorporated to ensure the accuracy of obstacle avoidance. Finally, through numerical simulations of various collision scenarios, it was determined that, under the assumption of ensuring a safe encounter distance, the maximum change rates of USV heading angle are optimized by 17.54%, 58.16%, and 28.63% when crossing, head-on, and overtaking, respectively. The results indicate that, by optimizing the heading angle, the enhanced velocity obstacle method can avoid the risk of ship rollover caused by an excessive heading angle during high-speed movement and achieve more accurate obstacle avoidance action in the event of a safety encounter.

1. Introduction

With the advancement of science and technology, an increasing number of USVs have been utilized in a variety of scientific experiments, civil fields, and military fields, including marine environment monitoring, unmanned submersible recovery, search and rescue in complex waters, and military defense. The USV path planning method must be safe, reliable, and precise in order for the USV to perform complex surface missions.

Local dynamic obstacle avoidance is the key technology of USV autonomous navigation and a crucial issue that must be resolved [1]. Many domestic and international researchers have developed and proposed many local path planning algorithms. Common algorithms consist of the artificial potential field method [2,3,4], the dynamic window method [5,6], the velocity obstacle method [7], the vector field histogram method, the fast random tree method, fuzzy logic [8,9], the set-based guidance (SBG) method [10,11,12,13,14,15], etc. Among them, velocity obstacle (VO) is a local path planning method first proposed by Fiorini in 1998 [16]. Due to its rapid response, high real-time performance, strong robustness, and intelligent obstacle avoidance, the velocity obstacle method has been widely adopted for the autonomous obstacle avoidance of various types of underwater robots. Dubey and Rahul proposed a hybrid technology that combines velocity obstacle (VO) and rapid exploration of random trees (RRT) in order to generate the safe trajectory of autonomous surface ships. By combining these two methods, the joint forward simulation guarantees that the generated path remains effective and collision-free as the situation changes [17]. Huang et al. proposed a collision avoidance algorithm based on the application of the generalized velocity obstacle (GVO) algorithm to ship collision protection, taking ship dynamics into consideration. They developed the GVO algorithm considering ship dynamics to deal with the near-distance multi-ship scene at sea [18]. However, the effect of model error and environmental disturbance on the performance of the GVO algorithm, as well as the effect of ship shape, are not taken into account. Zhang et al. applied the velocity obstacle method to the avoidance of dynamic obstacles by USV [19]. The ellipse is used to represent the USV and the obstacle, and the virtual obstacle is used to reduce the influence of error in obstacle motion information on the obstacle avoidance process. However, it does not use COLREGs. There is a high risk of collision if the unmanned ship on the surface chooses to avoid the obstacle in front of the dynamic obstacle when it encounters a dynamic obstacle. Xu et al. proposed an intelligent hybrid collision avoidance algorithm based on deep reinforcement learning, as well as a preferential sampling mechanism with cumulative pruning [20]. However, there are limitations to the circular extension of all obstacles. The actual obstacle is not necessarily suitable for expanding into a circle. Wang et al. proposed an autonomous collision avoidance method based on deep reinforcement learning (DRL), in which the reward function includes a collision risk assessment model, responsibility avoidance rules, and interaction rules. The static obstacle’s virtual potential energy is introduced to improve the DRL [21]. However, it is impossible to predict the target ship’s collision avoidance behavior.

This paper aims to solve the problem of dynamic autonomous and precise obstacle avoidance of unmanned surface vehicles in a variety of environments by enhancing the velocity obstacle method, fitting the information gathered in the early stage, and predicting its collision time to determine the timing of starting and ending obstacle avoidance. The aim was also to integrate the international rules for collision avoidance at sea (COLREGs) to ensure that its obstruction avoidance is accurate. Through numerical simulation of various collision scenarios, it was determined that, under the premise of ensuring a safe encounter distance, the maximum change rates of USV heading angle are optimized by 17.54%, 58.16%, and 28.63% when crossing, head-on, and overtaking, respectively. The results show that, by optimizing the heading angle, the improved velocity obstacle method can avoid the danger of ship rollover during high-speed maneuvering and achieve more accurate obstacle avoidance action in the event of a safety encounter.

2. Improved Velocity Obstacle Method

2.1. Improved Dynamic Ship Collision Domain

The conventional velocity obstacle method reduces the irregularly moving obstacle object to a circle on the plane and the moving object to a particle. At the same time, it is assumed that the dynamic obstacles follow a specific trajectory. The instantaneous state (position and velocity) of dynamic obstacles is known or predictable at a specific time node [16,22].

As shown in Figure 1a, in the conventional velocity obstacle method, the dynamic obstacle is uniformly expanded into a circle with a radius of $R$, and then two tangents of ${\lambda }_1$ and ${\lambda }_2$ are made to the expanded circle through the particle of the moving object. Then, the set of relative velocity $V_{uo}$ is defined as the relative collision area of $RC{C}_{uo}$. The structure of this area is closely related to the relative size and shape of the dynamic obstacle.

Through the study of dynamic obstacles on the sea, various types of ships pose the greatest threat to USV. Thus, various types of ships are the main research objects. In the conventional speed obstacle method, all dynamic obstacles are expanded into a circle with a radius $R$. Thus, the included angle formed by ${\lambda }_1$ and ${\lambda }_2$ in any direction is the same, that is, the same relative collision area $RC{C}_{uo}$ will be obtained. In fact, due to the different lengths and widths of the hull, the included angles formed in the different directions ${\lambda }_1$ and ${\lambda }_2$ are different, and the obtained relative collision area $RC{C}_{uo}$ is also different. Therefore, due to the inaccuracy of $RC{C}_{uo}$, it may occur in the actual collision avoidance process that obstacle avoidance is performed when obstacle avoidance is not required, and large-angle obstacle avoidance is performed when only a small angle is required.

To solve the aforementioned issues, it is necessary to design a geometric expansion region that can maximally overcome the actual dynamic obstacles. Since the overall shape of the ship is more similar to that of a triangle, a special isosceles triangle is proposed as the geometric expansion region of the dynamic obstacle, which is more consistent with the actual shape of the dynamic obstacle. Further research reveals that the length–width ratio of ordinary transport ships is 7:1~9:1, the length–width ratio of ice breakers is 4:1~6:1, and the length–width ratio of small high-speed transport boats is 3:1~4:1. Therefore, the ratio of height ($h$) to base-side ($d$) of the geometric expansion region of the special isosceles triangle is defined as 2:1 ($h=2d$), which corresponds to the actual length and width of the dynamic obstacle, respectively. In the actual process of obstacle avoidance, the value of the bottom edge is derived from the high value. Since the defined ratio is 2:1, the value of the bottom edge in the geometric expansion area of different ship types will exceed the actual ship width. Therefore, the additional range can withstand the interference of uncertain factors.

As shown in Figure 1b, it is assumed that $P_u$ is the center of a moving object with a velocity of $V_u$ and length of $L_u$. Assuming that $P_o$ is the center of the dynamic obstacle, the velocity is $V_o$ and the length is $L_o$ [23]. Assuming that the current time is $T$, the $P_u$ of the moving object is abstracted into a point, and the $P_o$ of the dynamic obstacle is expanded into an isosceles triangle with height ($h=L_u+L_o$) twice the bottom. In this figure, ${\lambda }_1$ and ${\lambda }_2$ and are two rays emitted by $P_u$ passing through the two vertices of an isosceles triangle formed with $P_o$ as the center.

The relative velocity of a moving object $P_u$ and a dynamic obstacle $P_o$ is expressed by $V_{uo}$. The mathematical formula is as follows:

$$V_{uo}=V_u-V_o$$

(1)

As shown in the figure, starting from the center point of the moving object $P_u$ and making a ray along the relative velocity $V_{uo}$, it is defined as:

$$\lambda \left(P_u,V_{uo}\right)=\left\{P_u+V_{uo}t|t\ge 0\right\}$$

(2)

where $t$ represents the time of motion. Then, a ray $\lambda (P_u,V_{uo})$ made along the relative velocity $V_{uo}$ represents the real-time relative position between the moving object $P_u$ and the dynamic obstacle $P_o$ at time $t$.

The set of the relative velocity $V_{uo}$ of moving object $P_u$ and dynamic obstacle $P_o$ is defined as the relative collision region of $RC{C}_{uo}$ (relative collision cone). Its mathematical formula is defined as follows:

$$RC{C}_{uo}=\left\{V_{uo}|\lambda \left(P_u,V_{uo}\right)\cap O_{P_O}\ne \varnothing \right\}$$

(3)

where $O_{P_O}$ represents the special triangular area expanded by the dynamic obstacle $P_o$.

As shown in the figure, if the current direction and speed of the moving object $P_u$ and the dynamic obstacle $P_o$ remain unchanged during the movement, and as long as the relative speed $V_{uo}$ falls within the fan-shaped relative collision area $RC{C}_{uo}$, the moving object and the dynamic obstacle will collide. In contrast, any relative velocity $V_{uo}$ that falls outside the sector will not cause a collision between the two objects.

2.2. Selection of Rays Forming Relative Collision Regions

Since the set of relative velocity $V_{uo}$ is the relative collision region of $RC{C}_{uo}$, it is composed of two rays ${\lambda }_1$ and ${\lambda }_2$ passing through two vertices of the isosceles triangle. Therefore, the correct two rays can be obtained by selecting the passing vertex.

As shown in Figure 2a, three vertices are obtained by the center point $P_o(x_o,y_o)$ of a special triangle. The formula is as follows:

$$A\left(x_a,y_a\right):\left\{\begin{array}{c}{x}_a=x_o\pm \frac{h}{2}\cos {\theta }_A\\ y_a=y_o\pm \frac{h}{2}\sin {\theta }_A\end{array}\right.$$

(4)

$$B\left(x_b,y_b\right):\left\{\begin{array}{c}{x}_b=x_o\pm \frac{\sqrt{5}h}{4}\cos {\theta }_B\\ y_b=y_o\pm \frac{\sqrt{5}h}{4}\sin {\theta }_B\end{array}\right.$$

(5)

$$C\left(x_c,y_c\right):\left\{\begin{array}{c}{x}_c=x_o\pm \frac{\sqrt{5}h}{4}\cos {\theta }_C\\ y_c=y_o\pm \frac{\sqrt{5}h}{4}\sin {\theta }_C\end{array}\right.$$

(6)

where ${\theta }_A$, ${\theta }_B$, and ${\theta }_C$ are the angle between $P_{o}A$, $P_{o}B$, $P_{o}C$, and the X axis. $\alpha$ is the angle between $P_{o}A$ and the positive half axis of X, representing the direction angle of the obstacle ship. ‘+, −’ is related to the size of $\alpha$ angle. $h$ represents the height of the triangle.

The two vertices that make up the relative collision area of $RC{C}_{uo}$ are selected by the angle $\beta (0\le \beta \le 2\pi )$ between the center point $P_u$ of the USV and the center point $P_o$ of the obstacle and the direction angle $\alpha (0\le \alpha \le 2\pi )$ of the triangle.

As shown in Figure 2a,b, the correct vertex is selected by the following conditions:

$$\gamma =\left\{\begin{array}{c}\beta -\alpha \\ \beta +2\pi -\alpha \end{array}\right.\begin{array}{c},\beta -\alpha \ge 0\\ ,\beta -\alpha <0\end{array}$$

(7)

According to the range of γ in

Table 1. Selection of two vertices through which the ray passes.

$\mathbf{Range} \mathbf{of} \mathit{\gamma }$	The Two Vertices the Ray Passes through
$\gamma \in \left\{\begin{array}{l}\left[0,\arctan \frac{1}{4}\right] \hfill \\ \left(\arctan \frac{1}{4},\arctan \frac{1}{2}\right) and D\left(P_o,P_u\right)<D\left(P_o,W_{AC}\right)\hfill \\ \pi \hfill \\ \left(2\pi -\arctan \frac{1}{2},2\pi -\arctan \frac{1}{4}\right) and D\left(P_o,P_u\right)<D\left(P_o,W_{AB}\right)\hfill \\ \left[2\pi -\arctan \frac{1}{4},2\pi \right]\hfill \end{array}\right.$	B and C
$\gamma \in \left\{\begin{array}{l}\left(\arctan \frac{1}{4},\arctan \frac{1}{2}\right) and D\left(P_o,P_u\right)\ge D\left(P_o,W_{AC}\right)\hfill \\ \left[\arctan \frac{1}{2},\frac{\pi }{2}\right] and D\left(P_o,P_u\right)\ge D\left(P_o,W_{BC}\right)\hfill \\ \left(\pi ,\frac{3\pi }{2}\right)\hfill \\ \left[\frac{3\pi }{2},2\pi -\arctan \frac{1}{2}\right] and D\left(P_o,P_u\right)<D\left(P_o,W_{CB}\right)\hfill \end{array}\right.$	A and B
$\gamma \in \left\{\begin{array}{l}\left[\arctan \frac{1}{2},\frac{\pi }{2}\right] and D\left(P_o,P_u\right)<D\left(P_o,W_{BC}\right)\hfill \\ \left(\frac{\pi }{2},\pi \right)\hfill \\ \left[\frac{3\pi }{2},2\pi -\arctan \frac{1}{2}\right] and D\left(P_o,P_u\right)\ge D\left(P_o,W_{CB}\right)\hfill \\ \left(2\pi -\arctan \frac{1}{2},2\pi -\arctan \frac{1}{4}\right) and D\left(P_o,P_u\right)\ge D\left(P_o,W_{AB}\right)\hfill \end{array}\right.$	A and C

Where $P_u$ represents the center point of USV, $P_o$ is the center point of obstacles, $W_{AC}$ is the intersection point of straight AC and straight $P_u{P}_o$, $W_{BC}$ represents the intersection point of straight BC and straight $P_u{P}_o$, $W_{CB}$ denotes the intersection point of straight CB and straight $P_u{P}_o$, and $W_{AB}$ is the intersection point of straight AB and straight $P_u{P}_o$. $D(P_o,P_u)$ is the distance between point $P_o$ and point $P_u$, which is known by other similar theories. $l_1$ and $l_4$ are parallel to BC, $l_2$ is parallel to AC, and $l_3$ is parallel to AB.

, determine the vertex through which the two rays pass.

3. Collision Time Prediction and Obstacle Avoidance Timing Judgment

This paper performs polynomial fitting of the collected data using the least squares method and predicts the collision time based on the detected dynamic obstacle distance and other relevant information. It is then combined with the collision risk to determine the timing of starting and ending obstacle avoidance.

3.1. Collision Time Prediction

In this paper, the sum of squares of errors ${\sum }_{i=0}^m{r}_i^2$ is often used in curve fitting to calculate the size of the error $r_i=P(x_i)-y_i$ between a given point $(x_i,y_i) (i=0,1,\dots ,m)$ and the approximate function $P(x)$.

Through known points (x_i,y_i), in the given function class Φ, find P(x)∈Φ, and minimize the sum of squares of the error r_i = P(x_i) − y_i. The formula is as follows:

$${\displaystyle \sum _{i=0}^m}{\displaystyle r_i^2}={\displaystyle \sum _{i=0}^m}{\left[P\right(x_i)-y_i]}^2$$

(8)

The objective is to find a curve y = p(x) with the smallest sum of squares of distances to a given point (x_i,y_i) geometrically. The fitting function or least square solution is known as the P(x) function, and the least square technique of curve fitting is known as the least square method of curve fitting [24].

For known points (x_i,y_i), Φ is a set of functions made up of polynomials with a degree of no more than n. $P_n(x)={\sum }_{k=0}^n{a}_k{x}^k\in \mathsf{\Phi }$ is calculated, and the following Formula (9) is obtained:

$$I={\displaystyle \sum _{i=0}^m{\left[p(x_i)-y_i\right]}^2}={\displaystyle \sum _{i=0}^m{\left({\displaystyle \sum _{k=0}^n{a}_k{x}_i^k-y_i}\right)}^2}$$

(9)

Polynomial fitting is used when the fitting function is polynomial, and the least square fitting polynomial that satisfies Equation (10) is known as the least square fitting polynomial. If $n$ = 1, this is known as linear fitting [25].

$$I={{\displaystyle \sum _{i=0}^m\left({\displaystyle \sum _{k=0}^n{a}_k{x}_i^k-y_i}\right)}}^2$$

(10)

The aforementioned issue is the extreme value problem of $I=I(a_0,a_1,\dots ,a_n)$, since it is a multivariate function of $a_0,a_1,\dots ,a_n$. Through the necessary conditions for solving the extreme value using a multivariable function, the following formulas are obtained:

$$\frac{\partial I}{\partial a_j}=2{\displaystyle \sum _{i=0}^m\left({\displaystyle \sum _{k=0}^n{a}_k{x}_i^k-y_i}\right)}{x}_i^j=0\left(j=0,1,\dots ,n\right)$$

(11)

$${\displaystyle \sum _{k=0}^n\left({\displaystyle \sum _{i=0}^m{x}_i^{j+k}}\right)}{a}_k={\displaystyle \sum _{i=1}^n{x}_i^j}{y}_i\left( j=0,1,\dots ,n\right)$$

(12)

Formula (12) is a linear system of equations about a₀, a₁, …, a_n. Use matrix representation:

$$\left[\begin{array}{cccc}m+1& {\displaystyle \sum _{i=0}^m{x}_i}& \cdots & {\displaystyle \sum _{i=0}^m{x}_i^n}\\ {\displaystyle \sum _{i=0}^m{x}_i}& {\displaystyle \sum _{i=0}^m{x}_i^2}& \cdots & {\displaystyle \sum _{i=0}^m{x}_i^{n+1}}\\ ⋮& ⋮& \ddots & ⋮\\ {\displaystyle \sum _{i=0}^m{x}_i^n}& {\displaystyle \sum _{i=0}^m{x}_i^{n+1}}& \cdots & {\displaystyle \sum _{i=0}^m{x}_i^{2n}}\end{array}\right]\left[\begin{array}{c}{a}_0\\ a_1\\ ⋮\\ a_n\end{array}\right]=\left[\begin{array}{c}{\displaystyle \sum _{i=0}^m{y}_i}\\ {\displaystyle \sum _{i=0}^m{y}_i{x}_i}\\ ⋮\\ {\displaystyle \sum _{i=0}^m{x}_i^n{y}_i}\end{array}\right]$$

(13)

Using the inverse proof technique, it is demonstrated that the system of Equation (13) has a unique solution. It is assumed that the coefficient matrix of the equation set (13) is singular and that the homogeneous equation set corresponding to it has a has a non-zero solution. It can be represented by the following formula:

$${\displaystyle \sum _{k=0}^n\left({\displaystyle \sum _{i=0}^m{x}_i^{j+k}}\right)}{a}_k=0\left(j=0,1,\dots ,n\right)$$

(14)

Multiply the $j$-th equation in Formula (14) by $a_j\left(j=0,1,\dots ,n\right)$, and then add the left and right ends of the newly obtained $n+1$ equations, respectively, to obtain ${\displaystyle \sum _{j=0}^n{a}_j\left[{\displaystyle \sum _{k=0}^n\left({\displaystyle \sum _{i=0}^m{x}_i^{j+k}}\right)}{a}_k\right]}=0$. Because ${\displaystyle \sum _{j=0}^n{a}_j\left[{\displaystyle \sum _{k=0}^n\left({\displaystyle \sum _{i=0}^m{x}_i^{j+k}}\right)}{a}_k\right]}={\displaystyle \sum _{i=0}^m{\displaystyle \sum _{j=0}^n{\displaystyle \sum _{k=0}^n{a}_k{a}_j{x}_i^{j+k}}}}={\displaystyle \sum _{i=0}^m\left({\displaystyle \sum _{j=0}^n{a}_j{x}_i^j}\right)\left({\displaystyle \sum _{k=0}^n{a}_k{x}_i^k}\right)}={\displaystyle \sum _{i=0}^m{{\displaystyle \left[p_n\left(x_i\right)\right]}}^2}$, in the formula $P_n(x)={\displaystyle \sum _{k=0}^n{a}_k{x}^k}$, there is $P_n\left({{\displaystyle x}}_i\right)=0\left(i=0,1,\cdots ,m\right)$. $P_n\left({{\displaystyle x}}_i\right)$ is a polynomial of a degree no more than n. It has $m+1>n$ different zeros, and $a_0=a_1=\dots =a_n=0$ can be calculated from the basic algebra theorem. It contradicts the hypothesis that the homogeneous equation has a non-zero solution. The normal Equation (13) must, therefore, have a unique solution.

Therefore, $a_k (k=0,1,\dots ,n)$ can be solved using equation set (13), and polynomial (15) can be obtained.

$$P_n(x)={\displaystyle \sum _{k=0}^n{a}_k{x}^k}$$

(15)

The polynomial fitting and prediction steps are as follows:

①

When the USV detects an obstacle, the onboard sensor measures the distance between the USV and the obstacle ship in real time, and sample data with time and distance as variables are obtained. At the same time, the degree n of the fitting polynomial is determined;

➁

List calculation ${\displaystyle \sum _{i=0}^m{x}_i^j}$ and ${\displaystyle \sum _{i=0}^m{x}_i^j}{y}_i (j=0,1,\dots ,2n)$;

➂

Write the equations and find $a_0,a_1,\dots ,a_n$;

➃

Obtain the fitting polynomial $P(x)=a_0+a_{1}x+\dots +a_n{x}^n$;

➄

Calculate the maximum distance $D_{\max }$ from the center of the expansion triangle to the boundary as the minimum collision distance between the USV and the obstacle ship;

➅

Bring the known $D_{\max }$ is into the curve equation to solve the collision time $T_z$;

➆

The collision time T_Z can be predicted in real time according to the distance change between the USV and the obstacle ship during the navigation.

3.2. Start and End Obstacle Avoidance Timing Judgment

Through the velocity obstacle method, it is possible to predict whether the USV and the dynamic obstacle will collide at a future time, but it is not possible to predict the timing of the beginning and ending obstacle avoidance. Therefore, the timing of obstacle avoidance is determined by combining the velocity obstacle method and the collision risk [26,27].

The collision point between USV and the expansion geometric region of dynamic obstacles is defined as CPA, the distance between USV and CPA is defined as $D_{CPA}$ at a certain moment, and the time is defined as $T_{CPA}$.

$$D_{CPA}=D_{uo}-D_s$$

(16)

As in Formula (16), $D_{uo}$ is the distance between USV and dynamic obstacles at a certain time, and $D_s$ is the distance from the expanded special triangle center to the triangle boundary.

$$T_{CPA}=T_Z-T_d$$

(17)

According to Formula (17), $T_Z$ is the collision time predicted by the least square fitting polynomial and $T_d$ represents the current time of USV.

$D$ is the relative distance between the USV and the dynamic obstacle, and is defined as follows:

$$D=\sqrt{{{\displaystyle \left({{\displaystyle x}}_o-{{\displaystyle x}}_u\right)}}^2+{{\displaystyle \left({{\displaystyle y}}_o-{{\displaystyle y}}_u\right)}}^2}$$

(18)

Definition $B$ is the relative azimuth angle between the USV and the dynamic obstacle as follows:

$$B=\beta -\alpha$$

(19)

$K$ is defined as the speed ratio of USV to the dynamic obstacle as follows:

$$K={{\displaystyle V}}_o/{{\displaystyle V}}_u$$

(20)

When USV finds dynamic obstacles, the collision risk assessment set is established as $\{D_{CPA},T_{CPA},D,B,K\}$. The hazard membership functions are ${\mu }_d(D_{CPA})$, ${\mu }_t(T_{CPA})$, ${\mu }_D(D)$, μ_B(B), and ${\mu }_K(K)$:

$${\mu }_d(D_{CPA})=\left\{\begin{array}{ll}0& ,D_{CPA}>d_2\\ \frac{1}{2}-\frac{1}{2}\sin \left[\frac{\pi }{d_1-d_2}\left(D_{CPA}-\frac{d_1+d_2}{2}\right)\right]& ,d_1<D_{CPA}\le d_2\\ 1& ,D_{CPA}\le d_1\end{array}\right.$$

(21)

$${\mu }_t(T_{CPA})=\left\{\begin{array}{ll}0& ,\left|T_{CPA}\right|>t_2\\ \left(\frac{t_2-\left|T_{CPA}\right|}{t_2-t_1}\right)& ,t_1<\left|T_{CPA}\right|\le t_2\\ 1& ,\left|T_{CPA}\right|\le t_1\end{array}\right.$$

(22)

$${\mu }_D(D)=\left\{\begin{array}{ll}1& ,0\le D\le D_1\\ \left(\frac{D_2-D}{D_2-D_1}\right)& ,D_1<D\le D_2\\ 0& ,D_2<D\end{array}\right.$$

(23)

$${\mu }_B(B)=\frac{1}{2}\left[\cos \left(B-{{\displaystyle 19}}^{°}\right)+\sqrt{\frac{440}{289}+{\cos }^2\left(B-{{\displaystyle 19}}^{°}\right)}\right]-\frac{5}{17} , \left(0\le B<{{\displaystyle 360}}^{°}\right)$$

(24)

$${\mu }_K(K)=\frac{1}{1+\frac{2}{K\sqrt{{{\displaystyle K}}^2+1+2K\sin C}}} , \left(K\ge 0\right)$$

(25)

In Equations (21)–(25), $d_1$ and $d_2$ are the range of the distance to the nearest encounter point, $t_1$ and $t_2$ are the time interval to the nearest encounter point, $D_1$ represents the latest starting obstacle avoidance distance, D₂ is the earliest starting obstacle avoidance distance, and $C$ is the collision angle between the USV and the dynamic obstacle.

Each indicator has different impacts on the collision risk. The collision risk assessment set ${\mu }_{dtDBK}$ can be obtained by combining the weighting coefficient with the risk membership function:

$${\mu }_{dtDBK}=k_1{\mu }_d(D_{CPA})+k_2{\mu }_t(T_{CPA})+k_3{\mu }_D(D)+k_4{\mu }_B(B)+k_5{\mu }_K(K),(k_1+k_2+k_3+k_4+k_5=1)$$

(26)

Through a large number of data statistics, and in combination with the weighting parameter calculation method in reference [28], the specific weighting coefficients can be calculated as $k_1=0.41$, $k_2=0.35$, $k_3=0.18$, $k_4=0.04$, and $k_5=0.02$.

At this point, the opportunity to start and end obstacle avoidance is determined by:

First, the collision distance $D_{CPA}$ between USV and obstacles is less than the set safe distance $D_s$.

Second, the relative velocity $V_{uo}$ between USV and obstacles is located in the relative collision area $RC{C}_{uo}$.

Third, when the collision risk is evaluated as high risk, i.e., when ${\mu }_{dtDBA}>0.5$.

When the USV decision-making system needs to meet the above three conditions in the actual obstacle avoidance process, the obstacle avoidance can be terminated if it is not satisfied.

3.3. International Regulations for Preventing Collisions at Sea (COLREGS)

According to the COLREGs collision rules, when there is a collision risk between two ships and one of the ships must be avoided, the encounter scenarios must be classified [29,30]. It is primarily divided according to rules 13 to 15 in COLREGs. These rules define three distinct encounter scenarios: overtaking, head-on, and crossing. To ensure the safety of maritime navigation, the relative azimuth of the USV and obstacle ship is used to distinguish and define ship encounters. The formula for calculating the relative azimuth angle is shown in Formula (27).

$$\omega ={\tan }^{-1}\left(\frac{y_{pu}(t_k)-y_{po}(t_k)}{x_{pu}(t_k)-x_{po}(t_k)}\right)-{\psi }_{po}(t_k)$$

(27)

where $x_{pu}(t_k)$ and $y_{pu}(t_k)$ denote the co-ordinate position of USV at $t_k$, $x_{po}(t_k)$ and $y_{po}(t_k)$ denote the co-ordinate position of obstacle ship at $t_k$, and ${\psi }_{po}(t_k)$ represents the heading angle of obstacle ship at $t_k$.

As shown in Figure 3 and Figure 4, it follows the relevant rules of COLREGs combined with the relative azimuth angle to determine the encounter situations of ships:

Rule 13 (overtaking): when $\omega \in [{112.5}^{°},{247.5}^{°}]$, it is judged that the encounter scenario is overtaking. In this case, the speed and direction of the obstacle ship remain unchanged, the USV right rudder passes the obstacle ship, or the left rudder passes the obstacle ship.

Rule 14 (head-on): when $\omega \in [0^{°},{15}^{°}]\cup [{345}^{°},{360}^{°}]$, it is judged that the encounter scene is head-on. In this case, the USV right rudder passes through the obstacle ship.

Rule 15 (crossing): when $\omega \in [{15}^{°},{112.5}^{°}]\cup [{247.5}^{°},{345}^{°}]$, the encounter scenario is judged to be (left and right) cross. When the obstacle ship is on the USV’s starboard side, the right rudder of the USV steers away, while the obstacle ship maintains its original driving posture. When the obstacle ship is on the USV’s starboard side, the USV maintains its original driving position, and the obstacle ship’s right rudder steers away.

4. Simulation and Discussion

Simulation validates the improved velocity obstacle method proposed in this paper. To verify and compare the accuracy of the algorithm, several different simulation experiments are carried out. All experiments are based on USV detection devices that can detect moving obstacles from varying distances and directions. According to the actual size of the ship, the length of the obstacle ship is set to 16 m and the length of the USV is set to 6 m. The traditional velocity obstacle method abstracts the obstacle ship as a circle with a 22-m diameter, whereas the improved velocity obstacle method abstracts the obstacle as an isosceles triangle with a 22-m height and an 11-m base. To obtain valid comparison results, the obstacle avoidance speed of the USV is held constant.

4.1. Simulation Study of Predicting Obstacle Avoidance

As shown in Figure 5, a dynamic obstacle avoidance simulation is carried out for an obstacle vessel with variable speed and direction. By obtaining the real-time distance between the USV and the obstacle vessel, the collision time $T_Z$ is measured using the least square fitting polynomial when the obstacle avoidance scheme is not adopted. Then, the real-time $T_{CPA}$ is obtained by subtracting the current time $T_d$ of the USV. Through the obtained $T_{CPA}$, the hazard membership function is established to determine the timing of starting and ending obstacle avoidance.

As shown in Figure 5d, collision time $T_Z$ can be predicted in real time by collecting distance information before obstacle avoidance. Combined with (a), (d), and (c), this method can accurately implement obstacle avoidance for obstacle ships with variable speed and variable direction, where the minimum distance between the two ships is greater than the safe distance.

4.2. Comparison of Accurate Implementation of Obstacle Avoidance Measures

To verify that the improved velocity obstacle method can be a more accurate implementation of obstacle avoidance measures, the traditional velocity obstacle method and the improved velocity obstacle method are compared under identical conditions in terms of their implementation of obstacle avoidance measures.

As shown in Figure 6, the beginning point co-ordinate of the USV is (155, 20), the ending point co-ordinate is (175, 240), and the velocity is 3 m/s. The starting co-ordinates of obstacles are (155, 220), the ending co-ordinates are (155, 80), and the speed is 2 m/s. It can be found through experiments that the improved velocity obstacle method in Figure 6a determines that there is no collision danger at this time, and no collision avoidance measures are implemented. The conventional velocity obstacle method depicted in Figure 6b determines that a collision hazard exists at the current time and implements collision avoidance measures. By comparing the relative distances in Figure 6c, it can be determined that the center distance between the USV and the obstacle remains above the safe distance, indicating that no obstacle avoidance measures are necessary at this time. This demonstrates that the improved velocity obstacle method is more accurate for evaluating the implementation of collision avoidance measures.

4.3. Comparison of Obstacle Avoidance Simulation for Single-Ship and Multi-Scenario Encounter

Three distinct encounter scenarios are defined based on the COLREG collision rules for the single-ship encounter simulation experiment: overtaking, head-on, and crossing. According to the avoidance rule, the ship does not need to avoid the left cross-meeting scenario; it can simply maintain its original state of navigation. The obstacle avoidance in the scenarios of crossing from the right (Figure 7), head-on (Figure 8), and overtaking (Figure 9) is simulated, respectively.

Comparing and analyzing the experimental data in

Table 2. Simulation data of traditional VO for a single ship.

Simulation Scene	Vessel	Velocity (m/s)	Heading Angle (rad)	Starting Point	Diameter of Expansion Circle (m)	Minimum Distance (m)	USV Maximum Steering Angle (rad)	Safety Distance (m)
Crossing	USV	3	0.9109	(83, 55)	22	15.32	0.5233	11
	Obstacle	3	2.9997	(205, 115)
Head-on	USV	3	1.5708	(100, 20)	22	18.41	0.4343	5.5
	Obstacle	2	4.7124	(100, 220)
Overtaking	USV	4	0.7854	(30, 30)	22	16.45	0.3308	11
	Obstacle	1	0.7854	(80, 80)

,

Table 3. Simulation data of improved VO for a single ship.

Simulation Scene	Vessel	Velocity (m/s)	Heading Angle (rad)	Starting Point	Expanded Triangle Height (m)	Minimum Distance (m)	USV Maximum Steering Angle (rad)	Safety Distance (m)
Crossing	USV	3	0.9109	(83, 85)	22	13.98	0.4315	11
	Obstacle	3	2.9997	(205, 115)
Head-on	USV	3	1.5708	(110, 20)	22	7.77	0.1817	5.5
	Obstacle	2	4.7124	(100, 220)
Overtaking	USV	4	0.7854	(30, 30)	22	13.29	0.2361	11
	Obstacle	1	0.7854	(80, 80)

and

Table 4. Optimization rate of steering angle in obstacle avoidance.

Simulation Scene	USV Maximum Steering Angle (rad)		Optimization Rate
	Traditional VO	Improved VO
Crossing	0.5233	0.4315	17.54%
Head-on	0.4343	0.1817	58.16%
Overtaking	0.3308	0.2361	28.63%

reveals that, assuming a safe encounter distance, the maximum change rate of USV heading angle is optimized by 17.54%, 58.16%, and 28.63% in crossing, head-on, and overtaking, respectively. The improved velocity obstacle method is clearly superior to the conventional velocity obstacle method. The improved velocity obstacle method can avoid the danger of ship rollover due to excessive heading angle in the process of high-speed motion by optimizing the heading angle, and can realize more accurate obstacle avoidance action in the case of a safe encounter. In addition, the USV that has completed obstacle avoidance no longer returns to the original path but instead plans a new route directly at the obstacle’s end point, which can reduce the USV’s bypass path and ensure that it maintains a greater distance under limited power or fuel.

5. Conclusions

In this paper, the conventional velocity obstacle method is improved. Through further analysis of obstacles, the geometric obstacle model in the traditional velocity obstacle method is redefined. A special triangular obstacle geometric model is proposed for reconstructing the velocity obstacle area. Using the detected obstacle’s distance, azimuth, and other relevant data, the least square fitting polynomial was applied to the previously gathered data to predict the collision time. Then, it was combined with the collision risk to determine when obstacle avoidance should begin and end. Under the assumption of ensuring safe driving between path points, the international maritime collision avoidance rules (COLREGs) are incorporated to ensure the accuracy of obstacle avoidance.

The experimental data indicate that, under the assumption of ensuring a safe encounter distance, the maximum change rate of USV heading angle is optimized by 17.54%, 58.16%, and 28.6% in crossing, head-on, and overtaking, respectively. The enhanced velocity obstacle method is superior to the conventional velocity obstacle method. The improved velocity obstacle method can avoid the danger of ship rollover due to excessive heading angle in the process of high-speed motion by optimizing the heading angle and can realize more accurate obstacle avoidance action in the case of a safe encounter. At the same time, the USV that has completed obstacle avoidance no longer returns to its original path but instead plans a new path directly at the obstacle’s end point, which can reduce the USV’s bypass path and ensure that it maintains a greater distance under limited power or fuel. In the future, it will be determined if the method can effectively avoid dynamic obstacles in confined waters using multi-sensor information fusion, and the stability control method in complex environments will be investigated in conjunction with dynamics.