Constrained Iterative LQG for Real-Time Chance-Constrained Gaussian Belief Space Planning

TL;DR

This paper introduces CILQG, a real-time algorithm for motion planning under uncertainty that efficiently handles chance constraints in Gaussian belief space, improving safety and computational speed for autonomous driving.

Contribution

The paper presents the constrained iterative LQG (CILQG) algorithm, a novel real-time method for chance-constrained Gaussian belief space planning in nonlinear, non-convex systems.

Findings

CILQG effectively manages uncertainties in autonomous driving scenarios.

CILQG achieves faster computation times compared to baseline methods.

CILQG demonstrates improved handling of chance constraints in simulations.

Abstract

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.