Incremental
Path Planning
Dynamic and Incremental A*
Prof. Brian Williams
(help from Ihsiang Shu)
16.412/6.834 Cognitive Robotics
February 17
th
, 2004
Outline
? Review: Optimal Path Planning in
Partially Known Environments.
? Continuous Optimal Path Planning
?Dynamic A*
?Incremental A* (LRTA*)
[Zellinsky, 92]
1. Generate global path plan from initial map.
2. Repeat until goal reached or failure:
? Execute next step in current global path plan
? Update map based on sensors.
? If map changed generate new global path
from map.
Compute Optimal Path
JMNO
EILG
BDHK
SACF
Begin Executing Optimal Path
JMNO
EILG
BDHK
SACF
h = 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.
Obstacle Encountered!
JMNO
EILG
BDHK
SACF
h = 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.
Continue Path Execution
JMNO
EILG
BDHK
SACF
h = 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.
Second Obstacle, Replan!
JMNO
EILG
BDHK
SACF
h = 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.
Path Execution Achieves Goal
h = 5
JMNO
EILG
BDHK
SACF
h = 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!
Outline
? Review: Optimal Path Planning in
Partially Known Environments.
? Continuous Optimal Path Planning
?Dynamic A*
?Incremental A* (LRTA*)
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*.
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.
Map and Path Concepts
? c(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.
D* Search Concepts
? OPEN 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.
D* Fundamental Search Concepts
? k(G,X) : key function
minimum of
? h(G,X) before modification, and
? all 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.
Running D* First Time on Graph
Initially
? Mark G Open and Queue
? Mark all other states New
? Run 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).
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
States initially tagged NEW (no cost determined yet).
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
OPEN
NEW
NEW
OPEN List
1 (0,G)
h = 0
8: if kold = h(X) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: 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.
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.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
NEW
OPEN
NEW
NEW
OPEN List
1 (0,G)
h = 0
8: if kold = h(X) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: b(Y) = X; Insert(Y,h(X) + c(X,Y))
? Add new neighbors of G onto the OPEN list
? Create backpointers to G.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(0,G)
2 (1,K) (1,L) (1,O)
8: if kold = h(X) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: b(Y) = X; Insert(Y,h(X) + c(X,Y))
? Add new neighbors of G onto the OPEN list
? Create backpointers to G.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(0,G)
2 (1,K) (1,L) (1,O)
3 (1,L) (1,O) (2,F) (2,H)
8: if kold = h(X) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: b(Y) = X; Insert(Y,h(X) + c(X,Y))
? Add new neighbors of K on to the OPEN list and create backpointers.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(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) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: 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.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(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.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(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.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(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.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(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.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(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.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
1(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..
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
10 (3,E) (3,M) (4,A) (4,B)
11 (3,M) (4,A) (4,B) (4,J)
12
13
14
? Continue until current state S is closed.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
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 List
10 (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
14
? Continue until current state S is closed.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
h = 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 List
10 (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
15
? Continue until current state S is closed.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
h = 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 List
10 (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.
Use D* to Compute Initial Path
JMNO
EILG
BDHK
SACF
h = 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 List
10 (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.
D* Completed Initial Path
JMNO
EILG
BDHK
SACF
h = 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 List
10 (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.
Begin Executing Optimal Path
JMNO
EILG
BDHK
SACF
h = 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 goal
? Uses sensors to detect discrepancies along way.
Obstacle Encountered!
JMNO
EILG
BDHK
SACF
h = 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*
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.
D* Update From First Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1(2,H)
2
3
4
Function: Modify-Cost(X,Y,eval)
1: c(X,Y) = eval
2: if t(X) = CLOSED
then Insert(X,h(X))
3: return Get-Kmin()
? Propagate changes starting at H
D* Update From First Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1(2,H)
2(3,D)
3
4
8: if kold = h(X) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: b(Y) = X; Insert(Y,h(X) + c(X,Y))
? Raise cost of H’s descendant D, and propagate.
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.
D* Update From First Obstacle
h =
JMNO
EILG
BD
HK
SACF
h = 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 List
1(2,H)
2(3,D)
3
4
OPEN
4: if kold < h(X) then
5: for each neighbor Y of X:
6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then
7: 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.
D* Update From First Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1
2(3,D)
3 NULL
4
? All neighbors of D have consistent h-values.
? No further propagation needed.
Continue Path Execution
JMNO
EILG
BDHK
SACF
h = 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 List
1
2(3,D)
3 NULL
4
? A’s path optimal.
? Continue moving robot along backpointers.
Second Obstacle!
JMNO
EILG
BDHK
SACF
h = 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 List
1(2,F) (2,H)
2
3
4
Function: Modify-Cost(X,Y,eval)
1: c(X,Y) = eval
2: 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).
D* Update From Second Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1(2,F) (2,H)
2(3,C)
3
4
8: if kold = h(X) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: b(Y) = X; Insert(Y,h(X) + c(X,Y))
? Processing F raises descendant C’s cost, and propagates.
? Processing H does nothing.
D* Update From Second Obstacle
h = 4
JMNO
EILG
BDHK
SACF
h = 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 List
1(2,F) (2,H)
2(3,C)
3
4
h = 5
4: if kold < h(X) then
5: for each neighbor Y of X:
6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then
7: 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!
D* Update From Second Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1(2,F) (2,H)
2(3,C)
3(4,A)
4
h = 5002
h = 5003
15: for each neighbor Y of X:
16: if t(Y) = NEW or
17: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y))
then
18: b(Y) = X; Insert(Y,h(X) + c(X,Y))
? Don’t change C’s path to A (yet).
? Instead, propagate increase to A.
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.
D* Update From Second Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1(2,F) (2,H)
2(3,C)
3(4,A)
4
4: if kold < h(X) then
5: for each neighbor Y of X:
6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then
7: 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.
D* Update From Second Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1(2,F) (2,H)
2(3,C)
3(4,A)
4
4: if kold < h(X) then
5: for each neighbor Y of X:
6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then
7: 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.
D* Update From Second Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1(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)) then
18: b(Y) = X; Insert(Y,h(X) + c(X,Y))
19: else
20: if b(Y) ≠ X and h(Y) > h(X) + c(X,Y) then
21: Insert(X,h(X))
h = 5
? A can improve neighbor C, so queue C.
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.
D* Update From Second Obstacle
JMNO
EILG
BDHK
SACF
h = 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 List
1(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) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: b(Y) = X; Insert(Y,h(X) + c(X,Y))
Bug? How does C get
pointed to A?
Complete Path Execution
h = 5
JMNO
EILG
BDHK
SACF
h = 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!
D* Pseudo Code
Function: Process-State()
1: X = Min-State()
2: if X=NULL then return -1
3: kold = Get-Kmin(); Delete(X)
4: if kold < h(X) then
5: for each neighbor Y of X:
6: if h(Y) ≤ kold and h(X) > h(Y) + C(Y,X) then
7: b(X) = Y; h(X) = h(Y) + c(Y,X);
8: if kold = h(X) then
9: for each neighbor Y of X:
10: if t(Y) = NEW or
11: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) or
12: (b(Y) ≠ X and h(Y) > h(X) + c(X,Y)) then
13: b(Y) = X; Insert(Y,h(X) + c(X,Y))
14: else
15: for each neighbor Y of X:
16: if t(Y) = NEW or
17: (b(Y) = X and h(Y) ≠ h(X) + c(X,Y)) then
18: b(Y) = X; Insert(Y,h(X) + c(X,Y))
19: else
20: if b(Y) ≠ X and h(X) > h(X) + c(X,Y) then
21: Insert(X,h(X))
22: else
23: if b(Y) ≠ X and h(X) > h(Y) + c(Y,X) and
24: t(Y) = CLOSED and h(Y) > kold then
25: Insert(Y,h(Y))
26: return Get-Kmin()
Function: Modify-Cost(X,Y,eval)
1: c(X,Y) = eval
2: if t(X) = CLOSED
then Insert(X,h(X))
3: return Get-Kmin()
Function: Insert(X, h
new
)
1: if t(X) = NEW
then k(x) = h
new
2: else if t(X) = OPEN
then k(X) = min(k(X), h
new
)
3: else
k(X) = min(h(X), h
new
)
4: h(X) = h
new
;
5: t(X) = OPEN
Outline
? Review: Optimal Path Planning in
Partially Known Environments.
? Continuous Optimal Path Planning
?Dynamic A*
?Incremental A* (LRTA*)
Incremental A* On Gridworld
[Koenig and Likhachev, 01]
Incremental A* Versus Other Searches
Incremental A* Versus Other Searches
Definitions: Graph and Costs
Succ(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 s
start
to vertex s.
g(s) : estimate of the start distance of vertex s.
h(s) : heuristic estimate of goal distance of vertex s.
Definitions: Local Consistency & Update
rhs(s) : one-step lookahead estimate for g(s)
if s = s
start
then 0
else min
s’∈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
Incremental A* in Brief
1) 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.
Definitions: Incremental A* Search Queue
U : priority queue, contains all
locally inconsistent vertices.
U ordered by key k(s)
k(s) : [k
1
(s), k
2
(s)]
k
1
(s) : min(g(s), rhs(s)) + h(s)
k
2
(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.
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
6
1
5
2
3
4
2
∞
∞
∞
∞
∞
∞
∞
∞
0
3
1
1
0
2
Initialize all values and add start to queue.
Only start is locally inconsistent .
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = ∞ g(A) = ∞
rhs(B) = ∞ g(B) = ∞
rhs(C) = ∞ g(C) = ∞
rhs(D) = ∞ g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
1 (S, [0;0])
0
C
S
B
G
A
D
∞
∞
∞
2
6
1
5
2
3
4
2
∞
∞
∞
∞
∞
∞
∞
0
0
3
1
1
0
2
Check local consistency of smallest node in queue,
and update g-value.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = ∞
rhs(B) = ∞ g(B) = ∞
rhs(C) = ∞ g(C) = ∞
rhs(D) = ∞ g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1 (S, [0;0])
0
C
S
B
G
A
D
∞
∞
∞
2
6
1
5
2
3
4
2
∞
∞
∞
∞
∞
∞
2
0
0
3
1
1
0
2
Update successor nodes.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = ∞
rhs(B) = ∞ g(B) = ∞
rhs(C) = ∞ g(C) = ∞
rhs(D) = ∞ g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(A, [4;2])
(S, [0;0])
0
C
S
B
G
A
D
∞
∞
∞
2
6
1
5
2
3
4
2
∞
∞
∞
∞
∞
∞
2
0
0
3
1
1
0
2
Add changed successor node A to priority queue.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = ∞
rhs(B) = 6 g(B) = ∞
rhs(C) = ∞ g(C) = ∞
rhs(D) = ∞ g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(A, [4;2])
(S, [0;0])
0
C
S
B
G
A
D
∞
∞
∞
2
6
1
5
2
3
4
2
∞
∞
6
∞
∞
∞
2
0
0
3
1
1
0
2
Update change to successor node B.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = ∞
rhs(B) = 6 g(B) = ∞
rhs(C) = ∞ g(C) = ∞
rhs(D) = ∞ g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(A, [4;2]) (B, [9;6])
(S, [0;0])
0
C
S
B
G
A
D
∞
∞
∞
2
6
1
5
2
3
4
2
∞
∞
6
∞
∞
∞
2
0
0
3
1
1
0
2
Add successor node B to priority queue.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 6 g(B) = ∞
rhs(C) = ∞ g(C) = ∞
rhs(D) = ∞ g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(A, [4;2]) (B, [9;6])
(S, [0;0])
0
C
S
B
G
A
D
2
∞
∞
2
6
1
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.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 6 g(B) = ∞
rhs(C) = 4 g(C) = ∞
rhs(D) = ∞ g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(A, [4;2]) (B, [9;6])
(S, [0;0])
3 (B, [9;6])
0
C
S
B
G
A
D
2
∞
∞
2
6
1
5
2
3
4
2
∞
∞
6
∞
∞
4
2
0
0
3
1
1
0
2
Update change to successor node C.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 6 g(B) = ∞
rhs(C) = 4 g(C) = ∞
rhs(D) = ∞ g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(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
6
1
5
2
3
4
2
∞
∞
6
∞
∞
4
2
0
0
3
1
1
0
2
Add successor C to priority queue.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 6 g(B) = ∞
rhs(C) = 4 g(C) = ∞
rhs(D) = 6 g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(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
6
1
5
2
3
4
2
∞
∞
6
6
∞
4
2
0
0
3
1
1
0
2
Update change to successor node D.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 6 g(B) = ∞
rhs(C) = 4 g(C) = ∞
rhs(D) = 6 g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(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
6
1
5
2
3
4
2
∞
∞
6
6
∞
4
2
0
0
3
1
1
0
2
Add successor D to priority queue.
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 6 g(B) = ∞
rhs(C) = 4 g(C) = 4
rhs(D) = 6 g(D) = ∞
rhs(G) = ∞ g(G) = ∞
U - Priority Queue
2
1
(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
6
1
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.
0
C
S
B
G
A
D
2
4
∞
2
6
1
5
2
3
4
2
6
∞
6
6
8
4
2
0
0
3
1
1
0
2
First Call To Incremental A* Search
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 6 g(B) = ∞
rhs(C) = 4 g(C) = 4
rhs(D) = 6 g(D) = 6
rhs(G) = 8 g(G) = ∞
(G, [8;8]) (B, [9;6])5
U - Priority Queue
2
1
(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])
Check local consistency of smallest key, D.
Update successor G, and add to queue.
First Call To Incremental A* Search
g(G) = 8rhs(G) = 8
g(D) = 6rhs(D) = 6
g(C) = 4rhs(C) = 4
g(B) = ∞rhs(B) = 6
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
(G, [8;8]) (B, [9;6])5
U - Priority Queue
2
1
(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
8
2
6
1
5
2
3
4
2
6
∞
6
6
8
4
2
0
0
3
1
1
0
2
Check local consistency of smallest key, G.
First Call To Incremental A* Search
g(G) = 8rhs(G) = 8
g(D) = 6rhs(D) = 6
g(C) = 4rhs(C) = 4
g(B) = ∞rhs(B) = 6
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
(G, [8;8]) (B, [9;6])5
U - Priority Queue
2
1
(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
8
2
6
1
5
2
3
4
2
6
∞
6
6
8
4
2
0
0
3
1
1
0
2
The goal is locally consistent, hence search terminates.
First Call To Incremental A* Search
g(G) = 8rhs(G) = 8
g(D) = 6rhs(D) = 6
g(C) = 4rhs(C) = 4
g(B) = ∞rhs(B) = 5
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
(G, [8;8]) (B, [9;6])5
U - Priority Queue
2
1
(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
6
8
4
2
0
0
3
1
1
0
2
The search does not necessarily make all vertices locally consistent,
only those expanded.
0
rhs(B) = 6
First Call To Incremental A* Search
g(G) = 6rhs(G) = 6
g(D) = 6rhs(D) = 6
g(C) = 4
rhs(C) = 4
g(B) = ∞
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
U - Priority Queue
1
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
8
6
6
6
8
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.
06
rhs(B) = 6
Incremental A* After Edge Decrease
g(G) = 6rhs(G) = 6
g(D) = 6rhs(D) = 6
g(C) = 4
rhs(C) = 4
g(B) = ∞
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
U - Priority Queue
1
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
8
6
6
6
8
4
2
0
0
3
1
1
0
2
(B, [9;6])
∞
We discover that the cost of edge S-B decreases to 0.
rhs(B) = 0
Incremental A* After Edge Decrease
g(G) = 6rhs(G) = 6
g(D) = 6rhs(D) = 6
g(C) = 4
rhs(C) = 4
g(B) = ∞
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
U - Priority Queue
1
(B, [3;0])
0
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
8
6
∞
0
6
8
4
2
0
0
3
1
1
0
2
The affected nodes, B in this case, are updated and added to the queue.
g(G) = 6rhs(G) = 6
g(D) = 6rhs(D) = 6
g(C) = 4rhs(C) = 4
g(B) = 0rhs(B) = 0
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
U - Priority Queue
1 (B, [3;0])
Incremental A* After Edge Decrease
0
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
8
6
0
0
6
8
4
2
0
0
3
1
1
0
2
Check local consistency of smallest key, B, in queue.
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 0 g(B) = 0
rhs(C) = 4 g(C) = 4
rhs(D) = 1 g(D) = 6
rhs(G) = 6 g(G) = 6
U - Priority Queue
2
1 (B, [3;0])
(D, [2;1])
Incremental A* After Edge Decrease
0
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
8
6
0
0
1
8
4
2
0
0
3
1
1
0
2
Update rhs value of successor node D and add to queue.
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 0 g(B) = 0
rhs(C) = 4 g(C) = 4
rhs(D) = 1 g(D) = 6
rhs(G) = 5 g(G) = 6
U - Priority Queue
2
1
(D, [2;1])
(B, [3;0])
(D, [2;1]) (G, [5;5])
Incremental A* After Edge Decrease
0
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
8
6
0
0
1
5
4
2
0
0
3
1
1
0
2
Update rhs value of successor node G and add to queue.
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 0 g(B) = 0
rhs(C) = 4 g(C) = 4
rhs(D) = 1 g(D) = 1
rhs(G) = 5 g(G) = 6
U - Priority Queue
2
1
(D, [2;1]) (G, [5;5])
(B, [3;0])
3 (G, [3;3])
Incremental A* After Edge Decrease
0
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
8
1
0
0
1
5
4
2
0
0
3
1
1
0
2
Check local consistency of D, and expand successors.
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 0 g(B) = 0
rhs(C) = 4 g(C) = 4
rhs(D) = 1 g(D) = 1
rhs(G) = 5 g(G) = 5
U - Priority Queue
2
1
(D, [2;1]) (G, [5;5])
(B, [3;0])
3 (G, [3;3])
Incremental A* After Edge Decrease
0
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
5
1
0
0
1
5
4
2
0
0
3
1
1
0
2
Check local consistency of smallest key, G, in queue.
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 0 g(B) = 0
rhs(C) = 4 g(C) = 4
rhs(D) = 1 g(D) = 1
rhs(G) = 5 g(G) = 5
U - Priority Queue
2
1
(D, [2;1]) (G, [5;5])
(B, [3;0])
3 (G [3;3])
Incremental A* After Edge Decrease
0
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
5
1
0
0
1
5
4
2
0
0
3
1
1
0
2
Goal is locally consistent, thus finished.
At the end of search, if g(s
goal
) = ∞, then there is no path from s
start
to s
goal
.
4
0
rhs(B) = 0
Incremental A* After Edge Increase
g(G) = 5rhs(G) = 5
g(D) = 1rhs(D) = 1
g(C) = 4
rhs(C) = 4
g(B) = 0
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
U - Priority Queue
1
2
1
5
2
3
4
2
0
C
S
B
G
A
D
2
4
5
1
0
1
5
4
2
0
0
3
1
1
0
2
0
We discover that the cost of edge B-D increases to 4.
rhs(B) = 0
rhs(D) = 1
Incremental A* After Edge Increase
g(G) = 5rhs(G) = 5
g(D) = 1rhs(D) = 4
g(C) = 4
rhs(C) = 4
g(B) = 0
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
U - Priority Queue
1
(D, [2;1])
0
2
4
5
2
3
4
2
0
C
S
B
G
A
D
2
4
5
1
0
0
4
5
4
2
0
0
3
1
1
0
2
The affected node, D, is updated and added to the queue.
g(G) = 5rhs(G) = 5
g(D) = 1rhs(D) = 4
g(C) = 4rhs(C) = 4
g(B) = 0rhs(B) = 0
g(A) = 2rhs(A) = 2
g(s) = 0rhs(S) = 0
U - Priority Queue
1 (D, [2;1])
Incremental A* After Edge Increase
0
2
4
5
2
3
4
2
0
C
S
B
G
A
D
2
4
5
1
0
0
4
5
4
2
0
0
3
1
1
0
2
Check local consistency of smallest key, D, in queue.
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 0 g(B) = 0
rhs(C) = 4 g(C) = 4
rhs(D) = 4 g(D) = INF
rhs(G) = 5 g(G) = 5
U - Priority Queue
2
1 (D, [2;1])
(D, [5;4])
Incremental A* After Edge Increase
0
2
4
5
2
3
4
2
0
C
S
B
G
A
D
2
4
5
inf
0
0
4
5
4
2
0
0
3
1
1
0
2
Update g value of node D and add to queue.
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 0 g(B) = 0
rhs(C) = 4 g(C) = 4
rhs(D) = 4 g(D) = inf
rhs(G) = 5 g(G) = 5
U - Priority Queue
2
1
(D, [5;4])
(B, [2;1])
Incremental A* After Edge Increase
0
2
4
5
2
3
4
2
0
C
S
B
G
A
D
2
4
5
inf
0
0
4
5
4
2
0
0
3
1
1
0
2
rhs values of successor nodes C and G are consistent,
don’t add to queue.
rhs(S) = 0 g(s) = 0
rhs(A) = 2 g(A) = 2
rhs(B) = 0 g(B) = 0
rhs(C) = 4 g(C) = 4
rhs(D) = 4
rhs(G) = 5 g(G) = 5
U - Priority Queue
2
1
(D, [5;4])
(B, [2;1])
3
Incremental A* After Edge Increase
0
2
4
5
2
3
4
2
0
C
S
B
G
A
D
2
4
5
4
0
0
4
5
4
2
0
0
3
1
1
0
2
g(G) = 4
Check local consistency of D, and update; successors consistent.
Incremental A* Pseudo Code
procedure Initialize()
{02} U := ?
{03} for all s∈S rhs(s) = g(s) = ∞
{04} rhs(s
start
) = 0;
{05} U.Insert(s
start
, [h(s
start
); 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);
Incremental A* Pseudo Code
procedure ComputeShortestPath()
{09} while (U.TopKey()<CalculateKey(sgoal)
OR rhs(s
goal
) ≠ g(s
goal
))
{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))];
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.
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*.