Autonomous Driving Trajectory Optimization With Dual-Loop Iterative Anchoring Path Smoothing and Piecewise-Jerk Speed Optimization

Abstract

This letter presents a free space trajectory optimization algorithm for autonomous driving, which decouples the collision-free trajectory generation problem into a Dual-Loop Iterative Anchoring Path Smoothing (DL-IAPS) problem and a Piecewise-Jerk Speed Optimization (PJSO) problem. The work leads to remarkable driving performance improvements including more robust and precise collision avoidance, higher control feasibility, higher computation efficiency and stricter driving comfort guarantee, compared with other existing algorithms. The advantages of our algorithm are attributed to our fast iterative collision checks with exact vehicle/obstacle shapes, strict non-holonomic dynamic constraints and accurate kinematics-based speed optimization. It has been validated that, through batch simulation and road experiments, compared with prior works, our algorithm is with the highest robustness and capable to maintain the lowest failure rate (~7%) at nearly all test conditions, achieves 10x faster computational speed than other planners, fulfills 100% driving-comfort standards in complex driving scenarios, and does not induce significant time increase as boundaries or obstacles scale up.