Incremental Path Planning

Transcription

1 Incremental Path Planning April 4, 206 Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto

2 References Koenig, S., & Likhachev, M. (2002, July). D* Lite. In AAAIIAAI (pp ). Koenig, S., Likhachev, M., & Furcy, D. (2004). Lifelong planning A. Artificial Intelligence, 55(), Stentz, A. (994, May). Optimal and efficient path planning for partially-kno n environments. In Proceedings of the IEEE International Conference on Robotics and Automation. Likhachev, M., Ferguson, D. I., Gordon, G. J., Stentz, A., & hrun, S. (200, June). Anytime Dynamic A*: An Anytime, Replanning Algorithm. In ICAPS (pp ). Hofmann, A., Fernandez, E., Helbert, J., Smith, S., & illiams,. (20). Reactive Integrated Motion Planning and Euecution. In Proceedings of the Twenty-Fourth International Joint Conference on Artificial Intelligence. 2

3 Outline Motivation Incremental Search The D* Lite Algorithm D* Lite Example When to Use Incremental Path Planning? Algorithm Extensions and Related Topics Application to Mobile Robotics 3

4 Motivation 4

5 Motivation 5

6 Motivation e obstacle detected Replan 6

7 Motivation e obstacle detected Replan 7

8 Motivation e obstacle detected Replan 8

9 Motivation e obstacle detected Replan 9

10 Motivation 0

11 Motivation

12 Motivation 2

13 Motivation 3

14 Motivation hange detected Replan 4

15 Motivation Environmental hange 5

16 Motivation hange detected Replan 6

17 Motivation hange detected Replan 7

18 Motivation 8

19 Motivation 9

20 Motivation 20

21 Motivation 2

22 Motivation 22

23 Motivation 23

24 Motivation 24

25 Motivation 25

26 Motivation 26

27 Motivation 27

28 Motivation RR Sertac Karaman. All rights reserved. This content is excluded from our Creative Commons license. For more information, see Watch the video on YouTube ( 28

29 Motivation After significant offline computation time... 29

30 Motivation RR * Sertac Karaman. All rights reserved. This content is excluded from our Creative Commons license. For more information, see 30

31 Problems Changing environmental conditions: o Obstacles o Utility or cost Sensor limitations: o Partial observability Computation time: o Complete, optimal replanning is slow o Stay put or move in wrong direction? 3

32 n Reuse data nrom previous search Incremental Search Methods 32

33 Incremental APSP Alg. Mean Std omp. Opt. hekhov (APSP) es es RR Prob. o RR connect Prob. o PRM Prob. Asym. Courtesy of Hofmann, Andreas et al. License: cc by-nc-sa. 33

34 Outline Motivation Incremental Search The D* Lite Algorithm D* Lite Example When to Use Incremental Path Planning? Algorithm Extensions and Related Topics Application to Mobile Robotics 34

35 Incremental Search Pernorm graph search Repeat: o Execute path/plan o Receive graph changes o Update previous search results 35 From Koenig, Likhachev, & Furcy (2004)

36 Revie of Graph Search Problem Input: graph search problem S = <gr, w, h, s start, s goal > directed graph: gr = <V, E> edge eighting function: w: s x s heuristic function: h: s x s goal start verteu: s V start goal verteu: s goal V Output: simple path P = <s, s 2, ", s goal > start s start (s,s ) start (s start,s 2 ) s (s,s goal ) h(s start,s goal ) (s 2,s goal ) s 2 h(s,s goal ) s goal h(s 2,s goal ) 36

37 Revie of Graph Search Problem g: cost-so-far (s start,s ) s h(s,s goal ) o g(s) = g(s predecessor ) + (s predecessor, s) (s,s goal ) h: heuristic value, cost-to-go s start h(s start,s goal ) s goal f(s) = g(s) + h(s) (s start,s 2 ) (s 2,s goal ) h(s 2,s goal ) s 2 37

38 Graph Representations Incremental algorithms generalize to any graph (usually non-negative edges). Grids used for ease of representation. 4-onnected Graph -onnected Graph Admissible Manhattan Distance mau(lu, ly) Heuristic: = lu + ly ( ith unit edge weights) 38

39 Relauation - Dikstras Algorithm

40 Relauation - Dikstras Algorithm

41 Relauation - Dikstras Algorithm

42 Relauation - Dikstras Algorithm

43 Relauation - Dikstras Algorithm

44 Relauation - Dikstras Algorithm

45 Relauation - Dikstras Algorithm

46 Relauation - Dikstras Algorithm

47 Relauation - Dikstras Algorithm

48 Relauation - Dikstras Algorithm

49 Relauation - Dikstras Algorithm

50 Relauation - Dikstras Algorithm

51 Relauation - Dikstras Algorithm

52 Relauation - Dikstras Algorithm

53 Relauation - Dikstras Algorithm

54 Relauation - Dikstras Algorithm

55 Ho to Reuse Previous Search Results Store optimal results: o shortest paths o g-values Find inconsistencies: o local consistency Make consistent: o relaxations (a) (b) (c) Courtesy of Elsevier, Inc., Used with permission. Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy. "Lifelong Planning A*." In Artificial Intelligence, 55 (- 2): From Koenig, Likhachev, & 55 Furcy (2004)

56 Incremental Search Methods - Euamples General o o Incremental APSP Lifelong Planning A* (LPA*) o Dynamics Strictly eakly Superior Function - Fiued Point (DynamicsS SF-FP) o... Mobile Robots o D* o D* Lite emporal Planning o Incremental emporal onsistency (I ) Propositional Satisfiability o Incremental Unit Propagation 56

57 D* Incremental Path Planning Approach Heuristic Search (e.g. A*) Incremental + Search = (e.g. DynamicsS SF-FP) Efficient Incremental Path Planning (e.g. LPA*, D* Lite,...) Optimal & Efficient Fast Replanning Fast, Optimal Replanning 57

58 Initial Search Courtesy of Elsevier, Inc., Used with permission. Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy. "Lifelong Planning A*." In Artificial Intelligence, 55 (- 2): From Koenig, Likhachev, & Furcy (2004)

59 Follo -on Search From Koenig, Likhachev, & Furcy (2004) Courtesy of Elsevier, Inc., Used with permission. Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy. "Lifelong Planning A*." In Artificial Intelligence, 55 (- 2):

60 Outline Motivation Incremental Search The D* Lite Algorithm D* Lite Example When to Use Incremental Path Planning? Algorithm Extensions and Related Topics Application to Mobile Robotics 60

61 A* Reminder A* is best-first search from start to goal sorted by a cost f(s): f(s) = g(s) + h(s,s goal ) f(s) = total node cost g(s) = path cost to reach verteu from s start h(s,s goal ) = heuristic for cost to reach verteu s goal from verteu s 6

62 A*: hoosing et een Ties Introduce a ne notation: f(s) = ( f (s), f 2 (s) ) f(s) = ( g(s) + h(s,s goal ), g(s) ) 3 h = 2 h = s f(s ) = s start s goal f(s 2 ) = 2 s 2 h = 3 2 h = 62

63 A*: hoosing et een Ties Introduce a ne notation: f(s) = ( f (s), f 2 (s) ) f(s) = ( g(s) + h(s,s goal ), g(s) ) s start 3 h = 2 h = s f(s ) = (, 3 ) s goal f(s 2 ) = 2 s 2 h = 3 2 h = 63

64 A*: hoosing et een Ties Introduce a ne notation: f(s) = ( f (s), f 2 (s) ) f(s) = ( g(s) + h(s,s goal ), g(s) ) s start 3 h = 2 h = s f(s ) = (, 3 ) s goal f(s 2 ) = (, 2 ) 2 s 2 h = 3 2 h = 64

65 A*: hoosing et een Ties Introduce a ne notation: f(s) = ( f (s), f 2 (s) ) f(s) = ( g(s) + h(s,s goal ), g(s) ) 3 h = 2 h = s f(s ) = (, 3 ) s start s goal f(s 2 ) = (, 2 ) 2 s 2 h = 3 2 h = 65

66 Successors and Predecessors Successors of node s: Every node that can be reached from s, Succ(s) Predecessors of verteu s: Every node from which s can be reached, Pred(s) 66

67 Successors and Predecessors Successors of node s: Every node that can be reached from s, Succ(s) Predecessors of verteu s: Every node from which s can be reached, Pred(s) 67

68 Successors and Predecessors Successors of node s: Every node that can be reached from s, Succ(s) Predecessors of verteu s: Every node from which s can be reached, Pred(s) 68

69 Successors and Predecessors Successors of node s: Every node that can be reached from s, Succ(s) Predecessors of verteu s: Every node from which s can be reached, Pred(s) 69

70 hat is D* Lite Efficient repeated best-first search through a graph with changing edge weights as the graph is traversed. s start s goal s goal s start an be vie ed as replanning through relauation of path costs. 70

71 Reformulation: Minimize Recomputation When the start changes, the path cost from start to s is not preserved. ase : ase 2: s start 2 2 s s goal 2 s start 2 s s goal (s) = = 4 g g (s) t g 2 (s) g 2 (s) = 2 7

72 Reformulation: Minimize Recomputation Reformulate search from goal to start. Path cost from goal to s is preserved. ase : ase 2: s start 2 s goal s s s start s goal (s) = g g (s) = g 2 (s) g 2 (s) = 72

73 Overall D* Lite:. Initialize all nodes as uneupanded. 2. est-first search until s is consistent with neighbors and eupanded. start 3. Move to neut best verteu. 4. If any edge costs change: a. rack ho heuristics have changed. b. Update source nodes of changed edges.. Repeat from 2. 73

74 Overall D* Lite:. Initialize all nodes as uneupanded. 2. est-first search until s start is consistent with neighbors and eupanded. start 3. Move to neut best verteu. 4. If any edge costs change: a. rack ho heuristics have changed. Most computation b. Update source nodes of changed edges.. Repeat from 2. 74

75 Overall D* Lite:. Initialize all nodes as uneupanded. 2. est-first search until s start is consistent with neighbors and eupanded. 3. Move to neut best verteu. 4. If any edge costs change: a. rack ho heuristics have changed. b. Update source nodes of changed edges.. Repeat from 2. Incremental component 75

76 Overall D* Lite:. Initialize all nodes as uneupanded. 2. est-first search until s start is consistent with neighbors and eupanded. 3. Move to neut best verteu. 4. If any edge costs change: a. rack ho heuristics have changed. b. Update source nodes of changed edges.. Repeat from 2. 76

77 Eutracting a Path Given Path ost Move from s to the successor which gives s the lo est path cost. start start s argmin (c(s,s) + g(s)) start s E Succ(s start ) start 2 g = g = 7 6 g = 4 77

78 Overall D* Lite:. Initialize all nodes as uneupanded. 2. est-first search until s start is consistent with neighbors and eupanded. start 3. Move to neut best verteu. 4. If any edge costs change: a. rack ho heuristics have changed. b. Update source nodes of changed edges.. Repeat from 2. 78

79 Handling Weight hanges Locally Self-consistent graph: g(s) = min s E Succ(s) (g(s) + c(s,s)) May no longer be true When edge Weights change 2 g = 2 g = g = 7 6 g = 4 g = 7 g = 4 79

80 Handling Weight hanges Locally hanges propagate to predecessors. For efficient search, update lo est cost nodes first. - Use a priority queue like A*. Update nodes until the goal is first eupanded. 2 g = g = 7 g = 4 80

81 Handling Weight hanges Locally Store an additional value: Local inconsistency: rhs(s) t g(s) rhs(s) = min (g(s) + c(s,s)) s E Succ(s) 2 g = g = 7 rhs = g = 4 8

82 Handling Weight hanges Locally Store an additional value: Local inconsistency rhs(s) t g(s) rhs(s) = min (g(s) + c(s,s)) s E Succ(s) 2 g = g = 7 rhs = g = 4 82

83 Local Inconsistencies Signal recomputation is necessary for node and predecessors. Locally overconsistent: g(s) > rhs(s) 2. Locally underconsistent: g(s) < rhs(s) 83

84 Updating and Eupanding odes Update by recomputing rhs and placing the node on the priority queue if locally inconsistent. s g = 7 rhs = g = g = 4 84

85 Updating and Eupanding odes Update by recomputing rhs and placing the node on the priority queue if locally inconsistent. s g = 7 rhs = 2 g = g = 4 85

86 Updating and Eupanding odes Update by recomputing rhs and placing the node on the priority queue if locally inconsistent. 2 g = =..., s,... s g = 7 rhs = g = 4 86

87 Updating and Eupanding odes Eupand by taking it off the priority queue and changing g. Eupand in order of total cost: ( min g(s), rhs(s) + h(s,s ), min g(s), rhs(s) ) start 2 g = s g = 7 rhs = g = 4 h = 4 87

88 Updating and Eupanding odes Eupand by taking it off the priority queue and changing g. Eupand in order of total cost: ( min g(s), rhs(s) + h(s,s ), min g(s), rhs(s) ) start 2 g = s g = rhs = g = 4 h = 4 88

89 89

90 g(s) > rhs(s) (Overconsistent): e path cost rhs(s) is better than the old path cost g(s). Immediately update g(s) to rhs(s) and propagate to all predeessors - Set g(s) = rhs(s) - Update all predecessors of s Verteu is no locally consistent and will remain that way. 90

91 g(s) < rhs(s) (Underconsistent): Old path cost g(s) is better than ne path cost rhs(s). Delay vertex expansion and propagate to all predeessors - Set g(s) = - Update all predecessors of s and s itself Verteu is no locally consistent or locally overconsistent. It Will remain on the queue until rhs(s) is the neut best cost. 9

92 g(s) < rhs(s) (Underconsistent): g = 9 s rhs = 9 2 h = 4 2 g = s 6 g = 7 rhs = 7 g = 4 h = 4

93 g(s) < rhs(s) (Underconsistent): Assume after update: s 2 g = 9 rhs = h = 4 g = s 6 g = 7 rhs = 0 h = 4 g = 4 93

94 g(s) < rhs(s) (Underconsistent): Assume after update: s 2 g = 9 rhs = h = 4 g = s 6 g = 7 rhs = 0 h = 4 g = 4 94

95 g(s) < rhs(s) (Underconsistent): s 2 g = 9 rhs = h = 4 g = s 6 g = 7 rhs = 0 h = 4 g = 4 95

96 g(s) < rhs(s) (Underconsistent): s 2 g = 9 rhs = h = 4 g = s g = rhs = 0 h = 4 6 g = 4 96

97 g(s) < rhs(s) (Underconsistent): s 2 g = 9 rhs = h = 4 g = s 6 g = 4 97

98 g(s) < rhs(s) (Underconsistent): s 2 g = rhs = h = 4 g = s g = rhs = 0 h = 4 6 g = 4 98

99 g(s) < rhs(s) (Underconsistent): s 2 g = rhs = h = 4 g = s g = rhs = 9 h = 4 6 g = 4 99

100 g(s) < rhs(s) (Underconsistent): s 2 g = rhs = h = 4 s g = g = rhs = 9 h = 4 6 g = 4 00

101 g(s) < rhs(s) (Underconsistent): s 2 g = rhs = h = 4 g = s g = 9 rhs = 9 h = 4 6 g = 4 0

102 g(s) < rhs(s) (Underconsistent): s 2 g = rhs = h = 4 g = s g = 9 rhs = 9 h = 4 6 g = 4 02

103 Overall D* Lite:. Initialize all nodes as uneupanded. 2. est-first search until s start is consistent With neighbors and eupanded. 3. Move to neut best verteu. 4. If any edge costs change: a. rack ho heuristics have changed. b. Update source nodes of changed edges.. Repeat from 2. 03

104 arrying Over the Priority Queue After a path is found, the priority queue is not empty. he value on the priority queue is: ( min g(s), rhs(s) + h(s,s ), min g(s), rhs(s) ) start hen s is different, the heuristics are different start 04

105 arrying Over the Priority Queue s s : All heuristics lo ered by at most h(s,s ). last start last start hen adding ne nodes to the queue, increase total cost by h(s last,s ). start Increase k = k + h(s last,s ): m m start ( min g(s), rhs(s) + h(s,s ) + k, min g(s), rhs(s) ) start m 05

106 Overall D* Lite:. Initialize all g(s) =, rhs(s t s goal ) =, rhs(s goal ) = 0, k = 0, s last = s. 2. est-first search until s is locally consistent and eupanded. start 3. Move so s = argmin (c(s,s) + g(s)). start s E Succ(s start ) start 4. If any edge costs change: a. k = k + h(s last,s ). m m start b. Update rhs and queue position for source nodes of changed edges.. Repeat from 2. m start Al ays sort by ( min g(s), rhs(s) + h(s,s ) + k, min g(s), rhs(s) ) start m 06

107 Outline Motivation Incremental Search The D* Lite Algorithm D* Lite Example When to Use Incremental Path Planning? Algorithm Extensions and Related Topics Application to Mobile Robotics 07

108 Overall D* Lite:. Initialize all g(s) =, rhs(s t s goal ) =, rhs(s goal ) = 0, k = 0, s last = s. 2. est-first search until s is locally consistent and eupanded. start 3. Move so s = argmin (c(s,s) + g(s)). start s E Succ(s start ) start 4. If any edge costs change: a. k = k + h(s last,s ). m m start b. Update rhs and queue position for source nodes of changed edges.. Repeat from 2. m start Al ays sort by ( min g(s), rhs(s) + h(s,s ) + k, min g(s), rhs(s) ) start m 08

109 D* Lite Euample Heuristic: h(a, ) = minimum number of nodes to get from A to o h(node, start) ritten in each node idirectional graph Edge eights labeled 0 3 Robot starts here Goal node 09

110 k = 0 m 0

111 D* Lite Euample 2. Plan initial path (A*) dequeue node update g(node) 2 k = 0 m g rhs = 0 (deueued)

112 D* Lite Euample m k = 0 2. Plan initial path (A*) dequeue node In this euample, the presence of a key <u,y> indicates that the update g(node) node is in the min priority queue. update rhs(neighbor) 2 queue inconsistent neighbors g rhs = 0 (deueued)

113 D* Lite Euample 2. Plan initial path (A*) dequeue node update g(node) update rhs(neighbor) queue inconsistent neighbors g rhs = (deueued) k = 0 m g = 0 rhs = 0 3 rhs = 0 <2,0>

114 D* Lite Euample 2. Plan initial path (A*) dequeue node update g(node) update rhs(neighbor) queue inconsistent neighbors g rhs = (deueued) k = 0 m g = 0 rhs = 0 ote that this rhs decreased, and the nodes key changed accordingly.

115 D* Lite Euample 2. Plan initial path (A*) dequeue node update g(node) update rhs(neighbor) queue inconsistent neighbors g = rhs = g 2 rhs = 2 (deueued) 2 0 k = 0 m g = 0 rhs = 0 5

116 D* Lite Euample 2. Plan initial path (A*) dequeue node update g(node) update rhs(neighbor) queue inconsistent neighbors g = rhs = his nodes rhs as updated, but the value stayed the same. 0 3 g 2 rhs = 2 (deueued) his nodes rhs as updated, but the value stayed the same. k m = 0 g = 0 rhs = 0

117 D* Lite Euample 2. Plan initial path (A*) dequeue node update g(node) update rhs(neighbor) queue inconsistent neighbors g = rhs = 0 3 g 3 rhs = 3 (deueued) g = 2 rhs = k = 0 m g = 0 rhs = 0 7

118 D* Lite Euample Found shortest path to goal (edges in blue) g = rhs = 2 k = 0 m 0 3 g = 3 rhs = 3 g = 2 rhs = g = 0 rhs = 0 8

119 D* Lite Euample 3. Robot moves (green) update heuristics for ne start node maintain queue from before g = rhs = k = 0 m g = 0 rhs = 0 g = 3 rhs = 3 g = 2 rhs = 2 9

120 g = rhs = m g = 0 rhs = 0 g = 3 rhs = 3 g = 2 rhs = 2 20

121 m g = 3 rhs = g = 2 rhs 4 <3,2> 2 0 g = 0 rhs = 0 rhs(goal) = 0, al ays

122 D* Lite Euample. Repeat from 2 2. Replan path Underconsistent k m = g = 3 g = 2 rhs = 3 rhs = 4 Update rhs of g = 0 rhs = 0 <3,2> neighbors, but no values change. 22

123 D* Lite Euample 2. Replan path k = m g = 3 rhs = 3 Underconsistent g = 0 rhs = 0 23

124 D* Lite Euample 2. Replan path k = m g = 0 rhs = 0 24

125 D* Lite Euample 2. Replan path k = m g = 0 rhs = 0 25

126 D* Lite Euample 2. Replan path k = m g = 0 rhs = 0 26

127 D* Lite Euample 2. Replan path k = m g = 0 rhs = 0 27 g rhs = 0 (deueued)

128 D* Lite Euample 2. Replan path k = m g rhs = 0 (deueued) g = 0 rhs = 0

129 D* Lite Euample 2. Replan path k = m 0 2 g rhs = (deueued) 0 g = 0 rhs = 0 g = 0 rhs = 0 29

130 D* Lite Euample 2. Replan path k = m g = 0 rhs = 0 g rhs = (deueued) g = 0 rhs = 0 30

131 D* Lite Euample Found ne shortest path (blue) from current start (green) to goal (red) k = m 0 2 g = rhs = 0 g = 0 rhs = 0 g = 0 rhs = 0 3

132 D* Lite Euample 3. Robot moves (green) update heuristics for ne start node maintain queue from before k = m 2 g = rhs = 0 0 g = 0 rhs = 0 g = 0 rhs = 0 32

133 2 m 2 g = rhs = 0 0 g = 0 rhs = 0 g = 0 rhs = 0 33

134 2 m g = 0 rhs = 0 34 g = 0 rhs = 0

135 D* Lite Euample. Repeat from 2 2. Replan path g rhs = (deueued) k = 2 m g = 0 rhs = 0 g = 0 rhs = 0 35

136 D* Lite Euample 2. Replan path g = 0 rhs = 0 36 g = 0 rhs 2 <4,2>

137 D* Lite Euample 2. Replan path g = rhs = k = 2 m 2 0 g = 0 rhs = 0 g = rhs = g = rhs = <4,> 37 0 g 2 rhs = 2 (deueued)

138 D* Lite Euample Found ne shortest path g = rhs = k = 2 m 2 g = rhs = g = rhs = <4,> 38 0 g = 2 rhs = 2 0 g = 0 rhs = 0

139 D* Lite Euample 3. Robot moves update heuristics for ne start node maintain queue from before g = rhs = 0 k = 2 m 2 0 g = 0 rhs = 0 g = rhs = g = rhs = <4,> g = 2 rhs = 2 39

140 g = rhs = 0 3 m 2 g = 0 rhs = 0 g = rhs = g = rhs = <4,> g = 2 rhs = 2 40

141 g = rhs = 0 3 m 2 g = 0 rhs = 0 g = rhs 2 <7,2> g = rhs 2 <6,> 4 g = 2 rhs <6,2>

142 D* Lite Euample. Repeat from 2 2. Replan path o need to dequeue any nodes because the smallest key (<6,2>) is greater than key(start) (<4,>). 2 g = rhs = 2 <7,2> g = rhs = 2 <6,> 42 g = rhs = 0 g = 2 rhs = <6,2> k = 3 m g = 0 rhs = 0

143 D* Lite Euample 3. Robot moves Robot reaches goal: Done g = rhs = k = 3 m g = 0 rhs = 0 g = rhs = 2 <7,2> g = rhs = 2 <6,> 43 g = 2 rhs = <6,2>

144 Outline Motivation Incremental Search The D* Lite Algorithm D* Lite Example When to Use Incremental Path Planning? Algorithm Extensions and Related Topics Application to Mobile Robotics 44

145 Efficiency A* eupands each node at most once D* Lite (and LPA*) eupands each node at most t ice o ut ill in most cases eupand fe er nodes than A* A* might performs better if: o o hanges are close to start node Large changes to the graph 45

146 hen to use incremental path planning Goal 46

147 hen to use incremental path planning Goal 47

148 hen to use incremental path planning Goal 48

149 hen to use incremental path planning Goal 49

150 hen to use incremental path planning 50

151 hen to use incremental path planning Goal 5

152 hen to use incremental path planning Goal 52

153 hen to use incremental path planning Goal 53

154 hen to use incremental path planning Goal 54

155 hen to use incremental path planning 55

156 hen to use incremental path planning 56

157 hen to use incremental path planning 57

158 hen to use incremental path planning Goal 58

159 hen to use incremental path planning Goal 59

160 hen to use incremental path planning Goal 60

161 hen to use incremental path planning Goal 6

162 hen to use incremental path planning Goal 62

163 hen to use incremental path planning Goal 63

164 hen to use incremental path planning Goal 64

165 hen to use incremental path planning Goal 65

166 hen to use incremental path planning Goal 66

167 hen to use incremental path planning Goal 67

168 hen to use incremental path planning 68

169 hen to use incremental path planning 69

170 hen to use incremental path planning Goal 70

171 hen to use incremental path planning Goal 7

172 hen to use incremental path planning Goal 72

173 hen to use incremental path planning Goal 73

174 hen to use incremental path planning Goal 74

175 hen to use incremental path planning Goal 75

176 hen to use incremental path planning Goal 76

177 Outline Motivation Incremental Search The D* Lite Algorithm D* Lite Example When to Use Incremental Path Planning? Algorithm Extensions and Related Topics Application to Mobile Robotics 77

178 Greedy mapping 78

179 Greedy mapping Goal 79

180 Greedy mapping Goal 80

181 Greedy mapping Goal 8

182 Greedy mapping Goal 82

183 Greedy mapping Goal 83

184 Greedy mapping Goal 84

185 Greedy mapping Goal 85

186 Greedy mapping Goal 86

187 Greedy mapping Goal 87

188 Greedy mapping Goal 88

189 Greedy mapping Goal 89

190 Greedy mapping Goal 90

191 Greedy mapping Goal 9

192 Greedy mapping Goal 92

193 Greedy mapping Goal 93

194 Greedy mapping Goal 94

195 Greedy mapping Goal 95

196 Greedy mapping Goal 96

197 Anytime Dynamic A* (AD*) ombines the benefits of anytime and incremental planners In the real orld: o hanging environment (incremental planners) o Agents need to act upon decisions quickly (anytime planners) Ref: Likhachev, M., Ferguson, D. I., Gordon, G. J., Stentz, A., & hrun, S. (200, June). Anytime Dynamic A*: An Anytime, Replanning Algorithm. In ICAPS(pp ). 97

198 Anytime Planners Usually start off by computing a highly suboptimal solution o hen improves the solution over time A* ith inconsistent heuristics for euample o Inflated heuristic values give substantial speed-up o Inflated heuristics make the algorithm prefer to keep eupanding paths o Use incrementally less inflated heuristics to approach optimality 98

199 Anytime Dynamic A* (AD*) Starts off by setting a sufficiently high inflation factor o Generates suboptimal plan quickly Decreases inflation factor to approach optimality hen changes to edge costs are detected the current solution is repaired o If the changes are substantial the inflation factor is increased to generate a ne plan quickly 99

200 Outline Motivation Incremental Search The D* Lite Algorithm D* Lite Example When to Use Incremental Path Planning? Algorithm Extensions and Related Topics Application to Mobile Robotics 200

201 Application to mobile robotics This image in the public domain. 20 Reference: Lecture given by Michal p in 2.66 and paper currently in revie

202 Application to mobile robotics This image in the public domain. 202 Reference: Lecture given by Michal p in 2.66 and paper currently in revie

203 Holonomic System onholonomic System o differential constraints Differential constraints 203

204 Methods ell decomposition Visibility graph Sampling-based roadmap construction 204

205 ell Decomposition 205

206 ell Decomposition 206

207 ell Decomposition 207

208 ell Decomposition 208

209 ell Decomposition 209

210 ell Decomposition 20

211 ell Decomposition 2

212 ell Decomposition 22

213 ell Decomposition 23

214 ell Decomposition 24

215 ell Decomposition 25

216 ell Decomposition 26

217 ell Decomposition 27

218 ell Decomposition 28

219 ell Decomposition 29

220 Visibility Graph 220

221 Visibility Graph 22

222 Visibility Graph 222

223 Visibility Graph 223

224 Visibility Graph 224

225 Visibility Graph 225

226 Sampling-based Roadmap onstruction Deterministic sampling Random sampling 226

227 Sampling-based Roadmap onstruction 227

228 onnecting Samples Steering function used o Steer(a, b) gives feasible path bet een samples o onholonomic Dubins path o Shortest path bet een t o points o an be constructed of mauimum curvature and straight line segments RSR, RSL, LSR, LSL, RLR or LRL 228

229 Outline Motivation Incremental Search The D* Lite Algorithm D* Lite Example When to Use Incremental Path Planning? Algorithm Extensions and Related Topics Application to Mobile Robotics 229

230 230

231 ackup 23

232 Incremental Path Planning Dynamic A* (D*) - Stentz, 994 o Initial combination on A* and incremental search nor mobile robot path planning. DynamicsSWSF-FP - Ramalingam and Reps, 99 o Incremental search technique. Linelong Planning A* (LPA*) - Koenig and Likhachev, 200 o Generalizes A* and DynamicsSWSF-FP nor indeninite re-planning on ninite graph. D* Lite - Koenig and Likhachev, 2002 o o Extends LPA* to provide D* nunctionality nor mobile robots. Simpler normulation than D*. More recent extensions to D* and D* Lite, including Anytime D* (AD*), Field D*, etc. Other methods

233 D* Algorithm 233

234 LPA* Algorithm From Koenig, Likhachev, & Furcy (2004) Courtesy of Elsevier, Inc., Used with permission. Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy. "Lifelong Planning A*." In Artificial Intelligence, 55 (- 2):

235 Revie of A* Use admissible heuristic, h(s) < h*(s), to guide search. Keep track of total cost distance from start, g(s). Order node eupansions using priority ueue, with priorities (s) g(s) h(s). Avoid re-eupanding nodes by using eupanded list. etter heuristics (ho closely h(s) approuimates of h*(s)) improve search speed. Guaranteed to return optimal solution if one euists. 235

236 RHS Values One-step look-ahead on g-values, rhs(s) = 0 if s is beginning node of search, other ise: rhs(s) = min (g(s') + c(s', s)) s' ( pred(s) Potentially better innormed than g-value anter changes to search graph. Note: term comes nrom grammar rules used in DynamicsSWSF-FP algorithm, no other signinicance. 236

237 Local onsistency Tells us which nodes may need g-values updated in order to nind shortest path. Node s is locally consistent inn: g(s) = rhs(s) Node s is locally overconsistent inn: g(s) > rhs(s) Node s is locally underconsistent inn: g(s) < rhs(s) Initially, all nodes are locally consistent with g(s) = rhs(s) =, with exception on start node, rhs(s ) = 0 and g(s ) = start start 237

238 238

239 Anytime Dynamic A* (AD*) American Association for Artificial Intelligence. All rights reserved. This content is excluded from our Creative Commons license. For more information, see Fig: Likhachev, M., Ferguson, D. I., Gordon, G. J., Stentz, A., & hrun, S. (200, June). Anytime Dynamic A*: An Anytime, Replanning Algorithm. In ICAPS(pp ). 239

240 MIT OpenCourseWare J / 6.834J Cognitive Robotics Spring 206 For information about citing these materials or our Terms of Use, visit: