# A Two-Stage Optimization-based Motion Planner for Safe Urban Driving

arXiv: https://arxiv.org/abs/2002.02215 · HTML: https://mlsanity.com/paper/2002.02215/

## Key facts

- Cited by 9 papers in the map (data through 14 Oct 2026).
- First posted on arXiv 6 Feb 2020.

## Built on by (most cited first, 9 of 9)

- [PILOT: Efficient Planning by Imitation Learning and Optimisation for Safe Autonomous Driving](https://mlsanity.com/paper/2011.00509/) (2020)
- [InteractionNet: Joint Planning and Prediction for Autonomous Driving with Transformers](https://mlsanity.com/paper/2309.03475/) (2023)
- [Bilevel Multi-Armed Bandit-Based Hierarchical Reinforcement Learning for Interaction-Aware Self-Driving at Unsignalized Intersections](https://mlsanity.com/paper/2502.03960/) (2025)
- [Proactive Emergency Collision Avoidance for Automated Driving in Highway Scenarios](https://mlsanity.com/paper/2310.17381/) (2023)
- [Learning the References of Online Model Predictive Control for Urban Self-Driving](https://mlsanity.com/paper/2308.15808/) (2023)
- [Attacking Motion Planners Using Adversarial Perception Errors](https://mlsanity.com/paper/2311.12722/) (2023)
- [Synergizing Decision Making and Trajectory Planning Using Two-Stage Optimization for Autonomous Vehicles](https://mlsanity.com/paper/2411.18974/) (2024)
- [Adaptive Splitting of Reusable Temporal Monitors for Rare Traffic Violations](https://mlsanity.com/paper/2405.15771/) (2024)
- [Conflict-Aware Additive Guidance for Flow Models under Compositional Rewards](https://mlsanity.com/paper/2605.20758/) (2026)

## Abstract (arXiv)

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.