Local Collision Avoidance Algorithm for a Unmanned Surface Vehicle Based on Steering Maneuver Considering COLREGs

In order to sail safely in a marine environment with neighboring obstacles, a local collision avoidance algorithm based on a steering maneuver considering International Regulations for Preventing Collisions at Sea (COLREGs) is proposed for unmanned surface vehicles (USVs). In this paper, the algorithm consists of three parts: collision risk assessment, steering occasion determination, and navigation waypoint update. The collision risk assessment is used to judge a collision risk based on an analytical feasible angle interval by the closest point of approach method. The steering occasion determination provides a safe distance interval from an obstacle by analyzing a marginal steering angular velocity. The navigation waypoint update generates a temporary waypoint to navigate the USV and considers the overtaking, head-on, and crossing rules of COLREGs. The temporary waypoint is calculated based on a real-time optimal orientation angle, which is determined by an optimization objective function for angle deviation. The three parts for local collision avoidance are determined by a finite state machine. In simulations, a USV with actual dynamic constraints runs successfully in a static and dynamic multiobstacle environment, and the results indicate the feasibility and validity of the proposed algorithm.


I. INTRODUCTION
Research on unmanned surface vehicles (USVs) has received increasing attention because of the rapidly increasing requirements of marine applications in recent years [1]. A robust and reliable guidance, navigation, and control (GNC) system is essential for a USV to undertake complex missions [1]- [3]. One important function of a GNC system is to plan a quasioptimal path while avoiding obstacles safely. Path planning usually includes global and local path planning. The former is based on known map information and is usually performed before the navigation of USVs. The latter is mainly used to avoid obstacles that are unknown prior and can be detected by onboard sensors within a local area.
Researchers have proposed many methods for the local collision avoidance of USVs. There are several types of local The associate editor coordinating the review of this manuscript and approving it for publication was Huiping Li. collision avoidance algorithms, such as the trajectory-based algorithm, behavior-based algorithm, and artificial intelligence algorithm.
In trajectory-based algorithms, appropriate trajectories or waypoints are generated for USVs. Svec et al. [4] utilized a fast heuristic search algorithm to find a trajectory in a space of candidate trajectories. The candidate trajectories are generated using a discretized set of dynamically feasible control actions and their corresponding nondeterministic state transitions. Soltan et al. [5] proposed a method that combines trajectory planning with real-time tracking control based on a sliding mode control law. The trajectory is defined by ordinary differential equations (ODEs), whose solution is the limit cycle. Du et al. [6] used a hydrodynamic model of the USVs to generate a trajectory unit that contains different orbit segments, and then the trajectory unit is searched on a grid map based on waypoint and heading choice rules. However, the shapes of trajectories in the trajectory unit are limited.
Wang et al. [7] constructed a local normal distribution-based trajectory (LNDT) to avoid an obstacle. A set of waypoints with timestamps is extracted from the LNDT for the USV to complete obstacle avoidance by changing its speed and course. One disadvantage is that the adaptability of the trajectory needs to be improved.
In behavior-based algorithms, guidance velocities are generated and realized by the navigation controllers of USVs in real time to avoid collisions. The velocity obstacle (VO) method is a practical approach for determining valid velocities to avoid possible collisions. Fiorini and Shiller [8] introduced the basic theory of the VO method for robot motion planning in a dynamic environment. The universality of this method makes it also suitable for collision avoidance of USVs [9]. Nonlinear VO and probabilistic VO algorithms were introduced in [10] for collision avoidance with target ships whose trajectories are nonlinear and predictable, and a generalized VO algorithm was used to design a collision avoidance system by Huang et al. [11]. Line-of-sight (LOS) guidance and a VO algorithm were modified and applied in [12], and the performance and practical validity of the algorithm were demonstrated by real sea experiments. The VO can be treated as a set of constraints in the velocity space of the USV in order to avoid moving obstacles, and the USV selects a suitable velocity to avoid a collision [9]. Zhang et al. [13] and Tang et al. [14] proposed a local reflection collision avoidance algorithm for high-speed USVs in which the direction is altered by a directional steady-state model and the speed is altered by a translational velocity model. However, this algorithm is only suitable for static obstacle environments.
For artificial intelligence algorithms, two neuroevolutionary methods were used to build a collision avoidance system of USVs in [15]. Deep reinforcement learning algorithms [16], [17] have also recently been proposed for USV collision avoidance. These algorithms can achieve good collision avoidance effects in some situations. However, the main shortcoming is that the algorithm performance depends significantly on the training data.
The main dynamic obstacles on the ocean are ships, and their operations are required to obey the International Regulations for Preventing Collisions at Sea (COLREGs) [18]. Although COLREGs are regulations formulated for manned vessels, some rules can also be applied to USVs, and many studies have considered COLREGs for the collision avoidance of USVs. Benjamin et al. [19] proposed a behavior-based control framework that considers COLREGs with multiobjective optimization and interval programming. Kuwata et al. [9] combined the VO method with COLREGs by considering the three primary situations to navigate USVs safely in dynamic and cluttered environments. COLREGs were also taken into account in the replanning procedure for a rapid dynamic path planning system developed in [20]. Although a collision avoidance algorithm considering COLREGs may not be optimal in performance, it can effectively reduce collision risks when both the USV and the encounter vessel comply with COLREGs. Therefore, COLREGs should be considered in collision avoidance algorithms for USVs.
Most navigation and collision avoidance processes of USVs can be treated as the execution of a sequence of actions. When the actions can be divided into finite categories, a finite state machine (FSM) can be used to organize the process effectively. It is a useful computational model for both hardware and certain types of software [21]. In terms of ocean platform applications, a new programming architecture based on an FSM was presented by Woithe and Kremer [22]. In this FSM-based architecture, the programming model expresses autonomous underwater vehicle (AUV) missions at a higher level of abstraction, while the low-level software and hardware details are left to the compiler and runtime system. In order to improve the adaptability of AUVs in unknown obstacle environments, Xu and Feng [23] designed five basic obstacle avoidance behaviors and used an FSM to select a suitable avoidance behavior. Saad et al. [24] applied an FSM to execute a high-level hybrid strategy to coordinate a group of AUVs. For USV applications, Redding et al. [25] presented a collaborative mission planning, autonomy, and control technology (CoMPACT) using an FSM structure to enhance USV capabilities, where the FSM decision rules enable event-based or information-based transitions to govern behaviors of USVs. In the Autonomous Maritime Navigation (AMN) project, which was led by Spatial Integrated Systems, Inc. (SIS) with government support and direction from the Naval Surface Warfare Center (NSWC) Combatant Craft Division (NSWCCD), the mission-level behavior code is written as an FSM [26]. These successful applications imply the potential of FSM in the software architecture design for the collision avoidance process of USVs.
Although many novel algorithms for local collision avoidance in a cluttered environment have been proposed, local obstacle avoidance still limits the wide application of USVs. For example, the trajectories designed in trajectory-based algorithms usually have constraints on the number and shape. In addition, these trajectories are closely related to the control performance of USVs, and they may not be well realized by USVs in actual applications. The quantity and quality of training data seriously affect the performance of artificial intelligence algorithms. There may be situations that are not well-trained, in which the algorithms may perform unpredictable operations and may lead to serious consequences. The concept of behavior-based algorithms is in line with human habits in practical applications of ship operations. Most algorithms alter the velocity, including the speed and course of a USV in real time. However, accurate velocity may be difficult to achieve in an actual marine environment when it alters its speed and course simultaneously. In encounter situations, it is usually more difficult for the other encounter vessels to accurately determine the motion of the USV. As introduced in Rule 8 of COLREGs, alteration of course alone for USVs may be the most effective action in a maritime environment when it is made in good time and 49234 VOLUME 9, 2021 is substantial [18]. In addition, when the USV runs at an economical speed, this is beneficial to the energy consumption in long-range applications. Therefore, we intend to develop an algorithm for USV local collision avoidance by altering USV headings alone under some dynamic constraints. COLREGs are also taken into account to reduce collision risks, and an FSM is designed to better organize the collision avoidance process of USVs.
In this paper, the proposed algorithm consists of three main parts that will be performed based on the specific collision situation: collision risk assessment, steering occasion determination, and navigation waypoint (NWP) update. In the collision risk assessment, the feasible angle interval considering COLREGs is calculated based on the concept of the VO method, and the risk of a collision is assessed by the closest point of approach (CPA) method. Based on the collision result, the steering occasion determination will be performed, and a safe distance interval is determined. If it is not satisfied, then the state of the USV will remain; otherwise, the NWP will be updated and is used to navigate the USV. The navigation and avoidance procedure based on the algorithm is reflected in the designed FSM.

II. ENVIRONMENT AND RISK ASSESSMENT MODELS A. ENVIRONMENT MODEL
Three coordinate systems are established for the environment model, including an absolute coordinate system OXY , relative coordinate system oxy, and a polar coordinate system ox. As shown in Fig. 1, OXY is a rectangular coordinate system in geodesy. Its origin is determined when an application starts. The north and east are the positive directions of the X-axis and Y-axis. oxy is dynamic with the motion of the USV. Its origin is the USV centerĈ, and its x-axis and y-axis directions are the USV heading and the USV starboard direction. ox is the polar form of oxy. Angles in all coordinate systems begin from their own positive X-axis/x-axis and increase along a clockwise rotation.
It is assumed that the velocities of obstacles are nearly constant in a short time, and the basic information of these obstacles can be estimated, such as positions and sizes. Then, every obstacle can be equivalent to a circular obstacle with a radius of r obs . Using the known USV position and heading, the relative bearing angle β in oxy and the relative distance d can be easily calculated. Furthermore, a narrow and long obstacle can be further equivalent to multiple connected or overlapped circular obstacles, as shown in Fig. 1.

B. RISK ASSESSMENT MODEL
A risk assessment model combining the CPA method and a simple risk level model is proposed to judge whether there will be a possible collision between a USV and obstacles.
In risk assessment applications of the CPA method, the distance to the closest point of approach (DCPA) and the time to the closest point of approach (TCPA) are usually used in combination. However, they have different priorities in risk FIGURE 1. Schematic diagram of three coordinate systems and equivalent obstacles. Relative coordinate system oxy is dynamic with USV navigation. Obstacle is equivalent to circular obstacle with radius of r obs , or multiple connected or overlapped circular obstacles when it is narrow and long. β is relative bearing angle of obstacle in oxy , and d is relative distance between obstacle and the USV. assessment. DCPA can be used to judge the possibility of a collision first, and TCPA can be completely ignored when the judgment is no possible collision. Therefore, the risk assessment in this study is mainly dependent on DCPA, and the function of TCPA is reflected by the steering occasion (see part C of Section III). For the risk level model, four perimeters B x with different radii r x are defined as shown in Table 1, where the subscript x represents obstacle, prohibition, warning, or buffer, respectively. The surrounding area of an obstacle is divided into five regions S y by the four perimeters, where the subscript y represents obstacle, prohibition, warning, buffer, or safety, respectively (see Fig. 2). S O is the region with radius r O = r obs + L U 2, where L U is the USV length. The S P is set for the detection deviations of obstacles. S W is set considering the braking effect and lag in the USV control. S B is a buffer region, and S S is the safe region for a USV. When a USV sails into the S B , r W < d CPA ≤ r B . In such situations, there is a small probability of collision, and collision avoidance maneuvers should be performed. When d CPA ≤ r W , it is dangerous, and collision avoidance maneuvers must be performed. r P , r W , and r B are determined based on the characteristics of the USV and the experience of operations.   Table 1. USV should perform maneuvers when it will sail into S B , and maneuvers must be performed when USV will sail into S W , S P or S O .

III. LOCAL COLLISION AVOIDANCE ALGORITHM
In the local collision avoidance algorithm considering COLREGs, a final navigation waypoint (NWP) result is set for the navigation of the USV. To determine the NWP in a collision risk situation, a risk assessment, steering occasion determination, and NWP update processes are selectively executed according to the detailed situation. For the risk assessment, a feasible angle interval is determined based on the similar concept of the VO method first, and then the collision risk is assessed based on the result of by using the risk assessment model. If there is no possible collision risk, then the NWP remains unchanged. Otherwise, a steering occasion considering COLREGs is determined to judge whether an action will be taken. When the steering occasion is satisfied, the NWP is updated by a navigation waypoint model; otherwise, the NWP remains unchanged again. Based on the above process, the feasible angle interval, COLREG selection, steering occasion, and navigation waypoint model used in the algorithm should be determined.

A. FEASIBLE ANGLE INTERVAL
The feasible angle interval represents a set of navigable USV headings with respect to a perimeter B x . A was acquired in [27] under the condition that the detected obstacle is in front of a USV. As an extension, the more common result without this condition is introduced as follows.

1) CATEGORIES OF ANGLE INTERVALS
The velocity variables in oxy are listed in Table 2. The corresponding velocities in ox are represented by V U = υ U e iθ U , FIGURE 3. Schematic diagram of velocity variables and geometrical relationship between USV and obstacle. θ UO is direction of V UO . If USV continuously rotates θ at a constant speed, then hodograph of resulting velocity V U_ θ forms circular trajectory. ψ is USV heading. l L and l R radiate from USV center and are tangent to perimeter B x . Their angles are β L and β R in oxy . β is deviation angle. d CPA is value of DCPA. Fig. 3). The corresponding directions in OXY and oxy can be converted to each other through the USV heading ψ. When an angle is out of U = (−π, π], it can be mapped into U by plus or minus multiples of 2π. This mapping operation is defined as the function f M (θ). For an actual USV, υ U and ψ can be acquired by the navigation system, while (υ O , θ O ), and (υ UO , θ UO ) can be estimated by the obstacle detection system.
It is assumed that there is a virtual USV with a heading θ in oxy at time T . The velocity of this virtual USV is (υ U , θ) in oxy, and it is expressed by V U_ θ = υ U e i θ in ox. The relative velocity V UO_ θ = υ UO_ θ e iθ UO_ θ is determined by where υ UO_ θ and θ UO_ θ are the virtual relative speed and direction, respectively. When θ is determined, θ UO_ θ can be calculated by taking the four-quadrant inverse tangent of then θ is defined as an infeasible angle with respect to B x ; otherwise, it is a feasible angle. All infeasible angles constitute the infeasible angle interval¯ . Define U as the universal set. The feasible angle interval is the complementary set of¯ , which can be expressed by For a certain perimeter B x , the condition d CPA ≤ r x can be equivalent to another condition θ UO_ θ ∈ , where is an angle interval with the value of This angle interval is between the two boundary rays (l L and l R ) that radiate from the USV centerĈ and tangent to B x , as shown in Fig. 3. β L and β R are the boundary angles calculated by For the perimeter B x , r = r x . Based on (1), the hodograph of V UO_ θ forms a circular trajectory with a continuous rotation of θ, as shown in Fig. 3. According to the relationship between the hodograph and the two rays l L and l R , there are six categories with 14 situations. The legends of the geometric relationships between the hodograph and the two rays l L and l R for the different situations are shown in Table 3.

2) CONDITIONS OF ANGLE INTERVALS
θ UO_ θ can be calculated by where Re (V ) and Im (V ) represent the real and imaginary parts of V , respectively, and z= atan2(y, x) is the four-quadrant inverse tangent in U. Based on (6), where δ = υ O υ U . This is a necessary but not sufficient condition of (6).
Based on the legends in Table 3, and¯ may contain one or two continuous independent subintervals (except ∅ or U) with two or four boundary values. These possible boundary values are denoted by θ i , i = 1, . . . , 4. When θ UO_ θ = β i , β i is a given value of the valid θ i can be determined based on (7), which is where The conditions to determine the categories of Cate. j, j = 1, . . . , 6 are denoted by C j . For Cate.1, there is a unique θ value that makes θ UO_ θ = β L and θ UO_ θ = β R . Based on (7), condition C 1 should be For Cate.j, j = 2, . . . , 6, the conditionC 1 = (δ > 1) should be satisfied first. Then, additional conditions C A j , j = 2, . . . , 6 are used for the further classification of Cate.j, j = 2, . . . , 6.
The C A j , j = 2, . . . , 6 can be determined based on the geometric relationship between the two boundary rays and an open interval˜ θ UO_ θ .˜ θ UO_ θ is the open interval of θ UO_ θ , which represents the range of θ UO_ θ . φ min VOLUME 9, 2021 and φ max are the two boundary values of θ UO_ θ . When δ > 1, there is θ UO_ θ = U, and θ UO_ θ is continuously differentiable in U. Therefore, φ min and φ max are the unique minimum and maximum extremum of θ UO_ θ , and they can be determined by calculating the two extreme points θ ext,m , θ UO_ θ,m , m = 1, 2, which are Then, φ min = min θ UO_ θ,m and φ max = max θ UO_ θ,m , where m = 1, 2. The correct interval of the alternative intervals determined by φ min and φ max should be selected as θ UO_ θ . Set a temporary variable θ midT = (φ min + φ max ) 2, and let θ UO_ θ = θ midT . One θ solution of (7) can be calculated by This calculated θ is used as the independent variable of (6) to determine a new θ UO_ θ , which is denoted by θ mid . If θ mid = θ midT , then θ midT ∈ θ UO_ θ ; otherwise, θ midT / ∈ θ UO_ θ . Therefore, θ UO_ θ takes Finally, C A j , j = 2, . . . , 6 can be determined by where˜ is the open interval of . The final classification conditions for Cate.j, j = 2, . . . , 6 are C j =C 1 & C A j . One and only one condition of C A j , j = 1, . . . , 6 can be used to determine the categories of and¯ .

3) RESULTS OF ANGLE INTERVALS
When the category is known,¯ can be determined based on the category and the additional valid θ i , i= 1, . . .  Table 4.
When there are multiple obstacles,¯ is the union of¯ i , that is,¯ = θ| θ ∈ N i=1¯ i (15) where¯ i is the infeasible angle interval of the i th obstacle, and N is the number of obstacles.

B. COLREGS SELECTION
There is an additional constraint for the feasible angle interval when COLREGs are considered. Rules 13 to 15 define three encounter situations, including overtaking, head-on, and crossing situations [18].
Based on the relative locations between the USV and the encounter vessel, we divide the overtaking situation (Rule 13) into a USV-behind-overtaking situation and USV-ahead-overtaking situation. When the USV is behind and overtakes another vessel, it is the USV-behind-overtaking situation, and the USV is a give-way vessel that is directed to keep out of way of the encounter vessel [18]. When the USV is ahead and going to be overtaken by another vessel, it is the USV-ahead-overtaking situation, and the USV is the stand-on vessel that keeps its course and speed. Based on COLREGs, the USV shall take early and substantial action to keep out of the way of the other vessel when the USV is a give-way vessel [18]. COLREGs do not define a detailed action for the USV to give way in the overtaking situation. Therefore, an action with a right turn is adopted in the proposed algorithm.
In the head-on situation (Rule 14), both the USV and the encounter vessel are give-way vessels, and each shall alter their own course to starboard [18]. Similar to the overtaking situation, the crossing situation (Rule 15) is further divided into a left-crossing situation and a right-crossing situation.
In the left-crossing situation, the encounter vessel is on the port side of the USV, and the USV is the stand-on vessel. In the right-crossing situation, the USV has another vessel on its starboard side, and it is the give-way vessel that shall keep out of the way and shall avoid crossing ahead of the other vessel when the circumstance requires it [18].
The definitions for the head-on and crossing situations are not defined quantitatively in COLREGs. There are some differences in the quantitative definitions of these encounter situations in different studies [28]- [30]. A compromise definition is selected in this study. The detailed quantitative definitions are shown in Fig. 4, in which the gray areas represent the range of the relative direction θ UO_ θ . The conditions for different encounter situations and corresponding required roles played by the USV are shown in Table 5. Based on the above introduction about the different situations and the requested actions of the USV, a timely and sufficient steering action with a right turn is available when the USV is a give-way vessel. When the USV is a stand-on vessel, it will maintain its course and speed based on COLREGs. However, most USVs are small and difficult to detect by other vessels in many situations. Therefore, the other give-way vessels may not take valid actions to avoid collision. In such situations, the USV shall take action to avoid collision.
For the USV-ahead-overtaking situation, it may still work by turning right with a substantial angle in time. The action to alter the course to port is not suggested by the COLREGs in the left-crossing situation [18]. Therefore, the USV should also take action by turning right to avoid collision if the other give-way vessel does not take the appropriate action. Whether the USV should take action to avoid collision depends on the steering occasion introduced in the next section.
Based on the above discussion, when the USV should take action to avoid collision considering COLREGs, is limited to COLREGs = ( 0, π] . In other words, the final infeasible angle interval¯ alg for the algorithm is adjusted tō alg =¯ ¯ COLREGs (16) where¯ COLREGs is a complementary set of COLREGs in the complete set U and can be expressed bȳ If no COLREGs are considered, then¯ alg == . The final feasible angle interval alg for the algorithm can be determined by alg = U¯ alg .

C. STEERING OCCASION WITH RESPECT TO OBSTACLES
According to Rule 8 of COLREGs, it is better to avoid a collision as soon as possible when a USV detects a possible collision with a moving vessel [18]. Unfortunately, deviations exist in most information about detected obstacles, and they are much higher when the obstacles are far from the USV with high probability. There may even be false obstacles in some environments. Therefore, an immediate avoidance action will not be a good choice in many actual situations. In order to reduce the influence of these factors, a steering occasion that can be equivalent to a safe distance interval is adopted for collision avoidance.

1) MARGINAL STEERING ANGULAR VELOCITIES
For a valid virtual USV heading θ i , there exists a minimum angular velocity for the USV collision avoidance, which is defined as the marginal steering angular velocity ω i (d).
Except for Cate.2 and Cate.3, θ i , i = 1, . . . , 4 in other VOLUME 9, 2021 categories are related to the relative distance d, which are where θ UO , θ O , β, and d CPA are the values at time T , and r is the corresponding r x with respect to B x . The geometric relationship between the USV and obstacles at time T and T + t is shown in Fig. 5. Based on the triangle cosine theorem, Based on (22), ω i (d) can be calculated by where In a possible collision situation d CPA ≤ r, |ω i (d)| increases with decreasing d, except for the only condition that satisfies δ = 1 and χ cos,i (d) ≥ 0. For this condition, ω i (d) ≡ 0.

2) STEERING OCCASION
Every USV has constraints on its dynamic performance such as maximum speed, acceleration, and angular velocity. The steering occasion is mainly related to the angular velocity constraint of a USV. For every valid ω i (d), it can be equivalent to a relative distance d i value based on the monotonicity of |ω i (d)|. Therefore, the steering occasion is designed to be related to a distance interval D SO = d SO_s ,d SO_l , where d SO_s and d SO_l are the short-distance and long-distance boundaries, respectively.
The short-distance boundary d SO_s is important for collision avoidance by altering the course. The d i value, which corresponds to ω i,S (d i ) = ω max 2, is selected as a candidate d SO_s,i , where ω max is the maximal angle velocity of a USV, and ω i,S (d i ) are the ω i (d i ) values with respect to the perimeter B S . The analytical solution of d SO_s,i is difficult to obtain. A discrete numerical solution was adopted to approximate d SO_s,i . Then, d SO_s takes the largest value of all valid d SO_s,i . The long-distance boundary d SO_l is set to d SO_l = d SO_s + d SO , where d SO is the setting distance. Finally, the steering occasion is determined by comparing d and d SO_l . If d > d SO,l , then the steering occasion is not satisfied, and the contribution of¯ caused by a dynamic obstacle is ignored. Otherwise, the USV should take a steering action to avoid possible collisions.

D. NAVIGATION WAYPOINT MODEL
A USV path commonly consists of multiple waypoints. The mature waypoint navigation technology has been applied well to USV control. Therefore, a navigation waypoint model is used for the USV to avoid collision. In this model, an updatable NWP is set for the entire navigation of a USV. Two types of waypoints are set for navigation: the goal waypoint P G and the temporary waypoint P i , which is determined by a real-time optimal orientation angle and a setting distance.

1) NAVIGATION WAYPOINT
When a USV navigates to a certain waypoint of an original path, this waypoint is treated as the current P G . It is set as the NWP when the USV can navigate to it directly without a collision. Otherwise, an updatable P i is set as the NWP. When a P i needs to be set or updated, there is where xĈ , yĈ and x P i , y P i are locations of the USV and P i , θ opt is the real-time optimal orientation angle, and d P i is the setting distance.

2) REAL-TIME OPTIMUM ORIENTATION ANGLE
θ opt is used to evaluate whether the NWP needs to be updated and to determine P i+1 when necessary. The optimization objective function F opt (θ) is designed by a weighting deviation degree combination of the orientation angle θ with respect to the goal orientation and obstacles: where λ 1 and λ 2 are weighted values satisfying λ 1 + λ 2 = 1. F goal (θ) is the degree of proximity to the goal orientation, and F safe (θ) is the combination of deviation degrees to infeasible angle intervals: where θ G is the goal orientation, N is the number of valid independent infeasible angle subintervals, and θ¯ alg ,j is the center angle of the corresponding independent subinterval. When considering COLREGs,¯ alg takes the result of (16); otherwise,¯ alg =¯ .
Cate.2 is safe for a USV, and no action is taken for collision avoidance. Cate.3 can no longer achieve safe collision avoidance by using steering alone. In such situations, other methods to alter the USV speed and heading simultaneously for collision avoidance can be used, but they are not discussed in this paper. For other categories, there may be one or two continuous independent infeasible angle subintervals for one obstacle. N = N 1 + 2N 2 , where N 1 and N 2 are the numbers of obstacles for these two situations. The corresponding θ that maximizes F opt (θ) is selected as θ opt .

IV. LOCAL COLLISION AVOIDANCE USING FSM
Local collision avoidance based on waypoint navigation mainly includes the processes of NWP navigation and NWP updates. To design the processes with suitable elements of FSM, collision avoidance can be effectively implemented using FSM.
The designed FSM is recorded as F = S, C, T , S 0 , S g . S is a finite set of discrete states. Several states have corresponding actions for collision avoidance. C is a finite set of conditions for transitions between states. T is a finite set of transition relations. In addition to the state transition from one state to another, there are self-state transitions. S 0 and S g are the start and goal states, respectively, where S 0 , S g ⊂ S. The main purpose of the FSM design is to determine S, C, T , and the state transition diagram.
In addition to the start, goal, and emergency states, two different navigation states and two NWP update states for P G and P i are designed. Therefore, S = {S 0 , . . . , S 6 }, where S g = S 6 . The detailed state definitions and corresponding actions are shown in Table 6. A state transition T i→j ⊂ T indicate a transition from the current state S i to the next state S j . When i = j, it is a self-state transition. Considering the collision avoidance process, T is designed as follows: C and S jointly determine T . The corresponding condition C i→j ⊂ C for T i→j can be determined by combining the relevant basic conditions C B k ,k = 0, 1, 2, . . .. The detailed C B k conditions are shown in Table 7, and the relevant parameters are listed in Table 8. The values of C B k , k = 1, 2, . . .. are ordered by priority. If the k th condition is satisfied, then set The detailed combinations for C i→j are shown in Table 9.C B 1 · · ·C B k−1 C B k ,k ≥ 2 means that C B 1 to C B k−1 are not satisfied and C B k is satisfied, whileC B 1 · · ·C B k , k ≥ 2 represents that no C B 1 to C B k are satisfied.    The state transition diagram can be designed as shown in Fig. 6. The single/double circle with state S i represents a state with its action. The arrow represents the transition from S i to S j . Based on the diagram, a state modification that includes adding, deleting, or changing a state will only influence the state itself and the relevant conditions. Therefore, the FSM is easily modified when the algorithm is improved.

V. SIMULATION RESULTS AND DISCUSSIONS A. SIMULATION ENVIRONMENTS AND THE USV
A virtual obstacle module is designed to simulate an environment with both static and dynamic obstacles. In this module, obstacles are created with rand positions and radii. Gaussian noises are added to obstacle positions to simulate the detected position errors of actual obstacles as far as possible. The estimated position x O i ,ỹ O i for the simulation satisfies   where d 0 and d 1 are the distance deviations. d i is the distance between the USV and the i th obstacle. r O i is the radius of the i th obstacle. d det is the detection distance, which is where d detB and d detF are the base detection distance and the fluctuant distance, respectively; and rand (1) takes a rand value between 0 and 1. The correlation coefficient always takes ρ O i = 0. For a dynamic obstacle, the corresponding noises are also added to the speed and direction. The estimated speed and direction of the j th dynamic obstacleν O j and θ O j fluctuate and satisfỹ whereῡ O j andθ O j are the base speed and direction in OXY , respectively; m is an integer between 1 and 5 with a discrete FIGURE 9. USV navigation and NWP update progress in static obstacle environment. (a) NWP is set to temporary waypoint P 1 at time T 1 . (b) USV reaches P 1 , and NWP updates to P 2 at T 2 . (c) NWP updates to P G at T 3 . (d) New obstacles are detected, and NWP updates to P 3 at T 4 . (e) NWP updates to P 4 at T 5 . (f) NWP updates to P 5 to navigate USV through gap between obstacles at T 6 . (g) USV reaches P 5 , and NWP updates to P G at T 7 . (h) NWP updates to P 6 at T 8 . (i) NWP updates to P 7 at T 9 . (j) USV reaches P 5 , and NWP updates to P G at T 10 . (k) NWP updates to P G at T 11 . (l) USV reaches P G at T 12 .
probability P(m); and θ is the angle deviation. Static obstacles are stable when they are produced at the beginning of the simulation. Dynamic obstacles are generated randomly during each period. The ones within the detected range of a USV will persist and be updated based on its basic motion, while the others will be removed. The basic parameters are listed in Table 10 and are designed for simulation of the ''JiuHang490'' USV, as shown in Fig. 7 [31]. A particle model with dynamic performance constraints for the USV was used to simulate an actual situation. Using these common parameters, four types of simulation cases are conducted to validate the feasibility and validity of the proposed algorithm for different purposes. It is assumed that all obstacles are unknown before the navigation of the USV. The simulations are performed on a computer with a configuration of i7-6700 CPU and 12 GB RAM. The operating system is a 64-bit Win7, and the algorithm is executed in the MATLAB environment. The execution time of the algorithm in each cycle is completely in real time for the USV control cycle.

B. COLLISION AVOIDANCE IN STATIC OBSTACLE ENVIRONMENT
For USV missions in complex marine environments, there may be unknown obstacles such as small reefs on the planned global path. Therefore, the USV should have the ability to avoid such obstacles. Although the proposed algorithm is mainly for dynamic obstacles, it is also effective in environments with static obstacles. More information about the USV navigation and NWP update progress is shown in Fig. 9.
When the USV sails from the start position, NWP is first set to the goal waypoint P G . Then, the USV detects O 1 and O 2 at time T 1 , and a temporary waypoint P 1 is set as the NWP to navigate the USV bypass the two obstacles, as shown in Fig. 9(a). As the USV sails toward P 1 , the influence of obstacles still exists. The NWP does not update until the USV reaches P 1 at T 2 , and then it updates to P 2 , as shown in Fig. 9(b). In Fig. 9(c), the influence of O 1 and O 2 is negligible at T 3 , and no other obstacles are detected. Therefore, the NWP is set to P G again, and the USV turns toward it directly. When O 3 and O 4 are detected at T 4 , the NWP updates to P 3 to navigate the USV, as shown in Fig. 9(d). Before the USV reaches P 3 , O 5 is detected at T 5 , and the USV turns towards the new P 4 [see Fig. 9(e)]. After sailing for a while, the USV finds a better evaluated path and updates the NWP to P 5 to sail through the gap at T 6 , as shown in Fig. 9(f). When the USV reaches P 5 at T 7 , the NWP is updated again [see Fig. 9(g)]. Although O 5 and O 6 are within the detection range of the USV, their influence can be ignored, and the USV turns to P G again. Then, O 7 is detected at T 8 , and this is dangerous for the USV. Therefore, the NWP updates to P 6 to navigate the USV, as shown in Fig. 9(h). When the USV detects O 8 before the USV reaches P 6 at T 9 , the NWP updates to P 7 to avoid possible collisions with O 8 , as shown in Fig. 9(i). When the USV reaches P 7 at T 10 , the NWP updates to P 8 to navigate the USV to avoid O 8 continuously [see Fig. 9(j)]. At T 11 , the USV finds that there is no collision risk if it turns toward P G , so the USV navigates to P G directly until it reaches to P G finally, as shown in Fig. 9(k) and Fig. 9(l).

C. EFFECT OF STEERING OCCASION
A virtual false dynamic obstacle O d is set to simulate the influence of the steering occasion. If the algorithm does not adopt the steering occasion, then the USV alters its course immediately when it detects O d . The trajectory acquired by this algorithm is shown in Fig. 10 and is composed of green and blue trajectory portions. In this case, the USV turns right to avoid a false obstacle when false information is detected. However, the USV finds that this information is false after several periods. Therefore, it turns left to navigate to the goal position again. Such redundant avoidance is not necessary. Instead, the USV will ignore the false obstacle at first if the steering occasion is adopted. The resulting trajectory is actually an optimal straight line, as shown by the dashed line in Fig. 10. By adopting a suitable steering occasion, the influence of false obstacles or large detection deviations of obstacles can be reduced.

D. COLLISION AVOIDANCE CONSIDERING COLREGs
In the algorithm, COLREGs restrict the feasible angle interval when the USV is going to avoid a collision with a moving vessel O d . Fig. 11 to Fig. 13 show collision avoidance simulations considering COLREGs in three typical encounter situations. A USV-behind-overtaking situation is shown in Fig. 11, where COLREGs require the USV to give way to the overtaken vessel. As shown in Fig. 11(a), the USV detects O d at time T 1 . When the steering occasion is satisfied, the USV turns right to give way [see Fig. 11(b)]. When the USV overtakes O d , it turns and sails to the goal position, as shown in Fig. 11(c). Figure 12 shows a head-on situation, and both the USV and O d should alter their courses to starboard based on COLREGs. In this case, O d is simulated to maintain its course, and the USV can still avoid it by turning right. It will be safer when O d also alters its course under COLREGs. In the right-crossing situation, as shown in Fig. 13, the USV turns right and sails behind O d to stay out of the way. All of these cases show the ability of the proposed algorithm to avoid a moving vessel based on COLREGs.

E. COLLISION AVOIDANCE IN ENVIRONMENT WITH STATIC AND DYNAMIC OBSTACLES
A simulation case in an environment with both static and dynamic obstacles is shown in Fig. 14    information of encounter situations with O d1 and O d3 are shown in Fig. 15 and Fig. 16 respectively. Both of these situations are crossing situations. The difference between them is that the USV takes the same avoidance maneuver in the encounter situation with O d1 regardless of whether the algorithm considers COLREGs; however, different avoidance  maneuvers are taken in the encounter situation with O d3 when COLREGs are considered. Fig. 15 shows the collision avoidance process in the encounter situation with O d1 . The USV detects O d1 at time T 1 for the first time and does not steer immediately. Then, the steering occasion is satisfied and the USV turns right to avoid collision. The positions and trajectories of the USV and O d1 at T 2 show that the USV sails safely behind O d1 . T 3 is the time at which O d1 is detected for the last time.  The collision avoidance process in the encounter situation with O d3 is similar and is shown in Fig. 16. If the algorithm does not consider COLREGs, then the USV turns left and sails from the front of O d3 . The corresponding collision avoidance is shown in Fig. 17. This is not recommended based on Rule 15 of COLREGs.

VI. CONCLUSION AND FUTURE WORKS
A local collision avoidance algorithm based on steering maneuvers considering COLREGs was proposed for a USV in environments with obstacles that are unknown before a mission. Under the dynamic constraints of the ''Jiuhang490'' USV, simulations in an environment with static obstacles alone or both static and dynamic obstacles were carried out. The results indicated the feasibility and validity of the algorithm.
This algorithm takes the steering maneuver alone at a constant speed to avoid collision for a USV in general situations. Such a maneuver is also suggested by COLREGs. In addition, the algorithm contains an emergency state in which collision avoidance cannot be achieved by a steering maneuver alone. In this situation, the algorithm can avoid collision by using other methods that alter the speed and heading of a USV simultaneously, such as the VO method. This process can ensure the functional integrity of the algorithm. In situations where the USV is a stand-on vessel, the steering occasion design can ensure that the USV will perform the necessary collision avoidance action when the other give-way vessel cannot perform effective collision avoidance. In addition, in the USV steering operation, the adopted a waypoint navigation method that seamlessly combines local collision avoidance with the global path. This is suitable for actual USVs using waypoint navigation technology, which is maturely applied.
In future work, an implementation of the proposed algorithm on the practical USV platform will be carried out to verify the actual performance. A combination with global path planning, such as the fast marching method, may be more effective in avoiding collisions in practical environments. Deep learning will be considered for research on large enough actions; this is required in Rule 8 of COLREGs. A virtual obstacle model that can respond to the USV motion is being constructed to study the effectiveness and feasibility of the algorithm when obstacles significantly alter their velocities. He is currently a Professor with HIT. His research interests include array signal processing, communication and radar system design, and the theory on advanced radar systems.