The big picture: from Perception to Planning to Control

Transcription

1 The big picture: from Perception to Planning to Control Perception Location, Map Signals: video, inertial, range Sensors Real World 1

2 Planning vs Control 0. 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 map 2. Given a map go from A to B avoiding obstacles 3. We can find waypoints and go from waypoint to waypoint using control. B A 2

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 3

4 The world as a graph The world as a street map: Intersections are vertices in a graph Traversable paths are edges. The world as regular grid: Every cell is a vertex, adjacent cells are connected with vertices 4

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. 5

6 In practice: from point clouds to cell grids 6

7 Shortest Paths in Weighted Graphs 1. Depth First Search (DFS) and Breadth First Search (BFS) 2. Dijkstra 3. A* 7

8 DFS: From CIS121 8

9 From CIS 121 9

10 From CIS

11 BFS gives shortest path in unweighted graphs Each cell corresponds to the length of the path from start cell 11

12 But DFS might find the goal faster Each cell corresponds to the length of the path from start cell 12

13 Shortest Paths on Weighted Graphs Property 1: A subpath of a shortest path is itself a shortest path Property 2: There is a tree of shortest paths from a start vertex to all the other vertices 13

14 Shortest Paths on Weighted Graphs (Dijkstra) Given graph G with weighted edges and a start vertex, find shortest path to every vertex Algorithm Dijkstra(G, start) Q priority queue for all v G.vertices() if v = start setdistance(v, 0) else setdistance(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) 14

15 Dijkstra Example B A C 4 D 4 B A C 4 D E F E F B A C 4 D 3 B A C 4 D E F E F 15 15

16 Dijkstra (cont.) B A C 4 D E F B A C 4 D E F 16 16

17 Dijkstra on grid 17

18 Running times DFS and BFS run in Θ( V + E ) while Dijkstra runs in Θ(( E + V ) log V ) 18

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. 19

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. 20

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. 21

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) 22

23 Possible guesses for heuristic h(v) Straight line (the L 2 distance) Sqrt(Dx 2 +Dy 2 ) The Manhattan distance (in math known as L 1 ): Dx + Dy 23

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 (L 2 ) satisfies both properties 24

25 Adding a CLOSED set CLOSED = 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) 25

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)-h optimal (v) is big-oh of log(h optimal (v)) 26

27 Dijkstra vs A* on 2D grid 27

28 Dijkstra vs A* on 2D grid with obstacles H is Manhattan Distance 28