incremental path planning - mit opencourseware · incremental path planning april 4, 2016. joe...

Incremental Path Planning

1

April 4, 2016Joe Leavitt, Ben Ayton, Jessica Noss, Erlend Harbitz, Jake Barnwell & Sam Pinto

1

References • Koenig, S., & Likhachev, M. (2002, July). D* Lite. In AAAI�IAAI (pp. 476-4�3). • Koenig, S., Likhachev, M., & Furcy, D. (2004). Lifelong planning A�. Artificial

Intelligence, 155(1), 93-146. • Stentz, A. (1994, May). Optimal and efficient path planning for partially-kno n

environments. In Proceedings of the IEEE International Conference on Robotics and Automation.

• Likhachev, M., Ferguson, D. I., Gordon, G. J., Stentz, A., & hrun, S. (200�, June). Anytime Dynamic A*: An Anytime, Replanning Algorithm. In ICAPS (pp. 262-271).

• Hofmann, A., Fernandez, E., Helbert, J., Smith, S., & illiams, �. (201�). Reactive Integrated Motion Planning and Euecution. In Proceedings of the Twenty-Fourth International Joint Conference on Artificial Intelligence.

2

Outline

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

3

Motivation

4

Motivation

5

Motivation

�e obstacle detected�

Replan

6

Motivation


Replan

7

Motivation


Replan

8

Motivation


Replan

9

Motivation

10

Motivation

11

Motivation

12

Motivation

13

Motivation

�hange detected�

Replan

14

Motivation Environmental �hange

15

Motivation


Replan

16

Motivation


Replan

17

Motivation

18

Motivation

19

Motivation

20

Motivation

21

Motivation

22

Motivation

23

Motivation

24

Motivation

25

Motivation

26

Motivation

27

Motivation RR

Watch the video on YouTube ( https://www.youtube.com/watch?v=vW74bC-Ygb4)28

© Sertac Karaman. All rights reserved. This content is excluded from our CreativeCommons license. For more information, see https://ocw.mit.edu/help/faq-fair-use/.

https://www.youtube.com/watch?v=vW74bC-Ygb4

https://ocw.mit.edu/help/faq-fair-use/

Motivation

After significant offline computation time...

29

Motivation RR *

30

© Sertac Karaman. All rights reserved. This content is excluded from our CreativeCommons license. For more information, see https://ocw.mit.edu/help/faq-fair-use/.


Problems • Changing environmental conditions:

o Obstacles

o Utility or cost

• Sensor limitations:

o Partial observability

• Computation time:

o Complete, optimal replanning is slow

o Stay put or move in wrong direction?

31

n�

Reuse data nrom previous search�

Incremental Search Methods

32

Incremental APSP

Alg. Mean Std �omp. Opt.

�hekhov (APSP) 0.042� 0.0213 es

RR 0.1�� 0.12 Prob. �o

RR connect 0.042 0.04 Prob. �o

PRM 0.12 0.0� Prob. Asym.

8��"�*��8��!�$��"�#��(�� '�?��/��=1 33

Courtesy of Hofmann, Andreas et al. License: cc by-nc-sa.

es

Outline


34

Incremental Search

• Pernorm graph search • Repeat:

o Execute path/plan o Receive graph changes o Update previous search results

From Koenig, Likhachev, & Furcy (2004)

35

asin33

Line

asin33

Line

asin33

Line

asin33

Line

Revie of Graph Search Problem

Input: graph search problem S = <gr, w, h, sstart

• directed graph: gr = <V, E> • edge eighting function: w: s x s � � • heuristic function: h: s x sgoal � � • start verteu: s € Vstart• goal verteu: sgoal € V

Output: simple path P = <s , s2, ", sgoal>start

, sgoal> s1

sstart sgoal

s2

(sstart,s2)

(s2,sgoal)

(s ,s1)start

(s1,sgoal)

h(s2,sgoal)

h(s1,sgoal)

h(sstart,sgoal)

36

Revie of Graph Search Problem

• g: cost-so-far

o ) + , s)g(s) = g(spredecessor (spredecessor

• h: heuristic value, cost-to-go

• f(s) = g(s) + h(s)

s1

sstart sgoal

s2

(sstart,s2)

(s2,sgoal)

(sstart,s1)

(s1,sgoal)

h(s2,sgoal)

h(s1,sgoal)

h(sstart,sgoal)

37

Graph Representations • Incremental algorithms generalize to any graph (usually non-negative edges). • Grids used for ease of representation.

�-�onnected Graph4-�onnected Graph

Admissible Manhattan Distance mau(lu, ly) Heuristic: = lu + ly ( ith unit edge eights)

38

w

Relauation - Di�kstra�s Algorithm

0

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

39


3

0 �

1

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

0

L L

L

LL

40


3

0 4

1 6

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

1 L

L L

L

41


3 7

0 4

1 6

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

3

L

L

L

42


3 6 9

0 4 11

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

4

43


3 6 9

0 4 11

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

� 44


3 6 9

0 4 11

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

6

45


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

7 46


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

9

47


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

10

48


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

49


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

50


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

51


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

52


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

53


3 6 9

0 4 10

1 � 7

3

�

1

4 3

3

� 3

1 3

6 2

7

�

3

3

54

Ho to Reuse Previous Search Results�

• Store optimal results: o shortest paths o g-values

• Find inconsistencies: o local consistency

• Make consistent: o relaxations

(a) (b) (c)

From Koenig, Likhachev, & Furcy (2004) 55

Courtesy of Elsevier, Inc., http://www.sciencedirect.com. Used with permission.

Source: Figures 3 and 4 in Keonig, Sven, M. Likhachev, and D. Furcy.

"Lifelong Planning A*." In Artificial Intelligence, 155 (1-2): 93-146.

http://www.sciencedirect.com

https://doi.org/10.1016/j.artint.2003.12.001

Incremental Search Methods - Euamples • General

o Incremental APSP o Lifelong Planning A* (LPA*) o Dynamics Strictly eakly Superior Function - Fiued Point (DynamicsS SF-FP) o ...

• Mobile Robots o D* o D* Lite

• emporal Planning o Incremental emporal �onsistency (I �)

• Propositional Satisfiability o Incremental Unit Propagation

56

D* Incremental Path Planning Approach

Heuristic Search (e.g. A*)

Optimal & Efficient

Incremental + Search =

(e.g. DynamicsS SF-FP)

Fast Replanning

Efficient Incremental Path Planning

(e.g. LPA*, D* Lite, ...)

Fast, Optimal

Replanning

57

Initial Search


58




swani1

Line



Follo -on Search


59




swani1

Line



Outline


60

A* Reminder A* is best-first search from start to goal sorted by a cost f(s):

f(s) = g(s) + h(s,sgoal)

f(s) = total node cost

g(s) = path cost to reach verteu from sstart

h(s,sgoal) = heuristic for cost to reach verteu sgoal from verteu s

61

A*: �hoosing �et een ies Introduce a ne notation: f(s) = ( f1(s), f2(s) )

f(s) = ( g(s) + h(s,sgoal), g(s) )

sstart

2 2

3 1

1

1s2

�

h = 2 h = 1

s1 f(s1) = �

f(s2) = �sgoal

h = 1h = 3 62

T


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


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


f(s) = ( g(s) + h(s,sgoal), g(s) )

f(s1) = ( �, 3 )

f(s2) = ( �, 2 )sstart sgoal

2 2

3 1

1

1s2

�

h = 2 h = 1

s1

h = 1h = 3 65

T

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

Predecessors of verteu s: Every node from hich s can be reached, Pred(s)

66

w



67

w



68

w



69

w

hat is D* Lite�

Efficient repeated best-first search through a graph ith changing edge eights as the graph is traversed.

sstart sgoal sgoal

sstart

�an be vie ed as replanning through relauation of path costs.

70

w w

Reformulation: Minimize Recomputation

hen the start changes, the path cost from start to s is not preserved. W

�ase 1: �ase 2:

s 2

s 2 1

s sstart

2 2

1 s sgoalsgoalstart

g2(s) = 2(s) = 2 + 2 = 4g1 g1(s) t g2(s)

71

Reformulation: Minimize Recomputation

Reformulate search from goal to start. Path cost from goal to s is preserved.

�ase 1: �ase 2:

s 2

s 2 s 2 1

s sgoalsgoal1

start

sstart

2

g2(s) = 1(s) = 1g1 g1(s) = g2(s)

72

Overall D* Lite: 1. Initialize all nodes as uneupanded.

2. �est-first search until s is consistent ith neighbors and eupanded.start

3. Move to neut best verteu.

4. If any edge costs change:

a. rack ho heuristics have changed.

b. Update source nodes of changed edges.

�. Repeat from 2.

73

w

start is consistent


2. �est-first search until s ith neighbors and eupanded.





�. Repeat from 2.

2. �est-first search until s start is consistent ith neighbors and eupanded.

Most computation

74

w

rack ho


2. �est-first search until sstart is consistent ith neighbors and eupanded.



a. heuristics have changed.


�. Repeat from 2.




Incremental component

75

w







�. Repeat from 2.


76

w

Eutracting a Path Given Path �ost Move from s to the successor hich gives s the lo est path cost.start start

s � argmin (c(s ,s�) + g(s�))start s� E Succ(sstart) start

g = 7

g = �

g = 4

2

6

77

w

start is consistent


2. �est-first search until s ith neighbors and eupanded.





�. Repeat from 2.


2. �est-first search until s start is consistent ith neighbors and eupanded.

78

w

Handling eight �hanges Locally Self-consistent graph:

g(s) = mins� E Succ(s) (g(s�) + c(s,s�))

May no longer be true hen edge eights change�

g = 7

g = �

g = 4

2

6

g = �

g = 4

2

1

g = 7

79

W

W W

Handling eight �hanges Locally �hanges propagate to predecessors.

For efficient search, update lo est cost nodes first.

- Use a priority queue like A*. Update nodes until the goal is first eupanded.

g = �

g = 4

1

2

g = 7

80

W

Handling eight �hanges Locally Store an additional value:

rhs(s) = min (g(s�) + c(s,s�))s� E Succ(s)

Local inconsistency: rhs(s) t g(s)

g = 7

rhs = �

g = �

g = 4

2

1

81

W

Handling eight �hanges Locally Store an additional value:

rhs(s) = min (g(s�) + c(s,s�))s� E Succ(s)

Local inconsistency rhs(s) t g(s)

g = 7

rhs = �

g = �

g = 4

2

1

82

W

Local Inconsistencies Signal recomputation is necessary for node and predecessors

1. Locally overconsistent:

g(s) > rhs(s)

2. Locally underconsistent:

g(s) < rhs(s)

83

Updating and Eupanding �odes Update by recomputing rhs and placing the node on the priority queue if locally inconsistent.

g = 7 rhs = 7

g = �

g = 4

2

6 s

84


g = 7 rhs = �

g = �

g = 4

2

1s

85


g = 7 rhs = �

g = �

g = 4

2

1s = �..., s, ...�

86

Updating and Eupanding �odes Eupand by taking it off the priority queue and changing g. Eupand in order of total cost:

( min� g(s), rhs(s) � + h(s,s ) , min� g(s), rhs(s) � )start

g = 7 rhs = �

h = 4

g = �

g = 4

2

1s

S�F�T/��9�=�1��---U

87

Updating and Eupanding �odes Eupand by taking it off the priority queue and changing g. Eupand in order of total cost:


g = � rhs = �

h = 4

2

1s

g = �

g = 4 S�F�T---U

88

M�!��!� @��!��!��? ��0��/�1�� /�1E

� .%��0�� &�� 0�!��!��0�� 0!��&�� */�1�*��-�

� ��#��0�!��!��0��#�*��@�� /�1�� P0�0�-

89

g(s) > rhs(s) (Overconsistent): �e path cost rhs(s) is better than the old path cost g(s).

Immediately update g(s) to rhs(s) and propagate to all prede�essors�

- Set g(s) = rhs(s) - Update all predecessors of s

Verteu is no locally consistent and will remain that way.

90

g(s) < rhs(s) (Underconsistent): Old path cost g(s) is better than ne path cost rhs(s).

Delay vertex expansion and propagate to all prede�essors�

- Set g(s) = - Update all predecessors of s and s itself

Verteu is no locally consistent or locally overconsistent. It ill remain on the queue until rhs(s) is the neut best cost.

91

W

L

g

s rh2

h

1

2

s1 6

g = 7 rhs = 7

h = 4

= 9 s = 9

g = �

g = 4

g(s) < rhs(s) (Underconsistent):

92

s2

h = 4

g(s) < rhs(s) (Underconsistent): Assume after update:

g = 4

g = ��

6

1

s1

s2

g = 9 rhs = �

h = 4

6 g = 7 rhs = 10

h = 4

93

g(s) < rhs(s) (Underconsistent): Assume after update:

g = 4

g = ��

6

1

s1

s2

g = 9 rhs = �

h = 4

6 g = 7 rhs = 10

h = 4

S�F�T/��4�1��/��6�1U

94


g = 4

g = ��

6

1

s1

s2

g = 9 rhs = �

h = 4

6 g = 7 rhs = 10

h = 4

S�F�T/��4�1��/��6�1U

95


g = 4

g = ��

6

1

s1

s2

g = 9 rhs = �

h = 4

6 g = rhs = 10

h = 4

S�F�T/��4�1��/��6�1U

96

L


g = 4

g = ��

6

1

s1

s2

g = 9 rhs = �

h = 4

6

S�F�T/��6�1��/��1U

��F�L� ��F��

�F��

97


g = 4

g = ��

6

1

s1

s2

g = � rhs = �

h = 4

6 g = rhs = 10

h = 4

S�F�T/��6�1��/��1U

98

L


g = 4

g = ��

6

1

s1

s2

g = � rhs = �

h = 4

6 g = rhs = 9

h = 4

S�F�T/��1U

99

L


g = 4

g = ��

6

1

s1

s2

g = � rhs = �

h = 4

6 g = rhs = 9

h = 4

1 S�F�T/� ��7�9�1U

100

L


g = 4

g = ��

6

1

s1

s2

g = � rhs = �

h = 4

6 g = 9 rhs = 9

h = 4

S�F�T/��7�9�1U

101


g = �

�

6

1

s1

s2 rhs = �

h = 4

6 g = 9 rhs = 9

h = 4

g = �

g = 4

S�F�TU

102

rack ho





a. heuristics have changed.


�. Repeat from 2.


103

W

�arrying Over the Priority ueue

After a path is found, the priority queue is not empty.

he value on the priority queue is:


hen s is different, the heuristics are different�start

104

Q

�arrying Over the Priority ueue

� s : All heuristics lo ered by at most h(slast,s ).slast start start

hen adding ne nodes to the queue, increase total cost by h(slast,s ).start

Increase k = k + h(s last,s ):m m start

( min� g(s), rhs(s) � + h(s,s ) + k , min� g(s), rhs(s) � )start m

105

Q

Overall D* Lite: 1. Initialize all g(s) = , rhs(s t sgoal) = , rhs(sgoal) = 0, k = 0, slast = s . m start

2. �est-first search until s is locally consistent and eupanded.start

3. Move so s = argmin (c(s ,s�) + g(s�)).start s� E Succ(sstart) start


a. k = k + h(s last,s ).m m start

b. Update rhs and queue position for source nodes of changed edges.

�. Repeat from 2.

Al ays sort by ( min� g(s), rhs(s) � + h(s,s ) + k , min� g(s), rhs(s) � )start m

106

LL

Outline


107

Overall D* Lite: 1. Initialize all g(s) = , rhs(s t sgoal) = , rhs(sgoal) = 0, k = 0, slast = s . m start

2. �est-first search until s is locally consistent and eupanded.start

3. Move so s = argmin (c(s ,s�) + g(s�)).start s� E Succ(sstart) start


a. k = k + h(s last,s ).m m start

b. Update rhs and queue position for source nodes of changed edges.

�. Repeat from 2.

Al ays sort by ( min� g(s), rhs(s) � + h(s,s ) + k , min� g(s), rhs(s) � )start m

108

LL

D* Lite Euample • Heuristic: h(A, �) = minimum number of nodes to get from A to �

o h(node, start) ritten in each node • •

�idirectional graph Edge eights labeled

0 3 1

1

1

1

10

1

Robot starts here

Goal node

2

2

1

109

k = 0 m 23�� @��-��$�+ ��F�� F�L��@��+ !!��P0�0��&�� %��R7��+ %��!�*��F��

0 1 1 3

1

1

1

10

1

��F�L� ��F�L

��F�L� ��F�L

��F�L� ��F�L

��F�L� ��F�L

��F�L� ��F��R7��Q

Q 2

2

110

D* Lite Euample 2. Plan initial path (A*) • dequeue node • update g(node)

0 3

1

1

1

1

10

1

k = 0 m

g � � rhs = 0 (de�ueued)

��F�L� ��F�L

��F�L� ��F�L

��F�L� ��F�L

��F�L� ��F�L

2

1

2

111

k = 0D* Lite Euample

m

2. Plan initial path (A*) • dequeue node In this euample, the presence of

• • •

g � � rhs = 0 (de�ueued)

update g(node) update rhs(neighbor) queue inconsistent neighbors

0

3 1

1

1

1

10

1

a key <u,y> indicates that the node is in the min priority queue.

��F�L� ��F�L

��F�L� ��F�L

��F�L�� !��"

��F�L��#! "

2

2

1

112

k = 0 mg � 1D* Lite Euample rhs = 12. Plan initial path (A*) (de�ueued)• dequeue node

• • •


0 3 1

1

1

1

10

1

rhs = 10 <12,10>

g = 0 rhs = 0

��F�L� ��F�L

��F�L

��F�L� ��F�L 2

2

1

113

k = 0 mg � 1D* Lite Euample rhs = 12. Plan initial path (A*) (de�ueued)• dequeue node

• • •


0 3 1

1

1

1

10

1

g = 0 rhs = 0

�ote that this rhs decreased, and the node�s key changed accordingly.

��F�L� ��F�L

��F�L�� #! "

��F�L�� $! "

1

2

2

114

k = 0 mg = 1D* Lite Euample rhs = 12. Plan initial path (A*)

• dequeue node • • •


2

0 3 1

1

1

1

10

1

g � 2 rhs = 2 (de�ueued)

g = 0 rhs = 0

��F�L� ��F��R��Q

��F�L� ��F�L 2

2

1

115

D* Lite Euample 2. Plan initial path (A*) • dequeue node • • •

g = 1 rhs = 1

k m = 0 his node�s rhs as updated, but the value stayed the same.


0 3 1

1

1

1

10

1


g = 0 rhs = 0

his node�s rhs as updated, but the value stayed the same.

��F�L� ��F��R��Q

��F�L��#�#!#"

2

2

1

116

k = 0 mg = 1D* Lite Euample rhs = 12. Plan initial path (A*)

• dequeue node • • •


0 3 1

1

1

1

10

1


g = 2 rhs = 2

g = 0 rhs = 0

��F�L� ��F��R��Q

2

2

1

117

k = 0 mg = 1D* Lite Euample rhs = 1Found shortest path to goal�

(edges in blue)

0 3 1

1

1

1

10

1

g = 3 rhs = 3

g = 2 rhs = 2

g = 0 rhs = 0

��F�L� ��F��R��Q

2

2

1

118

k = 0 mg = 1D* Lite Euample rhs = 13. Robot moves (green)

• update heuristics for ne start node • maintain queue from before

0 2

1

1

1

1

10

1

g = 3 rhs = 3

g = 2 rhs = 2

g = 0 rhs = 0

��F�L� ��F��R��Q

119

1

1

1

� � 1 m= 1s = 1

1

0 2 1

1

10

g = 2 rhs = 2

g = 0 rhs = 0

g = 3 rhs = 3

��F�L� ��F��R��Q

1

1

23�� @�� g �-�:#��!��!�/!�%��1 rh+ !J��!��&�� F�L+ 0�!��%�

120

� � 1m

rhs(goal) = 0,

1

0 2

1 10

g = 2 rhs � 4 <3,2>

g = 0 rhs = 0

g = 3 al ays rhs = 3

��F�L��#�'!#"

��F��%�#!�"

1

1

%%

23�� @��-�:#��!��!�/!�%��1+ !J��!��&�� F�L+ 0�!��%�+ 0�!�� *��0��!��

% 1

121

k m = 1Underconsistent�

D* Lite Euample �. Repeat from 2 2. Replan path

g = 0 rhs = 0

g = 3 g = 2 rhs = 3 rhs = 4 Update rhs of

<3,2> neighbors, but no values change.

1

0 2

1

1 10

��F�L� ��F�7R=�7Q

��%� ��F�L��

1

1

%

%

%

122

k = 1 m

g = 0 rhs = 0

g = 3 rhs = 3

Underconsistent�

D* Lite Euample 2. Replan path

1

0 2

1

1 10

��F�L� ��F�7R=�7Q

��F�L� ��F�L

��%� ��F��

1

1

% %

%

123

D* Lite Euample

2. Replan path

k = 1 m

1

0 2

1

1 10

g = 0 rhs = 0

��F�L� ��F�L

��F�L�� !��"

��%� ��F��

��F�7��%�'!#"

1

1

%

%

%

124


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


k = 1 m

1

0 2 1

1 10

g = 0 rhs = 0

��F�L� ��F�L

��F�L� ��F��R��Q

��F�L� ��F�� !��"

��%� ��F�L�� 1

1 %

%

%

126

D* Lite Euample

2. Replan path

k = 1 m

g = 0 rhs = 0

1

0 2 1

1 10

g � 1� rhs = 10 (de�ueued)

��F�L� ��F�L

��F�L� ��F��R��Q

��F�L� ��F�L

1

1

%

%

%

127

D* Lite Euample

2. Replan path

k = 1m

g = 0 rhs = 0

1

0 2 1

1 10

g � 1� rhs = 10 (de�ueued)

��F�L� ��F��R��Q

��F�L� ��F�L

��F�L� ��F�L

< ��!�K�� &��0�!��!��#0�� 0��!�� -

1

1

%

% %

128

D* Lite Euample

2. Replan path

k = 1 m

1

0 2

1

1 10


g = 10 rhs = 10

g = 0 rhs = 0

��F�L� ��F�L��F�L� ��F�L

��F�L� ��F�L

1

1

%

%

%

129

D* Lite Euample

2. Replan path

k = 1 m

1

0 2 1

1 10


g = 10 rhs = 10

g = 0 rhs = 0

��F�L�� $!� "

��F�L� ��F�L

1

1

%

%

%

130

k = 1 mD* Lite Euample

Found ne shortest path (blue) from current start (green) to goal (red)

g = 10 rhs = 10

1

0 2

1

1 10

g = 11 rhs = 11

g = 0 rhs = 0

��F�L� ��F��R��Q

��F�L� ��F�L

1

1

%

%

%

131

k = 1 mD* Lite Euample

3. Robot moves (green) • update heuristics for ne start node • maintain queue from before

1

1

0

1

1 10

g = 11 rhs = 11

g = 10 rhs = 10

g = 0 rhs = 0

��F�L� ��F��R��Q

��F�L� ��F�L

1 2 %

%

%

132

� � 2 m

g = 10 rhs = 10

1 1

0

1

10

1

g = 11 rhs = 11

g = 0 rhs = 0

��F�L� ��F�L

��F�L� ��F��R��Q

1

2 %

%

%

23�� @��-�:#��A+ 0�!��!��&�� + 0�!��%�

133

1 1

0

1

10

1

g = 10 rhs = 10

� � 2 m

g = 0 rhs = 0

��F�L��%

��F��%��$!��"

��F�L��$!�"

1

2 %

%

�� !��&��!�*�� P0�0��#��0��&��

23�� @��-�:#��A

+ 0�!��!��&�� + 0�!��%�+

0�!�� *��0��!��

%

134

k = 2 mg � 1D* Lite Euample rhs = 1�. Repeat from 2 (de�ueued)2. Replan path

g = 0 rhs = 0

1 1

0

1

10

1

g = 10 rhs = 10

��F�� F�LR��Q

��F�L� ��F�L

1

2 %

%

%

135

%��F��D* Lite Euample 2. Replan path

g = 0 rhs = 0

1 1

0

1

10

1

g = 10 rhs � 2 <4,2>

�� F��

��F�� F�LR��Q

��F�L� ��F�L

%%

%

1

2

136

g = 1D* Lite Euample rhs = 12. Replan path

k = 2 m

g = 0 rhs = 0

g = rhs =

1 1

0

1

10

1

g = 11 rhs = <14,11>


%% %

%

%

%

1

2

137

D* Lite Euample Found ne shortest path�

1 1

0

1

10

1

g = 11 rhs = <14,11>

g = 2 rhs = 2

g = 1 rhs = 1

k = 2 m

g = 0 rhs = 0

g = rhs =

%%

%

%% %

�� &��!�!�K��!�P0�0�� !��0�!��

1

2

138

k = 2 mg = 1D* Lite Euample rhs = 13. Robot moves

• update heuristics for ne start node • maintain queue from before

0

1 1

1

10

1

g = rhs =

g = 11 rhs = <14,11>

g = 2 rhs = 2

g = 0 rhs = 0

%

%% %

%

%

1

2

139

� � 3 mg = 1rhs = 1

g = 2 rhs = 2

0

1

1

1 1 1

g = 11 rhs = <14,11>

g = 0 rhs = 0

g = rhs =

%%

%

%

%%

1 2

23�� @��-�:#�� #��A+ 0�!��!��&�� + 0�!��%�

140

� � 3 m 1

g = 2

1

1

1

g = 11 rhs � 2 <6,11>

g = 0 rhs = 0

g = rhs � 12 <17,12>

rhs � <6,2>

% %

%%

%

2 1

23�� @�� g = 1rhs =�-�:#�� #��A

+ 0�!��!��&�� + 0�!��%�+ 0�!�� *��0��!�� 0

1 1

%

141

k = 3 mg = 1D* Lite Euample rhs = 1�. Repeat from 2

2. Replan path� • �o need to dequeue any nodes

because the smallest key (<6,2>) is greater than key(start) (<4,1>).

0

1

1

1 1 1

g = rhs = 12 <17,12>

g = 11 rhs = 2 <6,11>

g = 2 rhs = <6,2>

g = 0 rhs = 0

%

%

%%

%

1 2

142

k = 3 mg = 1D* Lite Euample rhs = 13. Robot moves

• Robot reaches goal: Done�

g = 0 rhs = 0

g = rhs = 12 <17,12>

1 1 1

g = 11 rhs = 2 <6,11>

g = 2 rhs = <6,2>

%

%

%%

%

143

Outline


144

Efficiency

• A* eupands each node at most once • D* Lite (and LPA*) eupands each node at most t ice

o �ut ill in most cases eupand fe er nodes than A* • A* might performs better if:

o �hanges are close to start node o Large changes to the graph

145

hen to use incremental path planning�

Goal

146


Goal

;�

147


Goal

148


Goal

149


;�

150


Goal

151


Goal

152


Goal

153


Goal Goal

154


;�

155


;�

156


;�

157


Goal

158


Goal

159


Goal

160


161

Goal


162

Goal


163

Goal


164

Goal


165

Goal


166

Goal


Goal

167


;�

168


;�

169


Goal

170


Goal

171


Goal

172


Goal

173


Goal

174


Goal

175


Goal

176

Outline


177

Greedy mapping

178

Greedy mapping Goal

179

Greedy mapping

180

Goal

Greedy mapping

181

Goal

Greedy mapping Goal

182

Greedy mapping Goal

183

Greedy mapping Goal

184

Greedy mapping Goal

185

Greedy mapping Goal

186

Greedy mapping Goal

187

Greedy mapping Goal

188

Greedy mapping Goal

189

Greedy mapping Goal

190

Greedy mapping Goal

191

Greedy mapping Goal

192

Greedy mapping Goal

193

Greedy mapping Goal

194

Greedy mapping Goal

195

Greedy mapping Goal

196

Anytime Dynamic A* (AD*)

• �ombines the benefits of anytime and incremental planners

• In the real orld: o �hanging environment (incremental planners) o Agents need to act upon decisions quickly

(anytime planners)

Ref: Likhachev, M., Ferguson, D. I., Gordon, G. J., Stentz, A., & hrun, S. (200�, June). Anytime Dynamic A*: An Anytime, Replanning Algorithm. In ICAPS(pp. 262 -271). 197

Anytime Planners

• Usually start off by computing a highly suboptimal solution o hen improves the solution over time

• A* ith inconsistent heuristics for euample o Inflated heuristic values give substantial speed-up o Inflated heuristics make the algorithm prefer to keep eupanding

paths o Use incrementally less inflated heuristics to approach optimality

198


• Starts off by setting a sufficiently high inflation factor o Generates suboptimal plan quickly

• Decreases inflation factor to approach optimality • hen changes to edge costs are detected the current solution is repaired o If the changes are substantial the inflation factor is increased to generate a ne

plan quickly

199

Outline


200

Application to mobile robotics

Reference: Lecture given by Michal ��p in 2.166 and paper currently in revie

+ "�&�!��&��*��*��0�� E

201

This image in the public domain.

Application to mobile robotics

Reference: Lecture given by Michal ��p in 2.166 and paper currently in revie

+ "�&�!��&��*��*��0�� E

202

This image in the public domain.

Holonomic System �onholonomic System

• �o differential constraints • Differential constraints

203

Methods

• �ell decomposition • Visibility graph • Sampling-based roadmap construction

204

�ell Decomposition

205


206


207


208


209


210


211


212


213


214


215


216


217


218


+ ?��%��& ��#��2��

+ �� + :��

219

Visibility Graph

220

Visibility Graph

221

Visibility Graph

222

Visibility Graph

223

Visibility Graph

224

Visibility Graph

+ ?��%��& ��#��2��

+ :�� + :�� + B��0��#��

225

Sampling-based Roadmap �onstruction

Deterministic sampling Random sampling

226

Sampling-based Roadmap �onstruction

227

�onnecting Samples

• Steering function used o Steer(a, b) gives feasible path bet een samples o �onholonomic

• Dubins path o Shortest path bet een t o points o �an be constructed of mauimum curvature and straight lineline

segments • RSR, RSL, LSR, LSL, RLR or LRL

228

Outline


229

�ackup

231

Incremental Path PlanningIncremental Path Planning • Dynamic A* (D*) - Stentz, 1994

o Initial combination on A* and incremental search nor mobile robot path planning.

• DynamicsSWSF-FP - Ramalingam and Reps, 199�

o Incremental search technique.

• Linelong Planning A* (LPA*) - Koenig and Likhachev, 2001

o Generalizes A* and DynamicsSWSF-FP nor indeninite re-planning on ninite graph.

• D* Lite - Koenig and Likhachev, 2002

o Extends LPA* to provide D* nunctionality nor mobile robots.

o Simpler normulation than D*.

• More recent extensions to D* and D* Lite, including Anytime D* (AD*), Field D*, etc.

• Other methods... 232

D* Algorithm

233

LPA* Algorithm


234






Revie of A* • Use admissible heuristic, h(s) < h*(s), to guide search. • Keep track of total cost distance from start, g(s). • Order node eupansions using priority �ueue, ith priorities �(s) � g(s) � h(s). • Avoid re-eupanding nodes by using eupanded list. • �etter heuristics (ho closely h(s) approuimates of h*(s)) improve search

speed. • Guaranteed to return optimal solution if one euists.

235

w

RHS Values • One-step look-ahead on g-values, rhs(s) = 0 if s is beginning node of search,

other ise:

rhs(s) = min (g(s') + c(s', s))s' ( pred(s)

• Potentially better innormed than g-value anter changes to search graph. • Note: term comes nrom grammar rules used in DynamicsSWSF-FP algorithm, no

other signinicance.

236

Local �onsistency

• Tells us which nodes may need g-values updated in order to nind shortest path. • Node s is locally consistent inn:

g(s) = rhs(s)

• Node s is locally overconsistent inn:

g(s) > rhs(s)

• Node s is locally underconsistent inn:

g(s) < rhs(s)

• Initially, all nodes are locally consistent with g(s) = rhs(s) = , with exception on start

node, rhs(s ) = 0 and g(s ) =start start

237

LL

B��*�� 3(��>

+ 8�� @��!��!��!��3��*��3�#��%��*��*��5�0��-

2�**��>

+ ��P0�0��!��!�0��%��%/�1>G %/�1�F�T%�/�1\�%�/�1U

�G % /�1�F�*/�1�F��/�/�1�� /�11�H� /�1�G % /�1�F��/�1�F��/�/�1�� /�11

G ��@�� !��%/�1�R�%K/�1��**>�Z % /�1�R�%�K/�1

Z :)�/%�/�1�F�%�K/�1��2�%�/�1�R�%�K/�11+ ��@��!�!��!��5�@��!�#�� %�-+ ��!��#��@��!�!��&��!��!�� *��& ��

0�!��!��& ��-238


Fig: Likhachev, M., Ferguson, D. I., Gordon, G. J., Stentz, A., & hrun, S. (200�, June). Anytime Dynamic A*: An Anytime, Replanning Algorithm. In ICAPS(pp. 262-271).

239

© American Association for Artificial Intelligence. All rights reserved.

This content is excluded from our Creative Commons license. For

more information, see https://ocw.mit.edu/help/faq-fair-use/.


MIT OpenCourseWarehttps://ocw.mit.edu

16.412J / 6.834J Cognitive RoboticsSpring 2016

For information about citing these materials or our Terms of Use, visit: https://ocw.mit.edu/terms.

https://ocw.mit.edu

https://ocw.mit.edu/terms

incremental path planning - mit opencourseware · incremental path planning april 4, 2016. joe...

Documents