Asymptotically Optimal Planning by Feasible Kinodynamic Planning in State-Cost Space
Kris Hauser, Yilun Zhou
Summary
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 EST and RRT algorithms as subroutines, at a superior or comparable level to related planners.
- K. Hauser and Y. Zhou. Asymptotically Optimal Planning by Feasible Kinodynamic Planning in State-Cost Space. IEEE Transactions of Robotics, 32(6): 1431-1443, 2016. Also in arXiv:1505.04098 [cs.RO], 2015.
- Python Optimal Motion Planning software library used for all examples (v0.1).
Benchmarking results for a variety of planners. The AO-EST and AO-RRT planners use the proposed state-cost space planning technique.
Dubins car example, new AO-EST algorithm. Tree of motions is drawn in workspace, with best solution depicted in green. One frame taken per 100 iterations. The planner slowly converges to the optimal homotopy class (right pathway).
Dubins car example, new AO-RRT algorithm. The planner rapidly converges to the optimal homotopy class (right pathway).
Dubins car example, prior Stable-Sparse-RRT algorithm (Dobson and Bekris 2015). Paths are found in the optimal homotopy class but they are quite suboptimal, leading it to find better paths in the suboptimal homotopy class.
Double integrator example, new AO-EST algorithm. Tree of motions is drawn in workspace, with best solution depicted in green. One frame taken per 100 iterations. The planner slowly converges to a near-optimal straight line path.
Double integrator example, new AO-RRT algorithm. The planner improves the optimal path slowly.
Double integrator example, prior Stable-Sparse-RRT algorithm (Dobson and Bekris 2015). Due to a poor choice of a two key sparseness parameters, roadmap growth stops after some amount of time. Choosing these parameters appropriately is a challenging issue, requiring a balance between time-to-first solution and convergence rate. To address this issue, the authors propose an adaptive variant called SST*, shown below.
Double integrator example, prior SST* algorithm (Dobson and Bekris 2015). This performs better than Sparse-Stable-RRT (above).
Pendulum example, new AO-EST algorithm. Tree of motions is drawn in state space, with best solution depicted in green. One frame taken per 100 iterations. The planner makes progressive improvement, with the path taking fewer spirals as more planning is performed.
Pendulum example, new AO-RRT algorithm. Progress appears to "stall out".
Pendulum example, prior Stable-Sparse-RRT algorithm (Dobson and Bekris 2015). Progress appears to "stall out".
