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

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. In this letter, 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. The proposed method produces safe inputs with an adequate level of conservatism. Our algorithm demonstrated a reduced collision rate and computation time in autonomous driving simulations through improved state estimation.