Download
1 / 27
270 likes | 465 Views
The Stochastic Motion Roadmap: A Sampling Framework for Planning with Markov Motion Uncertainty. Changsi An You-Wei Cheah. Introduction. Problem. a narrow passageway is unlikely to be robust to motion uncertainty. Sources of uncertainty Imprecision of execution of action.
E N D
The Stochastic Motion Roadmap: A Sampling Framework for Planning with Markov Motion Uncertainty Changsi An You-Wei Cheah
Problem a narrow passageway is unlikely to be robust to motion uncertainty • Sources of uncertainty • Imprecision of execution of action
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
g s Build Road Map • Probabilistic Road Map (PRM) • Stochastic Motion Roadmap (SMR) SMR: Enhanced PRM Describe the possibility of transition from one state to another PRM Capture the connectivity
g s Stochastic Motion Roadmap 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 V1,V2,V3 ∈C V1 V2 V3 x
GetTransitions(x, u) Obstacles Sampled next states V1,V2,V3 ∈C V1 V2 V3 x
Obstacles V1,V2,V3 ∈C
Obstacles V1,V2,V3 ∈C
Obstacles V1,V2,V3 ∈C V1 V2 V3
Transition Probability Matrix Vi,Xi∈C μi∈Μ The table means: when currently at state In, if control Control is taken, there is Probchance to move to state Next
Query • Goal • At configuration i∈C, choose μi∈Μ • maximize the success probability
Bellman equation • State Evaluation Employ the idea of Dynamic Programming to memorize the intermediate results of J*(j) , If j is within destination area , If j will lead to collision with obstacle Compute recursively with BE , If otherwise
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, si= (xi, yi, θi, bi) 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. randδare random variables from δ~ N (δ0 , σδa ) &r~ N (r0 , σra ),a∈ {0, 1}
SMR Implementation of workspace Workspace: Rectangle of width xmaxand height ymax. Obstacles: Polygons in the plane Zero-winding rule is used to detect obstacles. distance(s1 , s2) = √[(x1 − x2)2 +(y1 − y2)2 +α(θ1 −θ2)2] + M, where M → ∞ if b1≠b2, 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 tr centered at a point t∗.
Evaluation of SMR ps improves as the sampling density of the configuration space and the motion uncertainty distribution increase As n and m increase, ps(s) is more accurately approximated over the configuration space, resulting in better action decisions. Difficult problem: ps effectively converges for n ≥ 100,000 and m ≥ 20
