Hierarchical Solution of Non-Convex Optimal Control Problems with Application to Autonomous Driving

This article considers the problem of planning collision-free reference trajectories for autonomous vehicles. Because optimal solutions to the problem may be hard to obtain in real time, we devise a procedure which yields potentially suboptimal but feasible solutions under standard assumptions. A hierarchical decomposition of the original problem based on a novel interpretation of the maneuver concept allows to consider independently the questions of which maneuver to choose and how to execute it in detail. The computation of trajectories relies on mixed-integer programming and can be performed efficiently based on the proposed decomposition, while controllable sets are used to assess the feasibility of a maneuver prior to its execution. An exemplary overtaking scenario demonstrates the efficacy of our approach.