Introduction
Cooperative trajectory planning is a cornerstone of future autonomous fleet management, enabling Connected Autonomous Vehicles (CAVs) to negotiate complex environments like intersections and highway merges. However, as the number of vehicles increases, centralized planning systems face a "curse of dimensionality." This paper introduces a decentralized framework that combines iterative Linear Quadratic Regulator (iLQR) techniques with Dual Consensus ADMM, allowing vehicles to plan safe, feasible paths in parallel while significantly reducing computational overhead.
The Challenge of Multi-Agent Coordination
Real-time cooperative planning is notoriously difficult due to three primary factors:
- Non-linearity: Vehicle dynamics, including steering and acceleration, do not follow simple linear paths.
- Non-convexity: Collision avoidance creates "holes" in the searchable optimization space, making it difficult for standard tools to find global solutions.
- Poor Scalability: Centralized solvers typically scale at $O(N^3)$, where $N$ is the number of vehicles. In high-density scenarios, the time required to find a solution exceeds the limits of real-time performance on standard hardware.
Existing decentralized methods often struggle to handle the "coupling" between vehicles—ensuring that Vehicle A’s planned path does not conflict with Vehicle B’s—without relying on a central coordinator that reintroduces a single point of failure and a computational bottleneck.
Methodology: Decentralized iLQR via Dual Consensus ADMM
The authors propose a three-stage approach to decouple the trajectory planning problem:
1. Convex Reformulation
The original non-convex problem is transformed into a series of convex sub-problems. By applying Taylor expansion to host costs and a Gauss-Newton approximation to collision avoidance constraints, the researchers ensure the mathematical model remains well-behaved and solvable during optimization.
2. Dual Consensus ADMM
This is the framework's core innovation. Rather than solving the complex primal problem directly, the system solves its dual. In the dual domain, the coupling constraints between vehicles take on a "consensus" structure. This allows each vehicle to compute its own trajectory update independently and then "negotiate" with neighbors via communication to reach an agreement on safe distances.
3. Primal Recovery and Feasibility
Once the dual variables converge, each vehicle recovers its specific trajectory by solving a local LQR problem. To ensure the resulting paths are physically executable, the authors implement a "dynamically feasible update" strategy using a line search to determine the optimal step size for each vehicle's movement.
Experimental Evaluation
The algorithm was benchmarked in two simulated environments: a T-Junction (3 vehicles) and a high-density Four-way Intersection (12 vehicles). These scenarios compared the proposed method against industry-standard baselines, including Centralized iLQR, IPOPT (Interior Point Optimizer), and SQP (Sequential Quadratic Programming).
Key Results
- Significant Speedup: In the 12-vehicle scenario, the decentralized algorithm completed its task in just 0.250 seconds, compared to 1.186s for centralized iLQR and 11.140s for IPOPT.
- Superior Scalability: When scaling from 4 to 12 vehicles, the centralized solver's computation time increased by 12.1x, while the proposed decentralized method's time increased by only 2.74x.
- Safety and Precision: Despite the speed, safety was not compromised. The algorithm maintained a minimum distance of 2.98m in dense traffic, well within the defined safety buffers.
Insights and Future Directions
The primary breakthrough of this work is the use of duality as a decoupler. By shifting the perspective to dual variables, the global collision avoidance problem is transformed into a local consensus problem, achieving near $O(1)$ complexity for individual vehicles when computed in parallel. Furthermore, "warm-starting" the dual variables from previous iterations significantly accelerates convergence in dynamic environments.
Future research will focus on addressing real-world communication challenges, such as signal dropouts and range limits, and implementing the framework on High-Performance Parallel Processors (HPPP) to validate its performance in massive-scale deployments.
Summary updated