Outline
Incremental
Path Planning
Optimal Path Planning in
Partially Known Environments.
� Continuous Optimal Path Planning
�
�Dynamic
A*
�Incremental
Continuous Planning and Dynamic A*
A* (LRTA*) [Appendix]
Prof. Brian Williams
(help from Ihsiang Shu)
16.412/6.834 Cognitive Robotics
March 16 th, 2004
[Zellinsky, 92]
Compute Optimal Path
1.
Generate global path plan from initial map.
J
M
N
O
2.
Repeat until goal reached or failure:
E
I
L
G
B
D
H
K
S
A
C
F
�
�
�
Execute next step in current global path plan
Update map based on sensors.
If map changed generate new global path
from map.
Begin Executing Optimal Path
�
�
Obstacle Encountered!
h=4
h=3
h=2
h=1
h=4
h=3
h=1
J
M
N
O
J
M
N
O
h=3
h=2
h=1
h=0
h=3
h=2
h=1
h=0
h=1
E
I
L
G
E
I
L
G
h=4
h=3
h=2
h=1
h=4
h=3
h=2
h=1
B
D
H
K
B
D
H
K
h=5
h=4
h=3
h=2
h=5
h=4
h=3
h=2
S
A
C
F
S
A
C
F
Robot moves along backpointers towards goal.
Uses sensors to detect discrepancies along way.
�
�
At state A, robot discovers edge from D to H is blocked (cost 5,000 units).
Update map and reinvoke planner.
1
Continue Path Execution
�
�
h=4
h=3
h=1
h=1
h=4
h=3
h=1
J
M
N
O
J
M
N
O
h=3
h=2
h=1
h=0
h=3
h=2
h=1
h=0
E
I
L
G
E
I
L
G
h=4
h=3
h=2
h=1
h=4
h=3
h=2
h=1
B
D
H
K
B
D
H
K
h=5
h=4
h=3
h=2
h=5
h=4
h=3
h=2
S
A
C
F
S
A
C
F
A’s previous path is still optimal.
Continue moving robot along back pointers.
Path Execution Achieves Goal
�
�
h=4
h=3
h=1
J
M
N
h=3
h=2
h=1
h=1
O
h=0
E
I
L
G
h=4
h=3
h=2
h=1
B
D
H
h=5
h=4
h=5
S
A
C
�
�
�
�
�
�
h=1
At C robot discovers blocked edges from C to F and H (cost 5,000 units).
Update map and reinvoke planner.
Outline
Optimal Path Planning in
Partially Known Environments.
� Continuous Optimal Path Planning
�
�Dynamic
K
A*
�Incremental
A* (LRTA*) [Appendix]
h=2
F
Follow back pointers to goal.
No further discrepancies detected; goal achieved!
What is Continuous
Optimal Path Planning?
�
Second Obstacle, Replan!
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.
2.
Generate global path plan from initial map.
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.
2
Map and Path Concepts
�
D* Search Concepts
�
c(X,Y) :
Cost to move from Y to X.
c(X,Y) is undefined if move disallowed.
�
Neighbors(X) :
�
o(G,X) :
�
h(G,X) :
State tag t(X) :
� NEW
:
has no estimate h.
estimate needs to be propagated.
estimate propagated.
� OPEN
:
� CLOSED :
Any Y such that c(X,Y) or c(Y,X) is defined.
�
True optimal path cost to Goal from X.
States with estimates to be propagated to other states.
� States on list tagged OPEN
� Sorted by key function k (defined below).
Estimate of optimal path cost to goal from X.
�
OPEN list :
b(X) = Y : backpointer from X to Y.
Y is the first state on path from X to G.
D* Fundamental Search Concepts
�
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
�
Running D* First Time on Graph
k(G,X) : key function
decrease to descendants and other nodes.
Initially
� Mark G Open and Queue it
� 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
�
�
�
Raised state : k(G,X) < current h(G,X),
Update h(X )
Mark X open and queue with key h(X ).
� Propagate
� Try
increase to dscendants and other nodes.
to find alternate shorter paths.
Use D* to Compute Initial Path
J
NEW
Use D* to Compute Initial Path
M
N
O
NEW
NEW
NEW
NEW
M
N
O
NEW
NEW
NEW
G
E
NEW
I
NEW
L
NEW
J
OPEN List
1 (0,G)
h=0
E
NEW
I
NEW
L
NEW
NEW
B
D
H
K
NEW
NEW
NEW
NEW
S
A
C
F
NEW
NEW
NEW
NEW
States initially tagged NEW (no cost determined yet).
�
�
G
OPEN
B
D
H
K
NEW
NEW
NEW
NEW
S
A
C
F
NEW
NEW
NEW
NEW
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.
3
Process_State: New or Lowered State
�
�
Use D* to Compute Initial Path
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
J
NEW
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.
�
�
h=1
NEW
J
M
N
O
NEW
NEW
OPEN
h=1
h=0
E
I
NEW
L
OPEN
G
CLOSED
NEW
h=1
�
�
B
D
H
K
NEW
NEW
NEW
OPEN
S
A
C
F
NEW
NEW
NEW
NEW
h =2
J
NEW
E
NEW
h=1
M
N
O
NEW
OPEN
CLOSED
h=2
h=1
h=0
I
OPEN
L
CLOSED
G
CLOSED
h=2
h=1
B
D
H
K
NEW
NEW
OPEN
CLOSED
h=2
�
�
S
A
C
F
NEW
NEW
NEW
OPEN
1
2
3
4
L
NEW
OPEN List
1 (0,G)
G
OPEN
B
D
H
K
NEW
NEW
NEW
S
A
C
F
NEW
NEW
NEW
NEW
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.
h=1
NEW
M
N
O
NEW
NEW
OPEN
h=1
h=0
E
NEW
I
NEW
L
OPEN
CLOSED
h =2
h=1
G
B
D
H
K
NEW
NEW
OPEN
CLOSED
S
A
C
F
NEW
NEW
NEW
OPEN
h=2
�
�
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
Create backpointers .
Use D* to Compute Initial Path
h=2
OPEN List
(0,G)
(1,K) (1,L) (1,O)
(1,L) (1,O) (2,F) (2,H)
(2,F) (2,H) (2,I) (2,N)
J
NEW
E
NEW
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
Create backpointers .
I
NEW
NEW
J
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 ))
Use D* to Compute Initial Path
O
NEW
Use D* to Compute Initial Path
OPEN List
1 (0,G)
2 (1,K) (1,L) (1,O)
Add new neighbors of G onto the OPEN list
Create backpointers to G.
N
NEW
h=0
E
NEW
� If
Use D* to Compute Initial Path
M
NEW
N
O
OPEN
CLOSED
h=2
h=1
h=0
I
OPEN
L
CLOSED
CLOSED
h=2
h=1
G
B
D
H
K
NEW
NEW
OPEN
CLOSED
h =3
�
h=1
M
NEW
1
2
3
4
OPEN List
(0,G)
(1,K) (1,L) (1,O)
(1,L) (1,O) (2,F) (2,H)
(2,F) (2,H) (2,I) (2,N)
5 (2,H) (2,I) (2,N) (3,C)
h=2
S
A
C
F
NEW
NEW
OPEN
CLOSED
Continue until current state S is closed.
4
Use D* to Compute Initial Path
h=2
J
NEW
N
O
OPEN
CLOSED
h=2
E
NEW
I
OPEN
h =3
�
h=1
M
NEW
h=1
L
CLOSED
h=2
h=0
G
CLOSED
h=1
B
D
H
K
NEW
OPEN
CLOSED
CLOSED
h=3
h=2
S
A
C
NEW
NEW
OPEN
1
2
3
4
L
h=2
h=0
G
CLOSED
h=1
K
CLOSED
h=3
h=2
F
S
A
C
F
CLOSED
NEW
NEW
OPEN
CLOSED
�
h=1
O
h=3
h=2
h=1
h=0
E
I
L
G
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
B
D
H
K
NEW
OPEN
CLOSED
CLOSED
h=3
h=2
S
A
C
F
NEW
NEW
OPEN
CLOSED
1
2
3
4
5
6
7
8
M
N
O
OPEN
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
OPEN
I
CLOSED
L
CLOSED
G
CLOSED
h =4
h=3
h=2
h=1
B
D
H
K
OPEN
CLOSED
CLOSED
CLOSED
h=2
S
A
C
F
NEW
OPEN
CLOSED
CLOSED
h=3
1
2
3
4
5
6
7
8
9
Continue until current state S is closed.. 10
OPEN List
(0,G)
(1,K) (1,L) (1,O)
(1,L) (1,O) (2,F) (2,H)
(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)
h=1
M
N
O
OPEN
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
I
CLOSED
L
CLOSED
CLOSED
h=3
h=2
h=1
G
B
D
H
K
NEW
OPEN
CLOSED
CLOSED
h =4
h=3
h=2
S
A
C
F
NEW
OPEN
CLOSED
CLOSED
1
2
3
4
OPEN List
(0,G)
(1,K) (1,L) (1,O)
(1,L) (1,O) (2,F) (2,H)
(2,F) (2,H) (2,I) (2,N)
5
6
7
8
(2,H) (2,I) (2,N) (3,C)
(2,I) (2,N) (3,C) (3,D)
(2,N) (3,C) (3,D) (3,E) (3,M)
(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
h =4
J
h=3
h=2
h=1
OPEN
M
N
O
OPEN
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h =4
h=3
h=2
h=1
(2,H) (2,I) (2,N) (3,C)
(2,I) (2,N) (3,C) (3,D)
(2,N) (3,C) (3,D) (3,E) (3,M)
(3,C) (3,D) (3,E) (3,M)
(3,D) (3,E) (3,M) (4,A)
(3,E) (3,M) (4,A) (4,B)
h=2
NEW
OPEN
�
h=1
NEW
OPEN List
(0,G)
(1,K) (1,L) (1,O)
(1,L) (1,O) (2,F) (2,H)
(2,F) (2,H) (2,I) (2,N)
Continue until current state S is closed.
J
(2,H) (2,I) (2,N) (3,C)
(2,I) (2,N) (3,C) (3,D)
(2,N) (3,C) (3,D) (3,E) (3,M)
(3,C) (3,D) (3,E) (3,M)
Continue until current state S is closed.
1
2
3
4
Use D* to Compute Initial Path
OPEN List
(0,G)
(1,K) (1,L) (1,O)
(1,L) (1,O) (2,F) (2,H)
(2,F) (2,H) (2,I) (2,N)
Use D* to Compute Initial Path
�
h=3
h=1
CLOSED
H
CLOSED
h=3
I
CLOSED
N
h=4
h=2
CLOSED
D
CLOSED
h=2
O
CLOSED
OPEN
M
h=3
E
h=1
N
OPEN
B
OPEN
J
h=3
OPEN
h=2
M
OPEN
NEW
NEW
OPEN
�
h=2
J
NEW
5 (2,H) (2,I) (2,N) (3,C)
6 (2,I) (2,N) (3,C) (3,D)
Use D* to Compute Initial Path
h=3
h =3
OPEN List
(0,G)
(1,K) (1,L) (1,O)
(1,L) (1,O) (2,F) (2,H)
(2,F) (2,H) (2,I) (2,N)
Continue until current state S is closed.
J
Use D* to Compute Initial Path
B
D
H
K
OPEN
CLOSED
CLOSED
CLOSED
h=4
�
G
h=3
OPEN List
10 (3,E) (3,M) (4,A) (4,B)
11 (3,M) (4,A) (4,B) (4,J)
12
13
14
h=2
S
A
C
F
NEW
OPEN
CLOSED
CLOSED
Continue until current state S is closed.
5
Use D* to Compute Initial Path
h =4
J
OPEN
h=3
E
h=3
h=2
h=1
M
N
O
CLOSED
CLOSED
CLOSED
h=2
I
h=1
L
h=0
G
CLOSED
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
h=1
�
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
h=4
J
OPEN
h=3
E
h=3
h=2
h=1
M
N
O
CLOSED
CLOSED
CLOSED
h=2
I
h=1
L
h=0
G
CLOSED
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
h=1
14
B
D
H
K
B
D
H
K
OPEN
CLOSED
CLOSED
CLOSED
OPEN
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
h=5
h=4
h=3
h=2
S
A
C
F
S
A
C
F
NEW
OPEN
CLOSED
CLOSED
OPEN
CLOSED
CLOSED
CLOSED
Continue until current state S is closed.
�
Use D* to Compute Initial Path
h=4
J
h=3
h=2
h=1
OPEN
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
G
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
�
Use D* to Compute Initial Path
h=4
h=3
10
11
12
13
OPEN List
(3,E) (3,M) (4,A) (4,B)
(3,M) (4,A) (4,B) (4,J)
(3,M) (4,A) (4,B)
(4,B) (4,J) (5,S)
14 (4,J) (5,S)
15
h=4
J
h=3
h=2
h=1
CLOSED
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h=4
h=3
h=2
h=1
G
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h=2
h=4
h=3
C
F
S
A
C
F
CLOSED
CLOSED
OPEN
CLOSED
CLOSED
CLOSED
�
h=4
J
h=3
h=2
h=1
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
E
I
L
h=0
G
CLOSED
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h=4
h=3
h=2
10
11
12
13
OPEN List
(3,E) (3,M) (4,A) (4,B)
(3,M) (4,A) (4,B) (4,J)
(3,M) (4,A) (4,B)
(4,B) (4,J) (5,S)
14 (4,J) (5,S)
15 (5,S)
16 NULL
14 (4,J) (5,S)
15 (5,S)
Begin Executing Optimal Path
h=4
J
h=3
h=2
h=1
CLOSED
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
E
I
L
h=0
G
CLOSED
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h=4
h=3
h=2
S
A
C
F
S
A
C
F
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
Done: Current state S is closed, and Open list is empty.
OPEN List
(3,E) (3,M) (4,A) (4,B)
(3,M) (4,A) (4,B) (4,J)
(3,M) (4,A) (4,B)
(4,B) (4,J) (5,S)
Continue until current state S is closed.
CLOSED
�
10
11
12
13
h=2
A
CLOSED
CLOSED
(4,B) (4,J) (5,S)
14
15
Use D* to Compute Initial Path
S
D* Completed Initial Path
OPEN List
(3,E) (3,M) (4,A) (4,B)
(3,M) (4,A) (4,B) (4,J)
(3,M) (4,A) (4,B)
Continue until current state S is closed.
OPEN
Continue until current state S is closed.
10
11
12
13
�
�
Robot moves along backpointers towards goal
Uses sensors to detect discrepancies along way.
6
Obstacle Encountered!
h=4
h=3
J
CLOSED
h=3
h=1
N
O
CLOSED
CLOSED
h=2
E
h=1
M
CLOSED
I
h=1
L
G
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
OPEN
CLOSED
h=5
h=4
h=3
h=2
S
A
C
F
CLOSED
CLOSED
CLOSED
CLOSED
�
When edge cost c(X,Y) changes
� If X is marked Closed, then
h=0
CLOSED
�
�Update h(X)
�Mark
�
�or
h=4
h=3
J
h=1
h=1
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
G
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
OPEN
CLOSED
h=5
h=4
h=3
h=2
S
A
C
F
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()
Assign cost of 5,000 for D to H
Propagate changes starting at H
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.
�
Propagate X’s cost to each neighbor Y
�
path to current state is shown optimal,
queue Open List is empty.
At state A, robot discovers edge D to H is blocked off (cost 5,000 units).
Update map and reinvoke D*
CLOSED
�
X open and queue, key is new h(X).
Run Process_State on queue
�until
D* Update From First Obstacle
�
Running D* After Edge Cost Change
�
h=4
J
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))
avoids creating cycles.
h=1
N
O
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h=4
h = 5002
h=2
h=1
G
B
D
H
K
CLOSED
OPEN
CLOSED
CLOSED
h=5
h=4
h=3
h=2
S
A
C
F
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.
D* Update From First Obstacle
h=4
J
h=3
h=1
h=1
CLOSED
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h=4
h h= =
5002
3
h=2
h=1
B
CLOSED
h=5
D
OPEN
h=4
G
H
K
CLOSED
CLOSED
h=3
h=2
S
A
C
F
CLOSED
CLOSED
CLOSED
CLOSED
Postpone: Queue Y to propagate when optimal (reach current h(Y )).
� Postponement
h=1
M
CLOSED
If Y can lower X’s path cost, and Y is suboptimal,
�
h=3
CLOSED
h(Y ) = [h(X ) before it was raised]
�
�
D* Update From First Obstacle
�
�
OPEN List
1 (2,H)
2 (3,D)
3
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);
D may not be optimal, check neighbors for better path.
Transitioning to I is better, and I’s path is optimal, so update D.
7
D* Update From First Obstacle
h=4
J
CLOSED
h=3
E
CLOSED
h=4
h=3
h=1
h=1
M
N
O
CLOSED
CLOSED
CLOSED
h=2
I
CLOSED
h=3
h=1
L
CLOSED
h=2
h=0
G
CLOSED
OPEN List
1
2 (3,D)
3 NULL
4
Continue Path Execution
h=4
J
CLOSED
h=3
E
CLOSED
h=4
h=1
h=3
h=1
h=1
M
N
O
CLOSED
CLOSED
CLOSED
h=2
I
CLOSED
h=3
h=1
L
CLOSED
h=2
h=0
G
CLOSED
h=1
B
D
H
K
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h=4
h=3
h=2
h=5
h=4
h=3
h=2
S
A
C
F
S
A
C
F
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
CLOSED
�
�
All neighbors of D have consistent h-values.
No further propagation needed.
Second Obstacle!
h=4
J
h=3
h=1
h=1
CLOSED
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
G
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
OPEN
CLOSED
h=5
h=4
h=3
h=2
S
A
C
F
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()
D* Update From Second Obstacle
h=4
J
h=3
h=1
h=1
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
G
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h=4
h=
h 5002
=5
h=2
S
A
C
F
CLOSED
Closed
Open
CLOSED
�
�
�
A’s path optimal.
Continue moving robot along backpointers.
D* Update From Second Obstacle
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).
CLOSED
�
OPEN List
1
2 (3,D)
3 NULL
4
OPEN List
1 (2,F) (2,H)
2 (3,C)
3
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);
C may be suboptimal, check neighbors; � Better path through A!
However, A may be suboptimal, and updating would create a loop!
h=4
J
h=3
h=1
h=1
CLOSED
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h=4
h=3
h=2
h=1
G
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=4
h = 5002
h=5
h=2
S
A
C
F
CLOSED
CLOSED
OPEN
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
J
h=3
h=1
h=1
CLOSED
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h=4
h=3
h=2
h=1
G
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h = 5003
h = 5002
h=2
S
A
C
F
CLOSED
OPEN
CLOSED
CLOSED
�
�
OPEN List
1 (2,F) (2,H)
2 (3,C)
3 (4,A)
4
15: for each neighbor Y of X:
16: if t(Y) = NEW or
(b(Y) = X and h(Y) ? h(X) + c(X,Y))
17:
then
b(Y) = X; Insert(Y,h(X) + c(X,Y))
18:
Don’t change C’s path to A (yet).
Instead, propagate increase to A.
8
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.
�
propagate X’s cost to each neighbor Y
�
�
h=4
J
CLOSED
h(Y ) = [h(X ) before it was raised]
h=3
�
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,
�
�
D* Update From Second Obstacle
Postpone: Queue X to propagate when optimal (reach current h(X))
E
CLOSED
h=4
�
h=4
J
h=3
h=1
h=1
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h=4
h=3
h=2
h=1
G
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h =4
h = 5002
h=2
S
A
C
F
CLOSED
OPEN
CLOSED
CLOSED
�
�
OPEN List
1 (2,F) (2,H)
2 (3,C)
3 (4,A)
4
I
h=3
h=1
L
CLOSED
h=2
h=0
G
CLOSED
OPEN List
1 (2,F) (2,H)
2 (3,C)
3 (4,A)
4
h=1
H
K
CLOSED
CLOSED
h=5
h = 5003
h = 5002
h=2
S
A
C
F
CLOSED
OPEN
CLOSED
CLOSED
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 );
A may not be optimal, check neighbors for better path.
Transitioning to D is better, and D’s path is optimal, so update A.
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.
Y is a descendant of X, propagate any change.
� Else, if X can lower Y’s path cost,
Then do so and propagate.
� If
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 );
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
h=4
J
h=3
h=1
h=1
CLOSED
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
E
I
L
h=0
G
CLOSED
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h =4
h=5
h=2
S
A
C
F
CLOSED
CLOSED
OPEN
CLOSED
�
h=2
CLOSED
D
�
CLOSED
O
CLOSED
CLOSED
�
D* Update From Second Obstacle
h=1
N
CLOSED
B
Postpone: Queue Y to propagate when optimal (reach current h(Y )).
avoids creating cycles.
h=1
M
CLOSED
If Y can lower X’s path cost, and Y is suboptimal,
� Postponement
h=3
CLOSED
A can improve neighbor C, so queue C.
1
2
3
4
OPEN List
(2,F) (2,H)
(3,C)
(4,A)
(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 ))
D* Update From Second Obstacle
h=4
J
h=3
h=1
h=1
CLOSED
M
N
O
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
E
I
L
h=0
G
CLOSED
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
h=1
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h=4
h=5
h=2
S
A
C
F
CLOSED
CLOSED
OPEN
CLOSED
�
�
OPEN List
1
2
3
4
(2,F) (2,H)
(3,C)
(4,A)
(5,C)
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 ))
C lowered to optimal; no neighbors affected.
Current state reached, so Process_State terminates.
9
Complete Path Execution
h=4
J
h=3
M
h=1
N
CLOSED
CLOSED
CLOSED
h=3
h=2
h=1
E
CLOSED
h=4
I
CLOSED
h=3
L
CLOSED
h=2
O
CLOSED
h=0
G
CLOSED
h=1
B
D
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h=4
h=5
h=2
S
CLOSED
�
�
A
CLOSED
H
h=1
C
CLOSED
K
F
CLOSED
Follow back pointers to Goal.
No further discrepancies detected; goal achieved!
Recap: Continuous Optimal Planning
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:
if t(Y ) = NEW or
16:
( b(Y) = X and h(Y) ? h(X ) + c(X,Y)) then
17:
b(Y ) = X; Insert(Y,h(X ) + c(X,Y))
18:
19:
else
if b(Y) ? X and h(X) > h(X ) + c(X,Y ) then
20:
Insert(X,h(X))
21:
22:
else
if b(Y) ? X and h(X) > h(Y ) + c(Y,X ) and
23:
t(Y) = CLOSED and h(Y ) > kold then
24:
Insert(Y,h(Y))
25:
26: return Get -Kmin()
2.
�
Supports search as a repetitive online process.
Exploits similarities between a series of searches to
solve much faster than 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 Dijkstra.
Generate global path plan from initial map.
�
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.
Function: Insert(X , h new)
1: if t(X ) = NEW
then k(x ) = hnew
2: else if t(X) = OPEN
then k(X ) = min(k(X ), hnew)
3: else
k(X) = min(h(X ), h new)
4: h(X) = h new;
5: t(X) = OPEN
Recap: Dynamic A*
�
1.
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()
1 to 3 orders of magnitude speedup
relative to a non-incremental path planner.
10