A Bidirectional, Holonomic RRT Path Planner

Algorithm Description

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 collision-free, 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 Rapidly-Exploring 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 collision-free 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 "rapidly-exploring". 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 non-convex spaces. (see references)

Like most heuristic search algorithms, RRT-Connect 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 self-intersections. 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 nearest-neighbor queries. A simple brute-force 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, k-d trees).