Constrained Iterative LQR for On-Road Autonomous Driving Motion Planning

There exist a lot of challenges in trajectory planning for autonomous driving: 1) Needs of both spatial and temporal planning for highly dynamic environments; 2) Nonlinear vehicle models and non-convex collision avoidance constraints. 3) High computational efficiency for real-time implementation. Iterative Linear Quadratic Regulator (ILQR) is an algorithm which solves predictive optimal control problem with nonlinear system very efficiently. However, it can not deal with constraints. In this paper, the Constrained Iterative LQR (CILQR) is proposed to handle the constraints in ILQR. Then an on road driving problem is formulated. Simulation case studies show the capability of the CILQR algorithm to solve the on road driving motion planning problem.