Search papers, labs, and topics across Lattice.
This paper introduces W-SQP, a novel weighted tiered-slack nonlinear model predictive controller designed for autonomous vehicles that integrates nine driving-rule families into a shared-slack nonlinear program. By employing a priority-biased approach, the controller effectively manages conflicts among safety, regulation, comfort, and efficiency in real time, achieving a median solver time of 28 ms while maintaining compliance with safety and regulatory standards. Evaluation against baseline methods demonstrates that W-SQP performs comparably to expert human trajectories, highlighting its potential as an auditable and flexible solution for autonomous driving scenarios.
W-SQP achieves real-time compliance with complex driving rules while prioritizing safety and efficiency, outperforming traditional methods in dynamic environments.
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.
