Abstract
We introduce a novel optimization-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.
Access this chapter
Tax calculation will be finalised at checkout
Purchases are for personal use only
Similar content being viewed by others
References
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)
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)
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)
LaValle, S.M.: Planning Algorithms. Cambridge University Press, Cambridge (2006)
Bell, B.M.: The iterated Kalman smoother as a Gauss-Newton method. SIAM J. Optim. 4(3), 626–636 (1994)
Brock, O., Khatib, O.: Elastic strips: a framework for motion generation in human environments. Int. J. Robot. Res. 21(2), 1031–1052 (2002)
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–2498
Pan, J., Zhang, L., Manocha, D.: Collision-free and smooth trajectory computation in cluttered environments. Int. J. Robot. Res. 31(10) 1155–1175 (2012)
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 2013
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 2014
Toussaint, M.: Robot trajectory optimization using approximate inference. In: Proceedings of the International Conference on Machine Learning (ICML) (2009)
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)
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)
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)
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)
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)
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–730
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)
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)
Platt Jr., R., Tedrake, R., Kaelbling, L., Lozano-Perez, T.: Belief space planning assuming maximum likelihood observations. In: Robotics: Science and Systems (RSS) (2010)
Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics. MIT Press, Cambridge (2005)
Welch, G., Bishop, G.: An introduction to the Kalman filter. Technical Report TR 95-041, University of North Carolina at Chapel Hill, July 2006
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 2010
Acknowledgments
This research was supported in part by the National Science Foundation (NSF) under awards IIS-1117127 and IIS-1149965 and by the National Institutes of Health (NIH) under award R21EB017952.
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
Sun, W., van den Berg, J., Alterovitz, R. (2015). Stochastic Extended LQR: Optimization-Based Motion Planning Under Uncertainty. 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_35
Download citation
DOI: https://doi.org/10.1007/978-3-319-16595-0_35
Published:
Publisher Name: Springer, Cham
Print ISBN: 978-3-319-16594-3
Online ISBN: 978-3-319-16595-0
eBook Packages: EngineeringEngineering (R0)