Dynamic Risk-Aware Trajectory Planning for Autonomous Vehicles: A Two-Stage Optimization with Space-Time Graphs

Introduction

This paper introduces a two-stage trajectory planning framework that considers safety, efficiency, and ride comfort simultaneously in dynamic environments. The first stage generates spatially feasible trajectories using quintic polynomials and a Dynamic Risk Field (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. Simulations show the framework produces safe, efficient, and comfortable trajectories in challenging traffic scenarios including highway lane changes and intersections.

Performance Under Different Traffic scenarios

To validate our risk-Aware trajectory planning framework, we tested its adaptability across various scenarios while balancing safety, efficiency, and comfort.

Case 1: Lane changing on two-lane highway

Case 2: Overtaking on a two-lane, two-way road.

Conclusion

This paper presents a novel two-stage trajectory planning for AVs in dynamic environments. Key contributions include a DRF for unified representation of static and dynamic obstacles, a two-stage planning combining spatial trajectory generation with temporal optimization, an SQP-based efficient trajectory optimization method, and a multi-objective cost function balancing safety, efficiency, and comfort. A receding horizon strategy is also implemented for continuous replanning. Simulation results across three scenarios demonstrate the framework's effectiveness in generating safe, efficient, and comfortable trajectories, highlighting its potential to balance competing objectives and adapt to complex driving environments. Future work will focus on integrating learning-based methods for enhanced obstacle prediction.