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\times \) over related trajectory optimization methods .
This is a preview of subscription content, log in via an institution.
Buying options
Tax calculation will be finalised at checkout
Purchases are for personal use only
Learn about institutional subscriptionsReferences
Agha-mohammadi, A., Chakravorty, S., Amato, N.M.: FIRM: sampling-based feedback motion planning under motion uncertainty and imperfect measurements. Int. J. Robot. Res. 33(2), 268–304 (2014)
Andersson, J., Åkesson, J., Diehl, M.: CasADi: a symbolic package for automatic differentiation and optimal control. In: Recent Advances in Algorithmic Differentiation, pp. 297–307. Springer (2012)
Bergstra, J.: Theano: a CPU and GPU math expression compiler. http://deeplearning.net/software/theano/ (2011)
Bertsekas, D.: Dynamic Programming and Optimal Control. Athena Scientific (2001)
Betts, J.T.: Practical Methods for Optimal Control and Estimation Using Nonlinear Programming. SIAM, Philadelphia (2010)
Bry, A., Roy, N.: Rapidly-exploring random belief trees for motion planning under uncertainty. In: Proceedings of IEEE International Conference Robotics and Automation (ICRA), pp. 723–730 (2011)
Camacho, E.F., Bordons, C.: Model Predictive Control. Springer, London (2004)
Dallaire, P., Besse, C., Ross, S., Chaib-draa, B.: Bayesian Reinforcement learning in continuous POMDPs with Gaussian processes. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 2604–2609 (2009)
Deisenroth, M., Mchutchon, A., Hall, J., Rasmussen, C.E.: PILCO policy search framework. http://mloss.org/software/view/508/ (2013)
Deisenroth, M.P., Peters, J.: Solving nonlinear continuous state-action-observation POMDPs for mechanical systems with Gaussian noise. In: European Workshop on Reinforcement Learning (EWRL 2012) (2013)
Diehl, M.: Numerical optimal control. http://homes.esat.kuleuven.be/mdiehl/TRENTO/numopticon.pdf (2011)
Domahidi, A., Zgraggen, A., Zeilinger, M., Morari, M., Jones, C.: Efficient interior point methods for multistage problems arising in receding horizon control. In: IEEE Conference on Decision and Control (CDC), pp. 668–674 (2012)
Domahidi, A.: FORCES: Fast optimization for real-time control on embedded systems. http://forces.ethz.ch (2012)
Erez, T., Smart, W.D.: A scalable method for solving high-dimensional continuous POMDPs using local approximation. In: Conference on Uncertainty in Artificial Intelligence, pp. 160–167 (2010)
Griewank, A., Walther, A.: Evaluating Derivatives: Principles and Techniques of Algorithmic Differentiation. SIAM, Philadelphia (2008)
Hauser, K.: Randomized belief-space replanning in partially-observable continuous spaces. In: Algorithmic Foundations of Robotics IX, pp. 193–209. Springer (2011)
Hollinger, G., Sukhatme, G.: Stochastic motion planning for robotic information gathering. In: Robotics: Science and Systems (RSS) (2013)
Indelman, V., Carlone, L., Dellaert, F.: Towards planning in generalized belief space. In: International Symposium on Robotics Research (ISRR) (2013)
Kaelbling, L.P., Littman, M.L., Cassandra, A.R.: Planning and acting in partially observable stochastic domains. Artif. Intell. 101(1–2), 99–134 (1998)
Kaelbling, L.P., Lozano-Pérez, T.: Integrated task and motion planning in belief space. Int. J. Robot. Res. 32(9–10), 1194–1227 (2013)
Kontitsis, M., Theodorou, E.A., Todorov, E.: Multi-robot active SLAM with relative entropy optimization. In: Proceedings of the American Control Conference (ACC), pp. 2757–2764 (2013)
Kurniawati, H., Bandyopadhyay, T., Patrikalakis, N.M.: Global motion planning under uncertain motion, sensing, and environment map. Auton. Robot. 33(3), 255–272 (2012)
Leung, C., Huang, S., Kwok, N.: Planning under uncertainty using model predictive control for information gathering. Robot. Auton. Syst. 54(11), 898–910 (2006)
Nocedal, J., Wright, S.: Numerical Optimization. Springer, New York (1999)
Papadimitriou, C.J.T.: The complexity of Markov decision processes. Math. Oper. Res. 12(3), 441–450 (1987)
Patil, S., Duan, Y., Schulman, J., Goldberg, K., Abbeel, P.: Gaussian belief space planning with discontinuities in sensing domains. In: Proceedings of the IEEE International Conference Robotics and Automation (ICRA) (2014)
Platt, R., Tedrake, R., Kaelbling, L., Lozano-Perez, T.: Belief space planning assuming maximum likelihood observations. In: Robotics: Science and Systems (RSS) (2010)
Porta, J., Vlassis, N., Spaan, M., Poupart, P.: Point-based Value iteration for continuous POMDPs. J. Mach. Learn. Res. 7, 2329–2367 (2006)
Prentice, S., Roy, N.: The belief roadmap: efficient planning in belief space by factoring the covariance. Int. J. Robot. Res. 28(11–12), 1448–1465 (2009)
Schulman, J., Ho, J., Lee, A., Bradlow, H., Awwal, I., Abbeel, P.: Finding locally optimal, collision-free trajectories with sequential convex optimization. In: Robotics: Science and Systems (RSS) (2013)
Sommer, H., Pradalier, C., Furgale, P.: Automatic differentiation on differentiable manifolds as a tool for robotics. In: International Symposium on Robotics Research (ISRR) (2013)
Stachniss, C., Grisetti, G., Burgard, W.: Information gain-based exploration using Rao-Blackwellized particle filters. In: Proceedings of the Robotics: Science and Systems (RSS), vol. 2 (2005)
Valencia, R., Morta, M., Andrade-Cetto, J., Porta, J.M.: Planning reliable paths with Pose SLAM. IEEE Trans. Robot. 29(4), 1050–1059 (2013)
van den Berg, J., Patil, S., Alterovitz, R.: Efficient approximate value iteration for continuous Gaussian POMDPs. In: Proceedings of the AAAI Conference on Artificial Intelligence (2012)
van den Berg, J., Abbeel, P., Goldberg, K.: LQG-MP: optimized path planning for robots with motion uncertainty and imperfect state information. Int. J. Robot. Res. 30(7), 895–913 (2011)
van den Berg, J., Patil, S., Alterovitz, R.: Motion planning under uncertainty using iterative local optimization in belief space. Int. J. Robot. Res. 31(11), 1263–1278 (2012)
Vitus, M.P., Tomlin, C.J.: Closed-Loop belief space planning for linear, Gaussian systems. In: Proceedings of the IEEE International Conference Robotics and Automation (ICRA), pp. 2152–2159 (2011)
Webb, D.J., Crandall, K.L., van den Berg, J.: Online parameter estimation via real-time replanning of continuous Gaussian POMDPs (2013)
Acknowledgments
This research has been funded in part by AFOSR-YIP Award #FA9550-12-1-0345, by NSF under award IIS-1227536, by a DARPA Young Faculty Award #D13AP00046, CITRIS Seed Grant, and by a Sloan Fellowship. Michael Laskey has been funded by an NSF Graduate Research Fellowship.
Author information
Authors and Affiliations
Corresponding author
Editor information
Editors and Affiliations
Rights and permissions
Copyright information
© 2015 Springer International Publishing Switzerland
About this chapter
Cite this chapter
Patil, S., Kahn, G., Laskey, M., Schulman, J., Goldberg, K., Abbeel, P. (2015). Scaling up Gaussian Belief Space Planning Through Covariance-Free Trajectory Optimization and Automatic Differentiation. In: Akin, H., Amato, N., Isler, V., van der Stappen, A. (eds) Algorithmic Foundations of Robotics XI. Springer Tracts in Advanced Robotics, vol 107. Springer, Cham. https://doi.org/10.1007/978-3-319-16595-0_30
Download citation
DOI: https://doi.org/10.1007/978-3-319-16595-0_30
Published:
Publisher Name: Springer, Cham
Print ISBN: 978-3-319-16594-3
Online ISBN: 978-3-319-16595-0
eBook Packages: EngineeringEngineering (R0)