Non-Conservative Trajectory Planning for Automated Vehicles by Estimating Intentions of Dynamic Obstacles

Motion planning algorithms for urban automated driving must handle uncertainty due to unknown intention and future motion of Dynamic Obstacles (DOs). Considering a single future trajectory for each DO is not adequate, especially in urban frameworks where traffic participants exhibit very different behaviors. However, including multiple candidate trajectories representing different behaviors results in an excess of conservatism. We present a novel combination of the Interactive Multiple Model (IMM) algorithm and Stochastic Model Predictive Control (SMPC) that allows non-conservative safe motion planning in presence of DOs with unknown intention. We introduce a framework based on LQR and IMM to predict multiple candidate future trajectories, each interpreted as a high-level intention that the DO is pursuing, and dynamically estimate their probabilities. Then, the future trajectory of the automated vehicle is iteratively planned in an SMPC fashion, in which collision avoidance constraints are generated for multiple future trajectories of each DO, with a focus on the most likely. Our method improves safe motion planning fully exploiting the benefits of multi-modal predictions of the DOs, avoiding excessive conservatism. Advantages of the proposed method are discussed through simulations in the CARLA environment.