Stochastic Extended LQR: Optimization-Based Motion Planning Under Uncertainty

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.

Two-phase A*: A real-time global motion planning method for non-holonomic unmanned ground vehicles

“…The motion planning problem can also be addressed in the framework of the nonlinear continuous optimization. 13,14 The common optimization-based motion planning approaches include the Euler method, 15 the linear–quadratic regulator (LQR), 16 the Adams approximation, 17 the shooting method, 18 the convex optimization, 19 the conjugate-gradient (CG) descent, 20 and so on. These methods are widely used in nonplanar environments.…”

Speeding up Gaussian Belief Space Planning for Underwater Robots Through a Covariance Upper Bound

“…Rather than generating random paths, the belief trees proposed in [33] solves for a globally optimal solution by utilizing an RRT*-like pruning strategy and constructing a belief tree. Belief space iLQR [18] and B-LQR [34] solve a locally optimal solution based on the gradient descent optimization method by using the decoupled process in an iterative manner. As in our work, they also take state-dependent motion and observation noise into consideration.…”

High-Frequency Replanning Under Uncertainty Using Parallel Sampling-Based Motion Planning

“…These approaches are difficult to scale, and computational costs may prohibit their application to robots navigating using non-discrete controls in uncertain, dynamic environments. Sampling-based approaches that consider uncertainty [1], [4], [13], [26], [37] or approaches that compute a locally-optimal trajectory and an associated control policy (in some cases in belief space) [32], [36], [41], [43], [46] are effective for a variety of scenarios. But these methods are currently not suitable for real-time planning in uncertain, dynamic, and changing environments where during task execution the path may need to change substantially, potentially across different homotopic classes.…”