ADMM-iCLQG: A Fast Solver of Constrained Dynamic Games for Planning Multi-Vehicle Feedback Trajectory

Trajectory planning for autonomous driving requires safe, robust and efficient algorithms in complex, dynamic environments where multiple vehicles interact. Constrained Dynamic Games (CDG) provide a unifying framework to model such multi-agent interactions, with solutions corresponding to Generalized Nash Equilibria (GNE). Existing solving methods of CDG can be divided into the open-loop strategies, which is computational-efficient but lack robustness to disturbances, and the feedback strategies, which offer adaptability but often struggle to guarantee real-time performance when handling hard constraints. To address these limitations, we introduce ADMM-iCLQG, a solver of CDG tailored for planning a feedback trajectory in real-time. We iteratively approximate the nonlinear CDG with a linearly Constrained Linear Quadratic Game (CLQG). By leveraging the Alternating Direction Method of Multipliers (ADMM), the solver efficiently manages constraints while computing feedback strategies that converge to GNE. We demonstrate the algorithm effectiveness through extensive simulations, showing its superiority in computational efficiency and reliability. Furthermore, physical experiments confirm the approach's potential of real-time implementation, achieving a planning frequency exceeding 40 Hz for up to six vehicles.