A Two-Stage Spatiotemporal Trajectory Optimization Framework for Autonomous Lane Changing With Dynamic Risk Fields

Autonomous vehicles (AVs) face challenges in making accurate decisions and planning optimal trajectories in complex environments. Current methods often overlook future dynamics or are too computationally demanding for real-world use, with many studies limited to simple scenarios. To address these limitations, this paper introduces a two-stage trajectory planning framework that considers safety, efficiency, and ride comfort simultaneously in dynamic environments. The key contribution is a novel Dynamic Risk Field (DRF) that provides a unified representation of static and dynamic obstacles, enabling comprehensive risk assessment without treating them separately. The first stage generates spatially feasible trajectories using quintic polynomials in the Frenet frame, guided by the DRF to assess risks from obstacles. The second stage optimizes these trajectories temporally with Space-Time (ST) graphs and Sequential Quadratic Programming (SQP), ensuring collision avoidance and smooth dynamics. Compared to existing methods, our framework achieves efficient real-time planning through the two-stage decomposition while maintaining trajectory quality. Simulations across highway lane changes, overtaking, and intersection scenarios demonstrate that the framework produces safe, efficient, and comfortable trajectories, outperforming benchmark algorithms in terms of acceleration and jerk metrics.