Optimal Trajectory Planning for Autonomous Vehicles in Unstructured Environments

In this letter, we propose a bi-stage optimal trajectory planning method to address the challenges of planning in unstructured environments. In the first stage, we leverage the incremental sampling method, optimal rapidly-exploring random tree star, combined with the Reeds-Shepp curve, to identify a feasible path in complex environments. The generated path is then smoothed using cubic spline interpolation. In the second stage, we address collision avoidance from multiple obstacles by first reducing the obstacle regions and then transforming the non-convex obstacle constraints into convex form through dual reformulation. The formulated optimal control problem is transcribed into a nonlinear programming and solved using a direct method, embedding the smoothed first-stage path as a feasible initial guess to generate optimal trajectories. We demonstrate the applicability of our approach in diverse environments, showing reduced computation time compared to representative methods due to fewer collision evaluations and improved feasibility. Additionally, the generated trajectories account for passenger comfort while minimizing travel time.