Optimal path planning in complex cost spaces with sampling-based algorithms

Sampling-based algorithms for path planning, such as the Rapidly-exploring Random Tree (RRT), have achieved great success, thanks to their ability to efficiently solve complex high-dimensional problems. However, standard versions of these algorithms cannot guarantee optimality or even high-quality for the produced paths. In recent years, variants of these methods, such as T-RRT, have been proposed to deal with cost spaces: by taking configuration-cost functions into account during the exploration process, they can produce high-quality (i.e., low-cost) paths. Other novel variants, such as RRT, can deal with optimal path planning: they ensure convergence toward the optimal path, with respect to a given path-quality criterion. In this paper, we propose to solve a complex problem encompassing this two paradigms: optimal path planning in a cost space. For that, we develop two efficient sampling-based approaches that combine the underlying principles of RRT∗ and T-RRT. These algorithms, called T-RRT∗ and AT-RRT, offer the same asymptotic optimality guarantees as RRT. Results presented on several classes of problems show that they converge faster than RRT∗ toward the optimal path, especially when the topology of the search space is complex and/or when its dimensionality is high.