Motion planning under uncertainty using iterative local optimization in belief space

Abstract

We present a new approach to motion planning under sensing and motion uncertainty by computing a locally optimal solution to a continuous partially observable Markov decision process (POMDP). Our approach represent beliefs (the distributions of the robot's state estimate) by Gaussian distributions and is applicable to robot systems with non-linear dynamics and observation models. The method follows the general POMDP solution framework in which we approximate the belief dynamics using an extended Kalman filter and represent the value function by a quadratic function that is valid in the vicinity of a nominal trajectory through belief space. Using a belief space variant of iterative LQG (iLQG), our approach iterates with secondorder convergence towards a linear control policy over the belief space that is locally optimal with respect to a user-defined cost function. Unlike previous work, our approach does not assume maximum-likelihood observations, does not assume fixed estimator or control gains, takes into account obstacles in the environment, and does not require discretization of the state and action spaces. The running time of the algorithm is polynomial (O[n 6 ]) in the dimension n of the state space. We demonstrate the potential of our approach in simulation for holonomic and nonholonomic robots maneuvering through environments with obstacles with noisy and partial sensing and with non-linear dynamics and observation models.

Safe Motion Planning for Imprecise Robotic Manipulators by Minimizing Probability of Collision

“…Another class of approaches rely on sampling-based methods to compute a path and then compute an LQG feedback controller to follow that path [26,4,21,1]. Other approaches compute a locally-optimal trajectory and an associated control policy [19,30,25]. Recent work has begun to investigate computing plans for manipulators that are robust to uncertainty using local optimization [15], but place restrictions on robot geometry and do not accurately estimate probability of collision.…”

“…The general POMDP formulation enables such information gathering. However, this typically comes at the cost of additional computational complexity [14] or the ability to only compute locally optimal rather than globally optimal plans [25]. Although in this paper we address a broad class of problems, the use of sampling-based motion planning in our approach does place restrictions, e.g., the optimal plan must be goal-oriented (i.e., optimality is not guaranteed for problems that require returning to previously explored regions of the state space for information gathering).…”

“…Extensive prior work has investigated integrated motion planning and control under uncertainty for robots that can be approximated as points in the workspace (e.g., [26,25,19,4]), but robotic manipulators raise new challenges. Whereas for point robots analytically estimating probability of collisions is facilitated by the fact that the robot's configuration space and workspace share parameters, for manipulators the configuration space and workspace are disjoint.…”

Dual execution of optimized contact interaction trajectories

“…Interestingly, Hsiao et al [14] also explicitly consider the execution problem-but given a complete optimized belief plan. More recently, efficient approaches for belief planning in (locally approximated) Gaussian belief space using iLQG have been proposed [15], [16]. However, the running costs is still O(n 6 ) in the configuration space dimension.…”

Uncertainty Models for TTC-Based Collision-Avoidance

“…POMDPs lead to optimal solutions, but their computational complexity increases exponentially with the dimensions of the state space. To address this issue, approximate models have also been proposed that have a polynomial running time in the state space, such as approaches that locally optimize an input feasible trajectory assuming Gaussian beliefs [30,34]. However, such approaches are still intractable to the problem of navigating multiple agents in real time without collisions.…”