Probabilistic Roadmap Path Planning
Reference: Principles of Robot Motion
H. Choset et. al.
MIT Press
Probabilistic Roadmap Path Planning
Explicit Geometry based planners (grown
obstacles, Voronoi etc) impractical in high
dimensional spaces.
Exact solutions with complex geometries are
provably exponential
Sampling based planners can often create plans
in high-dimensional spaces efficiently
Rather than Compute the C-Space explicitly, we
Sample it
Explicitly computing C-Space for more than 3 DOF is prohibitive!
Notion of Completeness in Planning
Complete Planner: always answers a path
planning query correctly in bounded time
Probabilistic Complete Planner: if a solution
exists, planner will eventually find it, using
random sampling (e.g. Monte Carlo sampling)
Resolution Complete Planner: same as above
but based on a deterministic sampling (e.g
sampling on a fixed grid).
Sampling Based-Planners
Do not attempt to explicitly construct the C-Space and its
boundaries
Simply need to know if a single robot configuration is in
collision
Exploits simple tests for collision with full knowledge of
the space
Collision detection is a separate module- can be tailored
to the application
As collision detection improves, so do these algorithms
Different approaches for single-query and multi-query
requests
PRM Planner
Roadmap is a graph G(V,E)
Robot configuration qQ_free is a vertex
Edge (q1, q2) implies collision-free path
between these robot configurations
A metric is needed for d(q1,q2) (e.g.
Euclidean distance)
Uses coarse sampling of the nodes, and
fine sampling of the edges
Result: a roadmap in Q_free
PRM Planner: Step 1, Learning the Map
Initially empty Graph G
A configuration q is randomly chosen
If qQ_free then added to G (collision detection
needed here)
Repeat until N vertices chosen
For each q, select k closest neighbors
Local planner connects q to neighbor q
If connect successful (i.e. collision free local
path), add edge (q, q)
PRM Planner: Step 2, Finding a Path
Given q_init and q_goal, need to connect
each to the roadmap
Find k nearest neigbors of q_init and
q_goal in roadmap, plan local path
Problem: Roadmap Graph may have
disconnected components
Need to find connections from q_init,
q_goal to same component
Once on roadmap, use Dijkstra algorithm
PRM Planner unanswered questions
How are random configurations chosen?
How are closest neighbors found?
How do we choose distance function?
How are local paths generated?
PRM Sampling and Connectivity
Sampling: Uniform random sampling of Q_free
Can be multi-dimensional (e.g. translation and
rotation, both 2-D or 3-D or higher)
Connectivity: need to find nearest neighbors
Nave search is O(n)
K-D trees are efficient ways to find nearest
neighbors
Cost: O(sqrt(n)) for d=2
Applet: http://donar.umiacs.umd.edu/quadtree/points/kdtree.html
Local Planner
Important aspect of PRM algorithm
Tradeoff:
powerful planners are slow, but can find paths
between relatively isolated nodes
fast planner is less accurate, more nodes
need to be generated, called more often
Local planner also needed for finding path
from q_init and q_goal to roadmap
Local Planner
Simplest: straight line planner. Connect q
and q by linear segment
Now check the segment for collisions:
Incremental: use a small step size and iterate
over the linear segment
Subdivision: use binary search decomposition
to check for collisions
Postprocessing: Path Improvement
Once a path is found, it can be optimized
Try connecting non-adjacent
configurations. Choose q_1 and q_2
randomly, try to connect.
Greedy approach: try connecting points
q_0, q_1, q_n to q_goal.
If q_k connects to q_goal, do the above
with q_k as q_goal
Example: 6-DOF Path Planning
Robot: Rigid non-convex object in 3 space
Obstacle: Solid wall with small opening
Random configuration is chosen from R3 for
translation
Axis and angle of rotation randomly chosen for
rotation (quaternion representation)
Collision Detection
Given configuration q and nearest neighbor
q we can use straight line collision detection
Each configuration
q=(p,r)=(trans,quaternion)
Check for collision by interpolating along line
(p,p) and along spherical interpolation (r,r).
video
Distance Calculation for Rigid Object in 3D
Distance function needed between 2 configurations q, q
Ideally, distance is the swept volume of the robot as it moves between
configurations q and q. Difficult to compute exactly
Method I: Can approximate this distance with an embedding in a Euclidean
metric space: d(q,q) = || embed(q) - embed(q)||
Choose set of p points on robot, concatenate them, and create a vector of
size p x dimension of workspace.
Example of rigid object in 3D: Create vector of size 3p, choosing p points
on the object. Intuitively, a sampling of the objects Euclidean domain.
For configuration q, embed(q) is the vector of p points transformed by the
translation and rotation that is configuration q. Transform each of the p
points into the vector embed(q).
Do the same for configuration q, create embed(q).
Distance is now Euclidean distance between the 3p vectors:
d(q,q) = || embed(q) - embed(q)||
How do you choose the p points?
Cheaper solution: choose 2 points p1 and p2 of maximum extent on the object.
Method II: Separate a configuration q into a translation X and a rotation R:
q=(X,R)
Calculate a weighted distance function d(q,q) = w1||X-X|| + w2 f(R,R).
Need to use a metric on rotations quaternions are good for this
Weights w1 and w2 need to be chosen, no real insight into this
OBPRM: Obstacle PRM
If tight, small regions of the Cspace are needed to
create a path (e.g. small opening in the wall),
sampling may miss this
Problem areas tend to be near the obstacles in
tight spaces
Solution: generate configuration q. If q in collision,
choose random direction v and move q away from
obstacle in direction v a small distance. If q now in
Q_free, use this node
Biases sampling near obstacles
Single-Query Sampling Based Planners
PRM samples the entire space, plans
paths anywhere
Single query planners dont explore all of
Q_free, only relevant parts
PRM can be used this way, inserting q_init
and q_goal in Graph at beginning, then
checking for a path
Random search
Often combined with potential field methods to escape minima
Filling in local minima
Random walks
random walks are not perfect...
RRT: Rapidly-exploring Random Trees
Idea: sample Q_free for path from q_init to
q_goal
Use 2 trees, rooted at q_init and q_goal.
As trees grow, the eventually share a
common node, and are merged into a path
RRTs
1) Maintain a tree of configurations reachable from the starting point.
2) Choose a point at random from free space
3) Find the closest configuration already in the tree
4) Extend the tree in the direction of the new configuration
connects global & local information
EXTEND step
Growth of an RRT
Example growth of an RRT
- Biased toward the unexplored free space at each step.
Voronoi diagrams
A Mature RRT
RRT - blue
Voronoi - red
http://msl.cs.uiuc.edu/rrt
http://www.kuffner.org/james/humanoid/planning.php
Path Planning with RRT Algorithm
2 trees, T_init, T_goal, rooted at q_init,
q_goal
Each tree is expanded by:
q_rand is generated from uniform dist.
q_near is found, nearest tree node to q_rand
move step-size along line (q_near, q_rand) to
q_new. If no collision, add q_new to tree
If trees merge, path is found
RRT Algorithm
Algorithm sensitive to step-size
How far do we move along line (q_near,
q_rand)?
Can a greedier algorithm work better?
Why not move all the way to q_rand?
RRT Tradeoffs
If step-size is small, many nodes
generated, close together
As number of nodes increases, nearestneighbor computation slows down
May be better to only add the last sample
along the line (q_near, q_rand)
Shaping the RRT
q_rand determines what direction we go
What if q_rand == q_goal?
Very greedy algorithm. Introduces too much
bias
Becomes a potential field planner that gets
stuck in local minima
Idea: use uniform q_rand with occasional
q_rand ==q_goal (maybe we get lucky?)
Introducing just .05 bias towards goal, results
improve
Merging RRTs
Using RRTs
http://msl.cs.uiuc.edu/rrt/
Bidirectional search
Additional complexity
additional degrees of freedom
Additional complexity
additional degrees of freedom
xy projections
Additional complexity
additional degrees of freedom
xy projections
time-lapse paths
Additional complexity
articulated linkages
Additional complexity
articulated linkages
http://msl.cs.uiuc.edu/rrt
http://www.kuffner.org/james/humanoid/planning.html
