Randomized Motion Planning for Car-like Robots with C-PRM Guang Song and Nancy M. Amato Department of Computer Science Texas A&M University College Station, Texas, USA

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

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]

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

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

Our Contribution l 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. l Introduce control roadmap concept that helps generate good nodes along ‘roadways’ and provides natural control polygon for path optimization.

Customizable PRM (C-PRM) Overview l Roadmap Construction: Build an approximate 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 Related Work: similar motivation for Lazy PRM and Fuzzy PRM proposed by Kavraki and others, but they do not explore customization.

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

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

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

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

C-PRM for Car-like Robots First construct a ‘control roadmap’ for quickly estimating the connectivity of free space. l Approximate robot with a disc (orientation-free) & generate nodes — (e.g., disc diameter may equal robot width, but be less than robot length) l Connect each node to k nearest neighbors — Check collision at edge midpoint only. A Control Roadmap

C-PRM for Car-like Robots l Node generation: — each node consists of a control roadmap edge midpoint and the orientation along that edge. (nodes aligned with ‘roadways’!) Control roadmap The approximate roadmap for robot invalid l Node connection: — Connections are attempted for each pair of nodes that correspond to adjacent edges in control roadmap. — Edge added if has ‘low’ curvature below some threshold (no collision checking)

A Computed 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 Approximate roadmap Node Edge Path robot obstacles

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 than 1/r. l Find the shortest path. 1. Run Dijkstra’s algorithm to find shortest path. 2. Check validity of each edge along the path. 3. If any invalid edge found, remove it. 4. Repeat until the entire path is valid or start and goal are not connected any more.

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 reduce number of transitions. — 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 its control polygon ABCDEF, which is from the control roadmap.

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

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

Scene 2: Parallel Parking l Two cases with different turning radii — The same roadmap used for both cases — Turning radii specified at query time A path with an unrealistic turning radius A path with a more realistic turning radius

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 The shortest path with a lot of backward motion. Path found after backward motion penalized by a factor of 10.

Scene 4: Navigation with Many Obstacles A cluttered scene with 19 randomly- placed triangles. start goal Control roadmap Roadmap

Conclusion l New approach using PRMs for car-like robot 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

More info at: contact: {gsong, Acknowledgements: Supported in part by the NSF, Dept of Energy ASCI program, state of Texas

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

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.