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.
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.
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