Real Time Motion Planning Using Constrained Iterative Linear Quadratic Regulator for On-Road Self-Driving

TL;DR

This paper presents a real-time motion planning algorithm for self-driving cars that uses an iterative LQR approach to avoid collisions by considering traffic behavior uncertainties and maintaining safe distances.

Contribution

It introduces a novel spatiotemporal motion planning method using constrained iLQR that accounts for traffic uncertainties and collision risk modeling.

Findings

Validated in simulation and real-world tests

Achieved efficient collision avoidance in real-time scenarios

Demonstrated robustness to traffic behavior uncertainties

Abstract

Collision avoidance is one of the most challenging tasks people need to consider for developing the self-driving technology. In this paper we propose a new spatiotemporal motion planning algorithm that efficiently solves a constrained nonlinear optimal control problem using the iterative linear quadratic regulator (iLQR), which takes into account the uncertain driving behaviors of the traffic vehicles and minimizes the collision risks between the self-driving vehicle (referred to as the "ego" vehicle) and the traffic vehicles such that the ego vehicle is able to maintain sufficiently large distances to all the surrounding vehicles for achieving the desired collision avoidance maneuver in traffic. To this end, we introduce the concept of the "collision polygon" for computing the minimum distances between the ego vehicle and the traffic vehicles, and provide two different solutions for designing the constraints of the motion planning problem by properly modeling the behaviors of the traffic vehicles in order to evaluate the collision risk. Finally, the iLQR motion planning algorithm is validated in multiple real-time tasks for collision avoidance using both a simulator and a level-3 autonomous driving test platform.