D* / D* Lite, in depth: the g and rhs invariant, backwards search, the km key trick, a tested implementation and a measured comparison with repeated A*

From D* to D* Lite

Anthony Stentz published D* (Dynamic A*) in 1994 for mobile robots in partially known terrain, and Focussed D* in 1995, which adds a heuristic to focus the repairs. Both are efficient but notoriously intricate to implement. Sven Koenig and Maxim Likhachev then built Lifelong Planning A* (LPA*), an incremental version of A* for a fixed start and goal on a changing graph, and in 2002 derived D* Lite from it. D* Lite is the same idea as Focussed D*, a backwards incremental search with a moving robot, but with far simpler code, and its authors reported performance at least comparable to Focussed D*. Today, when people say they use D*, they almost always mean D* Lite.

Two estimates per node: g and rhs

D* Lite stores two numbers per node s. The value g(s) is the current estimate of the distance from s to the goal. The value rhs(s) is a one-step lookahead based on the neighbours' g-values:

rhs(goal) = 0
rhs(s)    = min over successors s' of ( c(s, s') + g(s') )      for s != goal

A node is consistent when g(s) = rhs(s). If every node is consistent, the g-values are exact shortest distances, just as at the end of Dijkstra's algorithm. When an edge cost changes, only the rhs of the nodes touching that edge changes. Those nodes become inconsistent and go into a priority queue, and the search processes inconsistent nodes until the robot's own node is consistent and no queued node could improve it.

There are two kinds of inconsistency. An overconsistent node has g > rhs: a shorter path appeared, so the fix is to set g = rhs and propagate the improvement to its predecessors, exactly like a Dijkstra relaxation. An underconsistent node has g < rhs: its old path got worse or disappeared. The fix is to set g to infinity, recompute its rhs, and push the damage outwards. The node is then processed again later with a correct value. Handling both cases is what lets D* Lite cope with costs going up as well as down.

Why the search runs backwards

D* Lite searches from the goal towards the robot, so g(s) means the distance from s to the goal. There are two reasons. First, the goal does not move but the robot does. A forward search rooted at the robot would have a new root after every step, invalidating every stored value. A backward search keeps its root fixed. Second, sensors see obstacles near the robot. In a backward search, the nodes whose g-values depend on a cell near the robot are the few nodes behind it relative to the goal, so a nearby change usually repairs a small region. A change near the goal would invalidate much more, but robots rarely discover those until they are close, by which point the remaining search is small anyway.

Once the search finishes, the robot picks its next step greedily: move to the successor s' that minimises c(s_start, s') + g(s'). No path needs to be stored. The g-values are a gradient the robot follows downhill to the goal.

Keys and the km modifier

Queue priorities resemble A*'s f = g + h, using min(g, rhs) so both kinds of inconsistent node sort correctly. The heuristic h(s_start, s) estimates the distance from the robot to s, and it must be consistent, as in A*. The problem is that s_start changes every time the robot moves, so every key in the queue is suddenly based on the wrong start. Re-keying the whole queue after every move would cost more than the search.

D* Lite's fix is a single number, km. When the robot moves from s_last to s_start, km grows by h(s_last, s_start). New keys add km, so they are comparable with old ones: the heuristic to the new start can drop by at most h(s_last, s_start), and old keys were computed with that much smaller km, so they remain lower bounds. When the search pops a node whose stored key is smaller than its freshly computed key, it does not process it but re-inserts it with the new key. That lazy correction replaces a full queue rebuild.

key(s) = [ min(g(s), rhs(s)) + h(s_start, s) + km ,  min(g(s), rhs(s)) ]

Keys are compared lexicographically. The second component breaks ties in favour of nodes closer to the goal, which play the role of A*'s smaller g.

A complete implementation

The implementation below works on a 4-connected grid with unit costs and blocked cells. It uses Python's heapq with lazy deletion: instead of removing a node from the heap, it records each node's current key in a dictionary and skips heap entries that no longer match. That keeps every operation O(log n) without an indexed heap.

import heapq, math
INF = math.inf

class DStarLite:
    def __init__(self, rows, cols, start, goal, blocked):
        self.rows, self.cols, self.start, self.goal = rows, cols, start, goal
        self.blocked = set(blocked)
        self.g, self.rhs, self.km = {}, {goal: 0.0}, 0.0
        self.open, self.key_of = [], {}       # heap + live key per queued node
        self._push(goal)

    def h(self, a, b): return abs(a[0] - b[0]) + abs(a[1] - b[1])
    def cost(self, a, b): return INF if a in self.blocked or b in self.blocked else 1.0
    def neighbors(self, s):
        r, c = s
        for nr, nc in ((r+1, c), (r-1, c), (r, c+1), (r, c-1)):
            if 0 <= nr < self.rows and 0 <= nc < self.cols:
                yield (nr, nc)

    def key(self, s):
        m = min(self.g.get(s, INF), self.rhs.get(s, INF))
        return (m + self.h(self.start, s) + self.km, m)

    def _push(self, s):
        self.key_of[s] = k = self.key(s)
        heapq.heappush(self.open, (k[0], k[1], s))

    def _top(self):
        while self.open:
            k1, k2, s = self.open[0]
            if self.key_of.get(s) == (k1, k2):
                return (k1, k2), s
            heapq.heappop(self.open)          # stale entry
        return (INF, INF), None

    def update_vertex(self, u):
        if u != self.goal:
            self.rhs[u] = min((self.cost(u, s) + self.g.get(s, INF)
                               for s in self.neighbors(u)), default=INF)
        self.key_of.pop(u, None)              # lazy removal from the queue
        if self.g.get(u, INF) != self.rhs.get(u, INF):
            self._push(u)

    def compute_shortest_path(self):
        while True:
            k_old, u = self._top()
            s = self.start
            if u is None or not (k_old < self.key(s)
                                 or self.rhs.get(s, INF) != self.g.get(s, INF)):
                return
            heapq.heappop(self.open)
            del self.key_of[u]
            if k_old < self.key(u):           # key stale because km grew
                self._push(u)
            elif self.g.get(u, INF) > self.rhs.get(u, INF):   # overconsistent
                self.g[u] = self.rhs[u]
                for p in self.neighbors(u):
                    self.update_vertex(p)
            else:                             # underconsistent
                self.g[u] = INF
                for p in [u, *self.neighbors(u)]:
                    self.update_vertex(p)

    def next_step(self):
        return min(self.neighbors(self.start),
                   key=lambda s: self.cost(self.start, s) + self.g.get(s, INF))

    def move_and_sense(self, new_start, newly_blocked):
        self.km += self.h(self.start, new_start)
        self.start = new_start
        for cell in newly_blocked:
            self.blocked.add(cell)
            for v in [cell, *self.neighbors(cell)]:
                self.update_vertex(v)
        self.compute_shortest_path()

The driver loop is short: call compute_shortest_path() once, then repeatedly stop if rhs(start) is infinite (no path), take next_step(), move, and pass any newly sensed blocked cells to move_and_sense. Because the grid is undirected, predecessors and successors are the same neighbours. On a directed graph, update_vertex must use successors for rhs and the queue loop must update predecessors.

Worked example

The robot starts at S0 = (2, 0) with the goal G at (2, 6) on an empty map. The first search pops 7 nodes and sets g(S0) = 6, the straight-line distance. The robot moves to S1 = (2, 1), so km becomes h(S0, S1) = 1. Its sensors report blocked cells at (1, 2), (2, 2) and (3, 2). Updating those cells and their neighbours makes the nodes around the wall inconsistent. The repair pops 26 nodes, including underconsistent nodes whose old paths ran through the wall, and ends with g(S1) = 9: up one row twice, across, and back down, or the mirror route through row 4.

On a grid this small the repair costs more pops than the first search, because the wall invalidates most of the map. The benefit appears on large maps, where each discovery touches a small region and the rest of the solution is reused.

Performance, measured

To check that claim, the same code was run on a 100 x 100 grid with about 25 percent hidden obstacles. The robot senses a 5 x 5 window around itself and replans whenever it finds new obstacles. Over two solvable random maps it replanned 138 and 130 times. Total D* Lite queue pops, including the first search, were about 10,400 on each map.

Repeated A* from scratch on the same maps depended heavily on tie-breaking. When ties in f go to the node with the larger g, the usual recommendation on grids, it needed about 14,900 and 13,100 pops. When ties went to the smaller g, it needed about 470,000 and 402,000. So against a well-tuned A*, D* Lite saved about 30 percent of pops here, not orders of magnitude. Each D* Lite pop also costs more, because update_vertex scans neighbours to recompute rhs. The lesson is to benchmark on your own maps, sensor range and obstacle density before choosing. The gap grows with map size, long horizons and small, local discoveries, and shrinks when discoveries are large or close to the goal. For tie-breaking details see A* pathfinding.

Engineering choices that matter in practice:

• Queue. Lazy deletion is simple and fast. An indexed heap with decrease-key, as in the indexed priority queue article, keeps the heap smaller when nodes are re-keyed often.
• Optimised D* Lite. The paper's optimised version updates keys in place and avoids some redundant rhs recomputations. Use it once the basic version is tested.
• Cost increases versus decreases. Decreases only create overconsistent nodes and are cheap. Increases create underconsistent nodes, which may be processed twice.
• Large changes. If a single update touches a large fraction of the map, re-initialising and searching from scratch can be cheaper than repairing. Set a threshold.
• Diagonals. On 8-connected grids use octile costs and the octile heuristic, and decide whether diagonal moves may cut corners. The grid variants article covers both.

Failure modes

• Forgetting km. Keys go stale after a move, nodes come out in the wrong order, and the search stops too early with wrong g-values. The bug shows up only after several moves.
• Inconsistent heuristic. The km argument needs h to satisfy the triangle inequality. An inadmissible or inconsistent h gives suboptimal or wrong paths.
• Updating only the changed cell. When a cell becomes blocked, every edge into it changes, so its neighbours need update_vertex too.
• Using the heuristic towards the goal. The search is backwards, so h measures from the robot to s, not from s to the goal.
• Moving obstacles. D* Lite assumes the world changes rarely relative to planning. People and vehicles need a local planner or a time-aware method on top.
• No oracle test. Incremental code fails silently. Compare rhs(start) with a fresh BFS or Dijkstra on the known map after every replan in tests, as was done for the code above.

What to do next

1. Copy the implementation and add the oracle test: after every replan, compare rhs(start) with BFS on the known map over a few hundred random grids.
2. Benchmark it against repeated A* with large-g tie-breaking on maps like yours.
3. Add 8-connectivity with octile costs and decide your corner-cutting rule.
4. Add a threshold that falls back to a fresh search after very large map changes.
5. Move to an indexed heap or the optimised variant once profiling shows the queue is hot.
6. Read Koenig and Likhachev's D* Lite paper alongside your code; the pseudocode maps line by line.