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

Real-time kinodynamic trajectory planning in dy-namic environments is critical yet challenging for autonomous driving. In this paper, we propose an efficient trajectory plan-ning system for autonomous driving in complex dynamic sce-narios through iterative and incremental path-speed optimization. Exploiting the decoupled structure of the planning prob-lem, a path planner based on Gaussian process first generates a continuous arc-length parameterized path in the Frené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.