Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces

1. Lydia E. KavrakiPetr Švetka Jean-Claude Latombe Mark H. Overmars Presented by Derek Chan and Stephen Russell October 10, 2007 Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces

2. Outline • Introduction • Previous work • Algorithm • Learning phase • Query phase • Simulation/Results • Conclusion

3. 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

4. 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)‏

5. 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

6. Graphs and Configurations • Example Roadmaps • Example Configurations

7. Set of configuration graph nodes (N) is empty 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} • ForallnЄ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

8. X,Y,Z above are neighbour candidates No cycles are created 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} • ForallnЄ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 Dist(c,x)‏ Dist(c,y)‏ c Z Dist(c,z)‏ New Node

9. 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 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} • ForallnЄ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

10. 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)‏ 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

11. 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 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} • ForallnЄ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

12. 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

13. 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

14. 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

15. Query Phase g' g • 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 s' s

16. Query Phase g' g • 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 s' s

17. 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

18. Planar robot arm J4 J2 J5 J1 J3 Simulation setup • Test cases for customizedimplementation

19. Results • Expansion phase is key • Tc/Te = 2 With expansion Without expansion

20. General Implementation Simple environments

21. General Implementation Revisit complex scene fromcustomized implementation Results(w/ expansion)‏

22. 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