Post on 18-Mar-2020
1
The big picture: from Perception to Planning to Control
Real World
Sensors
PerceptionLocation, Map
Signals: video, inertial, range
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
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
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
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.
6
In practice: from point clouds to cell grids
https://www.frc.ri.cmu.edu/~hpm/book98/fig.ch2/p035.html
7
Shortest Paths in Weighted Graphs
1. Depth First Search (DFS) and Breadth First Search (BFS)2. Dijkstra3. A*
8
DFS: From CIS121
9
From CIS 121
10
From CIS 121
11
BFS gives shortest path in unweighted graphs
Each cell corresponds to the length of the path from start cell
12
But DFS might find the goal faster
Each cell corresponds to the length of the path from start cell
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
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
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
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
17
Dijkstra on grid
18
Running times
DFS and BFS run in Θ(|V|+|E|)while Dijkstra runs in Θ((|E|+|V|) log |V|)
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.
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.
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.
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)
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|
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
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)
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))
27
Dijkstra vs A* on 2D grid
28
Dijkstra vs A* on 2D grid with obstacles
H is Manhattan Distance