Trajectory planning for autonomous driving on urban curvy roads: A search-resampling-enhanced iterative optimization framework

Autonomous vehicles face significant challenges in balancing scene applicability, trajectory quality, and computational efficiency when generating rapid, accurate, and optimal trajectories on urban curvy roads. The prevailing approach is to frame this task as an optimal control problem (OCP). However, the nominal OCP still faces problems due to its high-dimensional complexity. Although replacing large-scale collision avoidance constraints with driving corridor constraints reduces computational complexity, it inevitably sacrifices part of the free space and affects trajectory optimality. To address this issue, A Search-Resampling-Enhanced Iterative Optimization (SREIO) framework is proposed in this paper. The framework consists of three main stages: (1) Trajectory Search and Decision-Making, (2) Resampling Process, and (3) Enhanced Iterative Optimization. In the first stage, the dynamic programming (DP) algorithm is employed to find a coarse trajectory on three-dimensional space of S-L-T, defined by the longitudinal direction along the lane centerline (S), the lateral direction perpendicular to the lane centerline (L), and time (T). In the second stage, adjacent decision points are connected using a series of quintic polynomials and resampled along the resulting curve. After transforming the resampled trajectory from the Frenet frame to the Cartesian frame, it serves as the initial guess for the enhanced iterative optimization stage. In the third stage, a novel safe driving corridor (SDC) constraint is designed, which ensures collision avoidance by keeping the ego vehicle confined within the SDC. In addition, an intermediate OCP is constructed in each iteration process, where the current SDC is built based on the SDC from the previous iteration. Both simulation and real-vehicle experiments validate that the SREIO method exhibits significant advantages in scene applicability, trajectory quality, and computational efficiency. Notably, higher trajectory quality means safer and smoother planned trajectories.