Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces
Lydia E. KavrakiPetr Švetka
Jean-Claude LatombeMark H. Overmars
Presented by Derek Chan and Stephen RussellOctober 10, 2007
Outline
• Introduction
• Previous work
• Algorithm– Learning phase– Query phase
• Simulation/Results
• Conclusion
Introduction
• Applications where robot must execute several point-to-point motions in static workspace– e.g. maintenance of cooling pipes in nuclear plant,
welding in car assembly, cleaning of airplane fuselages
• Require many dof to achieve desired configurations and avoid collision
• Explicit programming is tedious• Efficient, reliable planner would considerably
reduce burden
Previous Work
• Potential field methods– Heuristic is easily adapted to problem, but
local minima produce problems– Can be overcome with learning methods or
randomized walks (RPP)
• Other roadmap methods– Visibility graph and Voronoi diagram (limited
to low-dimensional C-space)– Silhouette method (prohibitively complex)
General Roadmap Algorithm Learning Phase:
Generate a roadmap which is an undirected graph R = (N,E) in 2 steps
Construction Step:obtain a graph that relatively uniformly covers the free C-space
Expansion Stepincrease connectivity by adding nodes in 'difficult' regions of the C-space
Query Phase:
Given a starting configuration and an ending configuration use the roadmap to determine a feasible path for the robot
In theory these two steps could be interwoven for better performance and both successful and unsuccessful query nodes could be added to the roadmap
Graphs and Configurations Example Roadmaps
Example Configurations
Construction Algorithm• N 0
• E
• Loop
• c a randomly chosen free configuration
• Nc a set of candidate neighbours of c chosen from N
• N N U {c}
• Forall n Є Nc in order of increasing Distance(c,n) do
• If notSameConnectedComponent(c,n) and
• localPlanner(c,n) then
• E E U {(c,n)}
• Update R's connected components
(1) Set of configuration graph nodes (N) is empty
(2) Set of simple path graph edges (E) is empty
• Loop through the steps below
• Randomly choose a configuration and name it c
• Select a set, Nc, of neighbours to c from N
• Add c to the set N
• For each member, n, of Nc in order of the distance between n and c do the following
• If c and n are not part of the same connect component
• And if the local planner can find a path from c to n
• Add edge (c,n) to E
• Update the connected components of the roadmap
Construction Algorithm• N 0
• E
• Loop
• c a randomly chosen free configuration
• Nc a set of candidate neighbours of c chosen from N
• N N U {c}
• Forall n Є Nc in order of increasing Distance(c,n) do
• If notSameConnectedComponent(c,n) and
• localPlanner(c,n) then
• E E U {(c,n)}
• Update R's connected components
X,Y,Z above are neighbour candidates
No cycles are created
c
X Y
Z
New Node
Dist(c,x) Dist(c,y)
Dist(c,z)
Selecting Configurations• N 0
• E
• Loop
• c a randomly chosen free configuration
• Nc a set of candidate neighbours of c chosen from N
• N N U {c}
• Forall n Є Nc in order of increasing Distance(c,n) do
• If notSameConnectedComponent(c,n) and
• localPlanner(c,n) then
• E E U {(c,n)}
• Update R's connected components
Step 4:
create a random configuration by randomly sampling each DOF uniformly over the interval of possible values
Check if there is a collision with self or an obstacle
If there is a collision randomly pick another configuration
Distance Function• N 0
• E
• Loop
• c a randomly chosen free configuration
• Nc a set of candidate neighbours of c chosen from N
• N N U {c}
• Forall n Є Nc in order of increasing Distance(c,n) do
• If notSameConnectedComponent(c,n) and
• localPlanner(c,n) then
• E E U {(c,n)}
• Update R's connected components
Step 5:
Select all nodes of N that are within a selected max distance from c according to some distance function Distance(c,n)
Distance Function
One possibility is to measure the area/volume swept by the robot between the two configurations
Function should reflect the failure rate of localPlanner(c,n)
Local Planner• N 0
• E
• Loop
• c a randomly chosen free configuration
• Nc a set of candidate neighbours of c chosen from N
• N N U {c}
• Forall n Є Nc in order of increasing Distance(c,n) do
• If notSameConnectedComponent(c,n) and
• localPlanner(c,n) then
• E E U {(c,n)}
• Update R's connected components
Step 9:
The Local Planner:
Determines whether a path exists to connect two nodes
Represents whether a robot can move from a configuration to another
The planner should be deterministic and fast but not necessarily powerful
More nodes at expense of greater failure rate for planner
To save space the path is calculated but not stored
Expansion Step
When there are obstacles, the roadmap might not be entirely connected where connectivity could exist
Nodes can be added in 'difficult' areas to increase connectivity
Expansion Step
Randomly Select nodes using a weighted distribution based on the 'difficulty' of each node
Possible difficulty functions for nodes:
Check if the number of nearby connected nodes is low
Check if the nearest non-connected component is close
Learning Phase Conclusion
Small connected components of roadmap are discarded
Unnecessarily long paths can be created
These can be smoothed or cycles can be introduced
Query Phase
After the learning phase is finished, use the roadmap to determine paths between configurations
Given a start configuration s and a goal configuration g, first connect s and g to the roadmap
Failure occurs if they can not be connected to the same component
g'
s'
s
g
Query Phase
Traverse the graph to find a path within the graph component between s' and g'
Paths must be recomputed since they were not stored
Collisions do not need to be rechecked
g'
s'
s
g
Application
• Algorithm was applied to planar articulated robots
• Two implementations:– Customized– General
• Customized implementation employs specific techniques for:– Local path-planning– Distance computation– Collision checking
Simulation setup
• Test cases for customizedimplementation
• Planar robot arm
J4
J2
J3J1
J5
Results
– Expansion phase is key• Tc/Te = 2
With expansion
Without expansion
General ImplementationSimple environments
General Implementation
Revisit complex scene fromcustomized implementation
Results(w/ expansion)
Conclusions
• Two phase method for robot motion planning in static workspace– Learning phase: construct roadmap (with increased
density in difficult regions)– Query phase: connect start/end locations to roadmap
to quickly create entire trajectory• Verified algorithm speed for complex scene
(esp. with customization)– Faster than RPP, which took tens of minutes to solve
• Consistency• Learning phase is precomputation, one-time cost