A two-stage optimization-based motion planner for safe urban driving

Francisco Eiras¹ ², Majd Hawasly¹, Stefano V. Albrecht¹ ³, Subramanian Ramamoorthy¹ ³

¹ – Five AI Ltd., UK
² – School of Informatics, University of Edinburgh, UK
³ – Department of Engineering Science, University of Oxford, UK

Published in IEEE Transactions on Robotics (T-RO) 2021

Abstract: 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.

Figure 1: A Two-Stage Optimization Planner Architecture: (a) from an initial scene – comprising driveable surface limits, static vehicles (yellow), moving vehicles with predicted trajectories (red), and a reference path to follow (light blue) – a transform T yields the input to the planner in the reference path-based coordinate frame. The MILP stage (b) solves a linearized version of the planning problem, which initializes (c) the nonlinear, kinematically-feasible NLP stage. Then, T⁻¹ transforms the output back to a trajectory in the world coordinate frame (d).