An Admissible Heuristic to Improve Convergence in Kinodynamic Planners Using Motion Primitives

This letter introduces a new heuristic function that can be incorporated in any kinodynamic planner using motion primitives, to the purpose of increasing its convergence rate. The heuristic function is proven to be admissible and, hence, the optimality properties of the planning algorithm are preserved. Notably, it can be applied to planning problems with generic agent motion models and cost criteria, since it depends only on the database of motion primitives. The proposed heuristic has been integrated into a randomized sampling-based and a deterministic kinodynamic planner, and its effectiveness has been shown in numerical examples with different agent motion models and cost criteria.