1. CS 326A: Motion Planning
Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces (1996)
L. Kavraki, P. Švestka,
J.-C. Latombe, M. Overmars
Presented by Miler Lee, 15 April 2002

2. Path planning Low-dimensionality (= few dof) cases are easy, fast
High-dof situations problematic, due to computational / space complexity
Solution: sacrifice completeness in favor of performance

3. Probabilistic roadmaps
PRMs don’t represent the entire free configuration space, but rather a roadmap through it

4. Probabilistic roadmaps
PRMs don’t represent the entire free configuration space, but rather a roadmap through it

5. Probabilistic roadmaps
PRMs don’t represent the entire free configuration space, but rather a roadmap through it S G

6. Probabilistic roadmaps
PRMs don’t represent the entire free configuration space, but rather a roadmap through it S G

7. Probabilistic roadmaps
PRMs don’t represent the entire free configuration space, but rather a roadmap through it S G

8. PRM algorithm overview
Roadmap is an undirected acyclic graph R = (N, E)
Nodes N are robot configurations in free C-space
Edges E represent local paths between configurations

9. PRM algorithm overview
Learning Phase
Construction step: randomly generate nodes and edges
Expansion step: improve graph connectivity in “difficult” regions
Query Phase

10. PRM algorithm overview
Learning Phase
Construction step: randomly generate nodes and edges
Expansion step: improve graph connectivity in “difficult” regions
Query Phase

11. Learning: construction overview
1. R = (N, E) begins empty
2. A random free configuration c is generated and added to N
3a. Candidate neighbors to c are partitioned from N
3b. Edges are created between these neighbors and c, such that acyclicity is preserved
4. Repeat 2-3 until “done”

12. Learning: construction overview
1. R = (N, E) begins empty
2. A random free configuration c is generated and added to N
3a. Candidate neighbors to c are partitioned from N
3b. Edges are created between these neighbors and c, such that acyclicity is preserved
4. Repeat 2-3 until “done”

13. Generating random configurations
2. A random free configuration c is generated and added to N
Random sampling over uniform probability distribution of values for each dof
New configuration is checked for collision (intersect obstacles, intersect self)

14. Learning: construction overview
1. R = (N, E) begins empty
2. A random free configuration c is generated and added to N
3a. Candidate neighbors to c are partitioned from N
3b. Edges are created between these neighbors and c, such that acyclicity is preserved
4. Repeat 2-3 until “done”

15. Nc = { c  N | D(c, c)  maxdist }
Neighbor set
3a. Candidate neighbors to c are partitioned from N
Only want to consider k neighbors some maximum distance maxdist away from c, according to a distance function D:
Nc = { c  N | D(c, c)  maxdist } ~ ~

16. D(c, n) = max || x(n) - x(c) ||
Distance function
D should reflect the chance that the local planner will fail to connect two nodes, e.g.:
D(c, n) = max || x(n) - x(c) ||
x  robot
max Euclidean distance between a point on the robot at the two configurations

17. Learning: construction overview
1. R = (N, E) begins empty
2. A random free configuration c is generated and added to N
3a. Candidate neighbors to c are partitioned from N
3b. Edges are created between these neighbors and c, such that acyclicity is preserved
4. Repeat 2-3 until “done”

18. Creating edges
3b. Edges are created between these neighbors and c, such that acyclicity is preserved
neighbors are considered in order of increasing distance from c
nodes that are already in the same connected component of c at the time are ignored
local planner is used to determine whether a local path exists

19. Local planner best when deterministic and fast:
if deterministic, then don’t have to store local paths
if fast, calls are cheap (fast querying), but need denser graph because higher failure rate

20. General local planner
1. Connect the two configurations in C-space with a straight line segment
2. Check the joint limits
3. Discretize the line segment into a sequence of configurations c1, …, cm such that for every (ci , ci+1), no point on the robot at ci lies further than  away from its position at ci+1
4. For each ci, grow robot by , check for collisions

21. Construction step example
Graph after several iterations...

22. Construction step example
2. A random free configuration c is generated and added to N c

23. Construction step example
3a. Candidate neighbors to c are partitioned from N c

24. Construction step example
3b. Edges are created between these neighbors and c, such that acyclicity is preserved c

25. Construction step example
3b. Edges are created between these neighbors and c, such that acyclicity is preserved c

26. PRM algorithm overview
Learning Phase
Construction step: randomly generate nodes and edges
Expansion step: improve graph connectivity in “difficult” regions
Query Phase

27. Expansion step
Problem: construction step generates uniform covering of free C- space, might fail to capture connectivity of narrow passages

28. Expansion step
Solution: improve graph connectivity in “difficult” areas, measured heuristically

29. Expansion step
1. Associate each node c with a positive weight w(c), that is a heuristic measure of the “difficulty” of the region around c
2. Normalize all w(c) so they sum to 1
3a. Select c such that P(c is selected) = w(c)
3b. “Expand” c using a random-bounce walk
4. Repeat 2-3 until “done”

30. Expansion step
1. Associate each node c with a positive weight w(c), that is a heuristic measure of the “difficulty” of the region around c
2. Normalize all w(c) so they sum to 1
3a. Select c such that P(c is selected) = w(c)
3b. “Expand” c using a random-bounce walk
4. Repeat 2-3 until “done”

31. Heuristic weight functions
1. Associate each node c with a positive weight w(c), that is a heuristic measure of the “difficulty” of the region around c
Count number of nodes some d distance from c
Look at distance from c to nearest connected sub-graph not connected to c
Use failure rate of local planner

32. Expansion step
1. Associate each node c with a positive weight w(c), that is a heuristic measure of the “difficulty” of the region around c
2. Normalize all w(c) so they sum to 1
3a. Select c such that P(c is selected) = w(c)
3b. “Expand” c using a random-bounce walk
4. Repeat 2-3 until “done”

33. Random-bounce walk
3b. “Expand” c using a random-bounce walk
1a. Pick a random direction of motion in C-space, move in this direction from c
1b. If collision occurs, pick a new direction and continue
2. The final configuration n and the edge (c, n) are inserted into the graph
3. Attempt to connect n to other nodes using the construction step technique

34. Random-bounce walk
3b. “Expand” c using a random-bounce walk
Path between c and n must be stored, since process is non-deterministic
Best case reduces the number of disjoint components of the graph; worst case keeps this number constant

35. Expansion step example
Graph after construction step...

36. Expansion step example
3a. Select c such that P(c is selected) = w(c)
.030
.091
.091
.061
.030
.15
.091
.12
.030
.091
.12
.091

37. Expansion step example
3a. Select c such that P(c is selected) = w(c)
.030
.091
.091
.061
.030
.15
.091
.12
.030
.091
.12
.091

38. Expansion step example
3b. “Expand” c using a random-bounce walk
.030
.091
.091
.061
.030
.15
.091
.12
.030
.091
.12
.091

39. Expansion step example
3b. “Expand” c using a random-bounce walk
.030
.091
.091
.061
.030
.15
.091
.12
.030
.091
.12
.091

40. Expansion step example
3b. “Expand” c using a random-bounce walk
.030
.091
.091
.061
.030
.15
.091
.12
.030
.091
.12
.091

41. Expansion step example
3b. “Expand” c using a random-bounce walk
.030
.091
.091
.061
.030
.15
.091
.12
.030
.091
.12
.091

42. Expansion step example
3b. “Expand” c using a random-bounce walk
.030
.091
.091
.061
.030
.15
.091
.12
.030
.091
.12
.091

43. Expansion step
Leftover disjoint pieces of the graph are discarded if their nodes constitute less than some mincomponent percent of the total number of nodes in the graph

44. PRM algorithm overview
Learning Phase
Construction step: randomly generate nodes and edges
Expansion step: improve graph connectivity in “difficult” regions
Query Phase

45. Query phase
1. Given start configuration s, goal configuration g, calculate paths Ps and Pg such that Ps and Pg connect s and g to nodes s and g that are themselves connected in the graph
2. Return the path consisting of Ps concated with the path from s to g concated with the reverse of Pg

46. Calculating Ps , Pg
Consider nodes in graph in order of increasing distance from Ps, up to a maxdist away from s
Use local planner to find connection
Use random-bounce walk if local planner fails

47. Experimental results (briefly)
Tested with up to 7-dof robot, both free- and fixed-base
Given enough learning time, able to achieve 100% success, but not unreasonable results even with shorter learning periods
Queries are fast (not more than a couple of seconds)

48. Remarks
Basis of comparison is the Randomized Path Planner; PRMs perform comparably or better than RPPs on the scenes tested
PRMs do not suffer from local minima as RPPs do (narrow passages)
RPPs have succeeded with up to 31 dof; would be nice to see some PRM experimental results for >> 7 dof

49. Conclusions
Pros
Once learning is done, queries can be executed quickly
Complexity reduction over full C-space representation
Adaptive: can incrementally build on roadmap
Probabilistically complete, which is usually good enough

50. Conclusions Cons (?) Probabilistically complete
Paths are not optimal -- can be long and indirect, depending on how graph was created; can apply smoothing, but...