An Efficient Optimization-Based Two-Stage Trajectory Planning Framework for Autonomous Vehicles on Structured Roads
摘要
This paper focuses on the trajectory planning problem for structured roads, which is formulated as an optimal control problem (OCP) to produce rapid, accurate, and optimal trajectories. However, the standard OCP faces inherent challenges due to its high-dimensional computational complexity. Additionally, based on human driving experience, the speed limits for vehicles passing through special areas (e.g., speed bumps on the road) differ from those in other free areas. This “if-else” structure complicates the OCP problem. To address this issue, a two-stage trajectory optimization framework is proposed. The first phase is the coarse trajectory search stage, which involves making an initial trajectory decision in the Frenet frame, then connecting adjacent decision points, transforming coordinates, and formulating an initial guess. The second phase is the enhanced iterative optimization stage, during which the trajectory planning problem is formulated. During this phase, traditional collision avoidance constraints are replaced by driving corridors generated using an iterative box-generation strategy. In addition, a novel special area constraint is introduced to address the challenges posed by the “if-else” structure, and the trajectory is refined using iterative optimization techniques. The experimental results confirm that the proposed trajectory planning method is both effective and robust.