Path Planning in Partially-Known Environments. Prof. Brian Williams (help from Ihsiang Shu) /6.834 Cognitive Robotics February 17 th, 2004

Transcription

1 Path Planning in Partially-Known Enironments Prof. Brian Williams (help from Ihsiang Sh) 16.41/6.834 Cognitie Robotics Febrary 17 th, 004

2 Otline Path Planning in Partially Known Enironments. Finding the Shortest Path to the Goal. Alternatie Approaches to Path Planning in Partially Known Enironments. Continos Optimal Path Planning Dynamic A* Incremental A*

3 Example: Partially Unknown Eniron J M N O E I L G B D H K S A C F States F and H are blocked. Robot doesn t know this.

4 Soltions: [Goto & Stenz, 87] 1. Generate global path plan from initial map.. Execte path plan While pdating map based on sensors. 3. If a new obstacle is fond obstrcting the path, Then locally circment. 4. If rote is completely obstrcted, Then globally replan with pdated map.

5 Robot Generates Global Path J M N O E I L G B D H K h = 0 S A C F Single Sorce Shortest path from Goal. Stop when robot location ( ) reached.

6 Robot Generates Global Path h =4 J M N O E I L G h = 4 B D H K h = 5 h = 4 h = 0 S A C F Single Sorce Shortest path from Goal. Stop when robot location ( ) reached.

7 Robot Exectes Global Path h = 4 J M N O E I L G h = 4 B D H K h = 5 h = 4 h = 0 S A C F

8 Robot Locally Circments h = 4 J M N O E I L G h = 4 B D H K h = 5 h = 4 S A C F

9 Robot Globally Replans h = 4 J M N O E I L G h = 4 B D H K h = 5 h = 4 h =5 h = 0 S A C F Rern Single Sorce Shortest path from Goal. Stop when robot location ( ) reached.

10 Otline Path Planning in Partially Known Enironments. Finding the Shortest Path to the Goal. Alternatie Approaches to Path Planning in Partially Known Enironments. Continos Optimal Path Planning Dynamic A* Incremental A*

11 s Weighted Graphs and Path Lengths 10 5 x Graph G = <V, E> Weight fnction w: E R Path p = < o, 1, k > Path weight w(p) = Σ w( i-1, i ) Shortest path weight δ(,) = min {w(p) : p } else 7 y

12 Single Sorce Shortest Path s 10 5 x y Problem: Compte shortest path to all ertices from sorce s

13 Single Sorce Shortest Path s x y Problem: Compte shortest path to all ertices from sorce s estimate d[] estimated shortest path length from s to

14 Single Sorce Shortest Path s x y Problem: Compte shortest path to all ertices from sorce s estimate d[] estimated shortest path length from s to predecessor π[] final edge of shortest path to indces shortest path tree

15 Single Destination 9 7 Shortest Path y x g 7 Problem: Compte shortest path from all ertices to goal g estimate h[] estimated shortest path length from s to descendant b[] first edge of shortest path from to g indces shortest path tree 5

16 Properties of Shortest Path s Sbpaths of shortest paths are shortest paths. s p = <s, x,, > s p = <s, x, > s p x = <s, x> x p = <x,, > x p = <x, > p = <, > x 7 y

17 Properties of Shortest Path s x y Corollary: Shortest paths can be grown from shortest paths. The length of shortest path s p is δ(s,) = δ(s,) + w(,). <,> E δ(s,) δ(s,) + w(,)

18 Idea: Start With Upper Bond s x Initialize-Single-Sorce(G, s) 1. for each ertex V[G]. do d[] 3. π[] NIL 4. d[s] 0 y O()

19 Idea: Relax Bonds to Shortest Path 5 9 Relax(, ) Relax(, ) 5 6 Relax (,, w) 1. if d[] + w(,) < d[]. do d[] d[] + w(,) 3. π[]

20 Properties of Relaxing Bonds Relax(, ) 5 9 After calling Relax(,, w) d[] + w(,) d[] Relax(, ) 5 6 d remains a shortest path pperbond after repeated calls to Relax. Once d[] is the shortest path its ale persists

21 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x y Q = {s,,,x,y} S = {} Vertices to relax Vertices with shortest path ale

22 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x y Q = {x,y,,} S = {s} Vertices to relax Vertices with shortest path ale

23 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x 7 y Q = {x,y,,} S = {s} Vertices to relax Vertices with shortest path ale Shortest path edge Π[] = bold arrows

24 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x 7 y Q = {x,y,,} S = {s} Vertices to relax Vertices with shortest path ale Shortest path edge Π[] = arrows

25 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x 7 y Q = {x,y,,} S = {s} Vertices to relax Vertices with shortest path ale Shortest path edge Π[] = arrows

26 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x 7 y Q = {y,,} S = {s, x} Vertices to relax Vertices with shortest path ale Shortest path edge Π[] = arrows

27 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x 7 y Q = {y,,} S = {s, x} Vertices to relax Vertices with shortest path ale Shortest path edge Π[] = arrows

28 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x 7 y Q = {,} S = {s, x, y} Vertices to relax Vertices with shortest path ale Shortest path edge Π[] = arrows

29 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x 7 y Q = {} S = {s, x, y, } Vertices to relax Vertices with shortest path ale Shortest path edge Π[] = arrows

30 s Dijkstra s Algorithm Assme all edges are non-negatie. Idea: Greedily relax ot arcs of minimm cost nodes x 7 y Q = {} S = {s, x, y,, } Vertices to relax Vertices with shortest path ale Shortest path edge Π[] = arrows

31 Dijkstra s Algorithm Repeatedly select minimm cost node and relax ot arcs DIJKSTRA(G,w,s) 1. Initialize-Single-Sorce(G, s). S 3. Q V[G] 4. while Q 5. do Extract-Min(Q) 6. S S {} 7. for each ertex Adj[] 8. do Relax(,, w) O(V) O(V) or O(lg V) w fib heap * O(V) O(E) = O(V +E) = O(VlgV+E)

32 Otline Path Planning in Partially Known Enironments. Finding the Shortest Path to the Goal. Alternatie Approaches to Path Planning in Partially Known Enironments. Continos Optimal Path Planning Dynamic A* Incremental A*

33 [Lmelsky, Mkhopadhya & Sn, 86] 1. Follow path directly towards goal, while assming there are no obstacles.. When an obstacle is fond obstrcting the path, Then moe arond perimeter. 3. Stop at point on the obstacle nearest the goal. 4. Proceed from point, directly towards the goal (i.e., goto 1).

34 Robot Generates Global Path J M N O E I L G B D H K S A C F Single Sorce Shortest path from Goal. Stop when robot location ( ) reached.

35 [Pirazdeh, Snyder, 90] Idea: Wander map ntil goal is achieed, with bias towards exploration. Map state cost denotes # times isited. 1. Initialize all states to 0.. At each step: Greedily moe to adjacent state with lowest cost. Increment cost of a state each time isited. Creates penalty for repeatedly traersing same state. 3. Stop when goal achieed.

36 Greedily Moe to Unexplored States J M N O E I L G B D H K S A C F Moe to lowest cost neighbor. Increment of new state.

37 Greedily Moe to Unexplored States J M N O E I L G B D H K = S A C F Moe to lowest cost neighbor. Increment of new state.

38 Greedily Moe to Unexplored States J M N O E I L G 1 B D H K = 1 = 1 = 1 S A C F Moe to lowest cost neighbor. Increment of new state.

39 Learning Realtime A* (LRTA*) [Korf, 90] Use initial map to estimate cost of path to goal for each state. Update map with backp cost as the robot moes throgh the enironment.

40 LRTA* <s,a, s> crrent state, preios action and state. reslt table of <s, a> s H table of s cost estimate LRTA*(s ) retrns an action If s = goal then retrn stop If s is a new state, then H(s ) h(s ) Unless s is nll reslt [a, s] s H [s] min One_Step_Cost(s, b, reslt[b,s], H) b Actions(s) a b Actions(s ) minimizing One_Step_Cost(s, b, reslt[b,s ], H) s s retrn a One_Step_Cost(s, a, s, H) retrns cost estimate If s is ndefined then retrn h(s) Else retrn c(s,a,s ) + H[s ]

41 LRTA* Example J M N O E I L G H = inf B D H K H = 5 H = 4 H = 3 H = inf S A C F

42 LRTA* Example J M N O E I L G H = 4 H = 3 H = 3 H = H = 3 H = H = 1 H = inf H = 0 B D H K H = 5 H = 4 H = 5 H = inf S A C F

43 [Zellinsky, 9] 1. Generate global path plan from initial map.. Repeat ntil goal reached or failre: Execte next step in crrent global path plan Update map based on sensors. If map changed generate new global path from map.

44 Compte Optimal Path J M N O E I L G B D H K S A C F

45 Begin Execting Optimal Path h = 4 J M N O E I L G h = 4 B D H K h = 5 h = 4 h = 0 S A C F Robot moes along backpointers towards goal. Uses sensors to detect discrepancies along way.

46 Obstacle Encontered! h = 4 J M N O E I L G h = 4 B D H K h = 5 h = 4 h = 0 S A C F At state A, robot discoers edge from D to H is blocked (cost 5,000 nits). Update map and reinoke planner.

47 Contine Path Exection h = 4 J M N O E I L G h = 4 B D H K h = 5 h = 4 h = 0 S A C F A s preios path is still optimal. Contine moing robot along back pointers.

48 Second Obstacle, Replan! h = 4 J M N O E I L G h = 4 B D H K h = 5 h = 4 h = 0 S A C F At C robot discoers blocked edges from C to F and H (cost 5,000 nits). Update map and reinoke planner.

49 Path Exection Achiees Goal h = 4 J M N O E I L G h = 4 B D H K h = 5 h = 4 h = 5 h = 0 S A C F Follow back pointers to goal. No frther discrepancies detected; goal achieed!

50 Continos Optimal Planning 1. Generate global path plan from initial map.. Repeat ntil Goal reached, or failre. Execte next step of crrent global path plan. Update map based on sensor information. Incrementally pdate global path plan from map changes. 3. Next Lectre: Incremental Methods 1 to 3 orders of magnitde speedp relatie to a non-incremental path planner.