Reliable Multi-Level Optimization for Safe Predictive Control of Autonomous Vehicles to Avoid Uncertain Multimodal PLEVs

Safety assurance using all perceptual information to predict the motion of dynamic agents is critical in urban environments and remains an open challenge. For Autonomous Vehicles (AV) operating around vulnerable road users, the risk assessment strategy often needs to address stochastic uncertainties in the multiple possible trajectories (or multimodal motion) of the surrounding traffic agents. However, this increases the complexity of the navigation problem using the existing planners. To address this issue, this paper presents a multi-level optimization strategy that combines sampling-based and direct optimization methods for decision-making and control with improved safety and trajectory smoothness. In the primary stage, a sampling-based optimization framework systematically identifies safe candidate trajectories by employing the Fusion of stochastic Predictive Inter-Distance Profile (F-sPIDP). F-sPIDP encapsulates the multimodal dynamics of traffic agents and explicitly computes the uncertainties in their estimated or tracked states. From the set of trajectories, a reference optimal trajectory and its F-sPIDP setpoints are selected, adhering to stringent safety constraints and motion smoothness. Subsequently, a secondary local control optimization refines the optimal trajectory to ensure compliance with the AV's kinematic and dynamic constraints while accounting for the quantified uncertainty within the F-sPIDP framework. The performance of the proposed method was assessed through simulations and statistical analyses, evaluating its robustness to diverse levels of uncertainty.