Real-Time Rulebook-Aware Nonlinear MPC for Autonomous Driving with Priority-Biased Tiered Slacks

2026-07-12

AI Summary

This work addresses the challenge of autonomous driving motion planning under real-time constraints, requiring a balance among safety, rule compliance, comfort, and efficiency while enabling auditable decision-making. The authors propose W-SQP, a nonlinear model predictive controller based on weighted hierarchical slack variables, which encodes nine categories of driving rules into a four-layer nonlinear program with shared slack variables. A strongly separated hierarchical penalty mechanism prioritizes satisfaction of higher-priority rules while preserving hard actuator constraints. Leveraging CasADi and IPOPT, the system solves the optimization problem online at 10 Hz, guaranteeing feasible solutions at every time step and logging rule residuals for auditability. In closed-loop evaluations across 150 scenarios from the Waymo Open Motion Dataset, the method exhibits no systematic failures in safety or compliance metrics, with only localized performance degradation observed in highly ambiguous, complex scenes.

Abstract

Autonomous-vehicle motion planners must resolve conflicts among safety, regulation, comfort, and efficiency in real time while exposing those decisions for audit. We present W-SQP, a weighted tiered-slack nonlinear model predictive controller (NMPC) that compiles nine driving-rule families into a four-tier shared-slack nonlinear program solved online with CasADi and IPOPT; the name denotes the weighted quadratic slack penalty, not a sequential-quadratic-programming solver. Strongly separated tier penalties bias residual violations toward lower-priority rules while leaving actuation bounds hard. The controller replans from its executed state at $10$\,Hz and records per-rule residuals on every cycle. A $90$\,ms solver-time limit returns an anytime iterate that is projected through the vehicle dynamics before execution; median and maximum observed wall-clock solve times were $28$ and $104$\,ms. We evaluate W-SQP in closed loop on 150 Waymo Open Motion Dataset scenarios in Waymax against reactive and proposal-and-select baselines, and introduce a log-independent protocol that separates safety and regulatory compliance from resemblance to the recorded human trajectory. Under this protocol, W-SQP shows no systematic group-level deficit relative to expert replay on the log-independent safety and regulatory rules, with several localized regressions in the hardest, highest-divergence scenarios. The results characterize W-SQP as an auditable, priority-biased, anytime-capable NMPC prototype rather than a hard-real-time or formally safe controller.

Problem

• autonomous driving
• real-time decision-making
• rule compliance
• multi-objective conflict
• auditability

Innovation

• tiered-slack NMPC
• rulebook-aware planning
• priority-biased optimization
• anytime solver
• auditable autonomy