Improved Fast Replanning for Robot Navigation in Unknown Terrain

Sven Koenig Maxim Likhachev

College of Computing School of Computer Science
Georgia Institute of Technology Carnegie Mellon University
Atlanta, GA 30312-0280 Pittsburgh, PA 15213
[email protected] [email protected]

Abstract cost one) or untraversable, this navigation strategy must ter-

minate because the robot either follows the planned path to
Mobile robots often operate in domains that are only incom- the goal vertex or increases its knowledge about the true
pletely known, for example, when they have to move from given edge costs, which can happen only once for each edge.
start coordinates to given goal coordinates in unknown terrain. In
this case, they need to be able to replan quickly as their knowledge To implement the navigation strategy, the robot needs to
of the terrain changes. Stentz’ Focussed Dynamic A* is a heuristic replan a shortest path from its current vertex to the goal
search method that repeatedly determines a shortest path from the vertex whenever it detects that its current path is blocked.
current robot coordinates to the goal coordinates while the robot The robot could use conventional graph-search methods.
moves along the path. It is able to replan one to two orders of mag- However, the resulting search times can be on the order
nitudes faster than planning from scratch since it modifies previous of minutes for the large graphs that are often used, which
search results locally. Consequently, it has been extensively used adds up to substantial idle times [13]. Several solutions
in mobile robotics. In this paper, we introduce an alternative to to this problem have been proposed in the robotics liter-
Focussed Dynamic A* that implements the same navigation strat- ature [18, 1, 16, 11, 3, 6]. Focussed Dynamic A* (D*)
egy but is algorithmically different. Focussed Dynamic A* Lite is
[14] is probably the most popular solution at the moment
simpler, easier to understand, easier to analyze and easier to ex-
since it combines the efficiency of heuristic and incremental
tend than Focussed Dynamic A*, yet is more efficient. We believe
that our results will make D*-like replanning algorithms even more searches, yet still finds shortest paths. It achieves a speedup
popular and enable robotics researchers to adapt them to additional of one to two orders of magnitudes(!) over repeated A* [10]
applications. searches by modifying previous search results locally. D*
has been extensively used on real robots, including outdoor
HMMWVs [15], including UGV Demo II vehicles as part of
the DARPA Unmanned Ground Vehicle program. It is cur-
1 Introduction
rently also being integrated into Mars Rover prototypes and
Mobile robots often operate in domains that are only in- tactical mobile robot prototypes for urban reconnaissance
completely known. In this paper, we study a goal-directed [5, 9, 17]. D* is also used as part of other software, includ-
navigation problem in unknown terrain where a mobile ing the GRAMMPS mission planner for multiple robots [2].
robot has to move from its current coordinates to given goal However, D* is very complex and thus hard to under-
coordinates. A robot can solve it with the following naviga- stand, analyze, and extend. For example, while D* has been
tion strategy: It always plans a shortest path from its current widely used as a black-box method, it has not been extended
coordinates to the goal coordinates under the assumption by other researchers. Building on our Lifelong Planning A*
that unknown terrain is traversable. If it observes obstacles algorithm [7], we therefore present D* Lite, a novel replan-
as it follows this path, it enters them into its map and then ning method that implements the same navigation strategy
repeats the procedure, until it eventually reaches the goal as D* but is algorithmically different. Lifelong Planning A*
coordinates or all paths to them are blocked. If we model is an incremental version of A* and thus very similar to A*.
the navigation problem as a navigation problem on an eight- It is efficient and has well-understood properties (for exam-
connected grid with edges that are either traversable (with ple, we can prove theorems about its similarity to A* and its
 We thank Anthony Stentz for his support of this work. The Intelligent efficiency). This also allows us to extend it easily, for exam-
Decision-Making Group is partly supported by NSF awards under con- ple, to use inadmissible heuristics and different tie-breaking
tracts IIS-9984827, IIS-0098807, and ITR/AP-0113881 as well as an IBM criteria to gain efficiency. Since D* Lite is based on LPA*,
faculty partnership award. The views and conclusions contained in this
it is simple, easy to understand, easy to analyze and easy
document are those of the authors and should not be interpreted as repre-
senting the official policies, either expressed or implied, of the sponsoring to extend. It also inherits all of the properties of LPA* and
organizations and agencies or the U.S. government. can be extended in the same way as LPA*. It has more
 Knowledge Before the First Move of the Robot
14 13
14 13
12
12
11
11
10
10
9
9
8
8
7
7
6
6
6
5
6
5
6
5
6
5
6
5
6
5
6
5
6
5
6
5
ficiently recalculate a shortest path from the current vertex
14 13
14 13
12
12
11
11
10
10
9
9
8
8
7
7
6
6
5
5
4
4
4
3
4
3
4
3
4
3
4
3
4
3
4
3 of the robot to the goal vertex by recalculating only those
14 13 12 11 10 9 8 7 6 5 4 3 2 2 2 2 2 3
14 13
14 13
12
12
11
11
10
10
9
9
8
8
7
7
6
6
5
5
4
4
3
3
2
2
1 1
1 sgoal
1
1
2
2
3
3
goal distances that have changed (or have not been calcu-
8
14 13 12
5
11
4
10
9
9
2
8
1
7
1
6
5
5
4
4
3
3
2
2
1
2
1
2
1
2
2
2
3
3 lated before) and are relevant for recalculating the shortest
14 13 12 11 10 9 3 3 3 5 4 3 3 3 3 3 3 3
14 13
14 13
12
12
11
11
10
11
10
11
4
5
7
7
6
6
5
5
4
5
4
5
4
5
4
5
4
5
4
5
4
5
4
5
path. This is what D* Lite does. The challenge is to identify
14 13 12 12 12 12 6 7 6 6 6 6 6 6 6 6 6 6
8 7 7 7 7 13 7 7 7 7 7 7 7 7 7 7 7 7 these vertices efficiently.
18 sstart 16 15 14 14 8 8 8 8 8 8 8 8 8 8 8 8
Knowledge After the First Move of the Robot
14 13 12 11 10 9 8 7 6 6 6 6 6 6 6 6 6 6
14 13 12 11 10 9 8 7 6 5 5 5 5 5 5 5 5 5
14 13 12 11 10 9 8 7 6 5 4 4 4 4 4 4 4 4
14 13 12 11 10 9 8 7 6 5 4 3 3 3 3 3 3 3
14
14
13
13
12
12
11
11
10 9
10 9
8
8
7
7
6
6
5
5
4
4
3
3
2
2
2
1
2
1
2
1
2
2
3
3 3 Lifelong Planning A*
14 13 12 11 10 9 8 7 6 5 4 3 2 s
1 goal 1 2 3
8 5 4 10 2 1 1 5 4 3 2 1 1 1 2 3
14 12 11 11 2 7 6 5 4 3 2 2 2 2 2 3
15
15 14
13
13 12 12 sstart 3 3 3 5 4 3 3 3 3 3 3 3 We first describe Lifelong Planning A* (LPA*) [7], our
15 14 13 13 13 13 4 7 6 5 4 4 4 4 4 4 4 4
15
15
14
15
14
15
14
15
14 14
15 15
5
6
7
7
6
6
5
6
5
6
5
6
5
6
5
6
5
6
5
6
5
6
5
6
incremental heuristic search method that repeatedly deter-
8 7 7 7 7 16 7 7 7 7 7 7 7 7 7 7 7 7
21 20 19 18 17 17 8 8 8 8 8 8 8 8 8 8 8 8 mines shortest paths between two given vertices as the edge
Figure 1: Simple Example (Part 1). costs of a graph change. LPA* is an incremental search
method that uses heuristics to focus the search. An in-
than thirty percent fewer lines of code than D* (without any cremental search tends to only recalculate those start dis-
coding tricks), uses only one tie-breaking criterion when tances (that is, distance from the start vertex to a vertex)
comparing priorities which simplifies the maintenance of that have changed (or have not been calculated before) [4]
the priorities, and does not need nested if-statements with and a heuristic search tends to only recalculate those start
complex conditions that occupy up to three lines each which distances that are relevant for recalculating a shortest path
simplifies the analysis of the program flow. Yet, our exper- from the start vertex to the goal vertex [10]. LPA* thus re-
iments show that D* Lite is more efficient than D*. The calculates only very few start distances. We later use LPA*
simplicity of D* Lite is important for optimizing it, inte- to develop D* Lite.
grating it into complete robot architectures, and using it to
solving navigation tasks other than goal-directed navigation 3.1 Notation
in unknown terrain.
LPA* searches directed graphs, just like A*. We use the
following notation. S denotes the finite set of vertices of
the graph. S u (s)  S denotes the set of successors of
2 Motivation s 2 S in the graph. Similarly, P red(s)  S denotes the set

Consider a robot-navigation task in unknown terrain, of predecessors of s 2 S in the graph. 0 < (s; s0 )  1
where the robot always observes which of its eight adja- denotes the cost of moving from s to s0 2 S u (s). The
cent cells are traversable and then moves with cost one to task of LPA* is to maintain a shortest path from the start
one of them. The robot starts at the start vertex and has to vertex sstart 2 S to the goal vertex sgoal 2 S , knowing
move to the goal vertex. It always computes a shortest path both the topology of the graph and the current edge costs.
from its current vertex to the goal vertex under the assump-
tion that cells with unknown blockage status are traversable. 3.2 Lifelong Planning A*
It then follows this path until it reaches the goal vertex, in
which case it stops successfully, or some edge costs change, LPA* is shown in Figure 2. It maintains an estimate g (s)
in which case it recomputes a shortest path from the current of the start distance of each vertex s. LPA* also maintains
vertex to the goal vertex. It can utilize initial knowledge rhs-values, a second kind of estimates of the start distances.
about the blockage status of cells in case it is available. Fig- The rhs-values are one-step lookahead values based on the
ure 1 shows the goal distances of all traversable cells and g-values and thus potentially better informed than the g-
the shortest paths both before and after the robot has moved values. They always satisfy the following relationship:
along the path and discovered the first blocked cell it did not
know about. Cells whose goal distances have changed are

0 if s = sstart
rhs(s) = 0 0 ; s)) (1)
shaded gray. The goal distances are important because one mins0 2P red(s) (g (s ) + (s otherwise.
can easily determine a shortest path from the current vertex
of the robot to the goal vertex by greedily decreasing the A vertex is called consistent iff its g-value equals its rhs-
goal distances once the goal distances have been computed. value, otherwise it is called inconsistent. This concept is
Notice that the goal distances of only about 15 percent of the important because the g-values of all vertices equal their
cells have changed, and most of the changed goal distances start distances iff all vertices are consistent. However, LPA*
are irrelevant for recalculating a shortest path from the cur- does not make every vertex consistent after some of the edge
rent vertex of the robot to the goal vertex. Thus, one can ef- costs have changed. First, we can prove that LPA* does not
The pseudocode uses the following functions to manage the priority queue: U.TopKey() returns the smallest priority of
all vertices in priority queue U . (If U is empty, then U.TopKey() returns [1; 1℄.) U.Pop() deletes the vertex with a vector with two compo-
the smallest priority in priority queue U and returns the vertex. U.Insert (s; k) inserts vertex s into priority queue U
with priority k. Finally, U.Remove(s) removes vertex s from priority queue U . nents where k1 (s) = min(g (s); rhs(s)) + h(s; sgoal ) and
procedure CalculateKey(s) k2 (s) = min(g (s); rhs(s)). The priorities are compared
f01g return [min(g(s); rhs(s)) + h(s; sgoal ); min(g(s); rhs(s))℄;
procedure Initialize()
according to a lexicographic ordering. For example, prior-
f02g U = ;;
f03g for all s 2 S rhs(s) = g(s) = 1;
ity k (s) is smaller than or equal to priority k 0 (s), denoted by
f04g rhs(sstart ) = 0; k (s) _ k 0 (s), iff either k1 (s) < k 0 (s) or (k1 (s) = k 0 (s) and
f05g U.Insert(sstart ; CalculateKey(sstart )); 1 1
0
k2 (s)  k2 (s)). LPA* recalculates the g-values of vertices
procedure UpdateVertex(u)
f06g if (u 6= sstart ) rhs(u) = min
s0 2P red(u)
(g(s0 ) + (s0 ; u));
f07g if (u 2 U ) U.Remove(u);
in the priority queue (“expands the vertices” by executing
f08g if (g(u) 6= rhs(u)) U.Insert(u; CalculateKey(u)); lines f10-16g) in the order of increasing first priority com-
procedure ComputeShortestPath()
_ CalculateKey(sgoal ) OR rhs(sgoal ) 6= g(sgoal ))
f09g while (U.TopKey()< ponents, which correspond to the f-values of an A* search,
f10g u = U.Pop();
f11g if (g(u) > rhs(u))
and vertices with equal first priority components in order of
f12g g(u) = rhs(u);
f13g for all s 2 Su (u) UpdateVertex (s);
increasing second priority components, which correspond
f14g
f15g
else
g(u) = 1;
to the g-values of an A* search. Thus, it expands vertices in
f16g for all s 2 Su (u) [ fug UpdateVertex(s);
a similar order as an A* search, that expands vertices in the
procedure Main()
f17g Initialize(); order of increasing f-values (since the heuristics are consis-
f18g forever
f19g ComputeShortestPath(); tent) and vertices with equal f-values that are on the same
f20g Wait for changes in edge costs;
f21g for all directed edges (u; v) with changed edge costs branch of its search tree in order of increasing g-values. A
f22g Update the edge cost (u; v);
f23g UpdateVertex (v); more detailed and formal description of LPA*, its compar-
Figure 2: Lifelong Planning A* (forward search). ison to A*, and proofs of its correctness can be found in
[8].
recompute the start distances that have been computed be-
fore and have not changed (incremental search) [8], simi-
lar to DynamicSWSF-FP [12]. Second, LPA* uses heuris- 4 D* Lite
tic knowledge (in form of approximations of the goal dis- So far, we have described our LPA*, that repeatedly de-
tances) to focus the search and determine that some start termines shortest paths between the start vertex and the goal
distances need not get computed at all (heuristic search), vertex as the edge costs of a graph change. We now use
similar to A* [10]. The heuristics h(s; s0 ) estimate the dis- LPA* to develop D* Lite, that repeatedly determines short-
tance between vertex s and vertex s0 . They need to satisfy est paths between the current vertex of the robot and the
0 0
h(sgoal ; sgoal ) = 0 and h(s; sgoal )  (s; s ) + h(s ; sgoal )
goal vertex as the edge costs of a graph change while the
for all vertices s 2 S and s0 2 S u (s). We can prove robot moves towards the goal vertex. D* Lite does not make
that ComputeShortestPath() of LPA* recalculates the g- any assumptions about how the edge costs change, whether
value of each vertex at most twice and thus terminates. If they go up or down, whether they change close to the cur-
g (sgoal ) = 1 after the search, then there is no finite-cost
rent vertex of the robot or far away from it, or whether they
path from sstart to sgoal . Otherwise, one can trace back a change in the world or only because the robot revised its ini-
shortest path from sstart to sgoal by always moving from tial estimates. The goal-directed navigation problem in un-
the current vertex s, starting at sgoal , to any predecessor s0 known terrain then is a special case of this problem, where
that minimizes g (s0 ) + (s0 ; s) until sstart is reached (ties the graph is an eight-connected grid whose edge costs are
can be broken arbitrarily). Thus, LPA* does not explicitly initially one and change to infinity when the robot discov-
maintain a search tree. Instead, it uses the g-values to en- ers that they cannot be traversed. We first describe a simple
code it implicitly. version of D* Lite and then a more sophisticated version.
Because both versions of D* Lite are based on LPA*, they
3.3 Similarity of Lifelong Planning A* and A* share many properties with A* and are efficient.
LPA* is an incremental version of A*, the most popu-
lar search method in artificial intelligence, and thus shares 4.1 The Basic Version of D* Lite
many similarities with it. For example, both search meth-
We have already argued that many goal distances remain
ods maintain a priority queue. The priority queue of LPA*
unchanged as the robot moves to the goal vertex and ob-
always contains exactly the inconsistent vertices. These are
serves obstacles in the process. Thus, we can use a ver-
the vertices whose g-values LPA* potentially needs to up-
sion of LPA* for the goal-directed navigation problem in
date to make them consistent. The priority of vertex s in the
unknown terrain. We first need to switch the search direc-
priority queue is always
tion of LPA*. The version presented in Figure 2 searches
from the start vertex to the goal vertex and thus its g-values
k (s) = [k1 (s); k2 (s)℄; (2) are estimates of the start distances. The version presented
The pseudocode uses the following functions to manage the priority queue: U.TopKey() returns the smallest priority of The pseudocode uses the following functions to manage the priority queue: U.TopKey() returns the smallest priority of
all vertices in priority queue U . (If U is empty, then U.TopKey() returns [1; 1℄.) U.Pop() deletes the vertex with all vertices in priority queue U . (If U is empty, then U.TopKey() returns [1; 1℄.) U.Pop() deletes the vertex with
the smallest priority in priority queue U and returns the vertex. U.Insert (s; k) inserts vertex s into priority queue U the smallest priority in priority queue U and returns the vertex. U.Insert (s; k) inserts vertex s into priority queue U
with priority k. Finally, U.Remove(s) removes vertex s from priority queue U . with priority k. U.Update(s; k) changes the priority of vertex s in priority queue U to k. (It does nothing if the
current priority of vertex s already equals k.) Finally, U.Remove(s) removes vertex s from priority queue U .
procedure CalculateKey(s)
f01g return [min(g(s); rhs(s)) + h(sstart ; s); min(g(s); rhs(s))℄; procedure CalculateKey(s)
f01’g return [min(g(s); rhs(s)) + h(sstart ; s); min(g(s); rhs(s))℄;
procedure Initialize()
f02g U = ;; procedure Initialize()
f03g for all s 2 S rhs(s) = g(s) = 1; f02’g U = ;;
f04g rhs(sgoal ) = 0; f03’g for all s 2 S rhs(s) = g(s) = 1;
f05g U.Insert(sgoal ; CalculateKey(sgoal )); f04’g rhs(sgoal ) = 0;
f05’g U.Insert(sgoal ; CalculateKey(sgoal ));
procedure UpdateVertex(u)
f06g if (u 6= sgoal ) rhs(u) = min
s0 2Su (u)
( (u; s0 ) + g(s0 )); procedure UpdateVertex (u)
f07g if (u 2 U ) U.Remove(u);
f06’g if (u 6= sgoal ) rhs(u) = min 0
s 2Su (u)
( (u; s0 ) + g(s0 ));
f08g if (g(u) 6= rhs(u)) U.Insert(u; CalculateKey(u)); f07’g if (u 2 U ) U.Remove(u);
f08’g if (g(u) 6= rhs(u)) U.Insert(u; CalculateKey(u));
procedure ComputeShortestPath()
f09g while (U.TopKey()< _ CalculateKey(sstart ) OR rhs(sstart ) 6= g(sstart )) procedure ComputeShortestPath()
f10g u = U.Pop(); f09’g while (U.TopKey()< _ CalculateKey(sstart ) OR rhs(sstart ) 6= g(sstart ))
f11g if (g(u) > rhs(u)) f10’g u = U.Pop();
f12g g(u) = rhs(u); f11’g if (g(u) > rhs(u))
f13g for all s 2 P red(u) UpdateVertex (s); f12’g g(u) = rhs(u);
f14g else f13’g for all s 2 P red(u) UpdateVertex (s);
f15g g(u) = 1; f14’g else
f16g for all s 2 P red(u) [ fug UpdateVertex(s); f15’g g(u) = 1;
f16’g for all s 2 P red(u) [ fug UpdateVertex (s);
procedure Main()
f17g Initialize(); procedure Main()
f18g forever f17’g Initialize();
f19g ComputeShortestPath(); f18’g ComputeShortestPath();
f20g Wait for changes in edge costs; f19’g while (sstart 6= sgoal )
f21g for all directed edges (u; v) with changed edge costs f20’g /* if (g(sstart ) = 1) then there is no known path */
f22g Update the edge cost (u; v);
f21’g sstart = arg min 0
s 2Su (sstart )
( (sstart ; s0 ) + g(s0 ));
f23g UpdateVertex (u);
f22’g Move to sstart ;
f23’g Scan graph for changed edge costs;
Figure 3: Lifelong Planning A* (backward search). f24’g if any edge costs changed
f25’g for all directed edges (u; v) with changed edge costs
f26’g Update the edge cost (u; v);
f27’g UpdateVertex (u);
for all s 2 U
in Figure 3 searches from the goal vertex to the start vertex f28’g
f29’g U.Update(s; CalculateKey(s));
ComputeShortestPath();
and thus its g-values are estimates of the goal distances. It f30’g

was derived from the original version by reversing all edges Figure 4: D* Lite: Basic Version.
of the graph and exchanging the start and goal vertex. The
heuristics h(s; s0 ) now need to satisfy h(sstart ; sstart ) = be reached during the search. The basic version of D* Lite
0 and h(sstart ; s)  h(sstart ; s0 ) + (s0 ; s) for all ver- then computes a shortest path from the current vertex of the
tices s 2 S and s0 2 P red(s). More generally, since robot sstart to the goal vertex f18’g. If the robot has not
the robot moves and thus changes sstart , the heuristics reached the goal vertex yet f19’g, it makes one transition
needs to satisfy this property for all sstart 2 S . To along the shortest path and updates sstart to reflect the cur-
solve the goal-directed navigation problem in unknown ter- rent vertex of the robot f21’-22’g. (In the pseudocode, we
rain, the CalculateKey(), Initialize(), UpdateVertex(), and have included a comment on how the robot can detect that
ComputeShortestPath() functions can remain unchanged. there is no path but do not prescribe what it should do in
However, the Main() function needs to get extended so that this case. For the goal-directed navigation problem in un-
it moves the robot and then recalculates the priorities of the known terrain, for example, it should stop and announce
vertices in the priority queue appropriately. This is neces- that there is no path since obstacles do not disappear.) It
sary because the heuristics change when the robot moves, then scans for changes in edge costs f23’g. If some edge
since they are computed with respect to the current vertex costs have changed, it updates the edge costs f26’g and
of the robot. This only changes the priorities of the vertices calls UpdateVertex() f27’g to update the rhs-values of the
in the priority queue but not which vertices are consistent vertices potentially affected by the changed edge costs so
and thus in the priority queue. Figure 4 shows the resulting that they again satisfy the equivalent of Equation 1 and the
algorithm, called the basic version of D* Lite. priority queue so that it again contains exactly the incon-
The basic version of D* Lite first calls Initialize() f17’g sistent vertices with priorities that satisfy the equivalent of
to initialize the g-values of the vertices to infinity, the rhs- Equation 2. The basic version of D* Lite then updates the
values of the vertices so that they satisfy the equivalent of priorities of all vertices in the priority queue f28’-29’g, re-
Equation 1, and the priority queue so that it contains ex- calculates a shortest path f30’g, and iterates.
actly the inconsistent vertices with priorities that satisfy the We can prove a variety of properties of the ba-
equivalent of Equation 2. The priority queue only contains sic version of D* Lite, including the correctness of its
the goal vertex since all other vertices are initially consis- ComputeShortestPath() function which implies the correct-
tent. Thus, in an actual implementation, the basic version of ness of the basic version of D* Lite (all proofs can be found
D* Lite needs to initialize a vertex only when it is encoun- in [8]):
tered during the search and thus does not need to initialize
all vertices up front. This is important because the num- Theorem 1 ComputeShortestPath() of the basic version of
ber of vertices can be large and only a few of them might D* Lite always terminates and one can then follow a short-
The pseudocode uses the following functions to manage the priority queue: U.TopKey() returns the smallest priority of
all vertices in priority queue U . (If U is empty, then U.TopKey() returns [1; 1℄.) U.Pop() deletes the vertex with uses for the corresponding vertices. They are initialized
the smallest priority in priority queue U and returns the vertex. U.Insert (s; k) inserts vertex s into priority queue U
with priority k. Finally, U.Remove(s) removes vertex s from priority queue U . in the same way as the basic version of D* Lite initializes
procedure CalculateKey(s) them. After the robot has moved from vertex s to some
+ h(sstart ; s)+km ; min(g(s); rhs(s))℄;
vertex s0 where it detects changes in edge costs, the first ele-
f01”g return [min(g(s); rhs(s))

procedure Initialize()
f02”g U = ;;
f03”g km = ; 0 ment of the priorities can have decreased by at most h(s; s0 ).
f04”g for all s 2 S rhs(s) = g(s) = 1;
f05”g rhs(sgoal ) = 0;
(The second component does not depend on the heuristics
f06”g U.Insert(sgoal ; CalculateKey(sgoal )); and thus remains unchanged.) Thus, in order to maintain
procedure UpdateVertex(u)
f07”g if (u 6= sgoal ) rhs(u) = min 0 ( (u; s0 ) + g(s0 ));
lower bounds, D* Lite needs to subtract h(s; s0 ) from the
s 2Su (u)
f08”g if (u 2 U ) U.Remove(u); first element of the priorities of all vertices in the priority
queue. However, since h(s; s0 ) is the same for all vertices
f09”g if (g(u) 6= rhs(u)) U.Insert(u; CalculateKey(u));

procedure ComputeShortestPath()
f10”g while (U.TopKey()< _ CalculateKey(sstart ) OR rhs(sstart ) 6= g(sstart ))
f11”g kold = U.TopKey();
in the priority queue, the order of the vertices in the priority
f12”g u = U.Pop();
f13”g if( kold _ <CalculateKey( )) u queue does not change if the subtraction is not performed.
f14”g u
U.Insert( ; CalculateKey( )); u Then, when new priorities are computed, their first compo-
f15”g else if (g(u) > rhs(u))
f16”g
f17”g
g(u) = rhs(u);
for all s 2 P red(u) UpdateVertex (s);
nents are by h(s; s0 ) too small relative to the priorities in the
f18”g
f19”g
else
g(u) = 1;
priority queue. Thus, h(s; s0 ) has to be added to their first
f20”g for all s 2 P red(u) [ fug UpdateVertex (s);
components every time some edge costs change. If the robot
procedure Main()
f21”g slast = sstart
; moves again and then detects cost changes again, then the
f22”g Initialize();
f23”g ComputeShortestPath(); constants need to get added up. We do this in the variable
f24”g while (sstart = 6 sgoal )
f25”g /* if (g(sstart ) = 1) then there is no known path */ km f30”g. Thus, whenever new priorities are computed,
f26”g sstart = arg min 0 ( (sstart ; s0 ) + g(s0 ));
f27”g Move to sstart ;
s 2Su (sstart ) the variable km has to be added to their first components, as
f28”g
f29”g
Scan graph for changed edge costs;
if any edge costs changed
done in f01”g. Then, the order of the vertices in the priority
f30”g km = km + h(slast ; sstart ); queue does not change after the robot moves and the pri-
f31”g slast = sstart ;
f32”g
f33”g
for all directed edges (u; v) with changed edge costs
Update the edge cost (u; v);
ority queue does not need to get reordered. The priorities,
f34”g
f35”g
UpdateVertex(u);
ComputeShortestPath();
on the other hand, are always lower bounds on the corre-
sponding priorities of the basic version of D* Lite after the
Figure 5: D* Lite: Final Version. first component of the priorities of the basic version of D*
Lite has been increased by the current value of km . We
est path from sstart to sgoal by always moving from the cur- exploit this property by changing ComputeShortestPath()
rent vertex s, starting at sstart , to any successor s0 that as follows. After ComputeShortestPath() has removed a
minimizes (s; s0 ) + g (s0 ) until sgoal is reached (ties can vertex u with the smallest priority kold = U.TopKey()
be broken arbitrarily). from the priority queue f12”g, it now uses CalculateKey()
to compute the priority that it should have had. If
kold <_ CalculateKey(u) then it reinserts the removed ver-
4.2 The Final Version of D* Lite tex with the priority calculated by CalculateKey() into the
priority queue f13”-14”g. Thus, it remains true that the
The basic version of D* Lite has the disadvantage that priorities of all vertices in the priority queue are lower
the repeated reordering of the priority queue can be expen- bounds on the corresponding priorities of the basic ver-
sive since the priority queue often contains a large num- sion of D* Lite after the first components of the priori-
ber of vertices. The final version of D* Lite, shown in ties of the basic version of D* Lite have been increased
Figure 5, uses a method derived from D* [14] to avoid by the current value of km . If kold  _ CalculateKey(u),
having to reorder the priority queue. Differences to the then it holds that kold = _ CalculateKey(u). In this case,
basic version of D* Lite are shown in bold. The heuris- ComputeShortestPath() performs the same operations for
tics h(s; s0 ) now need to satisfy h(s; s0 )   (s; s0 ) and vertex u as ComputeShortestPath() of the basic version
00 0 0 00 0 00
h(s; s )  h(s; s ) + h(s ; s ) for all vertices s; s ; s 2 S ,
 0
of D* Lite f15”-20”g. ComputeShortestPath() performs
where (s; s ) denotes the cost of a shortest path from ver- these operations for vertices in the exact same order as
tex s 2 S to vertex s0 2 S . This property implies the prop- ComputeShortestPath() of the basic version of D* Lite,
erty that heuristics for the basic version of D* Lite need to which implies that the final version of D* Lite shares many
satisfy. This is not overly restrictive, however, since both properties with the basic version of D* Lite, including the
properties are guaranteed to hold if the heuristics are de- correctness of ComputeShortestPath() which implies the
rived by relaxing the search problem, which will almost al- correctness of the final version of D* Lite:
ways be the case and holds for the heuristics used in this
paper. Theorem 2 ComputeShortestPath() of the final version of
The final version of D* Lite uses priorities that are lower D* Lite always terminates and one can then follow a short-
bounds on the priorities that the basic version of D* Lite est path from sstart to sgoal by always moving from the cur-
The pseudocode uses the following functions to manage the priority queue: U.Top() returns a vertex with the smallest
priority of all vertices in priority queue U . U.TopKey() returns the smallest priority of all vertices in priority queue (that is, a vertex whose g-value is larger than its rhs-value)
U . (If U is empty, then U.TopKey() returns [1; 1℄.) U.Insert(s; k) inserts vertex s into priority queue U with
priority k. U.Update(s; k) changes the priority of vertex s in priority queue U to k. (It does nothing if the current it is unnecessary to take the minimum over all of its respec-
priority of vertex s already equals k.) Finally, U.Remove (s) removes vertex s from priority queue U .
tive successors since only the g-value of the overconsistent
procedure CalculateKey(s)
f01”’g return [min(g(s); rhs(s)) + h(sstart ; s) + km ; min(g(s); rhs(s))℄; vertex has changed. Since it decreased, it cannot increase
procedure Initialize()
f02”’g U = ;;
the rhs-values of the predecessors. Thus, it is sufficient to
f03”’g km = 0;
f04”’g for all s 2 S rhs(s) = g(s) = 1;
compute the rhs-value as the minimum of its old rhs-value
f05”’g rhs(sgoal ) = 0;
f06”’g U.Insert(sgoal ; [h(sstart ; sgoal ); 0℄);
and the sum of the cost of moving from the predecessor to
procedure UpdateVertex(u) the overconsistent vertex and the new g-value of the over-
f07”’g if (g(u) 6= rhs(u) AND u 2 U ) U.Update(u; Cal ulateKey(u));
f08”’g else if (g(u) 6= rhs(u) AND u 2
= U ) U.Insert(u; Cal ulateKey(u));
consistent vertex (f20”’g). A similar optimization can be
f09”’g else if (g(u) = rhs(u) AND u 2 U ) U.Remove (u);
made for the computation of the rhs-value of a vertex after
procedure ComputeShortestPath()
f10”’g while (U.TopKey()< _ CalculateKey(sstart ) OR rhs(sstart ) 6= g(sstart )) the cost of one of its outgoing edges has changed (f44”’g).
f11”’g u = U.Top();
f12”’g kold = U.TopKey(); Third, when UpdateVertex() on line f20”g computes the
f13”’g knew = CalculateKey(u));
f14”’g if(kold <k _ new ) rhs-value for a predecessor of an underconsistent vertex
f15”’g U.Update(u; knew );
f16”’g else if (g(u) > rhs(u)) (that is, a vertex whose g-value is smaller than its rhs-value),
f17”’g g(u) = rhs(u);
f18”’g U.Remove (u); the only g-value that has changed is the g-value of the un-
f19”’g for all s 2 P red(u)
f20”’g if (s =
6 sgoal ) rhs(s) = min(rhs(s); (s; u) + g(u)); derconsistent vertex. Since it increased, the rhs-value of the
f21”’g UpdateVertex (s);
f22”’g else predecessor can only get affected if its old rhs-value was
f23”’g gold = g(u);
f24”’g g(u) = 1; based on the old g-value of the underconsistent vertex. This
f25”’g for all s 2 P red(u) [ fug
f26”’g if (rhs(s) = (s; u) + gold OR s = u) can be used to decide whether the predecessor needs to get
( (s; s0 ) + g(s0 ));
f27”’g
f28”’g
if (s 6= sgoal ) rhs(s) = min 0
UpdateVertex (s);
s 2Su (s) updated and its rhs-value needs to get recomputed (f26”’-
procedure Main() 27”’g). A similar optimization can be made for the compu-
f29”’g slast = sstart ;
f30”’g Initialize(); tation of the rhs-value of a vertex after the cost of one of its
f31”’g ComputeShortestPath();
f32”’g while (sstart = 6 sgoal ) outgoing edges has changed (f45”’-46”’g).
f33”’g /* if (g(sstart ) = 1) then there is no known path */
f34”’g sstart = arg min 0 ( (sstart ; s0 ) + g(s0 ));
Fourth, there are several small optimizations one can
s 2Su (sstart )
f35”’g Move to sstart ; perform. For example, the priority on line f06”g can be
calculated directly (f06”’g), CalculateKey() on lines f13”-
f36”’g Scan graph for changed edge costs;
f37”’g if any edge costs changed
km = km + h(slast ; sstart );
14”g needs to calculate the priority of vertex u only once
f38”’g
f39”’g slast = sstart ;
for all directed edges (u; v) with changed edge costs
(f13”’g), and the vertex with the highest priority needs to
f40”’g
f41”’g old = (u; v);
Update the edge cost (u; v);
get removed on line f12”g only if line f14”g does not rein-
f42”’g
f43”’g if ( old > (u; v))
if (u 6= sgoal ) rhs(u) = min(rhs(u); (u; v) + g(v));
sert it again immediately afterwards ( f12”’, 15”’, 18”’g).
f44”’g
f45”’g else if (rhs(u) = old + g(v))
f46”’g if (u 6= sgoal ) rhs(u) = min 0
s 2Su (u)
( (u; s0 ) + g(s0 ));
f47”’g UpdateVertex (u);
f48”’g ComputeShortestPath();

Figure 6: D* Lite: Final Version (optimized version).

5 An Example
We illustrate D* Lite using the two eight-connected grids
rent vertex s, starting at sstart , to any successor s0 that from Figure 1. To make the search algorithms comparable,
minimizes (s; s0 ) + g (s0 ) until sgoal is reached (ties can their search always starts at sgoal and proceeds towards the
be broken arbitrarily). current cell of the robot. We use the maximum of the ab-
solute differences of the x and y coordinates of two cells
as an approximation of their distance. Cells expanded by
4.3 Optimizations the algorithms are shaded gray in Figure 7. (We consider
We implemented both the basic and final version of D* the current cell of the robot to be expanded by breadth-first
Lite, using standard binary heaps as priority queues. There search and A*.) As the figure shows, the heuristic search
are several ways of optimizing both versions of D* Lite outperforms the uninformed searches, and the incremental
without changing their overall operation, which we discuss search outperforms the complete (that is, nonincremental)
in the following in the context of the final version of D* ones after the first move of the robot (where previous search
Lite, the optimized version of which is shown in Figure 6. results are available). The figures also illustrate that the
First, a vertex sometimes gets removed from the prior- combination of heuristic and incremental search performed
ity queue on line f08”g and then immediately reinserted on by D* Lite decreases the number of expanded cells even
line f09”g. In this case, it is often more efficient to leave more than either a heuristic search or an incremental search
the vertex in the priority queue and only update its priority individually. In particular, the initial search of D* Lite ex-
(f07”’g). pands exactly the same cells as an A* search if A* breaks
Second, when UpdateVertex() on line f17”g computes ties between vertices with the same f-values suitably. In our
the rhs-value for a predecessor of an overconsistent vertex example, we broke ties in the most advantageous way for
 Before the First Move of the Robot
uninformed search heuristic search
breadth-first search A*

complete search
sgoal sgoal

sstart sstart

DynamicSWSF-FP (with early termination) D* Lite

incremental search

sgoal sgoal

sstart sstart

After the First Move of the Robot

uninformed search heuristic search
breadth-first search A*
complete search

sgoal sgoal

sstart sstart

DynamicSWSF-FP (with early termination) D* Lite

incremental search

sgoal sgoal

sstart sstart

Figure 7: Simple Example (Part 2).

percent of extra vertex expansions

A* and thus D* Lite and A* expand not exactly the same 60

cells. The second search of D* Lite expands only a subset 50

Overhead of Focussed D* Relative

of those cells whose goal distances changed (or had not been to the Final Optimized Version
40

of D* Lite (in percent) 30

calculated before). Thus, D* Lite results in substantial sav-
20
ings over an A* search that replicates most of its previous 10
search. 0
10x10 20x20 30x30 40x40
maze size

percent of extra vertex accesses percent of extra heap percolates

55 120

6 Experimental Results 50

45
110

40 100
We now compare focussed D* and the optimized final 35 90
version of D* Lite. Since both methods move the robot in 30
80
the same way and focussed D* has already been demon- 25

20 70
strated with great success on real robots, we only need 10x10 20x20 30x30
maze size
40x40 10x10 20x20 30x30
maze size
40x40

to perform a simulation study here. We need to compare

the total planning time of the two methods until the robot
reaches the goal vertex or recognizes that this is impossible. Figure 8: Performance Results for Different Maze Sizes.
Since the actual runtimes are implementation-dependent,
we instead use three measures that all correspond to com-
mon operations performed by the algorithms and thus heav- tex accesses (for example, to read or change their values).
ily influence their runtimes: the total number of vertex ex- We perform experiments with goal-directed navigation
pansions, the total number of heap percolates (exchanges of tasks in unknown terrain that is modeled as an eight-
a parent and child in the heap), and the total number of ver- connected grid. As approximations of the distance between
two vertices we use again the maximum of the absolute dif- [4] D. Frigioni, A. Marchetti-Spaccamela, and U. Nanni. Fully
ferences of their x and y coordinates. Figure 8 shows results dynamic algorithms for maintaining shortest paths trees.
for grids of varying sizes, averaging over 50 randomly gen- Journal of Algorithms, 34(2):251–281, 2000.
erated grids of each size, where the robot can always ob- [5] M. Hebert, R. McLachlan, and P. Chang. Experiments with
serve which of its eight adjacent cells are traversable. It driving modes for urban robots. In Proceedings of the SPIE
shows the three measures for the two algorithms as well Mobile Robots, 1999.
as the corresponding 95 percent confidence intervals to [6] Y. Huiming, C. Chia-Jung, S. Tong, and B. Qiang. Hy-
demonstrate that our conclusions are statistically signifi- brid evolutionary motion planning using follow boundary
cant. D* Lite performs better than D* with respect to all repair for mobile robots. Journal of Systems Architecture,
three measures, justifying our claim that it is more efficient 47(7):635–647, 2001.
than D*. We do not present results here that compare D* [7] S. Koenig and M. Likhachev. Incremental A*. In Proceed-
Lite to repeated A* searches since D* Lite is more efficient ings of the Neural Information Processing Systems, 2001.
than D*, and D* has already been shown to outperform re- [8] M. Likhachev and S. Koenig. Lifelong Planning A* and Dy-
peated A* searches by one to two orders of magnitude [14]. namic A* Lite: The proofs. Technical report, College of
Computing, Georgia Institute of Technology, Atlanta (Geor-
gia), 2001.
7 Conclusions [9] L. Matthies, Y. Xiong, R. Hogg, D. Zhu, A. Rankin,
B. Kennedy, M. Hebert, R. Maclachlan, C. Won, T. Frost,
In this paper, we have presented D* Lite, a novel fast re- G. Sukhatme, M. McHenry, and S. Goldberg. A portable,
planning method for goal-directed navigation in unknown autonomous, urban reconnaissance robot. In Proceedings of
terrain that implements the same navigation strategy as (fo- the International Conference on Intelligent Autonomous Sys-
cussed) D*. Both algorithms search from the goal vertex tems, 2000.
towards the current vertex of the robot, use heuristics to fo- [10] J. Pearl. Heuristics: Intelligent Search Strategies for Com-
cus the search, and use similar ways to minimize having to puter Problem Solving. Addison-Wesley, 1985.
reorder the priority queue. However, D* Lite builds on our [11] L. Podsedkowski, J. Nowakowski, M. Idzikowski, and I. Viz-
LPA*, that has a solid theoretical foundation, a strong sim- vary. A new solution for path planning in partially known
ilarity to A*, is efficient (since it does not expand any ver- or unknown environment for nonholonomic mobile robots.
tices whose g-values were already equal to their respective Robotics and Autonomous Systems, 34:145–152, 2001.
goal distances) and has been extended in a number of ways. [12] G. Ramalingam and T. Reps. An incremental algorithm for
Thus, D* Lite is algorithmically different from D*. It is a generalization of the shortest-path problem. Journal of Al-
also shorter, simpler, and consequently easier to understand gorithms, 21:267–305, 1996.
and extend than D*, yet is more efficient. We believe that [13] A. Stentz. Optimal and efficient path planning for partially-
our results will make D*-like replanning algorithms even known environments. In Proceedings of the International
more popular and enable robotics researchers to adapt them Conference on Robotics and Automation, pages 3310–3317,
to additional applications. More generally, we believe that 1994.
our experimental and analytical results provide a strong al- [14] A. Stentz. The focussed D* algorithm for real-time replan-
gorithmic foundation for further research on fast replanning ning. In Proceedings of the International Joint Conference
methods for mobile robots. on Artificial Intelligence, pages 1652–1659, 1995.
[15] A. Stentz and M. Hebert. A complete navigation system
for goal acquisition in unknown environments. Autonomous
Robots, 2(2):127–145, 1995.
References
[16] M. Tao, A. Elssamadisy, N. Flann, and B. Abbott. Optimal
[1] M. Barbehenn and S. Hutchinson. Efficient search and hier- route re-planning for mobile robots: A massively parallel
archical motion planning by dynamically maintaining single- incremental A* algorithm. In International Conference on
source shortest paths trees. IEEE Transactions on Robotics Robotics and Automation, pages 2727–2732, 1997.
and Automation, 11(2):198–214, 1995. [17] S. Thayer, B. Digney, M. Diaz, A. Stentz, B. Nabbe, and
M. Hebert. Distributed robotic mapping of extreme environ-
[2] B. Brumitt and A. Stentz. GRAMMPS: a generalized mis-
ments. In Proceedings of the SPIE: Mobile Robots XV and
sion planner for multiple mobile robots. In Proceedings of
Telemanipulator and Telepresence Technologies VII, volume
the International Conference on Robotics and Automation,
4195, 2000.
1998.
[18] K. Trovato. Differential A*: An adaptive search method il-
[3] T. Ersson and X. Hu. Path planning and navigation of mo- lustrated with robot path planning for moving obstacles and
bile robots in unknown environments. In Proceedings of the
goals, and an uncertain environment. Journal of Pattern
International Conference on Intelligent Robots and Systems,
Recognition and Artificial Intelligence, 4(2), 1990.
2001.