Safe Hierarchical Model Predictive Control and Planning for Autonomous Systems

Planning and control for autonomous vehicles usually are hierarchical separated. However, increasing performance demands and operating in highly dynamic environments requires an frequent re-evaluation of the planning and tight integration of control and planning to guarantee safety. We propose an integrated hierarchical predictive control and planning approach to tackle this challenge. Planner and controller are based on the repeated solution of moving horizon optimal control problems. The planner can choose different low-layer controller modes for increased flexibility and performance instead of using a single controller with a large safety margin for collision avoidance under uncertainty. Planning is based on simplified system dynamics and safety, yet flexible operation is ensured by constraint tightening based on a mixed-integer linear programming formulation. A cyclic horizon tube-based model predictive controller guarantees constraint satisfaction for different control modes and disturbances. Examples of such modes are a slow-speed movement with high precision and fast-speed movements with large uncertainty bounds. Allowing for different control modes reduces the conservatism, while the hierarchical decomposition of the problem reduces the computational cost and enables real-time implementation. We derive conditions for recursive feasibility to ensure constraint satisfaction and obstacle avoidance to guarantee safety and ensure compatibility between the layers and modes. Simulation results illustrate the efficiency and applicability of the proposed hierarchical strategy.