Asymptotically optimal planning by feasible kinodynamic planning in a state-cost space

This paper presents an equivalence between feasible kinodynamic planning and optimal kinodynamic planning, in that any optimal planning problem can be transformed into a series of feasible planning problems in a state-cost space, whose solutions approach the optimum. This transformation yields a meta-algorithm that produces an asymptotically optimal planner, given any feasible kinodynamic planner as a subroutine. The meta-algorithm is proven to be asymptotically optimal and a formula is derived relating expected running time and solution suboptimality. It is directly applicable to a wide range of optimal planning problems because it does not resort to the use of steering functions or numerical boundary-value problem solvers. On a set of benchmark problems, it is demonstrated to perform, using the expansive space tree (EST) and rapidly-exploring random tree (RRT) algorithms as subroutines, at a level that is superior or comparable to related planners.