Real-time kinodynamic trajectory planning is one of the core requirements for autonomous driving in dynamic environments. Most existing works solve planning problems by sampling-based, searching-based, and optimization-based methods. They may fail to solve the planning problem or need a long time to plan trajectories because of the methods’ properties. In this thesis, through iterative and incremental path-speed optimization, we propose an integrated planning framework with probabilistic inference for continuous-time trajectories.

The framework is divided into three parts with decoupled planning problems to path and speed. Firstly, a path planner based on the Gaussian process generates a continuous arc-length parameterized path in the Fren´et frame, considering static obstacle avoidance...[ Read more ]

Real-time kinodynamic trajectory planning is one of the core requirements for autonomous driving in dynamic environments. Most existing works solve planning problems by sampling-based, searching-based, and optimization-based methods. They may fail to solve the planning problem or need a long time to plan trajectories because of the methods’ properties. In this thesis, through iterative and incremental path-speed optimization, we propose an integrated planning framework with probabilistic inference for continuous-time trajectories.

The framework is divided into three parts with decoupled planning problems to path and speed. Firstly, a path planner based on the Gaussian process generates a continuous arc-length parameterized path in the Fren´et frame, considering static obstacle avoidance and curvature constraints. We theoretically illustrated the reason for the Gaussian process, which can speed up solving time during the optimization process. Additionally, it is a good generalization of the well-known jerk optimal solution. Then, an efficient s-t graph search approach is developed to identify a speed profile along the created path to cope with changing situations. Finally, to guarantee kinodynamic feasibility, the path and speed are optimized incrementally and iteratively. By eliminating the factor graph to the Bayes tree, the whole system could achieve real-time compared to other planning frameworks using the efficient s olver p roposed from t he SLAM problem.

Various simulated scenarios with both static obstacles and dynamic agents verify the effectiveness and robustness of our proposed method.