The Stochastic Motion Roadmap: A Sampling Framework for Planning with Markov Motion Uncertainty Changsi An You-Wei Cheah

Introduction

Problem Sources of uncertainty  Imprecision of execution of action a narrow passageway is unlikely to be robust to motion uncertainty

Failure other than collision Nonholonomic constraint Irreversible deflection in the way a deflection in the path due to motion uncertainty can result in failure to reach the goal

Countermeasure in the paper Stochastic Motion Roadmap  Probabilistic method  Explicitly consider the motion uncertainty  Increase success rate of achieving the goal Characteristic  Decide the most promising next move at each step  Rely on percepts(feedbacks) to know current state

Build Road Map Probabilistic Road Map (PRM) Stochastic Motion Roadmap (SMR) s g SMR: Enhanced PRM Describe the possibility of transition from one state to another PRM Capture the connectivity

Stochastic Motion Roadmap s g Motion uncertainty

Build SMR Same as PRM  isCollisionFree(x)  isCollisionFreePath(x, y) Sample configurations Motion emulator with uncertainty  getTransitions(x, u) Environment Describer x, y : configurations x, y u: control

GetTransitions(x, u)

Sampled next states: from continuous configuration space x Voronoi Graph

GetTransitions(x, u) Sampled next states x V1V1 V2V2 V3V3 V 1, V 2, V 3 ∈ C InControlNextProb xμV1V xμV2V xμV3V

GetTransitions(x, u) Sampled next states x V1V1 V2V2 V3V3 V 1, V 2, V 3 ∈ C Obstacles InControlNextProb xμV1V xμV2V xμV3V

V 1, V 2, V 3 ∈ C Obstacles InControlNextProb xμV1V xμV2V xμV3V

V 1, V 2, V 3 ∈ C Obstacles InControlNextProb xμV1V xμV2V xμOBTC0.15..

V 1, V 2, V 3 ∈ C Obstacles InControlNextProb xμV2V xμOBTC V1V1 V2V2 V3V3

V i, X i ∈ C μ i ∈ Μ Transition Probability Matrix InControlNextProb x1x1 μ1μ1 V2V x1x1 μ1μ1 OBTC0.33 x1x1 μ2μ2 VaVa PaPa x1x1 μ2μ2 VbVb PaPa.. x2x2 μ1μ1 VcVc PcPc The table means: when currently at state In, if control Control is taken, there is Prob chance to move to state Next

Query Goal  At configuration i ∈ C, choose μ i ∈ Μ  maximize the success probability

Bellman equation State Evaluation Compute recursively with BE, If j is within destination area, If j will lead to collision with obstacle, If otherwise Employ the idea of Dynamic Programming to memorize the intermediate results of J*(j)

Circle in Recursion Cyclic transition probability graph Utility function: penalty penalty

Stochastic Road Map Strengths  Maximize the success rate for nonholonomic systems  High fault tolerance from dynamic decisions  General framework, very flexible for modeling the uncertainty Drawbacks  Rely on an accurate percepts of current state  Omit goal and obstacle dynamics  Optimality restricted by magnitude of discrete representatives of CSpace

SMR for Medical Needle Steering Steerable needles are controlled by 2 degrees of freedom:  Insertion distance  Bevel direction Workspace is extracted from a medical image Obstacles are tissues that should not be cut by the needle

Bang bang steering car model State of the car is represented by a 4 dimension state space, s i = (x i, y i, θ i, b i ) Bevel direction of needle can be set to point left (b = 0) or right (b = 1)

SMR Implementation of bang bang steering car model A car moves δ between sensor measurements of states The set U consists of two actions: move forward turning left (u = 0), or move forward turning right (u = 1). As the car moves forward, it traces an arc of length δ with radius of curvature r and direction based on u. r and δ are random variables from δ ~ N (δ 0, σ δa ) & r ~ N (r 0, σ ra ), a ∈ {0, 1}

SMR Implementation of workspace Workspace: Rectangle of width x max and height y max. Obstacles: Polygons in the plane Zero-winding rule is used to detect obstacles. distance(s 1, s 2 ) = √[(x 1 − x 2 ) 2 +(y 1 − y 2 ) 2 +α(θ 1 −θ 2 ) 2 ] + M, where M → ∞ if b 1 ≠ b 2, and M = 0 otherwise. CGAL implementation of kd-trees is used to calculate fast nearest-neighbor Goal T ∗ as all configuration states within a ball of radius t r centered at a point t ∗.

Evaluation of SMR p s improves as the sampling density of the configuration space and the motion uncertainty distribution increase As n and m increase, p s (s) is more accurately approximated over the configuration space, resulting in better action decisions. Difficult problem: p s effectively converges for n ≥ 100,000 and m ≥ 20

Evaluation of SMR

T HANK YOU