Hierarchical Motion Planning With Dynamical Feasibility Guarantees for Mobile Robotic Vehicles

Motion planning for mobile vehicles involves the solution of two disparate subproblems: the satisfaction of high-level logical task specifications and the design of low-level vehicle control laws. A hierarchical solution of these two subproblems is efficient, but it may not ensure compatibility between the high-level planner and the constraints that are imposed by the vehicle dynamics. To guarantee such compatibility, we propose a motion-planning framework that is based on a special interaction between these two levels of planning. In particular, we solve a special shortest path problem on a graph at a higher level of planning, and we use a lower level planner to determine the costs of the paths in that graph. The overall approach hinges on two novel ingredients: a graph-search algorithm that operates on sequences of vertices and a lower level planner that ensures consistency between the two levels of hierarchy by providing meaningful costs for the edge transitions of a higher level planner using dynamically feasible, collision-free trajectories.