Safe hierarchical model predictive control and planning for autonomous systems

Abstract

Planning and control for autonomous vehicles usually are hierarchically separated. However, increasing performance demands and operating in highly dynamic environments requires a frequent re-evaluation of the planning and tight integration of control and planning to guarantee safety, performance, and reliability. We propose an integrated hierarchical predictive control and planning approach to tackle this challenge. The planner and controller are based on repeated solutions of moving horizon optimal control problems. To increase flexibility and feasibility, 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 different modes are slow-speed movement with high precision and fast-speed movements with large uncertainty bounds. Allowing for different control modes reduces 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 compatibility between the layers and modes. Simulation results illustrate the efficiency and applicability of the proposed hierarchical strategy.