incremental path planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · incremental path...

99
Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834 Cognitive Robotics February 17 th , 2004

Upload: others

Post on 19-Jul-2020

22 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Incremental Path Planning

Dynamic and Incremental A*

Prof. Brian Williams (help from Ihsiang Shu)

16.412/6.834 Cognitive RoboticsFebruary 17th, 2004

Page 2: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Outline

Review: Optimal Path Planning in Partially Known Environments.Continuous Optimal Path Planning

Dynamic A*Incremental A* (LRTA*)

Page 3: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

[Zellinsky, 92]

1. Generate global path plan from initial map.

2. Repeat until goal reached or failure:Execute next step in current global path planUpdate map based on sensors.If map changed generate new global path from map.

Page 4: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Compute Optimal Path

J M N O

E I L G

B D H K

S A C F

Page 5: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Begin Executing Optimal Path

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

Robot moves along backpointers towards goal.Uses sensors to detect discrepancies along way.

Page 6: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Obstacle Encountered!

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

At state A, robot discovers edge from D to H is blocked (cost 5,000 units).Update map and reinvoke planner.

Page 7: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Continue Path Execution

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

A’s previous path is still optimal.Continue moving robot along back pointers.

Page 8: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Second Obstacle, Replan!

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

At C robot discovers blocked edges from C to F and H (cost 5,000 units).Update map and reinvoke planner.

Page 9: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Path Execution Achieves Goal

h = 5

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

Follow back pointers to goal.No further discrepancies detected; goal achieved!

Page 10: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Outline

Review: Optimal Path Planning in Partially Known Environments.Continuous Optimal Path Planning

Dynamic A*Incremental A* (LRTA*)

Page 11: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

What is Continuous Optimal Path Planning?

Supports search as a repetitive online process.Exploits similarities between a series of searches to solve much faster than solving each search starting from scratch.Reuses the identical parts of the previous search tree, while updating differences.Solutions guaranteed to be optimal.On the first search, behaves like traditional algorithms.

D* behaves exactly like Dijkstra’s.Incremental A* A* behaves exactly like A*.

Page 12: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Dynamic A* (aka D*) [Stenz, 94]1. Generate global path plan from initial map.

2. Repeat until Goal reached, or failure.Execute next step of current global path plan.Update map based on sensor information.Incrementally update global path plan from map changes.

1 to 3 orders of magnitude speedup relative to a non-incremental path planner.

Page 13: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Map and Path Conceptsc(X,Y) : Cost to move from Y to X. c(X,Y) is undefined if move disallowed.

Neighbors(X) : Any Y such that c(X,Y) or c(Y,X) is defined.

o(G,X) : Optimal path cost to Goal from X.

h(G,X) : Estimate of optimal path cost to goal from X.

b(X) = Y : backpointer from X to Y. Y is the first state on path from X to G.

Page 14: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Search ConceptsOPEN list : States with estimates to be propagated to other states.

States on list tagged OPEN Sorted by key function k.

State tag t(X) : NEW : has no estimate h. OPEN : estimate needs to be propagated.CLOSED : estimate propagated.

Page 15: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Fundamental Search Conceptsk(G,X) : key function minimum of

h(G,X) before modification, andall values assumed by h(G,X) since X was placed on the OPEN list.

Lowered state : k(G,X) = current h(G,X), Propagate decrease to descendants and other nodes.

Raised state : k(G,X) < current h(G,X), Propagate increase to dscendants and other nodes.Try to find alternate shorter paths.

Page 16: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Running D* First Time on Graph

InitiallyMark G Open and QueueMark all other states NewRun Process_States on queue until path found or empty.

When edge cost c(X,Y) changes If X is marked Closed, then

Update h(X)Mark X open and queue with key h(X).

Page 17: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

States initially tagged NEW (no cost determined yet).

Page 18: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

OPEN

NEW

NEW

OPEN List1 (0,G)

h = 0

8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Add Goal node to the OPEN list.Process OPEN list until the robot’s current state is CLOSED.

Page 19: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Process_State: New or Lowered State

Remove from Open list , state X with lowest k If X is a new/lowered state, its path cost is optimal!Then propagate to each neighbor Y

If Y is New, give it an initial path cost and propagate.If Y is a descendant of X, propagate any change.Else, if X can lower Y’s path cost, Then do so and propagate.

Page 20: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

OPEN

NEW

NEW

OPEN List1 (0,G)

h = 0

8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Add new neighbors of G onto the OPEN listCreate backpointers to G.

Page 21: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 1

h = 1

h = 1

h = 0

h = 1

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

OPEN

NEW

NEW

OPEN

CLOSED

OPEN

NEW

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)

8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Add new neighbors of G onto the OPEN listCreate backpointers to G.

Page 22: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 2

h = 1

h = 2

h = 1

h = 0

h = 1

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

NEW

OPEN

OPEN

NEW

OPEN

CLOSED

CLOSED

OPEN

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)3 (1,L) (1,O) (2,F) (2,H)

8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Add new neighbors of K on to the OPEN list and create backpointers.

Page 23: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 2

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

NEW

NEW

NEW

NEW

NEW

OPEN

NEW

NEW

OPEN

CLOSED

OPEN

NEW

CLOSED

CLOSED

CLOSED

OPEN

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)3 (1,L) (1,O) (2,F) (2,H)4 (2,F) (2,H) (2,I) (2,N)8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Add new neighbors of L, then O on to the OPEN list and create backpointers.

Page 24: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 2

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

NEW

NEW

NEW

NEW

NEW

OPEN

NEW

NEW

OPEN

CLOSED

OPEN

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)3 (1,L) (1,O) (2,F) (2,H)4 (2,F) (2,H) (2,I) (2,N)5 (2,H) (2,I) (2,N) (3,C)

Continue until current state S is closed.

Page 25: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 3

h = 2

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

NEW

NEW

NEW

NEW

NEW

OPEN

OPEN

NEW

OPEN

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)3 (1,L) (1,O) (2,F) (2,H)4 (2,F) (2,H) (2,I) (2,N)5 (2,H) (2,I) (2,N) (3,C)6 (2,I) (2,N) (3,C) (3,D)

Continue until current state S is closed.

Page 26: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 3

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

NEW

OPEN

NEW

NEW

OPEN

CLOSED

OPEN

NEW

OPEN

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)3 (1,L) (1,O) (2,F) (2,H)4 (2,F) (2,H) (2,I) (2,N)5 (2,H) (2,I) (2,N) (3,C)6 (2,I) (2,N) (3,C) (3,D)7 (2,N) (3,C) (3,D) (3,E) (3,M)

Continue until current state S is closed.

Page 27: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 3

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

NEW

OPEN

NEW

NEW

OPEN

CLOSED

OPEN

NEW

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)3 (1,L) (1,O) (2,F) (2,H)4 (2,F) (2,H) (2,I) (2,N)5 (2,H) (2,I) (2,N) (3,C)6 (2,I) (2,N) (3,C) (3,D)7 (2,N) (3,C) (3,D) (3,E) (3,M)8 (3,C) (3,D) (3,E) (3,M)

Continue until current state S is closed.

Page 28: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 3

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

NEW

OPEN

NEW

NEW

OPEN

CLOSED

OPEN

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)3 (1,L) (1,O) (2,F) (2,H)4 (2,F) (2,H) (2,I) (2,N)5 (2,H) (2,I) (2,N) (3,C)6 (2,I) (2,N) (3,C) (3,D)7 (2,N) (3,C) (3,D) (3,E) (3,M)8 (3,C) (3,D) (3,E) (3,M)9 (3,D) (3,E) (3,M) (4,A)

Continue until current state S is closed.

Page 29: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 4

h = 3

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

NEW

OPEN

OPEN

NEW

OPEN

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (0,G)2 (1,K) (1,L) (1,O)3 (1,L) (1,O) (2,F) (2,H)4 (2,F) (2,H) (2,I) (2,N)5 (2,H) (2,I) (2,N) (3,C)6 (2,I) (2,N) (3,C) (3,D)7 (2,N) (3,C) (3,D) (3,E) (3,M)8 (3,C) (3,D) (3,E) (3,M)9 (3,D) (3,E) (3,M) (4,A)10 (3,E) (3,M) (4,A) (4,B)Continue until current state S is closed..

Page 30: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

OPEN

CLOSED

OPEN

NEW

OPEN

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List10 (3,E) (3,M) (4,A) (4,B)11 (3,M) (4,A) (4,B) (4,J)121314

Continue until current state S is closed.

Page 31: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C F

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

OPEN

CLOSED

OPEN

NEW

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List10 (3,E) (3,M) (4,A) (4,B)11 (3,M) (4,A) (4,B) (4,J)12 (3,M) (4,A) (4,B)1314

Continue until current state S is closed.

Page 32: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

OPEN

CLOSED

OPEN

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List10 (3,E) (3,M) (4,A) (4,B)11 (3,M) (4,A) (4,B) (4,J)12 (3,M) (4,A) (4,B)13 (4,B) (4,J) (5,S)1415

Continue until current state S is closed.

Page 33: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

OPEN

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List10 (3,E) (3,M) (4,A) (4,B)11 (3,M) (4,A) (4,B) (4,J)12 (3,M) (4,A) (4,B)13 (4,B) (4,J) (5,S)14 (4,J) (5,S)15

Continue until current state S is closed.

Page 34: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Use D* to Compute Initial Path

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List10 (3,E) (3,M) (4,A) (4,B)11 (3,M) (4,A) (4,B) (4,J)12 (3,M) (4,A) (4,B)13 (4,B) (4,J) (5,S)14 (4,J) (5,S)15 (5,S)

Continue until current state S is closed.

Page 35: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Completed Initial Path

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List10 (3,E) (3,M) (4,A) (4,B)11 (3,M) (4,A) (4,B) (4,J)12 (3,M) (4,A) (4,B)13 (4,B) (4,J) (5,S)14 (4,J) (5,S)15 (5,S)16 NULL

Done: Current state S is closed, and Open list is empty.

Page 36: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Begin Executing Optimal Path

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 2

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

Robot moves along backpointers towards goalUses sensors to detect discrepancies along way.

Page 37: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Obstacle Encountered!

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

At state A, robot discovers edge D to H is blocked off (cost 5,000 units).Update map and reinvoke D*

Page 38: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Running D* After Edge Cost Change

When edge cost c(Y,X) changes If X is marked Closed, then

Update h(X)Mark X open and queue, key is new h(X).

Run Process_State on queue until path to current state is shown optimal, or queue Open List is empty.

Page 39: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From First Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,H)234

Function: Modify-Cost(X,Y,eval)1: c(X,Y) = eval2: if t(X) = CLOSED

then Insert(X,h(X))3: return Get-Kmin()

Propagate changes starting at H

Page 40: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From First Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 5002

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,H)2 (3,D)34

8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Raise cost of H’s descendant D, and propagate.

Page 41: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Process_State: Raised State

If X is a raise state its cost might be suboptimal.Try reducing cost of X using an optimal neighbor Y.

h(Y) ≤ [h(X) before it was raised]propagate X’s cost to each neighbor Y

If Y is New, Then give it an initial path cost and propagate.If Y is a descendant of X, Then propagate ANY change.If X can lower Y’s path cost,

Postpone: Queue X to propagate when optimal (reach current h(X))If Y can lower X’s path cost, and Y is suboptimal,

Postpone: Queue Y to propagate when optimal (reach current h(Y)).Postponement avoids creating cycles.

Page 42: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From First Obstacle

h =

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,H)2 (3,D)34

OPEN

4: if kold < h(X) then5: for each neighbor Y of X:6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then7: b(X) = Y; h(X) = h(Y) + c(Y,X);

D may not be optimal, check neighbors for better path.Transitioning to I is better, and I’s path is optimal, so update D.

Page 43: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From First Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List12 (3,D)3 NULL4

All neighbors of D have consistent h-values.No further propagation needed.

Page 44: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Continue Path Execution

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List12 (3,D)3 NULL4

A’s path optimal.Continue moving robot along backpointers.

Page 45: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Second Obstacle!

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

OPEN List1 (2,F) (2,H)234

Function: Modify-Cost(X,Y,eval)1: c(X,Y) = eval2: if t(X) = CLOSED

then Insert(X,h(X))3: return Get-Kmin()

At C robot discovers blocked edges C to F and H (cost 5,000 units).Update map and reinvoke D* until H(current position optimal).

Page 46: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From Second Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 5002

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,F) (2,H)2 (3,C)34

8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Processing F raises descendant C’s cost, and propagates.Processing H does nothing.

Page 47: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From Second Obstacle

h = 4

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 3

h = 2

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

Closed

CLOSED

CLOSED

CLOSED

Open

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,F) (2,H)2 (3,C)34

h = 5

4: if kold < h(X) then5: for each neighbor Y of X:6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then7: b(X) = Y; h(X) = h(Y) + c(Y,X);

C may be suboptimal, check neighbors; Better path through A!However, A may be suboptimal, and updating creates a loop!

Page 48: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From Second Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 3

h = 2

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,F) (2,H)2 (3,C)3 (4,A)4

h = 5002h = 5003

15: for each neighbor Y of X:16: if t(Y) = NEW or17: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) then18: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Don’t change C’s path to A (yet).Instead, propagate increase to A.

Page 49: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Process_State: Raised State

If X is a raise state its cost might be suboptimal.Try reducing cost of X using an optimal neighbor Y.

h(Y) ≤ [h(X) before it was raised]propagate X’s cost to each neighbor Y

If Y is New, Then give it an initial path cost and propagate.If Y is a descendant of X, Then propagate ANY change.If X can lower Y’s path cost,

Postpone: Queue X to propagate when optimal (reach current h(X))If Y can lower X’s path cost, and Y is suboptimal,

Postpone: Queue Y to propagate when optimal (reach current h(Y)).Postponement avoids creating cycles.

Page 50: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From Second Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 5003

h = 3

h = 2

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,F) (2,H)2 (3,C)3 (4,A)4

4: if kold < h(X) then5: for each neighbor Y of X:6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then7: b(X) = Y; h(X) = h(Y) + c(Y,X);

h = 5002

A may not be optimal, check neighbors for better path.Transitioning to D is better, and D’s path is optimal, so update A.

Page 51: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From Second Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,F) (2,H)2 (3,C)3 (4,A)4

4: if kold < h(X) then5: for each neighbor Y of X:6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then7: b(X) = Y; h(X) = h(Y) + c(Y,X);

h = 5002

A may not be optimal, check neighbors for better path.Transitioning to D is better, and D’s path is optimal, so update A.

Page 52: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From Second Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,F) (2,H)2 (3,C)3 (4,A)4 (5,C)

for each neighbor Y of X:17: if (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) then18: b(Y) = X; Insert(Y,h(X) + c(X,Y))19: else20: if b(Y) ≠ X and h(Y) > h(X) + c(X,Y) then21: Insert(X,h(X))

h = 5

A can improve neighbor C, so queue C.

Page 53: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Process_State: New or Lowered State

Remove from Open list , state X with lowest k If X is a new/lowered state its path cost is optimal,Then propagate to each neighbor Y

If Y is New, give it an initial path cost and propagate.If Y is a descendant of X, propagate any change.Else, if X can lower Y’s path cost, Then do so and propagate.

Page 54: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Update From Second Obstacle

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 3

h = 2

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

OPEN

CLOSED

CLOSED

CLOSED

CLOSED

OPEN List1 (2,F) (2,H)2 (3,C)3 (4,A)4 (5,C)5

C lowered to optimal; no neighbors affected. Current state reached, so Process_State terminates.

h = 4 h = 5

8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))

Bug? How does C getpointed to A?

Page 55: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Complete Path Execution

h = 5

J M N O

E I L G

B D H K

S A C Fh = 5

h = 4

h = 3

h = 4

h = 4

h = 3

h = 2

h = 3

h = 2

h = 1

h = 1

h = 2

h = 1

h = 0

h = 1

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

CLOSED

Follow back pointers to Goal.No further discrepancies detected; goal achieved!

Page 56: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

D* Pseudo CodeFunction: Process-State()1: X = Min-State()2: if X=NULL then return -13: kold = Get-Kmin(); Delete(X)4: if kold < h(X) then5: for each neighbor Y of X:6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then7: b(X) = Y; h(X) = h(Y) + c(Y,X);8: if kold = h(X) then9: for each neighbor Y of X:10: if t(Y) = NEW or11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then13: b(Y) = X; Insert(Y,h(X) + c(X,Y))14: else15: for each neighbor Y of X:16: if t(Y) = NEW or17: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) then18: b(Y) = X; Insert(Y,h(X) + c(X,Y))19: else20: if b(Y) ≠ X and h(X) > h(X) + c(X,Y) then21: Insert(X,h(X))22: else23: if b(Y) ≠ X and h(X) > h(Y) + c(Y,X) and24: t(Y) = CLOSED and h(Y) > kold then25: Insert(Y,h(Y))26: return Get-Kmin()

Function: Modify-Cost(X,Y,eval)1: c(X,Y) = eval2: if t(X) = CLOSED

then Insert(X,h(X))3: return Get-Kmin()

Function: Insert(X, hnew)1: if t(X) = NEW

then k(x) = hnew2: else if t(X) = OPEN

then k(X) = min(k(X), hnew)3: else

k(X) = min(h(X), hnew)4: h(X) = hnew;5: t(X) = OPEN

Page 57: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Outline

Review: Optimal Path Planning in Partially Known Environments.Continuous Optimal Path Planning

Dynamic A*Incremental A* (LRTA*)

Page 58: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Incremental A* On Gridworld[Koenig and Likhachev, 01]

Page 59: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Incremental A* Versus Other Searches

Page 60: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Incremental A* Versus Other Searches

Page 61: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Definitions: Graph and CostsSucc(s) : the set of successors of vertex s.Pred(s) : the set of predecessors of vertex s.c(s,s’) : the cost of moving from vertex s to vertex s’.

g*(s) : true start distance, from sstart to vertex s.g(s) : estimate of the start distance of vertex s.h(s) : heuristic estimate of goal distance of vertex s.

Page 62: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Definitions: Local Consistency & Updaterhs(s) : one-step lookahead estimate for g(s)

if s = sstart then 0 else mins’∈Pred(s)(g(s’) + c(s’,s))

locally consistent : g-value of a vertex equals it’s rhs value.Do nothing.

overconsistent : g-value of a vertex is greater than rhs value.Relax: set g-value to rhs value.

underconsistent : g-value of a vertex is less than rhs value.Upperbound: Set g-value to inf

Page 63: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Incremental A* in Brief1) Initialize start node with g estimates of 0 and enqueue.

All other g estimates (g,rhs values) are initialized to INF. Only the start node is locally inconsistent.

2) Take node with smallest key from priority queue. Update g-value, and add all successors to the priority queue.

3) Loop step 2 until either the goal is locally consistent or the priority becomes empty.

4) To find shortest path: Start with s’ = goal, working to start,next s’ = predecessor (s) of s’ with best g(s) + c(s, s’),

5) Wait for edge costs to change. Update edge costs and add edge’s destination node to the queue.

6) Go to step 2.

Page 64: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Definitions: Incremental A* Search QueueU : priority queue, contains all

locally inconsistent vertices.

U ordered by key k(s)k(s) : [k1(s), k2(s)]k1(s) : min(g(s), rhs(s)) + h(s)k2(s) : min(g(s), rhs(s))

k1(s) corresponds directly to f(s) of A*. First time through IA*, min(g(s), rhs(s)) = true g*(s).

k2(s) breaks ties, using minimum start distance.

Page 65: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(S) = ∞rhs(A) = ∞ g(A) = ∞rhs(B) = ∞ g(B) = ∞rhs(C) = ∞ g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue, (X, [k1(s);k2(s)])

1 (S, [0;0])

0

C

S

B

G

A

D

∞2

61

5

2

3

4

2

∞∞

∞∞

0

3

1

1

0

2

Initialize all values and add start to queue. Only start is locally inconsistent .

Page 66: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = ∞ g(A) = ∞rhs(B) = ∞ g(B) = ∞rhs(C) = ∞ g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue1 (S, [0;0])

0

C

S

B

G

A

D

∞2

61

5

2

3

4

2

∞∞

∞∞

0

0

3

1

1

0

2

Check local consistency of smallest node in queue,and update g-value.

Page 67: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = ∞rhs(B) = ∞ g(B) = ∞rhs(C) = ∞ g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21 (S, [0;0])

0

C

S

B

G

A

D

∞2

61

5

2

3

4

2

∞∞

∞∞

2

0

0

3

1

1

0

2

Update successor nodes.

Page 68: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = ∞rhs(B) = ∞ g(B) = ∞rhs(C) = ∞ g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2])(S, [0;0])

0

C

S

B

G

A

D

∞2

61

5

2

3

4

2

∞∞

∞∞

2

0

0

3

1

1

0

2

Add changed successor node A to priority queue.

Page 69: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = ∞rhs(B) = 6 g(B) = ∞rhs(C) = ∞ g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2])(S, [0;0])

0

C

S

B

G

A

D

∞2

61

5

2

3

4

2

∞6

∞∞

2

0

0

3

1

1

0

2

Update change to successor node B.

Page 70: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = ∞rhs(B) = 6 g(B) = ∞rhs(C) = ∞ g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

0

C

S

B

G

A

D

∞2

61

5

2

3

4

2

∞6

∞∞

2

0

0

3

1

1

0

2

Add successor node B to priority queue.

Page 71: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 6 g(B) = ∞rhs(C) = ∞ g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

0

C

S

B

G

A

D

2

∞2

61

5

2

3

4

2

∞6

∞∞

2

0

0

3

1

1

0

2

Check local consistency of smallest key, A, in the priority queue.

Page 72: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 6 g(B) = ∞rhs(C) = 4 g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (B, [9;6])

0

C

S

B

G

A

D

2

∞2

61

5

2

3

4

2

∞6

∞∞

4

2

0

0

3

1

1

0

2

Update change to successor node C.

Page 73: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 6 g(B) = ∞rhs(C) = 4 g(C) = ∞rhs(D) = ∞ g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (C, [5;4]) (B, [9;6])

0

C

S

B

G

A

D

2

∞2

61

5

2

3

4

2

∞6

∞∞

4

2

0

0

3

1

1

0

2

Add successor C to priority queue.

Page 74: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 6 g(B) = ∞rhs(C) = 4 g(C) = ∞rhs(D) = 6 g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (C, [5;4]) (B, [9;6])

0

C

S

B

G

A

D

2

∞2

61

5

2

3

4

2

∞6

6∞

4

2

0

0

3

1

1

0

2

Update change to successor node D.

Page 75: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 6 g(B) = ∞rhs(C) = 4 g(C) = ∞rhs(D) = 6 g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (C, [5;4]) (D, [7;6]) (B, [9;6])

0

C

S

B

G

A

D

2

∞2

61

5

2

3

4

2

∞6

6∞

4

2

0

0

3

1

1

0

2

Add successor D to priority queue.

Page 76: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 6 g(B) = ∞rhs(C) = 4 g(C) = 4rhs(D) = 6 g(D) = ∞rhs(G) = ∞ g(G) = ∞

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (C, [5;4]) (D, [7;6]) (B, [9;6])4 (D, [7;6]) (B, [9;6])

0

C

S

B

G

A

D

2

4

∞2

61

5

2

3

4

2

∞6

6∞

4

2

0

0

3

1

1

0

2

Check local consistency of smallest key, C, in queue.

Node C has no successors, so nothing is added to the queue.

Page 77: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

0

C

S

B

G

A

D

2

4

∞2

61

5

2

3

4

2

6

∞6

68

4

2

0

0

3

1

1

0

2

First Call To Incremental A* Search

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 6 g(B) = ∞rhs(C) = 4 g(C) = 4rhs(D) = 6 g(D) = 6rhs(G) = 8 g(G) = ∞

(G, [8;8]) (B, [9;6])5

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (C, [5;4]) (D, [7;6]) (B, [9;6])4 (D, [7;6]) (B, [9;6])

1

Check local consistency of smallest key, D.

Update successor G, and add to queue.

Page 78: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

g(G) = 8rhs(G) = 8g(D) = 6rhs(D) = 6g(C) = 4rhs(C) = 4g(B) = ∞rhs(B) = 6g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

(G, [8;8]) (B, [9;6])5

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (C, [5;4]) (D, [7;6]) (B, [9;6])4 (D, [7;6]) (B, [9;6])

0

C

S

B

G

A

D

2

4

82

61

5

2

3

4

2

6

∞6

68

4

2

0

0

3

1

1

0

2

Check local consistency of smallest key, G.

Page 79: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

g(G) = 8rhs(G) = 8g(D) = 6rhs(D) = 6g(C) = 4rhs(C) = 4g(B) = ∞rhs(B) = 6g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

(G, [8;8]) (B, [9;6])5

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (C, [5;4]) (D, [7;6]) (B, [9;6])4 (D, [7;6]) (B, [9;6])

0

C

S

B

G

A

D

2

4

82

61

5

2

3

4

2

6

∞6

68

4

2

0

0

3

1

1

0

2

The goal is locally consistent, hence search terminates.

Page 80: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

First Call To Incremental A* Search

g(G) = 8rhs(G) = 8g(D) = 6rhs(D) = 6g(C) = 4rhs(C) = 4g(B) = ∞rhs(B) = 5g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

(G, [8;8]) (B, [9;6])5

U - Priority Queue

21

(A, [4;2]) (B, [9;6])(S, [0;0])

3 (C, [5;4]) (D, [7;6]) (B, [9;6])4 (D, [7;6]) (B, [9;6])

2

6 1 5

2

3

4

2

0

C

S

B

G

A

D

2

4

8

6

∞6

68

4

2

0

0

3

1

1

0

2

The search does not necessarily make all vertices locally consistent, only those expanded.

Page 81: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

0rhs(B) = 6

First Call To Incremental A* Search

g(G) = 6rhs(G) = 6g(D) = 6rhs(D) = 6g(C) = 4rhs(C) = 4g(B) = ∞g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

U - Priority Queue1

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

8

6

6

68

4

2

0

0

3

1

1

0

2

(B, [9;6])

For next search, we keep rhs and g-values so that we know which nodes not to traverse again.

Page 82: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

06rhs(B) = 6

Incremental A* After Edge Decrease

g(G) = 6rhs(G) = 6g(D) = 6rhs(D) = 6g(C) = 4rhs(C) = 4g(B) = ∞g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

U - Priority Queue1

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

8

6

6

68

4

2

0

0

3

1

1

0

2

(B, [9;6])

We discover that the cost of edge S-B decreases to 0.

Page 83: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(B) = 0

Incremental A* After Edge Decrease

g(G) = 6rhs(G) = 6g(D) = 6rhs(D) = 6g(C) = 4rhs(C) = 4g(B) = ∞g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

U - Priority Queue1 (B, [3;0])

0

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

8

6

∞0

68

4

2

0

0

3

1

1

0

2

The affected nodes, B in this case, are updated and added to the queue.

Page 84: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

g(G) = 6rhs(G) = 6g(D) = 6rhs(D) = 6g(C) = 4rhs(C) = 4g(B) = 0rhs(B) = 0g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

U - Priority Queue1 (B, [3;0])

Incremental A* After Edge Decrease

0

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

8

6

00

68

4

2

0

0

3

1

1

0

2

Check local consistency of smallest key, B, in queue.

Page 85: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 0 g(B) = 0rhs(C) = 4 g(C) = 4rhs(D) = 1 g(D) = 6rhs(G) = 6 g(G) = 6

U - Priority Queue

21 (B, [3;0])

(D, [2;1])

Incremental A* After Edge Decrease

0

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

8

6

00

18

4

2

0

0

3

1

1

0

2

Update rhs value of successor node D and add to queue.

Page 86: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 0 g(B) = 0rhs(C) = 4 g(C) = 4rhs(D) = 1 g(D) = 6rhs(G) = 5 g(G) = 6

U - Priority Queue

21

(D, [2;1])(B, [3;0])(D, [2;1]) (G, [5;5])

Incremental A* After Edge Decrease

0

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

8

6

00

15

4

2

0

0

3

1

1

0

2

Update rhs value of successor node G and add to queue.

Page 87: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 0 g(B) = 0rhs(C) = 4 g(C) = 4rhs(D) = 1 g(D) = 1rhs(G) = 5 g(G) = 6

U - Priority Queue

21

(D, [2;1]) (G, [5;5])(B, [3;0])

3 (G, [3;3])

Incremental A* After Edge Decrease

0

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

8

1

00

15

4

2

0

0

3

1

1

0

2

Check local consistency of D, and expand successors.

Page 88: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 0 g(B) = 0rhs(C) = 4 g(C) = 4rhs(D) = 1 g(D) = 1rhs(G) = 5 g(G) = 5

U - Priority Queue

21

(D, [2;1]) (G, [5;5])(B, [3;0])

3 (G, [3;3])

Incremental A* After Edge Decrease

0

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

51

00

15

4

2

0

0

3

1

1

0

2

Check local consistency of smallest key, G, in queue.

Page 89: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 0 g(B) = 0rhs(C) = 4 g(C) = 4rhs(D) = 1 g(D) = 1rhs(G) = 5 g(G) = 5

U - Priority Queue

21

(D, [2;1]) (G, [5;5])(B, [3;0])

3 (G [3;3])

Incremental A* After Edge Decrease

0

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

5

1

00

15

4

2

0

0

3

1

1

0

2

Goal is locally consistent, thus finished.At the end of search, if g(sgoal) = ∞, then there is no path from sstart to sgoal.

Page 90: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

40

rhs(B) = 0

Incremental A* After Edge Increase

g(G) = 5rhs(G) = 5g(D) = 1rhs(D) = 1g(C) = 4rhs(C) = 4g(B) = 0g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

U - Priority Queue1

2

15

2

3

4

2

0

C

S

B

G

A

D

2

4

5

1

0

15

4

2

0

0

3

1

1

0

2

0

We discover that the cost of edge B-D increases to 4.

Page 91: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(B) = 0

rhs(D) = 1

Incremental A* After Edge Increase

g(G) = 5rhs(G) = 5g(D) = 1rhs(D) = 4g(C) = 4rhs(C) = 4g(B) = 0g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

U - Priority Queue1 (D, [2;1])

0

2

45

2

3

4

2

0

C

S

B

G

A

D

2

4

5

1

00

45

4

2

0

0

3

1

1

0

2

The affected node, D, is updated and added to the queue.

Page 92: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

g(G) = 5rhs(G) = 5g(D) = 1rhs(D) = 4g(C) = 4rhs(C) = 4g(B) = 0rhs(B) = 0g(A) = 2rhs(A) = 2g(s) = 0rhs(S) = 0

U - Priority Queue1 (D, [2;1])

Incremental A* After Edge Increase

0

2

45

2

3

4

2

0

C

S

B

G

A

D

2

4

5

1

00

45

4

2

0

0

3

1

1

0

2

Check local consistency of smallest key, D, in queue.

Page 93: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 0 g(B) = 0rhs(C) = 4 g(C) = 4rhs(D) = 4 g(D) = INFrhs(G) = 5 g(G) = 5

U - Priority Queue

21 (D, [2;1])

(D, [5;4])

Incremental A* After Edge Increase

0

2

45

2

3

4

2

0

C

S

B

G

A

D

2

4

5

inf

00

45

4

2

0

0

3

1

1

0

2

Update g value of node D and add to queue.

Page 94: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 0 g(B) = 0rhs(C) = 4 g(C) = 4rhs(D) = 4 g(D) = infrhs(G) = 5 g(G) = 5

U - Priority Queue

21

(D, [5;4])(B, [2;1])

Incremental A* After Edge Increase

0

2

45

2

3

4

2

0

C

S

B

G

A

D

2

4

5

inf

00

45

4

2

0

0

3

1

1

0

2

rhs values of successor nodes C and G are consistent, don’t add to queue.

Page 95: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

rhs(S) = 0 g(s) = 0rhs(A) = 2 g(A) = 2rhs(B) = 0 g(B) = 0rhs(C) = 4 g(C) = 4rhs(D) = 4rhs(G) = 5 g(G) = 5

U - Priority Queue

21

(D, [5;4])(B, [2;1])

3

Incremental A* After Edge Increase

0

2

45

2

3

4

2

0

C

S

B

G

A

D

2

4

5

4

00

45

4

2

0

0

3

1

1

0

2

g(G) = 4

Check local consistency of D, and update; successors consistent.

Page 96: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Incremental A* Pseudo Codeprocedure Initialize(){02} U := ∅{03} for all s∈S rhs(s) = g(s) = ∞{04} rhs(sstart) = 0;{05} U.Insert(sstart, [h(sstart); 0]);

Procedure Main(){17} Initialize();{18} forever{19} ComputerShortestPath();{20} Wait for changes in edge costs;{21} for all directed edges (u,v) with changed costs{22} Update the edge cost c(u,v); {23} UpdateVertex(v);

Page 97: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Incremental A* Pseudo Codeprocedure ComputeShortestPath(){09} while (U.TopKey()<CalculateKey(sgoal)

OR rhs(sgoal) ≠ g(sgoal)){10} u = U.Pop();{11} if (g(u) > rhs(u)){12} g(u) = rhs(u);{13} for all s ∈ Succ(u) UpdateVertex(s){14} else{15} g(u) = ∞{16} for all s ∈ Succ(u) ∪ {u} UpdateVertex(s)

procedure UpdateVertex(u){06} if (u ≠ sstart)rhs(u)= min s’∈Pred(u)(g(s’)+ c(s’,u)){07} if (u ∈ U) U.Remove(u);{08} if (g(u) ≠ rhs(u)) U.Insert(u, CalculateKey(u));

procedure CalculateKey(s){01} return [min(g(s), rhs(s)) + h(s); min(g(s), rhs(s))];

Page 98: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Continuous Optimal Planning

1. Generate global path plan from initial map.

2. Repeat until Goal reached, or failure.Execute next step of current global path plan.Update map based on sensor information.Incrementally update global path plan from map changes.

1 to 3 orders of magnitude speedup relative to a non-incremental path planner.

Page 99: Incremental Path Planningdspace.mit.edu/bitstream/handle/1721.1/36832/16... · Incremental Path Planning Dynamic and Incremental A* Prof. Brian Williams (help from Ihsiang Shu) 16.412/6.834

Recap: Dynamic & Incremental A*

Supports search as a repetitive online process.Exploits similarities between a series of searches to solve much faster than solving each search starting from scratch.Reuses the identical parts of the previous search tree, while updating differences.Solutions guaranteed to be optimal.On the first search, behaves like traditional algorithms.

D* behaves exactly like Dijkstra’s.Incremental A* A* behaves exactly like A*.