Randomized Motion Planning forCar-like Robots with C-PRM

Guang Song and Nancy M. AmatoDepartment of Computer Science

Texas A&M UniversityCollege Station, Texas, USA

http://www.cs.tamu.edu/faculty/amato/{gsong,amato}@cs.tamu.edu

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 processingstart

goal

C-obst

C-obst

C-obst

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

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

Potential field methods. Probabilistic Roadmap Methods (PRMs):

— Svestka & Overmars’s PPP algorithm [’93]— LaValle & Kuffner’s RRT algorithm. [’99]

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

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.

Introduce control roadmap concept that helps generate good nodes along ‘roadways’ and provides natural control polygon for path optimization.

Customizable PRM (C-PRM) Overview

Roadmap Construction: Build an approximate roadmap by approximate node and edge validation

Very fast and efficient

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 roadmapstart

goal

C-PRM for Car-like RobotsFirst construct a ‘control roadmap’ for quickly estimating the connectivity of free space.

Approximate robot with a disc (orientation-free) & generate nodes— (e.g., disc diameter may equal robot width, but be less than robot length)

Connect each node to k nearest neighbors— Check collision at edge midpoint only.

A Control Roadmap

C-PRM for Car-like Robots

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

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

Control map ‘shows’ where the roadways are and helps generate good nodes. 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 midpointAdjacent edges

Control Polygon

Approximate roadmap

NodeEdgePath

robot

obstacles

Query for a Car-like Robot

Query: find a path between start and goal for a robot with turning radius r.

Remove all edges with curvature larger than 1/r. 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 The path consists of arcs and line segments.

— Since the curvature is not continuous, the robot has to stop at each transition.

Cubic B-spline can help reduce number of transitions.— Control roadmap contains the control points/polygon.

Node in cntl-rdmp

Node in rdmpB

A

C

D

E

F

A path (a sequence of red nodes), and its control polygonABCDEF, which is from the control roadmap.

Results:

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

Scene 1: Head-in Parking

Path found can be smoothed using cubic B-splines.

A solution path After partly smoothed using cubic b-spline

Scene 2: Parallel Parking

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.

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

New approach using PRMs for car-like robot motion planning

Customizable roadmaps can be used by multiple robots with different turning radii

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:

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