Hierarchical motion planning with kinodynamic feasibility guarantees: Local trajectory planning via model predictive control

Motion planners for autonomous vehicles often involve a two-level hierarchical structure consisting of a high-level, discrete planner and a low-level trajectory generation scheme. To ensure compatibility between these two levels of planning, we previously introduced a motion planning framework based on multiple-edge transition costs in the graph used by the discrete planner. This framework is enabled by a special local trajectory generation problem, which we address in this paper. In particular, we discuss a trajectory planner based on model predictive control for complex vehicle dynamical models. We demonstrate the efficacy of our overall motion planning approach via examples involving non-trivial vehicle models and complex environments, and we offer comparisons of our motion planner with state-of-the-art randomized sampling-based motion planners.