Abstract
Motion planning for automated vehicles (AVs) in mixed traffic, where AVs share the road with human-driven vehicles, is a challenging task. To reduce the complexity, state-of-the-art planning approaches often assume that the future motion of surrounding vehicles can be predicted independently of the AV’s plan. This separation can lead to suboptimal, overly conservative behavior especially in highly interactive traffic situations. In this work, we introduce a motion planning algorithm to generate interaction-aware behavior for highly interactive scenarios. The presented algorithm is based upon a reformulation of a bi-level optimization problem, which frames interactions between a human driver and a AV as a Stackelberg game. In contrast to existing works, the algorithm can account for general nonlinear state and input constraints. Further, we introduce mechanisms to integrate cooperation and courtesy into motion planning to prevent overly aggressive driving behavior.
The Challenge: Beyond "Predict-then-Plan"
Most current AV planners follow a predict-then-plan scheme: they predict where other cars will go and then find a path around those predicted trajectories. In dense or highly interactive traffic, such as merging onto a busy highway, this often leads to the "Frozen Robot Problem." Because the planner assumes other drivers' paths are fixed, it may conclude that no safe path exists and simply stop. In contrast, a human driver would "nudge" into traffic, anticipating that others will slow down to accommodate them. The challenge lies in modeling this mutual influence mathematically without compromising real-time performance or creating "bully" behavior.
Methodology: The Stackelberg Game
The proposed method frames the interaction as a bi-level optimization problem using game theory. The AV is designated as the "Leader" and the human driver (HD) as the "Follower."
In this Stackelberg game, the AV chooses a trajectory first, and the HD optimizes their own path as a "best response" to the AV’s move. To make this computationally feasible for real-time use, the authors replace the human's optimization problem with its Karush-Kuhn-Tucker (KKT) conditions. This transforms the complex nested problem into a single-level Nonlinear Program (NLP) with complementarity constraints.
Unlike previous approaches, this method accounts for general nonlinear constraints, including vehicle dynamics and collision avoidance using superellipses, which provide a more accurate geometric representation of vehicles than standard circles.
Courtesy and Cooperation
To prevent the AV from exploiting the human's predictable reaction (e.g., forcing a merge because it "knows" the human will brake), the authors introduced two key mechanisms:
- Cooperative Cost: A weighted sum of both the AV's and the HD's objectives.
- Courtesy Constraints: Hard limits on the maximum deceleration the AV is allowed to impose on the human driver.
Key Insights and Results
The algorithm was evaluated in tailored simulation environments focusing on multi-lane straight roads and merging scenarios.
Deliberate Influence
The AV successfully demonstrated the ability to "push" a human driver into an adjacent lane or encourage them to slow down by positioning itself strategically. This interactive behavior emerges naturally from the efficiency goals of the optimizer; an AV that anticipates a human's reaction can plan smoother, faster maneuvers than one treating humans as static obstacles.
Real-Time Performance
Despite the complexity of the game-theoretic model, the solver achieved mean solve times between 83ms and 96ms on a standard laptop CPU. This performance is well within the requirements for real-time Model Predictive Control (MPC).
Behavioral Tuning
By adjusting a single parameter ($\alpha$), the AV's "personality" can be tuned from overly aggressive to overly conservative, allowing for flexible deployment across different driving cultures or safety requirements.
Limitations and Future Work
While promising, the current model assumes an "Attentive Human" who acts rationally to optimize their own cost function. Real-world drivers may be distracted or unpredictable. Future work involves pairing this system with an intention estimation module to determine if a human is actually responding to the AV's maneuvers. Additionally, while the Python/CasADi implementation is fast, a high-performance C++ implementation is expected to further reduce solve times for even more complex scenarios.
Summary updated