Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic

Jonathan D. Gammell, Siddhartha S. Srinivasa, Timothy D. Barfoot

Seminal April 2014

TL;DR Informed RRT* is RRT* that, once it has a path, samples only the ellipsoid of states that could still shorten it, and samples it directly instead of by rejection. In a narrow-gap test it finds the path through the gap in 4.00 s, where RRT* needs 12.32 s.

What they do

With uniform sampling, RRT* ends up optimizing the path to every state in the space, which is wasted effort in a single-query problem. When the cost is path length, only states inside an ellipsoid with the start and goal as foci can shorten the current solution. Informed RRT* samples that ellipsoid directly once it has a first solution. It keeps RRT*’s completeness and optimality guarantees and adds no tuning parameters.

How it works

Informed subset. Straight-line distance is an admissible heuristic, so every state that could beat the best cost so far lies in a prolate hyperspheroid. Its long diameter equals that cost, and it shrinks as the path improves.
Why direct sampling. A uniform sample can improve the path with probability at most the ellipsoid’s share of the sampled volume. That share goes to zero in large maps or as the path nears optimal. Rejection sampling from the tightest bounding box succeeds at most 8% of the time in 6D.
Sampler. Draw a point uniformly in the unit n-ball, scale it by the ellipsoid’s radii, rotate it onto the start-goal axis (rotation from an SVD) and shift it to the midpoint.
Algorithm. Same as RRT* until the first solution, then sample from the current ellipsoid and compute the rewiring radius for that smaller subproblem.

Results

Both planners share the same code; each setting is run 100 times.

Without obstacles, the cost provably converges linearly, reaching the optimum within machine zero after 1142 iterations.
As the map grows relative to the start-goal distance, Informed RRT* takes roughly constant time to come within 2% of optimal, while RRT* slows down.
It reaches tighter tolerances in significantly fewer iterations and finds narrow passages sooner. With a 3% off-centre gap, it finds the path through the gap in 4.00 s, versus 12.32 s for RRT*.
On random worlds in higher dimensions, its path 60 s after the first solution is cheaper than RRT*’s.

Limitations

The search is focused only after a first solution exists, and the focusing does nothing while the ellipsoid is larger than the map. The ellipsoid can shrink no further than the optimal path allows, so obstacles that force a detour limit the gain. Only path-length cost in R^n is covered. The convergence proof assumes no obstacles, and all experiments are simulated.

Videos

Figures

From Figure 1: RRT* and Informed RRT* reach paths of the same cost on a random world. After its first solution, Infor... — From Figure 1: RRT* and Informed RRT* reach paths of the same cost on a random world. After its first solution, Informed RRT* samples only the ellipse of states that could still improve the path, whatever their homotopy class.

From Figure 4: In 2D, the informed subset is an ellipse with the start and goal as foci. Its long axis is the best pa... — From Figure 4: In 2D, the informed subset is an ellipse with the start and goal as foci. Its long axis is the best path cost found so far, and its eccentricity is the straight-line start-goal distance divided by that cost.

From Figure 3: Without obstacles, each better path shrinks the ellipse, which makes the next improvement easier. With... — From Figure 3: Without obstacles, each better path shrinks the ellipse, which makes the next improvement easier. With start and goal 100 units apart, the first solution comes at 59 iterations; by 1142 iterations the ellipse has collapsed to the straight line, within machine zero of the optimum.

From Figure 8: Median time to get within 2% of the optimal cost as the map widens relative to the start-goal distance... — From Figure 8: Median time to get within 2% of the optimal cost as the map widens relative to the start-goal distance, over 100 runs. RRT* slows down as the map grows, while Informed RRT* takes about the same time.

Abstract

Rapidly-exploring random trees (RRTs) are popular in motion planning because they find solutions efficiently to single-query problems. Optimal RRTs (RRT*s) extend RRTs to the problem of finding the optimal solution, but in doing so asymptotically find the optimal path from the initial state to every state in the planning domain. This behaviour is not only inefficient but also inconsistent with their single-query nature. For problems seeking to minimize path length, the subset of states that can improve a solution can be described by a prolate hyperspheroid. We show that unless this subset is sampled directly, the probability of improving a solution becomes arbitrarily small in large worlds or high state dimensions. In this paper, we present an exact method to focus the search by directly sampling this subset. The advantages of the presented sampling technique are demonstrated with a new algorithm, Informed RRT*. This method retains the same probabilistic guarantees on completeness and optimality as RRT* while improving the convergence rate and final solution quality. We present the algorithm as a simple modification to RRT* that could be further extended by more advanced path-planning algorithms. We show experimentally that it outperforms RRT* in rate of convergence, final solution cost, and ability to find difficult passages while demonstrating less dependence on the state dimension and range of the planning problem.

Summaries are written automatically from each paper's text. Figures and videos come from the paper and its project page; all credit goes to the authors.

Matt Strong