# Multi-Modal Model Predictive Control through Batch Non-Holonomic Trajectory Optimization: Application to Highway Driving

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

## Key facts

- Cited by 9 papers in the map (data through 14 Oct 2026).
- First posted on arXiv 21 Sep 2021.

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

- [CALMM-Drive: Confidence-Aware Autonomous Driving with Large Multimodal Model](https://mlsanity.com/paper/2412.04209/) (2024)
- [Bi-Level Optimization Augmented with Conditional Variational Autoencoder for Autonomous Driving in Dense Traffic](https://mlsanity.com/paper/2212.02224/) (2022)
- [Learning the References of Online Model Predictive Control for Urban Self-Driving](https://mlsanity.com/paper/2308.15808/) (2023)
- [End-to-End Learning of Behavioural Inputs for Autonomous Driving in Dense Traffic](https://mlsanity.com/paper/2310.14766/) (2023)
- [Global Tensor Motion Planning](https://mlsanity.com/paper/2411.19393/) (2024)
- [MMD-OPT : Maximum Mean Discrepancy Based Sample Efficient Collision Risk Minimization for Autonomous Driving](https://mlsanity.com/paper/2412.09121/) (2024)
- [Re4MPC: Reactive Nonlinear MPC for Multi-model Motion Planning via Deep Reinforcement Learning](https://mlsanity.com/paper/2506.08344/) (2025)
- [Safe and Nonconservative Contingency Planning for Autonomous Vehicles via Online Learning-Based Reachable Set Barriers](https://mlsanity.com/paper/2509.07464/) (2025)
- [Personalized Autonomous Driving via Optimal Control with Clearance Constraints from Questionnaires](https://mlsanity.com/paper/2602.08326/) (2026)

## Abstract (arXiv)

Standard Model Predictive Control (MPC) or trajectory optimization approaches perform only a local search to solve a complex non-convex optimization problem. As a result, they cannot capture the multi-modal characteristic of human driving. A global optimizer can be a potential solution but is computationally intractable in a real-time setting. In this paper, we present a real-time MPC capable of searching over different driving modalities. Our basic idea is simple: we run several goal-directed parallel trajectory optimizations and score the resulting trajectories based on user-defined meta cost functions. This allows us to perform a global search over several locally optimal motion plans. Although conceptually straightforward, realizing this idea in real-time with existing optimizers is highly challenging from technical and computational standpoints. With this motivation, we present a novel batch non-holonomic trajectory optimization whose underlying matrix algebra is easily parallelizable across problem instances and reduces to computing large batch matrix-vector products. This structure, in turn, is achieved by deriving a linearization-free multi-convex reformulation of the non-holonomic kinematics and collision avoidance constraints. We extensively validate our approach using both synthetic and real data sets (NGSIM) of traffic scenarios. We highlight how our algorithm automatically takes lane-change and overtaking decisions based on the defined meta cost function. Our batch optimizer achieves trajectories with lower meta cost, up to 6x faster than competing baselines.