Asymptotically optimal feedback planning using a numerical Hamilton-Jacobi-Bellman solver and an adaptive mesh refinement

Abstract

We present the first asymptotically optimal feedback planning algorithm for nonholonomic systems and additive cost functionals. Our algorithm is based on three well-established numerical practices: 1) positive coefficient numerical approximations of the Hamilton-Jacobi-Bellman equations; 2) the Fast Marching Method, which is a fast nonlinear solver that utilizes Bellman’s dynamic programming principle for efficient computations; and 3) an adaptive mesh-refinement algorithm designed to improve the resolution of an initial simplicial mesh and reduce the solution numerical error. By refining the discretization mesh globally, we compute a sequence of numerical solutions that converges to the true viscosity solution of the Hamilton-Jacobi-Bellman equations. In order to reduce the total computational cost of the proposed planning algorithm, we find that it is sufficient to refine the discretization within a small region in the vicinity of the optimal trajectory. Numerical experiments confirm our theoretical findings and establish that our algorithm outperforms previous asymptotically optimal planning algorithms, such as PRM* and RRT*.