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

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*.