A Feasibility-Enforcing Primal-Decomposition SQP Algorithm for Optimal Vehicle Coordination

Mario Zanon, Robert Hult, Sébastien Gros, Paolo FalconearXiv 2017 · DOI · arXiv

Abstract

In this paper we consider the problem of coordinating autonomous vehicles approaching an intersection. We cast the problem in the distributed optimisation framework and propose an algorithm to solve it in real time. We extend previous work on the topic by testing two alternative algorithmic solutions in simulations. Moreover, we test our algorithm in experiments using real cars on a test track. The experimental results demonstrate the applicability and real-time feasibility of the algorithm.

Introduction

Coordinating multiple autonomous vehicles at an intersection represents a significant challenge in the field of intelligent transportation systems. This problem is characterized as a distributed mixed-integer optimal control problem, involving both discrete decisions (crossing order) and continuous control (velocity profiles). While the discrete ordering is NP-hard, this work focuses on the efficient, real-time solution of the continuous coordination problem.

Problem Formulation

The objective is to coordinate vehicles such that they pass through a shared intersection area without collision while optimizing for factors like fuel efficiency and passenger comfort.

Intersection coordination model defining entry and exit points.

As shown in Figure 1, the intersection is defined by entry ($p_i^{in}$) and exit ($p_i^{out}$) points for each vehicle $i$. The central coordinator manages the timing variables $t_{in}$ and $t_{out}$, which represent the times at which vehicles enter and leave the conflict zone.

Proposed Methodology: Distributed SQP

The researchers propose a Primal-Decomposition Sequential Quadratic Programming (SQP) algorithm. The architecture is distributed: a central coordinator suggests timing parameters, and individual vehicles solve local Model Predictive Control (MPC) problems.

Feasibility-Enforcing Projection

A core innovation is the "feasibility-enforcing projection" method. Unlike standard relaxation techniques that use slack variables, this method projects infeasible timing suggestions back onto the vehicle's physical capability limits. This ensures that local quadratic programs remain well-defined and solvable, significantly improving robustness compared to traditional penalty-based methods.

Sensitivity and Regularization

To update the global plan efficiently, the central node utilizes first- and second-order sensitivities provided by the vehicles. These sensitivities are computationally inexpensive to derive once the local MPC is solved. Additionally, block-diagonal Hessian regularization is employed to maintain convexity and ensure rapid convergence.

Experimental Evaluation

The algorithm was validated through both numerical simulations (up to six vehicles) and real-world experiments at the AstaZero proving ground using Volvo S60 and XC90 vehicles equipped with V2V communication and RTK GPS.

Vehicle trajectories and velocity profiles during coordinated crossing.

Key Results

  • Real-Time Performance: Central SQP iterations accounted for only 3.4% of total computation time, with the majority of processing occurring in parallel on the vehicles.
  • Robustness: The system successfully handled 1Hz GPS updates and unreliable wireless communication.
  • Convergence: The projection method typically converged in fewer than 10 iterations, outperforming relaxation methods which often required 4x more iterations due to distorted search directions.
  • Optimal Strategies: As illustrated in Figure 2, the optimal solution often requires vehicles to accelerate into the intersection to minimize time in the conflict zone, thereby reducing overall system cost.

Conclusion and Future Work

The experimental results demonstrate that the proposed distributed SQP algorithm is feasible for real-time vehicle coordination. Future work will focus on integrating discrete ordering logic into the framework and implementing "constraint tightening" to provide safety buffers against sensor noise and model mismatches.

Summary updated

Abstract

In this paper we consider the problem of coordinating autonomous vehicles approaching an intersection. We cast the problem in the distributed optimisation framework and propose an algorithm to solve it in real time. We extend previous work on the topic by testing two alternative algorithmic solutions in simulations. Moreover, we test our algorithm in experiments using real cars on a test track. The experimental results demonstrate the applicability and real-time feasibility of the algorithm.
BibTeX
@misc{zanon2017feasibility,
  author = {Mario Zanon and Robert Hult and Sébastien Gros and Paolo Falcone},
  title = {{A Feasibility-Enforcing Primal-Decomposition SQP Algorithm for Optimal Vehicle Coordination}},
  year = {2017},
  eprint = {1704.01081},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/1704.01081},
}
Figure