Trajectory Planning of USV: On-Line Computation of the Double S Trajectory Based on Multi-Scale A* Algorithm with Reeds–Shepp Curves

Abstract

Trajectory planning aims to provide a time-related control target that contains the concerned states. For an underactuated surface vehicle (USV), planning challenges include limitations on curvature, speed, acceleration, and jerk. These challenges are relevant for the precise control of USVs. To solve these problems, an on-line double S method and multi-scale A* trajectory planning algorithm is proposed by integrating Reeds–Shepp curves (RSC), where a quad-tree-based graph is used for path planning and collision detection. Simulations illustrate that the proposed method has a better performance than the smooth rapid-exploration random tree (smooth-RRT) method and jump point search (JPS) method, and that path length and the state limitations are satisfied.

1. Introduction

The planning of suitable trajectories is a key process in order to avoid undesired effects such as vibrations or even damage to the mechanical structure of USVs. This paper deals with problems related to global trajectories for the actuation system of USVs.

Commonly used planning methods are the line-of-sight (LOS) method [1], dynamic virtual ship (DVS) method [2,3], and coordinate transformation method [4,5]. They deal with local planning to avoid collisions and provide motion guidance. Generally, local planning is based on a pre-designed global path, derived using methods such as A*, RRT*, and ant colony algorithms. Besides finding a path, the satisfaction of constraints including smoothness and other limitations are also important for path or trajectory planning. In [6], an improved A* algorithm was proposed for USVs to solve map resolution constraints and increase the path smoothness. Reference [7] increased the number of neighboring nodes in an A* algorithm so that smoothness was improved. These research papers explored the configuration of the A* algorithm, but their computational load was relatively heavy. More commonly adopted methods are other variants of the A* algorithm such as the Theta* algorithm [8] or JPS algorithm [9]. Their searching speed is much faster. Reference [10] proposes a hybrid A* algorithm, which allows a continuous state association between grids and guarantees the feasibility of path tracking. In [11] and [12], a smooth-RRT* and an ant colony algorithm were used. The two methods introduced random searching in the scheme, which were efficient for general situations, but unsuitable for areas with narrow connections, such as dumbbell-shaped areas.

There are two main factors of concern in global planning: motion constraints and searching speed. Motion constraints of a USV consist mainly of kinodynamic constraints and non-holonomic constraints. In [13], an admissible velocity propagation method was proposed, that enabled path-velocity decomposition to discover truly dynamic motions. In [14], a framework was presented to extend a RRT algorithm to plan the motion of a wheeled robot under kinodynamic constraints. In [15], the double S trajectory planning method was proposed, which guaranteed a smooth change of speed and acceleration. For non-holonomic constraints, Reeds–Shepp curves [16] or Dubins curves [17] are commonly used. By setting a maximum curvature, the whole path meets the holonomic constraints.

As the demand for precision is different in different positions and environments, multi-scale algorithms are widely applied for searching speed improvements. Reference [18] clarified how to build a quad-tree-based multi-scale map. References [19,20] used multi-scale methods to build maps for unmanned aerial vehicles (UAVs) and wheeled robots, respectively. Reference [21] used a multi-scale Theta* algorithm to plan a path across the Singapore Strait for a USV. They all achieved good planning results.

Based on the methods and analysis above, the main contributions of this research are given as follows: (1) a multi-scale planning graph is established through a quad-tree method, resulting in fast searching. (2) Trajectories can be planned with only initial and final states; curvature constraints are satisfied by Reeds–Sheep curves (RSC), and kinodynamic constraints are satisfied by an on-line double S planning method.

2. Establishment of Multi-Scale Graph

In this research, a quad-tree method is applied to build a graph with finite rectangular regions. The basic idea of the quad-tree is to recursively divide geographic space into different levels of the tree structure. It divides the space of the known range into four equal subspaces, and recurses until the tree reaches a certain depth or does not contain any obstacles. The structure of the quad-tree is simple, and has a high efficiency for spatial data insertion and query. The structure of a regular quad-tree is shown in Figure 1. Geospatial objects are stored on leaf nodes, while internal nodes do not store geospatial objects.

There are several functions that can be achieved through quad-tree. For example, to find which region a point belongs to, the search moves down the tree until it reaches a leaf node, or the lowest level of the tree. This node defines the smallest region in the graph that contains the point. All other points that fall within this region have the same representation in the tree. To find the nodes of neighboring regions of a region on a multi-scale map, a grid of points around the search point can be constructed. After removal of duplicates, the quad-tree regions that contain these points are the neighbors of the initial region. To test for obstacles through the LOS method, sight lines are replaced by a series of points on the line or lines. In practice, generally discretization with small steps on the sight line is enough (Figure 2a). If one needs an assured detection in spite of the possibility of obtaining a longer path, the parallel lines as in Figure 2b should be tested as well. Figure 2 illustrates how the two LOS detection schemes work. If a test point belongs to an obstacle node, this sight line is seen as blocked.

In this research, a part of Long Beach Harbor is taken as the planning environment. The land and obstacles are transferred into points by image processing, so that the quad-tree decomposition can be carried out. Figure 3 is the original navigation chart; Figure 4 is the corresponding multi-scale graph. The nodes containing the colored points are defined as obstacle nodes. Others are traversable nodes. Note, however, that the nodes surrounded by obstacle nodes are untraversable. For this reason, points inside the seashore are not defined as obstacles, to avoid unnecessary calculations.

3. Path Planning Based on Multi-Scale A* Algorithm and RSC

With the established multi-scale graph and neighboring region-searching method, the A* algorithm can be used directly. The implementation of the A* algorithm is given as Algorithm 1, where N is a node, F(N) is the cost of the node, which consists of g(N), the moving cost from start point to node N, and h(N) the heuristic cost.

$$F\left(N\right) = g\left(N\right) + h\left(N\right)$$

(1)

Algorithm 1 A* algorithm

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17

OPEN //the set of nodes to be evaluated  CLOSED //the set of nodes already evaluated  add the start node to OPEN  loop   current=node in OPEN with the lowest cost F(N)   remove current from OPEN   add current to CLOSED   if current is the target node //path has been found  return   foreach neighbor of the current node  if neighbor is not traversable or neighbor is in CLOSED  skip to the next neighbor   if new path to neighbor has smaller f_cost or neighbor is not in OPEN  set f_cost of neighbor  set parent of neighbor to current  if neighbor is not in OPEN  add neighbor to OPEN

By the direct use of the multi-scale A* algorithm, the node connections and path planning result are shown as Figure 5.

From Figure 5, it can be concluded that the pure A* algorithm causes unnecessary zig-zag paths. This problem may be not that serious in a normal grid-based A* algorithm because it has more nodes than a multi-scale A* algorithm, and the path it produces can easily be corrected with a smoothing algorithm. However, in the multi-scale scheme, some nodes are large in size, meaning that the zig-zags in the path cannot be smoothed easily. To resolve this problem, at each step before checking whether the neighbor of a node is traversable or not by the LOS method in the quad-tree, the parent to the neighbor is checked first. If it is traversable, the parent node of the current node is set as the parent of this neighbor, and corresponding operations are made thereafter. Modifications can be inserted between line 9 and line 10 as:

1 2 3 4 5 6 7 8

foreach neighbor of the current node  if neighbor is not traversable from its parent or neighbor is in CLOSED  skip to the next neighbor  if new path to neighbor has smaller f_cost or neighbor is not in OPEN  set f_cost of neighbor  set parent of neighbor to parent  if neighbor is not in OPEN  add neighbor to OPEN

The effect of this modification is shown in Figure 6. Unnecessary zig-zags are avoided after modification.

The path under this modified A* algorithm has a much better performance in path length and turnings. Then, the path is smoothed by the G2CBS path-smoothing algorithm [22], as shown in Figure 7. By now, this path can be seen as a satisfactory one for vehicles without non-holonomic constraints. However, for a USV, because the initial and end states are not arbitrary, this path is still not suitable.

RSC [23] refers to the shortest path with, at most, two cusps or reversals, where the direction of the starting point and the end point and the maximum curvature of the path are specified. As long as there is a collision-free path between two nodes, there will be a collision-free RSC between the two nodes. In [10], this method is used to plan the arrival part of the path for a hybrid A* algorithm with impressive results. In light of their thinking, this research applied RSC at the beginning and end parts of the path to facilitate the berthing process. When using this method, penalties should be adjusted from pure distance to weighted distance, so that long backward motions are avoided.

With these methods, and setting the maximum curvature as 0.005, initial pose as [x₀, y₀, Ψ₀] = [4138, 417, 0], and final pose as [x₁, y₁, Ψ₁] = [2548, 3777, 0], the path-planning result is shown in Figure 8. The hollow circles denote nodes that are added once into the open set, and crosses correspond to the closed set. The benefits of the multi-scale method are visible, in that the number of opened nodes is small in spite of the large covered region. Solid circles denote zero-speed points, which are used to denote points where the direction of motion changes in a complex route.

Smooth-RRT and JPS are widely used path-planning methods. Figure 9 and Figure 10 illustrate the plans they generate for the same problem; evaluation metrics are summarized in

Table 1. Comparison to common planning algorithms.

Algorithms	Path Length (m)	Time Used (s)	Nodes Expanded	Are Constraints Met
Proposed method	4962.7	3.3	68	Yes
Smooth-RRT	5617.6	74.0	549	Yes
JPS	4655.4	1.7	21	No

for comparison. The three algorithms were executed using the same devices, so the comparison reflected their complexity. It can be concluded that the proposed method is much faster than smooth-RRT and is close to the speed of JPS (grid size 10 m). JPS is the fastest method, but the constraints are not satisfied. Notably, JPS can only find paths on regular grid maps, and the nodes or edges on the map cannot be weighted. As a result, the path is next to the seashore and increases the risk of collision. Compared to JPS, the route obtained by the proposed method is a bit longer, but it is completely within the safety margin. Expansions are the nodes checked during path planning, and the amount of them directly effects the speed of path planning. Multi-scale A* only uses 12.4% of the nodes used in smooth-RRT. JPS uses 3.8% of the nodes of smooth-RRT, but the constraints are not satisfied.

4. On-Line Computation of the Double S Trajectory

On-line computation of double S trajectories is suitable for paths that include several double S segments, and it is defined in discrete time. At each step the parameters are calculated based on current states, so this trajectory calculation is called on-line. Let

$$\{\begin{array}{ll}q\left(t=k{T}_s\right)\hfill & =q_k\hfill \\ \dot{q}\left(t=k{T}_s\right)\hfill & ={\dot{q}}_k\hfill \\ \ddot{q}\left(t=k{T}_s\right)\hfill & ={\ddot{q}}_k\hfill \\ \stackrel{⃛}{q}\left(t=k{T}_s\right)\hfill & ={\stackrel{⃛}{q}}_k\hfill \end{array}$$

(2)

denote the values of position, velocity, acceleration, and jerk at the k-th time instant, respectively, and let T_s denote the sampling period. The structure of the trajectory planner is shown in Figure 11. Given the initial and final values of position, velocity, and acceleration and their constraints (v_max, v_min, a_max, a_min, j_max, j_min), recursion to the next time step is computed as Equation (3). During the planning process, jerk is only selected from three values, j_max, j_min, and 0, to approach all the desired constraints and values.

$$\begin{array}{l}{\ddot{q}}_k={\ddot{q}}_{k-1}+\frac{T_s}{2}\left({\stackrel{⃛}{q}}_{k-1}^{}+{\stackrel{⃛}{q}}_k^{}\right)\\ {\dot{q}}_k={\dot{q}}_{k-1}+\frac{T_s}{2}\left({\ddot{q}}_{k-1}+{\ddot{q}}_k\right)\\ q_k=q_{k-1}+\frac{T_s}{2}\left({\dot{q}}_{k-1}+{\dot{q}}_k\right)\end{array}$$

(3)

The computation of the trajectory is composed of two phases [15]; Figure 12 is an intuitive illustration of the process.

Phase 1: Acceleration and constant velocity phase

An acceleration profile is computed with the classical trapezoidal acceleration, possibly followed by a constant maximum velocity phase. The jerk is calculated as

$${\stackrel{⃛}{q}}_k=\{\begin{array}{c}{j}_{\max }, \mathrm{if} {\dot{q}}_k-\frac{{\ddot{q}}_k^2}{2j_{\min }}<v_{\max }\hspace{1em}\mathrm{and}\hspace{1em}{\ddot{q}}_k<a_{\max }\\ 0, \mathrm{if} {\dot{q}}_k-\frac{{\ddot{q}}_k^2}{2j_{\min }}<v_{\max }\hspace{1em}\mathrm{and}\hspace{1em}{\ddot{q}}_k\ge a_{\max }\\ j_{\min }, \mathrm{if} {\dot{q}}_k-\frac{{\ddot{q}}_k^2}{2j_{\min }}\ge v_{\max }\hspace{1em}\mathrm{and}\hspace{1em}{\ddot{q}}_k>0\\ 0, \mathrm{if} {\dot{q}}_k-\frac{{\ddot{q}}_k^2}{2j_{\min }}\ge v_{\max }\hspace{1em}\mathrm{and}\hspace{1em}{\ddot{q}}_k\le 0\end{array}$$

(4)

Phase 2: Deceleration phase

During the motion, at each time instant kT_s it is checked to determine whether deceleration from the current velocity ${\dot{q}}_k$ to the final one v₁ is possible with the constraints on ${\ddot{q}}_k$ and ${\stackrel{⃛}{q}}_k$, and with the goal to reach exactly q₁. To verify this, the position displacement produced by the acceleration and velocity profiles is obtained as

$$h_k=\frac{1}{2}{\ddot{q}}_k{T}_d^2+\frac{1}{6}\left(j_{\min }{T}_{j2a}\left(3T_d^2-3T_d{T}_{j2a}+T_{j2a}^2\right)+j_{\max }{T}_{j2b}^3\right)+T_d{\dot{q}}_k$$

(5)

where corresponding parameters are calculated by Equation (6) or Equation (7) as follows:

If minimum acceleration a_min is reached, one obtains

$$\begin{array}{l}{T}_{j2a}=\frac{a_{\min }-{\ddot{q}}_k}{j_{\min }}\\ T_{j2b}=\frac{a_0-a_{\min }}{j_{\max }}\\ T_d=\frac{v_1-{\dot{q}}_k}{a_{\min }}+T_{j2a}\frac{a_{\min }-{\ddot{q}}_k}{2a_{\min }}+T_{j2b}\frac{a_{\min }-a_1}{2a_{\min }}\end{array}$$

(6)

Otherwise,

$$\begin{array}{l}{T}_{j2a}=-\frac{{\ddot{q}}_k}{j_{\min }}+\frac{\sqrt{\left(j_{\max }-j_{\min }\right)\left({\ddot{q}}_k^2{j}_{\max }-j_{\min }\left(a_1^2+2j_{\max }\left({\dot{q}}_k-v_1\right)\right)\right)}}{j_{\min }\left(j_{\min }-j_{\max }\right)}\\ T_{j2b}=\frac{a_1}{j_{\max }}+\frac{\sqrt{\left(j_{\max }-j_{\min }\right)\left({\ddot{q}}_k^2{j}_{\max }-j_{\min }\left(a_1^2+2j_{\max }\left({\dot{q}}_k-v_1\right)\right)\right)}}{j_{\max }\left(j_{\max }-j_{\min }\right)}\\ T_d=T_{j2a}+T_{j2b}\end{array}$$

(7)

The position displacement h_k is checked to determine whether h_k < q₁ − q_k. If this condition holds, the trajectory computation is carried out according to the renewed parameters in each step, otherwise the deceleration phase must start and the jerk is computed as

$${\stackrel{⃛}{q}}_k=\{\begin{array}{lll}{j}_{\min },\hfill & \mathrm{if} \hfill & (k-\overline{k})\in \left[0,\frac{T_{j2a}}{T_s}\right]\hfill \\ 0,\hfill & \mathrm{if} \hfill & (k-\overline{k})\in \left[\frac{T_{j2a}}{T_s},\frac{T_d-T_{j2b}}{T_s}\right]\hfill \\ j_{\max },\hfill & \mathrm{if} \hfill & (k-\overline{k})\in \left[\frac{T_d-T_{j2b}}{T_s},\frac{T_d}{T_s}\right]\hfill \end{array}$$

(8)

where $\overline{k}$ is the time instant in which Phase 2 starts.

For the path derived by the multi-scale A* algorithm with RSC in Figure 8, the trajectory is planned as Figure 13 and the position, velocity, acceleration, and jerk profiles are given in Figure 14, with constraints $v_{\max }=7$, $v_{\min }=-5$, $a_{\max }=a_{\min }=0.1$, $j_{\max }=j_{\min }=0.1$, and time interval T_s = 0.1. The planned trajectory is smooth and satisfies the constraints.

The proposed scheme not only satisfies the requirement of forward navigation, but is also applicable for the navigation with forward and backward motions. When the final course orients outside of the port, the planned path may have segments with backward motion, as Figure 15 illustrates, where maximum curvature is set as 0.005, initial pose [x₀, y₀, Ψ₀] = [4138, 417, 0], and final pose [x₂, y₂, Ψ₂] = [3500, 3330, 0]. The corresponding motion configuration is given in Figure 16. The planned trajectory meets the constraints and realizes backward motion planning.

5. Discussions

The time-related trajectory is available as per the given steps. To summarize and make the structure clearer, Figure 17 illustrates the information flow of the whole process.

As shown in Figure 17, the proposed method is quite straightforward and easily understood. The strengths and weaknesses (potential enhancement) of this method can be summarized as follows:

(1)

Strengths: high efficiency, constraints applicable (kinodynamic and nonholonomic constraints), easy to understand, suitable for motion planning including backward motions;

(2)

Weaknesses or potential enhancements: dynamic map is not suitable, no adaptive change of quad-tree structure, additional costs on environmental situation such as wind and wave are not considered.

6. Conclusions

In this research, a trajectory-planning scheme is proposed that combines multi-scale A*, RSC, and online double S trajectory planning. Only the initial and final states are needed to derive a feasible trajectory. Berthing at Long Beach Harbor is taken as an example. Both alongside and astern berthing satisfy the constraints. The length of the path is reasonable, and the safety of the path is guaranteed by the minimum grid size, which is decided by the quad-tree depth. Compared to the grid-based A* algorithm, the proposed method is fast and is able to produce paths that stay well clear of obstacles. The trajectory can be used in the control process directly to facilitate various control algorithms, by providing time-related references such as position, course, speed, and acceleration. This application is quite common in the field of vehicle motion control and manipulator systems. One can also use it to define the trajectories of actuators to avoid vibration.