Fillet-based $\text {RRT}^{*}$: A Rapid Convergence Implementation of RRT* for Curvature Constrained Vehicles

Abstract

Rapidly exploring random trees (RRTs) have proven effective in quickly finding feasible solutions to complex motion planning problems. RRT* is an extension of the RRT algorithm that provides probabilistic asymptotic optimality guarantees when using straight-line motion primitives. This work provides extensions to RRT and RRT* that employ fillets as motion primitives, allowing path curvature constraints to be considered when planning. Two fillets are developed, an arc-based fillet that uses circular arcs to generate paths that respect maximum curvature constraints and a spline-based fillet that uses Bézier curves to additionally respect curvature continuity requirements. Planning with these fillets is shown to far exceed the performance of RRT* using Dubin’s path motion primitives, approaching the performance of planning with straight-line path primitives. Path sampling heuristics are also introduced to accelerate convergence for nonholonomic motion planning. Comparisons to established RRT* approaches are made using the Open Motion Planning Library (OMPL).

Appendix: Reverse Fillet

This section describes a novel reverse fillet formulation that enables the ability to plan paths that go forward and backward. The reverse fillet formulation uses a generic one-directional fillet internally that can be replaced by any fillet primitive desired. This section ends with an example planning problem that necessitates reverse and forward motion to follow the shortest path possible.

When using the reverse fillet motion primitive the direction state, d, is added to the state space. $d=1$ when the vehicle is moving forward and $d=-1$ when the vehicle is moving backward. It can be determined if a fillet should keep the direction of travel the same or change it by comparing the d values of the nodes that the fillet connect. When using FB-RRT* with the reverse fillet formulation, d is randomly sampled from a uniform distribution when the state space is sampled.

Before the ReverseFillet procedure can be given a few pieces of notation must be defined. We use the notion $x_{0,d}$ to denote the d value of node $x_0$ and $x_{0,xy}$ to denote the position vector of node $x_0$. The function $R\left( \cdot \right)$ consumes an angle and produces a two-by-two right handed rotation matrix.

$X_{fillet} \leftarrow ReverseFillet\left( x_0,x_1,x_2,x_3\right)$.

Fig. 22

An illustration of the ReverseFillet procedure

Algorithm 16 gives the procedure for generating a reverse fillet and Fig. 22 illustrates the process. If the direction of travel for the two points being connected, $x_2$ and $x_3$, are the same then a normal fillet can be made to connect them, see lines 2 and 3. If the direction of travel changes, more logic is needed to make use of a unidirectional fillet primitive to make a fillet that changes direction. The process, shown on lines 6 through 17, involves flipping $x_3$ over the plane that intersects $x_2$ and is perpendicular to $\overline{x_1 x_2}$ to produce a new point, $x_{3f}$. An unidirectional fillet can be designed using $x_{3f}$. The fillet to execute is obtained by flipping the portion of the unidirectional fillet from $x_2$ to $x_{3f}$ back so that the path ends at $x_3$. The portion from $x_2$ to $x_{3}$ will then be executed in the opposite direction of that from $x_1$ to $x_2$.

First $x_0$, $x_1$, and $x_3$ are transformed into a coordinate frame with $x_2$ at the origin and $x_1$ on the x-axis, see lines 6 through 11 and Fig. 22b. This frame is defined to make flipping $x_3$ easier. The prescript 2 is used to denote a point in this frame, i.e., $_2x_0$ is the point $x_{0,xy}$ in the new frame. On line 12, $_2x_3$ is flipped across the y-axis, see Fig. 22c. The transformed set of points $_2x_{0}$, $_2x_{1}$, $_2x_2$, and $_{2}x_{3f}$ form a chain of nodes that do not change direction. Line 13 generates a unidirectional fillet called $X_{unidir}$ with these modified points, see Fig. 22d. Line 15 fills $X_{fillet}$ with a fillet that starts moving in the same direction at $x_2$ while the x component of $X_{unidir}$ is negative. When the x component of $X_{unidir}$ hits the y-axis the fillet switches to the direction of travel of $x_3$ and flips the fillet across the y-axis. The result is a fillet that comes to a point on the y-axis and switches direction at that point, see Fig. 22e. Line 17 transforms fillet back to the original coordinate frame, as shown in Fig. 22f.

One scenario where the ability to plan forward and reverse fillets is beneficial is shown in Fig. 23. Figure 23 uses the same planning configuration as Fig. 18 from Section 6.4. The only difference between Figs. 23 and 18 is that Fig. 23 shows the result of path planning using the ReverseFillet procedure in green and red instead of the solution found with straight-line primitives. The solution found from planning without the ReverseFillet procedure is shown in blue. Both planners are using arc-fillets with the same maximum curvature constraint, but the red path makes use of the ReverseFillet procedure.

Fig. 23

The resulting paths from running FB-RRT* in an environment where the shortest path turns sharply down a corridor. Solutions from planning with arc-fillet paths are shown in blue. Solutions from planning with reverse-arc-fillet paths are shown in green when $d=1$, forward, and red when $d=-1$, reverse

As is described in Section 6.4, a path that goes through the narrow hallway cannot satisfy the curvature constraints of the problem without hitting walls when solely forward motion is considered. Figure 23 shows that it is possible using the ReverseFillet procedure. Following the green and red solution, generated with forward and reverse motion, the path turns partially into the narrow hallway. When it nears the wall, the path stops and continues the turn in reverse. The path follows the hallway in reverse until it gets to $X_t$. Without the functionality added with the ReverseFillet procedure, the blue solution is unable to follow the hallway and instead must plan a significantly longer path that goes around the obstacles. Note that the inclusion of the reverse motion causes an increase in convergence time due to the added dimension in the sampling space. Future work could include methods to reduce this complexity and also to penalize long stretches of reverse motion.