Upload
others
View
3
Download
0
Embed Size (px)
Citation preview
Incremental Path Planning
1
April 4, 2016Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto
1
References • Koenig, S., & Likhachev, M. (2002, July). D* Lite. In AAAI�IAAI (pp. 476-4�3). • Koenig, S., Likhachev, M., & Furcy, D. (2004). Lifelong planning A�. Artificial
Intelligence, 155(1), 93-146. • Stentz, A. (1994, 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. 262-271).
• Hofmann, A., Fernandez, E., Helbert, J., Smith, S., & illiams, �. (201�). Reactive Integrated Motion Planning and Euecution. In Proceedings of the Twenty-Fourth International Joint Conference on Artificial Intelligence.
2
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
Motivation
4
Motivation
5
Motivation
�e obstacle detected�
Replan
6
Motivation
�e obstacle detected�
Replan
7
Motivation
�e obstacle detected�
Replan
8
Motivation
�e obstacle detected�
Replan
9
Motivation
10
Motivation
11
Motivation
12
Motivation
13
Motivation
�hange detected�
Replan
14
Motivation Environmental �hange
15
Motivation
�hange detected�
Replan
16
Motivation
�hange detected�
Replan
17
Motivation
18
Motivation
19
Motivation
20
Motivation
21
Motivation
22
Motivation
23
Motivation
24
Motivation
25
Motivation
26
Motivation
27
Motivation RR
Watch the video on YouTube ( https://www.youtube.com/watch?v=vW74bC-Ygb4)28
© Sertac Karaman. All rights reserved. This content is excluded from our CreativeCommons license. For more information, see https://ocw.mit.edu/help/faq-fair-use/.
Motivation
After significant offline computation time...
29
Motivation RR *
30
© Sertac Karaman. All rights reserved. This content is excluded from our CreativeCommons license. For more information, see https://ocw.mit.edu/help/faq-fair-use/.
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?
31
n�
Reuse data nrom previous search�
Incremental Search Methods
32
Incremental APSP
Alg. Mean Std �omp. Opt.
�hekhov (APSP) 0.042� 0.0213 es
RR 0.1�� 0.12 Prob. �o
RR connect 0.042 0.04 Prob. �o
PRM 0.12 0.0� Prob. Asym.
8����"�*�����8����!�$��"�#�����(��� ��'�?�����/���=1 33
Courtesy of Hofmann, Andreas et al. License: cc by-nc-sa.
es
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
Incremental Search
• Pernorm graph search • Repeat:
o Execute path/plan o Receive graph changes o Update previous search results
From Koenig, Likhachev, & Furcy (2004)
35
Revie of Graph Search Problem
Input: graph search problem S = <gr, w, h, sstart
• directed graph: gr = <V, E> • edge eighting function: w: s x s � � • heuristic function: h: s x sgoal � � • start verteu: s € Vstart• goal verteu: sgoal € V
Output: simple path P = <s , s2, ", sgoal>start
, sgoal> s1
sstart sgoal
s2
(sstart,s2)
(s2,sgoal)
(s ,s1)start
(s1,sgoal)
h(s2,sgoal)
h(s1,sgoal)
h(sstart,sgoal)
36
Revie of Graph Search Problem
• g: cost-so-far
o ) + , s)g(s) = g(spredecessor (spredecessor
• h: heuristic value, cost-to-go
• f(s) = g(s) + h(s)
s1
sstart sgoal
s2
(sstart,s2)
(s2,sgoal)
(sstart,s1)
(s1,sgoal)
h(s2,sgoal)
h(s1,sgoal)
h(sstart,sgoal)
37
Graph Representations • Incremental algorithms generalize to any graph (usually non-negative edges). • Grids used for ease of representation.
�-�onnected Graph4-�onnected Graph
Admissible Manhattan Distance mau(lu, ly) Heuristic: = lu + ly ( ith unit edge eights)
38
w
Relauation - Di�kstra�s Algorithm
0
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
39
Relauation - Di�kstra�s Algorithm
3
0 �
1
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
0
L L
L
LL
40
Relauation - Di�kstra�s Algorithm
3
0 4
1 6
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
1 L
L L
L
41
Relauation - Di�kstra�s Algorithm
3 7
0 4
1 6
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
3
L
L
L
42
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 11
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
4
43
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 11
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
� 44
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 11
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
6
45
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
7 46
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
9
47
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
10
48
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
49
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
50
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
51
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
52
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
53
Relauation - Di�kstra�s Algorithm
3 6 9
0 4 10
1 � 7
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
54
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)
From Koenig, Likhachev, & Furcy (2004) 55
Courtesy of Elsevier, Inc., http://www.sciencedirect.com. Used with permission.
Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy.
"Lifelong Planning A*." In Artificial Intelligence, 155 (1-2): 93-146.
Incremental Search Methods - Euamples • General
o Incremental APSP o 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
D* Incremental Path Planning Approach
Heuristic Search (e.g. A*)
Optimal & Efficient
Incremental + Search =
(e.g. DynamicsS SF-FP)
Fast Replanning
Efficient Incremental Path Planning
(e.g. LPA*, D* Lite, ...)
Fast, Optimal
Replanning
57
Initial Search
From Koenig, Likhachev, & Furcy (2004)
58
Courtesy of Elsevier, Inc., http://www.sciencedirect.com. Used with permission.
Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy.
"Lifelong Planning A*." In Artificial Intelligence, 155 (1-2): 93-146.
Follo -on Search
From Koenig, Likhachev, & Furcy (2004)
59
Courtesy of Elsevier, Inc., http://www.sciencedirect.com. Used with permission.
Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy.
"Lifelong Planning A*." In Artificial Intelligence, 155 (1-2): 93-146.
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
A* Reminder A* is best-first search from start to goal sorted by a cost f(s):
f(s) = g(s) + h(s,sgoal)
f(s) = total node cost
g(s) = path cost to reach verteu from sstart
h(s,sgoal) = heuristic for cost to reach verteu sgoal from verteu s
61
A*: �hoosing �et een ies Introduce a ne notation: f(s) = ( f1(s), f2(s) )
f(s) = ( g(s) + h(s,sgoal), g(s) )
sstart
2 2
3 1
1
1s2
�
h = 2 h = 1
s1 f(s1) = �
f(s2) = �sgoal
h = 1h = 3 62
T
A*: �hoosing �et een ies Introduce a ne notation: f(s) = ( f1(s), f2(s) )
f(s) = ( g(s) + h(s,sgoal), g(s) )
sstart
2 2
3 1
1
1s2
�
h = 2 h = 1
s1 f(s1) = ( �, 3 )
f(s2) = �sgoal
h = 1h = 3 63
T
A*: �hoosing �et een ies Introduce a ne notation: f(s) = ( f1(s), f2(s) )
f(s) = ( g(s) + h(s,sgoal), g(s) )
sstart
2 2
3 1
1
1s2
�
h = 2 h = 1
s1 f(s1) = ( �, 3 )
f(s2) = ( �, 2 )sgoal
h = 1h = 3 64
T
A*: �hoosing �et een ies Introduce a ne notation: f(s) = ( f1(s), f2(s) )
f(s) = ( g(s) + h(s,sgoal), g(s) )
f(s1) = ( �, 3 )
f(s2) = ( �, 2 )sstart sgoal
2 2
3 1
1
1s2
�
h = 2 h = 1
s1
h = 1h = 3 65
T
Successors and Predecessors Successors of node s: Every node that can be reached from s, Succ(s)
Predecessors of verteu s: Every node from hich s can be reached, Pred(s)
66
w
Successors and Predecessors Successors of node s: Every node that can be reached from s, Succ(s)
Predecessors of verteu s: Every node from hich s can be reached, Pred(s)
67
w
Successors and Predecessors Successors of node s: Every node that can be reached from s, Succ(s)
Predecessors of verteu s: Every node from hich s can be reached, Pred(s)
68
w
Successors and Predecessors Successors of node s: Every node that can be reached from s, Succ(s)
Predecessors of verteu s: Every node from hich s can be reached, Pred(s)
69
w
hat is D* Lite�
Efficient repeated best-first search through a graph ith changing edge eights as the graph is traversed.
sstart sgoal sgoal
sstart
�an be vie ed as replanning through relauation of path costs.
70
w w
Reformulation: Minimize Recomputation
hen the start changes, the path cost from start to s is not preserved. W
�ase 1: �ase 2:
s 2
s 2 1
s sstart
2 2
1 s sgoalsgoalstart
g2(s) = 2(s) = 2 + 2 = 4g1 g1(s) t g2(s)
71
Reformulation: Minimize Recomputation
Reformulate search from goal to start. Path cost from goal to s is preserved.
�ase 1: �ase 2:
s 2
s 2 s 2 1
s sgoalsgoal1
start
sstart
2
g2(s) = 1(s) = 1g1 g1(s) = g2(s)
72
Overall D* Lite: 1. Initialize all nodes as uneupanded.
2. �est-first search until s is consistent ith 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
w
start is consistent
Overall D* Lite: 1. Initialize all nodes as uneupanded.
2. �est-first search until s ith 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.
2. �est-first search until s start is consistent ith neighbors and eupanded.
Most computation
74
w
rack ho
Overall D* Lite: 1. Initialize all nodes as uneupanded.
2. �est-first search until sstart is consistent ith neighbors and eupanded.
3. Move to neut best verteu.
4. If any edge costs change:
a. heuristics have changed.
b. Update source nodes of changed edges.
�. Repeat from 2.
4. If any edge costs change:
a. rack ho heuristics have changed.
b. Update source nodes of changed edges.
Incremental component
75
w
Overall D* Lite: 1. Initialize all nodes as uneupanded.
2. �est-first search until sstart is consistent ith 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.
3. Move to neut best verteu.
76
w
Eutracting a Path Given Path �ost Move from s to the successor hich gives s the lo est path cost.start start
s � argmin (c(s ,s�) + g(s�))start s� E Succ(sstart) start
g = 7
g = �
g = 4
2
6
77
w
start is consistent
Overall D* Lite: 1. Initialize all nodes as uneupanded.
2. �est-first search until s ith 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.
b. Update source nodes of changed edges.
2. �est-first search until s start is consistent ith neighbors and eupanded.
78
w
Handling eight �hanges Locally Self-consistent graph:
g(s) = mins� E Succ(s) (g(s�) + c(s,s�))
May no longer be true hen edge eights change�
g = 7
g = �
g = 4
2
6
g = �
g = 4
2
1
g = 7
79
W
W W
Handling eight �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.
g = �
g = 4
1
2
g = 7
80
W
Handling eight �hanges Locally Store an additional value:
rhs(s) = min (g(s�) + c(s,s�))s� E Succ(s)
Local inconsistency: rhs(s) t g(s)
g = 7
rhs = �
g = �
g = 4
2
1
81
W
Handling eight �hanges Locally Store an additional value:
rhs(s) = min (g(s�) + c(s,s�))s� E Succ(s)
Local inconsistency rhs(s) t g(s)
g = 7
rhs = �
g = �
g = 4
2
1
82
W
Local Inconsistencies Signal recomputation is necessary for node and predecessors
1. Locally overconsistent:
g(s) > rhs(s)
2. Locally underconsistent:
g(s) < rhs(s)
83
Updating and Eupanding �odes Update by recomputing rhs and placing the node on the priority queue if locally inconsistent.
g = 7 rhs = 7
g = �
g = 4
2
6 s
84
Updating and Eupanding �odes Update by recomputing rhs and placing the node on the priority queue if locally inconsistent.
g = 7 rhs = �
g = �
g = 4
2
1s
85
Updating and Eupanding �odes Update by recomputing rhs and placing the node on the priority queue if locally inconsistent.
g = 7 rhs = �
g = �
g = 4
2
1s = �..., s, ...�
86
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
g = 7 rhs = �
h = 4
g = �
g = 4
2
1s
S�F�T/���9�=�1��---U
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
g = � rhs = �
h = 4
2
1s
g = �
g = 4 S�F�T---U
88
M�!������!� @��!������!��? ������������0����/�1���� �������������� �/�1E
� .%���0���� ��&�� ���0�!��!���0���������� ����0!��&���� �����������*/�1�*����-�
� �����#��0�!��!��0�����������#�*�����@������������ �/�1����� �������� ��P0�0�-
89
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 prede�essors�
- Set g(s) = rhs(s) - Update all predecessors of s
Verteu is no locally consistent and will remain that way.
90
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 prede�essors�
- Set g(s) = - Update all predecessors of s and s itself
Verteu is no locally consistent or locally overconsistent. It ill remain on the queue until rhs(s) is the neut best cost.
91
W
L
g
s rh2
h
1
2
s1 6
g = 7 rhs = 7
h = 4
= 9 s = 9
g = �
g = 4
g(s) < rhs(s) (Underconsistent):
92
s2
h = 4
g(s) < rhs(s) (Underconsistent): Assume after update:
g = 4
g = ��
6
1
s1
s2
g = 9 rhs = �
h = 4
6 g = 7 rhs = 10
h = 4
93
g(s) < rhs(s) (Underconsistent): Assume after update:
g = 4
g = ��
6
1
s1
s2
g = 9 rhs = �
h = 4
6 g = 7 rhs = 10
h = 4
S�F�T/�������4�1��/�������6�1U
94
g(s) < rhs(s) (Underconsistent):
g = 4
g = ��
6
1
s1
s2
g = 9 rhs = �
h = 4
6 g = 7 rhs = 10
h = 4
S�F�T/�������4�1��/�������6�1U
95
g(s) < rhs(s) (Underconsistent):
g = 4
g = ��
6
1
s1
s2
g = 9 rhs = �
h = 4
6 g = rhs = 10
h = 4
S�F�T/�������4�1��/�������6�1U
96
L
g(s) < rhs(s) (Underconsistent):
g = 4
g = ��
6
1
s1
s2
g = 9 rhs = �
h = 4
6
S�F�T/�������6�1��/����������1U
��F�L� ��F���
�F��
97
g(s) < rhs(s) (Underconsistent):
g = 4
g = ��
6
1
s1
s2
g = � rhs = �
h = 4
6 g = rhs = 10
h = 4
S�F�T/�������6�1��/����������1U
98
L
g(s) < rhs(s) (Underconsistent):
g = 4
g = ��
6
1
s1
s2
g = � rhs = �
h = 4
6 g = rhs = 9
h = 4
S�F�T/����������1U
99
L
g(s) < rhs(s) (Underconsistent):
g = 4
g = ��
6
1
s1
s2
g = � rhs = �
h = 4
6 g = rhs = 9
h = 4
1 S�F�T/� ���7�9�1U
100
L
g(s) < rhs(s) (Underconsistent):
g = 4
g = ��
6
1
s1
s2
g = � rhs = �
h = 4
6 g = 9 rhs = 9
h = 4
S�F�T/�����7�9�1U
101
g(s) < rhs(s) (Underconsistent):
g = �
�
6
1
s1
s2 rhs = �
h = 4
6 g = 9 rhs = 9
h = 4
g = �
g = 4
S�F�TU
102
rack ho
Overall D* Lite: 1. Initialize all nodes as uneupanded.
2. �est-first search until sstart is consistent ith neighbors and eupanded.
3. Move to neut best verteu.
4. If any edge costs change:
a. heuristics have changed.
b. Update source nodes of changed edges.
�. Repeat from 2.
a. rack ho heuristics have changed.
103
W
�arrying Over the Priority ueue
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
104
Q
�arrying Over the Priority ueue
� s : All heuristics lo ered by at most h(slast,s ).slast start start
hen adding ne nodes to the queue, increase total cost by h(slast,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
105
Q
Overall D* Lite: 1. Initialize all g(s) = , rhs(s t sgoal) = , rhs(sgoal) = 0, k = 0, slast = s . m start
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(sstart) 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.
Al ays sort by ( min� g(s), rhs(s) � + h(s,s ) + k , min� g(s), rhs(s) � )start m
106
LL
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
107
Overall D* Lite: 1. Initialize all g(s) = , rhs(s t sgoal) = , rhs(sgoal) = 0, k = 0, slast = s . m start
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(sstart) 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.
Al ays sort by ( min� g(s), rhs(s) � + h(s,s ) + k , min� g(s), rhs(s) � )start m
108
LL
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 1
1
1
1
10
1
Robot starts here
Goal node
2
2
1
109
k = 0 m 23������ @����-�������$�+ ���F�� ��F�L���@�������+ !!�������P0�0��&�� �%���R7��+ %�����!�*����F��
0 1 1 3
1
1
1
10
1
��F�L� ��F�L
��F�L� ��F�L
��F�L� ��F�L
��F�L� ��F�L
��F�L� ��F��R7��Q
Q 2
2
110
D* Lite Euample 2. Plan initial path (A*) • dequeue node • update g(node)
0 3
1
1
1
1
10
1
k = 0 m
g � � rhs = 0 (de�ueued)
��F�L� ��F�L
��F�L� ��F�L
��F�L� ��F�L
��F�L� ��F�L
2
1
2
111
k = 0D* Lite Euample
m
2. Plan initial path (A*) • dequeue node In this euample, the presence of
• • •
g � � rhs = 0 (de�ueued)
update g(node) update rhs(neighbor) queue inconsistent neighbors
0
3 1
1
1
1
10
1
a key <u,y> indicates that the node is in the min priority queue.
��F�L� ��F�L
��F�L� ��F�L
��F�L���������� !��"
��F�L��������#! "
2
2
1
112
k = 0 mg � 1D* Lite Euample rhs = 12. Plan initial path (A*) (de�ueued)• dequeue node
• • •
update g(node) update rhs(neighbor) queue inconsistent neighbors
0 3 1
1
1
1
10
1
rhs = 10 <12,10>
g = 0 rhs = 0
��F�L� ��F�L
��F�L
��F�L� ��F�L 2
2
1
113
k = 0 mg � 1D* Lite Euample rhs = 12. Plan initial path (A*) (de�ueued)• dequeue node
• • •
update g(node) update rhs(neighbor) queue inconsistent neighbors
0 3 1
1
1
1
10
1
g = 0 rhs = 0
�ote that this rhs decreased, and the node�s key changed accordingly.
��F�L� ��F�L
��F�L������ �#! "
��F�L������ �$! "
1
2
2
114
k = 0 mg = 1D* Lite Euample rhs = 12. Plan initial path (A*)
• dequeue node • • •
update g(node) update rhs(neighbor) queue inconsistent neighbors
2
0 3 1
1
1
1
10
1
g � 2 rhs = 2 (de�ueued)
g = 0 rhs = 0
��F�L� ��F��R���Q
��F�L� ��F�L 2
2
1
115
D* Lite Euample 2. Plan initial path (A*) • dequeue node • • •
g = 1 rhs = 1
k m = 0 his node�s rhs as updated, but the value stayed the same.
update g(node) update rhs(neighbor) queue inconsistent neighbors
0 3 1
1
1
1
10
1
g � 2 rhs = 2 (de�ueued)
g = 0 rhs = 0
his node�s rhs as updated, but the value stayed the same.
��F�L� ��F��R���Q
��F�L������#�#!#"
2
2
1
116
k = 0 mg = 1D* Lite Euample rhs = 12. Plan initial path (A*)
• dequeue node • • •
update g(node) update rhs(neighbor) queue inconsistent neighbors
0 3 1
1
1
1
10
1
g � 3 rhs = 3 (de�ueued)
g = 2 rhs = 2
g = 0 rhs = 0
��F�L� ��F��R���Q
2
2
1
117
k = 0 mg = 1D* Lite Euample rhs = 1Found shortest path to goal�
(edges in blue)
0 3 1
1
1
1
10
1
g = 3 rhs = 3
g = 2 rhs = 2
g = 0 rhs = 0
��F�L� ��F��R���Q
2
2
1
118
k = 0 mg = 1D* Lite Euample rhs = 13. Robot moves (green)
• update heuristics for ne start node • maintain queue from before
0 2
1
1
1
1
10
1
g = 3 rhs = 3
g = 2 rhs = 2
g = 0 rhs = 0
��F�L� ��F��R���Q
119
1
1
1
� � 1 m= 1s = 1
1
0 2 1
1
10
g = 2 rhs = 2
g = 0 rhs = 0
g = 3 rhs = 3
��F�L� ��F��R���Q
1
1
23������ @��� g �-�:#�����!������!�/!�%����1 rh+ !J������!���&��� ���F�L+ 0�!���%�
120
� � 1m
rhs(goal) = 0,
1
0 2
1 10
g = 2 rhs � 4 <3,2>
g = 0 rhs = 0
g = 3 al ays rhs = 3
��F�L������#�'!#"
��F��������%�#!�"
1
1
%%
23������ @����-�:#�����!������!�/!�%����1+ !J������!���&��� ���F�L+ 0�!���%�+ 0�!���� ���*���0������!��
% 1
121
k m = 1Underconsistent�
D* Lite Euample �. Repeat from 2 2. Replan path
g = 0 rhs = 0
g = 3 g = 2 rhs = 3 rhs = 4 Update rhs of
<3,2> neighbors, but no values change.
1
0 2
1
1 10
��F�L� ��F�7R=�7Q
����%� ��F�L����������
1
1
%
%
%
122
k = 1 m
g = 0 rhs = 0
g = 3 rhs = 3
Underconsistent�
D* Lite Euample 2. Replan path
1
0 2
1
1 10
��F�L� ��F�7R=�7Q
��F�L� ��F�L
����%� ��F������������
1
1
% %
%
123
D* Lite Euample
2. Replan path
k = 1 m
1
0 2
1
1 10
g = 0 rhs = 0
��F�L� ��F�L
��F�L���������� !��"
����%� ��F������������
��F�7������%�'!#"
1
1
%
%
%
124
D* Lite Euample 2. Replan path
k = 1 m
1
0 2 1
1 10
g = 0 rhs = 0
����%� ��F�L����������
��F�L� ��F��
��F�L� ��F���R�����Q
��F�L� ��F�L
1
1 % %
%
125
D* Lite Euample 2. Replan path
k = 1 m
1
0 2 1
1 10
g = 0 rhs = 0
��F�L� ��F�L
��F�L� ��F���R�����Q
��F�L� ��F����� !��"
����%� ��F�L���������� 1
1 %
%
%
126
D* Lite Euample
2. Replan path
k = 1 m
g = 0 rhs = 0
1
0 2 1
1 10
g � 1� rhs = 10 (de�ueued)
��F�L� ��F�L
��F�L� ��F���R�����Q
��F�L� ��F�L
1
1
%
%
%
127
D* Lite Euample
2. Replan path
k = 1m
g = 0 rhs = 0
1
0 2 1
1 10
g � 1� rhs = 10 (de�ueued)
��F�L� ��F���R�����Q
��F�L� ��F�L
��F�L� ��F�L
< �����!�K��� ��&��0�!��!��#0��� ���0������!�� �����-
1
1
%
% %
128
D* Lite Euample
2. Replan path
k = 1 m
1
0 2
1
1 10
g � 11 rhs = 11 (de�ueued)
g = 10 rhs = 10
g = 0 rhs = 0
��F�L� ��F�L��F�L� ��F�L
��F�L� ��F�L
1
1
%
%
%
129
D* Lite Euample
2. Replan path
k = 1 m
1
0 2 1
1 10
g � 11 rhs = 11 (de�ueued)
g = 10 rhs = 10
g = 0 rhs = 0
��F�L������� ��$!� "
��F�L� ��F�L
1
1
%
%
%
130
k = 1 mD* Lite Euample
Found ne shortest path (blue) from current start (green) to goal (red)
g = 10 rhs = 10
1
0 2
1
1 10
g = 11 rhs = 11
g = 0 rhs = 0
��F�L� ��F���R�����Q
��F�L� ��F�L
1
1
%
%
%
131
k = 1 mD* Lite Euample
3. Robot moves (green) • update heuristics for ne start node • maintain queue from before
1
1
0
1
1 10
g = 11 rhs = 11
g = 10 rhs = 10
g = 0 rhs = 0
��F�L� ��F���R�����Q
��F�L� ��F�L
1 2 %
%
%
132
� � 2 m
g = 10 rhs = 10
1 1
0
1
10
1
g = 11 rhs = 11
g = 0 rhs = 0
��F�L� ��F�L
��F�L� ��F���R�����Q
1
2 %
%
%
23������ @����-�:#����������A+ 0�!����!���&��� ��+ 0�!���%�
133
1 1
0
1
10
1
g = 10 rhs = 10
� � 2 m
g = 0 rhs = 0
��F�L������%
��F���������%��$!��"
��F�L��������$!�"
1
2 %
%
������ ��� �����!��&��������!�*����� ��P0�0��#��0�����������&���������������
23������ @����-�:#����������A
+ 0�!����!���&��� �� + 0�!���%�+
0�!���� ���*���0������!��
%
134
k = 2 mg � 1D* Lite Euample rhs = 1�. Repeat from 2 (de�ueued)2. Replan path
g = 0 rhs = 0
1 1
0
1
10
1
g = 10 rhs = 10
��F���� ��F�LR�����Q
��F�L� ��F�L
1
2 %
%
%
135
%��F��D* Lite Euample 2. Replan path
g = 0 rhs = 0
1 1
0
1
10
1
g = 10 rhs � 2 <4,2>
������ ��F������������
��F���� ��F�LR�����Q
��F�L� ��F�L
%%
%
1
2
136
g = 1D* Lite Euample rhs = 12. Replan path
k = 2 m
g = 0 rhs = 0
g = rhs =
1 1
0
1
10
1
g = 11 rhs = <14,11>
g � 2 rhs = 2 (de�ueued)
%% %
%
%
%
1
2
137
D* Lite Euample Found ne shortest path�
1 1
0
1
10
1
g = 11 rhs = <14,11>
g = 2 rhs = 2
g = 1 rhs = 1
k = 2 m
g = 0 rhs = 0
g = rhs =
%%
%
%% %
������ ��&��!�!�K��!�P0�0��� �����!�����0�!��������
1
2
138
k = 2 mg = 1D* Lite Euample rhs = 13. Robot moves
• update heuristics for ne start node • maintain queue from before
0
1 1
1
10
1
g = rhs =
g = 11 rhs = <14,11>
g = 2 rhs = 2
g = 0 rhs = 0
%
%% %
%
%
1
2
139
� � 3 mg = 1rhs = 1
g = 2 rhs = 2
0
1
1
1 1 1
g = 11 rhs = <14,11>
g = 0 rhs = 0
g = rhs =
%%
%
%
%%
1 2
23������ @����-�:#������������� �������#��A+ 0�!����!���&��� ��+ 0�!���%�
140
� � 3 m 1
g = 2
1
1
1
g = 11 rhs � 2 <6,11>
g = 0 rhs = 0
g = rhs � 12 <17,12>
rhs � <6,2>
% %
%%
%
2 1
23������ @��� g = 1rhs =�-�:#������������� �������#��A
+ 0�!����!���&��� ��+ 0�!���%�+ 0�!���� ���*���0������!�� 0
1 1
%
141
k = 3 mg = 1D* Lite Euample rhs = 1�. Repeat from 2
2. Replan path� • �o need to dequeue any nodes
because the smallest key (<6,2>) is greater than key(start) (<4,1>).
0
1
1
1 1 1
g = rhs = 12 <17,12>
g = 11 rhs = 2 <6,11>
g = 2 rhs = <6,2>
g = 0 rhs = 0
%
%
%%
%
1 2
142
k = 3 mg = 1D* Lite Euample rhs = 13. Robot moves
• Robot reaches goal: Done�
g = 0 rhs = 0
g = rhs = 12 <17,12>
1 1 1
g = 11 rhs = 2 <6,11>
g = 2 rhs = <6,2>
%
%
%%
%
143
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
144
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 �hanges are close to start node o Large changes to the graph
145
hen to use incremental path planning�
Goal
146
hen to use incremental path planning�
Goal
;�
147
hen to use incremental path planning�
Goal
148
hen to use incremental path planning�
Goal
149
hen to use incremental path planning�
;�
150
hen to use incremental path planning�
Goal
151
hen to use incremental path planning�
Goal
152
hen to use incremental path planning�
Goal
153
hen to use incremental path planning�
Goal Goal
154
hen to use incremental path planning�
;�
155
hen to use incremental path planning�
;�
156
hen to use incremental path planning�
;�
157
hen to use incremental path planning�
Goal
158
hen to use incremental path planning�
Goal
159
hen to use incremental path planning�
Goal
160
hen to use incremental path planning�
161
Goal
hen to use incremental path planning�
162
Goal
hen to use incremental path planning�
163
Goal
hen to use incremental path planning�
164
Goal
hen to use incremental path planning�
165
Goal
hen to use incremental path planning�
166
Goal
hen to use incremental path planning�
Goal
167
hen to use incremental path planning�
;�
168
hen to use incremental path planning�
;�
169
hen to use incremental path planning�
Goal
170
hen to use incremental path planning�
Goal
171
hen to use incremental path planning�
Goal
172
hen to use incremental path planning�
Goal
173
hen to use incremental path planning�
Goal
174
hen to use incremental path planning�
Goal
175
hen to use incremental path planning�
Goal
176
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
177
Greedy mapping
178
Greedy mapping Goal
179
Greedy mapping
180
Goal
Greedy mapping
181
Goal
Greedy mapping Goal
182
Greedy mapping Goal
183
Greedy mapping Goal
184
Greedy mapping Goal
185
Greedy mapping Goal
186
Greedy mapping Goal
187
Greedy mapping Goal
188
Greedy mapping Goal
189
Greedy mapping Goal
190
Greedy mapping Goal
191
Greedy mapping Goal
192
Greedy mapping Goal
193
Greedy mapping Goal
194
Greedy mapping Goal
195
Greedy mapping Goal
196
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. 262 -271). 197
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
198
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
199
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
Application to mobile robotics
Reference: Lecture given by Michal ��p in 2.166 and paper currently in revie
+ "�&�!��&�����*����������������*��0������������������ E
201
This image in the public domain.
Application to mobile robotics
Reference: Lecture given by Michal ��p in 2.166 and paper currently in revie
+ "�&�!��&�����*����������������*��0������������������ E
202
This image in the public domain.
Holonomic System �onholonomic System
• �o differential constraints • Differential constraints
203
Methods
• �ell decomposition • Visibility graph • Sampling-based roadmap construction
204
�ell Decomposition
205
�ell Decomposition
206
�ell Decomposition
207
�ell Decomposition
208
�ell Decomposition
209
�ell Decomposition
210
�ell Decomposition
211
�ell Decomposition
212
�ell Decomposition
213
�ell Decomposition
214
�ell Decomposition
215
�ell Decomposition
216
�ell Decomposition
217
�ell Decomposition
218
�ell Decomposition
+ ?��%������������������& �����#����������2��������
+ �� �������������+ :��� ���������������
219
Visibility Graph
220
Visibility Graph
221
Visibility Graph
222
Visibility Graph
223
Visibility Graph
224
Visibility Graph
+ ?��%������������������& �����#����������2��������
+ :��� ���������������+ :������� + B���0����#��
225
Sampling-based Roadmap �onstruction
Deterministic sampling Random sampling
226
Sampling-based Roadmap �onstruction
227
�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 lineline
segments • RSR, RSL, LSR, LSL, RLR or LRL
228
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
�ackup
231
Incremental Path PlanningIncremental Path Planning • Dynamic A* (D*) - Stentz, 1994
o Initial combination on A* and incremental search nor mobile robot path planning.
• DynamicsSWSF-FP - Ramalingam and Reps, 199�
o Incremental search technique.
• Linelong Planning A* (LPA*) - Koenig and Likhachev, 2001
o Generalizes A* and DynamicsSWSF-FP nor indeninite re-planning on ninite graph.
• D* Lite - Koenig and Likhachev, 2002
o Extends LPA* to provide D* nunctionality nor mobile robots.
o Simpler normulation than D*.
• More recent extensions to D* and D* Lite, including Anytime D* (AD*), Field D*, etc.
• Other methods... 232
D* Algorithm
233
LPA* Algorithm
From Koenig, Likhachev, & Furcy (2004)
234
Courtesy of Elsevier, Inc., http://www.sciencedirect.com. Used with permission.
Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy.
"Lifelong Planning A*." In Artificial Intelligence, 155 (1-2): 93-146.
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, ith 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
w
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
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
LL
B����������*������������� ������������3(���������>
+ 8��������� ��@��!��������!������������!������3���*��3�#��%����������*�����*�������5�0��-
2�**�������>
+ ���������P0�0����!���!�0�����%����%/�1>G %/�1�F�T%�/�1\�%�/�1U
�G % /�1�F�*/�1�F����/�/�1�� �/�11�H� /�1�G % /�1�F��/�1�F����/�/�1�� �/�11
G ��@������ �����!�������%/�1�R�%K/�1��**>�Z % /�1�R�%�K/�1
Z :)�/%�/�1�F�%�K/�1���2�%�/�1�R�%�K/�11+ ����@��!�!��������!����5�@���������������!�#������������������ ��%�-+ ��!������#���@��!�!��&�����!����!������������ �������*����������& ���
0�!��������������!������& �����������������-238
Anytime Dynamic A* (AD*)
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. 262-271).
239
© American Association for Artificial Intelligence. All rights reserved.
This content is excluded from our Creative Commons license. For
more information, see https://ocw.mit.edu/help/faq-fair-use/.
MIT OpenCourseWarehttps://ocw.mit.edu
16.412J / 6.834J Cognitive RoboticsSpring 2016
For information about citing these materials or our Terms of Use, visit: https://ocw.mit.edu/terms.