PaperlayerView exact revision on arXiv

Source-linked AI summary

A heuristic for optimal coordination of autonomous vehicles: real-time algorithms

Saugat Shahi, Jonathan Frey, Filippo Fabiani, Moritz Diehl, Mario Zanon

arXiv:2610.37632v1eess.SYcs.SY

TL;DR

Coordinating connected and automated vehicles at unsignalized intersections requires jointly choosing crossing order and vehicle trajectories. The paper uses a bi-level formulation with sensitivity-based swap screening; in closed-loop simulation, its real-time variant decides within the sampling period and has lower cost than the delayed full-resolve controllers.

  • Problem

    Choosing crossing order and vehicle trajectories jointly forms a mixed-integer nonlinear program, and finding an optimal crossing order is combinatorially hard.

    Source: §I · p. 1

  • Method

    The paper formulates coordination bi-level, computes vehicle-value sensitivities, and uses a prediction QP to screen candidate swaps before confirming predicted improvements with a re-solve.

    Source: §I · p. 1, 2) Upper level · p. 4, D. Parametric sensitivity analysis · p. 6

  • Results

    The real-time variant decides within the 100 ms sampling period and has cost 19.7% above the ideal controller without delay, while full-resolve controllers are 156.3% and 197.2% above it.

    Source: §V · p. 9

  • Takeaways & Limitations

    Once computation time is accounted for, the paper concludes that a fast approximate decision is worth more than an exact but late one.

    Source: §V · p. 9

Abstract

from arXiv · show

When coordinating connected and automated vehicles at an unsignalized intersection, the crossing order and the vehicle trajectories must be decided jointly. For a fixed crossing order, the problem can be formulated conveniently in a bi-level form where each vehicle solves its own optimal control problem, whose optimal value is a function of its entry and exit times, and a coordinator optimizes these times subject to precedence constraints. We compute the first and second order sensitivities of lower-level problems, which are required to solve the upper-level problem, by means of algorithmic differentiation using the real-time software package \texttt{acados}. This implementation exploits tailored algorithms to obtain a computationally efficient implementation of the optimal coordination algorithm. We validate our implementation in numerical simulations that demonstrate both real-time feasibility and effectiveness of our algorithm.

I. INTRODUCTION

Coordinating vehicles at an unsignalized intersection requires jointly choosing crossing order and trajectories, a difficult mixed-integer problem. The paper uses a bi-level formulation and sensitivity-based swap predictions to reduce costly re-solves, and formulates vehicle motion and crossing times along fixed paths.

I. INTRODUCTION

  • Jointly choosing crossing order and trajectories forms a mixed-integer nonlinear program, and optimal ordering is combinatorially hard; fixing schedules can be far from optimal for heterogeneous fleets.Prior approaches include first-come-first-served reservations and static priorities.Source: §I · p. 1
  • Fixed-order methods decompose coordination into an upper-level NLP and independent vehicle OCPs, while swap heuristics still require complete re-solves that impede real-time operation as vehicle counts grow.The paper identifies parametric sensitivities as an alternative for handling these re-solves.Source: §I · p. 1
  • The paper computes gradients and Hessians of vehicle value functions and uses a prediction QP to screen schedule swaps, confirming predicted improvements with a re-solve.It also solves the upper level by SQP and compares centralized and decentralized formulations in closed-loop simulations with computation delay.Source: §I · p. 1

II. PROBLEM FORMULATION

  • Vehicles follow fixed paths, and their continuous-time trajectories are obtained by discretizing the motion model with zero-order-held inputs and propagating between sampling instants to define crossing times.Entry and exit times need not coincide with sampling instants.Source: §II · p. 2

A. Centralized formulation

For a fixed coordination set and crossing schedule, the centralized formulation optimizes continuous vehicle and timing variables. Jointly optimizing the schedule turns the problem into an MINLP, addressed with a multiplier-guided swapping heuristic.

A. Centralized formulation

  • For a fixed coordination set and schedule, V(Sk) is the minimum coordination cost over the continuous variables (x, u, t).The fixed-schedule problem is an NLP initialized with each vehicle’s measured state.Source: A. Centralized formulation · p. 2
  • Joint optimization of the schedule and trajectories yields an MINLP, whose combinatorial part is addressed with a multiplier-guided swapping heuristic.Binary variables select which vehicle in each pair crosses first.Source: A. Centralized formulation · p. 2
  • The fixed-schedule centralized problem optimizes continuous variables, while schedule choice introduces binary variables for pairwise crossing order.The measured initial state of each vehicle is included in the formulation.Source: A. Centralized formulation · p. 2

B. Multiplier-guided schedule refinement

The schedule-refinement heuristic ranks adjacent swaps using precedence-constraint multipliers, then verifies candidate swaps by re-solving and accepts only cost reductions. The decentralized formulation replaces most expensive swap re-solves with a QP using sensitivities from per-vehicle problems.

B. Multiplier-guided schedule refinement

  • The heuristic selects adjacent pairs with high λc for evaluation and maintains a memory of pairs already tried.A swap updates the schedule only when the re-solved cost decreases, giving monotone cost improvement.Source: B. Multiplier-guided schedule refinement · pp. 2–3
  • The multiplier λc estimates first-order optimal-cost sensitivity to relaxing an adjacent-vehicle precedence constraint, but a large value does not guarantee a beneficial finite swap.Each candidate swap is therefore verified by re-solving, and accepted only when its cost is lower.Source: B. Multiplier-guided schedule refinement · pp. 2–3

III. DECENTRALIZED FORMULATION

  • The centralized formulation has separability, information-sharing, and repeated-solve bottlenecks; the bi-level reformulation assigns each vehicle its own OCP and exchanges crossing times with a coordinator.Sensitivities of the value functions let a QP replace expensive candidate-swap re-solves.Source: §III · p. 3

A. Bi-level reformulation

For fixed crossing times, the trajectory problem separates into independent vehicle OCPs, while the coordinator handles crossing-time decisions and precedence coupling. The lower-level value functions are defined only for feasible times, whose domains constrain the upper level.

A. Bi-level reformulation

  • For fixed crossing times, each vehicle solves an independent OCP because costs are separable and inter-vehicle coupling occurs only through precedence constraints on crossing times.The vehicle problems can be solved in parallel.Source: A. Bi-level reformulation · p. 3, 1) Lower level · p. 3

1) Lower level:

  • Each vehicle's OCP defines a value function of its entry and exit times, with optimal primal variables and crossing-condition multipliers also depending on those times.The coordinator imposes crossing times and solves the upper-level problem.Source: A. Bi-level reformulation · p. 3, 1) Lower level · p. 3
  • A vehicle's value function is defined only where its OCP is feasible, so the upper level must enforce the corresponding domain constraints.Entry and exit times have reachable bounds; exit-time bounds depend on entry time.Source: 1) Lower level · p. 3
  • The coordinator sends crossing times to vehicles and receives their value functions and domain constraints, together with derivatives, rather than their trajectories.The architecture depicts crossing-time messages downward and vehicle outputs upward.Source: Figure 2 · p. 4

2) Upper level:

The upper level optimizes crossing times subject to precedence and domain constraints, retaining the centralized problem's optimal value for a fixed schedule. Its SQP procedure uses QP models and tailored implementations to update crossing times and evaluate candidate swaps.

2) Upper level:

  • For a fixed schedule, the upper-level problem is an exact reformulation whose optimal value matches the centralized problem, while retaining precedence coupling over crossing times.The coordinator uses value functions, domain constraints, and their derivatives without exchanging trajectories.Source: 2) Upper level · p. 4

B. Upper-level SQP

  • Warm-started SQP solves the crossing-time NLP, and its quadratic model is also used to predict the cost effect of a candidate swap.Replanning starts from prior crossing times and vehicle OCP primal-dual solutions.Source: B. Upper-level SQP · pp. 4–5
  • The SQP QP keeps precedence constraints exact, linearizes domain constraints, and modifies Hessian blocks to ensure convexity.The small Hessian blocks are made positive definite by replacing eigenvalues below a threshold.Source: B. Upper-level SQP · p. 4
  • The structured HPIPM formulation exploits chain structure and scales linearly with vehicle count, while the dense formulation is faster for smaller problems.A projected backtracking line search handles steps that may increase cost or violate feasibility.Source: B. Upper-level SQP · pp. 4–5

C. Derivatives of the value function and of the domain constraints

The QP requires first- and second-order derivatives of each vehicle's value function and domain constraints with respect to crossing times.

C. Derivatives of the value function and of the domain constraints

  • The QP uses value-function derivatives from the lower-level OCP's KKT system and domain-constraint derivatives from their closed form.Both first- and second-order derivatives are required with respect to crossing times.Source: C. Derivatives of the value function… · p. 5

1) First-order derivatives:

The lower-level value-function gradient comes from the multipliers of the crossing conditions. Its Hessian accounts for trajectory sensitivities and acceleration terms, with off-diagonal entries set by symmetry.

1) First-order derivatives:

  • The gradient of each vehicle’s value function with respect to its crossing times is obtained from the crossing-condition multipliers.The crossing-time parameter enters the lower-level problem only through those conditions.Source: 1) First-order derivatives · p. 5

2) Second-order derivatives:

  • The Hessian includes changes in the optimal trajectory and a diagonal acceleration term weighted by the crossing-condition multiplier.The acceleration term appears only on the diagonal; symmetry lets the method compute one off-diagonal entry and use it for both.Source: 2) Second-order derivatives · p. 5

3) Derivatives of the domain constraints:

Domain constraints depend on crossing times and measured vehicle state, with curvature arising only from exit-time bounds. Lower-level primal-dual sensitivities are computed from KKT systems under regularity assumptions.

3) Derivatives of the domain constraints:

  • The domain constraints depend on crossing times and measured state, not on the lower-level optimal solution.The first two components are affine, while only the exit-time bounds contribute curvature.Source: 3) Derivatives of the domain constraints · p. 5
  • Derivatives are obtained by algorithmic differentiation at a cost negligible compared with the lower-level solve; the Lagrangian Hessian with respect to t is block diagonal, with one 2 × 2 block per vehicle.Source: 3) Derivatives of the domain constraints · p. 5

D. Parametric sensitivity analysis

  • Under LICQ, strict complementarity, and second-order sufficiency, the lower-level primal-dual solution is locally differentiable in crossing times.Its sensitivities follow from two linear systems using the KKT matrix and the derivative of the KKT residual.Source: D. Parametric sensitivity analysis · pp. 5–6
  • The required primal-dual sensitivities are computed with acados solution-sensitivity functionality.The OCP uses an exact Lagrangian Hessian, and the factorized KKT matrix is reused for two back-substitutions.Source: D. Parametric sensitivity analysis · p. 6

E. Sensitivity-based swap prediction

The method estimates the cost change of an adjacent schedule swap with a QP built from the current second-order cost model. It screens candidate swaps, then confirms predicted improvements with a warm-started re-solve.

E. Sensitivity-based swap prediction

  • A candidate swap evaluation otherwise costs a complete upper-level SQP solve together with its associated lower-level OCPs.The QP prediction replaces most of these full re-solves.Source: E. Sensitivity-based swap prediction · p. 6
  • A prediction QP estimates an adjacent swap’s cost change by minimizing the local second-order model under the swapped schedule’s linearized constraints.It reuses the gradient and Hessian at the current KKT point and requires no lower-level solve.Source: E. Sensitivity-based swap prediction · p. 6
  • Only swaps predicted to improve are re-solved, and a swap is accepted only if the re-solve confirms a lower cost.This preserves the acceptance rule and monotonicity while replacing most re-solves with QPs.Source: E. Sensitivity-based swap prediction · p. 6

F. Real-time scheduling iteration

The real-time variant uses one derivative evaluation and two QPs per sampling instant, regardless of candidate-swap count. Simulations report faster coordination than the centralized formulation, while computation-time tests characterize the scheme’s components and QP scaling.

F. Real-time scheduling iteration

  • The real-time scheduling variant performs one derivative evaluation and two QPs per sampling instant, independently of the number of candidate swaps.A predicted improvement is accepted without confirmation at that instant; the next sample relinearizes at the new schedule.Source: F. Real-time scheduling iteration · pp. 6–7

IV. NUMERICAL RESULTS

  • The simulations use six vehicles per randomized scenario, with arrivals, vehicle classes, dynamics, and controller settings specified for the comparison.The lower-level OCPs are solved in parallel on one computer, with one thread per vehicle.Source: §IV · p. 7, A. Distributed vs Centralized Solution · p. 7

A. Distributed vs Centralized Solution

  • 382 ms versus 10.6 ms was the median computation time per event for the centralized and bi-level formulations, respectively.The largest event took 6.1 s and 79 ms, respectively; Fig. 3 shows upper-level SQP convergence across the 100 scenarios.Source: A. Distributed vs Centralized Solution · p. 7, Figure 3 · p. 7

B. Computation time of the bi-level scheme

C. Dense versus structured upper-level QP

  • The structured upper-level QP scales as M^1.03 versus M^2.85 for the dense formulation, crossing near M ≈ 39 and running 121 times faster at M = 512.The implementation uses the dense formulation below the crossover and the structured formulation above it.Source: C. Dense versus structured upper-level… · p. 8

D. Closed-loop operation under computation delay

With fixed computation delays, the real-time controller C3 achieves lower closed-loop cost than the full-resolve controllers while deciding within the 100 ms sampling period. The conclusion reports matching crossing orders in 100 randomized scenarios and linear scaling for the structured upper-level QP.

D. Closed-loop operation under computation delay

  • C3 stays within 69.2 ms and costs 19.7% above C0, versus C2 and C1 costs 156.3% and 197.2% above C0.C3 decides every sampling instant; the full-resolve controllers incur longer latency.Source: D. Closed-loop operation under… · p. 8, §V · p. 9, Table I · p. 9
  • C3’s mean computation time is 25.6 ms, including 17.1 ms for derivatives; evaluating derivatives one vehicle at a time on a single thread takes 0.75 ms per vehicle.In the Fig. 2 architecture, the authors estimate a C3 decision would take about 9 ms.Source: D. Closed-loop operation under… · pp. 8–9

V. CONCLUSION

  • The bi-level method matches the centralized formulation’s crossing orders in 100 randomized scenarios, and its prediction QP predicts the correct cost-change sign for every confirmed swap.The method computes value-function gradients from crossing-condition multipliers and Hessians from acados solution sensitivities.Source: §V · p. 9
  • The structured HPIPM formulation makes the upper-level QP scale linearly with the number of vehicles.Source: §V · p. 9

Related papers

Loading 2610.37632v1…