1. Randomized Motion Planning for Car-like Robots with C-PRM Guang Song, Nancy M. Amato Department of Computer Science Texas A&M University College Station, Texas, USA http://www.cs.tamu.edu/faculty/amato/

2. Given: an environment (descriptions of moveable object and obstacles), and start and goal positions Find: a valid path (continuous sequence of configurations) from start to goal (e.g., which avoids collision with obstacles) that meets certain requirements Motion Planning start goal obstacles

3. 1. Connect start and goal to roadmap Query processing startgoal C-obst Roadmap Construction (Pre-processing) 2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g., straightline) - discard paths that are invalid 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid C-obst C-space 2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap Probabilistic Roadmap Methods (PRMs) [Kavraki, Svestka, Latombe, Overmars 1996]

4. Three dof: x, y,  l Nonholonomic constraints: - 1) dx/dt sin(  ) + dy/dt cos(  ) =0, - Not reflected in c-space obstacles. - A constraint not on c-space nodes, but on how the nodes are connected, ie, the edges. 2) minimum turning radius r. l Traditional PRMs try to reflect these constraints exactly. X Y O (x,y)  Car-like robot

5. Previous Work on car-like robot motion planning l Potential field methods. l Probabilistic Roadmap Methods (PRM): a) Svestka & Overmars’s PPP algorithm [’93] b) LaValle & Kuffner’s RRT algorithm. [’99] l Difficulty in applying PRMs to Car-like robots: — The roadmap is constructed with a pre-defined radius of a pre-defined robot. — Different robots have to need their own roadmaps even the environment is the same.

6. Our contribution Our contribution: 1) a new PRM method that provides a customizable roadmap for a given environment that is independent of any specific robot, and can be tailored to meet different robot specifications. 2) introduce control roadmap concept that helps generate good nodes along ‘roadways’ and provides natural control polygon for path optimization.

7. An overview of Customizable PRM (C-PRM) l Roadmap Construction: Build a coarse roadmap by approximate node and edge validation Very fast and efficient l Query Phase: Complete validation only on those nodes and edges necessary to solve the query Customize the roadmap to meet certain requirements The same roadmap can be used to find paths that meet different requirements

8. Query Phase 1. Connect start and goal to roadmap start goal

9. Query Phase 1. Connect start and goal to roadmap 2. Search for shortest path between them start goal

10. Query Phase 1. Connect start and goal to roadmap 2. Search for shortest path between them 3. Remove all nodes that do not meet requirements 4. Remove all edges that do not meet requirements start goal

11. Query Phase 1. Connect start and goal to roadmap 2. Search for shortest path between them 3. Remove all nodes that do not meet requirements 4. Remove all edges that do not meet requirements 5. Repeat until a path is found or start and goal no longer connected through roadmap start goal

12. C-PRM for car-like robot First construct a ‘control roadmap’ for quickly estimating the connectivity of free space. l Approximate robot with a disc (orientation-free) & generate nodes. l Connect each node to k nearest neighbors — Check collision at edge midpoint only. A control roadmap

13. C-PRM for car-like robot l Node generation: — each node consists of, an edge midpoint and the orientation along that edge. (nodes are aligned with the ‘roadways’!) l Node connection: — Each pair of nodes that correspond to adjacent edges in cntl-rdmp is attempted to be connected. — Edge is added if it has a ‘low’ curvature. (No collision checking) Control roadmap The real roadmap for robot invalid

14. A real example: l Control map ‘shows’ where the roadways are and helps generate good nodes. l Approximate roadmap keeps free nodes, edges that meet some coarse curvature requirement. — Most edges generated are likely to be collision free. (No collision checking is done.) Control Roadmap Edge midpoint Adjacent edges Control Polygon Real roadmap Node Edge Path robot obstacles

15. Query for a car-like robot Query: find a path between start and goal for a robot with turning radius r. l Remove all edges with curvature larger then 1/r. l Find the shortest path. 1. Run the Dijkstra’s algorithm to get the shortest path. 2. Check the validity of each edge along the path. 3. If any invalid edge found, remove it, and 4. Repeat until the whole path is valid or start and goal are not connected any more.

16. Path Optimization l The path consists of arcs and line segments. — Since the curvature is not continuous, the robot has to stop at each transition. l Cubic B-spline can help. Control roadmap contains the control points/polygon. Node in cntl-rdmp Node in rdmp B A C D E F A path (a sequence of red nodes), and it control polygon ABCDEF, which is from control roadmap.

17. Results: l Solutions in all four scenes are found fairly quickly. (in a few seconds to tens of seconds.)

18. Scene 1: Head-in parking l Path found can be smoothed using cubic b-splines. A solution path After partly smoothed by cubic b-spline.

19. Scene 2: parallel parking l Two case with different turning radii. — Same roadmap used for both. — Turning radii specified at query time. 1) A path with an unrealistic turning radius. 2) A path with a more realistic turning radius.

20. Scene 3: Drive around obstacles. l Edge weights in roadmap select behavior. — Discourage backward motion with high weights. — Same roadmap used in both cases. start goal 1) The shortest path with a lot of backward motion. 2) Path found after backward motion penalized by a factor of 10.

21. Scene 4: Navigate around many obstacles. A cluttered scene with 19 randomly- placed triangles. start goal Control roadmap Roadmap

22. Conclusion l New approach using PRMs for car-like robots motion planning. l Customizable roadmaps can be used by multiple robots with different turning radii. l Control roadmap concept is proposed that can help generate good nodes and provide natural control polygon for path smoothing with cubic B-splines.

23. More info at: http://www.cs.tamu.edu/faculty/amato contact: {gsong, amato}@cs.tamu.edu Acknowledgements: Supported in part by the NSF, Dept of Energy ASCI program, state of Texas

24. Probabilistic Roadmap Methods (PRMs) [Kavraki, Svestka, Latombe, Overmars 1996] l PRMs can be inefficient l No support for multiple, variable query requirements: — maintaining a particular clearance — restricting a dof (tilting) — minimizing rotation (smoothness) — only allowing a maximum number of sharp turns

25. C-PRM for car-like robot l Next, an approximate roadmap is built from control roadmap. Nodes correspond to the midpoints of the edges in the control roadmap, and they are oriented along the direction of edge (aligned with the ‘roadway’). Roadmap nodes are connected if they correspond to adjacent edges in the control roadmap. Only a coarse bound is placed on the turning radius. The turning radius of the actual robot is enforced at query stage.