Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces Lydia E. Kavraki Petr Švetka Jean-Claude Latombe Mark H. Overmars Presented by Derek Chan and Stephen Russell October 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 Step increase 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 XY 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'g' s'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'g' s'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 customized implementation Planar robot arm J4J4 J2J2 J3J3 J1J1 J5J5

Results –Expansion phase is key T c /T e = 2 With expansion Without expansion

General Implementation Simple environments

General Implementation Revisit complex scene from customized 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