In many applications of motion planning, the motion of the robot in response to commanded actions cannot be precisely predicted. Whether maneuvering a vehicle over unfamiliar terrain, steering a flexible needle through human tissue to deliver medical treatment, guiding a micro-scale swimming robot through turbulent water, or displaying a folding pathway of a protein polypeptide chain, the underlying motions cannot be predicted with certainty. But in many of these cases, a probabilistic distribution of feasible outcomes in response to commanded actions can be experimentally measured. This stochastic information is fundamentally different from a deterministic motion model. Though planning shortest feasible paths to the goal may be appropriate for problems with deterministic motion, shortest paths may be highly sensitive to uncertainties: the robot may deviate from its expected trajectory when moving through narrow passageways in the configuration space, resulting in collisions.
We introduce a new motion planning framework that explicitly considers uncertainty in robot motion to maximize the probability of avoiding collisions and successfully reaching a goal. We 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 steerable needles, a new class of medical needles capable of following curved paths through soft tissue to avoid obstacles. The motion of a steerable needle can be modeled as a Dubins-car mobile robot with left-right bang-bang steering. These needles are capable of reaching targets inaccessible to traditional rigid needles, but their motion is subject to uncertainty due to patient variability and local tissue inhomogeneities at the needle tip. We confirm that SMR's generate motion plans with significantly higher probabilities of success compared to traditional shortest-path plans.