View
215
Download
0
Tags:
Embed Size (px)
Citation preview
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
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