topic 1: discrete and "2d" planning
TRANSCRIPT
Discrete and “2D” Planning Lydia Tapia [email protected]
Discrete Planning • Each state can be represented as a discrete state • World can be transformed by the application of actions • Given a start and a goal state • Find sequence of actions to take from start to goal
Labyrinth search • <=4 possible moves from
each state
Rubik’s Cube • 12 possible actions for
each state • Tree of possible actions • We want to build as little of
the tree as possible
Possible Search Methods • General Forward Search
Other Search Methods • Depth-First Search
• Priority queue becomes Last In First Out
• Dijkstra’s Algorithm • Need costs on edges • Priority queue is sorted by cost-to-come • Cost-to-come becomes optimal as all edges are searched
• A* • Attempt to reduces number of states explored by Dijkstra’s • Priority queue is sorted by sum of cost-to-come and cost-to-go • Cost-to-go estimate must be done well
Other Search Methods II • Best First
• Priority queue sorted by cost-to-go • Always selecting the “next best step” • Very Greedy approach
• Iterative deepening • Use depth-first search and find all states that are some distance, d,
from current state • If goal is not found, then find all states d+1 from current state • Works well with large branching factors
Other Search Methods III • Backwards Search
• Start search from goal • Depends on branching factor
• Bidirectional Search • Search from both start and goal at same time • Need to merge the search trees together at some point
Logic Formulations • Explicit representations of state space can be huge! • Implicit representations would be preferred • Logic-based representations were popular (1950s-1980s) • Outputs clear plan to user without any intervention • Extremely hard to generalize
STRIPS-like Planning • STRIPS: Stanford Research Institute Problem Solver
STRIPS-like Flashlight Problem 1. Set of Instances, I
I={Battery1, Battery2, Cap, Flashlight}
2. Set of Predicates, P On and In For example, On(Cap, Flashlight) In(Battery1, Flashlight)
3. Set of Operators, O
4. Initial State S= {On(Cap, Flashlight)}
5. Goal State G={On(Cap, Flashlight), In(Battery1, Flashlight), In(Battery2, Flashlight)}
Logic Formulations
Blocks world 1. Set of Instances, I
2. Set of Predicates, P
3. Set of Operators, O
4. Initial State
5. Goal State
What is Motion Planning
The Alpha Puzzle: A MP Benchmark
Objective: Separate the two tubes, one is the ‘robot’ the other is an ‘obstacle’.
start
goal obstacles
(Basic) Motion Planning (in a nutshell):
Given a movable object, find a sequence of valid configurations that moves the object from the start to the goal.
[Movie from the Parasol Lab, Texas A&M Univ.]
Configuration Space (C-Space)
C-obst
C-obst
C-obst
C-obst
C-obst
C-Space
6D C-space (x,y,z,pitch,roll,yaw)
3D C-space (x,y,z) 3D C-space
(α,β,γ) α β γ
• “robot” maps to a point in higher dimensional space • parameter for each degree of freedom (dof) of robot • C-space = set of all robot placements • C-obstacle = infeasible robot placements
2n-D C-space (φ1, ψ1, φ2, ψ2, . . . , φ n, ψ n)
robot
obst
obst
obst
obst
x y
C-obst
C-obst C-obst
C-obst
robot Path is swept volume
Motion Planning in C-space
Path is 1D curve
Workspace
C-space Simple workspace obstacle transformed Into complicated C-obstacle!!
Algorithms • Visibility Graph • Cell Decomposition • Potential Fields
• State of the Art: Sampling-based Algorithms • Basic PRM • OBPRM
• Current Open Problems: • Feature-Sensitive Motion Planning • Super high-dimensional problems
Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above
Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.
Algorithms • Visibility Graph • Cell Decomposition • Potential Fields
• State of the Art: Sampling-based Algorithms • Basic PRM • OBPRM
• Current Open Problems: • Feature-Sensitive Motion Planning • Super high-dimensional problems
Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above
Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.
The Visibility Graph in Action (Part 1)
• First, draw lines of sight from the start and goal to all “visible” vertices and corners of the world.
start
goal
The Visibility Graph in Action (Part 2)
• Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight.
start
goal
The Visibility Graph in Action (Part 3)
• Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight.
start
goal
The Visibility Graph in Action (Part 4)
• Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight.
start
goal
The Visibility Graph (Done) • Repeat until you’re done.
start
goal
Since the map was in C-space, each line potentially represents part of a path from the start to the goal.
Visibility graph drawbacks Visibility graphs do not preserve their optimality in higher dimensions:
In addition, the paths they find are “semi-free,” i.e. in contact with obstacles.
shortest path
shortest path within the visibility graph
No clearance
Algorithms • Visibility Graph • Voronoi Cells • Cell Decomposition • Potential Fields
• State of the Art: Sampling-based Algorithms • Basic PRM • OBPRM
• Current Open Problems: • Feature-Sensitive Motion Planning • Super high-dimensional problems
Goal reduce the N-dimensional configuration space to a set of one-D paths to search.
Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above
Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.
Goal optimize clearance
“official” Voronoi diagram
(line segments make up the Voronoi diagram isolates a set of points)
Roadmap: Voronoi diagrams
Generalized Voronoi Graph (GVG): locus of points equidistant from the closest two or more obstacle boundaries, including the workspace boundary.
Property: maximizing the clearance between the points and obstacles.
Roadmap: Voronoi diagrams • GVG is formed by paths equidistant from the two closest objects
• maximizing the clearance between the obstacles.
• This generates a very safe roadmap which avoids obstacles as much as possible
Algorithms • Visibility Graph • Cell Decomposition • Potential Fields
• State of the Art: Sampling-based Algorithms • Basic PRM • OBPRM
• Current Open Problems: • Feature-Sensitive Motion Planning • Super high-dimensional problems
Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above
Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.
29
Exact Cell Decomposition
Decomposition of the free space into trapezoidal & triangular cells
Connectivity graph representing the adjacency relation between the cells
(Sweepline algorithm)
Trapezoidal Decomposition:
Exact Cell Decomposition
Search the graph for a path (sequence of consecutive cells)
Trapezoidal Decomposition:
Exact Cell Decomposition
Transform the sequence of cells into a free path (e.g., connecting the mid-points of the intersection of two consecutive cells)
Trapezoidal Decomposition:
Obtaining the minimum number of convex cells is NP-complete.
Optimality
15 cells 9 cells
Trapezoidal decomposition is exact and complete, but not optimal
Trapezoidal Decomposition:
Quadtree Decomposition:
Approximate Cell Decomposition
recursively subdivides each mixed obstacle/free (sub)region into four quarters... Quadtree:
34
further decomposing...
recursively subdivides each mixed obstacle/free (sub)region into four quarters... Quadtree:
Quadtree Decomposition:
Again, use a graph-search algorithm to find a path from the start to goal
Quadtree is this a complete path-planning algorithm?
i.e., does it find a path when one exists ?
• The rectangle cell is recursively decomposed into smaller rectangles • At a certain level of resolution, only the cells whose interiors lie entirely in the free space are used • A search in this graph yields a collision free path
Quadtree Decomposition:
Algorithms • Visibility Graph • Cell Decomposition • Potential Fields
• State of the Art: Sampling-based Algorithms • Basic PRM • OBPRM
• Current Open Problems: • Feature-Sensitive Motion Planning • Super high-dimensional problems
Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above
Goal reduce the computational cost of configuration space, and only have a set of 1-D paths to search.
Potential Field Method Potential Field (Working Principle)
– The goal location generates an attractive potential – pulling the robot towards the goal – The obstacles generate a repulsive potential – pushing the robot far away from the obstacles – The negative gradient of the total potential is treated as an artificial force applied to the robot
-- Let the sum of the forces control the robot
C-obstacles
• Compute an attractive force toward the goal
Potential Field Method
C-obstacles
Attractive potential
Potential Field Method
Repulsive Potential
Create a potential barrier around the C-obstacle region that cannot be traversed by the robot’s configuration
It is usually desirable that the repulsive potential does not affect the motion of the robot when it is sufficiently far away from C-obstacles
• Compute a repulsive force away from obstacles
• Compute a repulsive force away from obstacles
Potential Field Method
• Repulsive Potential
• Sum of Potential
Potential Field Method
C-obstacle
Attractive potential Repulsive potential
Sum of potentials
• After get total potential, generate force field (negative gradient)
• Let the sum of the forces control the robot
To a large extent, this is computable from sensor readings
Equipotential contours
Negative gradient
Total potential
Potential Field Method
random walks are not perfect...
Potential Field Method • Spatial paths are not preplanned and can be generated in real time
• Planning and control are merged into one function
• Smooth paths are generated
• Planning can be coupled directly to a control algorithm
Pros:
• Trapped in local minima in the potential field
• Because of this limitation, commonly used for local path planning
• Use random walk, backtracking, etc to escape the local minima
Cons: