A Motion Planning Method for Unmanned Surface Vehicle Based on Improved RRT Algorithm

Abstract

Aiming at the problem that the path search rules in the traditional path planning methods are divorced from the actual maneuverability of an unmanned surface vehicle (USV), a motion planning method of state prediction rapidly exploring random tree (spRRT) is proposed. This method retains the discrete search of the original rules of RRT while adding the continuity of the motion of USV. Firstly, the state information for each movement (position, yaw angle, velocity, etc.), is calculated based on the mathematical model of USV’s motion which takes into account the complete dynamic constraints. Secondly, this information is added to the RRT path search rules to predict the state points that can be reached by the USV. Furthermore, in order to improve search efficiency and reduce cost, spRRT is enhanced by an elliptic sampling domain (spRRT-Informed). The simulation results indicate that spRRT can effectively plan smooth paths for smoothly navigating USV. The inclusion of the USV motion model has improved steering performance by an average of over 40%. Additionally, the spRRT-Informed enhanced with sampling optimization strategy improves performance by at least 10% over spRRT in terms of sailing time and distance of the path. The results of the simulation conducted in a realistic scenario validate that spRRT-Informed can be used as a reference for practical applications.

1. Introduction

As artificial intelligence develops and labor costs rise, unmanned systems are increasingly used in areas such as industrial production, supply chain logistics, and scientific exploration. The flexibility, low cost, and versatility of unmanned surface vehicles (USVs) have resulted in them being widely used by both the military and commercial sectors over the last decade, such as hydrographic surveys, environmental monitoring, maritime search and rescue, and port security [1,2,3,4,5]. However, in order to accomplish tasks in the above scenarios, path planning is without a doubt a significant technical challenge for USVs. Dynamic constraints will constrain the motion of USV as a result of its physical limitations (propeller speed threshold, thrust change rate, etc.). The path planning problem can be divided into three stages based on the different dynamic constraints: route planning, trajectory planning, and motion planning [6].

Route planning is the first step in the study of path planning problems, in which the object is usually considered a prime. Regardless of the dimensions and dynamics of the studied object, the research approach theoretically applies to mobile robots, unmanned cars, and unmanned surface vehicles. A large number of results have been achieved in this phase of research [7,8,9,10], such as the Dijkstra algorithm, A* algorithm, fuzzy logic algorithm, artificial potential field method, neural network algorithm, ant colony algorithm, particle swarm algorithm, etc. Route planning research specifically for USV is primarily concerned with optimizing route characteristics, such as path distance, path smoothness, or energy efficiency. The scenario may be set in a super-scale environment, in which the size of the ship does not matter [10,11,12,13,14]. However, in some small and complex scenarios, such as ports and inland rivers, the route planning methods are not applicable.

Trajectory (time parameterization continuous path) planning considers partial dynamic constraints such as velocity, yaw angle, and curvature. The trajectory planning study for USV is not limited to determining the path from the start point to the target point. In addition, the path must conform to one or more of USV’s physical characteristics. Wang et al. [15] simulated path planning in a realistic complex marine environment (including shore and water limitations). A multilayer path planner was proposed. In the obstacle avoidance layer, B spline curves were used to reduce the yaw angle cost value substantially. Petr Svec [16] incorporated the concept of prediction into the traditional heuristic search. A step-by-step planning strategy was developed that searches through a candidate trajectory space composed of dynamically feasible control actions. Gao et al. [17] designed a fuzzy control algorithm to dynamically adjust the weight coefficients of the velocity and obstacle distance terms in the cost function of the dynamic window approach (DWA) algorithm. All of these methods are able to obtain feasible trajectories. However, USVs have significant inertia, high drag, and a long response time compared to other unmanned systems such as UAVs and UGVs. Thus, it is still insufficient to consider only partial constraints.

Motion planning for USVs requires that all dynamic constraints be considered. The planning results are based on the motion characteristics of the USV and can be used in practical applications. Most USVs are equipped with limited propellers and rudders, which means USVs are underactuated systems [18]. Motion planning can be applied to underactuated systems in two general approaches [19]. The first approach consists of two steps [20]: Initially, an ideal route is determined by a route planning algorithm. Next, a control system model satisfying all dynamic constraints is designed to drive an underactuated system to its destination. The key to this approach lies in whether the motion control system can approximate the planned route. Essentially, this is an approach to indirect motion planning. Another approach is based on random sampling, such as the RRT algorithm and its variants [21,22,23,24]. The RRT algorithm is suitable for underactuated USVs because the extended new nodes are subject to dynamic constraints. They can be adapted to the target state by different control inputs. Ghosh et al. [25] proposed a bi-directional RRT (Bi-RRT) based kinematic constrained path planning algorithm. A limited number of nodes can be generated without sacrificing accuracy, and smooth trajectory generation can be achieved by incorporating kinematic constraints. Blanco et al. [26] developed a new RRT algorithm based on the trajectory parameter space (TP-space). The scheme establishes a navigation tree with poses as nodes whose edges are all kinematically feasible paths. Wang et al. [27] similarly proposed a method for asymptotically optimizing the kinematic constraints based on Bi-RRT. All three methods apply the control inputs $u=(v,w)$ to the path planning process, which results in a smoother path. However, in addition to the large inertia, large drag, and long response time, environmental disturbances such as wind, waves, and currents should be taken into consideration. For this reason, the random sampling-based approach needs to be further improved before it can be applied to the problem of USV motion planning.

In recent years, several scholars have examined the motion planning of underactuated equipment in the water. Du et al. [19] proposed a trajectory cell approach. This approach effectively bridges the gap between the search algorithm and the USV model, as well as achieving fine motion control in small-range scenarios. Han et al. [28] proposed a predicted trajectory approach, which takes into account all the dynamic constraints of the USV. On the basis of the predicted trajectory method, an enhancement of the global path guidance strategy is injected into the search process to increase the efficiency and accuracy of the search. Nevertheless, as the search area becomes larger, the computation time will increase significantly. Chu et al. [29] propose a path planning method based on deep reinforcement learning, which is able to overcome the disturbance of currents in an unknown environment and guide the autonomous underwater vehicle (AUV) through obstacle avoidance and to its destination. Mahmoud Zadeh et al. [30] developed a novel method of uninterrupted path planning. In order to complete a smooth circuit around the mission area, this system performs uninterrupted sampling tasks and prevents sharp changes in the USV’s heading angle. In the study of multiple USVs’ collaborative path planning problems, Tan et al. [31] considered the heterogeneities of USVs while proposing an adaptive adjustable fast marching square method based on fast marching (FM).

The contributions of this research in the field of USV motion planning can be summarized as follows: (i) a state prediction rapidly exploring random tree (spRRT) algorithm is proposed to perform motion planning for USV. This is an RRT algorithm that considers complete dynamic constraints, which is based on the maneuvering mathematical model of USV motion to predict reachable navigational state information (position, yaw angle, speed, etc.). (ii) An asymptotical optimization strategy relating to the elliptic domain is employed to enhance the sampling aspect of the algorithm in order to improve search efficiency and save path costs. Depending on the obstacle environment, spRRT-Informed is capable of setting different types of elliptic sampling domains prior to random sampling. (iii) we set up simulations for the proposed method at the original scale of 1:1.

The remainder of this paper is organized as follows: In Section 2, a mathematical model is presented briefly for describing USV motion under environmental perturbations, and a description of the problem of motion planning is given. Section 3 presents the theory related to spRRT and spRRT-Informed algorithms. Simulation results for three obstacle environments and a realistic scenario are presented and discussed in Section 4. Concluding remarks about our work are given in Section 5.

2. Preliminaries

2.1. USV Motion Mathematical Model

The motion of the underactuated USV actually behaves as a 6-degree-of-freedom spatial motion with a strong coupling phenomenon. In motion planning problems, the USV navigation process can be regarded as the 3-degree-of-freedom motion of a rigid body in the plane (surge, sway, and yaw). In order to simplify the model, two assumptions are made in this paper:

(1)

USV is symmetric about the longitudinal section amidships. The rigid body mass is constant and uniformly distributed.

(2)

Ignore the higher order damping terms and assume that the restoring force matrix is a zero array.

The USV motion is simulated by a mathematical model [32,33] based on Newtonian and Lagrangian mechanics, described as follows:

$$M\dot{\nu }+C\left(\nu \right)\nu +D\left(\nu \right)\nu ={\tau }_{man}+{\tau }_{env}$$

(1)

$$\dot{\eta }=R\left(\psi \right)\cdot \nu$$

(2)

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

(3)

where $\nu ={\left[u,v,r\right]}^{\top }$ and $\eta ={\left[x,y,\psi \right]}^{\top }$ represent the velocity (in the body reference system) and position (in the inertial reference system) in plane motion, respectively, as shown in Figure 1. $\nu$ and $\eta$ are interconverted by Formula (2). $M=M_{RB}+M_A$ is the system inertia matrix, consisting of rigid body inertia and hydrodynamic added inertia. $C\left(\nu \right)=C{\left(\nu \right)}_{RB}+C{\left(\nu \right)}_A$ is the centrifugal and Coriolis forces and moments acting on the ship. It is only effective when certain steering maneuvers are performed by the USV. $D\left(\nu \right)$ is the hydrodynamic damping forces and moments acting on the ship, ensuring that the system output is bounded.

The correlation coefficient matrix in Formula (1) is expressed as follows:

$$M=\left[\begin{array}{ccc}m-X_{\dot{u}}& 0& 0\\ 0& m-Y_{\dot{v}}& m{x}_g-Y_{\dot{r}}\\ 0& m{x}_g-Y_{\dot{r}}& I_z-N_{\dot{r}}\end{array}\right]=\left[\begin{array}{ccc}{m}_{11}& 0& 0\\ 0& m_{22}& m_{23}\\ 0& m_{32}& m_{33}\end{array}\right]$$

(4)

$$C\left(\nu \right)=\left[\begin{array}{ccc}0& 0& -\left(m_{22}v+m_{23}r\right)\\ 0& 0& m_{11}u\\ \left(m_{22}v+m_{23}r\right)& -m_{11}u& 0\end{array}\right]$$

(5)

$$D=\left[\begin{array}{ccc}-X_u& 0& 0\\ 0& -Y_v& -Y_r\\ 0& -Y_r& -N_r\end{array}\right]$$

(6)

where $m$ is the full ship mass of the USV, $I_z$ is the rotational inertia, $X\left(\cdot \right)$, $Y\left(\cdot \right)$ and $N\left(\cdot \right)$ represent the hydrodynamic coefficient. In Formula (1), the right part of the equal sign is the external input force and moment, which ${\tau }_{man}$ is the control input of the ship maneuvering system, and the environmental disturbance ${\tau }_{env}$. We consider a current field [34] with a fixed velocity and direction as an environmental disturbance in this paper. As shown in Figure 1, assuming that the current velocity $U_c$ and current direction angle ${\beta }_c$ are constant in the inertial system $X_{\mathrm{i}}{O}_{\mathrm{i}}{Y}_{\mathrm{i}}$. The current velocity in the x-axis and y-axis directions are calculated, respectively, as follows:

$$\left\{\begin{array}{c}{v}_{cx}=U_c\cdot \cos \left({\beta }_c\right)\\ v_{cy}=U_c\cdot \sin \left({\beta }_c\right)\end{array}\right.$$

(7)

The influence of the current field in the USV motion planning should be considered in terms of both current direction and current velocity. In Figure 1, it is assumed that the USV velocity $U$ at $O_b$, and the course angle relative to the axis $O_i{X}_i$ is $\chi$. The effect of the current field on the USV will be reflected in its sailing speed. $V_x$ and $V_y$ are the projections of the USV on the axis $O_i{X}_i$ and the axis $O_i{X}_i$, respectively. They can be calculated by Formula (8).

$$\left\{\begin{array}{c}{V}_x=U\cdot \cos \left(\chi \right)+v_{cx}\\ V_y=U\cdot \sin \left(\chi \right)+v_{cy}\end{array}\right.$$

(8)

2.2. Motion Planning Formulation

The global map is shown in Figure 2. This map is provided by Google Map, and the location is Dishui Lake in Lin-gang Special Area, Shanghai. Let $\mathcal{X}$ denote the state space, ${\mathcal{X}}_{obs}\in \mathcal{X}$ denote the obstacle space, and the free space is represented by a subset ${\mathcal{X}}_{free}=\mathcal{X}/{\mathcal{X}}_{obs}$. Motion planning aims to identify a collision-free path from the start point to the goal region on the global map. All dynamic constraints must be satisfied by this path. Let $\mathcal{U}\in {ℝ}^n$ denote the control space, and finding a path can be regarded as finding a set of motion control inputs $u:\left[0,T\right]\to \mathcal{U}$. The Eulerian integral representation is used in this study to express the USV motion dynamic expression:

$$x\left(t+1\right)=f\left(x\left(t\right),u\left(t\right)\right)$$

(9)

where $u\left(t\right)\in \mathcal{U}$, $x\left(t\right)$ and $x\left(t+1\right)$ denote the two neighboring states, respectively. For any $t\in \left[0,T\right]$ under $x:\left[0,T\right]\to {\mathcal{X}}_{free}$, $x\left(0\right)=x_{start},x\left(T\right)\in {\mathcal{X}}_{goal}$, the planning path is feasible if all satisfy $x\left(t\right)\in {\mathcal{X}}_{free}$.

3. Main Algorithms

In this section, the proposed spRRT algorithm and its enhanced spRRT-Informed are described in detail. The basic RRT algorithm is improved by an extension rule based on state prediction to obtain the spRRT algorithm. In order to enhance the search efficiency, an; improved elliptic sampling domain strategy is used to obtain the spRRT-Informed algorithm.

3.1. Extension Rule Based on State Prediction

RRT algorithm discretizes the continuous space by extending a step size. However, the basic RRT algorithm does not consider the actual manipulability of USV when setting the extension step. It adopts the extension rule that is reachable in all directions, which will largely reduce the adaptability of the planning results on the USV platform. In this paper, we retain the basic RRT topology tree notation as ${\mathcal{T}}_W$, which is responsible for exploring the space. An additional tree [21] generated by the USV motion mathematical model is noted as ${\mathcal{T}}_S$, which is responsible for the auxiliary extension. Considering that the lateral thrust acting on the underactuated USV is small during the motion of the USV, the lateral thrust is assumed to be zero. In order to simplify the model, the USV is configured to be fully propelled, and the longitudinal thrust is set to be a constant. A control moment space $M_s$ [28] is formed by only retaining the yaw moment as a control input.

Extend function based on state prediction is shown in Algorithm 1.

Algorithm 1 Extend function

1.  Function $Extend\left(T_W,T_S,q_{rand},M_s\right)$

2.      $Q_{nearest}\leftarrow NearestNeighbour\left(T_W,q_{rand}\right);$

3.      $Q_{new}\leftarrow Steer\left(Q_{nearest},q_{rand}\right);$

4.      if $ObstacleFree\left(Q_{nearest},Q_{new}\right)$ then

5.        for each moment ${\tau }_r\left[i\right]$ in $M_s$ do

6.          $X_{new}\leftarrow PredictedState\left(Q_{new},{\tau }_r\left[i\right]\right);$

7.          if $TrajectoryFree\left(X_{new}\right)$ then

8.            ${\mathcal{T}}_W.Add\left(Q_{new}\right);$

9.            ${\mathcal{T}}_S.Add\left(Q_{new}\right);$ 10.  return $X_{new},Q_{new};$

11.  End Function

In Algorithm 1, Line 2–3, space is extended in the same manner as in the basic RRT algorithm. As shown in Figure 3a, $q_{rand}$ is obtained by sampling the entire space at random. $Q_{nearest}$ is determined by traversing the entire ${\mathcal{T}}_W$ tree (${\mathcal{T}}_W$ tree nodes are marked with capital letters Q). In order to obtain the extended node $Q_{new}$, the tree is grown step by step starting from $Q_{nearest}$.

Algorithm 1, Line 4 detects whether the proposed extended node $Q_{new}$ collides with an obstacle. If there is no collision, it temporarily connects $Q_{new}$ to the nearest ${\mathcal{T}}_W$ tree node by a dashed line and goes on to the next step.

In Algorithm 1, Line 5–6, the state points of USV are predicted based on the mapping of $Q_{nearest}$ in the ${\mathcal{T}}_S$ tree (i.e., it is both a single and a full mapping) based on $X_{init}$ (the nodes of the ${\mathcal{T}}_S$ tree are denoted by capital letters X). Initially, the moment space $M_s$ is discretized into a series of feasible yaw moments, each denoted as ${\tau }_r\left[i\right]$. In this case, Formula (10) is obtained by updating Formula (9), where $x_0$ denotes the state property of $X_{init}$, $∆t$ refers to the prediction time, and k = [1, 2, … , 10] indicates the number of predicted state points for a single yaw moment. According to Formulas (1), (2), and (10), the predicted USV motion states are shown in Figure 3b. An effective extended region is established by using $Q_{new}$ as the center of a circle and Er as the radius. As shown in Figure 3c, the predicted state point falling within it is noted as $X_{new}$.

$$x_{}=x_0+{{\displaystyle \int }}_0^{k*∆t}f\left(x_0,{\tau }_r\right)dt$$

(10)

In Algorithm 1, Line 7–8, it is determined whether the trajectory property of $X_{new}$ collides with an obstacle. $X_{new}$ is inserted into the ${\mathcal{T}}_S$ tree if there is no collision. In the meantime, $Q_{new}$ is inserted into the ${\mathcal{T}}_W$ tree to complete this extension, as shown in Figure 3d.

3.2. Sampling Performance Enhancement

The RRT algorithm is susceptible to blind sampling and low search efficiency if it is explored randomly in the space. In other words, there is no probability that the RRT algorithm will construct an optimal path within a finite number of iterations [35]. In order to enhance the sampling aspect of the algorithm flow, an asymptotical optimization strategy [35,36] is used. The Informed-RRT* algorithm is composed of two phases, the initial path-finding phase and the initial path being found phase. In the previous phase, the process is the same as that in the basic RRT algorithm. In the later phase, the elliptic sampling domain is determined based on the found path length, the start point, and the goal position, as well as the distance $c_{\min }$ between these two points, as shown in Figure 4. The long-axis distance of the elliptic sampling domain is the current path $c_{best}$, whereas the short-axis distance is $\sqrt{c_{best}{}^2-c_{min}{}^2}$.

Considering the properties of an ellipse, the distance between the points within the domain and the two focal points is longer than the distance between the points on the side of the ellipse. As a result, the total length of the paths generated within the elliptic sampling domain will be shorter than the length of the paths generated outside of the elliptical sampling domain. A reduction in the length of the path $c_{best}$ is persistently observed as the algorithm searches. In this approach, the elliptical sampling domain is continually reduced, which ultimately results in optimization.

This study proposed a strategy for the improvement of the elliptic sampling domain based on the complexity of the map environment. The elliptic sampling domain is set in the above manner when no obstacle stands between the start point and the goal position. If the path between these two is blocked by obstacles, a double elliptic sampling domain should be used instead of the single elliptic sampling domain. Initially, a transition point is established in the approximate center of the map. Next, an elliptic sampling domain is constructed with the start point and transition point as focal points. Furthermore, as shown in Figure 5, another elliptic sampling domain is constructed with the transition point and goal position as focal points. The purpose of this setup is to increase the sampling probability of the center of the sampling domain (as shown in Figure 5) while maintaining the advantages of a single ellipse sampling domain. Due to the fact that optimal paths often require intermediate positions. Thus, the number of excellent nodes can be increased by increasing the sampling probability of the middle region. The algorithm is improved in terms of its ability to seek optimization.

3.3. Algorithm Summary

The flow of the spRRT-Informed algorithm is shown in Algorithm 2. An example of spRRT-Informed trace is shown in Figure 6.

Algorithm 2 spRRT-Informed

Input: $Q_{init}$, $X_{init}$, ${\mathcal{X}}_{goal}$, ${\mathcal{X}}_{center}$, map,$M_s$

Output: $X_{best}$, $c_{best}$

1.  ${\mathcal{T}}_W\leftarrow Q_{init}$

2.  ${\mathcal{T}}_S\leftarrow X_{init}$

3.  Flag $\leftarrow isFree\left(Q_{init},{\mathcal{X}}_{goal}\right)$

4.  while ($ite{r}_{max}$)

5.      if Flag == TRUE then

6.        $c_{best}\leftarrow RRTplanner\left(Q_{init},{\mathcal{X}}_{goal},map\right)$;

7.        $q_{rand}\leftarrow RandomSample\left(cbest,map\right)$;

8.        $\left[X_{new},Q_{new}\right]\leftarrow Extend\left(T_W,T_S,q_{rand},M_s\right);$

9.        $\left[{\mathcal{T}}_W,{\mathcal{T}}_S\right]\leftarrow UpdateStructure\left({\mathcal{T}}_W,{\mathcal{T}}_S\right)$;

10.      if Flag == FALSE then

11.        $\left[c_{best1},c_{best2}\right]\leftarrow RRTplanner\left(Q_{init},{\mathcal{X}}_{center},{\mathcal{X}}_{goal},map\right);$

12.        $q_{rand}\leftarrow RandomSample\left(c_{best1},c_{best2},map\right)$;

13.        $\left[X_{new},Q_{new}\right]\leftarrow Extend\left(T_W,T_S,q_{rand},M_s\right);$

14.        $\left[{\mathcal{T}}_W,{\mathcal{T}}_S\right]\leftarrow UpdateStructure\left({\mathcal{T}}_W,{\mathcal{T}}_S\right)$;

15.  return;

(1)

Initialization Stage, Line 1–2, Algorithm 2: The spRRT-Informed algorithm maintains both the spatial search tree $T_W$ and the auxiliary extension tree $T_S$. The root nodes of both trees, $Q_{init}$ and $X_{init}$, are initialized at the start point.

(2)

Sampling Stage, Line 3, 5–7 or Line 3, 10–12, Algorithm 2: For the purpose of conducting official sampling, it is necessary to determine whether obstacles prevent access to the goal position and the start point. As described in Section 3.2, if the concatenation is blocked, ${\mathcal{X}}_{center}$ is initialized and the double elliptic sampling domain is used. Conversely, a single elliptic sampling domain is used. Different types of elliptic domains follow similar sampling steps: The shortest distance $c_{best}$ (the two distances in the case of the double elliptic sampling domain, $c_{best1}$ and $c_{best2}$) is output by the basic RRT algorithm and saved in the database. After the elliptic sampling domain is constructed, the random position $q_{rand}$ is sampled and output. The value of $c_{best}$ (the two distances in the case of the double elliptic sampling domain, $c_{best1}$ and $c_{best2}$) is updated only when the basic RRT outputs a shorter distance.

(3)

Extension Stage, Line 8 or Line 13, Algorithm 2: This stage has been described in Section 3.1 in more detail. As shown in Figure 6, the blue dashed line indicates the predicted states. If $X_{new}$ falls within the goal region, then the path will be backtracked to the start point. According to Figure 6, the path produced by the backtracking is valid.

(4)

Optimization Stage, Line 9 or Line 14, Algorithm 2: Because of the inclusion of flow field disturbances, the path cost is replaced by the time cost [37]. In order to reduce time costs, the structure of the two trees is optimized at the end of each iteration. If a child node is unfeasible, it can be repropagated or deleted [38]. During each iteration, the success rate of extending the predicted states can only be increased by as many child nodes as possible. Figure 7 illustrates the process of optimizing the tree structure: As shown in Figure 7a, an optimization region is defined with the node $Q_{new}$ as its center and $O_r$ as its radius. A traversal of the $T_W$ tree is performed in order to identify all nodes within the optimization region except for the parent node of $Q_{new}$. Next, the $T_S$ tree mapping $X_{new}$ of $Q_{new}$ is used as the initial state to predict states. If the cost of the replacement state point is less than the original cost, as shown in Figure 7b,c, the wiring of the two trees is updated. In the end, the parents of the updated node in both trees are passed to the child as new parents, thus ending this optimization.

4. Simulation Results

In order to verify the superiority of the algorithm, simulations are conducted in three different types of obstacle environments. Simulation in Dishui Lake scenario is performed to test the ability of the spRRT-Informed algorithm to simulate the real application. All algorithms are implemented in MATLAB 2021b and executed on Intel Core 7 CPU 2.2 GHz with 16 GB RAM.

Considering the underactuated nature of USV, CybershipII [39] is used as a simulation experimental subject in this study. CybershipII has a maximum speed of 0.581 m/s, a full ship mass of 23.8 kg, a length of 1.255 m, and a width of 0.29 m. Due to the size of CybershipII, the environmental scale is set at 50 m × 50 m. As shown in

Table 1. Simulation parameters of CybershipII.

Description	Value
Unit predicted time $∆t$	1 s
Period predicted time $T$	10 s
Resolution of yaw moment $∆{\tau }_r$	0.02 N × m
Maximum values of yaw moment ${\tau }_{r\max }$	0.2 N × m
Minimum values of yaw momen ${\tau }_{r\min }$	−0.2 N × m
Surge force ${\tau }_u$	2 N × m
Longitudinal current field velocity $v_{cx}$	0.2 m/s
Transverse current field velocity $v_{cy}$	0.1 m/s

, the parameters related to the prediction state are presented. Parameters related to RRT are: step size is 3 m, tree structure optimization radius $O_r$ is 5 m, and prediction effective radius $E_r$ is 0.5 m.

4.1. Performance Analysis of Algorithms

4.1.1. Scenario 1

We set up three obstacles of different configurations. The start point is [5,5], the goal region consists of a circle with coordinates [45,45] as the center and 3 m as the radius, and the initial state of the USV is [0,0,0,5,5,0]. The simulation results for the four algorithms RRT*, Informed-RRT*, spRRT, and spRRT-Informed when the maximum number of iterations is set to 1000 are shown in Figure 8.

Despite the fact that the RRT* algorithm can obtain the path in a short time, the path shown in Figure 8a has many redundant nodes. As a result of the lack of consideration of dynamic constraints on USV in the extension operation, most newly created nodes become unfeasible. For example, many steering angles on the path are beyond the USV’s actual performance range. Compared with the RRT* algorithm, the spRRT algorithm and spRRT-Informed algorithm can output smooth and feasible paths considering the USV complete dynamic constraints. Each of the four algorithms is tested 100 times and the resulting path characteristic parameters are shown in

Table 2. Path characteristic parameters under scenario 1.

Methods	Performances	Mean	Min	Max	σ
RRT*	Sailing time (s)	-	-	-	-
	Distance (m)	57.559	55.4969	58.972	0.908
	Steering angle (rad)	0.284	0.002	0.957	0.202
Informed-RRT*	Sailing time (s)	-	-	-	-
	Distance (m)	55.099	54.818	56.385	0.485
	Steering angle (rad)	0.155	4.795 × 10−4	0.937	0.149
spRRT	Sailing time (s)	124.40	106.2	138.6	8.916
	Distance (m)	71.618	61.120	92.576	8.795
	Steering angle (rad)	0.0920	3.338 × 10−4	2.264	0.335
spRRT- Informed	Sailing time (s)	106.92	103.8	110.0	0.885
	Distance (m)	62.212	60.114	64.301	1.169
	Steering angle (rad)	0.0696	1.030 × 10−4	1.038	0.229

.

As the USV dynamic constraint is not considered, the sailing time of RRT* or Informed-RRT* cannot be calculated. The inclusion of the USV motion model improves steering performance by over 40.645%. The simulation results of spRRT and spRRT-Informed demonstrate that two parameters, the path distance and steering angle, can be applied to USV. Compared to spRRT, spRRT-Informed with a double elliptic sampling domain has an average reduction of 14.051% in the performance parameter of sailing time. The standard deviation is reduced by 90.074%. In the performance parameter of distance, spRRT-Informed is on average 13.134% shorter compared to spRRT, and the standard deviation is reduced by 86.708%.

4.1.2. Scenario 2

In scenario 2, the line from the start point to the goal region is blocked by obstacles. All obstacles need to be “bypassed”. The start point is [5,35], the goal region consists of a circle with coordinates [45,10] as the center and 3 m as the radius, and the initial state of the USV is [0,0,0,5,35,−pi/2]. When the maximum number of iterations is set to 1000, the simulation results comparing the four algorithms of RRT*, Informed-RRT*, spRRT, and spRRT-Informed are shown in Figure 9. Each of the three algorithms is tested 100 times and the resulting path characteristics parameters are shown in

Table 3. Path characteristics parameters under scenario 2.

Methods	Performances	Mean	Min	Max	σ
RRT*	Sailing time (s)	-	-	-	-
	Distance (m)	74.946	70.407	82.222	2.098
	Steering angle (rad)	0.382	8.553 × 10−4	1.941	0.367
Informed-RRT*	Sailing time (s)	-	-	-	-
	Distance (m)	71.372	69.058	72.989	0.564
	Steering angle (rad)	0.320	1.850 × 10−4	1.766	0.347
spRRT	Sailing time (s)	176.3	150.0	258.0	29.570
	Distance (m)	100.563	80.869	139.871	15.988
	Steering angle (rad)	0.0878	3.103 × 10−4	1.968	0.287
spRRT- Informed	Sailing time (s)	154.48	138.0	170.4	4.469
	Distance (m)	93.890	79.636	108.512	8.385
	Steering angle (rad)	0.0786	4.671 × 10−4	1.343	0.259

.

In scenario 2, the inclusion of the USV motion model improves steering performance by over 72.563%. In the performance indicator of sailing time, the spRRT-Informed has an average reduction of 12.377% compared to the spRRT. The standard deviation is reduced by 84.887%. In the performance indicator of distance, spRRT-Informed is on average 6.636% shorter compared to spRRT. The standard deviation is reduced by 47.554%. According to the analysis of the simulation results in Scenario 1, spRRT-Informed offers a significant advantage in terms of algorithm exploration performance over spRRT. As shown in Figure 10, in the case of different elliptic domains, the sailing time of optimal paths is examined at 100 intervals over a maximum of 1000 iterations. It is evident from the descent curves in the figure that, in a “bypassed” environment, the double elliptic sampling domain performs better in finding the optimal solution.

4.1.3. Scenario 3

An environment in which the line from the start point to the goal position is not blocked by obstacles. The start point is [5,42], the goal region consists of a circle with coordinates [45,5] as the center and 3 m as the radius, and the initial state of the USV is [0,0,0,5,42,−pi/4]. When the maximum number of iterations is set to 1000, the simulation results comparing the four algorithms of RRT*, Informed-RRT*, spRRT, and spRRT-Informed are shown in Figure 11. Each of the three algorithms is tested 100 times and the resulting path characteristics parameters are shown in

Table 4. Path characteristics parameters under scenario 3.

Methods	Performances	Mean	Min	Max	σ
RRT*	Sailing time (s)	-	-	-	-
	Distance (m)	53.020	51.759	54.216	0.661
	Steering angle (rad)	0.165	4.130 × 10−4	1.010	0.143
Informed-RRT*	Sailing time (s)	-	-	-	-
	Distance (m)	52.098	51.608	52.791	0.329
	Steering angle (rad)	0.038	2.230 × 10−4	0.226	0.041
spRRT	Sailing time (s)	99.54	91.6	112.0	5.397
	Distance (m)	57.224	52.756	64.453	3.119
	Steering angle (rad)	0.0562	2.110 × 10−4	1.327	0.188
spRRT- Informed	Sailing time (s)	92.2	89.0	96.4	1.778
	Distance (m)	53.214	51.644	56.464	1.027
	Steering angle (rad)	0.0571	1.055 × 10−4	0.899	0.167

.

In the performance indicator of sailing time, the spRRT-Informed has an average reduction of 7.374% compared to the spRRT. The standard deviation is reduced by 67.056%. In the performance indicator of distance, spRRT-Informed is on average 7.008% shorter compared to spRRT. The standard deviation is reduced by 67.073%. In scenario 3, a single elliptic sampling domain is selected because there is no obstruction in the line between the start point and the goal region. As shown in Figure 12, when the double elliptic sampling domain is used, the optimal paths are viewed at 100 intervals over 1000 maximum iterations. It can be concluded from the time cost values shown in Figure 12 that the use of a double elliptical sampling domain can improve the overall convergence by searching for better paths with fewer iterations.

4.1.4. Dishui Lake scenario

Due to the scale of the Dishui Lake scenario being 630 times that of the three obstacle scenarios, intermediate waypoints (shown in Figure 13a) are set accordingly. The start point is [1520,230], the goal region consists of a circle with coordinates [300,300] as the center and 3 m as the radius, and the initial state of the USV is [0.5,0,0,1520,230,−pi/4].

The goal region is eventually reached by cruising each waypoint. The result of spRRT-Informed for motion planning is demonstrated in Figure 13b. spRRT-Informed can plan a time-parametrized continuous path for USV in a real-world scenario. This path has no collisions and is a feasible path from the start point to the goal region. As shown in Figure 14, during the planning process, the information on each status point of the USV is displayed. It can be seen from various data that there is no sudden start and stop or sharp change in heading angle, which verifies the stability of USV navigation. Therefore, the spRRT-Informed algorithm can be used as a reference for practical applications.

The simulations above are all performed while maintaining the original size of the USV and without adjusting the resolution of the map. USV motion planning is tested with three small-scale obstacle scenarios at 50 m by 50 m. The spRRT-Informed is capable of supporting simulations related to traction simulation in the Dishui Lake scenario. Simulation results confirm the theoretical feasibility of the method being applied to real USV autonomous navigation. However, spRRT and spRRT-Informed can only be used for motion planning in situations in which global information is available. It is necessary to improve the algorithms when dynamic obstacles or time-varying environmental disturbances are present in the scene.

5. Conclusions

In this paper, we propose spRRT based on the RRT algorithm and its improved spRRT-Informed for USV motion planning. The complete dynamic constraints of the USV are taken into account in the proposed methods. The spRRT improves the extended link of the basic RRT algorithm by incorporating the USV manipulative motion mathematical model. In order to improve search efficiency and save path costs, the elliptic sampling domain in spRRT-Informed is set to optimize sampling based on the type of obstacle environment.

According to the simulation results obtained in three types of obstacle environments, spRRT and spRRT-Informed algorithms solve the problem of planning paths that do not match the actual performance of USVs. The spRRT-Informed with optimized sampling shows better performance in terms of path characteristics, with more than 30% improvement in algorithmic search performance over spRRT. The optimization performances of single and double elliptic domains are also compared in the scenario 3 simulation. The comparison result shows that the double elliptic domain gives the algorithm faster convergence. Simulation in the Dishui Lake scenario validates the practical feasibility of the spRRT-Informed algorithm.

The time cost is used as a reference for evaluating the path since perturbations in the flow field are considered. The USV may be able to avoid unnecessary energy consumption in future practical applications as a result of this. However, as shown in Figure 12b, considering only the time cost requires a certain amount of path smoothness and security to be sacrificed. Therefore, in future research, the problem of the interaction and implication of optimization objectives such as time cost, path smoothness, and path security arising from the motion planning process needs to be addressed.