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
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
Probabilistic roadmaps PRMs don’t represent the entire free configuration space, but rather a roadmap through it
Probabilistic roadmaps PRMs don’t represent the entire free configuration space, but rather a roadmap through it
Probabilistic roadmaps PRMs don’t represent the entire free configuration space, but rather a roadmap through it S G
Probabilistic roadmaps PRMs don’t represent the entire free configuration space, but rather a roadmap through it S G
Probabilistic roadmaps PRMs don’t represent the entire free configuration space, but rather a roadmap through it S G
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
PRM algorithm overview Learning Phase Construction step: randomly generate nodes and edges Expansion step: improve graph connectivity in “difficult” regions Query Phase
PRM algorithm overview Learning Phase Construction step: randomly generate nodes and edges Expansion step: improve graph connectivity in “difficult” regions Query Phase
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”
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”
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)
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”
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 } ~ ~
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
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”
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
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
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
Construction step example Graph after several iterations...
Construction step example 2. A random free configuration c is generated and added to N c
Construction step example 3a. Candidate neighbors to c are partitioned from N c
Construction step example 3b. Edges are created between these neighbors and c, such that acyclicity is preserved c
Construction step example 3b. Edges are created between these neighbors and c, such that acyclicity is preserved c
PRM algorithm overview Learning Phase Construction step: randomly generate nodes and edges Expansion step: improve graph connectivity in “difficult” regions Query Phase
Expansion step Problem: construction step generates uniform covering of free C- space, might fail to capture connectivity of narrow passages
Expansion step Solution: improve graph connectivity in “difficult” areas, measured heuristically
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”
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”
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
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”
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
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
Expansion step example Graph after construction step...
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
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
Expansion step example 3b. “Expand” c using a random-bounce walk .030 .091 .091 .061 .030 .15 .091 .12 .030 .091 .12 .091
Expansion step example 3b. “Expand” c using a random-bounce walk .030 .091 .091 .061 .030 .15 .091 .12 .030 .091 .12 .091
Expansion step example 3b. “Expand” c using a random-bounce walk .030 .091 .091 .061 .030 .15 .091 .12 .030 .091 .12 .091
Expansion step example 3b. “Expand” c using a random-bounce walk .030 .091 .091 .061 .030 .15 .091 .12 .030 .091 .12 .091
Expansion step example 3b. “Expand” c using a random-bounce walk .030 .091 .091 .061 .030 .15 .091 .12 .030 .091 .12 .091
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
PRM algorithm overview Learning Phase Construction step: randomly generate nodes and edges Expansion step: improve graph connectivity in “difficult” regions Query Phase
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
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
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)
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
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
Conclusions Cons (?) Probabilistically complete Paths are not optimal -- can be long and indirect, depending on how graph was created; can apply smoothing, but...