Algorithmic Foundations of Robotics XI pp 609-626

Part of the Springer Tracts in Advanced Robotics book series (STAR, volume 107) | Cite as

Stochastic Extended LQR: Optimization-Based Motion Planning Under Uncertainty

Chapter

Abstract

We introduce a noveloptimization-based motion planner, Stochastic Extended LQR (SELQR), which computes a trajectory and associated linear control policy with the objective of minimizing the expected value of a user-defined cost function. SELQR applies to robotic systems that have stochastic non-linear dynamics with motion uncertainty modeled by Gaussian distributions that can be state- and control-dependent. In each iteration, SELQR uses a combination of forward and backward value iteration to estimate the cost-to-come and the cost-to-go for each state along a trajectory. SELQR then locally optimizes each state along the trajectory at each iteration to minimize the expected total cost, which results in smoothed states that are used for dynamics linearization and cost function quadratization. SELQR progressively improves the approximation of the expected total cost, resulting in higher quality plans. For applications with imperfect sensing, we extend SELQR to plan in the robot’s belief space. We show that our iterative approach achieves fast and reliable convergence to high-quality plans in multiple simulated scenarios involving a car-like robot, a quadrotor, and a medical steerable needle performing a liver biopsy procedure.

References

  1. 1.
    Zucker, M., Ratliff, N., Dragan, A.D., Pivtoraiko, M., Matthew, K., Dellin, C.M., Bagnell, J.A., Srinivasa, S.S.: CHOMP: covariant Hamiltonian optimization for motion planning. Int. J. Robot. Res. 32(9), 1164–1193 (2012)Google Scholar
  2. 2.
    Schulman, J., Ho, J., Lee, A., Awwal, I., Bradlow, H., Abbeel, P.: Finding locally optimal, collision-free trajectories with sequential convex optimization. In: Robotics: Science and Systems (RSS) (June 2013)Google Scholar
  3. 3.
    Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P., Schaal, S.: STOMP: stochastic trajectory optimization for motion planning. In: Proceedings of the IEEE International Conference Robotics and Automation (ICRA), May 2011, pp. 4569–4574 (2011)Google Scholar
  4. 4.
    LaValle, S.M.: Planning Algorithms. Cambridge University Press, Cambridge (2006)CrossRefMATHGoogle Scholar
  5. 5.
    Bell, B.M.: The iterated Kalman smoother as a Gauss-Newton method. SIAM J. Optim. 4(3), 626–636 (1994)CrossRefMATHMathSciNetGoogle Scholar
  6. 6.
    Brock, O., Khatib, O.: Elastic strips: a framework for motion generation in human environments. Int. J. Robot. Res. 21(2), 1031–1052 (2002)CrossRefGoogle Scholar
  7. 7.
    Hauser, K., Ng-Thow-Hing, V.: Fast smoothing of manipulator trajectories using optimal bounded-acceleration shortcuts. In: Proceedings of the IEEE International Conference Robotics and Automation (ICRA), May 2010, pp. 2493–2498Google Scholar
  8. 8.
    Pan, J., Zhang, L., Manocha, D.: Collision-free and smooth trajectory computation in cluttered environments. Int. J. Robot. Res. 31(10) 1155–1175 (2012)Google Scholar
  9. 9.
    van den Berg, J.: Extended LQR: locally-optimal feedback control for systems with non-linear dynamics and non-quadratic cost. In: International Symposium on Robotics Research (ISRR), December 2013Google Scholar
  10. 10.
    van den Berg, J.: Iterated LQR smoothing for locally-optimal feedback control of systems with non-linear dynamics and non-quadratic cost. In: Proceedings of the American Control Conference, June 2014Google Scholar
  11. 11.
    Toussaint, M.: Robot trajectory optimization using approximate inference. In: Proceedings of the International Conference on Machine Learning (ICML) (2009)Google Scholar
  12. 12.
    Todorov, E.: A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems. In: Proceedings of the American Control Conference, pp. 300–306 (2005)Google Scholar
  13. 13.
    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)CrossRefMATHMathSciNetGoogle Scholar
  14. 14.
    Pineau, J., Gordon, G., Thrun, S.: Point-based value iteration: an anytime algorithm for POMDPs. In: International Joint Conference on Artificial Intelligence (IJCAI), pp. 1025–1032 (2003)Google Scholar
  15. 15.
    Kurniawati, H., Hsu, D., Lee, W.: SARSOP: efficient point-based POMDP planning by approximating optimally reachable belief spaces. In: Robotics: Science and Systems (RSS) (2008)Google Scholar
  16. 16.
    Bai, H., Hsu, D., Lee, W.S.: Integrated perception and planning in the continuous space: a POMDP approach. In: Robotics: Science and Systems (RSS) (2013)Google Scholar
  17. 17.
    Bry, A., Roy, N.: Rapidly-exploring random belief trees for motion planning under uncertainty. In: Proceedings of the IEEE International Conference Robotics and Automation (ICRA), May 2011, pp. 723–730Google Scholar
  18. 18.
    Prentice, S., Roy, N.: The belief roadmap: efficient planning in belief space by factoring the covariance. Int. J. Robot. Res. 28(11), 1448–1465 (2009)Google Scholar
  19. 19.
    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)Google Scholar
  20. 20.
    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)Google Scholar
  21. 21.
    Platt Jr., R., Tedrake, R., Kaelbling, L., Lozano-Perez, T.: Belief space planning assuming maximum likelihood observations. In: Robotics: Science and Systems (RSS) (2010)Google Scholar
  22. 22.
    Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics. MIT Press, Cambridge (2005)MATHGoogle Scholar
  23. 23.
    Welch, G., Bishop, G.: An introduction to the Kalman filter. Technical Report TR 95-041, University of North Carolina at Chapel Hill, July 2006Google Scholar
  24. 24.
    van den Berg, J., Patil, S., Alterovitz, R., Abbeel, P., Goldberg, K.: LQG-based planning, sensing, and control of steerable needles. In: Hsu, D., Others (eds.) Algorithmic Foundations of Robotics IX (Proceedings of the WAFR 2010). Springer Tracts in Advanced Robotics (STAR), vol. 68, pp. 373–389. Springer, Berlin, December 2010Google Scholar

Copyright information

© Springer International Publishing Switzerland 2015

Authors and Affiliations

  1. 1.University of North Carolina at Chapel HillChapel HillUSA
  2. 2.University of UtahSalt Lake CityUSA

Personalised recommendations