FOAD: Fast Optimization-based Autonomous Driving Motion Planner

Motion planning is one of the core modules for autonomous driving. Among the current motion planning techniques, optimization-based methods have unique advantages since they allow planning in continuous space and they can evaluate multiple objectives (such as hard constraints) in one formulation. However, it is hard to implement optimization-based methods in real-time in complicated environments due to 1) high computational complexity as the optimization problems are usually non-convex; and 2) difficulty to guarantee closed-loop performance because the low level trajectory tracking controller cannot perform perfect tracking. To solve the first challenge, convex feasible set algorithm (CFS) has been proposed for real time non-convex optimization. To solve the second challenge, a fast optimization-based autonomous driving motion planner (FOAD) is proposed in this paper which implements a soft constrained convex feasible set algorithm (SCCFS) as an enhanced version of CFS. The concept of closed-loop smoothness is defined and analyzed in this paper. Simulations and real vehicle experiments verify the efficiency and capability of the planner.