Cooperative Motion Planning System Architecture

Fig. 4: Overview of the proposed cooperative motion planning framework for unsignalized intersections. CAVs upload their states, targets, and constraints to the roadside infrastructure via V2X communication for cooperative planning, and the optimized executable trajectories are transmitted back to the vehicles for real-time motion execution. The core passage trajectory generation module consists of three stages: conflict search, experience reuse, and trajectory optimization. First, CBS initializes and expands the constraint tree to detect spatiotemporal conflicts; then, local trajectory optimization intervals are extracted based on the conflict time window, and parameterized spatiotemporal trajectory optimization is performed under unified kinematic and environmental constraints to generate dynamically feasible and collision-free local cooperative trajectories. Meanwhile, the experience reuse module incorporates historical feasible trajectories to accelerate the local replanning process. Finally, through the continuous expansion and convergence of CBS nodes, a globally feasible multi-vehicle cooperative trajectory solution is obtained.

Experimental results and analysis

We conducted a comparative analysis of the proposed algorithm against three state-of-the-art methods—PSB, Coupled OCP, and OPC-CBS—along two key dimensions: coordination efficiency and trajectory quality, under various task types and planning scales. The evaluation metrics encompass the following aspects:

Coordination Efficiency: Including algorithm runtime, total motion time of all vehicles, average passing speed, interaction data volume, and average vehicle speed.
Trajectory Quality: Focusing on the continuity and smoothness of the trajectory in dynamic aspects such as velocity, acceleration, and jerk.

Details of multi-vehicle cooperative motion planning in spatiotemporal conflict-intensive regions

Fig. 7: Details of multi-vehicle cooperative motion planning in spatiotemporal conflict-intensive regions

As shown in Fig.7a, dense spatiotemporal conflicts occur among CAV-2, CAV-3, and CAV-4 within the unsignalized intersection area. Traditional approaches typically perform global replanning over the entire trajectory to resolve such conflict-intensive regions, which incurs high computational overhead and often disrupts the consistency of the original trajectory. In contrast, the proposed method enables substantial trajectory inheritance for CAV-2, CAV-3, and CAV-4 before and after entering the intersection, with only localized trajectory reconstruction applied in the core conflict zone. As illustrated in Fig.7b, CAV-2 and CAV-3 exploit the strong deformation capability of the differentially flat vehicle model to generate spatial trajectories in continuous space. By temporarily utilizing the adjacent right lane, they execute high-speed evasive maneuvers around CAV-0 and CAV-4, achieving passage speeds of 14.914 m/s and 14.149 m/s, respectively. Subsequently, both vehicles return to their designated task lanes, as depicted in Fig.7c. This strategy maintains trajectory structural stability and prevents the propagation of new conflicts caused by large-scale trajectory modifications. By applying localized conflict segment replanning, the method significantly reduces the solution space explored during CBS node expansion. Therefore, by reusing trajectory segments outside the conflict zone and performing incremental correction only on localized conflict fragments, the proposed method substantially lowers the solution dimensionality and cost function perturbation, thereby improving overall planning efficiency and solution convergence.

Quantitative Analysis of Experimental Results

For each vehicle scale, we randomly generate the initial position, velocity, acceleration, and target lane for each vehicle within the parameter range specified in Table 2. To ensure fairness between different algorithms, all methods are compared on the same set of randomized scenarios within the same scale. In order to reduce the impact of randomness, 20 test cases are independently generated for each scale using the same random seed, and the success rate and average computation time are recorded. The experiments are considered failed if no feasible solution is found within 500s.

Figure 9 shows the success rate variation of the four methods across different scales, while Figure 10 displays the trend of the average computation time as the number of vehicles increases.

Based on the experimental results, the proposed LECTP method demonstrates a clear advantage in both success rate and computation time. As the vehicle scale increases from 2 to 40, the success rate of LECTP remains consistently high. It maintains a 100% success rate for up to 26 vehicles, and even in the most challenging scenario with 40 vehicles, the success rate is still around 75%. This indicates that LECTP can stably coordinate large-scale vehicles through intersections, significantly outperforming other comparison methods.

Regarding computation time, LECTP exhibits a smooth increase in average computation time as the number of vehicles grows, while methods like PSB and OPC-CBS encounter significant time bottlenecks at larger scales, with solving times rapidly increasing. Specifically, PSB reaches 36.73 seconds at around 10 vehicles, and for more than 18 vehicles, the computation time exceeds 100 seconds. Coupled OCP also shows significant computational bottlenecks at relatively small scales. In contrast, LECTP maintains effective control over computation time throughout the test range, demonstrating good scalability.

In summary, the LECTP method exhibits high success rates and low computation times in large-scale vehicle coordination planning, particularly maintaining stable performance even as the number of vehicles increases. This makes LECTP significantly superior to other comparison methods.

Computation Time Analysis of LECTP Method

Figure 11 shows the computation time statistics for the LECTP method as the vehicle scale increases by 2 vehicles at a time. The yellow values represent the median computation time. For the 14-vehicle scale, the median computation time is only in the range of a few hundred milliseconds. The maximum computation time at 14 vehicles is 1.691 seconds, and the minimum computation time is 0.483 seconds. In the 16–40 vehicle scenarios, the computation time increases linearly as the vehicle scale grows.

Tree Depth and Node Count Analysis of LECTP Method

Figure 12 illustrates how the tree depth (Depth) and node count (Node Count) of the LECTP algorithm change as the vehicle scale increases. The figure shows that both tree depth and node count increase approximately linearly with the vehicle scale. This linear growth trend in tree depth and node count corresponds to the computation time growth trends observed in Figures 10 and 11.

Node-to-Depth Ratio (NDR) Analysis of LECTP Method

To further explain the advantages of LECTP in computation time and conflict resolution from the perspective of constraint tree structure, we introduce a new metric — Node-to-Depth Ratio (NDR). NDR is defined as the ratio of the total number of expanded constraint tree (CT) nodes to the maximum depth of the tree, serving as a structural indicator to measure the expansion pattern of the constraint tree. Figure 13 presents the NDR curve of LECTP. In the range of 6–40 vehicles, the NDR remains around an average of 2.51. The figure shows that at the 40-vehicle scale, the median depth of the tree is 124 layers, with 345 expanded nodes. Notably, the NDR does not exhibit exponential growth with increasing scale. Compared to the node explosion in the traditional CBS framework, LECTP demonstrates stronger scalability and conflict localization ability at the structural level.

Ablation Experiments

To systematically evaluate the influence of key modules and parameter configurations in the proposed LECTP method, we conduct two categories of ablation experiments: (1) sensitivity analysis of the window-time scaling factor λ and the safety margin δ; and (2) assessment of the performance impact when enabling or disabling the experience-reuse mechanism. The experiments are performed under three representative traffic densities (12, 24, and 36 vehicles) to ensure the robustness of the conclusions across different scales. All scenarios are generated using the same randomized environment construction procedure as in the extended experiments discussed previously.

Real-World Test

Fig. 14: The setup mimics a bidirectional four-lane intersection with six AgileX LIMO Pro robotic platforms representing intelligent connected vehicles. Among them, four vehicles are assigned left-turn tasks and two are assigned straight-driving tasks. Each LIMO communicates in real-time with a simulated roadside computation unit carried by a quadrotor UAV. The LIMO robots send task waypoints and real-time state information to the UAV, which runs the proposed collaborative spatiotemporal trajectory optimization algorithm and returns parameterized trajectories to each vehicle for execution. The LIMOs follow the generated trajectories using Ackermann steering mode. For precise vehicle localization, we employ the FZMotion Motion Capture System.

Physical experiment results

Fig. 15: (a) Snapshot of the entire experiment process. (b) Historical trajectories and current vehicle states corresponding to (a). (c) Speed-time curves of all vehicles. (d) Temporal variation of the maximum and minimum inter-vehicle distances.