Recent road trials have shown that guaranteeing the safety of driving
decisions is essential for the wider adoption of autonomous vehicle technology.
One promising direction is to pose safety requirements as planning constraints
in nonlinear, non-convex optimization problems of motion synthesis. However,
many implementations of this approach are limited by uncertain convergence and
local optimality of the solutions achieved, affecting overall robustness. To
improve upon these issues, we propose a novel two-stage optimization framework:
in the first stage, we find a 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 of safety and road rule compliance generating a solution in
the right subspace, while the NLP stage refines the solution within the safety
bounds for feasibility and smoothness. We demonstrate the effectiveness of our
framework via simulated experiments of complex urban driving scenarios,
outperforming a state-of-the-art baseline in metrics of convergence, comfort
and progress.Comment: IEEE Transactions on Robotics (T-RO), 202
