Safe and Efficient Trajectory Optimization for Autonomous Vehicles using B-spline with Incremental Path Flattening

B-spline-based trajectory optimization has been widely used in the field of robot navigation, as the convex hull property of the B-spline curve guarantees its dynamical feasibility with a small number of control variables. Several recent works demonstrated that a holonomic system like a drone, which has simple dynamical feasibility constraints, fully utilizes the B-spline property for trajectory optimization. Nevertheless, it is still challenging to leverage the B-splined-based optimization algorithm to generate a collision-free trajectory for autonomous vehicles because their complex vehicle kinodynamic constraints make it difficult to use the convex hull property. In this paper, we propose a novel incremental path flattening method with a new swept volume method that enables a B-splined-based trajectory optimization algorithm to incorporate vehicle kinematic collision avoidance constraints. Furthermore, a curvature constraint is added with other feasibility constraints (e.g. velocity and acceleration) for the vehicle kinodynamic constraints. Our experimental results demonstrate that our method outperforms state-of-the-art baselines in various simulated environments and verifies its valid tracking performance with an autonomous vehicle in a real-world scenario.