Path Planning
in Configuration Space
slides are from different sources, e.g. C.J. Taylor (UPenn), J. Latombe (Stanford), D. ),…
Copyright By PowCoder代写 加微信 powcoder
Motion-Planning Framework
Continuous representation
Discretization
Graph searching
(Search: blind, best-first, informed) [e.g., Grassfire, Dijkstra’s, A*]
Recap: Configuration Space Obstacle
• On the right hand side of this figure we plot the configuration space obstacle corresponding to the geometric obstacle shown in the left half.
• Again the C-space obstacle denotes the set of configurations that the robot CANNOT attain because of collision with the obstacle.
Recap: Planning Path through Configuration Space
• In this setting the task of planning a path for our robot correspond to planning a trajectory through configuration space from the starting configuration to the end configuration.
Configuration Space: Tool to Map a Robot to a Point
free space s
semi-free path
Recal: Motion Constraints
There are numerous types of motion constraints that may be specified, which can be categorized by the manner in which they constrain the robot’s path:
• Local (kinematic): each point along the path must satisfy some condition, e.g., avoid collision with obstacles.
• Differential (dynamic): the derivative along the path must satisfy some condition, e.g., bounded curvature.
• Global: the entire path must satisfy some condition, e.g., path length must be within some bound.
When both kinematic and dynamic constraints are present, this is known as a kinodynamic planning problem.
Local constraints include collisions with obstacles. Differential constraints limit the derivative of the robot’s path. Global constraints are imposed on the entire path, e.g., the robot must return to its base before its battery is exhausted.
Types of Path Constraints
Local constraints:
lie in free space
Differential constraints:
have bounded curvature
Global constraints:
have minimal length
Motion-Planning Framework
Continuous representation
Discretization
Graph searching
(blind, best-first, A*)
Path-Planning Approaches
1. Roadmap
Represent the connectivity of the free space by
a network of 1-D curves
2. Cell decomposition
Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells
3. Potential field
Define a function over the free space that has a global minimum at the goal configuration and follow its steepest descent
Roadmap Methods
§Visibility graph
Introduced in the Shakey project at SRI in the late 60s.
Can produce shortest paths in 2- D configuration spaces.
Visibility Graph Path Planning
How does a Mobile Robot get from A to B?
• Assume robot is a point in 2-D planar space
• Assume obstacles are 2-D polygons
• Create a Visibility Graph:
– Nodes are start point, goal point, vertices of obstacles – Connect all nodes which are “visible” – straight line un-obstructed path between any 2 nodes
– Includes all edges of polygonal obstacles
• Use A*/Dijkstra’s to search for path from start to goal
Visibility Graph – VGRAPH
• Start, goal, vertices of obstacles are graph nodes • Edges are “visible” connections between nodes,
including obstacle edges
Visibility Graph – VGRAPH
• A* search for shortest path via visible vertices
VGRAPH: Grown Obstacles
• VGRAPH algorithm assumes point robot • What if robot has mass, size?
• Solution: expand each obstacle by size of the robot – create Grown Obstacle Set
• This effectively “shrinks” the robot back to a point
• Graph search of the VGRAPH will now find shortest path
if one exists using grown obstacle set
robot obstacle grown obstacle
Simple Algorithm
Install all obstacles vertices in VG, plus the start and goal positions
For every pair of nodes u, v in VG
If segment(u,v) is an obstacle edge then
10. Search VG using A*
insert (u,v) into VG else
for every obstacle edge e
if segment(u,v) intersects e
then goto 2 insert (u,v) into VG
Complexity
• Simple algorithm: O(n3) time
• Rotational sweep: O(n2 log n)
• Optimal algorithm: O(n2)
• Space: O(n2)
Visibility Graph – VGRAPH
Visibility Graph – VGRAPH
Visibility Graph – VGRAPH
Visibility Graph – VGRAPH
• Thisvisibilitygraphalgorithmisactually complete.
• Thatisitwillfindapathifoneexistsand report failure if no path can be constructed. This could happen if the start or destination is surrounded by an obstacle.
Visibility Graph – VGRAPH
• Itcanbeprovedthatthisalgorithmwill actually construct the shortest possible path between the two points.
• Youcanseetheintuitionforthisbythinking of the path between the two vertices as a piece of string, imagine what would happen if you pulled this string as tight as possible to eliminate all of the slack, the resulting trajectory would consist of a series of straight lines between vertices corresponding exactly to the edges of the visibility graph.
Path-Planning Approaches
1. Roadmap
Represent the connectivity of the free space
by a network of 1-D curves
2. Cell decomposition
Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells
3. Potential field
Define a function over the free space that has a global minimum at the goal configuration and follow its steepest descent
Cell-Decomposition Methods
Two classes of methods:
§Exact cell decomposition
The free space F is represented by a collection of non-overlapping cells whose union is exactly F
Example: trapezoidal decomposition
Trapezoidal decomposition
Trapezoidal decomposition
Trapezoidal decomposition
Trapezoidal decomposition
Trapezoidal decomposition
Trapezoidal decomposition
Trapezoidal decomposition
Trapezoidal decomposition
critical eventsàcriticality-based decomposition
Trapezoidal decomposition
Planar sweep ààO(n log n) time, O(n) space
Cell-Decomposition Methods
Two classes of methods:
§Exact cell decomposition
§Approximate cell decomposition
F is represented by a collection of
non-overlapping cells whose union is contained in F
Examples: quadtree, octree, 2n-tree
Octree Decomposition
Sketch of Algorithm
1. Compute cell decomposition down to some resolution
2. Identify start and goal cells
3. Search for sequence of empty/mixed cells between start and goal cells
4. If no sequence, then exit with no path
5. If sequence of empty cells, then exit with Solution
6. If resolution threshold achieved, then exit with failure
7. Decompose further the mixed cells
8. Return to 2
Path-Planning Approaches
1. Roadmap
Represent the connectivity of the free space
by a network of 1-D curves
2. Cell decomposition
Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells
3. Potential field
Define a function over the free space that has a global minimum at the goal configuration and follow its steepest descent
Potential Field Methods
§Approach initially proposed for real-time collision avoidance [Khatib, 86]. Hundreds of papers published on it.
Goal Force
Obstacle Force
Attractive and Repulsive fields
Local-Minimum Issue
§§Perform best-first search (possibility of combining with approximate cell decomposition)
§§Alternate descents and random walks
§§Use local-minimum-free potential (navigation function)
Sketch of Algorithm (with best-first search)
1. Place regular grid G over space
2. Search G using best-first search algorithm with potential as heuristic function
Simple Navigation Function
Simple Navigation Function
Simple Navigation Function
Completeness of Planner
• A motion planner is complete if it finds a collision-free path whenever one exists and return failure otherwise.
• Visibility graph, exact cell decomposition, navigation function provide complete planners.
• Weaker notions of completeness, e.g.: – resolution completeness
(PF with best-first search) – probabilistic completeness
(PF with random walks)
• A probabilistically complete planner returns a path with high probability if a path exists. It may not terminate if no path exists.
• A resolution complete planner discretizes the space and returns a path whenever one exists in this representation.
Preprocessing /Query Processing
• Preprocessing:
Compute visibility graph, Voronoi diagram, cell
decomposition, navigation function
Query processing:
– Connect start/goal configurations to visibility graph, Voronoi diagram
– Identify start/goal cell – Search graph
Some Issues
• Space dimensionality
• Geometric complexity of the free space
• Constraints other than avoiding collision
• The goal is not just a position to reach
程序代写 CS代考 加微信: powcoder QQ: 1823890830 Email: powcoder@163.com