Incremental path repair in hierarchical motion-planning with dynamical feasibility guarantees for mobile robotic vehicles

New requirements of autonomous mobile vehicles necessitate hierarchical motion-planning techniques that not only find a plan to satisfy high-level specifications, but also guarantee that this plan is suitable for execution under vehicle dynamical constraints. In this context, the H-cost motion-planning technique has been reported in the recent literature. We propose an incremental motion-planning algorithm based on this technique. The proposed algorithm retains the benefits of the original technique, while significantly reducing the associated computational time. In particular, the proposed iterative algorithm presents during intermediate iterations feasible solutions, with the guarantee that the algorithm eventually converges to an optimal solution. The costs of solutions at intermediate iterations are almost always nonincreasing. Therefore, the proposed algorithm is suitable for real-time implementations, where hard bounds on the available computation time are imposed, and where the original H-cost optimization algorithm may not have sufficient time to converge to a solution at all. We illustrate the proposed algorithm with numerical simulation examples.