Constrained Iterative LQG for Real-Time Chance-Constrained Gaussian Belief Space Planning
This addresses safety-critical planning for autonomous vehicles, but it appears incremental as it builds on existing LQG methods with constraints.
The paper tackles motion planning under uncertainty for autonomous vehicles by formulating it as chance-constrained Gaussian belief space planning and proposes the Constrained Iterative LQG (CILQG) algorithm, which shows faster computation time and better handling of uncertainties compared to baseline methods in simulations.
Motion planning under uncertainty is of significant importance for safety-critical systems such as autonomous vehicles. Such systems have to satisfy necessary constraints (e.g., collision avoidance) with potential uncertainties coming from either disturbed system dynamics or noisy sensor measurements. However, existing motion planning methods cannot efficiently find the robust optimal solutions under general nonlinear and non-convex settings. In this paper, we formulate such problem as chance-constrained Gaussian belief space planning and propose the constrained iterative Linear Quadratic Gaussian (CILQG) algorithm as a real-time solution. In this algorithm, we iteratively calculate a Gaussian approximation of the belief and transform the chance-constraints. We evaluate the effectiveness of our method in simulations of autonomous driving planning tasks with static and dynamic obstacles. Results show that CILQG can handle uncertainties more appropriately and has faster computation time than baseline methods.