Robust Real-Time Coordination of CAVs: A Distributed Optimization Framework under Uncertainty

TL;DR

This paper introduces a distributed optimization framework for real-time, safe coordination of connected autonomous vehicles under uncertainty, combining robust planning, parallel algorithms, and attention mechanisms.

Contribution

It proposes a novel robust cooperative planning method with adaptive safety constraints, a parallel ADMM-based negotiation algorithm, and an interactive attention mechanism for efficiency.

Findings

Reduced collision rates by up to 40.79% in simulations.

Achieved 15.4% reduction in computational demand.

Demonstrated robustness and real-time feasibility in real-world tests.

Abstract

Achieving both safety guarantees and real-time performance in cooperative vehicle coordination remains a fundamental challenge, particularly in dynamic and uncertain environments. Existing methods often suffer from insufficient uncertainty treatment in safety modeling, which intertwines with the heavy computational burden under complex multi-vehicle coupling. This paper presents a novel coordination framework that resolves this challenge through three key innovations: 1) direct control of vehicles' trajectory distributions during coordination, formulated as a robust cooperative planning problem with adaptive enhanced safety constraints, ensuring a specified level of safety regarding the uncertainty of the interactive trajectory, 2) a fully parallel ADMM-based distributed trajectory negotiation (ADMM-DTN) algorithm that efficiently solves the optimization problem while allowing configurable negotiation rounds to balance solution quality and computational resources, and 3) an interactive attention mechanism that selectively focuses on critical interactive participants to further enhance computational efficiency. Simulation results demonstrate that our framework achieves significant advantages in safety (reducing collision rates by up to 40.79\% in various scenarios) and real-time performance compared to representative benchmarks, while maintaining strong scalability with increasing vehicle numbers. The proposed interactive attention mechanism further reduces the computational demand by 15.4\%. Real-world experiments further validate robustness and real-time feasibility with unexpected dynamic obstacles, demonstrating reliable coordination in complex traffic scenes. The experiment demo could be found at https://youtu.be/4PZwBnCsb6Q.

Tables

Table 1. TABLE I : Simulation Parameters

Parameters	Values
size of intersection area	80 m $\times$ 80 m
lane width	10 m
maximum velocity $v_{\text{max}}$	10 m/s
maximum acceleration $a_{\text{max}}$	$[-5,5]$ m/s2
maximum steering angle ${\delta }_{\text{max}}$	$[-0.78,0.78]$ rad
vehicle geometry $L_{\text{c}}/W_{\text{c}}$	4.2 m/2.1 m
vehicle wheelbase $L_w$	3 m
weighting factor $\eta$	15
variable safety distance $d_{ij}$	[1.1-1.5]
time step $\tau$	0.1 s
horizon length $N$	20
state weight matrix $Q$	$\mathrm{diag}(2,2,1,0)$
input weight matrix $R$	$\mathrm{diag}(1,1)$
initial state covariance ${\hat{P}}_i^0$	$\mathrm{diag}(0.4,0.4,\pi /90,0.2)$
prior estimation error covariance ${\tilde{P}}_i^{0^{-}}$	$\mathrm{diag}(0.03,0.03,\pi /360,0.02)$
terminal state covariance $P_i^f$	$\mathrm{diag}(0.15,0.15,\pi /180,0.1)$
measurement matrix $C$	$I_4$

Table 2. TABLE II : Comparison of Coordination Safety and Efficiency over 100 Monte Carlo Simulations

Metric	STRSv2[23]	CFSDMPC[48]	Proposed
Collisions (/100)	8	12	3
Avg Speed (m/s)	8.33	8.81	9.37

Table 3. TABLE III : Ablation Study on Safety Formulation, ACT: Average Computation Time (s); TPT: Total Passing Time (s); CR: Collision Rate (%)

Setting	ACT (s)$\downarrow$	TPT (s)$\downarrow$	CR (%)$\downarrow$
Uncertainty-aware Formulation	0.0857	14.43	4.5
Nominal-trajectory-only Formulation	0.0346	16.85	8.0

Table 4. TABLE IV : Ablation Study on Parallel Computation and Interactive Attention, Average Computation Time (ms); w/o and w/: Proposed Parallel Method without and with Interactive Attention

NUM CAV	Centralized	Proposed(w/o)	Proposed(w/)
4	231.4	50.9	47.1
8	871.7	71.7	63.9
12	1648.5	83.6	72.6
16	3768.2	93.3	80.2
20	6573.9	101.4	85.7

Table 5. TABLE V : Statistics of the Real-World Experiments, ACT: Average Computation Time (ms); WCT: Worst-case Computation Time (ms); MSD: Minimum Separation Distance (cm); SR: Success Rate (%)

Experiment	ACT (ms)	WCT (ms)	MSD (cm)	SR (%)
Unprotected Left-turn	72.8	95.4	10.2	98.0
Unexpected Obstacle	76.6	104.9	8.8	96.0

Equations

$$\{\begin{array}{ll}(i,j)\in \mathcal{E}(t),& \Vert p_i-p_j\Vert \le d_{\mathrm{cmu}}\\ (i,j)\notin \mathcal{E}(t),& \text{ otherwise },\end{array}$$

$$x_i^{k+1}=f(x_i^k,u_i^k,w_i^k),$$

$$y_i^k=h(x_i^k,v_i^k),$$

$${\mathcal{C}}_{ij}^k:=\left\{x_i^k|\Vert p_i^k-p_j^k{\Vert }_{{\Omega }_{ij}}^2\le d_{\text{s}}\right\},$$

$$\mathrm{Pr}\left(x_i^k\notin {\mathcal{C}}_{ij}^k\right)\ge 1-{\xi }_{\text{coll }}^x,i\in \mathcal{V},j\in {\mathcal{N}}_i,$$

$$J(u)=\mathbb{E}\left[\sum _{i=1}^M\sum _{k=0}^{N-1}{\Vert x_i^k-x_{i,r}^k\Vert }_{Q_i^k}^2+{\Vert u_i^k\Vert }_{R_i^k}^2\right],$$

$${\mathcal{P}}_1:$$

$$x_i^{k+1}=f(x_i^k,u_i^k,w_i^k),k\in {\mathbb{I}}_{0:N-1}$$

$$y_i^k=h(x_i^k,v_i^k),k\in {\mathbb{I}}_{0:N}$$

$$\mathrm{Pr}(x_i^k\notin {\mathcal{C}}_{ij}^k)\ge 1-{\xi }_{\text{coll }}^x,i\in \mathcal{V},j\in {\mathcal{N}}_i$$

$$x_i^N\sim \mathcal{N}({\mu }_i^f,P_i^f)$$

$$u_i^k\in {\mathcal{U}}_i,k\in {\mathbb{I}}_{0:N-1}$$

$$x_i^k\in {\mathcal{X}}_i,k\in {\mathbb{I}}_{0:N}$$

$$\mathrm{Pr}\left(x_i^k\in {\mathcal{C}}_{ij}^k\right)={\int }_{{\Vert p_i^k-p_j^k\Vert }_{{\Omega }_{ij}}\le d_{\text{s}}}\mathbb{P}\left(p_i^k-p_j^k\right)d\left(p_i^k-p_j^k\right),$$

$${\overline{\stackrel{~}{p}}}_i^k={\Omega }_{ij}^{\frac{1}{2}}{p}_i^k,{\tilde{P}}_i^k={\Omega }_{ij}^{\frac{1}{2}\top }{P}_i^k{\Omega }_{ij}^{\frac{1}{2}},$$

$${\bar{\mathcal{C}}}_{ij}^k:=\left\{x\mid {\alpha }_{ij}^{k^{\top }}\left({\tilde{p}}_i^k-{\tilde{p}}_j^k\right)\le d_{\text{s}}\right\},$$

$$\mathrm{Pr}\left(x_i^k\in C_{ij}^k\right)\le \frac{1}{2}+\frac{1}{2}\mathrm{erf}\left(\frac{d_{\text{s}}-{\alpha }_{ij}^{k^{\top }}\left({\overline{\stackrel{~}{p}}}_i^k-{\overline{\stackrel{~}{p}}}_j^k\right)}{\sqrt{2{\alpha }_{ij}^{k^{\top }}({\tilde{P}}_i^k+{\tilde{P}}_j^k){\alpha }_{ij}^k}}\right),$$

$${\alpha }_{ij}^{k^{\top }}{\Omega }_{ij}^{\frac{1}{2}}({\bar{p}}_i^k-$$

$$\cdot \sqrt{2{\alpha }_{ij}^{k^{\top }}{\Omega }_{ij}^{\frac{1}{2}\top }\left(P_i^k+P_j^k\right){\Omega }_{ij}^{\frac{1}{2}}{\alpha }_{ij}^k}.$$

$$\mathbb{E}\left[x_i^N\right]\in {\mathcal{X}}_i^f,$$

$$\mathbb{E}\left[(x_i^N-\mathbb{E}[x_i^N\left]\right)(x_i^N-\mathbb{E}[x_i^N]{)}^{\top }\right]⪯P_i^f,$$

$$x_i^{k+1}=A_i^k{x}_i^k+B_i^k{u}_i^k+r_i^k+G_i^k{w}_i^k,$$

$$y_i^k=C_i^k{x}_i^k+D_i^k{v}_i^k,$$

$$A_i^k={\frac{\partial f}{\partial x}|}_{\left(x_{i,r}^k,u_{i,r}^k,0\right)}$$

$$r_i^k=$$

$$C_i^k=$$

$${\xi }_i^k=y_i^k-\mathbb{E}\left[y_i^k\mid {\mathcal{F}}_i^{k-1}\right]=C_i^k{\tilde{x}}_i^{k^{-}}+D_i^k{v}_i^k,$$

$$P_{{\xi }_i^k}=\mathbb{E}\left({\xi }_i^k{\xi }_i^{k^{\top }}\right)=C_i^k{\tilde{P}}_i^{k^{-}}{C}_i^{k^{\top }}+D_i^k{D}_i^{k^{\top }}.$$

$${\hat{x}}_i^{k+1}=A_i^k{\hat{x}}_i^k+B_i^k{u}_i^k+r_i^k+L_i^{k+1}{\xi }_i^{k+1},$$

$$u_i^k=m_i^k+K_i^k\left({\hat{x}}_i^k-{\bar{x}}_i^k\right),$$

$${\bar{x}}_i^{k+1}=A_i^k{\bar{x}}_i^k+B_i^k{m}_i^k+r_i^k,$$

$${\hat{P}}_i^{k+1}=\left(A_i^k+B_i^k{K}_i^k\right){\hat{P}}_i^k{\left(A_i^k+B_i^k{K}_i^k\right)}^{\top }+L_i^{k+1}{P}_{{\xi }_i^{k+1}}{L}_i^{{k+1}^{\top }}.$$

$$\hat{J}(u)=\mathbb{E}\left[\sum _{i=1}^M\sum _{k=0}^{N-1}{\Vert {\hat{x}}_i^k-x_{i,r}^k\Vert }_{Q_i^k}^2+{\Vert u_i^k\Vert }_{R_i^k}^2\right].$$

$$\hat{J}=\sum _{i=1}^M\sum _{k=0}^{N-1}$$

$$+\mathrm{tr}\left(R_i^k{U}_i^k{\hat{P}}_i^{k^{-1}}{U}_i^{k^{\top }}\right)+m_i^{k^{\top }}{R}_i^k{m}_i^k.$$

Full text

The experiment demo could be found at https://youtu.be/4PZwBnCsb6Q.

I Introduction

Connected and autonomous vehicles (CAVs) represent a transformative solution to critical transportation challenges, offering the potential to significantly reduce traffic accidents and congestion through coordinated decision-making[13, 38]. By integrating advanced sensing, computing, and vehicle-to-everything (V2X) communication capabilities, CAVs can share real-time information and make collaborative decisions that surpass the limitations of individual vehicle operations[47]. This technological convergence enables unprecedented opportunities for system-wide optimization of traffic flow and safety in intelligent transportation systems (ITS)[18, 35].

A fundamental challenge in realizing the full potential of CAVs lies in achieving reliable coordination in critical traffic scenarios, particularly intersections where multiple vehicles must safely negotiate shared space in real time[17]. While existing approaches can generate cooperative trajectories that satisfy nominal safety conditions and traffic rules[42, 29], two critical real-world challenges remain unresolved. First, ensuring robust safety under environmental uncertainties remains technically challenging. In practice, vehicles face various sources of uncertainty and interference[19], including unavoidable disturbances in controllers and actuators, as well as onboard sensor noise that corrupts measurements (e.g., GNSS, radar and inertial measurements)[24, 3]. These uncertainties can accumulate during vehicle movement, leading to potentially dangerous deviations from planned trajectories and increasing collision risks during vehicle interactions[30]. Second, the computational burden of coordination becomes prohibitive as the number of vehicles increases. Real-time trajectory planning for multiple vehicles in spatiotemporal domains requires solving complex optimization problems under tight time constraints, especially challenging with limited onboard computational resources.

These challenges create a fundamental tension between safety and real-time performance in CAV coordination systems. Traditional conservative approaches that prioritize safety often result in overly cautious behavior and reduced traffic efficiency, while faster but less robust methods may compromise safety guarantees. To this end, this paper addresses the tension by developing a novel framework that simultaneously ensures probabilistic safety guarantees and efficient real-time implementation.

I-A Related Works

There are two primary avenues for cooperative trajectory planning design: learning-based and optimization-based approaches[10]. Learning-based approaches, such as reinforcement learning and imitation learning, can capture complicated driving strategies and continuously refine their proficiency using real-world driving data[15, 26]. Specifically, [8, 37] develop an efficient deep reinforcement learning framework for intersection crossing in which CAVs collaboratively learn a driving policy to maximize traffic throughput. However, these data-driven methods lack the theoretical interpretability of neural networks and real-world cooperative driving data, which raises safety concerns. The other approach is optimization-based. This method typically formulates the cooperative planning problem as a mathematical program subject to system constraints arising from vehicle dynamics, collision avoidance, and physical limits, while maximizing certain objectives. Several studies[27, 28, 43, 2, 32] obtain optimal or suboptimal coordination solutions using well-known tools and algorithms from optimization theory such as optimal control[28], dynamic programming[32]. However, most of these studies focus on ideal coordination tasks with accurate state and trajectory information. In uncertain environments, these methods become ineffective due to various sources of uncertainty and interference.

Only a few works aim to develop robust coordination strategies in realistic and complex scenarios. For example, the work [31] obtains the velocity trajectories of CAVs using tube-based model predictive control with robust invariant sets to accommodate vehicle modeling and sensor noises. More recently, [39, 40] characterize vehicle state uncertainties as ellipses and compute an acceleration profile using integer linear programming, which ensures the ellipses do not intersect with each other under the assumption of not accounting for the vehicles’ approaching direction. It is pertinent to note that these studies adopt inflexible maneuvers, such as prohibiting turns and restricting CAVs to predefined fixed paths, limiting the ability to avoid local obstacles. [48] accounts for tracking errors and deviations from nominal trajectories, and proposes a convex feasible set-based replanning coordination approach that iteratively solves a sequence of transformed subproblems within the convex sets. Further, to provide finer-grained safety modeling for collision avoidance under control and localization noise, the authors in[7, 23] employ variable safety redundancy and search non-overlapping spatiotemporal resource blocks for vehicles. However, a major shortcoming in these studies is that they do not directly consider the evolution of trajectory uncertainties. During vehicle movement, trajectories tend toward deviations and divergence due to the accumulation of various uncertainties, which markedly increases the risk of interaction collisions. Moreover, to simplify the complexity of safety conditions under uncertainties, these studies employ simple and fixed safety constraints based on vehicle and collision area size, which often fail to accommodate uncertain scenarios and ensure effective safety.

Furthermore, real-world scenarios involving dynamic multi-directional vehicle flows typically introduce high complexity and non-convexity into the optimization problem, making real-time solutions challenging, especially for larger numbers of CAVs. Nowadays, numerical optimization tools have been well developed[6, 44]. As an emerging technique with scalable and parallel characteristics, the alternating direction method of multipliers (ADMM) can be well-suited to address the cooperative trajectory planning problem for CAVs[36, 45, 9, 16]. The basic idea of ADMM is to decompose the original optimization problem into manageable subproblems and solve them separately and in parallel, thereby alleviating the computational burden arising from the growth in system dimensions. [45, 9] propose a cooperative trajectory planning algorithm based on ADMM to separate the independent constraints from the coupling constraints of the multi-agent system, allowing the former ones to be handled in parallel, thus resulting in a partially parallel solution. Further, leveraging the ADMM, the authors in[16] mathematically formulate a consensus optimization problem over a connected undirected graph and then effectively distribute the computational load across all participating CAVs. However, collision avoidance requirements in distributed frameworks result in a significant increase in the number of coupling constraints among agents. Therefore, these approaches often suffer from substantial computational burdens and conservative coordination performance. To realize real-time implementation, it remains an open issue to further improve computational efficiency through advanced ADMM-based optimization techniques.

In summary, existing coordination methods often suffer from limited uncertainty treatment in safety modeling, where deterministic state assumptions or fixed safety margins cannot reliably capture uncertainty accumulation and interaction risks, and poor scalability and real-time performance under complex multi-vehicle coupling, where dense interactions render the resulting optimization highly non-convex and computationally expensive as the number of CAVs increases. As a result, ensuring reliable safety in dynamic uncertain environments without sacrificing computational efficiency remains a fundamental tension.

I-B Contributions

This paper proposes a robust and real-time coordination framework for cooperative trajectory planning of CAVs that directly addresses the twin challenges of safety under uncertainty and computational efficiency. This work makes the following key contributions:

• •

Novel uncertainty-aware safety formulation: We develop a robust multi-vehicle coordination framework that plans the nominal trajectory while directly controlling trajectory distribution evolution during vehicle interactions. Our formulation incorporates enhanced collision avoidance chance constraints with adaptive safety distances and terminal constraints that are expressed in terms of vehicle state mean and covariance. This formulation provides uncertainty-aware safety guarantees while dynamically adapting to time-varying scenarios.

• •

Computationally efficient distributed algorithm: We develop a fully parallel ADMM-based distributed trajectory negotiation (ADMM-DTN) algorithm that decomposes the complex optimization problem into manageable distributed subproblems. Our algorithm features two key innovations: 1) a negotiation process based on Jacobi updates that guarantees feasible solutions for any number of negotiation rounds, and 2) an interactive attention mechanism that identifies and prioritizes critical vehicle interactions, thereby significantly reducing computational overhead and coupling scale and enabling real-time deployment with varying computational resources.

• •

Comprehensive validation: We demonstrate the practical effectiveness of our framework through extensive simulations and real-world experiments. Our approach achieves up to 29.86% and 40.79% reductions in collision rates compared to benchmark methods while maintaining real-time performance and strong scalability with increasing vehicle numbers. Notably, practical experiments further validate real-time feasibility and robustness in the presence of environmental disturbances and unexpected non-CAV dynamic obstacles, addressing limitations common in existing coordination methods.

The proposed framework bridges the gap between theoretical guarantees and practical implementation in CAV coordination, offering a scalable solution for safe and efficient autonomous driving in complex traffic scenarios.

The paper is organized as follows. Section II presents the intersection model, the vehicle model, and the formulation of the cooperative vehicle coordination problem. Section III presents the enhanced safety constraints formulation and the robust multi-vehicle covariance steering model predictive control reformulation. Then, Section IV presents a fully parallel ADMM-based distributed trajectory negotiation algorithm. Extensive simulations and practical experiments are presented in Sections V and VI to confirm the robustness and effectiveness of the proposed framework. Finally, Section VII presents the conclusion and future work.

Notation: Throughout this article, $\bar{x}$, $\hat{x}$ denote the mean and estimate of vector $x$. $P$, $\hat{P}$ denote the corresponding covariances. The superscript $(\cdot)^{k}$ indicates the value at time step $k$. The subscript $(\cdot)_{i}$ indicates vehicle $i$. $\|x\|$ denotes the Euclidean norm of vector $x$. $\|x\|_{Q}^{2}=x^{T}Qx$ denotes the weighted 2-norm with appropriate dimensions of $x$ and matrix $Q$. $\mathbb{R}^{n}$ and $\mathbb{R}^{m\times n}$ denote the set of $n$-dimensional real vectors and $m$ × $n$ real matrices, respectively. The set of integers $\mathbb{I}_{a:b}$ is defined by $\{a,a+1,...,b\}$. $\succeq$ denotes positive semidefinite. $\mathbb{E}$[·] denotes the expectation operation. $\operatorname{Pr}$[·] indicates the probability of an event and $\mathbb{P}$[·] indicates the probability density function.

II Problem Statement

II-A Intersection Model

We consider a typical unsignalized intersection which consists of $R$ roads with lane width $\mathcal{W}_{\text{lane }}$ (usually $R=4$), as illustrated in Fig. 1. The area around the intersection is called the danger zone. A fleet of vehicles passes through the intersection. CAVs move along flexible paths rather than being restricted to predefined fixed paths to improve road space utilization and flexibly respond to local obstacles.

The uncertainty in coordination stems from motion uncertainty, which arises from factors such as model inaccuracies and external disturbances. Moreover, sensor noises further corrupt the measurements. As a result, only an imprecise approximation of CAVs’ present and future states can be obtained. In previous methods, as illustrated by expanding pink ellipses in Fig. 1(1(a)), the 3$\sigma$ error of vehicle state uncertainty gradually accumulates during vehicle movement. Consequently, vehicle trajectories tend to deviate and disperse from planned trajectories, leading to high interactive collision risks. In our proposed method, as illustrated by the shrinking green ellipses in Fig. 1(1(a)), the state uncertainty is steered, and trajectory distribution evolution is directly controlled, ensuring both convergence and robust interactions of the planned trajectories during vehicle interactions, as shown by the Monte Carlo trajectories in Fig. 1(1(b)). The specific mechanism for mitigating the uncertainties is detailed in Lemma 1.

Moreover, CAVs can share driving information (e.g., position, speed, heading angle, and intention) with others through wireless communication. The information topology of multiple CAVs can be represented by an undirected graph $\mathcal{G}=(\mathcal{V},\mathcal{E})$, where $\mathcal{V}=\left\{1,2,\ldots,M\right\}$ denotes the set of vehicles, and $\mathcal{E}=\left\{1,2,\ldots,m\right\}$ denotes the set of communication links between two interconnected vehicles, which can be defined as

[TABLE]

where $d_{\mathrm{cmu}}$ represents the communication range limit; $p_{i}$, $p_{j}$ are the position vectors of CAV $i$ and $j$. Based on the communication topology, the interactive neighbor set $\mathcal{N}_{i}$ of each CAV can be defined to construct collision avoidance relations. Meanwhile, the neighbor set $\mathcal{N}_{i}$ can be further refined by the interactive attention mechanism (discussed in Section IV-B) to selectively focus on critical interactive vehicles.

II-B Vehicle Model

Each CAV $i\in\mathcal{V}$ is subject to the following discrete-time, stochastic and nonlinear dynamics

[TABLE]

for $k=0,\ldots,N$, where $x_{i}^{k}=[\mathrm{x}_{i}^{k},\mathrm{y}_{i}^{k},{\theta}_{i}^{k},\mathrm{v}_{i}^{k}]^{\top}\in\mathcal{X}_{i}\subset\mathbb{R}^{n_{x}}$ denotes the state of the CAV (position, orientation and velocity) and $u_{i}^{k}\in\mathcal{U}_{i}=[a_{i}^{k},\delta_{i}^{k}]^{\top}\subset\mathbb{R}^{n_{u}}$ is the control input (acceleration and steering angle). $\mathcal{X}_{i}$ and $\mathcal{U}_{i}$ are state space and control space respectively. The increments of the process noise $w_{i}^{k}\in\mathbb{R}^{n_{w}}$ are the i.i.d. standard Gaussian random vectors. In this work, we adopt the nonlinear bicycle kinematic model as the coordination-layer motion model, focusing on safe interactive planning under uncertainty [26, 14]. Residual model mismatch and external disturbances (e.g., unmodeled tire/actuator dynamics, tire–road friction, and environmental disturbances) are captured by stochastic disturbance and noise terms and are handled through the uncertainty-aware formulation presented in Section III. The position of each vehicle in 2D space is extracted by $p_{i}^{k}=\Gamma x_{i}^{k}\in\mathbb{R}^{n_{p}}$, with $\Gamma\in\mathbb{R}^{n_{p}\times n_{x}}$ defined accordingly.

Furthermore, the states of vehicles are observed by the noisy and partial sensing model

[TABLE]

where $y_{i}^{k}\in\mathbb{R}^{n_{y}}$ is the measurement state and $v_{i}^{k}\in\mathbb{R}^{n_{y}}$ is the measurement noise which is the i.i.d. standard Gaussian random vector. The noise processes $w_{i}^{0:N-1}$ and $v_{i}^{0:N-1}$ are independent. We defined the estimated state as $\hat{x}_{i}^{k}=\mathbb{E}\left(x_{i}^{k}\mid\mathscr{F}_{i}^{k}\right)$ and the prior estimated state as $\hat{x}_{i}^{k^{-}}=\mathbb{E}\left(x_{i}^{k}\mid\mathscr{F}_{i}^{k-1}\right)$, and the corresponding estimation errors are denoted as $\tilde{x}_{i}^{k}=x_{i}^{k}-\hat{x}_{i}^{k}$ and $\tilde{x}_{i}^{k^{-}}=x_{i}^{k}-\hat{x}_{i}^{k^{-}}$.

II-C Problem Formulation

We consider a robust vehicle coordination task that cooperatively plans and controls $M$ CAVs through the intersection. Each CAV $i$ enters the intersection with its initial condition $x_{i}^{0}$, which is drawn from a multivariate Gaussian distribution $x_{i}^{0}\sim\mathcal{N}\big(\mu_{i}^{0},P_{i}^{0}\big)$, where $\mu_{i}^{0}\in\mathbb{R}^{n_{x}}$ is the mean and $P_{i}^{0}\succeq 0\in\mathbb{R}^{n_{x}\times n_{x}}$ is the covariance matrix, respectively.

The collision avoidance constraint is considered for safe interaction. In this work, vehicles are represented by rectangles, with $L_{\text{c}}$ and $W_{\text{c}}$ denoting the length and width of vehicles, respectively. Since the safety distances at the front and rear of the vehicle are larger, while the safety distances at the sides are smaller, the collision regions can be appropriately denoted as ellipses. We define the collision-checking region of vehicle $i$ with respect to vehicle $j$ by augmenting its length and width with the longitudinal size of vehicle $j$. This enables collision detection at any relative orientation while maintaining strict and rigorous discrimination. Thus, the collision condition of vehicle $i$ with vehicle $j$ at time $k$ is defined as:

[TABLE]

where $\Omega_{ij}\!=\!R_{o}^{T}\!\operatorname{diag}\!\big(1/\!\left(L_{\text{ci}}/2+\!L_{\text{cj}}/2\right)^{2}\!\!,1/\!\left(W_{\text{ci}}/2+\!L_{\text{cj}}/2\right)^{2}\!\big)\allowbreak R_{o}$ and $d_{\text{s}}\in\mathbb{R}$ is normalized safety distance between the vehicle $i$ and vehicle $j$, typically equal 1. It is automatically tuned via $l_{1}$-regularization in Section III-A. Given that the states of the vehicles are random variables, we employ the collision avoidance chance constraints instead of hard constraints. Thus, based on the collision condition, the probability of the collision-free state is enforced to exceed a pre-specified threshold, i.e.,

[TABLE]

where $\xi_{\text{coll }}^{x}\in(0,0.5)$ is the maximum allowed probability of inter-vehicle collision. To enhance robustness to various uncertainties, we aim to drive the vehicles’ final state distributions to target distributions $x_{i}^{f}\sim\mathcal{N}\big(\mu_{i}^{f},P_{i}^{f}\big)$, where $P_{i}^{f}\succ 0$.

Let $u=\big\{(u_{i}^{k})_{k=0}^{N-1},...,(u_{M}^{k})_{k=0}^{N-1}\big\}$ denote the control sequences of all vehicles, the cost function is given by

[TABLE]

where $N$ is short horizon, $Q_{i}^{k}\succeq 0$, $R_{i}^{k}\succ 0$ are the weighting matrices, and $x_{i,r}^{k}$ is the desired state. The first term of the cost function penalizes the deviation of the vehicle state $x_{i}^{k}$ from the corresponding desired state $x_{i,r}^{k}$, and the second term penalizes the magnitude of the control input $u_{i}^{k}$. Specifically, the terminal cost is explicitly replaced by constraints on the mean and covariance of the target state. Here, the desired state vector $x_{i,r}^{k}$ is sampled at the maximum speed. Thus, the efficiency of the coordination will be ensured.

As a result, at any time instant $t$, the centralized multi-vehicle coordination task can be formulated as the finite horizon stochastic optimization problem as follows

[TABLE]

Similar to[46, 49], since the prediction horizon is short and the control input at time $k$ is an affine function of the measurement data, it follows that the states will be Gaussian distribution over the horizon. The challenges of solving the problem $\mathscr{P}_{1}$ in dynamic and uncertain environments are twofold. The first challenge is to provide probabilistic guarantees for the interactions of stochastic state trajectories while ensuring highly robust coordination. Second, multi-vehicle coordination problem is computationally hard to solve, as the computational complexity grows drastically with the number of vehicles, states, and time steps.

In the following sections, we present a safe high-performance and computationally efficient multi-vehicle coordination framework.

III Uncertainty-Aware Coordination Safety Formulation

In this section, to address the key coordination safety challenges, we reformulate the original stochastic, nonlinear, and nonconvex coordination problem into a robust cooperative planning and control problem. First, we address the intractable safety constraints, including collision avoidance chance constraints and terminal constraints, which comprehensively account for uncertainty levels and time-varying scenarios, and develop an adaptive and robust collision avoidance ability. Then we reformulate the coordination problem into a multi-vehicle covariance steering model predictive control problem that quantify and steer the first and second moments of the vehicle state to ensure robust interactions among stochastic state trajectories.

III-A Enhanced Safety Constraints Formulation

First, we consider the inter-vehicle collision avoidance chance constraint. The position and uncertainty covariances of the two vehicles are extracted from the state variables and denoted by $p_{i}^{k}\sim\mathcal{N}\left(\bar{p}_{i}^{k},P_{i}^{k}\right)$, $p_{j}^{k}\sim\mathcal{N}\left(\bar{p}_{j}^{k},P_{j}^{k}\right)$, which follow multivariate Gaussian distributions. Then $p_{i}^{k}-p_{j}^{k}$ is also a multivariate Gaussian distribution, i.e. $p_{i}^{k}-p_{j}^{k}\sim\mathcal{N}\left(\bar{p}_{i}^{k}-\bar{p}_{j}^{k},P_{i}^{k}+P_{j}^{k}\right)$. Therefore, the instantaneous collision probability of inter-vehicle can be expressed as a double integral of a multivariate Gaussian probability density function

[TABLE]

where the integral region $\Omega_{ij}$, i.e., the collision region $\mathcal{C}_{ij}^{k}$ is an ellipse, as illustrated in Fig. 2(2(a)). However, there is no analytical form to express the collision probability. To obtain a tight upper bound, we first convert the collision region $\mathcal{C}_{ij}^{k}$ into a circle $\tilde{\mathcal{C}}_{ij}^{k}$ by employing an affine coordinate transformation $\tilde{x}=\Omega_{ij}^{\frac{1}{2}}x$, as shown in Fig. 2(2(b)). Thus the vehicle positions are transformed into new Gaussian distribution, i.e. $\tilde{p}_{i}^{k}\sim\mathcal{N}\big(\bar{\tilde{p}}_{i}^{k},\tilde{P}_{i}^{k}\big),\tilde{p}_{j}^{k}\sim\mathcal{N}\big(\bar{\tilde{p}}_{j}^{k},\tilde{P}_{j}^{k}\big)$, where

[TABLE]

In the new coordinate system, the collision probability (8) is equal to the integral of $\tilde{p}_{i}^{k}-\tilde{p}_{j}^{k}$ over the circle $\tilde{\mathcal{C}}_{ij}^{k}$. Second, we approximate the circular collision region $\tilde{\mathcal{C}}_{ij}^{k}$ with a half space $\bar{\mathcal{C}}_{ij}^{k}$, which is denoted as

[TABLE]

where $\alpha_{ij}^{k}=\left(\bar{\tilde{p}}_{i}^{k}-\bar{\tilde{p}}_{j}^{k}\right)/\left\|\bar{\tilde{p}}_{i}^{k}-\bar{\tilde{p}}_{j}^{k}\right\|$. By linearizing the collision condition, it is evident that $\tilde{\mathcal{C}}_{ij}^{k}\subset\bar{\mathcal{C}}_{ij}^{k}$, thus $\operatorname{Pr}\left(x_{i}^{k}\!\in\mathcal{C}_{ij}^{k}\right)=\operatorname{Pr}\big(x_{i}^{k}\!\in\tilde{\mathcal{C}}_{ij}^{k}\big)\leq\operatorname{Pr}\left(x_{i}^{k}\!\in\bar{\mathcal{C}}_{ij}^{k}\right)$. Using the approach outlined in[5], the collision probability upper bound between two vehicles is obtained:

[TABLE]

where $\operatorname{erf}(x)$ is the standard error function, which is a monotonically increasing function. Given $\xi_{\text{coll }}^{x}\in(0,0.5)$, thus, the probabilistic collision avoidance constraint (5) can be reformulated as a deterministic constraint,

[TABLE]

In practice, a static safety distance $d_{\text{s}}$ is inadequate for effective collision avoidance and struggles in highly dynamic intersection environments. To achieve time-varying collision avoidance ability, $d_{\text{s}}$ need to be adjusted flexibly. An intuitive mechanism would generate a larger $d_{\text{s}}$ for the upcoming time steps and a smaller $d_{\text{s}}$ for the farther time steps. Consequently, a dynamic safety distance approach is proposed, where a variable distance vector $d_{i}=(d_{i}^{1},d_{i}^{2},...,d_{i}^{N})$ replaces $d_{\text{s}}$. Each $d_{i}^{k}$ varies between the bounds $[d_{\text{min}},d_{\text{max}}]$. However, this configuration slips the different importance of the upcoming and farther states, resulting in $d_{i}^{k}=d_{\text{min}}$ for all $k$. We wish to automatically allocate different attention to different states. To this end, the sparsity is imposed on $d_{i}$ to generate uneven values across different time steps. Mathematically, $l_{1}$-regularization is implemented by adding a penalty function of $d_{i}$ to the cost $J$. Let $d=\big\{(d_{i}^{k})_{k=0}^{N},...,(d_{M}^{k})_{k=0}^{N}\big\}$ denote the dynamic distance of all vehicles. This penalty is defined as the negative $l_{1}$ norm of $d_{i}$, i.e., $\tilde{J}(d)=-\eta\sum_{i=1}^{M}\|d_{i}\|_{1}=-\eta\sum_{i=1}^{M}\sum_{k=0}^{N}d_{i}^{k}$, where $\eta\in\mathbb{R}$ is a factor to adjust the collision avoidance ability.

Next, we relax the terminal constraint (7e) to the pair of convex constraints

[TABLE]

[TABLE]

where $\mathcal{X}_{i}^{f}\subseteq\mathcal{X}_{i}\subset\mathbb{R}^{n_{x}}$ is compact and convex, and $P_{i}^{f}\in\mathbb{R}^{n_{x}\times n_{x}}$.

III-B Multivehicle Covariance Steering Model Predictive Control

For all vehicles, the trajectories are coordinated in a receding horizon fashion. Using the approach outlined in[46], the nonlinear system (2)-(3) can be effectively approximated by linearization. Given a nominal trajectory generated from the previous iteration, successive linearization is employed to approach the nonlinear model more closely. Thus, the stochastic and linear time-varying dynamics is constructed as

[TABLE]

[TABLE]

where $A_{i}^{k}\in\mathbb{R}^{n_{x}\times n_{x}}$, $B_{i}^{k}\in\mathbb{R}^{n_{x}\times n_{u}}$, $G_{i}^{k}\in\mathbb{R}^{n_{x}\times n_{w}}$ are the system matrices, and $C_{i}^{k}\in\mathbb{R}^{n_{y}\times n_{x}}$, $D_{i}^{k}\in\mathbb{R}^{n_{y}\times n_{y}}$ are the measurement matrices, and $r_{i}^{k}\in\mathbb{R}^{n_{x}}$ is the residual term computed from

[TABLE]

Since the true states of the CAVs are not available, the Kalman filter is used to estimate the states. By defining the innovation process $(\xi_{i}^{k})_{k=0}^{N}$ as

[TABLE]

where $\mathbb{E}\left(y_{i}^{k}\mid\mathscr{F}_{i}^{k-1}\right)=\mathbb{E}\left(C_{i}^{k}x_{i}^{k}+D_{i}^{k}v_{i}^{k}\mid\mathscr{F}_{i}^{k-1}\right)=C_{i}^{k}\hat{x}_{i}^{k^{-}}$. And $\xi_{i}^{k}$ is distributed as $\xi_{i}^{k}\sim\mathcal{N}\left(0,P_{\tilde{y}_{k^{-}}}\right)$. The innovation process at different time steps is uncorrelated as shown in[1]. Hence, the covariance of each $\xi_{i}^{k}$ at time $k$ is

[TABLE]

By substituting the innovation process (18) and the Kalman priori filtered state into the posterior update, we obtain the estimated state dynamics of each CAV

[TABLE]

where $L_{i}^{k+1}$ is the Kalman gain. The state and measurement process (2) and (3) are fused and converted into a corresponding estimated state process (20) with noise term $L_{i}^{k+1}\xi_{i}^{k+1}$. The coordination optimization problem can be posed in terms of the accessible estimated state.

To address the problem $\mathscr{P}_{1}$ robustly, we consider the following affine state feedback control policies

[TABLE]

where $m_{i}^{k}\in\mathbb{R}^{n_{u}}$ is the feed-forward term that interprets as planning the mean of the filtered state, and $K_{i}^{k}\in\mathbb{R}^{n_{u}\times n_{y}}$ is the feedback gain that controls the system uncertainty. In each horizon, the first control command $u_{i}^{0}$ is executed.

In this work, we choose the intermediate states of the planning horizon as decision variables, handling them in terms of their first and second moments. To this end, by substituting the control policy (21) into the estimated state dynamic (20), then taking the expectation and computing the variance, we can obtain the mean and covariance dynamics of the vehicle state as follows

[TABLE]

[TABLE]

Substituting $x_{i}^{k}=\hat{x}_{i}^{k}+\tilde{x}_{i}^{k}$ and using properties of conditional expectation, the state penalty of the cost function can be expressed in terms of the estimated state and the estimation error covariance. It is shown that the estimation error covariance is determined solely by the measurement model and thus is deterministic[34]. Thus, this term can be discarded without changing the problem. Then, we rewrite the cost function in terms of the estimated state as

[TABLE]

Similarly, the cost function can be further written in terms of the first two moments of the estimated state. Based on this controller structure and employing the transformation $U_{i}^{k}\triangleq K_{i}^{k}P_{i}^{k}$, the cost function (24) can be converted into

[TABLE]

Applying the same transformation process, the covariance dynamics (23) becomes

[TABLE]

The problem remains non-convex due to the nonlinear term $U_{i}^{k}\hat{P}_{i}^{k^{-1}}\!U_{i}^{k^{\top}}$ appearing in the cost and covariance dynamics. To address this issue, we introduce the following relaxation

[TABLE]

the cost function (25) and constraint (26) can be rewritten as

[TABLE]

[TABLE]

This convex relaxation turns out to be lossless, see[25] for details, where (27) can be further written as the linear matrix inequality (LMI) using the Schur complement.

[TABLE]

Using $P_{i}^{k}=\hat{P}_{i}^{k}+\tilde{P}_{i}^{k}$, the inter-vehicle collision avoidance constraint (12) is rewritten in terms of the estimated state.

Furthermore, within each optimization horizon, the cost function (28) is minimized with respect to the initial state conditions of the vehicles ($\bar{x}_{i}^{k},P_{i}^{k}$). The initial state involves selecting either the current measurement state or the predicted state. We employ a dual initialization strategy similar to[19], which determines the initial conditions for each optimization horizon based on the optimality of the cost function. Specifically, the measurement state is used if the problem is feasible and leads to a lower cost than the problem with predicted state. Otherwise, the predicted state is selected.

In a nutshell, the robust cooperative planning and control problem is reformulated as follows

[TABLE]

where $E_{i}^{k}$ and $F_{i}^{k}$ denote mean and covariance dynamics (22), (29) respectively. $H_{i}^{k}$ denotes the relaxation constraint (30). $I_{ij}^{k}$ denotes the inter-vehicle collision avoidance constraint (12). At each time instant $t$, the reformulated coordination problem $\mathscr{P}_{2}$ aims to find the decision variables $\bar{x}_{i},m_{i},P_{i},U_{i},Y_{i}$ for all vehicles. Then the control policies can be recovered from the computed variables. To better explain the uncertainty-mitigation mechanism of the proposed framework, we next present a conditional analytical result on covariance attenuation and bounded uncertainty evolution.

Lemma 1: Consider uncertainty-aware coordination under the affine feedback policy (21) and the corresponding mean/covariance propagation (22), (23). Given that the closed-loop covariance propagation is $\kappa$-contractive in the positive semidefinite order through the computed feedback gain $K_{i}^{k}$, i.e., $\left(A_{i}^{k}+B_{i}^{k}K_{i}^{k}\right)\hat{P}_{i}^{k}\left(A_{i}^{k}+B_{i}^{k}K_{i}^{k}\right)^{\top}\preceq\kappa^{2}\hat{P}_{i}^{k}$. Then, if the initial covariance satisfies $\hat{P}_{i}^{0}\succeq P_{\infty}$, then the covariance will decrease monotonically to the steady-state bound $P_{\infty}$:

[TABLE]

Proof. See Appendix B.

IV Parallel Trajectory Negotiation

To address key real-time computational challenges, we propose a completely parallel computation framework to efficiently solve the problem $\mathscr{P}_{2}$. We first present the ADMM-DTN algorithm to decompose the optimization problem into two manageable subproblems, which are solved in parallel across vehicles through trajectory negotiation. Then, we incorporate an interactive attention mechanism that selects the most critical interactive vehicles to further reduce computational burden. For the sake of clarity, we omit the time step index $k$.

IV-A Parallel Computation Via ADMM-DTN

The collision avoidance constraints remain intractable as the variables $\bar{x}$ and $P$ are coupled within the square root. To address this issue and enable parallel computation, firstly, we decompose the problem $\mathscr{P}_{2}$ into two tractable subproblems associated with different sets of variables by leveraging the ADMM. In particular, the augmented Lagrangian function of problem $\mathscr{P}_{2}$ can be formulated as

[TABLE]

where $\rho$ is the penalty parameter, and $\lambda_{ij}$ is the dual variable corresponding to equality constraint $I_{ij}\big(\bar{x}_{i},\hat{P}_{i},\bar{x}_{j},\hat{P}_{j},d_{i},\Delta_{ij}\big)=0$. The equality constraint is transformed from the constraint (31e), where the nonlinear function $I_{ij}\big(\bar{x}_{i},\hat{P}_{i},\bar{x}_{j},\hat{P}_{j},d_{i},\Delta_{ij}\big)$ is given by

[TABLE]

where $\Delta_{ij}$ is the slack variable. In (IV-A), the primal variables are divided into two groups: 1) $\{\bar{x},m,d\}$, which are associated with the mean dynamic; 2) $\{P,U,Y,\Delta\}$, which are related to the covariance dynamic. And updates are made to the dual variables $\lambda_{ij}$, which correspond to different inter-vehicle collision avoidance constraints. As a result, the ADMM algorithm for minimizing the augmented Lagrangian can be denoted by

[TABLE]

Subproblems (35a) and (35b) are convex, and (35c) is the subgradient for updating the dual variables. The termination criteria are given in terms of the primal residual and dual residual, i.e.,

[TABLE]

where constants $\epsilon^{\mathrm{pri}}\geq 0$ and $\epsilon^{\text{dual }}\geq 0$ are the threshold for the termination criteria.

Secondly, we present a distributed trajectory negotiation (DTN) algorithm based on Jacobi updates to achieve efficient parallel computation. Fig. 3 shows the distributed computation and negotiation architecture, encompassing inter-vehicle communication with on-board planning and control. The vehicles are interconnected and share the resulting information with neighboring vehicles. Based on the shared information, each vehicle simultaneously performs trajectory planning and control and updates its plan, after which the updated trajectory is broadcast again for the next negotiation round. Thus, the two subproblems (35a), (35b) can be solved in parallel for each vehicle. The local optimization decision of each vehicle is denoted as $s_{i}=(\bar{x}_{i}^{k},m_{i}^{k},\hat{P}_{i}^{k},U_{i}^{k},Y_{i}^{k},d_{i}^{k},\Delta_{ij})$, which includes the variables related to the $i$-th vehicle subsystem. This decoupled decomposition is practical and preserves the privacy of vehicle models[11, 20]. Each vehicle determines the local trajectory and the control input by computing $s_{i}$. To simplify notation, we state the local optimization problems as

[TABLE]

where $J_{i}\!\left(s_{i}\right)$ corresponds to the cost $J_{i}(\bar{x}_{i},m_{i},P_{i},U_{i},Y_{i})$ and $\tilde{J}_{i}(d_{i})$, and $\mathcal{A}_{ij}^{d}s_{i}+\mathcal{B}_{ij}^{d}s_{j}-b_{ij}^{d}$ corresponds to the coupling collision avoidance constraint. For the mean subproblem (35a), constraints (31b), (31e)-(31h) are collected in (37b), while for the covariance subproblem (35b), constraints (31c)-(31e) and (31i) are collected in (37b).

The procedure of the DTN is presented in Algorithm 1, to improve the quality of the planned trajectory and mitigate the conservatism of parallel computation, the DTN algorithm negotiates among vehicles to reach a solution, where each vehicle $i\in\mathcal{V}$ perform the optimization computation and negotiation update in lines 3 and 5 iteratively. For each individual vehicle, optimized trajectory information is modified through a local Jacobi update step (39) and shared with interactive neighbor vehicles through communication. The process can be interrupted after each negotiation while guaranteeing a feasible solution. Where $\dot{s}_{i}$ is an initial candidate at the start of negotiation procedure, $s^{(p)}_{i}$ is the feasible output of the $p$-th negotiation, and $s^{*}_{i}$ is the optimal solution of the local optimization problem. And the update variable $\omega_{i}$ sets the degree of over-relaxation. The negotiation process stops when either it reaches the maximum negotiation number $p_{\max}$ or the result $s_{i}$ converges to a specific stopping boundary $\gamma$.

The DTN algorithm has two properties that ensure the effectiveness of parallel computation. First, the output $s^{(p)}_{i}$ from each negotiation is feasible for local problems (37), meaning that the negotiation process can be interrupted after any round, making it suitable for real-world deployment. This is because, beginning with a feasible initial candidate, we select any vehicle $i$ and its any interacting neighbor vehicle $j\in\mathcal{N}_{i}$. The first negotiation is reorganized as (38). Two compact solution vectors $\binom{s_{i}^{*}}{s_{j}^{(0)}}$ and $\binom{s_{i}^{(0)}}{s_{j}^{*}}$ are feasible for local problems of vehicles $i$ and $j$. Since the constraints and cost of the problems are convex, the convex combination (39) with $\omega_{i}+\omega_{j}=1$ yields a new feasible solution $\binom{s_{i}^{(1)}}{s_{j}^{(1)}}$ for the same problems. The same result holds for $p>1$ by induction. Second, the cost over consecutive negotiation updates is non-increasing and converges as $p\rightarrow\infty$, thereby improving the solution quality. This is because, using the convexity of the cost and the fact that $s_{i}^{*}$ is the optimal solution of the $p$-th negotiation, we have $V_{i}\big(s_{i}^{(p+1)}\big)=V_{i}\big(\omega_{i}s_{i}^{*}+\left(1-\omega_{i}\right)s_{i}^{(p)}\big)\leq\omega_{i}V_{i}\big(s_{i}^{(p)}\big)+\left(1-\omega_{i}\right)V_{i}\big(s_{i}^{(p)}\big)=V_{i}\big(s_{i}^{(p)}\big)$. Thus, the cost sequence is bounded below and monotonic, ensuring convergence as $p\rightarrow\infty$.

[TABLE]

IV-B Interactive Attention Mechanism

In scenarios involving a large number of vehicles, vehicles are mostly influenced by their nearby vehicles rather than all others. Thus, it is essential for vehicles to focus on the most interactive vehicles that could potentially cause conflicts. Inspired by[22], we present an interactive attention mechanism based on a self-attention architecture, which selects the most interactive vehicles to reduce coupling constraints and further enhance computational efficiency.

The interactive attention mechanism is illustrated in Fig. 4 and works as follows. The collected state information is embedded into the feature vectors $\mathcal{X}=\left(\mathcal{F},\mathcal{O}\right)\in\mathbb{R}^{M\times d_{x}}$ through the linear layer, which contains the ego features $\mathcal{F}$ and other CAVs’ features $\mathcal{O}$. The weights are shared among all vehicles. The difference is that driving intentions are also included in the feature information, facilitating faster learning of interactive relationships. Then the feature vectors $\mathcal{X}$ are fed into a multi-head attention layer and embedded into the query $\mathbf{Q}$, keys $\mathbf{K}$, values $\mathbf{V}$ through multilayer perceptrons (MLPs).

[TABLE]

where $W^{Q}$, $W^{K}$, and $W^{V}\in\mathbb{R}^{d_{x}\times d_{k}}$. Unlike the attention layer in the Transformer model, this module produces only the query results (i.e., attention weights) which represent the degree of attention to surrounding vehicles.

To select interactive vehicles based on the surroundings, the vehicle first emits a single query $\mathbf{Q}=\left[q^{0}\right]\in\mathbb{R}^{1\times d_{k}}$, which is then compared to all of keys $\mathbf{K}=\left[k^{1},\cdots,k^{M}\right]\in\mathbb{R}^{M\times d_{k}}$, including the descriptive features of each vehicle. The similarity between the query $q^{0}$ and any vehicle’s key $k^{i}$ is calculated using the dot product. These similarities are scaled by the inverse-square-root-dimension $\frac{1}{\sqrt{d_{k}}}$ and normalized with a softmax function $\sigma$ to obtain attention weights. Then, these weights gather all of the values $\mathbf{V}=\left[v^{1},\cdots,v^{M}\right]\in\mathbb{R}^{M\times d_{k}}$, where each value $v^{i}$ is a feature computed through a shared linear projection. The attention computation for each head is expressed as

[TABLE]

Finally, we combine the attention weights from all heads to represent the vehicle’s attention to surrounding vehicles.

IV-C Proposed Algorithm

In outline, the proposed robust and real-time vehicle coordination framework is summarized in Algorithm 2. Specifically, the relative states of each vehicle and its interacting collision-avoidance vehicles do not change markedly between consecutive time steps. Warm starting can be leveraged to significantly reduce the number of iterations in ADMM. That is, the current solution can be used as the initial guess for the primal and dual variables in the next iteration procedure.

V Simulation Results

V-A Simulation Settings

We implement extensive simulations to validate the performance of the proposed coordination framework, with all experiments conducted on a desktop computer with Intel i7-9700F 3.0 GHz CPU and 16 GB of RAM. The considered intersection scenario is illustrated in Fig. 1. The road geometry is set as[27]. The vehicle dynamics is discretized by 2nd-order Runge-Kutta integration, see the Appendix A for derivation details. The probability threshold is set as $\xi_{\text{coll}}^{x}$ = 0.1 to provide CAVs a suitable safety level. The disturbance matrix is set as $G=\mathrm{diag}(0.08,0.08,\pi/180,0.1)$ based on the high-fidelity simulated driving data of the vehicle from[19]. To imitate realistic scenarios, measurement noises are added to the recorded state, with the noise matrix set as $D=\mathrm{diag}(0.35,0.35,\pi/150,0.2)$ based on the level of localization error in real-world experiments[24, 3]. The penalty parameter $\rho$ is set as 100. The thresholds of termination criteria $\epsilon^{\mathrm{pri}}$ and $\epsilon^{\text{dual }}$ are set to 0.1. Given that the output of each negotiation is feasible, we set $p_{\text{max}}=4$ for subproblem (35a) and $p_{\text{max}}=2$ for subproblem (35b) to trade off computational efficiency and overall coordination performance. The update variable $w_{i}$ is set as 0.5. The attention mechanism is deployed following the version specified by[22], where the MLPs consist of two linear layers and the layer size of 64 × 64. The attention layer contains two heads, and the feature size is 32. The maximum number of interactive vehicles for each vehicle is set to 5. The optimization frameworks CasADi and YALMIP, with the solvers IPOPT and MOSEK, respectively, are used to solve subproblems (35a) and (35b). The detailed parameters are summarized in Table I.

V-B Performance of the Proposed Framework

In this subsection, we conducted extensive comparative experiments, ranging from small-scale to large-scale, to validate the performance of the proposed coordination framework. We compare the proposed method with representative benchmarks in terms of coordination trajectory, safety, efficiency, and computational overhead. The benchmarks cover two complementary coordination paradigms closely related to this study. The accelerated space-time resource searching (STRSv2) approach[23] represents a resource-block-based coordination paradigm. It emphasizes the efficient allocation of non-overlapping feasible tunnel and vehicle-level safety planning, retaining flexible adaptation to environmental uncertainty. The convex feasible set-based distributed MPC (CFSDMPC) approach[48] represents a distributed optimization-based coordination paradigm. It accounts for trajectory deviation and tracking error and performs real-time multi-vehicle coordination by iteratively solving convexified subproblems.

1. Trajectory Uncertainty and Safety Analysis:

we first evaluate each method in an open-loop planning setting by performing 100 Monte Carlo simulations of intersection crossing. In this setting, two vehicles enter the intersection with initial state distributions of the same level, given by $\mathrm{diag}(0.4,0.4,0,0.2)$, $\mathrm{diag}(0.4,0.4,\pi/90,0.2)$, respectively. The resulting planned control sequence is executed forward without replanning. Fig. 5 presents the output trajectories of the three methods. It is evident that our approach achieves a significant reduction in the uncertainty of the future trajectory, thereby enhancing interaction safety. In contrast, the STRSv2 and CFSDMPC methods plan only nominal trajectories without directly controlling trajectory uncertainty, resulting in consistently large uncertainty throughout the entire interaction process.

Open-loop planning provides a transparent view of uncertainty evolution, while closed-loop replanning reflects operational behavior during real-time execution. Thus, we further evaluate a closed-loop replanning setting in a challenging unprotected left-turn scenario, where four left-turning vehicles enter the intersection. We test the robustness of the proposed method through 100 Monte Carlo simulations of intersection crossing. Fig. 6 illustrates the output trajectories of the three methods, while Table II records the number of collisions and the average speeds. It is obvious that our proposed method maintains smoother and more consistent trajectories than the STRSv2 and CFSDMPC methods while showing the smallest trajectory spread. This could lead to fewer potential collisions. According to Table II, the proposed method has the fewest collisions, recording 3 collisions, compared to 8 and 12 for STRSv2 and CFSDMPC methods, respectively. Additionally, our method achieves a higher average speed than the other two approaches. Our method demonstrates greater robustness to the motion uncertainties and sensory measurement noise. This robustness is attributed to our method’s explicit consideration and active steering of the trajectory distributions during vehicle coordination, thereby providing a high level of safety regarding the uncertainty of the planned trajectory.

2. Speed Profile Analysis:

Next, Fig. 7 presents the ST speed profile of the three methods in the Frenet coordinate system for one test case. The gray rectangular represents the conflict area in the center of the intersection. To facilitate a clear comparison, the four subfigures display the speed profile for the corresponding four vehicles. For vehicle 1, Fig. 7(7(a)) shows that the speed profiles of the three methods exhibit minor differences. The STRSv2 method demonstrates a slightly higher speed than the CFSDMPC and our method, as the sequential allocation of resource blocks in STRSv2 allows vehicle 1 to travel at maximum speed without accounting for other vehicles. For vehicles 2, 3, and 4, as shown in Fig. 7(7(b))-(7(d)), the speed profiles of our approach show clear advantages over the CFSDMPC and STRSv2 methods, especially in the case of vehicle 4. In the STRSv2 method, vehicle passage times gradually increase, and in the CFSDMPC method, vehicle speeds visibly decrease when entering the conflict area. In contrast, the speed profiles of the four vehicles in our method rise more rapidly and show minimal differences, indicating that all four vehicles found an efficient passage solution.

3. Collision Rate: We evaluated the crossing safety of different methods by scaling up the scenario with more vehicles. The actual performance of the compared methods cannot be fully demonstrated under a fixed vehicle flow. Therefore, we introduced two kinds of vehicle flow to reveal their intersection-crossing capabilities under varying vehicle patterns. Following the vehicle patterns established in [23], vehicle flow 1 consists of 50% straight-going vehicles, 30% left-turning vehicles, and 20% right-turning vehicles. Vehicle flow 2 consists of 40% left-turning vehicles, with straight-going and right-turning vehicles each accounting for 30%. Since our scenarios involve different vehicle patterns as well as random noise and disturbances, we conducted 200 Monte Carlo simulations for each scenario to mitigate potential biases and obtain collision rate statistics. Collision detection is performed using the Separating Axis Theorem (SAT) [12], which efficiently checks the inter-vehicle overlap.

The collision rate of the three methods is presented in Fig. 8. The three methods achieve similar collision rates with a small number of vehicles. However, our method exhibits increasingly pronounced safety advantages as the number of vehicles increases. In the scenario with 30 vehicles, it reduces collision rates by 29.86% and 40.79% compared to the STRSv2 and CFSDMPC methods, respectively. CFSDMPC method has the highest collision rate, as it does not explicitly account for vehicle dynamics and related uncertainties, making the execution trajectories prone to deviation. STRSv2 method avoids conflicts by preventing overlaps of space-time resource blocks, however, its safety margins fail to precisely accommodate error accumulation, leading to a relatively high collision risk. The safety advantages of our method stem from precise characterization of uncertainty evolution throughout the entire coordination process. This enables the planning of the nominal trajectory while directly controlling the evolution of the trajectory distribution, thereby ensuring robust interactions among stochastic state trajectories. Additionally, the dynamic safety distance is adjusted based on the surrounding vehicle density. In other words, CAVs comprehensively account for their uncertainties and the surrounding environment to develop time-varying collision avoidance abilities, thereby ensuring robust safety guarantees.

4. Total Passing Time: Based on the same vehicle flows, Fig. 9 shows the total crossing time of the three methods with respect to the number of vehicles. It is evident that the total passing time of the STRSv2 method is significantly higher than that of CFSDMPC and our method. This is because STRSv2 method conducts sequential optimization for vehicles to allocate non-overlapping resource blocks, which results in suboptimal trajectories and reduced driving efficiency. In both CFSDMPC and our method, there is no decision delay between vehicles. However, our method achieves a shorter passing time compared to the CFSDMPC method. The CFSDMPC method requires more time and distance to converge to the reference trajectory due to the tracking deviation, and its distributed strategy can be considered a single-round negotiation process. In contrast, our method introduces a negotiation mechanism, which can improve the optimality of the solution through multiple negotiations.

5. Computation Time: Fig. 10 presents the computation time of the various methods under different numbers of vehicles. It is evident that our method and the CFSCMPC method have significantly shorter computation time compared to the STRSv2 method, due to time-consuming resource block overlap verification and allocation process in STRSv2. The CFSDMPC method, which approximates the problem to a sequence of quadratic programming, achieves a satisfactory computation time. The proposed method demonstrates comparable computation efficiency while exhibiting better scalability than the CFSDMPC method. Notably, when the number of vehicles exceeds 14, the computation time of our method is shorter than the CFSDMPC method. This is because, as the number of vehicles increases, the CFSDMPC method requires more processing time to compute the convex feasible set in each iteration, leading to a faster increase in overall computation time. However, our non-convex collision constraint reformulation avoids iterative computations of feasible sets. Furthermore, our method leverages fully parallel computation and an efficient interactive attention mechanism, thus achieving superior computational efficiency and scalability.

6. Ablation on the safety formulation: To evaluate the contribution of the proposed uncertainty-aware safety design, we compare the full uncertainty-aware formulation with a nominal-trajectory-only formulation while fixing the number of vehicles at 20. All other experimental and parameter settings remain unchanged. As shown in Table III, the uncertainty-aware formulation reduces the collision rate from 8.0% to 4.5%, corresponding to a relative reduction of 43.8%, and also shortens the total passing time from 16.85 s to 14.43 s. This improvement stems from the more refined adaptive collision-avoidance constraints and the explicit control of trajectory distribution evolution during vehicle interactions, which allow the method to select relatively aggressive trajectories while still ensuring interaction safety in dense traffic. Although the uncertainty-aware formulation incurs higher average computation time than the nominal-only variant, the additional cost remains moderate and is justified by the clear gains in safety and passing efficiency.

7. Ablation of parallel computation and interactive attention: The proposed parallel computation framework is compared with the corresponding centralized implementation. Table IV presents the average computation time under different numbers of vehicles. Note that the collision rate and total passing time are not listed, as the three configurations yielded similar coordination performance. It can be seen that our method is an order of magnitude faster than the centralized approach and scales better with the number of vehicles. In our framework, convergence is generally achieved within 1-2 ADMM iterations, with single-iteration convergence occurring in approximately 80% of cases. Furthermore, the last two columns of Table IV provide an ablation of the interactive attention mechanism through the computation-time comparison with and without attention. The results show that interactive attention reduces computational demand by around 7.4–15.4%, with the benefit becoming more pronounced as the number of vehicles increases. In brief, by prioritizing the most critical neighboring vehicles and reducing coupling constraints, the proposed framework enables CAVs with interactive attention to solve local problems in parallel more efficiently, thereby further improving online computational performance.

8. Qualitative Results: We demonstrate the efficacy of the proposed framework by illustrating the robust and safe passage at intersection. As shown in Fig. 11, 12 vehicles simultaneously enter and navigate the intersection with different driving maneuvers. The solid black lines represent the road boundaries and centerlines to separate the lanes. Fig. 11(11(a))-(11(f)) illustrates the driving process of all vehicles at different time stamps under motion and sensory measurement uncertainties. The solid lines of the same color behind each vehicle represent their history trajectories. Fig. 11 clearly shows that all vehicles successfully navigate the intersection with collision-free and smooth trajectories. The collision avoidance buffer dynamically adapts to the degree of state uncertainties and the surrounding vehicle density in the process. All vehicles interact flexibly to avoid each other, demonstrating robustness to disturbances and uncertainties.

V-C Discussion

1. Balancing safety assurance, real-time computation, and passing efficiency: The proposed framework is fundamentally safety-oriented. It explicitly accounts for various sources of uncertainty and interference, and enforces an uncertainty-aware safety formulation throughout the coordination process. As a result, the evolution of trajectory distributions during multi-vehicle interactions can be directly regulated to enhance safety. Building on this safety-critical formulation, the framework achieves real-time performance through the fully parallel ADMM-DTN algorithm together with the interaction-selection mechanism, which jointly reduce computational burden and coupling scale without relaxing the safety requirements. Real-time replanning, in turn, improves practical safety by enabling timely responses to disturbances, uncertainty growth, and unexpected obstacles. At the same time, by precisely characterizing state uncertainty, the proposed method supports finer-grained safety interaction modeling with reduced conservatism, thereby improving passing efficiency when traffic conditions permit. Overall, the framework prioritizes safety-assured planning while simultaneously enabling efficient real-time computation and improved passing efficiency.

2. Handling Human-Driven Vehicles (HDVs): The proposed framework primarily focuses on uncertainty-aware trajectory coordination and real-time parallel computation. Nevertheless, it can accommodate HDVs at a coarse level by modeling them as moving obstacles and enforcing the corresponding collision-avoidance constraints. Existing approaches for handling human drivers in coordinated autonomy mainly fall into two categories: intent inference and behavior prediction [41], and utility- or social-preference-based behavior modeling [21]. Accordingly, a finer-grained treatment of HDVs could be enabled by incorporating a dedicated human-driver modeling module, such as intent inference, trajectory prediction, or preference-aware behavior modeling. Specifically, the resulting probabilistic HDV trajectory distributions can be integrated into the existing safety constraints, whereas inferred utilities or preferences can be used to parameterize game-theoretic interaction formulations.

VI Real-world Experiments

Finally, to further validate the practicality and robustness of the proposed framework in a real-world environment, we implement the algorithm on four autonomous mobile robots with the Robot Operating System (ROS). In the experiment, the robots used are based on the platform from[23] as shown in Fig. 12, which is equipped with the Intel D455 depth camera and the T265 tracking camera to perceive the environment and measure the robot’s position and velocity. Each mobile robot uses the Intel NUC 12 Pro microcomputer with Intel i7-1260P 2.10 GHz CPU and 16 GB of RAM as the onboard computing platform to process sensor data and compute local trajectories. The robots share trajectory information with the neighboring robots via Wi-Fi links. The disturbance matrix $G$ is estimated online using the deviation of the physical system from the nominal model. At each sampling time $t$, the disturbance matrix $G$ is updated by the square root of the sample covariance of the most recent deviation, similar to the approach in[19]. The measurement noise matrix is set as $D=\mathrm{diag}(0.1,0.1,\pi/180,0.1)$ based on the precision evaluated in[4]. The vehicle parameters are as follows: $l_{\text{car}}=0.45$m, $w_{\text{car}}=0.38$m, $v_{\text{max}}=0.6$m/s, $a_{\text{max}}=0.1$m/s2.

Fig. 13 shows a typical laboratory intersection scenario with sizes of 8.4 m × 8.4 m, and the lane width is 1.2m. It can be viewed as a size-scaled road intersection. In this experiment, four CAVs perform unprotected left-turn maneuvers. From the Fig. 13, we can observe that all four CAVs can enter the intersection simultaneously, which greatly improves crossing efficiency. They can flexibly and robustly avoid collisions at each time step despite disturbances and noise. Finally, all four CAVs successfully and safely navigate the intersection along stable trajectories.

To show the ability of the proposed framework to handle unexpected non-CAV vehicles during intersection crossing, we introduced an unexpected moving obstacle on the road, marked as a red box. This dynamic obstacle accelerates along the road within a speed range of 0.18 - 0.6 m/s and may appear in the CAVs’ paths. It does not communicate with other CAVs and thus can be regarded as a human-driven or legacy vehicle. To avoid potential collisions, the three CAVs entering the intersection need to not only avoid each other but also evade the unexpected moving obstacle. Fig. 14 shows the experimental results. From $t=3$s to $t=9$s, it is clear that CAV1 and CAV2 can safely navigate around the unexpected obstacle upon spotting it. Meanwhile, the three CAVs safely interact to avoid each other during navigation. This shows that the proposed framework is capable of simultaneously handling collision avoidance among CAVs and with unexpected dynamic obstacles, showcasing the advanced adaptability of cooperative driving.

To complement the visualization results shown in Figs. 13 and 14, we further repeated each real-world experiment 50 times. Table V reports the average computation time (ACT), worst-case computation time (WCT), minimum separation distance (MSD), and success rate (SR) for the two scenarios. For the unprotected left-turn experiment, these results indicate stable and safe intersection crossing while also confirming efficient online computation in the physical system. Compared with the unprotected left-turn case, the unexpected-obstacle experiment exhibits a slightly lower SR and a smaller MSD, due to additional dynamic non-CAV vehicles and the resulting unanticipated interactions. Although the worst-case computation time slightly exceeds the 0.1 s replanning interval, this occurs rarely. In such cases, the system continues executing the previous feasible solution until the updated plan becomes available. Overall, the results still demonstrate robust operation, stable safety margins, and practical online performance in this more challenging mixed-traffic scenario.

Our framework is explicitly designed to adapt to practical deployments with varying computational resources: (i) the proposed ADMM-DTN is fully parallel across agents, and (ii) the Jacobi-style negotiation guarantees a feasible solution for any number of negotiation rounds, enabling the system to trade off solution quality versus available compute. (iii) the decision variables are organized sequentially rather than in a batch form, which substantially accelerates computation [33].

VII Conclusion

This paper proposes a robust and real-time coordination framework for cooperative trajectory planning of CAVs in dynamic and uncertain environments. A novel uncertainty-aware coordination problem formulation is developed to directly control trajectory distribution evolution during vehicle interactions, and is solved using a fully parallel ADMM-based distributed trajectory negotiation algorithm (ADMM-DTN) and an interactive attention mechanism. The coordination framework adapts to various time-varying scenarios while simultaneously ensuring probabilistic safety guarantees and efficient real-time implementation. Comprehensive simulations and experiments have shown that the proposed framework outperforms representative benchmarks in terms of safety and efficiency. The collision rates can be reduced by up to 29.86% and 40.79% compared to various benchmark methods. Furthermore, the computation time is significantly shorter by an order of magnitude compared to STRSv2 and exhibits better scalability than CFSDMPC. Specifically, our framework can robustly and flexibly handle unexpected non-CAV obstacles in real-world experiments, addressing the applicability limitations of most existing vehicle coordination methods.

Future research directions include extending the proposed framework to explicitly model and predict human-driver social behaviors, as well as reason over such behaviors in a game-theoretic manner—moving beyond non-communicating obstacle handling—to further broaden its applicability.

Appendix A Bicycle Model and Dynamics Linearization and Discretization

Based on the bicycle model, the deterministic vehicle dynamics of CAVs follows the nonlinear kinematic function

[TABLE]

where $(\dot{\bullet}):=\frac{\partial}{\partial t}(\bullet)$ and $L_{w}$ denotes the distance between the front and rear wheels axles. Using the first-order Taylor polynomial, this nonlinear function can be linearized around a nominal trajectory $\bar{x}_{i}=\left[\bar{x}_{i}^{0},\ldots,\bar{x}_{i}^{N}\right]^{\top}$ and $\bar{u}_{i}=\left[\bar{u}_{i}^{0},\ldots,\bar{u}_{i}^{N-1}\right]^{\top}$, resulting in

[TABLE]

where the associated coefficients $(A_{i},B_{i},r_{i})$ are derived as

[TABLE]

[TABLE]

Subsequently, to ensure high accuracy and numerical stability, 2nd-order Runge-Kutta integration is employed to discretize the dynamics. The discrete-time interval is denoted as $\tau$. The stochastic and linear time-varying dynamics with noise added is constructed as follows

[TABLE]

where

[TABLE]

[TABLE]

[TABLE]

and the diagonal elements of $G_{i}^{k}$ correspond to the magnitude of the noise.

Appendix B Proof of Lemma 1

Proof. It follows from that the closed-loop covariance propagation is $\kappa$-contractive in the positive semidefinite order through the computed feedback gain

[TABLE]

Let $W_{i}^{k}$ denote the filtered noise term in (23), i.e., $W_{i}^{k}:=L_{i}^{k+1}P_{\xi,i}^{k+1}L_{i}^{k+1\top}\succeq 0$ and let $P_{\infty}$ denote the steady-state covariance achieved when the system converges. Substituting (50) into (23), the evolution of the covariance is bounded as:

[TABLE]

We now iterate the covariance evolution over time:

[TABLE]

the noise term $W_{i}^{k}$ is a bounded filtered noise term, satisfying $W_{i}^{k}\preceq\bar{W}$, we have,

[TABLE]

therefore, as $k\rightarrow\infty$, the covariance converges to the steady-state value:

[TABLE]

From (51), we have

[TABLE]

since $\hat{P}_{i}^{k}\succeq P_{\infty}=\frac{1}{1-\kappa^{2}}\bar{W}$, then $\bar{W}\preceq\left(1-\kappa^{2}\right)\hat{P}_{i}^{k}$. Substituting,

[TABLE]

Therefore, if $\hat{P}_{i}^{0}\succeq P_{\infty}$, then $\hat{P}_{i}^{k+1}\preceq\hat{P}_{i}^{k}$ for all $k$, and the covariance will monotonically decrease toward the steady-state bound $P_{\infty}$.