Scaling up Gaussian Belief Space Planning through
Covariance-Free Trajectory Optimization and
Automatic Differentiation

Sachin Patil, Gregory Kahn, Michael Laskey, John Schulman,
Ken Goldberg, Pieter Abbeel

[PDF]


This website supplements our WAFR 2014 submission, in which we present an algorithm for scaling up Gaussian belief space planning through covariance-free trajectory optimization and automatic diffentiation.

Abstract

 Belief space planning provides a principled framework to compute motion plans that explicitly gather information from sensing, as necessary, to reduce uncertainty about the robot and the environment. We consider the problem of planning in Gaussian belief spaces, which are parameterized in terms of mean states and covariances describing the uncertainty. In this work, we show that it is possible to compute locally optimal plans without including the covariance in direct trajectory optimization formulations of the problem. As a result, the dimensionality of the problem scales linearly in the state dimension instead of quadratically, as would be the case if we were to include the covariance in the optimization. We accomplish this by taking advantage of recent advances in numerical optimal control that include automatic differentiation and state of
the art convex solvers. We show that the running time of each optimization step of the covariance-free trajectory optimization is O(n^3T), where n is the dimension of the state space and T is the number of time steps in the trajectory. We present experiments in simulation on a variety of planning problems under uncertainty including manipulator planning, estimating unknown model parameters for dynamical systems, and active simultaneous localization and mapping (active SLAM). Our experiments suggest that our method can solve planning problems in 100 dimensional state spaces and obtain computational speedups of 400 × over related trajectory optimization methods.

Source code

Source code for the tracking algorithm is available on github.

The code is not suitable for public consumption yet in its current form. If you are interested in using it, please send Sachin and Greg an email saying that you would like to use the code and what you plan to do with it, and we will try to help you out.