Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe...
Transcript of Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe...
![Page 1: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/1.jpg)
Incremental Path Planning
1
April 4, 2016Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto
1
![Page 2: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/2.jpg)
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
![Page 3: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/3.jpg)
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
![Page 4: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/4.jpg)
Motivation
4
![Page 5: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/5.jpg)
Motivation
5
![Page 6: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/6.jpg)
Motivation
�e obstacle detected�
Replan
6
![Page 7: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/7.jpg)
Motivation
�e obstacle detected�
Replan
7
![Page 8: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/8.jpg)
Motivation
�e obstacle detected�
Replan
8
![Page 9: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/9.jpg)
Motivation
�e obstacle detected�
Replan
9
![Page 10: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/10.jpg)
Motivation
10
![Page 11: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/11.jpg)
Motivation
11
![Page 12: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/12.jpg)
Motivation
12
![Page 13: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/13.jpg)
Motivation
13
![Page 14: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/14.jpg)
Motivation
�hange detected�
Replan
14
![Page 15: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/15.jpg)
Motivation Environmental �hange
15
![Page 16: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/16.jpg)
Motivation
�hange detected�
Replan
16
![Page 17: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/17.jpg)
Motivation
�hange detected�
Replan
17
![Page 18: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/18.jpg)
Motivation
18
![Page 19: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/19.jpg)
Motivation
19
![Page 20: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/20.jpg)
Motivation
20
![Page 21: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/21.jpg)
Motivation
21
![Page 22: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/22.jpg)
Motivation
22
![Page 23: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/23.jpg)
Motivation
23
![Page 24: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/24.jpg)
Motivation
24
![Page 25: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/25.jpg)
Motivation
25
![Page 26: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/26.jpg)
Motivation
26
![Page 27: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/27.jpg)
Motivation
27
![Page 28: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/28.jpg)
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/.
![Page 29: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/29.jpg)
Motivation
After significant offline computation time...
29
![Page 30: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/30.jpg)
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/.
![Page 31: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/31.jpg)
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
![Page 32: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/32.jpg)
n�
Reuse data nrom previous search�
Incremental Search Methods
32
![Page 33: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/33.jpg)
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
![Page 34: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/34.jpg)
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
![Page 35: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/35.jpg)
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
![Page 36: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/36.jpg)
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
![Page 37: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/37.jpg)
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
![Page 38: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/38.jpg)
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
![Page 39: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/39.jpg)
Relauation - Di�kstra�s Algorithm
0
3
�
1
4 3
3
� 3
1 3
6 2
7
�
3
3
39
![Page 40: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/40.jpg)
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
![Page 41: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/41.jpg)
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
![Page 42: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/42.jpg)
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
![Page 43: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/43.jpg)
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
![Page 44: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/44.jpg)
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
![Page 45: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/45.jpg)
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
![Page 46: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/46.jpg)
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
![Page 47: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/47.jpg)
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
![Page 48: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/48.jpg)
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
![Page 49: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/49.jpg)
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
![Page 50: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/50.jpg)
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
![Page 51: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/51.jpg)
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
![Page 52: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/52.jpg)
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
![Page 53: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/53.jpg)
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
![Page 54: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/54.jpg)
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
![Page 55: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/55.jpg)
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.
![Page 56: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/56.jpg)
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
![Page 57: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/57.jpg)
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
![Page 58: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/58.jpg)
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.
![Page 59: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/59.jpg)
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.
![Page 60: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/60.jpg)
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
![Page 61: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/61.jpg)
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
![Page 62: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/62.jpg)
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
![Page 63: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/63.jpg)
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
![Page 64: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/64.jpg)
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
![Page 65: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/65.jpg)
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
![Page 66: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/66.jpg)
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
![Page 67: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/67.jpg)
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
![Page 68: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/68.jpg)
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
![Page 69: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/69.jpg)
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
![Page 70: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/70.jpg)
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
![Page 71: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/71.jpg)
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
![Page 72: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/72.jpg)
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
![Page 73: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/73.jpg)
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
![Page 74: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/74.jpg)
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
![Page 75: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/75.jpg)
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
![Page 76: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/76.jpg)
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
![Page 77: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/77.jpg)
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
![Page 78: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/78.jpg)
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
![Page 79: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/79.jpg)
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
![Page 80: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/80.jpg)
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
![Page 81: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/81.jpg)
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
![Page 82: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/82.jpg)
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
![Page 83: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/83.jpg)
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
![Page 84: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/84.jpg)
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
![Page 85: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/85.jpg)
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
![Page 86: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/86.jpg)
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
![Page 87: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/87.jpg)
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
![Page 88: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/88.jpg)
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
![Page 89: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/89.jpg)
M�!������!� @��!������!��? ������������0����/�1���� �������������� �/�1E
� .%���0���� ��&�� ���0�!��!���0���������� ����0!��&���� �����������*/�1�*����-�
� �����#��0�!��!��0�����������#�*�����@������������ �/�1����� �������� ��P0�0�-
89
![Page 90: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/90.jpg)
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
![Page 91: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/91.jpg)
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
![Page 92: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/92.jpg)
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
![Page 93: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/93.jpg)
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
![Page 94: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/94.jpg)
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
![Page 95: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/95.jpg)
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
![Page 96: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/96.jpg)
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
![Page 97: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/97.jpg)
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
![Page 98: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/98.jpg)
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
![Page 99: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/99.jpg)
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
![Page 100: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/100.jpg)
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
![Page 101: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/101.jpg)
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
![Page 102: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/102.jpg)
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
![Page 103: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/103.jpg)
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
![Page 104: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/104.jpg)
�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
![Page 105: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/105.jpg)
�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
![Page 106: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/106.jpg)
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
![Page 107: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/107.jpg)
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
![Page 108: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/108.jpg)
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
![Page 109: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/109.jpg)
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
![Page 110: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/110.jpg)
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
![Page 111: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/111.jpg)
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
![Page 112: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/112.jpg)
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
![Page 113: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/113.jpg)
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
![Page 114: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/114.jpg)
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
![Page 115: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/115.jpg)
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
![Page 116: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/116.jpg)
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
![Page 117: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/117.jpg)
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
![Page 118: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/118.jpg)
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
![Page 119: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/119.jpg)
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
![Page 120: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/120.jpg)
� � 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
![Page 121: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/121.jpg)
� � 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
![Page 122: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/122.jpg)
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
![Page 123: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/123.jpg)
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
![Page 124: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/124.jpg)
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
![Page 125: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/125.jpg)
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
![Page 126: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/126.jpg)
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
![Page 127: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/127.jpg)
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
![Page 128: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/128.jpg)
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
![Page 129: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/129.jpg)
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
![Page 130: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/130.jpg)
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
![Page 131: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/131.jpg)
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
![Page 132: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/132.jpg)
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
![Page 133: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/133.jpg)
� � 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
![Page 134: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/134.jpg)
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
![Page 135: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/135.jpg)
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
![Page 136: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/136.jpg)
%��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
![Page 137: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/137.jpg)
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
![Page 138: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/138.jpg)
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
![Page 139: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/139.jpg)
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
![Page 140: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/140.jpg)
� � 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
![Page 141: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/141.jpg)
� � 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
![Page 142: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/142.jpg)
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
![Page 143: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/143.jpg)
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
![Page 144: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/144.jpg)
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
![Page 145: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/145.jpg)
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
![Page 146: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/146.jpg)
hen to use incremental path planning�
Goal
146
![Page 147: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/147.jpg)
hen to use incremental path planning�
Goal
;�
147
![Page 148: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/148.jpg)
hen to use incremental path planning�
Goal
148
![Page 149: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/149.jpg)
hen to use incremental path planning�
Goal
149
![Page 150: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/150.jpg)
hen to use incremental path planning�
;�
150
![Page 151: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/151.jpg)
hen to use incremental path planning�
Goal
151
![Page 152: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/152.jpg)
hen to use incremental path planning�
Goal
152
![Page 153: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/153.jpg)
hen to use incremental path planning�
Goal
153
![Page 154: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/154.jpg)
hen to use incremental path planning�
Goal Goal
154
![Page 155: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/155.jpg)
hen to use incremental path planning�
;�
155
![Page 156: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/156.jpg)
hen to use incremental path planning�
;�
156
![Page 157: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/157.jpg)
hen to use incremental path planning�
;�
157
![Page 158: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/158.jpg)
hen to use incremental path planning�
Goal
158
![Page 159: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/159.jpg)
hen to use incremental path planning�
Goal
159
![Page 160: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/160.jpg)
hen to use incremental path planning�
Goal
160
![Page 161: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/161.jpg)
hen to use incremental path planning�
161
Goal
![Page 162: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/162.jpg)
hen to use incremental path planning�
162
Goal
![Page 163: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/163.jpg)
hen to use incremental path planning�
163
Goal
![Page 164: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/164.jpg)
hen to use incremental path planning�
164
Goal
![Page 165: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/165.jpg)
hen to use incremental path planning�
165
Goal
![Page 166: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/166.jpg)
hen to use incremental path planning�
166
Goal
![Page 167: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/167.jpg)
hen to use incremental path planning�
Goal
167
![Page 168: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/168.jpg)
hen to use incremental path planning�
;�
168
![Page 169: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/169.jpg)
hen to use incremental path planning�
;�
169
![Page 170: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/170.jpg)
hen to use incremental path planning�
Goal
170
![Page 171: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/171.jpg)
hen to use incremental path planning�
Goal
171
![Page 172: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/172.jpg)
hen to use incremental path planning�
Goal
172
![Page 173: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/173.jpg)
hen to use incremental path planning�
Goal
173
![Page 174: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/174.jpg)
hen to use incremental path planning�
Goal
174
![Page 175: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/175.jpg)
hen to use incremental path planning�
Goal
175
![Page 176: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/176.jpg)
hen to use incremental path planning�
Goal
176
![Page 177: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/177.jpg)
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
![Page 178: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/178.jpg)
Greedy mapping
178
![Page 179: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/179.jpg)
Greedy mapping Goal
179
![Page 180: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/180.jpg)
Greedy mapping
180
Goal
![Page 181: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/181.jpg)
Greedy mapping
181
Goal
![Page 182: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/182.jpg)
Greedy mapping Goal
182
![Page 183: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/183.jpg)
Greedy mapping Goal
183
![Page 184: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/184.jpg)
Greedy mapping Goal
184
![Page 185: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/185.jpg)
Greedy mapping Goal
185
![Page 186: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/186.jpg)
Greedy mapping Goal
186
![Page 187: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/187.jpg)
Greedy mapping Goal
187
![Page 188: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/188.jpg)
Greedy mapping Goal
188
![Page 189: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/189.jpg)
Greedy mapping Goal
189
![Page 190: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/190.jpg)
Greedy mapping Goal
190
![Page 191: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/191.jpg)
Greedy mapping Goal
191
![Page 192: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/192.jpg)
Greedy mapping Goal
192
![Page 193: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/193.jpg)
Greedy mapping Goal
193
![Page 194: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/194.jpg)
Greedy mapping Goal
194
![Page 195: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/195.jpg)
Greedy mapping Goal
195
![Page 196: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/196.jpg)
Greedy mapping Goal
196
![Page 197: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/197.jpg)
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
![Page 198: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/198.jpg)
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
![Page 199: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/199.jpg)
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
![Page 200: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/200.jpg)
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
![Page 201: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/201.jpg)
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.
![Page 202: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/202.jpg)
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.
![Page 203: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/203.jpg)
Holonomic System �onholonomic System
• �o differential constraints • Differential constraints
203
![Page 204: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/204.jpg)
Methods
• �ell decomposition • Visibility graph • Sampling-based roadmap construction
204
![Page 205: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/205.jpg)
�ell Decomposition
205
![Page 206: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/206.jpg)
�ell Decomposition
206
![Page 207: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/207.jpg)
�ell Decomposition
207
![Page 208: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/208.jpg)
�ell Decomposition
208
![Page 209: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/209.jpg)
�ell Decomposition
209
![Page 210: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/210.jpg)
�ell Decomposition
210
![Page 211: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/211.jpg)
�ell Decomposition
211
![Page 212: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/212.jpg)
�ell Decomposition
212
![Page 213: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/213.jpg)
�ell Decomposition
213
![Page 214: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/214.jpg)
�ell Decomposition
214
![Page 215: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/215.jpg)
�ell Decomposition
215
![Page 216: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/216.jpg)
�ell Decomposition
216
![Page 217: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/217.jpg)
�ell Decomposition
217
![Page 218: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/218.jpg)
�ell Decomposition
218
![Page 219: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/219.jpg)
�ell Decomposition
+ ?��%������������������& �����#����������2��������
+ �� �������������+ :��� ���������������
219
![Page 220: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/220.jpg)
Visibility Graph
220
![Page 221: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/221.jpg)
Visibility Graph
221
![Page 222: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/222.jpg)
Visibility Graph
222
![Page 223: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/223.jpg)
Visibility Graph
223
![Page 224: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/224.jpg)
Visibility Graph
224
![Page 225: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/225.jpg)
Visibility Graph
+ ?��%������������������& �����#����������2��������
+ :��� ���������������+ :������� + B���0����#��
225
![Page 226: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/226.jpg)
Sampling-based Roadmap �onstruction
Deterministic sampling Random sampling
226
![Page 227: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/227.jpg)
Sampling-based Roadmap �onstruction
227
![Page 228: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/228.jpg)
�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
![Page 229: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/229.jpg)
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
![Page 230: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/230.jpg)
230
![Page 231: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/231.jpg)
�ackup
231
![Page 232: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/232.jpg)
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
![Page 233: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/233.jpg)
D* Algorithm
233
![Page 234: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/234.jpg)
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.
![Page 235: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/235.jpg)
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
![Page 236: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/236.jpg)
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
![Page 237: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/237.jpg)
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
![Page 238: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/238.jpg)
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
![Page 239: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/239.jpg)
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/.
![Page 240: Incremental Path Planning - MIT OpenCourseWare · Incremental Path Planning April 4, 2016. Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto](https://reader035.fdocuments.us/reader035/viewer/2022062505/5e2fbe56bc6cc8192d7c688c/html5/thumbnails/240.jpg)
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.