The%big%picture:%from%Perception%to%Planning%...
Transcript of The%big%picture:%from%Perception%to%Planning%...
![Page 1: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/1.jpg)
1
The big picture: from Perception to Planning to Control
Real World
Sensors
PerceptionLocation, Map
Signals: video, inertial, range
![Page 2: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/2.jpg)
2
Planning vs Control0. In control we go from to A to B in free space given the relative pose to target B.1. Assume robot is a point and that it knows its position in a map2. Given a map go from A to B avoiding obstacles3. We can find waypoints and go from waypoint to waypoint using control.
A
B
![Page 3: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/3.jpg)
3
We need a discrete representation for the world: The world as a graph
• A graph is a collection of nodes (vertices) and edges G = (V, E)
• In motion planning, a node represents a salient location, and an edge connects two nodes if they one can be accessed from the other: if not mutual, then graph is directed.
• Edges can have a weight representing the cost of moving from one vertex to the other
![Page 4: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/4.jpg)
4
The world as a graph
The world as a street map:Intersections are vertices in a graphTraversable paths are edges.
The world as regular grid:Every cell is a vertex, adjacent cells are connected with vertices
![Page 5: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/5.jpg)
5
The world as a grid• A grid induces a graph where each node corresponds to a cell and an edge connects nodes of cellsthat neighbor each other.
• Four-point connectivity will only have edges to the north, south, east, and west
• eight-point connectivity will have edges to all pixels surrounding the current cell.
![Page 6: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/6.jpg)
6
In practice: from point clouds to cell grids
https://www.frc.ri.cmu.edu/~hpm/book98/fig.ch2/p035.html
![Page 7: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/7.jpg)
7
Shortest Paths in Weighted Graphs
1. Depth First Search (DFS) and Breadth First Search (BFS)2. Dijkstra3. A*
![Page 8: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/8.jpg)
8
DFS: From CIS121
![Page 9: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/9.jpg)
9
From CIS 121
![Page 10: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/10.jpg)
10
From CIS 121
![Page 11: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/11.jpg)
11
BFS gives shortest path in unweighted graphs
Each cell corresponds to the length of the path from start cell
![Page 12: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/12.jpg)
12
But DFS might find the goal faster
Each cell corresponds to the length of the path from start cell
![Page 13: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/13.jpg)
13
Shortest Paths on Weighted Graphs
Property 1:A subpath of a shortest path is itself a shortest pathProperty 2:There is a tree of shortest paths from a start vertex to all the other vertices
![Page 14: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/14.jpg)
14
Shortest Paths on Weighted Graphs (Dijkstra)
Algorithm Dijkstra(G, start)Q � priority queuefor all v �G.vertices()
if v = startsetDistance(v, 0)
elsesetDistance(v, �)
l � Q.insert(getDistance(v), v)setLocator(v,l)
while �Q.isEmpty()u �Q.removeMin()for all e �G.incidentEdges(u)
z � G.opposite(u,e)r � getDistance(u) + weight(e)if r < getDistance(z)
setDistance(z,r)Q.replaceKey(z,r)Given graph G with weighted
edges and a start vertex, find shortest path to every vertex
![Page 15: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/15.jpg)
1515
Dijkstra Example
CB
A
E
D
F
0
428
� �
48
7 1
2 5
2
3 9
CB
A
E
D
F
0
328
5 11
48
7 1
2 5
2
3 9
CB
A
E
D
F
0
328
5 8
48
7 1
2 5
2
3 9
CB
A
E
D
F
0
327
5 8
48
7 1
2 5
2
3 9
![Page 16: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/16.jpg)
1616
Dijkstra (cont.)
CB
A
E
D
F
0
327
5 8
48
7 1
2 5
2
3 9
CB
A
E
D
F
0
327
5 8
48
7 1
2 5
2
3 9
![Page 17: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/17.jpg)
17
Dijkstra on grid
![Page 18: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/18.jpg)
18
Running times
DFS and BFS run in Θ(|V|+|E|)while Dijkstra runs in Θ((|E|+|V|) log |V|)
![Page 19: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/19.jpg)
19
Remarks about Dijkstra I
• Dijkstra algorithm finds the shortest path to all nodes in the weighted graph (same does BFS in unweighted graphs)
• What if we are focused on finding the shortest path to the goal?
• Dijkstra builds a Priority Queue using as ordering value for each node the distance from start to this node.
![Page 20: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/20.jpg)
20
Remarks about Dijkstra II
• We could modify Dijkstra so that only PROMISING nodes are removed from the list.
• Instead of selecting the node closest to the starting point we could select the node with minimum “estimate” of the path from start to goal through this node.
![Page 21: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/21.jpg)
21
A* search
• Suppose that g(v) is the distance from starting vertex start to current vertex v.
• Assume that some oracle gives us an estimate of the shortest path from v to goal. We call this a heuristic h(v).
• We define a new function f(v) = g(v) + h(v) that is supposed to predict the shortest path from start to goal through v.
• Now we can run again Dijkstra and use f(v) as the priority value in the priority queue.
• Relaxation (replacing of vertices in the queue is still based on the g(v) function only.
![Page 22: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/22.jpg)
22
Dijkstra vs A*g[start]=0
for all v in G
g[v]=inf
Q.add(v)
while Q nonempty
u = Q.removeMin()
for v adjacent to u
if g[u]+w[u,v]<g[v]
g[v]=g[u]+w[u,v]
Q.replaceKey(v)
f[start]=0+h[start]
for all v in G
f[v]=inf+h[v]
Q.add(v)
while Q nonempty
u = Q.removeMin()
for v adjacent to u
if g[u]+w[u,v]<g[v]
g[v]=g[u]+w[u,v]
f[v] = g[v]+h[v]
Q.replaceKey(v)
![Page 23: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/23.jpg)
23
Possible guesses for heuristic h(v)
• Straight line (the L2 distance)• Sqrt(Dx2+Dy2)• The Manhattan distance (in math known as L1): |Dx| + |Dy|
![Page 24: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/24.jpg)
24
Any formal requirements for optimality?
• Admissible function: underestimates the true cost• h(v) <= true minimal cost (v, goal)
• Consistent function: satisfies triangle inequality• h(v) <= h(u,v) + cost(v,goal)
• The straight line (L2) satisfies both properties
![Page 25: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/25.jpg)
25
Adding a CLOSED setCLOSED = empty
f[start]=0+h[start]
for all v in G
f[v]=inf+h[v]
Q.add(v)
while Q nonempty
u = Q.removeMin()
add u to CLOSED
for v adjacent to u and not in CLOSED
if g[u]+w[u,v]<g[v]
g[v]=g[u]+w[u,v]
f[v] = g[v]+h[v]
Q.replaceKey(v)
![Page 26: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/26.jpg)
26
Optimality (in terms of number of nodes visited)
• A* implemented with CLOSED set is optimal if h is both admissible and consistent.
• A* considers the fewest nodes of any other algorithm that uses an admissible heuristic h.
• Time is polynomial (for a single goal) if • |h(v)-hoptimal(v)| is big-Oh of log(hoptimal(v))
![Page 27: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/27.jpg)
27
Dijkstra vs A* on 2D grid
![Page 28: The%big%picture:%from%Perception%to%Planning% to%Controlcis390/dynamic/slides/CIS390_Lecture4.pdf · 1 The%big%picture:%from%Perception%to%Planning% to%Control RealWorld Sensors Perception](https://reader033.fdocuments.in/reader033/viewer/2022042022/5e79d7ff7e75ef703672e372/html5/thumbnails/28.jpg)
28
Dijkstra vs A* on 2D grid with obstacles
H is Manhattan Distance