Multimodal Covariance Steering in Belief Space with Active Probing and Influence for Autonomous Driving

TL;DR

The paper tackles safe, efficient autonomous driving under multimodal human behavior by integrating a hierarchical belief model, active probing, and tail-risk aware control. It represents human intent as discrete intents $i$ with motion modes $k$, updated via Bayesian inference using likelihoods $\\ell_{i,k}(t)$, posteriors $\\pi_i(t+1)$, and mode weights $w_{i,k}(t+1)$, enabling coarse-to-fine reasoning. It adds active probing to minimize $J_{probe}(u) = -\\lambda_H E[H(\\pi(t+1), w(t+1))]$ and a covariance-steering planner that minimizes $J(u)=J_{probe}(u)+J_{influence}(u)$ with $J_{influence}(u)=D_{KL}(\\mathcal{N}(\\mu(t),\\Sigma(t)) \\| \\mathcal{N}(\\mu_{i,k},\\Sigma_{i,k}))$ under dynamics $\\mu(t+\\tau+1)=A\\mu(t\\+\\tau)+B u(t\\+\\tau)$ and $\\Sigma(t\\+\\tau+1)=A\\Sigma(t\\+\\tau)A^\\top+BWB^\\top$, plus CVaR constraints $\\text{CVaR}_\\alpha(i,k) \\le ar J$. A bi-directional loop (Perceive-Infer-Decide-Propagate) dynamically balances exploration and exploitation. Experiments in lane merging and unsignaled intersections show higher success rates (e.g., 96% and 94%), shorter completion times, closer but safe gaps, and smoother maneuvers compared with baselines, validating the benefits of coupling belief updates, probing, and tail-risk-aware planning for interactive driving.

Abstract

Autonomous driving in complex traffic requires reasoning under uncertainty. Common approaches rely on prediction-based planning or risk-aware control, but these are typically treated in isolation, limiting their ability to capture the coupled nature of action and inference in interactive settings. This gap becomes especially critical in uncertain scenarios, where simply reacting to predictions can lead to unsafe maneuvers or overly conservative behavior. Our central insight is that safe interaction requires not only estimating human behavior but also shaping it when ambiguity poses risks. To this end, we introduce a hierarchical belief model that structures human behavior across coarse discrete intents and fine motion modes, updated via Bayesian inference for interpretable multi-resolution reasoning. On top of this, we develop an active probing strategy that identifies when multimodal ambiguity in human predictions may compromise safety and plans disambiguating actions that both reveal intent and gently steer human decisions toward safer outcomes. Finally, a runtime risk-evaluation layer based on Conditional Value-at-Risk (CVaR) ensures that all probing actions remain within human risk tolerance during influence. Our simulations in lane-merging and unsignaled intersection scenarios demonstrate that our approach achieves higher success rates and shorter completion times compared to existing methods. These results highlight the benefit of coupling belief inference, probing, and risk monitoring, yielding a principled and interpretable framework for planning under uncertainty.

Figures (3)

• Figure 1: Overview of the lane-merging interaction between an autonomous ego vehicle and a human-driven vehicle, illustrating the evolution of hierarchical belief updates, CVaR-based risk assessment, control effort, and resulting trajectories from $t_1$ to $t_5$. The progression shows how active probing and Bayesian updates refine intent inference from uncertain to cooperative, while control effort adapts to balance safety and efficiency. Specifically, the hierarchical belief update displays Bayesian updates integrating noisy observations and probing-induced responses, evolving from a uniform intent distribution at $t_{1}$ to a concentrated cooperative belief at $t_{5}$. Instead of passively observing, the active probing controller at $t_{2}$ demonstrates how probing actions intentionally change the interaction and belief state to reveal latent human intentions. The CVaR risk assessment curve illustrates how risk-aware control adjusts the vehicle's aggressiveness and conservatism during the interaction by first rising and then falling over time steps. Lastly, the model's ability to dynamically combine efficiency and safety is demonstrated by the way the adaptive control effort changes in response to inferred risk and intent certainty.
• Figure 2: Unsignaled four-way intersection. The ego shapes the crossing agent’s belief, leading it to yield.
• Figure 3: Trajectory comparison across methods for the same lane merge scenario. AP-IH (brown), CC-MPC (pink), and AP-MP (blue) manage to merge but require longer times or more conservative maneuvers, while our method (yellow) merges more quickly and with smoother motion, reflecting the efficiency and risk-aware confidence of our planner.