Motion Planning by Search in Derivative Space and Convex Optimization with Enlarged Solution Space

To efficiently generate safe trajectories for an autonomous vehicle in dynamic environments, a layered motion planning method with decoupled path and speed planning is widely used. This paper studies speed planning, which mainly deals with dynamic obstacle avoidance given a planned path. The main challenges lie in the optimization in a non-convex space and the trade-off between safety, comfort, and efficiency. First, this work proposes to conduct a search in second-order derivative space for generating a comfort-optimal reference trajectory. Second, by combining abstraction and refinement, an algorithm is proposed to construct a convex feasible space for optimization. Finally, a piecewise Bézier polynomial optimization approach with trapezoidal corridors is presented, which theoretically guarantees safety and significantly enlarges the solution space compared with the existing rectangular corridors-based approach. We validate the efficiency and effectiveness of the proposed approach in simulations.