Informed RRT*: Optimal Incremental Path Planning Focused through an Admissible Ellipsoidal Heuristic

Rapidly-exploring random trees (RRTs) are popular in motion planning because they efficiently find solutions 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. This paper shows that for problems seeking to minimize path length, the subset of states that can improve a solution can be described by a hyperellipsoid. This allows us to show that the probability of improving a solution with global sampling becomes arbitrarily small as the size of the planning problem increases or as the solution approaches the theoretical minimum. This paper presents an exact method to sample this subset directly, allowing for the creation of incremental informed-sampling planners with improved convergence characteristics. 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. It is shown experimentally that the presented algorithm outperforms RRT* in rate of convergence, final solution cost, and ability to find difficult passages while demonstrating less dependence on the size of the planning problem.