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

TL;DR

This paper proposes novel disc-type swept volume (SV), incremental path flattening (IPF), and kinodynamic feasibility penalty methods, and demonstrates that the method outperforms state-of-the-art baselines in various simulated environments.

Abstract

Gradient-based trajectory optimization with B-spline curves is widely used for unmanned aerial vehicles (UAVs) due to its fast convergence and continuous trajectory generation. However, the application of B-spline curves for path-velocity coupled trajectory planning in autonomous vehicles (AVs) has been highly limited because it is challenging to reduce the over-approximation of the vehicle shape and to create a collision-free trajectory using B-spline curves while satisfying kinodynamic constraints. To address these challenges, this paper proposes novel disc-type swept volume (SV), incremental path flattening (IPF), and kinodynamic feasibility penalty methods. The disc-type SV estimation method is a new technique to reduce SV over-approximation and is used to find collision points for IPF. In IPF, the collision points are used to push the trajectory away from obstacles and to iteratively increase the curvature weight, thereby reducing SV and generating a collision-free trajectory. Additionally, to satisfy kinodynamic constraints for AVs using B-spline curves, we apply a clamped B-spline curvature penalty along with longitudinal and lateral velocity and acceleration penalties. Our experimental results demonstrate that our method outperforms state-of-the-art baselines in various simulated environments. We also conducted a real-world experiment using an AV, and our results validate the simulated tracking performance of the proposed approach.

Figures (18)

• Figure 1: Proposed algorithm. The algorithm can generate an optimized trajectory (red) that allows the vehicle to pass through a narrow corridor based on a reference path (hybrid $\text{A}^*$, green). The footprint (yellow) of the vehicle box inside of the safe area (blue) generated by the proposed sv estimation method shows that the proposed algorithm can generate a safe, tractable trajectory. The simulated video and more examples can be found at https://youtu.be/iRCl1vtn5dk.
• Figure 2: Collision penalty: (a) collision obstacle, (b) close obstacle. For the collision obstacle trajectory $\mathbf{\Phi}$, the anchor point $\mathbf{p}_{ij}$ is the closest point at the obstacle surface from the line between the corresponding control points $\mathbf{Q}_i$ and $\mathbf{Q}^{ref}_i$, and the direction vector $\mathbf{v}_{ij}$ is a unit vector pointing toward $\mathbf{p}_{ij}$ from $\mathbf{Q}_i$. For the close-obstacle trajectory $\mathbf{\Phi}$, which is within a safe clearance $s_f$, the anchor point $\mathbf{p}_{ij}$ is the closest point from the corresponding control point $\mathbf{Q}_i$, and the direction vector $\mathbf{v}_{ij}$ is pointing toward $\mathbf{Q}_i$ from $\mathbf{p}_{ij}$.
• Figure 3: ipf process. (a) No collision occurs along the optimized trajectory, which has a disc radius $\textsc{R}_\textsc{CIRCLE}$. (b) The optimized trajectory has a collision, as determined via sv collision checking, at a time knot $t_m$. (c) The flattening control points $\mathbf{Q}_i, i \in \mathbf{\Omega}^{fl}$ are found. (d) A collision-free path is found via ipf.
• Figure 4: Trajectory optimization result with double narrow corridors and multiple curves. A reference trajectory is generated using the hybrid $\text{A}^*$ algorithm. The zoomed-in images show the optimizing path in each iteration. The planned boxes are generated at every $0.1s$.
• Figure 5: Evaluation of sv estimation: (a) C-sv discs and planned vehicle boxes, (b) safe area from svs (d-sv, c-sv) and actual vehicle's footprint. In (a), the c-sv discs (orange) cover the outermost edge of the planned vehicle boxes (gray) at every time interval $\Delta t$ around the maximum curvature ($\approx \kappa_\text{max}$) of the path. In (b), the safe area (blue) generated using the proposed sv estimation method can cover the footprint (yellow) of the vehicle box, which is the actual sv of the vehicle.