Autonomous Driving Motion Planning With Constrained Iterative LQR

Motion planning is a core technique for autonomous driving. Nowadays, there still exists a lot of challenges in motion planning for autonomous driving in complicated environments due to: 1) the need of both spatial and temporal planning in highly dynamic environments; 2) nonlinear vehicle dynamic models and non-convex collision avoidance constraints; and 3) the need of high computation efficiency for real-time implementation. Iterative linear quadratic regulator (ILQR) is an algorithm to solve the 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 efficiently solve the optimal control problem with nonlinear system dynamics and general form of constraints. An autonomous driving motion planning problem is then formulated and solved using CILQR. Simulation case studies show the capability of the CILQR algorithm to solve these kind of problems and the computation efficiency of CILQR is shown to be much higher than the standard SQP solver.