The Stochastic Motion Roadmap: A Sampling Framework for Planning with Markov Motion Uncertainty

Abstract

Abstract-We present a new motion planning framework that explicitly considers uncertainty in robot motion to maximize the probability of avoiding collisions and successfully reaching a goal. In many motion planning applications ranging from maneuvering vehicles over unfamiliar terrain to steering flexible medical needles through human tissue, the response of a robot to commanded actions cannot be precisely predicted. We propose to build a roadmap by sampling collision-free states in the configuration space and then locally sampling motions at each state to estimate state transition probabilities for each possible action. Given a query specifying initial and goal configurations, we use the roadmap to formulate a Markov Decision Process (MDP), which we solve using Infinite Horizon Dynamic Programming in polynomial time to compute stochastically optimal plans. The Stochastic Motion Roadmap (SMR) thus combines a sampling-based roadmap representation of the configuration space, as in PRM's, with the well-established theory of MDP's. Generating both states and transition probabilities by sampling is far more flexible than previous Markov motion planning approaches based on problem-specific or grid-based discretizations. We demonstrate the SMR framework by applying it to nonholonomic steerable needles, a new class of medical needles that follow curved paths through soft tissue, and confirm that SMR's generate motion plans with significantly higher probabilities of success compared to traditional shortest-path plans.

Global motion planning under uncertain motion, sensing, and environment map

“…However, most work on motion planning under uncertainty takes into account only one or two causes of uncertainty: Stochastic Motion Roadmap [1] considers only control uncertainty, [5] considers only sensing uncertainty, [8,16] consider only imperfect information about the environment, [4,20] consider only control and sensing uncertainty, and restricts them to Gaussian. Our new planner takes into account all three sources of uncertainty and allows any type of distribution with bounded support for control, sensing, and environment map uncertainty.…”

“…To sample a new belief, these planners sample a node b ∈ T , an action a ∈ A, and an observation o ∈ O according to suitable probability distributions or heuristics. They then compute b = τ(b, a, o) using (1). Whenever the distance between b and its nearest node in T is larger than a given threshold ε, b is inserted into T as a child of b.…”

“…1 This property means that two nearby beliefs under this metric have similar values, and hence with a small error, one can be used to represent the other.…”

LQG-Based Planning, Sensing, and Control of Steerable Needles

“…2-D planners that address motion uncertainty have been presented in [2,3], which optimize a Markov decision process (MDP) over a discretized state space to provide feedback control assuming full state observation. The approach was extended in [22] and integrated with imaging feedback.…”

Planning active cannula configurations through tubular anatomy

“…Optimizationbased motion planning has been applied to steerable needles inserted in 2D tissue slices around polygonal obstacles [2]. Other approaches include diffusion-based motion planning to numerically compute a path in 3D stiff tissues [24], screwbased motion planning to compute steerable needle paths in 3D around spherical obstacles [12], rapidly exploring random trees [34], and planning methods that explicitly consider uncertainty in the needle's motion to maximize the probability of successfully reaching the target [1], [4]. The latter method, combined with an imaging and control system, has been successfully integrated with robot hardware [25].…”