Planning
Optimal Outline
Incremental
Path 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
G
E
I
L
G
h=2
h=1
h=4
h=3
h=2
h=1
D
H
K
B
D
H
K
h=5
h=4
h=3
h=2
h=5
h=4
h=3
h=2
S
L
h=3
B
I
h=4
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
Second Obstacle, Replan!
h=4
h=1
h=4
h=3
h=1
N
O
J
M
N
O
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
h=1
M
h=3
h=3
J
A
C
F
S
A
C
F
As 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
G
h=2
D
H
h=4
h=5
S
A
C
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?
h=1
h=5
L
h=3
B
I
h=4
h=1
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 Dijkstras .
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.
o(G,X) :
NEW
:
has no estimate h.
estimate needs to be propagated.
estimate propagated.
OPEN
:
CLOSED :
Neighbors(X) :
State tag t(X) :
h(G,X) :
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
N EW
Use D* to Compute Initial Path
M
N
O
NEW
NEW
NEW
N EW
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
NEW
NEW
B
NEW
States initially tagged NEW (no cost determined yet).
D
H
K
NEW
F
NEW
G
OPEN
NEW
NEW
N EW
S
A
C
F
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 ))
NEW
Add Goal node to the OPEN list.
Process OPEN list until the robots 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
N EW
M
N
O
NEW
NEW
NEW
h=0
E
NEW
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 Ys path cost,
Then do so and propagate.
I
NEW
L
NEW
G
OPEN
B
D
H
K
NEW
NEW
NEW
N EW
S
h=1
N EW
J
M
N
O
NEW
NEW
OPEN
h=1
h=0
E
I
NEW
L
OPEN
h=1
B
K
N EW
OPEN
A
C
F
NEW
H
NEW
S
D
NEW
NEW
NEW
h =2
O
CLOSED
h=1
h=0
I
OPEN
L
CLOSED
G
CLOSED
h=2
NEW
N
OPEN
h=2
E
h=1
B
D
H
K
NEW
N EW
OPEN
CLOSED
h=2
S
A
C
F
NEW
J
NEW
NEW
M
N
O
NEW
NEW
OPEN
h=1
h=0
I
NEW
L
OPEN
h=1
G
1
2
3
4
B
H
K
NEW
OPEN
CLOSED
S
A
C
F
NEW
N EW
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 ))
OPEN
h=2
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
h=1
NEW
N
O
OPEN
CLOSED
h=1
h=0
I
OPEN
L
CLOSED
CLOSED
h=2
E
M
NEW
h=2
N EW
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 ))
h=1
G
B
D
H
K
NEW
NEW
OPEN
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
1
2
3
4
CLOSED
h =3
OPEN
Add new neighbors of L, then O on to the OPEN list
Create backpointers .
D
NEW
OPEN List
1 (0,G)
2 (1,K) (1,L) (1,O)
3 (1,L) (1,O) (2,F) (2,H)
CLOSED
h =2
NEW
h=1
M
Add new neighbors of G onto the OPEN list
Create backpointers to G.
N EW
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
NEW
NEW
E
NEW
Add new neighbors of G onto the OPEN list
Create backpointers to G.
J
F
NEW
h=1
OPEN List
1 (0,G)
2 (1,K) (1,L) (1,O)
N EW
N EW
C
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 ))
Use D* to Compute Initial Path
G
CLOSED
NEW
A
NEW
Use D* to Compute Initial Path
OPEN List
1 (0,G)
NEW
OPEN
CLOSED
Continue until current state S is closed.
4
Use D* to Compute Initial Path
h=2
J
N EW
N
O
OPEN
CLOSED
h=2
E
NEW
h=1
M
NEW
I
OPEN
h =3
h=1
L
CLOSED
h=2
h=0
G
CLOSED
h=1
B
H
K
OPEN
CLOSED
CLOSED
h=3
A
C
NEW
N EW
OPEN
h=3
E
OPEN
h=3
h=2
O
CLOSED
h=2
I
CLOSED
h=3
K
h=3
h=2
F
S
A
C
NEW
NEW
OPEN
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)
6 (2,I) (2,N) (3,C) (3,D)
7 (2,N) (3,C) (3,D) (3,E) (3,M)
F
CLOSED
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
N EW
OPEN
CLOSED
CLOSED
h=3
h=2
S
A
C
F
NEW
NEW
OPEN
1
2
3
4
5
6
7
8
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)
J
N
O
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
N EW
OPEN
CLOSED
CLOSED
N
O
CLOSED
CLOSED
h=3
h=2
h=1
h=0
E
I
CLOSED
L
CLOSED
CLOSED
h=3
h=2
h=1
OPEN
G
B
D
H
K
NEW
OPEN
CLOSED
CLOSED
h =4
h=3
h=2
S
h=1
M
h=1
M
OPEN
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)
C
F
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)
A
NEW
Use D* to Compute Initial Path
OPEN
h=2
N EW
(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.
N EW
Continue until current state S is closed.
Use D* to Compute Initial Path
CLOSED
h=3
h=1
CLOSED
CLOSED
h=4
G
H
N
h=2
h=2
h=0
CLOSED
CLOSED
CLOSED
h=3
L
D
M
J
h=1
CLOSED
OPEN
OPEN
OPEN
h=1
N
OPEN
B
Continue until current state S is closed.
J
h=2
M
OPEN
N EW
N EW
J
N EW
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
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)
h=2
S
D
NEW
1
2
3
4
Use D* to Compute Initial Path
G
B
D
H
K
OPEN
CLOSED
CLOSED
14
CLOSED
h=4
h=3
h=2
S
A
C
F
NEW
OPEN List
10 (3,E) (3,M) (4,A) (4,B)
11 (3,M) (4,A) (4,B) (4,J)
12
13
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
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
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
S
A
C
F
S
A
C
(4,B) (4,J) (5,S)
14
15
F
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
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
Continue until current state S is closed.
Use D* to Compute Initial Path
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
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)
h=2
S
A
C
F
S
A
C
F
OPEN
OPEN List
(3,E) (3,M) (4,A) (4,B)
(3,M) (4,A) (4,B) (4,J)
(3,M) (4,A) (4,B)
h=2
NEW
10
11
12
13
CLOSED
CLOSED
CLOSED
OPEN
CLOSED
CLOSED
CLOSED
Continue until current state S is closed.
D* Completed Initial 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
10
11
12
13
OPEN (3,M) List
(3,E) (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
Continue until current state S is closed.
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
CLOSED
Done: Current state S is closed, and Open list is empty.
Robot moves along backpointers towards goal
Uses sensors to detect discrepancies along way.
6
Obstacle Encountered!
h=4
h=3
J
h=1
h=1
M
h=3
N
CLOSED
CLOSED
h=2
E
When edge cost c(X,Y) changes
If X is marked Closed, then
O
CLOSED
CLOSED
I
h=1
L
Running D* After Edge Cost Change
h=0
G
CLOSED
CLOSED
CLOSED
h=4
h=3
h=2
Update h(X)
CLOSED
h=1
B
D
H
CLOSED
OPEN
CLOSED
h=5
h=4
h=3
h=2
S
A
C
F
CLOSED
CLOSED
CLOSED
Mark
K
CLOSED
CLOSED
until
or
h=4
h=3
J
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
OPEN List
1 (2,H)
2
3
4
D* Update From First Obstacle
h=4
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
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 Xs 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 Ys path cost,
Postpone: Queue X to propagate when optimal (reach current h(X))
O
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h = 5002
h=2
avoids creating cycles.
OPEN List
1 (2,H)
2 (3,D)
3
4
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
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 ))
CLOSED
Raise cost of Hs 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 =002
=53
h=2
OPEN List
1 (2,H)
2 (3,D)
3
4
h=1
B
CLOSED
h=5
D
OPEN
h=4
G
H
K
CLOSED
CLOSED
h=3
h=2
S
A
C
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);
F
CLOSED
CLOSED
Postpone: Queue Y to propagate when optimal (reach current h(Y )).
Postponement
h=1
N
CLOSED
If Y can lower Xs path cost, and Y is suboptimal,
h=1
M
CLOSED
h(Y ) = [h(X ) before it was raised]
J
h=3
CLOSED
h=4
CLOSED
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,0 units).
00
Update map and reinvoke D*
D* Update From First Obstacle
X open and queue, key is new h(X).
Run Process_State on queue
D may not be optimal, check neighbors for better path.
Transitioning to I is better, and Is 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
OPEN List
1
2 (3,D)
3 NULL
4
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
D* Update From Second Obstacle
OPEN List
1 (2,F) (2,H)
2
3
4
h=4
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
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
B
D
H
K
CLOSED
CLOSED
CLOSED
h=5
h=4
h = 5002
h =5
h=2
S
A
C
F
CLOSED
Closed
Open
OPEN List
1 (2,F) (2,H)
2 (3,C)
3
4
O
CLOSED
h=3
h=2
h=1
h=0
E
CLOSED
I
CLOSED
L
CLOSED
CLOSED
h=3
h=2
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!
OPEN List
1 (2,F) (2,H)
2 (3,C)
3
4
h=1
G
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=4
h = 5002
h=5
h=2
S
A
C
F
CLOSED
OPEN
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 ))
CLOSED
Processing F raises descendant Cs 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
CLOSED
h=1
N
CLOSED
h=1
CLOSED
h=1
M
CLOSED
CLOSED
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
J
h=3
CLOSED
h=4
OPEN
As path optimal.
Continue moving robot along backpointers.
h=3
h=2
OPEN List
1 (2,F) (2,H)
2 (3,C)
3 (4,A)
4
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
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:
Dont change Cs 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 Xs 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 Ys 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
OPEN List
1 (2,F) (2,H)
2 (3,C)
3 (4,A)
4
B
D
H
K
CLOSED
CLOSED
CLOSED
CLOSED
h=5
h =4
h = 5002
h=2
S
A
C
F
CLOSED
OPEN
CLOSED
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
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 );
CLOSED
A may not be optimal, check neighbors for better path.
Transitioning to D is better, and Ds path is optimal, so update A.
Process_State: New or Lowered State
h=1
G
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 Xs path cost, and Y is suboptimal,
Postponement
h=3
CLOSED
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 Ys 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 );
CLOSED
A may not be optimal, check neighbors for better path.
Transitioning to D is better, and Ds 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
1
2
3
4
OPEN List
(2,F) (2,H)
(3,C)
(4,A)
(5,C)
D* Update From Second Obstacle
h=4
J
B
D
H
K
CLOSED
CLOSED
CLOSED
h=5
h =4
h=5
h=2
S
A
C
F
CLOSED
CLOSED
OPEN
CLOSED
A can improve neighbor C, so queue C.
h=1
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
h=1
CLOSED
h=3
CLOSED
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 ))
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