Unmanned Surface Vehicle Collision Avoidance Trajectory Planning in an Uncertain Environment

Unmanned surface vehicles (USVs) can encounter undetected moving obstacles while sailing along a planned global path. USVs need to plan collision avoidance trajectories for moving obstacles. In this paper, an algorithm based on the Gaussian mixture model (GMM) and Gaussian process regression (GPR) is proposed to predict the motion of moving obstacles and estimate the uncertainty of the prediction. A nonlinear finite-time velocity obstacle (NLFVO) method is developed for obstacle avoidance. The NLFVO method analyzes the velocity of the USV and the predicted uncertain velocity vectors of the moving obstacles and selects a collision-free velocity for the USV and minimizes the objective function. To enable the actual navigation of USVs, the International Regulations for Preventing Collisions at Sea (COLREGs) are considered in addition to the NLFVO method. The simulation results show that the prediction algorithm can effectively predict the trajectory of moving obstacles, and the NLFVO method can obtain a collision-free trajectory for a USV.


I. INTRODUCTION
An unmanned surface vehicle (USV), as a kind of autonomous marine vehicle, is suitable for tasks that are dangerous or unsuitable for manned vehicles. A USV typically plans a global path connecting the starting point and the target point according to the existing information before performing the task. Due to limited information, a USV can encounter moving obstacles that were not previously known. Therefore, the global path of a USV should be adjusted locally to avoid collisions [1]. As a part of local trajectory planning, USV collision avoidance for moving obstacles is carried out online based on real-time information from shipborne sensors.
Many studies on USV collision avoidance have been conducted. The velocity obstacle (VO) method is a collision avoidance method that was first proposed for robots to avoid collisions with obstacles in [2], and the potential of collisions between robots and obstacles was analyzed by their velocities. In [3], linear VO, nonlinear VO, and probabilistic The associate editor coordinating the review of this manuscript and approving it for publication was Heng Wang . VO methods were applied to avoid collisions with moving obstacles whose trajectories were nonlinear and predictable. Compared to traditional approaches, the algorithms in [3] could detect collision dangers with moving obstacles sailing nonlinearly and predictably and find collision-free velocities in multi-ship scenarios. In [4], COLREGs were considered as an additional constraint in the VO method. Due to the COL-REGs, VO methods specified on which side of an obstacle the vehicle should pass during an avoidance maneuver. In [5], a reciprocal VO (RVO) method for n-body collision avoidance was presented, where multiple mobile vehicles needed to avoid collisions with each other while moving in a common workspace. In [6], a finite-time VO (FVO) method was introduced by considering the collision avoidance duration. A mobile robot that completed an unknown environment crossing using the FVO method was reported in [7].
The dynamic window approach (DWA) calculates the velocity of a robot can reach within a given time window [8]. The reachable velocities constitute a set in the velocity space, known as a dynamic window. The DWA is widely used for robot collision avoidance in a dynamic environment. In [9], the DWA and VO methods were combined to calculate the reachable velocity and course in a given time window for a USV to avoid obstacles.
In practice, it is necessary to also consider environmental uncertainty, model uncertainty, and information uncertainty in collision avoidance. The velocity and course of a moving obstacle are uncertain. According to historical motion information, a USV can predict a moving obstacle's trajectory in the future, and then collision avoidance operations can be performed based on the predicted trajectory. It is difficult for shipborne sensors to explicitly show motion information such as the velocity and course of a moving obstacle. The motion information is implicit in the obstacle's position at the next timeframe, so the motion information of the obstacle is estimated by a prediction algorithm [10].
Kalman filtering, the Markov model, and the support vector machine (SVM) are popular prediction algorithms. However, their prediction accuracy degrades significantly in the case of large changes in motion state [11]. The development of theory and practice in recent years has made Gaussian process regression (GPR) an important algorithm for supervised learning applications [12]. The advantage of the GPR model is that it seamlessly integrates many machine learning tasks, including model training, hyperparameters and uncertainty estimation [13]. GPR has been applied in many fields, such as wear prediction [14], wind speed prediction [15], and trajectory prediction [16]. In [17], GPR has been adopted for representing vehicular motion patterns, because it is robust against noise in measured data.
Because a moving obstacle in the sea is affected by many factors, its motion patterns are complex. Therefore, it is necessary to analyze the motion patterns of obstacles by clustering and select the motion pattern with the maximum probability in the future. According to motion coherence, the trajectory was clustered into a few groups by the Gaussian mixture model (GMM) in [18] and [19]. This paper proposes a trajectory prediction algorithm based on GMM-GPR and a USV collision avoidance algorithm based on a nonlinear finite-time velocity obstacle (NLFVO) method. The main contributions are as follows: (1) The trajectory prediction algorithm based on GMM-GPR is proposed to predict the trajectory of a moving obstacle where the velocity may experience sudden changes. The Gaussian distributions that represent the uncertainty of the future velocity vectors of the moving obstacle are obtained. (2) According to the motion uncertainty of the moving obstacle, a collision avoidance trajectory planning algorithm based on the NLFVO method and the DWA is proposed for collision avoidance considering the changes in the velocities of obstacles, collision time, and kinematic constraints.
The remainder of this paper is organized as follows. In Section II, the collision avoidance trajectory planning algorithm based on the NLFVO method is proposed. In Section III, the trajectory prediction algorithm based on GMM-GPR is provided. In Section IV, a trajectory planning algorithm considering the uncertainty of the obstacle's motion is presented. Simulations for testing the performance of the proposed trajectory prediction algorithm and collision avoidance trajectory planning algorithm are presented in Section V. In Section VI, conclusions are provided.

II. COLLISION AVOIDANCE TRAJECTORY PLANNING ALGORITHM A. BASIC VELOCITY OBSTACLE METHOD
In this section, we briefly review the basic VO method. A schematic of the basic VO method is shown in Figure 1. Assuming that at time t that the motion state of the USV is defined as S usv (t) = (P usv (t), v usv (t)) in the coordinate system {X , Y }, where P usv = (x usv , y usv ) is the position of the USV, and v usv = (v x usv , v y usv ) is the velocity of the USV. The motion state of the moving obstacle (mo) is defined as S mo (t) = (P mo (t), v mo (t)). The motion state of the moving obstacle can be calculated according to the contents described in Section III. D(p, r) is a disk with p as the center and r as the radius: In this paper, the USV is simplified as a point, and the moving obstacle is expanded into a circle with r sum = r usv + r mo as the radius and any point P as the center, where r usv and r mo are the radii of the affected area of the USV and the moving obstacle, respectively. Therefore, D(P, r sum ) is represented as the area affected by the moving obstacle, that is, the prohibited zone of the USV.
The relative velocity vector v uo between the USV and the moving obstacle is The ray emitted from the point P usv in the direction of the velocity vector v uo is (3) VOLUME 8, 2020 When ray λ(P usv , v uo ) intersects the expanded obstacle, that is, ray λ(P usv , v uo ) is within the angle formed by the two rays tangent to D(P, r sum ) from P usv , the USV will collide with the moving obstacle in the future. The relative collision cone of the USV and D(P, r sum ) is defined as RCC uo : where RCC uo is an area located between the left tangent λ L and the right tangent λ R of D(P, r sum ) emitted from P usv . In other words, RCC uo is the set of all the relative velocities v uo that cause the USV to collide with the moving obstacle. The velocity obstacle for the USV induced by the moving obstacle, denoted as VO uo , is a set of the USV velocities that result in a collision with the moving obstacle in the future: where ⊕ is the Minkowski sum operation.
If the USV detects a moving obstacle at t 0 and it will collide with it at time t col , P usv (t col ) is in area P mo (t col ) ⊕ D(P, r sum ), that is P usv (t col ) ∈ P mo (t col ) ⊕ D(P, r sum ).
If v usv is not changed within [t 0 , t col ], the position of the USV at t col is Then, (6) is rewritten as P usv (t 0 ) + v usv (t col − t 0 ) ∈ P mo (t col ) ⊕ D(P, r sum ). (8) The following formula is obtained by transforming (8): where v usv represents that if the USV sails at this velocity during the time interval [t 0 , t col ], it will collide with the moving obstacle at the time t col [3]. Hence, (5) is equivalently expressed as where t col ∈ (t 0 , ∞). If the USV continues to sail at the velocity vector in V O uo , it will definitely collide with the moving obstacle at a time within (t 0 , ∞). In other words, the USV can avoid collision with the moving obstacle by adjusting the current velocity vector v usv out of V O uo within a given time interval.
If v mo is not changed within [t 0 , t col ], the position of the moving obstacle at t col is P mo (t col ) = P mo (t 0 ) + v mo (t col − t 0 ).
Then, (9) can be rewritten as where P mo (t 0 ), P usv (t 0 ), t 0 , and t col are known. Therefore, item P mo (t 0 )−P usv (t 0 ) can be obtained as a constant. As shown in Figure 2, the angle between the velocity vector v uo and the X-axis is defined as χ uo , the angle between the vector (P mo − P usv ) and the X-axis is θ uo , and the angle between the vector (P mo − P usv ) and the tangent λ L is α. The angles χ uo , θ uo , and α are respectively calculated as follows: where P uo = P usv − P mo . Geometrically, if (9) and (12) are true, that is, the USV will collide with the moving obstacle in the future, the angle χ uo of the velocity vector v uo is within the angle between the left tangent λ L and the right tangent λ R of D(P, r sum ) emitted from P usv . The formula for determining whether the USV will collide with a moving obstacle is:

B. NONLINEAR VELOCITY OBSTACLE METHOD
In the basic VO algorithm, the obstacle is regarded as moving uniformly in a straight line. In the nonlinear VO (NLVO) method, it is assumed that the motion of the moving obstacle is nonlinear, that is, its velocity vector v mo is time-varying, and v mo (t) within t ∈ [t 0 , t col ] can be detected by the USV. Thus, the position of the moving obstacle at t col is obtained by where N = t col −t 0 t and t is the length of the detection cycle of the shipborne sensors.
By substituting (17) into (9), the velocity vector v usv that may cause a collision between the USV and the moving 207846 VOLUME 8, 2020 obstacle with nonlinear motion is obtained as (18), as shown at the bottom of the page [8], [20].
Therefore, the NLVO for the USV is obtained as (19), as shown at the bottom of the page [3].
Two parts on the right side of (19) perform the Minkowski sum operation. The first part represents the shape of the area affected by the moving obstacle, whose radius is adjusted by (t col − t 0 ) −1 , and its center P lies on the curve represented by the first part [3]. According to the above two parts, a series of prohibited zones for the USV are obtained in the velocity space. The envelope of the prohibited zones is the set of NLV O uo , whose shape is similar to a curved cone in the velocity space, as shown in Figure 3.

C. NONLINEAR FINITE-TIME VELOCITY OBSTACLE METHOD
If there are numerous moving obstacles near the USV, the USV will have very few velocity candidates [9]. To solve the above problem, the collision time between the USV and the moving obstacle is considered in the VO method. Assuming the USV sails at the velocity vector in V O uo , it will collide with the moving obstacle after a long time. Thus, a finite-time velocity obstacle (FVO) method is proposed in [6], [7].

1) FINITE-TIME VELOCITY OBSTACLE METHOD
The finite-time velocity obstacle (FVO uo ) formed by the USV and the moving obstacle at time t 0 is defined as: (20) where τ is the minimum allowable collision time; if the USV sails at v usv , the collision will occur at least after the time interval (τ −t 0 ). Before time τ , the USV can sail at the current velocity v usv so that the USV has a larger range of optional collision avoidance velocity vectors.
As shown in Figure 4, FVO uo is a new polygon formed by VO uo being truncated by D(P F , r F ), where the position of P F is (P usv −P mo )/τ , and the radius is r F = r/τ . Geometrically, if v usv is in the area of FV O uo , it will cause the USV and the moving obstacle to collide at least after the time interval (τ − t 0 ); otherwise, it will not. In this way, FVO uo has a larger velocity space than V O uo to obtain the velocity that can avoid collisions, which is significant for the situation of multi-obstacle collision avoidance. However, the previous references on the FVO method are only for the linear motion of obstacles, and the nonlinear motion of obstacles and the minimum allowable collision time are rarely considered. Therefore, in the following content, FVO will be applied to the nonlinear motion of obstacles.

2) NONLINEAR FINITE-TIME VELOCITY OBSTACLE METHOD
Combining (19) and (20), the nonlinear finite-time velocity obstacle (NLFVO) caused by the USV and the moving obstacle with the nonlinear motion is obtained as (21), as shown at the bottom of the next page where 0 < t ≤ τ , and N = τ −t 0 t . The geometric expression of the NLFVO in velocity space is shown in Figure 5. In (21), the minimum collision time τ is significant for the determination of NLFV O uo . Meanwhile, the minimum collision time τ is determined by the current position and velocity of the USV and the moving obstacle, respectively. However, the motion uncertainty of the moving obstacles sailing in the sea leads to an inaccurate minimum collision time. Therefore, to obtain the minimum collision time in an uncertain environment, the trajectory prediction algorithm is used to predict the future trajectories of the moving obstacle, and then the range of the minimum collision time is calculated.

III. TRAJECTORY PREDICTION ALGORITHM
Trajectory prediction is important for collision detection and trajectory planning. In this section, we first introduce the motion pattern model based on GPR. In some complex environments, the velocity vectors of the moving obstacle will change randomly, and the GMM is used to cluster the motion patterns and calculate the probability distribution of different motion patterns. Finally, the method to predict the trajectory of the moving obstacle is proposed.

A. MODELING OF MOTION PATTERNS USING GPR
Assume that there is no communication between the USV and the moving obstacle. Some parameters of the moving obstacle, such as position, velocity, and course, are difficult to measure in real time. Thus, the trajectory prediction algorithm is used to obtain the future positions of the obstacle, and then the velocities of the obstacle are calculated. Due to the kinematic and kinetic constraints of the moving obstacles in the sea, their motion state will not change significantly. Therefore, we can assume that the motion state of the moving obstacles will not change significantly in a short time.
The position of the moving obstacle at time t is set as P mo (t) = (x mo (t), y mo (t)). The shipborne sensors on the USV measure the m + 1 historical trajectory points {P mo (t c − m), · · · , P mo (t c )} of the obstacle before the current time t c . The purpose of trajectory prediction is to obtain the n future trajectory points {P mo (t c + 1), · · · , P mo (t c + n)} of the obstacle after the current time t c .
Definition 1: The motion pattern is defined as a mapping from the location (x, y) to a distribution over trajectory t indicating the agent's future motion [17].
The m historical velocities of the obstacle are obtained by the m + 1 historical trajectory points of the obstacle. The two elements of the moving obstacle's velocity vector , v x mo is discussed only in the following content. v x mo is regarded as the output of the observation data set. Therefore, is obtained. According to the known input data, a random variable set v x mo (T ) is formed. A GPR model is determined by its mean function m(T ) and covariance function k(T , T ) [12]: where where σ f and σ l are hyperparameters. At the current time t c , t * j is defined as the future time, i.e., t * represents the component of the n future velocity vectors of the moving obstacle in the X-axis direction. T * = {t * 1 , · · · , t * n } represents n future time after the current time t c and is used as the input of the prediction data set.
The output v x mo of the observation data set and the output v x mo K(T , T ) is an m×m kernel matrix, called the Gram matrix, whose expression is shown as: The expressions of K(T * , T ) and K(T * , T * ) are shown as follows: After calculating the output v x mo of the observation data set, the conditional distribution of the output v x mo * of the prediction data set is obtained as [15]: where The mean value of the conditional distribution in (29) is used as the estimated value ofv At time t + 1, the new m velocity vectors of the moving obstacle are remeasured. In other words, the first of the predicted values is taken as the observation data for the next prediction cycle. This process is repeated, the velocities of the moving obstacle are obtained online, and the prediction errors are continuously corrected.

B. SELECTION OF MOTION PATTERNS BASED ON GMM
Since vehicles are affected by many factors, their motion patterns are complex. In a complex motion pattern scenario, a trajectory may belong to multiple motion patterns, which is difficult to describe by a Gaussian process. If a trajectory needs to be accurately described, a mixture model is required.
Assuming that a moving obstacle has K possible motion patterns, which are denoted as {b 1 , b 2 , · · · , b K }, the probabilities that the moving obstacle adopts each motion pattern to move are defined as in all motion patterns is expressed as follows: where w k is the weight, which satisfies 0 ≤ w k ≤ 1, and K k=1 w k = 1. GP v x mo (t i )|µ k , σ k is the Gaussian distribution of v x mo (t i ) in motion pattern b k . µ k and σ k represent the mean and covariance of the kth motion pattern, respectively. Since the expression of the GMM of v y mo (t i ) is similar to that of v x mo (t i ), v x mo (t i ) is discussed only in the following section. For the entire observation data set v x mo = {v x mo (t 1 ), · · · , v x mo (t m )}, the likelihood function of its GMM is The log-likelihood function is The expectation-maximization (EM) algorithm is used to train the model. First, the model parameters are initialized. Then, the E-step and M-step are iterated to update the parameters until the likelihood function value no longer changes significantly or the maximum number of iterations is reached [19]. E-step: Calculate the probability that the ith sample v x mo (t i ) belongs to the motion pattern b k as M-step: Recalculate the parameters of the Gaussian model according to the updated probability.

C. EXPRESSIONS OF THE PREDICTED VELOCITY AND POSITION OF THE OBSTACLE
After determining the motion pattern b i of the moving obstacle and the corresponding GPR model, the predicted velocity of the moving obstacle at time t is obtained by (32), that is,

VOLUME 8, 2020
If the current position P mo (t) = (x mo (t), y mo (t)) of the moving obstacle is known, the predicted position of the moving obstacle at time (t + 1) is calculated as: By analogy, the predicted position at time (t + n) is (40)

IV. TRAJECTORY PLANNING ALGORITHM CONSIDERING UNCERTAINTY
The trajectory prediction algorithm based on GMM-GPR has been introduced in Section III, and the Gaussian distributions representing the uncertainty of the moving obstacle's velocities are obtained. In the following, we will combine the trajectory planning algorithm based on the NLFVO method by considering the motion uncertainty of the obstacle to obtain a USV collision-free trajectory in an uncertain environment.

A. NLFVO METHOD WITH CONSIDERING UNCERTAINTY
As described in Section III, the two elements of v mo , v x mo and v y mo , follow Gaussian distributions. Hence, according to the 3σ rule of thumb, the velocity ranges of the moving obstacle can be obtained: According to the historical trajectory points of the moving obstacle from t 1 to t k , the uncertainty of the moving obstacle's velocity at t k+1 is described by Gaussian distributions. Therefore, the uncertainties of the moving obstacle's velocity represented by (41) and (42) are added to item N i=1 (v mo (i) t) in (21). Considering the uncertainty, the curve represented by replaced by a set of curves. The area indicated by NLFVO uo in the velocity space will be expanded accordingly.
In the time interval [t 1 , t n ], the Euclidean distance sequence between the USV and the moving obstacle is Comparing the Euclidean distance between the USV and the moving obstacle, if the following exists then it is considered that the two will collide at time t k , where t 1 ≤ t k ≤ t n . The time t k is the minimum collision time, that is τ = t k .

C. METHOD FOR OBTAINING USV COLLISION AVOIDANCE VELOCITY
According to the above contents, the velocity v usv needs to be adjusted out of the area of VO uo to avoid a collision between the USV and the moving obstacle. However, due to the kinematics of the USV, the adjustment range of the USV's velocity is limited. However, a USV at sea is subject to the rules of COLREGs. Therefore, we should choose the USV's velocity v usv to avoid collision and minimize the cost function value under the USV's kinematic constraints and the rules of COLREGs.

1) COLLISION AVOIDANCE RULES BASED ON COLREGs
According to the relative course angle between the USV and the moving obstacle, the angle ranges of the four encounters of overtaking, head-on, left crossing, and right crossing are determined by [21] and [22], as shown in Figure 6. Because the USV is unmanned, it cannot assume the responsibility of being avoided by other vehicles. Therefore, in the above four encounter situations, the USV should actively avoid the moving obstacle, and its collision avoidance trajectories are shown in Figure 7 (a), (b), (c), and (d), respectively. Based on the characteristics of USVs and the rules of COLREGs in [21] and [23], the rules for USV collision avoidance based on COLREGs are designed as follows: (1) Overtaking rule: when the relative course angle χ between the USV and the moving obstacle is within [112.5 • , 247.5 • ), the USV and the moving obstacle are in the situation of overtaking. The USV will sail along the port side of the moving obstacle, as shown in Figure 7 (a). (2) Head-on rule: when the relative course angle χ between the USV and the moving obstacle is within , the USV and the moving obstacle are in the situation of head-on. The USV will sail along the starboard side of the moving obstacle, as shown in Figure 7    course to its starboard side and sail along the tail of the moving obstacle, as shown in Figure 7 (d).

2) DYNAMIC WINDOW APPROACH
Due to the constraints of USV kinematics, the adjustment range of the velocity vector v usv is limited in a given duration. The dynamic window approach (DWA) is introduced to handle the kinematic constraints [24]. The reachable velocity of the USV in a given time window t, which is called the velocity window, is The reachable course of the USV in a given time window t, which is called the course window, is In (45), v c is the current velocity of the USV, andv ≥ 0 is the velocity acceleration of the USV. In (46), χ c is the current course of the USV, andχ ≥ 0 is the course angular velocity of the USV.
According to (45) and (46), the velocity window v d and the course window χ d of the USV within a given time window t are determined.

3) COST FUNCTION
There are several USV velocities outside V O uo in the velocity space. To select an optimized velocity vector v usv to avoid a collision, a cost function is established: where J 1 is the course variation function, and J 2 is the USV's energy consumption function. w 1 and w 2 are the weights of J 1 and J 2 , respectively. The course variation function is expressed as: where J 1 ∈ [0, 1]. As shown in Figure 8, χ(t) is the angle between the direction of v usv (t) and the X-axis direction, and θ goal (t) is the direction of the vector P goal − P usv (t) . Thus, θ goal (t) − χ(t) is the angle between the course of the USV at time t and the line connecting the USV's current position P usv (t) with the goal position P goal . The USV's energy consumption function is shown as follows [25], [26]: where ρ is the density of the water, C D is the drag coefficient, A is the reference area, and U is the relative velocity of the USV.

4) OBTAINING USV COLLISION AVOIDANCE VELOCITY
The velocity window v d and course window χ d of the USV are obtained. However, the elements in v d and χ d are continuous. Due to the large amount of calculation data, it takes a long time to obtain a velocity vector that satisfies (45) and (46), which is not conducive in practical application. Thus, the velocity window v d and course window χ d of the USV are discretized in this paper, that is, v d is dispersed into m velocities, and χ d is dispersed into n courses. In this way, a velocity vector v ij usv is composed whose magnitude is v i (1 ≤ i ≤ m) and direction is χ j (1 ≤ j ≤ n). The m × n velocities constitute a set of reachable velocities (RV ) that can be reached by the USV within a duration t, as shown in Figure 9 (a).
According to the encounter situation between the USV and the moving obstacle, the course selection of the USV is subject to the corresponding rules of COLREGs. In this paper, the set of velocity vectors prohibited by the USV based on COLREGs is defined as V REG , which is the red part shown in Figure 9 (b).
The USV needs to adjust its velocity v usv out of the area of V O uo to avoid collision with the moving obstacle. Therefore, a set of reachable avoidance velocities (RAV ) for the USV that can both avoid collision and meet the rules of COLREGs is obtained, which is shown in Figure 9 (c): The elements in RAV are calculated by the cost function in (47), and the element v usv (i) that minimizes the cost function value is selected. When v usv (i) is selected as the control target for navigation, the USV can avoid the moving obstacle while minimizing the cost function and subjecting it to the kinematic constraints and the rules of COLREGs.

V. SIMULATION STUDIES A. SIMULATIONS OF THE TRAJECTORY PREDICTION ALGORITHM 1) DATA PREPARATION
To verify the effectiveness of the proposed algorithm based on GMM-GPR, a real vehicle trajectory dataset for predicting the moving obstacle's trajectory is performed. The position information of the target vehicle sent by the GPS sensor is based on the latitude and longitude data of the WGS84 geographic coordinate system. To compare the predicted results accurately, it is essential to convert the latitude and longitude in the WGS84 coordinate system to the Cartesian coordinate system of the ground plane.
Assuming that the latitude and longitude coordinates of the target vehicle in the WGS84 coordinate system are (λ, ϕ), the position in the Cartesian coordinate system is (x, y), and the origin of the coordinate system is (λ 0 , ϕ 0 ). The Cartesian coordinate system of the ground plane takes the east as the X-axis and the north as the Y-axis. The conversion based on the Gauss-Kruger model is as follows: where In (52) and (53), d = 180 π is the conversion index, a is the major axis of the earth, e 2 is the flatness of the earth, and A, B, C, and D are the indexes.
Two different moving vehicles are selected as the prediction objects of Case 1 -2, which contain 2017 and 4441 trajectory points, respectively. Assuming that the moving obstacles do not change their motion states within the duration of t = 1 s, the time sequences of the velocity components of the moving obstacle are obtained according to Definition 1.

2) SIMULATIONS OF TRAJECTORY PREDICTION
The accuracy of the proposed algorithm in terms of prediction error is evaluated by the root mean square error (RMSE) metric defined as where N is the number of trajectories in the prediction dataset,x mo (i) andŷ mo (i) are the components of the predicted position. x mo (i) and y mo (i) are the components of the actual position. RMSE is calculated in every prediction step. By using the GMM clustering algorithm, the moving obstacle's velocity data in the observation dataset are partitioned into multiple clusters. We choose different cluster numbers to perform cluster analysis on historical velocity data in this paper. The analysis results, such as RMSE x and RMSE y between the predicted positions and the actual positions of the obstacle, and calculation time are listed in Table 1.
As shown in Table 1, when the number of clusters is large, the calculation time is long. In contrast, when the number of clusters is small, the prediction error is large. Hence, to balance the prediction effect and the calculation time, the cluster number is 15 for trajectory prediction in Case 1, and the cluster number is 20 for trajectory prediction in Case 2.  The prediction results of the moving obstacles' velocity based on GMM-GPR in Case 1 and Case 2 are shown in Figure 10 and Figure 12, respectively, where the blue solid line represents the mean values of the velocity, and the blue shaded portion represents the confidence interval of the velocity based on the 3σ rule of thumb. Based on the predicted velocity information, the trajectories of the moving obstacles are calculated. The comparisons between the actual and predicted trajectories of the moving obstacles in Case 1 and Case 2 are shown in Figure 11 and Figure 13, respectively.

B. SIMULATIONS OF USV COLLISION AVOIDANCE TRAJECTORY PLANNING
To verify the effectiveness and feasibility of the USV collision avoidance trajectory planning algorithm in uncertain environments, two sets of simulations are conducted: an encounter situation between the USV and a single moving obstacle and an encounter situation between the USV and multiple moving obstacles.

1) SIMULATIONS OF ENCOUNTER SITUATIONS BETWEEN THE USV AND A SINGLE MOVING OBSTACLE
In an uncertain environment, the starting point of the USV is (0, 0), and the target point is (500, 500). Meanwhile, to verify the effectiveness of the proposed algorithm on COLREGs  rules, we set up four sets of encounter cases. In Case 3 -6, the USV will encounter a single moving obstacle. The initial motion information of the USV and the obstacles in Case 3 -6 are listed in Table 2.
After calculation, the collision time t c between the USV and the moving obstacle in Case 3 -6 are 31 s. At collision time t c , the radius of the circle D(P, r sum ) of Obstacle 3 -6 are 25 m with considering the position noise in the kinematic model of the USV and the motion uncertainty of  the obstacle. For the USV, the velocity window v d is dispersed into 8 velocities, and the course window χ d is dispersed into 16 courses. The weights (w 1 , w 2 ) are (100, 1) in (47). The parameters in (49) are set as C D = 1 and A = 300.
The observation dataset in trajectory prediction should select the data at equal time intervals to ensure the regularity of the prediction. Hence, the first 70% of the historical velocity information of the moving obstacle is used as the observation dataset, and the remaining 30% is used as the prediction dataset. Figure 14-17 show the planned trajectories for USV collision avoidance considering the motion uncertainty of the obstacle in Case 3 -6, respectively. In Figure 14-17, the location of the USV is represented by a blue pentagon every 4 seconds, and the trajectory of the obstacle is represented by a red line. The prohibited zone D(P, r sum ) is represented as a yellow disk at t c , and the global path connecting the initial location of the USV and the location of the goal is represented by a dotted green line.
As shown in Table 2, the encounter situation of Case 3 between the USV and the moving obstacle is overtaking. According to the rules in COLREGs described in Section IV, the USV should pass along the port side of the  moving obstacle. As shown in Figure 14, the USV can obtain a collision-free trajectory that meets the rules of COLREGs on overtaking. The encounter situation of Case 4 between the  USV and the moving obstacle is right crossing. According to the rules in COLREGs, the USV should adjust the course to its starboard side and sail along the tail of the moving obstacle. As shown in Figure 15, the USV can obtain a collision-free trajectory that meets the rules of COLREGs on right crossing. The encounter situation of Case 5 between the USV and the moving obstacle is left crossing. According to the rules in COLREGs, the USV should adjust the course to its port side and sail along the tail of the moving obstacle. As shown in Figure 16, the USV can obtain a collision-free trajectory that meets the rules of COLREGs on left crossing. The encounter situation of Case 6 between the USV and the moving obstacle is head-on. According to the rules in COLREGs, the USV should sail along the starboard side of the moving obstacle. As shown in Figure 17, the USV  can obtain a collision-free trajectory that meets the rules of COLREGs on head-on.

2) SIMULATIONS OF ENCOUNTER SITUATIONS BETWEEN THE USV AND MULTIPLE MOVING OBSTACLES
The USV may encounter multiple moving obstacles that will collide with it. Therefore, a situation where the USV encounters multiple obstacles is designed in the simulation, and the proposed trajectory planning algorithm is used to enable the USV to avoid collisions with the obstacles. In the simulation, the USV will encounter moving obstacles whose initial motion information is listed in Table 3. The initial position of the USV is (0, 0), the position of the target point is (800, 800), the initial velocity of the USV is 12 m/s, and the initial course of the USV is 45 • . After calculating, the USV will collide with Obstacle 1 -5 at 32 s, 40 s, 42 s, 56 s, and 60 s, respectively. Hence, the USV needs to avoid collisions with the five moving obstacles simultaneously based on the NLVO and NLFVO methods, respectively. The motion uncertainties of the moving obstacles are also considered in the simulations. Figures 18 and 19 show the planned trajectories for the USV to avoid collision with five moving obstacles based on the NLVO and NLFVO methods, respectively. In Figure 18, the USV will collide with Obstacle 2 and Obstacle 3. In Figure 19, the USV will not collide with any obstacle because collision avoidance trajectory planning based on the NLFVO method will have more candidate velocities when encountering multiple obstacles. With more candidate velocities, the USV sailing along the planned trajectory obtained by the NLFVO method can avoid collision with Obstacle 1 -5 in an uncertain environment while constrained by its own kinematics and COLREGs.

VI. CONCLUSION
To enable USVs to avoid collisions with moving obstacles in uncertain environments, a trajectory prediction algorithm based on GMM-GPR and a collision avoidance trajectory planning algorithm based on the NLFVO method are proposed in this paper.
First, to solve the uncertainty of the obstacle's motion, the historical trajectories of moving obstacles are used to predict their trajectory. The motion patterns of the moving obstacle are modeled by GPR, and the GMM is used to select the future motion pattern with the maximum probability. Then, the predicted trajectory of the obstacle is calculated. The Gaussian distributions of the obstacle's velocity representing the uncertainty of the obstacle's future motion are obtained.
Second, the NLFVO method is proposed for USVs to effectively avoid obstacles with uncertain motion. Due to its own kinematic constraints, the velocity window and course window of the USV are determined by the DWA. COLREGs are also considered in the NLFVO method to meet the actual navigation requirements of USVs.
The simulation results show that the proposed algorithms can obtain the predicted trajectories of the moving obstacles and the USV collision avoidance trajectory in an uncertain environment.
In the future, an optimization algorithm that is both fast and accurate should be designed to obtain an optimized USV velocity vector. The position measurement noise of the USV needs to be processed by the corresponding filtering algorithm in the subsequent study.
ZHIWEI HAN received the bachelor's degree in electrical engineering and automation from Northeast Agricultural University (NEAU), in 2015. He is currently pursuing the Ph.D. degree in control science and engineering with Harbin Engineering University. His research interests include intelligent optimization algorithm, path planning and collision avoidance of unmanned surface vehicle, and thrust allocation of DP vessels.
BO ZHAO received the bachelor's degree in automation and the master's degree in navigation, guidance and control from the Beijing University of Aeronautics and Astronautics (BUAA), in 2006 and 2009, respectively, and the Ph.D. degree in marine cybernetics from the Norwegian University of Science and Technology, in 2015. From 2013 to 2018, he was with Global Maritime, as a Senior Marine System Advisor, and developed hardware-in-the-loop testing for dynamic positioning systems. He is currently working as an Associate Professor with Harbin Engineering University. His research interests include applying advanced control and artificial intelligence in the control of vessel, underwater robotics, and other marine systems.
XINWEI WANG received the bachelor's degree in electrical engineering and automation from Harbin Engineering University (HEU), in 2015, where he is currently pursuing the Ph.D. degree in control science and engineering. His research interests include ship motion control, nonlinear control theory, and intelligent control theory.