topic 1: discrete and "2d" planning
TRANSCRIPT
![Page 1: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/1.jpg)
Discrete and “2D” Planning Lydia Tapia [email protected]
![Page 2: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/2.jpg)
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
![Page 3: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/3.jpg)
Labyrinth search • <=4 possible moves from
each state
![Page 4: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/4.jpg)
Rubik’s Cube • 12 possible actions for
each state • Tree of possible actions • We want to build as little of
the tree as possible
![Page 5: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/5.jpg)
Possible Search Methods • General Forward Search
![Page 6: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/6.jpg)
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
![Page 7: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/7.jpg)
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
![Page 8: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/8.jpg)
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
![Page 9: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/9.jpg)
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
![Page 10: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/10.jpg)
STRIPS-like Planning • STRIPS: Stanford Research Institute Problem Solver
![Page 11: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/11.jpg)
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)}
![Page 12: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/12.jpg)
Logic Formulations
![Page 13: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/13.jpg)
Blocks world 1. Set of Instances, I
2. Set of Predicates, P
3. Set of Operators, O
4. Initial State
5. Goal State
![Page 14: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/14.jpg)
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.]
![Page 15: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/15.jpg)
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)
![Page 16: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/16.jpg)
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!!
![Page 17: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/17.jpg)
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.
![Page 18: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/18.jpg)
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.
![Page 19: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/19.jpg)
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
![Page 20: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/20.jpg)
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
![Page 21: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/21.jpg)
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
![Page 22: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/22.jpg)
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
![Page 23: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/23.jpg)
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.
![Page 24: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/24.jpg)
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
![Page 25: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/25.jpg)
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
![Page 26: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/26.jpg)
“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.
![Page 27: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/27.jpg)
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
![Page 28: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/28.jpg)
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.
![Page 29: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/29.jpg)
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:
![Page 30: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/30.jpg)
Exact Cell Decomposition
Search the graph for a path (sequence of consecutive cells)
Trapezoidal Decomposition:
![Page 31: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/31.jpg)
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:
![Page 32: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/32.jpg)
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:
![Page 33: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/33.jpg)
Quadtree Decomposition:
Approximate Cell Decomposition
recursively subdivides each mixed obstacle/free (sub)region into four quarters... Quadtree:
![Page 34: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/34.jpg)
34
further decomposing...
recursively subdivides each mixed obstacle/free (sub)region into four quarters... Quadtree:
Quadtree Decomposition:
![Page 35: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/35.jpg)
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:
![Page 36: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/36.jpg)
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.
![Page 37: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/37.jpg)
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
![Page 38: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/38.jpg)
• Compute an attractive force toward the goal
Potential Field Method
C-obstacles
Attractive potential
![Page 39: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/39.jpg)
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
![Page 40: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/40.jpg)
• Compute a repulsive force away from obstacles
Potential Field Method
• Repulsive Potential
![Page 41: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/41.jpg)
• Sum of Potential
Potential Field Method
C-obstacle
Attractive potential Repulsive potential
Sum of potentials
![Page 42: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/42.jpg)
• 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
![Page 43: Topic 1: Discrete and "2D" Planning](https://reader033.vdocuments.net/reader033/viewer/2022051318/5867733c1a28abd33f8bab51/html5/thumbnails/43.jpg)
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: