Contingency-Aware Spatiotemporal Optimization for Safe Autonomous Vehicle Trajectory Planning

Autonomous lane changing requires balancing safety, comfort, and efficiency while managing complex spatiotemporal vehicle interactions. Current methods often separate risk assessment from trajectory planning, leading to either conservative or unsafe maneuvers. This paper presents a contingency-aware spatiotemporal optimization framework that integrates dynamic risk assessment and trajectory optimization to ensure the autonomous host vehicle (HV) achieve safer, more efficient lane changes. First, the HV uses a dynamic risk field method to assess the collision risk with surrounding vehicles (SVs) in real-time, integrating dynamic obstacle interactions through modified Gaussian distributions. Second, a spatiotemporal safety corridor construction scheme leverages regression-based boundaries to transform spatiotemporal requirements into manageable optimization constraints. Third, the HV adopts a contingency-aware model predictive control framework that incorporates SVs uncertainty for human-like lane changes. The formulated optimization problem is solved using sequential quadratic programming with stability and recursive feasibility. Simulations confirm that our approach ensures safety and comfort of the HV across lane changing scenarios, achieving smoother trajectories, improved stability, and enhanced safety margins, with up to 95% reductions in longitudinal and lateral accelerations and a 27% decrease in lane-changing time.