M3-T6 — Replanning: D* Lite and LPA*

Learning objectives

After this lesson, you should be able to:

• Explain why searching from the goal gives a cost-to-go that is reusable when the robot moves.
• Define g and rhs values and the terms consistent, overconsistent and underconsistent.
• Trace how a cost increase invalidates cells and how the repair restores them.
• Say when replanning from scratch is the better choice.

The intuition

Think of a water-flow map of the hospital: for every cell, the time water poured at the pharmacy would take to reach it. Scout follows the map downhill. When a door closes, the water cannot flow through it. Most cells are unaffected: their water still arrives by the same route. Only cells whose every shortest route went through the door must be recomputed, and the new values spread in from the cells whose values are still true.

This is what incremental heuristic search does. It keeps two numbers per cell. One is the value the cell was given. The other is the value its neighbours now imply. Where they agree, nothing needs doing; where they differ, the cell is queued for repair. A closed door makes only a few cells disagree, and the search touches only those.

The mathematics

Search from the goal

If the search starts at the goal and works outwards, the value it gives each cell is the cost to the goal: a cost-to-go field. The robot follows it downhill from wherever it is, and when the robot moves, the field stays valid; only the place where the robot reads it changes. A search from the start would have to be redone whenever the start moves. This is why D* Lite searches backwards.

LPA*: g and rhs

Lifelong Planning A* (LPA*) keeps, for each node s, two values:

g(s)

the current estimate of the cost from s to the goal, from the previous search.

rhs(s)

a one-step look-ahead: 0 for the goal, otherwise the minimum over neighbours s′ of c(s, s′) + g(s′).

Only inconsistent nodes are kept in the priority queue. A change to the map, such as a closed door (an edge cost rising to infinity), recomputes rhs for the nodes at its ends and queues them if they are inconsistent. Processing the queue then repairs g:

1. An underconsistent node has a g that is too low. Set its g to infinity, which makes it overconsistent or consistent, and update the rhs values of its neighbours, which queues those that depended on it.
2. An overconsistent node has a g that is too high. Set g = rhs, which makes it consistent, and update the rhs values of its neighbours: this spreads the cheaper cost outwards.
3. Repeat until no inconsistent node has a key smaller than the key of the start. Nodes are ordered by the A* key [min(g, rhs) + h, min(g, rhs)], so the search is directed and stops early.

D* Lite

D* Lite applies LPA* backwards from the goal to a robot that moves and discovers new obstacles as it goes. Because the start changes, the heuristic values in the queue become stale; D* Lite adds an offset km, increased by the distance moved, to the keys so that old queue entries remain correctly ordered without re-sorting the queue. It produces the same paths as D* (Stentz, 1994) and is simpler to implement. When the robot senses that an edge cost changed, it updates the nodes at that edge's ends and calls the repair.

What the repair touches

Only nodes whose cost-to-go actually changes, and their neighbours, are processed. In the lab (M3-L4) this is the set of cells that lose every neighbour one step closer to the goal.

A worked example

A ring corridor. A three-row, seven-column corridor has a solid block in the middle row (columns 1 to 5). The goal is at the bottom middle, cell (2,3). Steps cost 1 and moves are up, down, left and right. The field before and after the cell (2,2) next to the goal closes:

before                              after (2,2) closed
 5  6  7  8  7  6  5               11 10  9  8  7  6  5
 4  #  #  #  #  #  4               12  #  #  #  #  #  4
 3  2  1  0  1  2  3               13 14  ×  0  1  2  3

Cell (2,1) had g = 2 and rhs = 1 + g(2,2). With (2,2) closed its rhs is 1 + g(2,0) = 4, higher than g: it is underconsistent. It is set to infinity, which makes (2,0) underconsistent (rhs = 1 + min(g(1,0), g(2,1)) = 5 against g = 3), then (1,0), (0,0), (0,1) and (0,2) in turn, each losing the neighbour it relied on. The cell (0,3) has g = 8 and is not touched, because its neighbour (0,4) with g = 7 supports it. The repair then gives the six cells new values from the cells whose values are unaffected: (0,2) takes 1 + g(0,3) = 9, then (0,1) = 10, (0,0) = 11, (1,0) = 12, (2,0) = 13 and (2,1) = 14.

Six of the 16 free cells changed. The other nine, plus the closed cell, are never recomputed. Scout, standing at (0,0), reads 11: the route now runs right over the top, down the right-hand side and back along the bottom, 11 steps instead of 5.

At building scale. On a floor of 240,000 cells, a door that closes in a side corridor might change a few thousand cells. Suppose 1,500 change (0.6%). If each repaired cell costs three times as much work as a cell in a fresh search because of the priority queue (an assumed figure), the repair does 4,500 units of work against 240,000: about fifty times less. If instead a closed main corridor changes 60% of the cells, the repair does more work than a fresh search, which is why the choice depends on the change.

Engineering implications and limitations

When to replan from scratch

Incremental search wins when changes are small, local and frequent, and the map is large. It loses when changes are big, when the planner is cheap anyway, or when the cost of keeping the extra state (g, rhs and a queue for every visited cell) matters. Many navigation stacks simply call the global planner again at a fixed rate; the standard Nav2 behaviour tree does so once per second (check the version you use), which is simple, robust and good enough on moderate maps. If the planner takes longer than the replanning period, or the map is very large, incremental search is worth the complexity.

Costs, not only closures

LPA* handles any edge cost change, increases and decreases. A decrease (a door opens) makes nodes overconsistent and spreads cheaper values; an increase makes nodes underconsistent, which is the more expensive case because values must first be invalidated. The lab covers the closure case.

Replan on change, not on a timer alone

Triggering a repair when the costmap reports a changed cell on the current path avoids work in a quiet corridor and reacts at once to a closed door. Combine it with a slow periodic replan as a safety net for changes you did not model.

Hysteresis for the human

A new plan that differs only slightly from the old one can make the robot dither between two routes of nearly equal cost. Keep the old route unless the new one is better by a margin, and record why the planner changed its mind.

Common misconceptions

Engineering takeaways

1. Searching from the goal gives a cost-to-go field that stays valid as the robot moves.
2. LPA* keeps g and rhs per node; only nodes where they differ are queued and repaired.
3. A cost increase makes nodes underconsistent and must be invalidated first; a decrease makes them overconsistent and spreads outwards.
4. D* Lite is LPA* searching backwards with a key offset km, so a moving robot can reuse its queue.
5. Replan from scratch when the change is large or the planner is cheap; use incremental search when changes are small and the map is large.

References and further reading

The papers behind LPA*, D* and D* Lite.

• S. Koenig, M. Likhachev and D. Furcy, “Lifelong Planning A*”, Artificial Intelligence, 155(1-2), 93-146, 2004.
• S. Koenig and M. Likhachev, “D* Lite”, AAAI Conference on Artificial Intelligence, 476-483, 2002.
• A. Stentz, “Optimal and Efficient Path Planning for Partially-Known Environments”, IEEE International Conference on Robotics and Automation, 3310-3317, 1994.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Next: planning on a costmap, where the cost of a step depends on the cells it passes, and a path must be usable by a controller.

Related content

Nothing below is required to finish this lesson.

• Before
  M3-T5: Any-Angle and Fast Search

  Explain how Theta*, jump-point search and bidirectional search reduce zig-zag paths or expansions, and when each helps.

• Lab
  M3-L4: Replan When the Corridor Closes

  A door closes mid-route: update the plan without searching the whole building again.