MOTION PLANNING FOR AUTONOMOUS DRIVING WITH EXTENDED CONSTRAINED ITERATIVE LQR

Autonomous driving planning is a challenging problem when the environment is complicated. It is difficult for the planner to find a good trajectory that navigates autonomous cars safely with crowded surrounding vehicles. To solve this complicated problem, a fast algorithm that generates a high-quality, safe trajectory is necessary. Constrained Iterative Linear Quadratic Regulator (CILQR) is appropriate for this problem, and it successfully generates the required trajectory in real-time. However, CILQR has some deficiencies. Firstly, CILQR uses logarithmic barrier functions for hard constraints, which will cause numerical problems when the initial trajectory is infeasible. Secondly, the convergence speed is slowed with a bad initial trajectory, which might violate the real-time requirements. To address these problems, we propose the extended CILQR by adding two new features. The first one is using relaxed logarithmic barrier functions instead of the standard logarithmic barrier function to prevent numerical issues. The other one is adding an efficient initial trajectory creator to generate a good initial trajectory. Moreover, this initial trajectory helps CILQR to converge to a desired local optimum. These new features extend CILQR's usage to more practical autonomous driving applications. Simulation results show that our algorithm is effective in challenging driving environments.