  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.
