Real-Time Trajectory Planning for Autonomous Driving with Gaussian Process and Incremental Refinement

TL;DR

This paper presents a real-time trajectory planning system for autonomous driving that combines Gaussian process-based path generation with incremental speed optimization, ensuring kinodynamic feasibility in dynamic environments.

Contribution

It introduces a novel iterative and incremental path-speed optimization framework leveraging Gaussian processes and graph search, with theoretical guarantees and real-time performance.

Findings

Operates at 20 Hz in simulated scenarios

Effectively handles static and dynamic obstacles

Ensures kinodynamic feasibility through incremental refinement

Abstract

Real-time kinodynamic trajectory planning in dynamic environments is critical yet challenging for autonomous driving. In this letter, we propose an efficient trajectory planning system for autonomous driving in complex dynamic scenarios through iterative and incremental path-speed optimization. Exploiting the decoupled structure of the planning problem, a path planner based on Gaussian process first generates a continuous arc-length parameterized path in the Fren\'{e}t frame, considering static obstacle avoidance and curvature constraints. We theoretically prove that it is a good generalization of the well-known jerk optimal solution. An efficient s-t graph search method is introduced to find a speed profile along the generated path to deal with dynamic environments. Finally, the path and speed are optimized incrementally and iteratively to ensure kinodynamic feasibility. Various simulated scenarios with both static obstacles and dynamic agents verify the effectiveness and robustness of our proposed method. Experimental results show that our method can run at 20 Hz. The source code is released as an open-source package.