Addressing Behavior Model Inaccuracies for Safe Motion Control in Uncertain Dynamic Environments

TL;DR

The paper addresses safety under environmental and behavioral model uncertainties by jointly estimating obstacle state and the input gap through SSIE and by applying distributionally robust MPC (DR-MPC) with a confidence-based ambiguity set. The SIED-MPC framework yields unbiased obstacle state estimates, adaptive conservatism via a dynamic Wasserstein radius, and a tractable optimization reformulation that improves collision avoidance in autonomous driving simulations. Key contributions include a closed-form DR-CVaR bound, a Mahalanobis-inspired confidence metric to tune ambiguity, and demonstration of zero collisions with competitive computation times in CARLA. This approach advances safe motion control in uncertain dynamic environments by tightly integrating estimation robustness with distributional risk controls.

Abstract

Uncertainties in the environment and behavior model inaccuracies compromise the state estimation of a dynamic obstacle and its trajectory predictions, introducing biases in estimation and shifts in predictive distributions. Addressing these challenges is crucial to safely control an autonomous system. In this paper, we propose a novel algorithm SIED-MPC, which synergistically integrates Simultaneous State and Input Estimation (SSIE) and Distributionally Robust Model Predictive Control (DR-MPC) using model confidence evaluation. The SSIE process produces unbiased state estimates and optimal input gap estimates to assess the confidence of the behavior model, defining the ambiguity radius for DR-MPC to handle predictive distribution shifts. This systematic confidence evaluation leads to producing safe inputs with an adequate level of conservatism. Our algorithm demonstrated a reduced collision rate in autonomous driving simulations through improved state estimation, with a 54% shorter average computation time.

Figures (6)

• Figure 1: Illustration of estimation bias and compounding predictive distribution shift induced by the behavior gap ${\hat{\pi}} - \pi$. Even small estimation error and behavior gap of an obstacle can result in large shifts in its predictive distributions. An unsafe trajectory can be predicted as safe.
• Figure 2: (Scenario description) The goal of the ego vehicle (red) is to safely turn left at the intersection without colliding with the obstacle vehicle (white). While the behavior model predicts the obstacle to adhere to the CSAV model, the actual obstacle unexpectedly steers to the left.
• Figure 3: SSIE makes accurate estimations across all variables, whereas EKF makes errors during $t\in[20,80]$ when the input estimations deviate from the behavior model predictions. $x,y$ are measured in $m$, $v$ in $m/s$, and the heading angle $\phi$ in degrees.
• Figure 4: The CSAV model relatively predicts acceleration accurately, but not the slip angle. SSIE effectively tracks these deviations, as reflected in the dynamic adjustments of the Wasserstein radius $\theta$.
• Figure 5: Mean-MPC failed to avoid collision in all episodes, whereas DR-MPC and our method achieved $75\%$ and $100\%$, respectively. To compare computational efficiency, computation times are normalized to that of Mean-MPC. The Mean-MPC had the shortest average computation time. Our algorithm required an additional $40\%$ computation time on average, while DR-MPC's computation time exceeded Mean-MPC's by $150\%$.

Theorems & Definitions (10)

• Proposition III.1
• proof
• Proposition III.2
• proof
• Proposition III.3
• proof
• Definition 1
• Lemma IV.1
• Theorem IV.2
• proof