[1]James Swedeen

[1]Department of Electrical and Computer Engineering, Utah State University,
Old Main Hill, Logan, 84322, UT, USA.

# Fillet-based RRT*: A Rapid Convergence Implementation of RRT* for Curvature
Constrained Vehicles

<EMAIL_ADDRESS>Greg Droge<EMAIL_ADDRESS>Randall Christensen
<EMAIL_ADDRESS>*

###### 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).

###### keywords:

Motion planning, sample-based algorithms, rapidly-exploring random trees, RRT*

Paper Categories: (1), (2), (3)

##  1 Introduction

The ability to plan paths through complex obstacles is a fundamental
requirement of many mobile robot applications and has been shown to be an NP-
complete problem in general [LaValle, 2006]. A variety of methods exist to
decompose this NP-complete problem into manageable subproblems. One class of
methods that has seen explosive growth in recent years is sample-based motion
planning techniques [Gammell et al., 2014, Qureshi and Ayaz, 2015, Karaman and
Frazzoli, 2011, Moon and Ching, 2015, Nasir et al., 2013, Noreen et al., 2016,
Yang et al., 2014a]. Of particular note is the Rapidly-exploring Random Tree
(RRT) and its optimal variant, RRT* [Karaman and Frazzoli, 2011]. RRT quickly
plans obstacle free paths for systems with arbitrary motion primitives. RRT*
provides probabilistic guarantees for asymptotically converging to the optimal
path, although it is not well-suited for nonholonomic motion constraints. This
work contributes to the RRT* literature by developing techniques that can
naturally consider the curvature constraints of a mobile robot with
convergence times similar to that of straight-line motion primitives.

In its most simple form, RRT iteratively builds a search tree to randomly
explore the environment. A sample point is randomly selected at each iteration
and primitive motions are used to extend the tree in the direction of the
sampled point. The use of general primitive motions enables the application of
RRT to a wide range of problems with guarantees of probabilistic completeness
[LaValle, 2006]. However, the path that RRT finds is typically far from
optimal. Karaman and Frazzoli [2011] developed RRT*, which makes two
modifications to the RRT algorithm that probabilistically result in asymptotic
optimality. Both modifications perform local optimizations to the tree using a
neighborhood of nodes around each new point being added to the tree. These
local optimizations work to add and remove edges between existing nodes,
requiring the motion primitives to be able to find a continuous path to
connect the corresponding states represented by the nodes. This exact
connection requirement is not required by the original RRT algorithm, so the
RRT* changes are not generally applicable to all applications of RRT.
Moreover, the same random sampling that ensures that RRT finds a solution
causes the asymptotic convergence to the optimal solution by RRT* to be quite
slow [Akgun and Stilman, 2011, Gammell et al., 2014, Kobilarov, 2012, Nasir et
al., 2013, Noreen et al., 2016].

Exactly connecting two states can become difficult when considering the motion
constraints of wheeled vehicles, such as path curvature. One common method for
considering curvature constraints is to plan with straight-lines and arcs
using techniques such as Dubin’s and Reed-Shepp paths [Beard and McLain, 2012,
LaValle, 2006]. While these techniques provide the shortest paths between
oriented waypoints, they are not well-suited for the local optimization
procedures in RRT* due to the inclusion of orientation [Cui et al., 2018]. The
path exactly connecting two oriented points can vary significantly with small
changes in orientation. An alternative motion primitive in the form of a
fillet was used in [Yang et al., 2014a, b]. Instead of connecting two points,
a fillet connects two straight-line segments with a curve that starts on the
first segment and ends on the second segment. Small changes in each line will
produce small changes in the path length, making the fillet approach amenable
to the local optimizations required by RRT*. In the case of [Yang et al.,
2014a, b], Bézier curves were used to connect the line segments, with the
added benefit that continuous change in curvature is guaranteed.

While fillets enable the use of local optimization techniques, convergence to
the optimal solution is naturally rather slow in RRT*. To overcome slow
convergence rates, many alternative sampling and path refinement procedures
have been introduced [Akgun and Stilman, 2011, Gammell et al., 2014,
Kobilarov, 2012, Nasir et al., 2013, Tahir et al., 2018]. This work utilizes
two such approaches. In [Gammell et al., 2014], Informed RRT* (I-RRT*)
attempts to reduce the sampling space by providing a conservative estimate of
the area that will contain the optimal solution. In [Nasir et al., 2013],
Smart RRT* (S-RRT*) provides an alternative sampling heuristic as well as a
path refinement procedure to avoid waiting for the probabilistic sampling to
straighten the path. We then combine ideas from [Gammell et al., 2014] and
[Nasir et al., 2013] to develop the novel Smart and Informed RRT* (SI-RRT*)
which provides greedy refinement of the solution without as many parameters to
tune as S-RRT*.

This paper develops a Fillet-based RRT* (FB-RRT*) algorithm for curvature
constrained path planning. Similar to [Yang et al., 2014a, b], a fillet
approach is used to locally connect points. Contributions to [Yang et al.,
2014a, b] include the generalization of the fillet structure for RRT planning,
a relaxation of connection assumptions that increases flexibility in growing
the tree, and a newly developed rewiring procedure to ensure continuity and
cost improvement in the resulting path. Established sampling and path
refinement procedures are also extended to the fillet structure. A minor
contribution of this work is the combination of two sampling heuristics
[Gammell et al., 2014, Nasir et al., 2013] within the fillet framework.

The remainder of the paper proceeds as follows. In Section 2, the basics of
RRT and RRT* are introduced. Section 3 then introduces the fillet approach to
local planning. Section 4 develops procedures for incorporating the fillet
approach into RRT* with a brief description of reverse fillet considerations
given in Appendix A. Section 5 then develops the smart-and-informed sampling
and path refinement procedures and presents the Fillet-based RRT* algorithm.
Results are presented in Section 6 using the Open Motion Planning Library
(OMPL) [Moll et al., 2015] to benchmark the performance of RRT* using a
straight-line motion primitive, an arc-based fillet, a Bézier curve fillet,
Dubin’s paths, and various sampling techniques. Concluding remarks are given
in Section 7.

##  2 The Rapidly-exploring Random Tree

RRT-based algorithms are commonly broken into a series of generalized space
sampling and tree growing procedures. RRT’s variants (including RRT*) refine
and augment these procedures. This section defines several basic procedures,
giving them context within RRT and RRT*. The procedures in this section are
found in [Karaman and Frazzoli, 2011, Yang and Sukkarieh, 2010] with
variations in notation. They are included for the sake of completeness in
presenting the Fillet-Based RRT* (FB-RRT*) formulation and sampling heuristics
in Sections 3 through 5.

###  2.1 Notation

RRT-based algorithms iteratively construct a rooted, out-branching tree to
find a path through the state space. The tree is an acyclic directed graph
denoted as $T=\\{V,E\\}$, where $V$ is the set of nodes or vertices within the
tree and $E\subset V\times V$ denotes the set of edges between vertices. If an
edge points from $v_{1}$ to $v_{2}$, $v_{2}$ is referred to as the child of
$v_{1}$ and $v_{1}$ as the parent of $v_{2}$. The root has no parent while all
other vertices have exactly one parent. Each vertex can have multiple
children. Each vertex within $V$ corresponds to a state in the $d$-dimensional
state space denoted as $X\subset\mathbb{R}^{d}$. Note that we will assume that
$X\subset\mathbb{R}^{2}$, although that is certainly not the case for general
RRT formulations. The tree is initialized with solely the root node, i.e.
$V=\\{x_{r}\\}$, $E=\varnothing$. Nodes are added to the tree to find a path
to the target set, $X_{t}\subset X$. The path must avoid the space blocked by
obstacles, $X_{obs}\subset X$, staying within the free space,
$X_{free}=X\setminus X_{obs}$.

Paths through the state space are written as an ordered subset of $X$. In
RRT*, the paths are assigned a cost, typically the path length. Allowing
$P(X)$ to denote the power set of $X$, the cost is a mapping
$c:P(X)\rightarrow\mathbb{R}_{+}$. The closed ball of radius
$r\in\mathbb{R}_{+}$ centered at $x\in\mathbb{R}^{d}$ is denoted as
$\mathcal{B}_{x,r}=\left\\{y\in X:\left\|y-x\right\|\leq r\right\\}$.
$X_{s}\subset X_{free}$ is the set of states from which the additional nodes
will be sampled. Additional notation is summarized in Table 1.

Table 1: Notation used throughout the paper Name |  Description |  Name |  Description
---|---|---|---
General sets
$\mathbb{Z}$ |  The set of all real-valued integers |  $\mathbb{R}_{+}$ |  The set of all positive real numbers
$\mathbb{R}$ |  The set of all real-valued numbers |  $\mathbb{R}^{d}$ |  The set of all real $d$-dimensional vectors
Configuration space
$X$ |  The full configuration space, $X\subset\mathbb{R}^{2}$ |  $X_{obs}$ |  Obstacle filled configuration space
$X_{free}$ |  Obstacle free configuration space |  $X_{t}$ |  The target set
$X_{s}$ |  The sampling set |  $X_{b}$ |  The beacon set
$X_{path}$ |  An ordered set of states |  $X_{near}$ |  A set of nodes near a given node
$X_{i}$ |  The set that, if sampled, will improve the path |  $X_{sol}$ |  The solution path
$x$ |  An element of $X$, a 2D position |  $\psi$ |  Used to denote orientation
Special sets
$\mathcal{E}_{x_{a},x_{b}}$ |  A subset of $X$ in an ellipse with focal points $x_{a}$ and $x_{b}$ |  $\mathcal{C}^{y}$ |  The set of all $y\in\mathbb{Z}_{+}$ times continuously differentiable paths
$\mathcal{B}_{x,r}$ |  The closed ball of radius $r$ and centered at $x$ |  |
Operators
$R(\theta)$ |  The right-handed rotation matrix parameterized by angle $\theta$ |  $a\mathrel{\vbox{\hbox{.}\hbox{.}\hbox{.}}}b$ |  Tests if $a$ is divisible by $b$ (i.e., true if $a\mbox{ }mod\mbox{ }b==0$)
$c\\!\left(X_{path}\right)$ |  Cost of a path through the state space |  $\left\|x\right\|$ |  The 2-norm of $x$
$\land$, $\lor$ |  Logical “and” and “or” operators |  $\binom{n}{k}$ |  N choose k, i.e. $\binom{n}{k}=\frac{n!}{k!(n-k)!}$
Tree notation
$V$ |  A set of vertices |  $T$ |  An acyclic directed graph, $T=\left\\{V,E\right\\}$
$E$ |  A set of edges that connect vertices |  $x_{r}$ |  The root node
Planning Parameters
$\alpha$ |  The max number of neighbors to consider |  $\eta$ |  The max distance to travel when steering a point
$\rho$ |  The radius for searching for nearest neighbors |  $b_{t}$ |  Determines how frequently $X_{t}$ will be sampled
$\gamma$ |  The angular displacement between two vectors |  $b_{b}$ |  Determines how frequently $X_{b}$ will be sampled
Tree Search Variables
$x_{p}$ |  A parent node |  $x_{c}$ |  A child node
$x_{gp}$ |  A parent of a parent (i.e., grandparent node) |  $x_{gc}$ |  A child node of a child (i.e., grandchild node)
$x_{n}$ |  A new node to be added to the tree |  $x_{rand}$ |  A randomly sampled state
$c_{y}$ |  The cost of the node $x_{y}$ |  $x_{nearest}$ |  The point that is closest to $x_{n}$ in $V$
$x_{best}$ |  The last node in the best $X_{sol}$ set found so far |  $x_{near}$ |  A point that is close to $x_{n}$, $x_{near}\in X_{near}$
Fillets
$\kappa_{max}$ |  The maximum path curvature allowed |  $s$ |  A spatial indexing
$x_{1}$, $x_{2}$, $x_{3}$ |  The point where a fillet begins, the center point, and the ending point |  $s_{0}$, $s_{1}$, $s_{2}$, $s_{3}$ |  The index of the fillet at $x_{1}$, $x_{s}$, $x_{e}$, and $x_{3}$ respectively
$x_{s}$, $x_{e}$ |  Points where fillet curve starts and ends |  $\operatorname{\mathcal{F}}_{i}$ |  Abbreviation for the full fillet length
$\Psi(s)$ |  The spatially indexed curve of the fillet |  $\Psi_{i}$ |  The path length of the curve centered at $x_{i}$
$b_{i}$ |  Distance from $x_{i-1}$ to the beginning of the fillet curve centered at $x_{i}$ |  $e_{i}$ |  Distance from $x_{i+1}$ to the end of the fillet curve centered at $x_{i}$
$d(\gamma)$ |  The distance from $x_{s}$ or $x_{e}$ to $x_{2}$ given the angular displacement between fillet lines |  $m_{i,j}$ |  Distance on the line $\overline{x_{i}x_{j}}$ that isn’t replaced by a fillet curve, i.e. $\left\|x_{i}-x_{j}\right\|-d(\gamma_{i})-d(\gamma_{j})$
Arcs
$r$ |  The radius of the circle made from executing $\kappa_{max}$ |  $\zeta$ |  Distinguishes clockwise and counterclockwise arcs
$\theta$ |  An angular variable ranging from $[0,\gamma)$ |  |
Bézier fillets
$B_{n,i}(\tau)$ |  A Berstein polynomial of degree $n$ and iteration $i$ |  $\tau$ |  A path parameterization index ranging from $0$ to $1$
$P_{n}(\tau)$ |  A Bézier curve of degree $n$ |  $p_{i}$ |  Control points for the Bézier curve
$\nu_{1-4}$ |  Constant scalars |  $h$,$g$,$k$ |  Weights that are dependent on $d(\gamma)$
Vectors and lines
$u_{ab}$ |  The unit vector formed from $(x_{b}-x_{a})/\left\|x_{b}-x_{a}\right\|$ |  $\overline{x_{a}x_{b}}$ |  The line that intersects points $x_{a}$ and $x_{b}$
$\overrightarrow{x_{a}x_{b}}$ |  The vector that goes from $x_{a}$ to $x_{b}$ |  $x_{a,i}$ |  Vector $a$’s $i$th element

###  2.2 Common Sampling-based Planning Procedures

The literature on sample-based planning defines planning algorithms using a
number of procedures. We now define several generic procedures that can be
found in [Karaman and Frazzoli, 2011] with notation changed to match the
sequel.

######  procedure 1 ($T\leftarrow Initialize(x_{r})$).

Returns an initialized tree with $x_{r}\in X$ as the root node and no edges,
i.e. $T\leftarrow\\{V,E\\}$, $V\leftarrow\\{x_{r}\\}$, and
$E\leftarrow\varnothing$.

######  procedure 2 ($x_{rand}\leftarrow Sample(X_{s})$).

Returns a random state from the set $X_{s}\subset X$.

######  procedure 3 ($x_{nearest}\leftarrow Nearest(x_{rand},T)$).

Finds the nearest vertex in the set $V$ to the state $x_{rand}\in X$ using the
2-norm as a distance.

######  procedure 4 ($X_{near}\leftarrow Near_{\rho,\alpha}(x_{rand},T)$).

Finds the nearest $\alpha\in\mathbb{Z}_{+}$ vertices that are within a given
radius111In this work $\rho$ is held constant, but many RRT* based algorithms
vary $\rho$ [Karaman and Frazzoli, 2011]., $\rho\in\mathbb{R}_{+}$, of the
point $x_{rand}\in X$.

The constant $\alpha$ is used to prevent too many connections from being
attempted in a given iteration [LaValle, 2006].

######  procedure 5 ($x_{n}\leftarrow Steer_{\eta}(x,y)$).

Returns a point that is within a predefined distance $\eta\in\mathbb{R}_{+}$
from $x\in X$ in the direction of $y\in X$, i.e.

$Steer_{\eta}(x,y)=argmin_{\\{z\in X:\|z-x\|\leq\eta\\}}\|z-y\|$

The Steer function prevents long edges from being added to RRT search trees.
This is important because it reduces the expected extension length at each
iteration and likewise reduces the likelihood that a given iteration will fail
to expand the search tree due to its edge being blocked by an obstacle [Lan
and Di Cairano, 2015].

######  procedure 6 ($T\leftarrow InsertNode(x_{n},x_{p},T)$).

Adds the node $x_{n}\in X_{free}$ to the tree with $x_{p}\in V$ as the new
node’s parent, i.e. $V\leftarrow V\cup\\{x_{n}\\}\mathchar
24635\relax\;E\leftarrow E\cup\\{(x_{p},x_{n})\\}$.

######  procedure 7 ($X_{sol}\leftarrow Solution(x_{v},T)$).

Finds the path through $T$, $X_{sol}\subset V$, that leads from the root node
to $x_{v}$.

######  procedure 8 ($X_{path}\leftarrow Path(x_{start},x_{end})$).

Builds an ordered set of states that connect the state $x_{start}\in X$ to
$x_{end}\in X$ without considering obstacles.

Note that $Solution$ is used to search the tree while $Path$ is used to search
$X$ in an attempt to grow the tree.

######  procedure 9 ($bool\leftarrow CollisionFree(X_{path})$).

Returns true if and only if $X_{path}$ is obstacle free, i.e. $X_{path}\subset
X_{free}$.

######  procedure 10 ($x_{p}\leftarrow Parent(x_{c},T)$).

Returns the parent node of $x_{c}\in V$ in the tree $T$, or $\varnothing$ if
$x_{c}$ is the root node.

######  procedure 11 ($X_{children}\leftarrow Children(x_{p},T)$).

Returns every node from the set $V$ in $T$ that has $x_{p}$ as its parent.

######  procedure 12 ($c_{v}\leftarrow Cost(x_{v},T)$).

Returns the cost of $x_{v}\in V$. The cost of a vertex is defined as the path
length traveled from $x_{r}$ to $x_{v}$ along the tree, i.e.

$Cost(x_{v},T)=c(Solution(x_{v},T))$

######  procedure 13 ($c_{n}\leftarrow CostToCome(x_{n},x_{p},T)$).

Calculates the cost of $x_{n}\in X$ if it were connected to the tree through
$x_{p}\in V$, returning an infinite cost if the path is not obstacle free. It
is defined in Algorithm 1.

Algorithm 1 $c_{n}\leftarrow CostToCome(x_{n},x_{p},T)$

1:$X_{path}\leftarrow Path(x_{p},x_{n})$

2:if $CollisionFree(X_{path})$ then

3: return $Cost(x_{p},T)+c(X_{path})$ $\ignorespaces\triangleright$ Path
length calculation

4:else

5: return $\infty$

6:end if

###  2.3 RRT

RRT quickly searches $X_{free}$ to find a feasible (not optimal), obstacle
free solution and can be used with complex motion primitives while maintaining
probabilistic completeness [Masehian and Kakahaji, 2014]. The RRT algorithm is
composed of two main steps that are repeatedly performed until a solution is
found. The first is taking a biased sample from $X$. The second is growing the
search tree toward the random sample using the $Extend$ procedure. These two
procedures are now stated.

######  procedure 14 ($x_{rand}\leftarrow
Biased\text{-}Sample(i,b_{t},X_{t})$).

Returns a random point at iteration $i\in\mathbb{Z}_{+}$ given the sampling
bias, $b_{t}\in\mathbb{Z}_{+}$, and the target set, $X_{t}$, as described in
Algorithm 2.

Algorithm 2 $x_{rand}\leftarrow Biased\text{-}Sample(i,b_{t},X_{t})$

1:if $i\mathrel{\vbox{\hbox{.}\hbox{.}\hbox{.}}}b_{t}$ then
$\ignorespaces\triangleright$ Bias towards target set

2: $x_{rand}\leftarrow Sample(X_{t})$

3:else $\ignorespaces\triangleright$ Otherwise sample a random point

4: $x_{rand}\leftarrow Sample(X)$

5:end if

6:return $x_{rand}$

The sample is biased towards the target set by selecting the sample from
$X_{t}$ every $b_{t}$ iterations. $b_{t}$ is a design parameter affecting
exploration and exploitation. A small $b_{t}$ will attempt to connect the tree
to the target set more frequently.

######  procedure 15 ($\\{x_{n},x_{p},c_{n}\\}\leftarrow Extend(x_{rand},T)$).

Given a sample, $x_{rand}\in X$, and tree, $T=\\{V,E\\}$, the $Extend$
procedure finds the closest vertex to $x_{rand}$ that is already in $V$ and
checks if a valid extension can be made from the tree towards $x_{rand}$, see
Algorithm 3.

The $Extend$ procedure is illustrated in Figure 1. Note that RRT does not make
use of the extension cost $c_{n}$; it is included for use in RRT*.

Algorithm 3 $\\{x_{n},x_{p},c_{n}\\}\leftarrow Extend(x_{rand},T)$

1:$x_{nearest}\leftarrow Nearest(x_{rand},T)$

2:$x_{n}\leftarrow Steer_{\eta}(x_{nearest},x_{rand})$
$\ignorespaces\triangleright$ Steer towards the nearest point

3:$c_{n}\leftarrow CostToCome(x_{n},x_{nearest},T)$
$\ignorespaces\triangleright$ Evaluate cost of resulting path

4:if $\infty\neq c_{n}$ then

5: return $\\{x_{n},x_{nearest},c_{n}\\}$

6:else

7: return $\\{\varnothing,\varnothing,\infty\\}$

8:end if

(a) The tree before $x_{rand}$ is sampled and Algorithm 3 starts.

(b) $x_{rand}$ is sampled and node D is found to be the closest node to
$x_{rand}$.

(c) $x_{rand}$ is steered toward node D resulting in the new node, node E.

(d) After checking for obstacles, node E is added to the tree.

Figure 1: An illustration of the $Extend$ procedure. Algorithm 4
$X_{sol}\leftarrow RRT(x_{r},X_{t},b_{t})$

1:$T\leftarrow Initialize(x_{r})$

2:for $i=1,\cdots,\inf$ do

3: $x_{rand}\leftarrow Biased\text{-}Sample(i,b_{t},X_{t})$
$\ignorespaces\triangleright$ Biased configuration sampling

4: $\\{x_{n},x_{p},c_{n}\\}\leftarrow Extend(x_{rand},T)$
$\ignorespaces\triangleright$ Extend tree towards new point

5: if $x_{n}\neq\varnothing$ then

6: $T\leftarrow InsertNode(x_{n},x_{p},T)$ $\ignorespaces\triangleright$ Add
point to tree

7: if $x_{n}\in X_{t}$ then

8: return $Solution(x_{n},T)$ $\ignorespaces\triangleright$ Return solution if
found

9: end if

10: end if

11:end for

The RRT algorithm can now be described. First, a random point is sampled from
the configuration space. If the tree can be extended, the new point is added
to the tree. If the new point is in the target set then RRT returns a
solution, as shown in Algorithm 4. RRT is known to quickly find solutions for
complex problems as it naturally explores unexplored areas of the state space,
a property called the Voronoi property [Kuffner and LaValle, 2000]. When using
the vertices of the tree to create a Voronoi diagram, unexplored regions
correspond to larger Voronoi cells. The probability that a Voronoi cell is
sampled is proportional to the size of that cell. Thus, the RRT tree naturally
extends towards regions that have not yet been explored, avoiding problems
with local minima and nonconvex obstacles.

###  2.4 RRT*

RRT* is an extension of RRT that adds probabilistic guarantees for asymptotic
optimality to the probabilistic completeness guarantees of RRT [Karaman and
Frazzoli, 2011]. RRT* does so by performing local optimizations on the edges
in the tree whenever a new node is added. As the number of iterations goes to
infinity, the repeated local optimization transforms the tree into a set of
globally optimal paths from the root node to every reachable point in the
obstacle free configuration space.

RRT* includes two significant changes to RRT, both of which concern the
neighborhood set of the node being added to the tree, i.e.
$X_{near}=Near_{\rho,\alpha}(x_{n},T)$. The first modification is replacing
the $Extend$ procedure with $Extend^{*}$. As illustrated in Figure 2, the
$Extend^{*}$ procedure selects a parent from $V$ within a specified distance
of the new point that minimizes the cost of the new node.

The second modification happens after $x_{n}$ is added to the tree in a new
procedure called $Rewire$. Each node in a local neighborhood is tested to see
if its cost would be improved by going through the new node instead of its
current parent node. If so, the edges are changed so that $x_{n}$ becomes the
node’s new parent as illustrated in Figure 3.

######  procedure 16 ($\\{x_{n},x_{p}\\}\leftarrow Extend^{*}(x_{rand},T)$).

Given a tree, $T=\\{V,E\\}$, the $Extend^{*}$ procedure finds the best “local”
connection for extending the tree in the direction of $x_{rand}\in X$. It
returns a new point to be added to the tree, $x_{n}$, and the parent,
$x_{p}\in V$, as defined in Algorithm 5.

Algorithm 5 $\\{x_{n},x_{p}\\}\leftarrow Extend^{*}\\!(x_{rand},T)$

1:$\\{x_{n},x_{p},c_{min}\\}\leftarrow Extend(x_{rand},T)$

2:if $x_{n}\neq\varnothing$ then $\ignorespaces\triangleright$ If an extension
is possible

3: $X_{near}\leftarrow Near_{\rho,\alpha}(x_{n},T)$

4: for all $x_{near}\in X_{near}$ do $\ignorespaces\triangleright$ Check for a
lower cost connection

5: $c_{tmp}\leftarrow CostToCome(x_{n},x_{near},T)$

6: if $c_{tmp}<c_{min}$ then

7: $x_{p}\leftarrow x_{near}$

8: $c_{min}\leftarrow c_{tmp}$

9: end if

10: end for

11: return $\\{x_{n},x_{p}\\}$

12:end if

13:return $\\{\varnothing,\varnothing\\}$;

(a) Tree after $Extend$ finishes.

(b) $x_{n}$’s neighborhood set is found to be node C.

(c) Connecting through nodes C and D result in total costs of $6.2$ and $9$
respectively.

(d) Because connecting through node C yields a lower total cost, node E is
connected to node C.

Figure 2: An illustration of the $Extend^{*}$ procedure.

######  procedure 17 ($T\leftarrow Rewire(x_{n},X_{near},T)$).

Given a tree, $T=\\{V,E\\}$, with node $x_{n}\in V$ and set $X_{near}\subset
V$, $Rewire$ returns a tree with a modified edge set such that $x_{n}$ is made
the parent of elements in $X_{near}$ if it results in a lower cost for the
elements of $X_{near}$. It is defined in Algorithm 6.

Algorithm 6 $T\leftarrow Rewire(x_{n},X_{near},T)$

1:for all $x_{near}\in X_{near}$ do

2: $c_{near}\leftarrow CostToCome(x_{near},x_{n},T)$

3: $\ignorespaces\triangleright$ Check $x_{near}$’s feasibility and that its
cost is reduced

4: if $c_{near}<Cost(x_{near},T)$ then

5: $x_{p}\leftarrow Parent(x_{near},T)$ $\ignorespaces\triangleright$ Rewire
around $x_{near}$

6:
$E\leftarrow\left(E\setminus\\{x_{p},x_{near}\\}\right)\cup\\{x_{n},x_{near}\\}$

7: end if

8:end for

9:return $T$

(a) The tree after $Extend^{*}$ adds node G to the tree.

(b) Nodes B, E, and F are found to be G’s neighborhood set.

(c) The original costs of nodes B, E, and F are $3$, $5.9$, and $5.9$ and
their potential costs are $5.5$, $4.4$, and $4.5$ respectively.

(d) Node B is not rewired because its cost would not be lowered by the
operation, however, nodes E and F are rewired.

Figure 3: An illustration of the $Rewire$ procedure. Algorithm 7
$X_{sol}\leftarrow RRT^{*}\\!(x_{r},X_{t},b_{t},n)$

1:$T\leftarrow Initialize(x_{r})$

2:$x_{best}\leftarrow\varnothing$

3:for $i=1,\ldots,n$ do

4: $x_{rand}\leftarrow Biased\text{-}Sample(i,b_{t},X_{t})$
$\ignorespaces\triangleright$ Biased configuration sampling

5: $\\{x_{n},x_{p}\\}\leftarrow Extend^{*}\\!(x_{rand},T)$
$\ignorespaces\triangleright$ Extend tree towards new point

6: if $x_{n}\neq\varnothing$ then

7: $T\leftarrow InsertNode(x_{n},x_{p},T)$ $\ignorespaces\triangleright$ Add
point to tree

8: $T\leftarrow Rewire(x_{n},Near_{\rho,\alpha}(x_{n},T),T)$
$\ignorespaces\triangleright$ Rewire edges around new point

9: if $x_{n}\in X_{t}\land\left(x_{best}=\varnothing\lor
Cost(x_{n},T)<Cost(x_{best},T)\right)$ then

10: $x_{best}\leftarrow x_{n}$ $\ignorespaces\triangleright$ Update best path
found

11: end if

12: end if

13:end for

14:if $x_{best}\neq\varnothing$ then $\ignorespaces\triangleright$ Return best
solution found

15: return $Solution(x_{best},T)$

16:else

17: return $\\{\varnothing\\}$

18:end if

The RRT* algorithm is shown in Algorithm 7. Note that RRT and RRT* are very
similar with the main difference being the addition of the $Extend^{*}$ and
$Rewire$ procedures. Additionally, RRT* runs for a specific number of
iterations, $n\in\mathbb{N}_{+}$, instead of stopping when the first solution
is found.

RRT* is both probabilistically complete and asymptotically optimal [Karaman
and Frazzoli, 2011]. However, RRT* tends to converge slowly because of the
Voronoi property. The Voronoi property helps RRT-based algorithms find valid
solutions by encouraging exploration. As the solution improves in RRT*, the
Voronoi regions around the solution get smaller, resulting in a diminishing
probability that a given sample will improve the solution [Akgun and Stilman,
2011].

##  3 The Fillet Approach for Local Planning

In many cases, planned paths must obey nonholonomic constraints, e.g. [Cui et
al., 2018, Lan and Di Cairano, 2015, LaValle, 2006]. The $Extend$ and $Path$
procedures can be modified to use basic atomic motions that satisfy such
constraints during RRT-based planning, enabling RRT to be used with virtually
any set of dynamics. RRT* variants, however, have no additional benefit if the
underlying dynamics or primitive motions do not allow the connection of any
two states using a single edge in open space [Li et al., 2016]. The reason
being that the $Rewire$ procedure cannot be performed if the nodes in the
neighborhood set cannot be exactly connected to each other.

This constraint is detrimental when planning with motion primitives that
enforce dynamic path constraints, such as maximum curvature. A common
technique for considering maximum curvature constraints is to use Dubin’s
paths. Dubin’s paths connect orientated points with the shortest path while
considering maximum curvature constraints [LaValle, 2006]. The issue with
using Dubin’s paths in a sample-based path planner is that a poor choice in
the orientation of the points along the solution can cause a significant
increase in path length, as shown in Figure 4. Furthermore, the orientation
that minimizes overall path length changes as the solution converges to
optimality.

Instead of attempting to connect two oriented points, fillets connect two line
segments (defined with three unoriented points) with a curve transitioning
smoothly between them, as shown in Figure 5. Without the addition of
orientation, fillets naturally allow incremental improvements to the solution.
The result is a path that is continuous in position and orientation.
Additional path qualities may be achieved depending on the choice of fillet.
This section will introduce the general fillet concept and requirements for
creating a path using fillets. Two fillets are then defined, one using an arc
and one using Bézier curves. Section 4 utilizes these fillets as motion
primitives in RRT-based algorithms.

Figure 4: Given the set of nodes that start with $x_{s}$ and end with $x_{e}$
their respective orientations are denoted with arrows pointing from them. The
blue path is the path that is made by the arc-fillet path generation and the
red path is the path that is made by Dubin’s paths.

###  3.1 General Fillets

Given three input points $x_{1},x_{2},x_{3}\in X$, a fillet connects $x_{1}$
to $x_{3}$ with the combination of two straight-line segments and a curve. The
line segments constitute portions of the lines $\overline{x_{1}x_{2}}$ and
$\overline{x_{2}x_{3}}$. The curve intersects line $\overline{x_{1}x_{2}}$ at
point $x_{s}$ and line $\overline{x_{2}x_{3}}$ at $x_{e}$. The resulting
fillet moves in a straight line from $x_{1}$ to $x_{s}$, along the curve from
$x_{s}$ to $x_{e}$, and then along the straight line from $x_{e}$ to $x_{3}$,
as depicted in Figure 5. The major differentiator between the different
fillets is the definition of the curve portion, which affects the placement of
$x_{s}$ and $x_{e}$.

This work assumes symmetric fillets, resulting in an equivalent distance
between the node that the fillet is centered at and the two ends of the fillet
curve, $x_{s}$ and $x_{e}$. This distance is a function of the fillet curve
type as well as the change in orientation between
$\overrightarrow{x_{1}x_{2}}$ and $\overrightarrow{x_{2}x_{3}}$.

The position along the fillet can be described using a spatial index $s$.
Allow $s_{0}$ to be where the fillet meets $x_{1}$, $s_{1}$ to be the index
where the fillet’s curve begins, $s_{2}$ to be the index of where the fillet’s
curve ends, and $s_{3}$ to be the index of where the fillet reaches $x_{3}$.
Allow $\Psi(s)$ to be the fillet’s curve such that $\Psi(0)=x_{s}$ and
$\Psi(s_{2}-s_{1})=x_{e}$. Also, define the unit vector from $x_{i}$ to
$x_{j}$ as $u_{ij}$ (see Table 1). The position along the fillet can be
written in a piecewise form as

$x(s)=\begin{cases}x_{1}+su_{12}&s_{0}\leq s\leq s_{1}\\\
\Psi(s-s_{1})&s_{1}<s\leq s_{2}\\\ x_{e}+(s-s_{2})u_{23}&s_{2}<s\leq
s_{3}\end{cases}.$ (1)

Defining $\gamma_{i}\in[0,\pi)$ as the angle measured from
$\overrightarrow{x_{i-1}x_{i}}$ to $\overrightarrow{x_{i}x_{i+1}}$, the fillet
distance for $x_{1},x_{2},x_{3}$ can be defined as
$d(\gamma_{2})=\left\|x_{s}-x_{2}\right\|=\left\|x_{e}-x_{2}\right\|$. Once
$d(\gamma_{2})$ is found, the start and end points of the curve can be written
as

$\begin{split}x_{s}&=x_{2}+d(\gamma_{2})u_{21}\\\
x_{e}&=x_{2}+d(\gamma_{2})u_{23}\end{split}.$ (2)

###  3.2 Fillet Paths

A smooth path to a destination node can be created using a sequence of points
where fillets are formed from point triplets and then combined, as shown in
Figure 5. Without loss of generality, it is assumed that the path starts at
node $1$ and moves to node $n$ using the sequence $x_{1},x_{2},...,x_{n}$. The
path is thus made from $n$ nodes, using $n-2$ fillets to arrive at $x_{n}$.
There are two major concerns when formulating the path. The first is the path
continuity; not every sequence of points can be combined using fillets to
create a continuous path. The second major consideration is path length; the
$Extend^{*}$ and $Rewire$ procedures depend upon path length for local
optimizations.

#### Path Continuity

The first key to using the fillets for planning purposes is to ensure that the
path resulting from joining multiple fillets is continuous. There are two
conditions to ensure feasibility: one to ensure that the fillet curve ends
before the final point in the fillet and one condition to ensure that all
fillets end before the next one begins. Assuming $x_{i}$ is the middle node,
these conditions can be expressed as

$\begin{split}d(\gamma_{i})&\leq\|x_{i}-x_{i+1}\|\\\
d(\gamma_{i-1})+d(\gamma_{i})&\leq\|x_{i}-x_{i-1}\|\end{split}.$ (3)

Figure 5: The fillet generated to connect $x_{2}$ and $x_{4}$ is shown in blue
with the fillet that comes before it shown in red. Note that
$d(\gamma_{2})+d(\gamma_{3})\leq\left\|x_{2}-x_{3}\right\|$, providing a
continuous path.

#### Length of Fillet Paths

In RRT*’s $Extend^{*}$ and $Rewire$ procedures, local optimizations are made
to the search tree to find the shortest path. These procedures are defined for
straight-line paths where the path length can be calculated as the distance
between nodes in the path. This is not the case for paths created from
fillets. Thus, the path length to a particular node and the length relation to
other nodes are now evaluated.

Each fillet consists of two line segments and a fillet curve. The length of a
single fillet with $x_{i}$ as the middle node of the fillet can be expressed
as

$\operatorname{\mathcal{F}}_{i}=b_{i}+\Psi_{i}+e_{i}$ (4)

where $b_{i}$, $\Psi_{i}$, and $e_{i}$ are the beginning component length, the
curve length, and the end component length (see Figure 5). Given (3), the two
straight-line lengths can be expressed as

$\begin{split}b_{i}&=\left\|x_{i-1}-x_{i}\right\|-d(\gamma_{i})\\\
e_{i}&=\left\|x_{i+1}-x_{i}\right\|-d(\gamma_{i})\end{split}.$ (5)

This definition of fillet length allows for the expression of a recursive
relationship for calculating the path length in the following lemma.

######  Lemma 1.

Assume an ordered sequence of nodes is used to create a path using fillets
with (3) satisfied for every intermediate node. Given a resulting path length
of $c_{i}$ to arrive at node $x_{i}$, $i\geq 3$, the path length to arrive at
node $x_{i+1}$ can be expressed as

$c_{i+1}=c_{i}+\operatorname{\mathcal{F}}_{i}-\left\|x_{i}-x_{i-1}\right\|.$
(6)

Proof: The path length to node $x_{i}$ along a sequence of curves and lines
can be written as a summation of individual parts. The path to $x_{i}$
contains $i-2$ curves of length $\Psi_{2}$ through $\Psi_{i-1}$. Let
$m_{j,j+1}$ be the length of the straight-line segment that connects curve $j$
to curve $j+1$, i.e.

$m_{j,j+1}=\left\|x_{j}-x_{j+1}\right\|-d(\gamma_{j})-d(\gamma_{j+1}),$ (7)

which is positive assuming (3) is satisfied for all fillets. The length of the
path to arrive at $x_{i}$ is

$c_{i}=b_{2}+\sum_{k=2}^{i-1}\Psi_{k}+\sum_{k=2}^{i-2}m_{k,k+1}+e_{i-1}.$ (8)

Note that the path connecting to node $x_{i+1}$ could be expressed similarly
with a length of

$c_{i+1}=b_{2}+\sum_{k=2}^{i}\Psi_{k}+\sum_{k=2}^{i-1}m_{k,k+1}+e_{i},$

which can be written in terms of $c_{i}$ as

$c_{i+1}=c_{i}+\Psi_{i}+m_{i-1,i}+e_{i}-e_{i-1}.$ (9)

Given (5) and (7), (9) becomes

$c_{i+1}=c_{i}+b_{i}+\Psi_{i}+e_{i}-\left\|x_{i}-x_{i-1}\right\|.$ (10)

Given the definition of $\operatorname{\mathcal{F}}_{i}$ in (4), (10)
simplifies to (6). A few properties can now be stated using the recursive
relationship in Lemma 1, beginning with the relationship between the path
length to a node and the path length to one of its descendants.

######  Corollary 1.

The path length to a node is not dependent upon the choice of any nodes that
come after it.

Proof: This can be seen by examining the summation form of the path length in
(8) and noting that none of the variables depend upon any node after node
$x_{i}$.

The recursive path length calculation using fillets depends on the parent as
well as the grandparent node. This is different from the straight-line motion
primitive where the path length to a node can be calculated using knowledge of
solely the parent node. This leads to the following lemma about path length,
which has significant implications for rewiring a tree connected with fillets.

######  Lemma 2.

Given a node $x_{i}$ with a child node $x_{i+1}$ and multiple possible parent
nodes, choosing the parent for $x_{i}$ to minimize $c_{i}$ will not
necessarily result in the smallest possible value for $c_{i+1}$.

Proof: The proof is given through a simple example where the shorter path to
$x_{i}$ results in a longer path to $x_{i+1}$. Consider Figure 6. Let the path
from $x_{r}$ to $x_{6}$ have the same length as the path from $x_{r}$ to
$x_{1}$ and the same be true of $x_{5}$ and $x_{2}$, i.e.

$\begin{split}Cost\left(x_{1},T\right)=Cost\left(x_{6},T\right)\\\
Cost\left(x_{2},T\right)=Cost\left(x_{5},T\right)\end{split}.$

Path A, shown in red, is the shortest path to $x_{3}$ with a path length of
$1.9$. Path A also results in a path length to $x_{4}$ of $2.9$. On path B,
shown in blue, let the fillet that connects $x_{5}$ to $x_{4}$ be an arc-
fillet with $r=0.5$, $d(\pi/2)=1/2$, and a resulting arc length of $\pi/4$.
Starting at $x_{6}$ and following path B results in a path length to $x_{3}$
of $2$, which is greater than the path length to get to $x_{3}$ on path A. To
minimize the path length to $x_{3}$ path A is chosen. However, the length of
the path from $x_{6}$ to $x_{4}$ is $1+1/2+\pi/4+1/2\approx 2.785$. Thus,
minimizing path length to $x_{3}$ does not minimize path length to $x_{4}$.

Figure 6: Let $c_{base}=Cost(x_{1},T)=Cost(x_{6},T)$ and
$Cost(x_{2},T)=Cost(x_{5},T)$. Path A, shown in red, yields a shorter path
length for $x_{3}$ than path B, shown in blue. However, path B yields a
shorter path length to $x_{4}$ than path A.

Therefore, in a $Rewire$ procedure, it is not sufficient to solely check the
path to the node being rewired, but the path lengths to descendants must also
be evaluated. As the tree grows larger, checking all descendants would be
overly cumbersome. The following corollary establishes that the only
descendants that need to be checked are the children nodes.

######  Corollary 2.

If a node’s parent and grandparent remain unchanged, but the tree is rewired
such that the cost of the node’s parent is lowered, then the cost of the node
will be lowered by the same amount as its parent.

Proof: If a node’s parent and grandparent remain the same then an examination
of (6) shows that the only portion of its path length that will change is the
length to the parent’s node, $c_{i}$, since both
$\operatorname{\mathcal{F}}_{i}$ and $\left\|x_{i}-x_{i-1}\right\|$ remain
unchanged. Thus, the same change in cost seen by a parent will be reflected in
the cost of the node in question if the grandparent node remains unchanged.

###  3.3 The Arc Fillet

The arc-fillet formulates the curve portion of the fillet using a circle of
radius $r\in\mathbb{R}_{+}$ that is tangential to the two line segments at
$x_{s}$ and $x_{e}$, see Figure 7. To respect curvature constraints, the
radius can be chosen as the inverse of the maximum curvature, i.e.
$r=1/\kappa_{max}$. The distance between the curve intersection points and the
intermediary point of the fillet is expressed in the following Lemma:

######  Lemma 3.

The arc-fillet distance, $d(\gamma)$, for a circular curve of radius $r$ is

$d(\gamma)=\frac{r\bigl{(}1-\cos(\gamma)\bigr{)}}{\sin(\gamma)}.$ (11)

Proof: Assume that the coordinate frame $f$ is defined such that $x_{s}$ is at
the origin and the $x$-axis is pointing along $\overrightarrow{x_{1}x_{2}}$.
Using the pre-subscript $f$ to denote a point expressed in frame $f$ then
${{}_{f}x_{s}}=\begin{bmatrix}0&0\end{bmatrix}^{T}$. As $d(\gamma)$ represents
the distance along $\overline{x_{1}x_{2}}$ from $x_{s}$ to $x_{2}$, the value
of $x_{2}$ in frame $f$ is

${{}_{f}x_{2}}=\begin{bmatrix}d(\gamma)\\\ 0\end{bmatrix}.$ (12)

As the orientation of the path must be tangential to the circle at $x_{s}$ for
continuity, the center point of the circle will lie upon the $y$-axis at a
distance $r$ from $x_{s}$, i.e.,

${{}_{f}x_{c}}=\begin{bmatrix}0\\\ r\zeta\end{bmatrix}\mbox{,
}\zeta\in\\{-1,1\\},$

where $\zeta=1$ corresponds to a counter-clockwise arc and $\zeta=-1$ to a
clockwise arc. Parameterizing the arc by its tangent angle, the arc can be
expressed in frame $f$ as

${{}_{f}\Psi_{arc}}(\theta)=r\begin{bmatrix}\sin(\theta)\\\
\zeta\left(1-\cos(\theta)\right)\end{bmatrix}$

where ${{}_{f}x_{s}}={{}_{f}\Psi_{arc}}(0)$. The parameter
$\theta\in[0,\gamma)$ represents the orientation of the path in frame $f$. The
tangent of ${{}_{f}\Psi_{arc}}\left(\gamma\right)$ should be parallel to
$\overline{x_{2}x_{3}}$. The task is to solve for $d(\gamma)$ such that
${{}_{f}x_{e}}={{}_{f}\Psi_{arc}}(\gamma)$.

The unit vector from ${{}_{f}x_{3}}$ to ${{}_{f}x_{2}}$ can be written in
terms of $\gamma$ as
$-\begin{bmatrix}\cos(\gamma)&\sin(\gamma)\end{bmatrix}^{T}$. The point
${{}_{f}x_{2}}$ can be expressed as a combination of this unit vector and
${{}_{f}x_{e}}$ as

$\begin{split}{{}_{f}x_{2}}&={{}_{f}x_{e}}-d(\gamma)\begin{bmatrix}\cos(\gamma)\\\
\sin(\gamma)\end{bmatrix}\\\ &=r\begin{bmatrix}\sin(\gamma)\\\
\zeta\left(1-\cos(\gamma)\right)\end{bmatrix}-d(\gamma)\begin{bmatrix}\cos(\gamma)\\\
\sin(\gamma)\end{bmatrix}\end{split}$ (13)

Equation (11) is found by setting (12) equal to (13) and solving for
$d(\gamma)$.

######  Remark 1.

It is important to note the singularities of $d(\gamma)$ and their
corresponding significance. A change of direction of $\gamma=0$ corresponds to
executing a straight line. A value of $\gamma=\pm\pi$ would correspond to
reversing direction.

One advantage of the arc-fillet is that the fillet curve can be directly
expressed using the spatial index $s$. The orientation of
$\overrightarrow{x_{1}x_{2}}$ can be written as

$\psi=atan2(u_{12,2},u_{12,1}).$

The point along the fillet curve can then be expressed in the inertial frame
as

$\displaystyle\Psi_{arc}(s)$
$\displaystyle=R(\psi)\cdot{{}_{f}\Psi_{arc}}\left(\frac{s}{r}\right)+x_{s}\mbox{,}$
$\displaystyle R(\psi)$
$\displaystyle=\begin{bmatrix}\cos(\psi)&-\sin(\psi)\\\
\sin(\psi)&\cos(\psi)\end{bmatrix}\mbox{,}$ (14)

where $\theta$ has been replaced with $\frac{s}{r}$ as the arc-length of a
circle is $s=\theta r$. Note that $\Psi(0)=x_{s}$ and $\Psi(\gamma r)=x_{e}$,
as expected.

Finally, the spatial switching indices from (1) are

$\begin{split}s_{1}&=s_{0}+\left\|x_{s}-x_{1}\right\|\\\ s_{2}&=s_{1}+\gamma
r\\\ s_{3}&=s_{2}+\left\|x_{3}-x_{e}\right\|\end{split}.$

Figure 7: The arc-fillet generated to connect $x_{1}$ and $x_{3}$.

The development of the arc-fillet enables a definition of a new procedure for
creating a path between three points:

######  procedure 18 ($X_{fillet}\leftarrow
ArcFillet\left(x_{0},x_{1},x_{2},x_{3}\right)$).

The $ArcFillet$ procedure uses (2), (11), and (14) to make an arc that
connects $\overline{x_{1}x_{2}}$ to $\overline{x_{2}x_{3}}$. Feasibility
checks are made with respect to the arc that connects $\overline{x_{0}x_{1}}$
to $\overline{x_{1}x_{2}}$ using (11) to define $d(\gamma)$ in (3). If the
checks fail the null path is returned.

######  Remark 2.

The arc-fillet is made from straight-line segments and arcs assuming forward
motion as is a Dubin’s path. As a result, a path made using arc-fillets will
have the same continuity properties as a Dubin’s path, i.e. $\mathcal{C}^{1}$
in position and orientation with curvature assuming instantaneous changes
between zero and maximum curvature [LaValle, 2006].

###  3.4 The Bézier Fillet

The arc-fillet assumes an instantaneous change in curvature which may be
inappropriate for some scenarios that require higher levels of smoothness. In
this section, Bézier curves are used to generate the fillet, resulting in
paths that are $\mathcal{C}^{2}$ continuous in position and orientation, and
$\mathcal{C}^{1}$ continuous in curvature. A detailed description of the curve
is left to [Yang et al., 2014a] and the references therein. The definition of
the curve is shown as follows for the sake of completeness.

A Bézier curve connects two points and is defined as,

$P_{n}(\tau)=\sum^{n}_{i=0}p_{i}B_{n,i}(\tau),$ (15)

where $p_{i}\in\mathbb{R}^{2}$ are control points, $n\in\mathbb{Z}_{+}$ is the
degree of the polynomial, $\tau$ is a path parameterization index such that
$0\leq\tau\leq 1$, $P_{n}(0)=p_{0}$, and $P_{n}(1)=p_{n}$. Note that there is
no direct relationship between changes in $\tau$ and path length [Gravesen,
1997]. The functions $B_{n,i}(\tau)$ are Berstein polynomials defined as

$B_{n,i}(\tau)=\binom{i}{n}\tau^{i}(1-\tau)^{n-i}.$ (16)

Figure 8: The fillet generated to connect $x_{1}$ and $x_{3}$. Note that sets
${}_{0}p_{i}$ and ${}_{1}p_{i}$ are control points that replace $p_{i}$ in
(15) and in analogy to Figure 7, ${}_{0}p_{0}=x_{s}$ and ${}_{1}p_{0}=x_{e}$.

Yang et al. [2014a] combines two cubic Bézier curves to generate the curve of
the fillet. In Figure 8, the curves are denoted as ${}_{0}P$ and ${}_{1}P$
with the connecting lines denoted as $b_{2}$ and $e_{2}$. The fillet distance
can now be stated.

######  Lemma 4.

The distance between the intermediary point, $x_{2}$, and the curve’s start
and end points can be expressed as

$d(\gamma)=\frac{\nu_{4}\sin\left(\frac{\gamma}{2}\right)}{\kappa_{max}\cos^{2}\left(\frac{\gamma}{2}\right)},$
(17)

where

$\begin{tabular}[]{c c c
c}$\nu_{1}=7.2364$&$\nu_{2}=\frac{2}{5}\left(\sqrt{6}-1\right)$&$\nu_{3}=\frac{\nu_{2}+4}{\nu_{1}+6}$&$\nu_{4}=\frac{\left(\nu_{2}+4\right)^{2}}{54\nu_{3}}$\end{tabular}.$

Proof: See Section 2.1 of [Yang et al., 2014a].

To define the fillet curve, four control points are needed for each of the two
Bézier curves. The control points for ${}_{0}P$ and ${}_{1}P$ are denoted as
${}_{0}p_{i}$ and ${}_{1}p_{i}$, respectively, and can be expressed as

$\begin{tabular}[]{l l}${}_{0}p_{0}=\;x_{2}+d\cdot
u_{12}$&${}_{1}p_{0}=\;x_{2}+d\cdot u_{32}$\\\
${}_{0}p_{1}={{}_{0}p_{0}}-g\cdot u_{12}$&${}_{1}p_{1}={{}_{1}p_{0}}-g\cdot
u_{32}$\\\ ${}_{0}p_{2}={{}_{0}p_{1}}-h\cdot
u_{12}$&${}_{1}p_{2}={{}_{1}p_{1}}-h\cdot u_{32}$\\\
${}_{0}p_{3}={{}_{0}p_{2}}+k\cdot u_{d}$&${}_{1}p_{3}={{}_{1}p_{2}}-k\cdot
u_{d}$\end{tabular},$ (18)

where $u_{d}$ is the unit vector pointing from ${{}_{0}p_{2}}$ to
${{}_{1}p_{2}}$ and the weights, $h$, $g$, and $k$ are defined as

$\begin{tabular}[]{c c
c}$h=\nu_{3}d$&$g=\nu_{2}\nu_{3}d$&$k=\frac{6\nu_{3}\cos\left(\frac{\gamma}{2}\right)}{\nu_{2}+4}d$\end{tabular}.$
(19)

A procedure that differs from what is found in [Yang et al., 2014a] is now
defined to generate curves. In [Yang et al., 2014a], a maximum curve angle,
$\gamma_{max}$, is employed with an associated distance
$d_{min}=d(\gamma_{max})$. To ensure subsequent fillet curves do not overlap,
connecting points are forced to be $2d_{min}$ apart. We found the $2d_{min}$
node separation to be overly restrictive as small path refinements are not
allowed under such a constraint. These small path refinements prove necessary,
especially around curves in the obstacles. This limitation significantly
reduces the ability to rewire, during which small refinements to the tree are
common.

The underlying desired constraint enforced with the $2d_{min}$ spacing is path
continuity. The conditions in (3) give the path generation process more
flexibility. An example is given in Figure 9(a) showing the points that could
be considered under both the conditions in (3) and under a $\gamma_{max}$ and
$2d_{min}$ constraint. The curve generating procedure can now be stated.

######  procedure 19 ($X_{fillet}\leftarrow
BezierFillet(x_{0},x_{1},x_{2},x_{3})$).

$BezierFillet$ uses the cubic Bézier spline to generate a $\mathcal{C}^{2}$
continuous curve from $x_{1}$ to $x_{3}$ using (15) through (17). Feasibility
checks are made with respect to the curve that starts at $x_{0}$ and goes to
$x_{2}$ as is described by the combination of (3) and (17). If the checks fail
the null path is returned.

###  3.5 A Comparison of Arc and Bézier Fillets

Arc and Bézier fillets provide different advantages and disadvantages. A big
advantage of the arc-fillet is its simplicity and speed. As can be seen in
Table 2, an arc-fillet can be generated in about half the time it takes to
make a Bézier-fillet. Another benefit of the arc-fillet is that it is less
constrained than the Bézier-fillet, resulting in a larger reachability set for
connecting points, as shown in Figure 9(b). This fact is critical to RRT
because it directly affects exploration and convergence.

Table 2: Three points, $x_{1},x_{2},x_{3}$, were randomly sampled 1 million times with each $x,y$ component bounded between 0 and 10 at each sample. For Dubin’s paths, the orientation of a point was set to be tangential to the vector pointing to it from its parent. The average length of the resulting paths and time it took to find them is presented with their respective standard deviations. Motion Primitive | Length ($m$) | Computation Time ($\mu s$)
---|---|---
Straight-line | $20.845\pm 7.417$ | $\,\,\,3.371\pm\,\,\,1.542$
Dubin’s path | $23.797\pm 7.729$ | $19.087\pm\,\,\,8.670$
Arc-fillet | $18.838\pm 6.811$ | $\,\,\,7.965\pm\,\,\,2.762$
Bézier-fillet | $18.909\pm 6.242$ | $15.568\pm 12.517$

(a) Comparison of the constraints in [Yang et al., 2014a] to those found in
(3) for Bézier curve fillets. The red shows the area that both sets of
feasibility conditions deem invalid for $x_{3}$. The yellow is area that only
(3) deems valid. The green is area that both sets of conditions deem valid.

(b) Visualizes the reachability regions of the arc-fillet and Bézier-fillet
generation. The red shows the area that both fillets cannot reach. The yellow
is area that arc-fillets can reach but Bézier-fillets cannot. The green is
area that both fillets can reach.

Figure 9: Comparisons of different constraints on the position of $x_{3}$ if a
fillet was made starting from $x_{1}$ through $x_{2}$ and to $x_{3}$. On the
left, the constraints in [Yang et al., 2014a] are compared to those found in
(3) for the Bézier curve fillets. On the right, the difference in the
definition of $d(\gamma)$ between arc-fillets and Bézier-fillets is expressed
in terms of where (3) is satisfied. Both figures assume $x_{1}$ is at the
origin, $x_{2}=\begin{bmatrix}0&3\end{bmatrix}^{T}$, $\kappa_{max}=2m^{-1}$,
$d(\gamma_{1})=0$, $d_{min}=1.5m$, and $\gamma_{max}=0.624\pi$ radians.

The major advantage of the Bézier-fillet is the smoothness of the resulting
path. The arc-fillet, like Dubin’s paths, guarantees only $\mathcal{C}^{1}$
continuity of pose. The Bézir-fillet guaranties $\mathcal{C}^{2}$ continuity
of pose and $\mathcal{C}^{1}$ continuity of curvature, as is shown in Figure
10.

(a) Fillets generated between the points
$\begin{bmatrix}-2&0\end{bmatrix}^{T}$,
$\begin{bmatrix}5&0\end{bmatrix}^{T}$, and
$\begin{bmatrix}-1&4\end{bmatrix}^{T}$.

(b) The curvature of each fillet over the fillet curve.

(c) The first derivative of the curvature of each fillet type.

Figure 10: An example of the arc and Bézier fillets with the corresponding
curvature and curvature rate. The arc-fillet is shown in blue and the Bézier-
fillet is shown in red. Note that the arc has zero curvature change as there
is an instantaneous jump from zero to maximum curvature.

##  4 Fillet-based RRT*

Fillet-based variants of the RRT and RRT* algorithms are proposed in this
section. The overall structure of the fillet-based variants is the same as
their standard counterparts. This section will only cover the procedures that
change, namely: $Initialize$, $CostToCome$, $Extend$, $Extend^{*}\\!$, and
$Rewire$. To construct the fillet-based variants, each of these procedures is
redefined with a procedure of the same name but with the prefix “$FB$”, i.e.
$Extend$ becomes $FB\text{-}Extend$. The first four procedures have small
changes to their standard counterparts and are presented first. The
$FB\text{-}Rewire$ procedure is then discussed in detail. This section ends
with a discussion of FB-RRT*’s advantages. The FB-RRT* algorithm is not
presented until Section 5 where a modified sampling procedure is discussed.

###  4.1 Procedures with Minor Changes

The first procedure to be modified is the $Initialize$ procedure. As defined,
the $Initialize$ procedure has no consideration of the vehicle orientation,
which must be respected to generate an executable path for the vehicle.
$FB\text{-}Initialize$ differs from $Initialize$ in that it initializes the
search tree to have an edge of length $d_{init}\in\mathbb{R}_{+}$ extending
from the root, $x_{r}$, to a point in the direction of the initial orientation
of the robot, $\psi_{r}\in[-\pi,\pi)$. Recall that connecting to a new point
using a fillet requires both a parent and a grandparent node. If $x_{r}$ is
returned from $Nearest$ or $Near$ any attempt to make an edge with $x_{r}$ is
ignored as it has no parent, ensuring that the starting orientation is
respected222In our implementation, $x_{r}$ is left out of the search in
$Nearest$ and $Near$.. The new initialization procedure is defined as follows.

######  procedure 20 ($T\leftarrow
FB\text{-}Initialize_{d_{init}}(x_{r},\psi_{r})$).

Returns a tree with two nodes and an edge of length $d_{init}$ based on the
root node, $x_{r}$, and initial orientation, $\psi_{r}$, as defined in
Algorithm 8.

Algorithm 8 $T\leftarrow FB\text{-}Initialize_{d_{init}}(x_{r},\psi_{r})$

1:$x_{n}\leftarrow
x_{r}+d_{init}\begin{bmatrix}\cos\left(\psi_{r}\right)&\sin\left(\psi_{r}\right)\end{bmatrix}^{T}$

2:$V\leftarrow\\{x_{r},x_{n}\\}$

3:$E\leftarrow\\{(x_{r},x_{n})\\}$

4:$T\leftarrow\\{V,E\\}$

5:return $T$

The $CostToCome$ procedure is redefined to accommodate the new fillet-based
path generation. Recalling Lemma 1, this depends upon both the parent and
grandparent nodes of the node in question. The new procedure is given as
follows.

######  procedure 21 ($c_{n}\leftarrow
FB\text{-}CostToCome(x_{n},x_{p},x_{gp},T)$).

Calculates the cost of $x_{n}$ if it is connected to the tree, T, through the
parent, $x_{p}$, and the grandparent, $x_{gp}$, as in Algorithm 9. Note that
the $Fillet$ procedure in Algorithm 9 can be replaced by $ArcFillet$ or
$BezierFillet$ depending upon the fillet being used.

Algorithm 9 $c_{n}\leftarrow FB\text{-}CostToCome(x_{n},x_{p},x_{gp},T)$

1:$X_{fillet}\leftarrow Fillet({Parent(x_{gp},T)},x_{gp},x_{p},x_{n})$

2:if $X_{fillet}\neq\varnothing\And CollisionFree(X_{fillet})$ then

3: return $Cost(x_{p},T)+c(X_{fillet})-\left\|x_{p}-x_{gp}\right\|$
$\ignorespaces\triangleright$ Path length calculation

4:else

5: return $\infty$

6:end if

The $Extend$ procedure is updated in two substantial ways. The first is the
use of $FB\text{-}CostToCome$. The second is the use of a “node orientation”
when performing the nearest neighbor search. By incorporating a sense of “node
orientation”, infeasible sharp turns can be avoided in the nearest neighbor
searching. There is no actual “orientation” of a node as the nodes are 2D
points that guide the creation of the path. However, we can bias the nearest
neighbor search to penalize turns by noting that a fillet ending at a node
will be oriented with the line extending from the node’s parent to the node.
After the fillet curve has been executed, the robot will be aligned with that
orientation. Given node $x_{i}$ and its parent $x_{i-1}$, the orientation of
$x_{i}$ is defined as $\psi_{i}=atan2(u_{i-1\mbox{ }i,2},u_{i-1\mbox{ }i,1})$.
The nearest neighbor search is performed over
$\begin{bmatrix}x_{i,1}&x_{i,2}&\cos(\psi_{i})&\sin(\psi_{i})\end{bmatrix}^{T}$
instead of $\begin{bmatrix}x_{i,1}&x_{i,2}\end{bmatrix}^{T}$. The inclusion of
a pseudo “node orientation” combined with the relaxed continuity constraints,
depicted in Figure 9(a), enables us to forgo the $k$-nearest-neighbor search
that Yang et al. [2014b] uses to aid in the success of $Extend$. The updated
procedure is now defined.

######  procedure 22 ($\\{x_{n},x_{p},c_{n}\\}\leftarrow
FB\text{-}Extend(x_{rand},T)$).

Given $x_{rand}\in X$ and a tree $T$, the $FB\text{-}Extend$ procedure finds
the closest vertex to $x_{rand}$ in terms of a combined position and
orientation metric and attempts to extend the tree in the direction of
$x_{rand}$, as defined in Algorithm 10.

Algorithm 10 $\\{x_{n},x_{p},c_{n}\\}\leftarrow FB\text{-}Extend(x_{rand},T)$

1:$x_{nearest}\leftarrow Nearest(x_{rand},T)$

2:$x_{n}\leftarrow Steer_{\eta}(x_{nearest},x_{rand})$
$\ignorespaces\triangleright$ Steer towards the nearest point

3:$x_{gp}\leftarrow Parent(x_{nearest},T)$

4:$c_{n}\leftarrow$ $FB\text{-}CostToCome(x_{n},x_{nearest},x_{gp},T)$
$\ignorespaces\triangleright$ Evaluate cost of path

5:if $\infty\neq c_{n}$ then

6: return $\\{x_{n},x_{nearest},c_{n}\\}$

7:end if

8:return $\\{\varnothing,\varnothing,\inf\\}$

The $FB\text{-}Extend^{*}$ procedure is identical to $Extend^{*}$ except for
the use of $FB\text{-}Extend$ and $FB\text{-}CostToCome$. The
$FB\text{-}Extend$ and $FB\text{-}Extend^{*}$ procedures are illustrated in
Figure 11. Note that the numbers shown in Figure 11 are the edge costs of the
edges they are near. The same is true of Figures 1, 2, and 3 except Figure 11
uses the fillet cost calculation described in Lemma 1.

######  procedure 23 ($\\{x_{n},x_{p}\\}\leftarrow
FB\text{-}Extend^{*}\\!(x_{rand},T)$).

Given $x_{rand}\in X$ and a tree $T=\\{V,E\\}$, the $FB\text{-}Extend^{*}$
uses $FB\text{-}Extend$ to find a node for extending the tree and then finds
the locally optimal path in $V$ for connecting to the new point. The procedure
is given in Algorithm 11.

Algorithm 11 $\\{x_{n},x_{p}\\}\leftarrow FB\text{-}Extend^{*}\\!(x_{rand},T)$

1:$\\{x_{n},x_{p},c_{min}\\}\leftarrow FB\text{-}Extend(x_{rand},T)$

2:if $x_{n}\neq\varnothing$ then $\ignorespaces\triangleright$ If an extension
is possible

3: $X_{near}\leftarrow Near_{\rho,\alpha}(x_{n},T)$

4: for all $x_{near}\in X_{near}$ do $\ignorespaces\triangleright$ Check for a
lower cost connection

5: $x_{gp}\leftarrow Parent(x_{near},T)$

6: $c_{tmp}\leftarrow FB\text{-}CostToCome(x_{n},x_{near},x_{gp},\\!T)$

7: if $c_{tmp}<c_{min}$ then

8: $x_{p}\leftarrow x_{near}$

9: $c_{min}\leftarrow c_{tmp}$

10: end if

11: end for

12: return $\\{x_{n},x_{p}\\}$

13:end if

14:return $\\{\varnothing,\varnothing\\}$;

(a) The tree before $x_{rand}$ is sampled and Algorithm 10 starts. The fillets
are shown in magenta.

(b) $x_{rand}$ is sampled and node E is found to be the closest node to
$x_{rand}$.

(c) $x_{rand}$ is steered toward node E and, after validity checking the new
fillet, node J is created.

(d) $x_{n}$’s neighborhood set is found to be the singleton set of just node
B.

(e) The correctness of connecting to B is verified. Connecting through B and E
results in costs of $5.1$ and $14$ respectively.

(f) Because connecting through node B yields a lower total cost, node J is
connected to node B. The fillets are shown in magenta.

Figure 11: An illustration of the $FB\text{-}Extend$ and
$FB\text{-}Extend^{*}$ procedures.

(a) The tree after $FB\text{-}Extend^{*}$ adds node J to the tree. The fillets
are shown in magenta.

(b) Node E is found to be the only element in node J’s neighborhood set.

(c) The validity of connecting E through J is checked. The original cost of
node E is $11.8$ and its potential cost is $8$.

(d) The validity of E’s children is checked. The original costs of F and G are
$13.7$ and $14.5$ and their potential costs are $10.5$ and $10.2$
respectively.

(e) The validity of E’s grandchildren, nodes H and I, is checked with respect
to connecting node E through node J.

(f) Node E is rewired to have node J as its parent. The fillets are shown in
magenta.

Figure 12: An illustration of the $FB\text{-}Rewire$ procedure.

###  4.2 The Fillet-based Rewire Procedure

In the FB-RRT* framework, care must be taken to ensure both path feasibility
and cost improvement when rewiring. Unlike its straight-line counterpart, it
is not sufficient to choose a parent based purely on the path length to the
node. The following lemma presents a set of sufficient conditions to ensure
that a rewiring will not be detrimental to the tree.

######  Lemma 5.

Assume that a tree $T=\\{V,E\\}$ is given such that (3) is satisfied for all
consecutive nodes. Rewiring $E$ to make $x_{n}\in V$ the new parent of
$x_{near}\in V$ will result in a continuous path with all node costs unchanged
or lowered if the following three conditions are met:

  1. 1.

The resulting path to $x_{near}$ using $x_{n}$ as its parent is obstacle free,
does not violate (3), and the cost of $x_{near}$ is improved, see Figure
12(c).

  2. 2.

The resulting path to each child of $x_{near}$ is obstacle free, does not
violate (3), and the cost of the child is not increased, see Figure 12(d).

  3. 3.

The resulting path to each grandchild of $x_{near}$ does not violate (3), see
Figure 12(e).

Proof: The only paths that will be affected by changing the parent of
$x_{near}$ will be the fillet connecting $x_{near}$ to its grandparent and the
fillets connecting $x_{n}$ to the children of $x_{near}$. Conditions 1, 2, and
3 employ obstacle checking and (3) to ensure that fillet curves do not overlap
in the new section of path nor with the preceding or subsequent sections of
the path.

The path cost to $x_{near}$ will be improved due to 1. The path costs to the
children are not increased per 2. As all other parent and grandparent nodes
remain unchanged, their respective path costs will not increase due to
Corollary 2. The $FB\text{-}Rewire$ procedure is now stated and illustrated in
Figure 12.

######  procedure 24 ($E\leftarrow FB\text{-}Rewire(x_{n},X_{near},T)$).

Given a tree, $T=\\{V,E\\}$, with node $x_{n}\in V$ and set $X_{near}\subset
V$, $FB\text{-}Rewire$ returns a modified tree with $E$ changed to have
$x_{n}$ be the parent to elements of $X_{near}$ if conditions in Lemma 5 are
satisfied. The procedure is given in Algorithm 12.

Algorithm 12 $T\leftarrow FB\text{-}Rewire(x_{n},X_{near},T)$

1:for all $x_{near}\in X_{near}$ do

2: $x_{p}\leftarrow Parent(x_{n},T)$

3: $c_{near}\leftarrow FB\text{-}CostToCome(x_{near},x_{n},x_{p},T)$

4: $\ignorespaces\triangleright$ Check $x_{near}$’s feasibility and that its
cost is reduced

5: if $c_{near}\geq Cost(x_{near},T)$ then

6: goto Continue

7: end if

8: for all $x_{c}\in Children(x_{near},T)$ do

9: $c_{c}\leftarrow FB\text{-}CostToCome(x_{c},x_{near},x_{n},T)$

10: $\ignorespaces\triangleright$ Check the feasibility and cost of
$x_{near}$’s children

11: if $c_{c}>Cost(x_{c},T)$ then

12: goto Continue

13: end if

14: for all $x_{gc}\in Children(x_{c},T)$ do

15: $X_{fillet}\leftarrow Fillet({x_{n},x_{near},x_{c},x_{gc}})$

16: if $\varnothing=X_{fillet}$ then $\ignorespaces\triangleright$ Check the
feasibility of $x_{near}$’s grandchildren

17: goto Continue

18: end if

19: end for

20: end for

21: $x_{p}\leftarrow Parent(x_{near},T)$

22:
$E\leftarrow\left(E\setminus\\{x_{p},x_{near}\\}\right)\cup\\{x_{n},x_{near}\\}$
$\ignorespaces\triangleright$ Rewire around $x_{near}$

23: Continue:

24:end for

25:return $T$

Note that the $Rewire$ procedure described above is different than that in
[Yang et al., 2014b]. In [Yang et al., 2014b], the neighborhood set of
$x_{n}$, $X_{near}$, is checked to ensure that:

  1. a.

Connecting $x_{n}$ and $x_{near}$ will not violate their max angle and
distance conditions.

  2. b.

The curve formed between $x_{n}$ and $x_{near}$ is obstacle free.

  3. c.

The cost of $x_{near}$ will be improved by the rewire operation.

Thus, condition 1 is met (with a conservative continuity condition), but
conditions 2 and 3 are not considered. If Yang et al. [2014b]’s conditions
hold, the children of $x_{near}$ are set to be the children of the parent of
$x_{near}$ and the parent of $x_{near}$ is set to be $x_{n}$, see Figure
13(b). It is important to note that the $Rewire$ operation in [Yang et al.,
2014b] could result in discontinuous paths due to not checking feasibility for
all affected nodes. There is no guarantee that connecting the parent of
$x_{near}$ directly to the children of $x_{near}$ will result in a valid tree.
The angles and distances formed by that connection must first be checked as
illustrated in Figure 13. Moreover, due to not checking costs on all affected
nodes, the $Rewire$ operation in [Yang et al., 2014b] may actually increase
costs to some nodes as shown in Lemma 2.

(a) The Node F is being rewired around the preexisting tree, and node C is F’s
neighborhood set.

(b) Node C is rewired to have node F as its parent, and node E becomes a child
of node B.

Figure 13: After node C has been rewired to node F the angle formed between
nodes A, B, and B’s child has increased, i.e. $\gamma_{b}>\gamma_{a}$. Without
checking, there is no way to know if $\gamma_{b}$ is less then the max angle
allowed.

Note that the FB-RRT and FB-RRT* algorithms are not yet stated as additional
improvements to the sampling and smoothing are first discussed in the
following section. Also note that reverse motion considerations are discussed
in Appendix A.

##  5 Overcoming the Voronoi Property

Nonholonomic constraints exacerbate the slow convergence of RRT*. This section
introduces two refinements to FB-RRT* designed to reduce convergence time.
There are multiple variants of RRT* that aim to improve convergence by
improving sampling [Akgun and Stilman, 2011, Gammell et al., 2014, Kuwata et
al., 2009, Nasir et al., 2013]. Two such variants are used as a basis for
design herein: Informed RRT* (I-RRT*) [Gammell et al., 2014] and Smart RRT*
(S-RRT*) [Nasir et al., 2013]. These algorithms are identical to RRT* before
the first solution is found. However, after the first path to the goal is
found, they use information about the solution to guide sampling toward space
that will improve the final solution. In addition, S-RRT* introduces a path
smoothing procedure that continuously refines the solution path, further
improving convergence. This section first presents I-RRT* and S-RRT*. These
techniques are then combined in the Fillet-based Smart and Informed RRT* (FB-
SI-RRT*) formulation.

###  5.1 Informed RRT*

Informed RRT* (I-RRT*) is an extension of RRT* that aims to reduce the amount
of time spent sampling space that will not improve the final path. It is
observed that any time spent sampling outside of the set that will improve the
path is wasted time. This set is referred to as the “informed” set and denoted
as $X_{i}\subset X_{free}$. Naturally, it would be best to directly sample
$X_{i}$. However, calculating $X_{i}$ can be difficult, if not impossible.

A conservative approximation of $X_{i}$, denoted as $X_{i}^{\prime}$, is
defined in [Gammell et al., 2014] based upon an approximation of the problem’s
optimal cost and the best cost found, $c_{best}$. The optimal cost is denoted
as $c_{min}$ with its approximation denoted as $c_{min}^{\prime}$. The
approximation for $c_{min}^{\prime}$ is calculated as the distance between the
root node and final state in the best-found path, i.e.
$c_{min}^{\prime}=\|x_{r}-x_{t}\|$. $X_{i}^{\prime}$ is defined as

$X_{i}^{\prime}=\mathcal{E}_{x_{r},x_{t}}$ (20)

where $\mathcal{E}_{x_{r},x_{t}}$ is the open set of states in an ellipse with
the focal points set at $x_{r}$ and $x_{t}$. The length of the major axis of
the ellipse is $c_{best}$ and the length of the minor axis is
$\sqrt{c_{best}^{2}-c_{min}^{\prime 2}}$, see Figure 14. With this definition
of $X_{i}^{\prime}$, it is guaranteed that the best solution found so far is
entirely inside the informed set.

Figure 14: The ellipse that defines $X_{i}^{\prime}$.

I-RRT* samples solely within $X_{i}^{\prime}$ once the first solution is
found, reducing the configuration space sampled and improving the probability
that a given sample will improve the solution. Note that I-RRT* is identical
to RRT* with the exception of using $X_{i}^{\prime}$ instead of $X_{s}$ for
sampling new points after the first solution is found. As long as
$c_{min}^{\prime}\leq c_{min}$, it is shown in [Gammell et al., 2014] that
I-RRT* maintains asymptotic optimality.

In an environment with small convex obstacles, I-RRT* is shown to converge
much faster than RRT* [Gammell et al., 2014]. If there are larger nonconvex
obstacles then I-RRT* degrades in performance to RRT*. This is because the
optimal cost, $c_{min}$, is much more than $c_{min}^{\prime}$, causing the
informed subset to be very large.

###  5.2 Smart RRT*

Smart RRT* (S-RRT*) proposes two alternative approaches to improving
convergence [Nasir et al., 2013]. First, similar to I-RRT*, the sampling set
is reduced once a solution is found. Second, S-RRT* proposes a path smoothing
procedure that is used as an integral part of the algorithm to further improve
convergence.

The sampling set is reduced by biasing sampling around the nodes that form the
shortest found path. The idea being that improvements near the path can help
to refine the chosen route around obstacles. These nodes are referred to as
the beacon set, $X_{b}$, and can be defined as

$X_{b}=Solution(x_{t},T)$

where $x_{t}\in X_{t}$ is the end of the shortest path to the target set.
Subsequent iterations then bias the sampling towards the union of balls of
radius $r_{b}$ around each beacon, i.e.

$X_{s}=\bigcup_{x_{b}\in X_{b}}\mathcal{B}_{x_{b},r_{b}}$

A major philosophical difference between S-RRT* and I-RRT* is that S-RRT*
biases the sampling around the beacon set whereas I-RRT* seeks to reduce the
size of the sampling space. The biasing encourages local path refinement with
a continued sampling of $X$ for exploration.

Significant improvements are also made through the addition of a path
smoothing procedure, referred to as $OptimizePath$ in [Nasir et al., 2013].
$OptimizePath$ seeks to straighten out the path each time a better path is
found. This avoids waiting for the sampling to straighten the path, something
that becomes decreasingly probable as the number of samples increases due to
the Voronoi property. The $OptimizePath$ procedure does this by performing
rewire operations on each beacon with every other beacon, as detailed in
Algorithm 13. Note that the $RemoveBetween$ procedure removes all of the nodes
in $X_{b}$ that lie between $x_{near}$ and $x_{ittr}$ not including $x_{near}$
and $x_{ittr}$. This removes nodes from the solution/beacon set that are not
needed. Which straightens out the solution and reduces the number of beacons
that have to be sampled.

Algorithm 13 $X_{b}^{\prime}\leftarrow OptimizePath(X_{b},T)$

1:$X_{b}^{\prime}\leftarrow\varnothing$

2:for all $x_{ittr}\in X_{b}$ do

3: for all $x_{near}\in X_{b}\cap X_{b}^{\prime}$ do
$\ignorespaces\triangleright$ Beacons that have not been used

4: $T\leftarrow Rewire(x_{ittr},\\{x_{near}\\},T)$

5: if $Parent(x_{near},T)=x_{ittr}$ then $\ignorespaces\triangleright$ Remove
unneeded beacons

6: $X_{b}\leftarrow RemoveBetween(x_{near},x_{ittr},X_{b})$

7: end if

8: end for

9: $X_{b}^{\prime}\leftarrow X_{b}^{\prime}\cup\\{x_{ittr}\\}$

10:end for

11:return $X_{b}^{\prime}$

Before the first solution is found, S-RRT* performs identically to RRT*.
However, after the first solution is found, the convergence of S-RRT* is much
faster than RRT*, especially for straight-line motion primitives. It is
important to note that S-RRT* has a tendency to spend more time converging on
local minima than RRT*. In fact, if only the space near the beacon set were to
be sampled after the first solution was found, then S-RRT* would lose its
asymptotic optimality because it would only refine the first path found. We
found that the beacon radius could be relatively small using straight-line
paths, but needed to be increased significantly for curvature constrained
paths.

###  5.3 Smart and Informed Sampling

I-RRT* performs well in small path planning problems where the obstacles are
convex and $c_{min}^{\prime}$ is a good approximation of $c_{min}$. S-RRT*
converges impressively fast, especially when planning using straight-line
paths. Both techniques add parameters that need to be determined – an estimate
of the best cost for informed and the beacon size for smart sampling. This
work develops smart-and-informed sampling that combines S-RRT*’s fast
convergence with an adaptive sample set similar to I-RRT*. It is designed
specifically for the nonholonomic motion primitives to provide a sampling
heuristic that does not require fine tuning of additional parameters.

As is the case with both of its predecessors, the sampling will be identical
to RRT* until the first solution is found. At that point, the $OptimizePath$
procedure, Algorithm 13, is called on the initial solution. Instead of using a
constant radius around each beacon (S-RRT*), or an adaptive set based upon the
optimality of the entire path (I-RRT*), the beacon set, $X_{b}\subset V$, is
used to generate ellipses around each adjacent pair of beacons along the path
that leads from $x_{r}$ to $X_{t}$, as illustrated in Figure 15. The axes of
the ellipse are adapted based on the local optimality of the path, producing a
larger sampling space when path refinement is needed and a smaller space when
the local path is near optimal. Similar to the sampling in S-RRT*, the
sampling after the beacons are found is biased towards the set formed by these
ellipses,

$\mathcal{E}_{X_{b}}=\bigcup_{i=0}^{\mid
X_{b}\mid-1}\mathcal{E}_{x_{b,i},x_{b,i+1}}\subset X.$

This sampling bias encourages path refinement. The full configuration space is
still sampled periodically for sake of exploration.

Figure 15: The four nodes, $x_{b,0}$ through $x_{b,4}$, are connected with
arc-fillets and each sampling ellipse, $\mathcal{E}_{x_{b,0},x_{b,1}}$,
$\mathcal{E}_{x_{b,1},x_{b,2}}$, $\mathcal{E}_{x_{b,2},x_{b,3}}$, and
$\mathcal{E}_{x_{b,3},x_{b,4}}$, is shown in red, blue, green, and orange
respectively. Note that the volume of $\mathcal{E}_{x_{b,0},x_{b,1}}$ is $0$,
because $c_{best,0}=c_{min,0}$.

For each beacon ellipse, $c_{min,i}^{\prime}$ is defined as the distance
between the two adjacent beacons that act as the focal points for that ellipse

$c_{min,i}^{\prime}=\|x_{b,i+1}-x_{b,i}\|$

and $c_{best,i}$ is the cost differential across the two beacons:

$c_{best,i}=Cost(x_{b,i+1})-Cost(x_{b,i})$

Note that for straight-line primitives $c_{best,i}=c_{min,i}^{\prime}$,
causing each ellipse to degenerate to the line between $x_{b,i}$ and
$x_{b,i+1}$. While sampling along this line can be beneficial, we show an
example where it slows convergence because it neglects the exploration half of
the exploration-exploitation paradigm. With the fillets, the ellipse rarely
degenerates to a straight-line. Even when it does the $OptimizePath$ procedure
removes the redundant intermediary points. The smart-and-informed sampling
procedure is now presented.

######  procedure 25 ($x_{rand}\leftarrow
SI\text{-}Sample(i,b_{t},b_{b},X_{b},X_{t})$).

Returns a random point at iteration $i\in\mathbb{Z}_{+}$ given the sampling
biases $b_{t}\in\mathbb{Z}_{+}$ and $b_{b}\in\mathbb{Z}_{+}$, the beacon set
$X_{b}$, and the target set $X_{t}$ as described in Algorithm 14.

The $SI\text{-}Sample$ procedure includes a biasing towards the target set and
a random sampling of the configuration space for sake of exploration.
Additionally, as is the case with S-RRT*’s sampling, some percentage of the
samples must be drawn from the full configuration space to maintain asymptotic
optimality. This is because SI-RRT* makes no guaranties about its ellipses
containing the optimal solution as I-RRT* does. Sampling within the ellipses
of SI-RRT* and the beacons of S-RRT* has the effect of biasing the search to
locally refine the current best path. The sampling of the beacon ellipses
works in conjunction with the $OptimizePath$ procedure to seek improvements to
the current best path found. The $OptimizePath$ procedure works to straighten
paths while the sampling of the beacon ellipses works to see if local
perturbations will improve path length.

Algorithm 14
$x_{rand}\leftarrow\\!SI\text{-}Sample(i,\\!b_{t},\\!b_{b},\\!X_{b},\\!X_{t})$

1:if $i\mathrel{\vbox{\hbox{.}\hbox{.}\hbox{.}}}b_{t}$ then
$\ignorespaces\triangleright$ Bias towards target set

2: $x_{rand}\leftarrow Sample(X_{t})$

3:else if $X_{b}\neq\varnothing\land
i\mathrel{\vbox{\hbox{.}\hbox{.}\hbox{.}}}b_{b}$ then
$\ignorespaces\triangleright$ Bias towards beacon ellipses

4: $x_{rand}\leftarrow Sample(\mathcal{E}_{X_{b}})$

5:else $\ignorespaces\triangleright$ Otherwise sample a random point

6: $x_{rand}\leftarrow Sample(X)$

7:end if

8:return $x_{rand}$

###  5.4 Fillet-based Smart and Informed RRT*

With the $SI\text{-}Sample$ procedure in hand, all of the components are in
place for presenting the Fillet-Based Smart and Informed RRT* (FB-SI-RRT*)
algorithm. The FB-SI-RRT* algorithm is defined in Algorithm 15. The straight-
line counterpart can be expressed by removing the $FB$ prefix in all of the
procedures. Furthermore, the fillet generation procedure used in
$FB\text{-}CostToCome$ can be replaced with either the arc or Bézier fillets.

The FB-SI-RRT* algorithm is very similar to the traditional RRT* algorithm as
defined in Algorithm 7. The major differences are the use of the fillet-based
procedures in place of the straight-line counterparts and the
$SI\text{-}Sample$ procedure in place of the $Biased\text{-}Sample$. The path
optimization procedure from S-RRT* is also included in SI-RRT* but is absent
in traditional RRT*.

The FB-SI-RRT* algorithm begins on line 1 by initializing the tree to consider
the initial orientation of the vehicle and setting $X_{b}$ and $c_{b}$ to the
empty set. As with RRT*, a prefixed number of iterations is specified for
refining the path. Each iteration begins with the sampling of a new point
using the $SI\text{-}Sample$ procedure. This balances biasing towards the
target set to find the goal, biasing towards the beacon set to refine the best
path found, and sampling from the general obstacle free space for exploration.
The sampled point is then used within $FB\text{-}Extend^{*}$ in line 6 to grow
the tree in the direction of the new sample while respecting the fillet
continuity constraints. If a connection to the tree is found, a new node is
then inserted into the tree on line 8. The edge set is then rewired around the
new node on line 9 using the $FB\text{-}Rewire$ to consider the fillet
continuity and cost improvement requirements. If the first path or a shorter
path to the target has been found, then the beacon set is updated in line 11.
If $c_{b}$ has changed, and thus $X_{b}$ has changed, a
$FB\text{-}OptimizePath$ procedure is used on line 14 in an attempt to refine
the beacon set. Note that the $FB\text{-}OptimizePath$ has not been defined,
but it can be expressed by changing line 4 of Algorithm 13 to use the
$FB\text{-}Rewire$ procedure.

Algorithm 15
$X_{sol}\\!\leftarrow\\!FB\text{-}SI\text{-}RRT^{*}\\!(x_{r},\\!\psi_{r},\\!X_{t},\\!b_{t},\\!b_{b},\\!n)$

1:$T\leftarrow FB\text{-}Initialize_{d_{init}}(x_{r},\psi_{r})$

2:$X_{b}\leftarrow\varnothing$

3:$c_{b}\leftarrow\varnothing$

4:for $i=1,\ldots,n$ do

5: $x_{rand}\leftarrow SI\text{-}Sample(i,b_{t},b_{b},X_{b},X_{t})$
$\ignorespaces\triangleright$ Smart-and-informed sampling

6: $\\{x_{n},x_{p},c_{n}\\}\leftarrow FB\text{-}Extend^{*}\\!(x_{rand},T)$
$\ignorespaces\triangleright$ Extend tree towards new point

7: if $x_{n}\neq\varnothing$ then

8: $T\leftarrow InsertNode(x_{n},x_{p},T)$ $\ignorespaces\triangleright$ Add
point to tree

9: $T\leftarrow FB\text{-}Rewire(x_{n},Near_{\rho,\alpha}(x_{n},T),T)$
$\ignorespaces\triangleright$ Rewire edges around $x_{n}$

10: if $x_{n}\\!\in\\!X_{t}\land\left(X_{b}=\varnothing\lor
Cost(x_{n},T)<c(X_{b})\right)$ then

11: $X_{b}\leftarrow Solution(x_{n},T)$ $\ignorespaces\triangleright$ Update
best path found

12: end if

13: if $X_{b}\neq\varnothing\land c_{b}\neq c(X_{b})$ then
$\ignorespaces\triangleright$ If the best path has changed

14: $X_{b}\leftarrow FB\text{-}OptimizePath(X_{b},T)$

15: $c_{b}\leftarrow c(X_{b})$

16: end if

17: end if

18:end for

19:return $X_{b}$

Both an informed variation and smart variation to the FB-RRT* algorithm can be
created with small variations to Algorithm 15. The informed algorithm can be
formed by replacing line 5 with informed sampling and removing line 14. The
smart algorithm can be formed by replacing line 5 with smart sampling.

##  6 Examples

A series of examples are now shown to demonstrate the Fillet-based RRT*
approach. Three environments were chosen to illustrate the performance of
various RRT*-based planners. Within each environment, 16 series of simulations
were conducted to illustrate and evaluate the sampling and motion primitive
variations. RRT*, I-RRT*, S-RRT*, and SI-RRT* planned using straight-line,
arc-fillet, Bézier-fillet, and Dubin’s path motion primitives.

Note that a comparison between motion primitive types is not meant to show
that one motion primitive is better than another. Planning with straight-line
paths is going to have better convergence characteristics due to their
simplicity and lack of dynamic constraints. In fact, the only primitives that
can be directly compared in this fashion are arc-fillets and the Dubin’s paths
as both assume the same dynamic constraints. The straight-line primitive is
included to show a best-case scenario, providing a pseudo cost of considering
the additional dynamic constraints. This section proceeds with details on the
simulations followed by a description of the different environments. A
comparison is made between the fillet formulation presented herein and that of
[Yang et al., 2014b] followed by an example that justifies the need to
consider curvature constraints while path planning. Finally, the section ends
with a discussion of the results.

###  6.1 Simulation Details

The obstacles are represented with an occupancy grid with each pixel
corresponding to one square millimeter. When performing obstacle collision
checks clearance of $0.5m$ is required on all sides. As orientation is well
defined in the case of Dubin’s paths and fillets, the points that are $0.5m$
to the right and left of the paths are checked. In the case of straight-line
paths, orientation is not well-defined at the nodes so points are checked
$0.5m$ in every cardinal direction. Paths are generated and checked at one-
centimeter resolution.

The steering constant, $\eta$, and neighbor search radius, $\rho$, are both
$3m$. The max number of neighbors to search, $\alpha$, is $100$. The check
target period, $b_{t}$, is $50$ and the target set, $X_{t}$, is a circle of
radius $0.1m$. The Dubin’s radius is set to $0.5m$ and likewise, the maximum
curvature constraint imposed on the fillet generation is $2m^{-1}$. The root
node to first node distance, $d_{init}$, is $1m$. Note that these values are
somewhat aggressive for some curvature constrained applications, but they
allow the straight-line primitive to provide a tighter bound on the possible
performance characteristics of the curvature constrained planners. For S-RRT*,
the beacon radius is $3m$ unless otherwise stated. Note that this is not
necessarily the best choice of beacon radius, as shown in Figure 21. However,
smaller radius values are very detrimental to the Dubin’s path results. The
beacon bias, $b_{b}$, is $3$ for both S-RRT* and SI-RRT*.

Results are gathered using OMPL [Moll et al., 2015]. Simulation code can be
found in our open-source repository
https://gitlab.com/utahstate/robotics/fillet-rrt-star. As the sampling is
random, each simulation series consists of 100 individual simulations with the
average results being presented. The results were gathered on an AMD
Ryzen™Threadripper™2990WX processor. Convergence plots were made by fitting a
10th order polynomial using a least-squares fitting algorithm as described in
[Venables and Ripley, 2002].

A least-squares approach is used as the sampling times for path length are not
uniform across all simulations and not all simulations find the initial path
at the same time. Note that while the path length for any one run will be
monotonically decreasing with time, the least squares fitted plot does not
always have the same monotonic property. The reason is that a particular run
may not find a solution until well after other runs and the initial solution
it finds may be much larger than the current solution of the other runs,
effectively causing the average to increase at the time the run first produces
path length data.

###  6.2 Environments

The three environments shown in Figure 16 were chosen to present and evaluate
the performance of differing RRT* approaches. The environments are referred to
as the Spiral world, the Cluttered world, and the Maze world.

The Spiral world is made up of one narrow passage that twists around the
starting point. The world is $40m$ by $40m$,
$x_{r}=\begin{bmatrix}0&0\end{bmatrix}^{T}$, and the center of $X_{t}$ is at
$\begin{bmatrix}-15&-15\end{bmatrix}^{T}$. The only path from $x_{r}$ to
$X_{t}$ is through a narrow hallway forming a “bug trap” like set of
obstacles. The Spiral world tests planners’ abilities to find a way out of the
confined starting area and then converge through all of the passageways. The
“bug trap” like design makes it difficult for Dubin’s paths based planners to
find an initial solution, and I-RRT*’s cost heuristic is a poor estimate of
$c_{min}$ in this environment. The environment is well suited for smart
sampling as there is only one path and refinements are beneficial at each
turn.

The Cluttered world is composed of many overlapping circular obstacles. The
world is $100m$ by $100m$, $x_{r}=\begin{bmatrix}-40&-40\end{bmatrix}^{T}$,
and the center of $X_{t}$ is at $\begin{bmatrix}40&40\end{bmatrix}^{T}$. The
abundance of small obstacles results in many small local minima, but there are
still large open areas for exploration. The Cluttered world tests the
planners’ ability to break out of local minima. I-RRT* based sampling is well-
suited in the environment as the I-RRT* heuristic is a good estimate of the
optimal path length.

The Maze world features a series of narrow passages and dead ends. The world
is $50m$ by $50m$, $x_{r}=\begin{bmatrix}-11&-22.5\end{bmatrix}^{T}$, and the
center of $X_{t}$ is at $\begin{bmatrix}2.5&12.5\end{bmatrix}^{T}$. The Maze
world has fewer local minima than the Cluttered world and consists of long
narrow corridors. This world tests the planners’ ability to find high-quality
initial solutions quickly and then converge past those initial solutions.
While the local minima can be detrimental to beacon-based sampling, the
optimal cost heuristic in I-RRT*’s sampling is poor in this case, proving
detrimental to I-RRT*’s convergence.

Figure 16: The resulting paths from running RRT* for 30 seconds in the Spiral
world (left), Cluttered world (middle), and the Maze world (right). Straight-
line, Dubin’s path, Arc-fillet, and Bézeir-fillet paths are shown in green,
red, blue, and brown respectively.

###  6.3 Comparison with Previous Work

Note that this work’s FB-RRT* differs from what is given in [Yang et al.,
2014a, b] in the following ways:

  * •

The generalization of [Yang et al., 2014a, b]’s $\gamma_{max}$ and $d_{min}$
based path continuity constraints to the less restrictive form given in (3).
See Figure 9(a) for an illustration of how (3) is less restrictive than using
the $\gamma_{max}$/$d_{min}$ constraints.

  * •

The addition of a pseudo “node orientation” in the nearest neighbor search
heuristically penalizes turns and enables us to forgo the k-nearest-neighbor
search that [Yang et al., 2014b] uses. See the explanation of
$FB\text{-}Extend$ in Section 4.1 for more information.

  * •

A newly developed rewiring procedure that ensures continuity and cost
improvement in the resulting path. See Section 4.2 for more information on
$FB\text{-}Rewire$.

  * •

The generalization of the fillet-based planner structure to make use of any
fillet type instead of just Bézeir-fillets.

This section uses convergence plots to compare the formulation of the fillet
constraints in this work to the formulation given in [Yang et al., 2014a, b].
Specifically, instead of constraining node addition in the tree with (3) we
use the constants given in [Yang et al., 2014a]. [Yang et al., 2014a] defines
a max node-to-node angle, $\gamma_{max}$, and then uses that angle to define a
minimum node-to-node distance, $d_{min}=d\\!\left(\gamma_{max}\right)$. Any
nodes that form an angle greater than $\gamma_{max}$ or are closer together
than $2d_{min}$ are deemed invalid. This is a conservative approximation of
the constraints defined in (3), see Section 3.4 for more details. We refer to
the version of FB-RRT* that uses [Yang et al., 2014a]’s constraints as SB-
RRT*.

Note that SB-RRT* differs from what is given in [Yang et al., 2014b] because
the $FB\text{-}Rewire$ procedure is used to avoid the invalid tree
configurations that result from the $Rewire$ procedure in [Yang et al.,
2014b], see Figure 13. The Bézeir-fillet is used for comparison because that
is the fillet used in [Yang et al., 2014a, b]. The Cluttered world was chosen
for this simulation because it is similar to the simulated environment used in
[Yang et al., 2014a]. The max allowed curvature is kept at $2m^{-1}$,
$\gamma_{max}=\frac{\pi}{2}rad.$, and $d_{min}=0.7938m$.

(a) Convergence of solution cost in the Cluttered world over 5 minutes. FB-
RRT* is shown in blue and SB-RRT* is shown in purple.

(b) Example solutions found in the Cluttered world after 5 minutes. FB-RRT* is
shown in blue and SB-RRT* is shown in purple.

Figure 17: A comparison of the solution quality found by FB-RRT* and SB-RRT*.
FB-RRT* converges faster and to a shorter solution then SB-RRT*. FB-RRT* is
shown in blue and SB-RRT* is shown in purple.

Figure 17(a) shows the convergence of SB-RRT* and FB-RRT* in the scenario
described. FB-RRT* far outperforms SB-RRT* in both initial convergence speed
and the solution to which it settles over time. FB-RRT* converges to a much
shorter path then SB-RRT* as the constraints given in [Yang et al., 2014a]
force each node along the solution to be at least $2d_{min}$ distance away
from each other. As the solution converges, it becomes difficult to shorten
the path further without reducing the number of nodes that make up the
solution path. We emphasize that this simulation only shows the benefit of
using the constraints in (3). A significant benefit is also received from the
updated rewiring procedure that ensures continuous paths are produced.

###  6.4 Curvature Constrained Paths

This section provides an example of when considering curvature constraints
during path planning is advantageous. A common approach to curvature
constrained path planning is to plan using straight-line motion primitives and
then smooth the path after planning; see, for example, Chapter 11 of [Beard
and McLain, 2012]. However, this can lead to invalid paths.

Figure 18: The resulting paths from running RRT* in an environment where the
straight-line path turns sharply down a corridor. Straight-line and arc-fillet
paths are shown in green and blue respectively.

One of the scenarios where this would be a problem is shown in Figure 18. Path
planning using straight-line primitives converges to the solution shown in
green. This is the shortest path between the start and goal locations while
ensuring a minimum distance is maintained between the path and obstacles.

However, a path that goes through this hallway can not satisfy the curvature
constraints of the problem without hitting the walls or performing a complex,
multi-turn maneuver. See Appendix A for details on generating a multi-turn
maneuver with FB-RRT*. If curvature constants are considered during path
planning, as is the case for FB-RRT*, then the path planner will return a
solution through the second corridor, which is wide enough to make the turn.

###  6.5 Results

The different motion primitives are now compared in terms of the initial
solution, the convergence, and the effects of the various sampling techniques.

####  6.5.1 Initial Solution Comparison

Table 3 shows the performance of each motion primitive in finding the initial
solution in terms of the mean time and length of the initial solution for each
environment. The results in Table 3 apply regardless of the sampling technique
used as each sampling approach behaves identically to RRT* before the first
solution is found.

The Spiral and Maze worlds take longer on average to find an initial solution
than the Cluttered world. There are a couple of reasons that this may occur.
Both worlds have more narrow passages, resulting in a large number of
iterations that fail to extend the tree because their paths become invalidated
by obstacles. Furthermore, the Maze world has many dead-ends, allowing the
tree to spend time growing in a direction that will not lead to the target
set.

Planners using the arc-fillet primitive find an initial solution in a time
comparable to that of the planners using straight-line paths in all of the
environments considered, performing slightly faster in the Maze world.
Similarly, the path lengths found are comparable, with the straight-line based
paths being slightly shorter in all cases. The added cost of considering
continuity in curvature is seen as the planners using Bézier-fillets require
roughly twice the time to find an initial solution as their arc-fillet
counterparts in all three environments. Table 3(b) shows the Bézeir-fillet
based planners finding a slightly shorter initial solution than their arc-
fillet counterparts. This may be due to the fact that the Bézier-fillet
planners take more time and iterations to find an initial solution. During
that same time, the planners using arc-fillets are refining their solutions,
always having a shorter path than the Bézier-fillet planners.

It takes significantly longer for planners using Dubin’s paths to find an
initial solution in each of the worlds tested here. The only world where
Dubin’s paths based planners found an initial solution in a comparable amount
of time is the Cluttered world, although the initial path is also
significantly longer. This is, in part, due to loops in the path, similar to
that depicted in Figure 4. In the Cluttered world, the first path found has
many loops and bends, resulting in a longer path length on average. These
loops prove even more detrimental in the Spiral and Maze worlds with their
narrow passageways. The path length does not increase as dramatically for
Dubin’s paths in the Spiral and Maze worlds as it does in the Cluttered world
because there is not as much room for looping paths. However, there is a
significant impact to the initial solution time as planning with Dubin’s paths
requires over 2 to 9 times as long as Bèzier-fillets and 5 to 15 times as long
as arc-fillets.

It is important to note that the least squares fitting distorts the average
transients plots in Figure 19. The average initial solution length for each
planner is equivalent for any particular motion primitive. However, the rapid
convergence of some planners cause the least squares solution to appear lower
at the initial solution time.

Table 3: Initial solution results.

Motion Primitive | Spiral | Cluttered | Maze
---|---|---|---
Straight-line | $\,\,\,4.328\pm\,\,\,1.158$ | $0.498\pm 0.196$ | $11.183\pm\,\,\,3.455$
Dubin’s path | $96.792\pm 38.124$ | $1.585\pm 1.357$ | $40.849\pm 17.587$
Arc-fillet | $\,\,\,6.373\pm\,\,\,2.124$ | $0.586\pm 0.527$ | $\,\,\,7.473\pm\,\,\,2.057$
Bézeir-fillet | $10.288\pm\,\,\,3.292$ | $1.081\pm 1.966$ | $14.002\pm\,\,\,4.04\,\,\,$
(a) The means and standard deviations of the time in seconds it takes for RRT*
to find an initial solution.

Motion Primitive | Spiral | Cluttered | Maze
---|---|---|---
Straight-line | $136.05\pm 3.91$ | $167.42\pm 12.14$ | $142.25\pm 7.84$
Dubin’s path | $161.68\pm 7.73$ | $275.07\pm 39.55$ | $162.65\pm 9.86$
Arc-fillet | $144.45\pm 6.47$ | $184.54\pm 21.81$ | $148.56\pm 8.4\,\,\,$
Bézeir-fillet | $137.76\pm 5.45$ | $181.45\pm 23.76$ | $147.2\,\,\,\pm 8.03$
(b) The means and standard deviations of the initial path length in meters
after RRT* has found an initial solution.

Figure 19: The convergence plots of the Spiral world on top, Cluttered world
in the middle, and the Maze world on bottom over 50 seconds. Images from left
to right show the transients of the straight-line, Dubin’s path, arc-fillet,
and Bézeir-fillet motion primitives. RRT* is shown in blue, I-RRT* in brown,
S-RRT* in green, and SI-RRT* in red. From the top going down, the red shaded
area starts once the solution length is within 5 percent of the best averaged
path found after 50 seconds. The yellow shading starts withing 2 percent, and
the green shading starts at the best solution length found.

Figure 20: The average time it took each planner to find a solution that was
within 5 and 2 percent of the best averaged solution found after 50 seconds.
Images from left to right show the results of the straight-line, arc-fillet,
and Bézeir-fillet motion primitives. RRT* is shown in blue, I-RRT* in brown,
S-RRT* in green, and SI-RRT* in red.

####  6.5.2 Convergence Times of Motion Primitives

Once an initial solution is found, the algorithms work to converge on the
shortest path. In this section, the resulting path lengths generated from
planning using different motion primitives are directly compared. Note that
this is inherently an unfair comparison as the primitives have different
kinematic constraints. The only two motion primitives that use the same
kinematic constraints are Dubin’s paths and arc-fillets. The straight-line
motion primitive does not respect curvature constraints while Bézeir-fillet
primitive considers a curvature rate constraint in addition to the curvature
constraint considered by the arc-fillet primitive. Each scenario has been
designed such that the optimal path for each motion primitive will be similar,
unlike the scenario in Figure 18. This allows the straight-line convergence
rate to be a pseudo best-case solution. The results will show that the fillet-
based planners have comparable results to the straight-line planners.

Figure 19 shows the performance of each motion primitive with the four
different sampling techniques. The green shaded areas start at the path length
of the best averaged solution found for that world in 50 seconds. The yellow
shaded area starts once the path length is within 2 percent of the best
averaged solution for the respective environment. Similarly, the red shaded
area starts once the path length is within 5 percent of the best averaged
solution for the respective environment.

Table 4 shows the best performing planners in each world and type of motion
primitive. In the Spiral world, the best averaged solution found within 50
seconds was found by S-RRT* planning with straight-lines with a path length of
$116.14m$. For the Cluttered world, the best was found by I-RRT* planning with
straight-lines with a path length of $128.79m$. For the Maze world, the best
was found by SI-RRT* planning with straight-lines with a path length of
$129.54m$. Note that in each case, the planner that found the shortest
averaged solution was planning with straight-line motion primitives. This is
expected because the straight-line primitive is the least constrained.

Table 4: The best performing planner and associated average path length for that planner in each environment and motion primitive combination. Results are calculated after running the planners for 50 seconds. Motion Primitive | Spiral | Cluttered | Maze
---|---|---|---
Straight-line |  S-RRT* | $116.14$ | I-RRT* | $128.79$ | SI-RRT* | $129.54$
Dubin’s path | SI-RRT* | $122.6$ | I-RRT* | $145.54$ | SI-RRT* | $143.17$
Arc-fillet | SI-RRT* | $116.34$ | I-RRT* | $129.46$ | SI-RRT* | $130.31$
Bézeir-fillet | SI-RRT* | $116.49$ | I-RRT* | $129.98$ | SI-RRT* | $130.92$

Figure 20 shows the 5 and 2 percent convergence times in a bar graph for a
quick comparison of results. It is worth noting that planners using the
Dubin’s path primitive struggle to approach the 5 percent convergence region
and are subsequently left out of Figure 20. On the other hand, the arc-fillet
planners perform quite well despite assuming the same motion constraints as
the Dubin’s primitive. The arc-fillet based planners perform comparably, and
in some cases better, than the straight-line primitives in converging to the 5
percent threshold. Convergence begins to suffer for the 2 percent threshold,
with some arc-fillet planners unable to cross that threshold in the Maze
world.

The Bézier-fillet planners show an increase in convergence time, underscoring
the cost of requiring continuity in curvature. SI-RRT* is the only planner
that is able to cross the 2 percent threshold when planning with Bèzier-
fillets in the Maze world. However, when planning with Bézier-fillets in the
Cluttered world, the smart sampling techniques (S-RRT* and SI-RRT*) fail to
cross the 2 percent threshold while both RRT* and I-RRT* are able.

####  6.5.3 The Effect of Sampling Techniques

The environments have little effect on the trends of ranking the performance
of the motion primitives. The planning with straight-lines typically
outperforms the arc-fillets, which outperforms the Bèzier-fillets, which in
turn outperforms planning with Dubin’s paths. However, the environment has a
significant effect on the performance of the sampling procedures.

Table 4 shows that each environment has a sampling procedure that works best
in that environment. For the Cluttered world, I-RRT* performs the best across
all motion primitives. Whereas in the Spiral and Maze worlds, the greedier
sampling heuristics perform better. In the Maze world, SI-RRT* performs the
best for all motion primitives. In the Spiral world, S-RRT* performs the best
when planning with straight-lines but SI-RRT* performs best for all other
motion primitives. This shows that S-RRT*’s sampling works very well when
planning with straight-lines but tends to struggle more when planning with
kinematic constraints.

As mentioned, the Cluttered world is ideal for I-RRT*’s sampling approach and,
as expected, the I-RRT* based planners perform the best in terms of 5 and 2
percent convergence. However, the transient plots in Figure 19 show a very
interesting trend. The smart approaches (S-RRT* and SI-RRT* based planners)
show significantly faster initial convergence. This is due to the fact that
the smart approaches focus on refining the best solution instead of searching
the environment, which also means that they tend to spend more time in local
minima. This is particularly noticeable in the Cluttered world where the smart
approaches plateau for a time before dropping again. The Spiral and Maze
worlds present environments in which there are fewer local minima and as such
the smart approaches continue rapidly refining the solution until the 5 and 2
percent thresholds.

Figures 19 and 20 show that the SI-RRT* based planners focus heavily on
refining the current shortest solution. In the Cluttered world, this proves
detrimental as it causes the planner to focus on local minima instead of
searching for other paths through the obstacle topology. SI-RRT*’s greedy
convergence to local solutions is why SI-RRT*’s solution cost plateaus in the
Cluttered world, see Figure 19. In the Maze and Spiral worlds, it proves
beneficial and results in the fastest convergence, both initially and to the 5
and 2 percent thresholds for all but the straight-line motion primitives. This
may be because there are fewer local minima in the obstacle topology of these
worlds. As expected, the smart-and-informed sampling does quite poorly for
straight-line primitives.

Figure 21 shows that the SI-RRT* approach provides greedy refinement for
fillet-based primitives without requiring the tuning of an extra beacon-size
parameter. Figure 21 compares the results from S-RRT* based planners over
various beacon sizes to the results of the respective SI-RRT* based planner.
In the Cluttered world, SI-RRT* spends most of its time refining local minima
and results in the worst convergence times. In both the Maze and the Spiral
worlds, the smart-and-informed sampling performs near the best, except when
considering straight-line primitives. While an iterative search over beacon
sizes for S-RRT* can produce similar convergence results to SI-RRT*, the
smart-and-informed sampling does not require additional tuning to find the
best beacon size. Note that the resulting best beacon size for S-RRT* sampling
is dependent upon both the environment and the motion primitive, making such a
search difficult prior to execution.

Figure 21: The average time it took each planner to find a solution what was
within 5 and 2 percent of the best averaged solution found after 50 seconds.
Images from left to right show the results of the straight-line, arc-fillet,
and Bézeir-fillet motion primitives. S-RRT* with a beacon sizes of $0.1m$,
$0.5m$, $1m$, $3m$, $5m$, $10m$, and $15m$ are shown in brown, cyan, magenta,
green, orange, purple, and violet respectfully. SI-RRT* is shown in red.

##  7 Conclusion

In this work, an RRT-based path planning algorithm is proposed that uses
general fillets as motion primitives. An arc-fillet is designed to provide
path continuity similar to a Dubin’s path while a Bèzier-fillet is developed
to provide continuity in path curvature. RRT*-like procedures are developed to
accommodate fillet-based motion primitives. Simulation results show that
planning with arc-fillets significantly outperforms the use of Dubin’s paths
as a motion primitive. Planning with arc-fillets is shown to perform almost as
well as straight-line motion primitives. Planning with Bèzier-fillets exhibits
slightly worse performance than arc-fillets, although it far outperformed
planning with Dubin’s despite considering more complex dynamic constraints. A
comparison is made between informed sampling, smart sampling, and a new smart-
and-informed sampling technique. Like their straight-line counterparts, the
fillet-based planners perform better with informed sampling when the heuristic
for the shortest path is valid and better with smart sampling when there are
fewer local minima. The smart-and-informed sampling performed well for fillet-
based motion primitives and was found to be most applicable to environments
with fewer local minima. In such environments, it performed on par with the
best smart beacon size without the need for an iterative search for that
beacon size.

##  8 Declarations

###  8.1 Funding

The authors declare that no funds, grants, or other support were received
during the preparation of this manuscript.

###  8.2 Competing Interests

The authors have no relevant financial or non-financial interests to disclose.

###  8.3 Code Availability

Simulation code can be found in our open-source repository
https://gitlab.com/utahstate/robotics/fillet-rrt-star.

###  8.4 Author Contributions

All authors contributed to the work’s conception and design. Software
development, material preparation, and data collection were performed by James
Swedeen. Data analyses were performed by James Swedeen and Dr. Greg Droge.
James Swedeen is the primary author with major editorial and conceptual
contributions made by Dr. Greg Droge and Dr. Randall Christensen at various
stages of the writing process. All authors read and approved the final
manuscript.

###  8.5 Ethics Approval

Not applicable.

###  8.6 Consent to Participate

Not applicable.

###  8.7 Consent for Publication

Not applicable.

## References

  * Akgun and Stilman [2011] B. Akgun and M. Stilman.  Sampling heuristics for optimal motion planning in high dimensions.  In _2011 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , pages 2640–2645, 2011.  10.1109/IROS.2011.6095077.
  * Beard and McLain [2012] R. Beard and T. McLain.  _Small Unmanned Aircraft: Theory and Practice_.  Princeton University Press, USA, 2012.  ISBN 0691149216.
  * Cui et al. [2018] P. Cui, W. Yan, and X. Guo.  Path planning for robot with pose constraints using dubins-rrt.  In _2018 3rd International Conference on Advanced Robotics and Mechatronics (ICARM)_ , pages 560–565, 2018.  10.1109/ICARM.2018.8610812.
  * Gammell et al. [2014] J. Gammell, S. Srinivasa, and T. Barfoot.  Informed rrt*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic.  _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2014.  10.1109/IROS.2014.6942976.
  * Gravesen [1997] J. Gravesen.  Adaptive subdivision and the length and energy of bézier curves.  _Computational Geometry_ , 8(1):13–31, 1997.  ISSN 0925-7721.  https://doi.org/10.1016/0925-7721(95)00054-2.  URL https://www.sciencedirect.com/science/article/pii/0925772195000542.
  * Karaman and Frazzoli [2011] S. Karaman and E. Frazzoli.  Sampling-based algorithms for optimal motion planning.  _The International Journal of Robotics Research_ , 30(7):846–894, 2011.  10.1177/0278364911406761.  URL https://doi.org/10.1177/0278364911406761.
  * Kobilarov [2012] M. Kobilarov.  Cross-entropy motion planning.  _The International Journal of Robotics Research_ , 31(7):855–871, 2012.  10.1177/0278364912444543.  URL https://doi.org/10.1177/0278364912444543.
  * Kuffner and LaValle [2000] J. J. Kuffner and S. M. LaValle.  Rrt-connect: An efficient approach to single-query path planning.  In _Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065)_ , volume 2, pages 995–1001 vol.2, 2000.
  * Kuwata et al. [2009] Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How.  Real-time motion planning with applications to autonomous urban driving.  _IEEE Transactions on Control Systems Technology_ , 17(5):1105–1118, 2009.  10.1109/TCST.2008.2012116.
  * Lan and Di Cairano [2015] X. Lan and S. Di Cairano.  Continuous curvature path planning for semi-autonomous vehicle maneuvers using rrt.  In _2015 European Control Conference (ECC)_ , pages 2360–2365, 2015\.  10.1109/ECC.2015.7330891.
  * LaValle [2006] S. M. LaValle.  _Planning Algorithms_.  Cambridge University Press, Cambridge, U.K., 2006.  Available at http://planning.cs.uiuc.edu/.
  * Li et al. [2016] Y. Li, Z. Littlefield, and K. E. Bekris.  Asymptotically optimal sampling-based kinodynamic planning.  _The International Journal of Robotics Research_ , 35(5):528–564, 2016.  10.1177/0278364915614386.  URL https://doi.org/10.1177/0278364915614386.
  * Masehian and Kakahaji [2014] E. Masehian and H. Kakahaji.  Nrr: a nonholonomic random replanner for navigation of car-like robots in unknown environments.  _Robotica_ , 32(7):1101–1123, 2014.  10.1017/S0263574713001276.
  * Moll et al. [2015] M. Moll, I. A. Sucan, and L. E. Kavraki.  Benchmarking motion planning algorithms: An extensible infrastructure for analysis and visualization.  _IEEE Robotics Automation Magazine_ , 22(3):96–102, 2015.
  * Moon and Ching [2015] C. Moon and W. Ching.  Kinodynamic planner dual-tree rrt (dt-rrt) for two-wheeled mobile robots using the rapidly exploring random tree.  _IEEE Transactions on Industrial Electronics_ , 62:1080–1090, 2015.  10.1109/TIE.2014.2345351.
  * Nasir et al. [2013] J. Nasir, F. Islam, U. Malik, Y. Ayaz, O. Hasan, M. Khan, and M. Muhammad.  Rrt*-smart: A rapid convergence implementation of rrt*.  _International Journal of Advanced Robotic Systems_ , 10, 2013.  10.5772/56718.
  * Noreen et al. [2016] I. Noreen, A. Khan, and Z. Habib.  Optimal path planning using rrt* based approaches: A survey and future directions.  _International Journal of Advanced Computer Science and Applications_ , 7(11), 2016.  10.14569/IJACSA.2016.071114.  URL http://dx.doi.org/10.14569/IJACSA.2016.071114.
  * Qureshi and Ayaz [2015] A. H. Qureshi and Y. Ayaz.  Intelligent bidirectional rapidly-exploring random trees for optimal motion planning in complex cluttered environments.  _Robotics and Autonomous Systems_ , 68:1–11, 2015.  ISSN 0921-8890.  https://doi.org/10.1016/j.robot.2015.02.007.  URL https://www.sciencedirect.com/science/article/pii/S0921889015000317.
  * Tahir et al. [2018] Z. Tahir, A. H. Qureshi, Y. Ayaz, and R. Nawaz.  Potentially guided bidirectionalized rrt* for fast optimal path planning in cluttered environments.  _Robotics and Autonomous Systems_ , 108:13–27, 2018.  ISSN 0921-8890.  https://doi.org/10.1016/j.robot.2018.06.013.  URL https://www.sciencedirect.com/science/article/pii/S0921889017309387.
  * Venables and Ripley [2002] W. N. Venables and B. D. Ripley.  _Modern Applied Statistics with S_.  Springer, New York, fourth edition, 2002.  URL https://www.stats.ox.ac.uk/pub/MASS4/.  ISBN 0-387-95457-0.
  * Yang and Sukkarieh [2010] K. Yang and S. Sukkarieh.  An analytical continuous-curvature path-smoothing algorithm.  _IEEE Transactions on Robotics_ , 26(3):561–568, 2010.
  * Yang et al. [2014a] K. Yang, S. Moon, S. Yoo, J. Kang, N. L. Doh, H. B. Kim, and S. Joo.  Spline-based rrt path planner for non-holonomic robots.  _Journal of Intelligent & Robotic Systems_, 73(1-4):763–782, 2014a.
  * Yang et al. [2014b] K. Yang, S. K. Gan, J. Huh, and S. Joo.  Optimal spline-based rrt path planning using probabilistic map.  In _2014 14th International Conference on Control, Automation and Systems (ICCAS 2014)_ , pages 643–646, 2014b.

##  Appendix A 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.

Algorithm 16 $X_{fillet}\leftarrow
ReverseFillet\left(x_{0},x_{1},x_{2},x_{3}\right)$

1:if $x_{2,d}=x_{3,d}$ then $\ignorespaces\triangleright$ Fillet with uniform
path direction

2: $X_{unidir}\leftarrow
Fillet\left(x_{0,xy},x_{1,xy},x_{2,xy},x_{3,xy}\right)$

3:
$X_{fillet}\leftarrow\left\\{\begin{bmatrix}x_{u,x}&x_{u,y}&x_{3,d}\end{bmatrix}^{T},x_{u}\in
X_{unidir}\right\\}$

4:else $\ignorespaces\triangleright$ Fillet with switching path direction

5: $\ignorespaces\triangleright$ Rotation matrix for the orientation of
$x_{2}$

6: $R_{2}\leftarrow
R\left(-atan2\left(x_{2,y}-x_{1,y},x_{2,x}-x_{1,x}\right)\right)$

7: $\ignorespaces\triangleright$ Put each point in the frame with $x_{2}$ at
the origin and $x_{1}$ on the x-axis

8: ${}_{2}x_{0}\leftarrow R_{2}\cdot\left[x_{0,xy}-x_{2,xy}\right]$

9: ${}_{2}x_{1}\leftarrow R_{2}\cdot\left[x_{1,xy}-x_{2,xy}\right]$

10: ${}_{2}x_{2}\leftarrow\begin{bmatrix}0&0\end{bmatrix}^{T}$

11: ${}_{2}x_{3}\leftarrow R_{2}\cdot\left[x_{3,xy}-x_{2,xy}\right]$

12:
${}_{2}x_{3f}\leftarrow\begin{bmatrix}-{{}_{2}x_{3,x}}&{{}_{2}x_{3,y}}\end{bmatrix}^{T}$
$\ignorespaces\triangleright$ Flip ${}_{2}x_{3}$ across y-axis

13: $X_{unidir}\leftarrow
Fillet\left({{}_{2}x_{0}},{{}_{2}x_{1}},{{}_{2}x_{2}},{{}_{2}x_{3f}}\right)$
$\ignorespaces\triangleright$ Make unidirectional fillet

14: $\ignorespaces\triangleright$ Flip any part of the fillet that is in front
of $x_{2}$ over the y-axis

15:
$X_{fillet}\leftarrow\left\\{\begin{array}[]{lr}\begin{bmatrix}x_{u,x}&x_{u,y}&x_{2,d}\end{bmatrix}^{T}&\text{for
}x_{u,x}\leq 0\\\
\begin{bmatrix}-x_{u,x}&x_{u,y}&x_{3,d}\end{bmatrix}^{T}&\text{for
}x_{u,x}>0\end{array},x_{u}\in X_{unidir}\right\\}$

16: $\ignorespaces\triangleright$ Move the fillet back to the original
coordinate frame

17: $X_{fillet}\leftarrow\left\\{\begin{bmatrix}R_{2}^{T}x_{f,xy}+x_{2,xy}\\\
x_{f,d}\end{bmatrix},x_{f}\in X_{fillet}\right\\}$

18:end if

19:return $X_{fillet}$

(a) The four points that the fillet will be made between.

(b) The four points after they have been transformed.

(c) ${}_{2}x_{3}$ is flipped over the y-axis.

(d) The unidirectional fillet is generated.

(e) The second half of the unidirectional fillet is flipped to reverse it.

(f) The fillet is transformed back into the original coordinate frame.

Figure 22: An illustration of the $ReverseFillet$ procedure.

Algorithm 16 gives the procedure for generating a reverse fillet and Figure 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 Figure 22(b). This frame is defined to make flipping $x_{3}$ easier. The
prescript $2$ is used to denote a point in this frame, i.e., ${}_{2}x_{0}$ is
the point $x_{0,xy}$ in the new frame. On line 12, ${}_{2}x_{3}$ is flipped
across the y-axis, see Figure 22(c). The transformed set of points
${}_{2}x_{0}$, ${}_{2}x_{1}$, ${}_{2}x_{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 Figure 22(d). 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 Figure 22(e). Line 17 transforms fillet back to the original
coordinate frame, as shown in Figure 22(f).

Figure 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.

One scenario where the ability to plan forward and reverse fillets is
beneficial is shown in Figure 23. Figure 23 uses the same planning
configuration as Figure 18 from Section 6.4. The only difference between
Figures 23 and 18 is that 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.

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.