A Two-Stage Optimization Approach to Safe-by-Design Planning for Autonomous Driving

Lessons learned from the increasing diversity of road trial deployments of autonomous vehicles have made clear that guaranteeing safety of driving decisions is a crucial bottleneck on the path towards wider adoption. A promising direction is to pose safety requirements as planning constraints in nonlinear optimization problems for motion synthesis. However, many implementations of this approach are limited by uncertain convergence and local optimality of the solutions achieved, affecting overall robustness. In this paper, we propose a novel two-stage optimization framework: in the first stage, we find a global but approximate solution to a Mixed-Integer Linear Programming (MILP) formulation of the motion synthesis problem, the output of which initializes a second Nonlinear Programming (NLP) stage. The MILP stage enforces hard constraints including safety and road rules, while the NLP stage refines that solution within safety bounds to make it feasible with respect to vehicle dynamics and smoothness. We demonstrate the usefulness of our framework through experiments in complex driving situations, showing it outperforms a state of the art baseline in terms of convergence, comfort and progress metrics.