Source-linked AI summary
A heuristic for optimal coordination of autonomous vehicles: dynamic problems
Saugat Shahi, Filippo Fabiani, Mario Zanon
TL;DR
Autonomous-vehicle coordination at unsignalized intersections must jointly address vehicle trajectories, crossing order, and changing traffic. This paper uses arrival-triggered receding-horizon optimization with multiplier-ranked schedule swaps, recovering on average more than 85% of the FIFO scheduling optimality gap in reported dynamic scenarios.
Problem
Intersection coordination must jointly choose vehicle trajectories and crossing order as the set of vehicles changes, while existing MPC schemes either freeze the order or use an external scheduler.
Source: §I · p. 1Method
An arrival-triggered receding-horizon scheme appends each new vehicle to the order, re-optimizes trajectories and crossing times with an NLP, and uses precedence multipliers and swap memory to guide schedule refinement.
Source: §I · p. 1, §III · pp. 3–4Results
More than 85% of the optimality gap incurred by first-in-first-out scheduling is recovered on average in the reported dynamic traffic scenarios.
Source: §I · p. 1Takeaways & Limitations
The framework bridges FIFO scheduling and the brute-force global optimum through repeated continuous-NLP solves rather than combinatorial enumeration.
Source: §V · p. 9Each candidate swap still requires a full NLP solve whose time grows with the number of vehicles, and replanning is assumed to finish within the prescribed time.
Source: §V · p. 9
Abstract
from arXiv · showhide
The coordination of autonomous vehicles at unsignalized intersections requires deciding both the trajectories of the vehicles and their crossing order, with the additional difficulty that the set of vehicles to be coordinated changes as traffic flows through the intersection. Since the resulting mixed-integer optimal control problem is hard to solve online, we propose an arrival-triggered receding-horizon scheme built around a schedule-refinement heuristic: for a fixed crossing order, a single nonlinear program (NLP) optimizes all trajectories and crossing times, and the first-order sensitivities of the cost with respect to the vehicle schedule are used to select adjacent swaps that are likely to reduce the overall cost. Replanning is triggered only when a new vehicle arrives, the new vehicle is appended at the tail of the current crossing order, and a memory of evaluated swaps that persists across replanning steps avoids redundant computations. Simulations of traffic scenarios involving streams of a few hundred heterogeneous vehicles show that the proposed scheme recovers most of the optimality gap incurred by first-in-first-out scheduling.
I. INTRODUCTION
Autonomous-vehicle intersection coordination couples trajectory planning, crossing-order scheduling, and ongoing revision as traffic changes. The paper embeds multiplier-guided schedule refinement in arrival-triggered receding-horizon control, reporting recovery of most of the FIFO optimality gap.
I. INTRODUCTION
- The coordination problem combines continuous trajectory planning, discrete crossing-order scheduling, and revision as vehicles enter or leave the intersection area.Direct negotiation at unsignalized intersections is associated with potential travel-time, energy, and throughput gains.Source: §I · p. 1
- Joint mixed-integer optimization of order and trajectories grows combinatorially, while fixed-priority and first-come-first-served rules ignore vehicle dynamics and individual objectives.Such rule-based approaches can be heavily penalized for heterogeneous fleets.Source: §I · p. 1
- Prior schedule-refinement approaches use motion-planner information or precedence-constraint multipliers to identify promising order changes.The cited multiplier approach uses first-order cost information to prioritize vehicle swaps for evaluation.Source: §I · p. 1
- Existing MPC coordination either freezes the crossing order over the horizon or relies on an external scheduler independent of trajectory optimization.When the order changes, the controller must be re-initialized, and some achievable performance is lost.Source: §I · p. 1
- On dynamic traffic scenarios, the scheme recovers on average more than 85% of the optimality gap incurred by first-in-first-out scheduling.The reported advantage is preserved as congestion increases, with a small number of NLP solves per arrival.Source: §I · p. 1
- New arrivals trigger replanning: each vehicle is appended to the current order, trajectories and crossing times are re-optimized, and precedence multipliers rank candidate adjacent swaps.A swap is committed only after it verifiably reduces cost; a persistent swap memory avoids re-testing evaluated pairs.Source: §I · p. 1
II. PROBLEM FORMULATION
The problem coordinates cooperative vehicles on predefined paths while allowing the active set to change over time. For a fixed order, trajectories, controls, and crossing times are optimized by an NLP; the schedule is then refined incrementally as vehicles arrive.
II. PROBLEM FORMULATION
- Vehicles follow predefined paths without changing lanes, while the set inside the coordination area can change through arrivals and departures.The coordination set contains vehicles that have arrived but have not yet left the area.Source: §II · p. 2
- Collision-free crossing is enforced through precedence constraints on vehicle entry and exit times, encoding which vehicle crosses before another.The optimization includes vehicle states, controls, and entry/exit times.Source: §II · p. 2
- For a fixed coordination set and order, the problem is a continuous-variable NLP; jointly optimizing the schedule and trajectories yields a MINLP whose exact solution scales poorly with vehicle count.Exhaustive enumeration of all |C_k|! schedules is viable only for a handful of vehicles.Source: §II · p. 2
- When a vehicle leaves, the optimal schedule cannot change; when one arrives, appending it to the previous order provides the initial schedule for the new problem.Consecutive coordination sets differ by at most one vehicle, enabling an incremental update.Source: §II · p. 3
III. ARRIVAL-TRIGGERED MPC WITH LAGRANGE-MULTIPLIER-BASED REORDERING
The scheme replans when vehicles arrive, appending each new vehicle to the current order before optimizing trajectories and crossing times. It then uses precedence-constraint multipliers to prioritize adjacent swaps, with swap memory limiting repeated NLP solves.
Swap memory
- Swap memory stores previously evaluated pairs to exclude them from candidates and reduce NLP solves at replanning steps.H1 stores accepted and rejected pairs permanently; H2 stores rejected pairs and clears memory at each new arrival, allowing re-evaluation after arrivals.Source: §III · pp. 3–4
Arrival-triggered outer loop
- Replanning is triggered by new arrivals: each new vehicle joins the tail of the current order, and the inner loop refines the schedule and recomputes trajectories.Vehicles that leave are removed without changing the relative order of those remaining; the first control from the latest solution is applied in MPC fashion.Source: §III · p. 3
Implementation caveat
- Because NLP solution time is non-negligible, the initial states must anticipate where vehicles will be at the algorithm's termination time; a distant termination time risks a vehicle already reaching the intersection.The algorithm may therefore solve only part of the problem before the outer loop is invoked again with updated states and a new termination time.Source: §III · pp. 3–4
Multiplier-guided schedule refinement
- The largest precedence multiplier ranks the next adjacent swap to test, but the swap is accepted only if re-optimization reduces the overall cost.The multiplier is first-order sensitivity information: a larger value flags a pair as more likely to reduce cost when swapped.Source: §III · p. 4
H1 and H2 variants
- H1 evaluates each pair at most once over the simulation, whereas H2 can revisit rejected pairs after a new arrival and can later revert accepted swaps.H1's candidate set is a subset of H2's at each replanning step, and it typically performs fewer NLP solves.Source: §III · p. 4
IV. NUMERICAL SIMULATIONS
The numerical simulations implement the arrival-time scheduling problem at an unsignalized intersection using a discrete-time double-integrator vehicle model. They use quadratic costs and three vehicle classes to represent heterogeneous behavior.
Simulation setup
- The simulation uses randomly generated vehicle arrival times at an unsignalized intersection, with crossing order scheduled in real time.The implementation uses Python, CasADi, and IPOPT.Source: §IV · p. 4
Vehicle model
- Vehicle dynamics use a discrete-time double integrator, with initial positions and velocities generated randomly and entry and exit positions fixed at 10 m.The fixed entry and exit positions apply to all vehicles.Source: §IV · p. 4
Cost and vehicle classes
- Each vehicle's quadratic cost penalizes acceleration and deviation from reference velocity, with three vehicle classes assigned different cost weights.Larger r_i discourages aggressive control, while larger q_i enforces tighter reference-velocity tracking.Source: §IV · p. 4, Table II · p. 4
A. Illustrative scenarios
Illustrative six- and nine-vehicle scenarios show how fleet composition and new arrivals shape accepted swaps. In both nine-vehicle scenarios, the arrival of heavy vehicle 6 triggers multiple swaps and the largest single-step cost reduction.
A. Illustrative scenarios
- The six-vehicle examples use identical arrival times but different mixes of light, medium, and heavy vehicles, whose cost weights differ.Scenario 1 is heavy-dominated; Scenario 2 has a more balanced mix.Source: A. Illustrative scenarios · p. 5
- For the heavy-dominated six-vehicle scenario, H2 attempts more swaps than H1 because it reevaluates previously evaluated pairs after new arrivals.The swap diagrams distinguish accepted swaps with solid lines and rejected swaps with dotted lines.Source: A. Illustrative scenarios · p. 5
- In the balanced six-vehicle scenario, medium vehicle 3 and then heavy vehicles 4 and 5 are each moved ahead of light vehicle 2.No swap is beneficial before vehicle 3 arrives.Source: A. Illustrative scenarios · p. 5
- In both nine-vehicle scenarios, heavy vehicle 6 triggers multiple accepted swaps and the largest single-step cost reduction.The scenarios use the same arrival times but different vehicle-class compositions.Source: A. Illustrative scenarios · p. 5
B. Randomly-generated scenarios
Across two batches of 100 randomly generated six-vehicle scenarios, the heuristic performs better than FIFO and remains close to brute-force optimal costs. Wider arrival gaps reduce the mean optimality gap, while H1 retains a computational advantage over H2.
B. Randomly-generated scenarios
- The evaluation compares FIFO, H1/H2, and brute-force enumeration of all 720 crossing orders across two 100-instance batches of six-vehicle scenarios.Batch 1 uses inter-arrival gaps of [0.2, 0.4] s; Batch 2 uses [0.3, 0.5] s.Source: B. Randomly-generated scenarios · p. 6
- H1 recovers a mean 85.5% of the FIFO-to-optimal gap in Batch 1, with a mean 18.8% gap remaining to brute-force optimal.The heuristics stay near optimal costs in most scenarios and below FIFO; H1 has lower or equal cost to H2 across all scenarios.Source: B. Randomly-generated scenarios · p. 6, Figure 6 · p. 7
- H1 requires 7–10 swap evaluations per scenario in Batch 1, compared with 15–19 for H2.H2’s swap count falls in Batch 2 as the conflict structure becomes simpler, while H1’s remains essentially unchanged.Source: B. Randomly-generated scenarios · p. 7, Figure 8 · p. 7, Figure 11 · p. 8
- Wider inter-arrival gaps reduce the mean optimality gap from 18.8% to 3.9%, reflecting lower scheduling complexity.The same gap values are reported for Batch 1 and Batch 2, respectively.Source: B. Randomly-generated scenarios · p. 7, Figure 7 · p. 7, Figure 10 · p. 8
- Across both batches, H1 strictly dominates H2 in computational cost while delivering equal or better solution quality.The persistent swap memory is identified as reducing redundant NLP solves.Source: B. Randomly-generated scenarios · p. 7, Table VII · p. 8
- 87.0% mean optimality gap reduction relative to FIFO and 3.9% mean optimality gap relative to brute-force optimal were achieved in Batch 2.For Batch 1, the corresponding means were 85.5% and 18.8%.Source: Figure 7 · p. 7, Figure 10 · p. 8
C. Continuous arrival evaluation
In 300-second closed-loop simulations of continuous arrivals, the heuristic reduced mean cost relative to FIFO in both traffic densities, with the reduction sustained across streams of several hundred vehicles. Multiplier-ranked swap acceptance was higher in the dense regime.
C. Continuous arrival evaluation
- The 300-second evaluation compared separate FIFO and heuristic closed loops driven by the same arrivals, with cost accumulated and normalized per coordinated vehicle.The experiment used sparse and dense regimes; all 301 and 411 vehicles crossed under both strategies without precedence violations.Source: C. Continuous arrival evaluation · pp. 8–9
- The heuristic reduced mean closed-loop cost relative to FIFO by 51.1% in the sparse regime and 67.4% in the dense regime.The cumulative cost curves diverged from the first arrivals and the gap widened over the horizon.Source: C. Continuous arrival evaluation · p. 9, Figure 13 · p. 9
- Candidate-swap acceptance rates were 52.4% in the sparse regime and 70.1% in the dense regime.The authors interpret the higher dense-regime rate as consistent with larger coordination sets offering more improving reorderings among ranked candidates.Source: C. Continuous arrival evaluation · p. 9
V. CONCLUSION
The paper extends multiplier-guided schedule refinement to arrival-triggered receding-horizon coordination, using repeated continuous NLP solves rather than combinatorial enumeration. The authors report polynomial NLP size and performance between FIFO and the enumerated global optimum, while noting that real-time execution remains a limitation.
V. CONCLUSION
- The arrival-triggered framework appends new vehicles to the current order, warm-starts each NLP from the previous solution, and uses swap memory and precedence-constraint KKT multipliers to focus computation.Candidate swaps are ranked by multipliers, and the memory T prevents redundant NLP solves across replanning instants.Source: §V · p. 9
- The method avoids combinatorial enumeration through repeated continuous NLP solves whose size scales polynomially with vehicle count, and bridges the gap between FIFO and the brute-force global optimum.Source: §V · p. 9
- Real-time execution is not established: the scheme assumes replanning finishes within talg, while each candidate swap requires a full NLP solve whose time grows with vehicle count.The authors suggest bi-level decomposition and quadratic sensitivity approximations as a possible extension.Source: §V · p. 9