Stochastic Extended LQR for Optimization-based Motion Planning Under Uncertainty.
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.
Robotics Institute, Carnegie Mellon University, Pittsburgh, PA 15213 USA.
Wen
W
Sun
School of Computing, University of Utah, Salt Lake City, UT 84112 USA.
Jur
J
van den Berg
Department of Computer Science, University of North Carolina at Chapel Hill, Chapel Hill, NC 24799 USA ( ron@cs.unc.edu ).
Ron
R
Alterovitz
motion planning under uncertainty; nonholonomic motion planning