CosineOpt: Optimization-Based Centralized Cooperative Speed Planning for Multiple CAVs Along Intersected Fixed Paths

This paper focuses on cooperative speed planning for multiple connected and automated vehicles (CAVs) traversing along intersected fixed paths. Nominally, this task is formulated as an optimal control problem incorporating logical operators to represent collision-avoidance constraints. This formulation requires solving a mixed-integer nonlinear programming (MINLP) problem, while handling non-differentiable integer variables remains challenging for gradient-based solvers. Instead of solving the MINLP, we propose a cosine-based method, a novel geometric strategy for formulating collision-avoidance constraints between CAVs. Constructing such a geometric model introduces potential approximation errors, which are mitigated by fitted correction terms designed to compensate for geometric deviations and refine the distance calculation. We propose a simulation-based planner to provide the speed profile with the globally optimal passing order, serving as a warm start for the solver. A lightweight iterative optimization strategy is also adopted to enhance robustness. Additionally, we propose a fault-tolerant strategy to ensure both system safety and operational efficiency. Extensive simulation results verify the proposed method, and comparative experiments demonstrate its efficiency.