Path Planning in Partially-Known Environments Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834 Cognitive Robotics February 17 th , 2004 Outline ? Path Planning in Partially Known Environments. ? Finding the Shortest Path to the Goal. ? Alternative Approaches to Path Planning in Partially Known Environments. ? Continuous Optimal Path Planning ? Dynamic A* ? Incremental A* Example: Partially Unknown Environ JMNO EILG BDHK SACF ? States F and H are blocked. ? Robot doesn’t know this. Solutions: [Goto & Stenz, 87] 1. Generate global path plan from initial map. 2. Execute path plan ? While updating map based on sensors. 3. If a new obstacle is found obstructing the path, ? Then locally circumvent. 4. If route is completely obstructed, ? Then globally replan with updated map. Robot Generates Global Path JMNO EILG BDHK SACF h = 3 h = 3 h = 3 h = 3 h = 2 h = 2 h = 2 h = 2 h = 2 h = 2 h = 0 h = 1 h = 1 h = 1 h = 1 h = 1 ? Single Source Shortest path from Goal. ? Stop when robot location ( ) reached. Robot Generates Global Path JMNO EILG BDHK SACF h = 5 h = 4 h = 3 h =4 h = 4 h = 3 h = 2 h = 3 h = 3 h = 2 h = 0 h = 2 h = 1 h = 2 h = 1 h = 1 ? Single Source Shortest path from Goal. ? Stop when robot location ( ) reached. Robot Executes Global Path JMNO EILG BDHK SACF h = 5 h = 4 h = 3 h = 4 h = 4 h = 3 h = 2 h = 3 h = 3 h = 2 h = 0 h = 2 h = 1 h = 2 h = 1 h = 1 Robot Locally Circumvents JMNO EILG BDHK SACF h = 5 h = 4 h = 3 h = 4 h = 4 h = 3 h = 2 h = 3 h = 3 h = 2 h = 2 h = 1 h = 2 h = 1 h = 1 Robot Globally Replans JMNO EILG BDHK SACF h = 3 h = 2 h = 3 h = 5 h = 4 h = 3 h = 4 h = 4 h =5 h = 0h = 1 h = 2 h = 1 h = 1 ? Rerun Single Source Shortest path from Goal. ? Stop when robot location ( ) reached. Outline ? Path Planning in Partially Known Environments. ? Finding the Shortest Path to the Goal. ? Alternative Approaches to Path Planning in Partially Known Environments. ? Continuous Optimal Path Planning ? Dynamic A* ? Incremental A* u v 1 x y 2 10 5 7 9 23 46 s Weighted Graphs and Path Lengths Graph G = <V, E> Weight function w: E →? Path p = < v o , v 1 , … v k > Path weight w(p) = Σ w(v i-1 ,v i ) Shortest path weight δ(u,v) = min {w(p) : u→ p v } else ∞ Single Source Shortest Path u v 1 x y 2 10 5 7 9 23 46 s Problem: Compute shortest path to all vertices from source s Single Source Shortest Path 8 9 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Problem: Compute shortest path to all vertices from source s ? estimate d[v] estimated shortest path length from s to v Single Source Shortest Path 8 9 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Problem: Compute shortest path to all vertices from source s ? estimate d[v] estimated shortest path length from s to v ? predecessor π[v] final edge of shortest path to v ? induces shortest path tree 89 u v 1 57 x y 2 0 10 5 7 9 2346 g Single Destination Shortest Path Problem: Compute shortest path from all vertices to goal g ? estimate h[v] estimated shortest path length from s to v ? descendant b[v] first edge of shortest path from v to g ? induces shortest path tree 8 9 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Properties of Shortest Path ? Subpaths of shortest paths are shortest paths. ?s → p v = <s, x, u, v> ?s → p u = <s, x, u> ?s → p x = <s, x> ?x → p v = <x, u, v> ?x → p v = <x, u> ?u → p v = <u, v> Properties of Shortest Path 8 9 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Corollary: Shortest paths can be grown from shortest paths. ? The length of shortest path s → p u →v is δ(s,v) = δ(s,u) + w(u,v). ? ? <u,v> ∈ E δ(s,v) ≤ δ(s,u) + w(u,v) ∞ ∞ u v 1 ∞ ∞ x y 2 0 10 5 7 9 23 46 s Initialize-Single-Source(G, s) 1. for each vertex v ∈ V[G] 2. do d[u]←∞ 3. π[v]←NIL 4. d[s] ←0 Idea: Start With Upper Bound O(v) Idea: Relax Bounds to Shortest Path 5 9 u v 2 5 6 u v 2 5 6 u v 2 Relax(u, v) 5 7 u v Relax(u, v) 2 Relax (u, v, w) 1. if d[u] + w(u,v) < d[v] 2. do d[v] ←d[u] + w(u,v) 3. π[v] ←u Properties of Relaxing Bounds 5 9 u v 2 5 6 u v 2 5 9 u v 2 Relax(u, v) 5 6 u v Relax(u, v) 2 After calling Relax(u, v, w) ? d[u] + w(u,v) ≥ d[v] d remains a shortest path upperbound after repeated calls to Relax. Once d[v] is the shortest path its value persists ∞ ∞ u v 1 ∞ ∞ x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {s,u,v,x,y} S = {} Vertices to relax Vertices with shortest path value ∞ ∞ u v 1 ∞ ∞ x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {x,y,u,v} S = {s} Vertices to relax Vertices with shortest path value 10 ∞ u v 1 ∞ ∞ x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {x,y,u,v} S = {s} Vertices to relax Vertices with shortest path value Shortest path edge Π[v] = bold arrows 10 ∞ u v 1 5 ∞ x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {x,y,u,v} S = {s} Vertices to relax Vertices with shortest path value Shortest path edge Π[v] = arrows 10 ∞ u v 1 5 ∞ x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {x,y,u,v} S = {s} Vertices to relax Vertices with shortest path value Shortest path edge Π[v] = arrows 8 14 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {y,u,v} S = {s, x} Vertices to relax Vertices with shortest path value Shortest path edge Π[v] = arrows 8 14 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {y,u,v} S = {s, x} Vertices to relax Vertices with shortest path value Shortest path edge Π[v] = arrows 8 13 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {u,v} S = {s, x, y} Vertices to relax Vertices with shortest path value Shortest path edge Π[v] = arrows 8 9 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {v} S = {s, x, y, u} Vertices to relax Vertices with shortest path value Shortest path edge Π[v] = arrows 8 9 u v 1 5 7 x y 2 0 10 5 7 9 23 46 s Dijkstra’s Algorithm Idea: Greedily relax out arcs of minimum cost nodes Assume all edges are non-negative. Q = {} S = {s, x, y, u, v} Vertices to relax Vertices with shortest path value Shortest path edge Π[v] = arrows Dijkstra’s Algorithm Repeatedly select minimum cost node and relax out arcs DIJKSTRA(G,w,s) 1. Initialize-Single-Source(G, s) 2. S ← ? 3. Q ← V[G] 4. while Q ≠? 5. do u ←Extract-Min(Q) 6. S ←S ∪ {u} 7. for each vertex v ∈ Adj[u] 8. do Relax(u, v, w) O(V) O(V) or O(lg V) w fib heap * O(V) O(E) = O(V 2 +E) = O(VlgV+E) Outline ? Path Planning in Partially Known Environments. ? Finding the Shortest Path to the Goal. ? Alternative Approaches to Path Planning in Partially Known Environments. ? Continuous Optimal Path Planning – Dynamic A* – Incremental A* [Lumelsky, Mukhopadhya & Sun, 86] 1. Follow path directly towards goal, while assuming there are no obstacles. 2. When an obstacle is found obstructing the path, Then move around perimeter. 3. Stop at point on the obstacle nearest the goal. 4. Proceed from point, directly towards the goal ? (i.e., goto 1). Robot Generates Global Path JMNO EILG BDHK SACF ? Single Source Shortest path from Goal. ? Stop when robot location ( ) reached. [Pirazdeh, Snyder, 90] Idea: Wander map until goal is achieved, with bias towards exploration. ? Map state cost denotes # times visited. 1. Initialize all states to 0. 2. At each step: ? Greedily move to adjacent state with lowest cost. ? Increment cost of a state each time visited. ? Creates penalty for repeatedly traversing same state. 3. Stop when goal achieved. Greedily Move to Unexplored States JMNO EILG BDHK SACF v = 0 v = 0 v = 0 v = 0 v = 0 v = 0 v = 0 v = 0 v = 0 v = 0v = 0 v = 0 v = 0 v = 0 ? Move to lowest cost neighbor. ? Increment v of new state. Greedily Move to Unexplored States JMNO EILG BDHK SACF v = 0 v = 0 v = 0 v = 1 v = 0 v = 0 v = 0 v = 0 v = 0 v = 0v = 0 v = 0 v = 0 v = 0 v = 1 v = 1 ? Move to lowest cost neighbor. ? Increment v of new state. Greedily Move to Unexplored States JMNO EILG BDHK SACF v = 0 v = 0 v = 0 v = 1 v = 0 v = 0 v = 0 v = 1 v = 1 v = 0v = 0 v = 0 v = 0 v = 0 v = 2 v = 2 v = 1 ? Move to lowest cost neighbor. ? Increment v of new state. Learning Realtime A* (LRTA*) [Korf, 90] ? Use initial map to estimate cost of path to goal for each state. ? Update map with backup cost as the robot moves through the environment. LRTA* <s’,a, s> current state, previous action and state. result table of <s, a> →s H table of s →cost estimate LRTA*(s’) returns an action If s’ = goal then return stop If s’ is a new state, then H(s’) ←h(s’) Unless s is null result [a, s] ←s’ H [s] ←min One_Step_Cost(s, b, result[b,s], H) b∈Actions(s) a ←b∈Actions(s’) minimizing One_Step_Cost(s’, b, result[b,s’], H) s ←s’ return a One_Step_Cost(s, a, s’, H) returns cost estimate If s’ is undefined then return h(s) Else return c(s,a,s’) + H[s’] LRTA* Example JMNO EILG BDHK SACF H = 5 H = 4 H = 3 H = inf H = inf LRTA* Example JMNO EILG BDHK SACF H = 5 H = 4 H = 5 H = inf H = infH = 3H = 4 H = 2 H = 3 H = 3 H = 1 H = 2 H = 0 [Zellinsky, 92] 1. Generate global path plan from initial map. 2. Repeat until goal reached or failure: ? Execute next step in current global path plan ? Update map based on sensors. ? If map changed generate new global path from map. Compute Optimal Path JMNO EILG BDHK SACF Begin Executing Optimal Path JMNO EILG BDHK SACF h = 5 h = 4 h = 3 h = 4 h = 4 h = 3 h = 2 h = 3 h = 3 h = 2 h = 1 h = 2 h = 2 h = 1 h = 0 h = 1 ? Robot moves along backpointers towards goal. ? Uses sensors to detect discrepancies along way. Obstacle Encountered! JMNO EILG BDHK SACF h = 5 h = 4 h = 3 h = 4 h = 4 h = 3 h = 2 h = 3 h = 3 h = 2 h = 1 h = 1 h = 2 h = 1 h = 0 h = 1 ? At state A, robot discovers edge from D to H is blocked (cost 5,000 units). ? Update map and reinvoke planner. Continue Path Execution JMNO EILG BDHK SACF h = 5 h = 4 h = 3 h = 4 h = 4 h = 3 h = 2 h = 3 h = 3 h = 2 h = 1 h = 1 h = 2 h = 1 h = 0 h = 1 ? A’s previous path is still optimal. ? Continue moving robot along back pointers. Second Obstacle, Replan! JMNO EILG BDHK SACF h = 5 h = 4 h = 3 h = 4 h = 4 h = 3 h = 2 h = 3 h = 3 h = 2 h = 1 h = 1 h = 2 h = 1 h = 0 h = 1 ? At C robot discovers blocked edges from C to F and H (cost 5,000 units). ? Update map and reinvoke planner. Path Execution Achieves Goal h = 5 JMNO EILG BDHK SACF h = 5 h = 4 h = 3 h = 4 h = 4 h = 3 h = 2 h = 3 h = 2 h = 1 h = 1 h = 2 h = 1 h = 0 h = 1 ? Follow back pointers to goal. ? No further discrepancies detected; goal achieved! Continuous Optimal Planning 1. Generate global path plan from initial map. 2. Repeat until Goal reached, or failure. ? Execute next step of current global path plan. ? Update map based on sensor information. ? Incrementally update global path plan from map changes. 3. Next Lecture: Incremental Methods ? 1 to 3 orders of magnitude speedup relative to a non-incremental path planner.