A Bidirectional, Holonomic RRT Path Planner
Algorithm Description
The fundamental operation used in growing an RRT is the EXTEND
operation depicted in the animation below. Given a target
configuration (randomly selected or not), a distance metric defined on
the configuration space is used to calculate the node q_near in
the existing tree that is nearest to a target configuration
q_target.
If the distance to the target configuration is less than the parameter
e (the RRT step size), then the algorithm attempts to grow a
new branch connecting directly to q_target. Otherwise, an
intermediate node q_new is generated at a distance of e
along the straight line between q_near and q_target.
If the new branch is collisionfree, then it is added to the tree. If
an intermediate node was generated, the algorithm continues to attempt
to grow the new branch towards q_target (the "CONNECT"
operation) until either the configuration is reached, or a collision
occurs.
Planning queries are solved by incrementally building two
RapidlyExploring Random Trees (RRTs) of free configurations rooted at
the start and the goal. The algorithm attempts to establish a
connection between the two trees, which implicitly defines a
collisionfree path between the start and the goal.
When a new branch is successfully added to one of the trees, the
branch terminal node becomes the target configuration for the other
tree.
Example
The sequence of images below shows snapshots of two RRTs during the
planning process for a simple 2D example. The final image shows the
computed path after optimization.
Here is a link to a collection of pictures and animations of a variety
of example path planning problems.
Analysis
RRTs are biased to expand towards unexplored regions of the space,
which is why they are called "rapidlyexploring". The greedy
behavior in growing branches towards target configurations and
attempting to connect the two trees allows many planning queries to be
solved very efficiently. Despite this greediness, the limiting
distribution of nodes in an RRT have been shown to ultimately converge
towards the sampling distribution, even for nonconvex spaces. (see
references)
Like most heuristic search algorithms, RRTConnect sacrifices path
optimality and completeness in exchange for practical
efficiency in searching high dimensional spaces. However, the
paths computed have been observed to be relatively short, and rarely
suffer from loops or selfintersections. In addition, the algorithm
has been shown to be probabilistically complete, which means
that the probability of failing to find a solution path (if one
exists) decreases exponentially with planning time.
The main drawback to the algorithm is its frequent use of
nearestneighbor queries. A simple bruteforce linear algorithm for
computing nearest neighbors works fine for trees with approximately
less than 20,000  40,000 nodes. For difficult planning queries
requiring larger trees, efficient data structures and algorithms are
needed to quickly locate nearest neighbors (e.g. BSPs, kd trees).
