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.

I. Introduction

Motion Planning is one of the most important modules in autonomous driving. A typical motion planner for autonomous vehicles takes into account the information from the environment (e.g. motions of the host vehicle and the surrounding vehicles), and plans a trajectory for the vehicle to execute. Although motion planning has been investigated for decades, it is still challenging to design a motion planner for autonomous vehicles satisfying the following requirements simultaneously: (1) The computation needs to be in real time to interact with the dynamic environment and deal with emergencies. (2) The planning needs to be spatiotemporal to deal with dynamic obstacles. (3) The planner needs to be able to deal with complex constraints such as collision avoidance.