D* Lite an incremental version of A* for navigating in unknown terrain Focussed Dynamic A* It implements the same behavior as Stentz’ Focussed Dynamic A* but is algorithmically different.

Mars Rover

How to search efficiently using heuristic to guide the search How to search efficiently by using re-using information from previous search results Incremental search + heuristic search

Stentz 1995 A* Clever heuristic method that achieves a speedup of one to two orders of magnitudes over repeated A* searches The improvement is achieved by modifying previous search results locally Extensively used on real robots, including outdoor high mobility multi-wheeled vehicle (HMMWV) Integrated into Mars Rover prototypes and tactical mobile robot prototypes for urban reconnaissance Focussed Dynamic A* (D*)

D* Lite D* D* Lite implements the same navigation strategy as D*, but is algorithmically different D* Substantially shorter than D* Uses only one tie-breaking criterion when comparing priorities (simplified maintenance) No nested if statements with complex conditions Simplifies the analysis of program flow Easier to extend D* At least as efficient as D* D* Lite vs. D*

Previously, we learned LPA* original eight-connected gridworld LPA* S start LPA* repeatedly determines shortest paths between S start and S goal as the edge costs of a graph change.

Path Planning original eight-connected gridworld LPA* S start LPA* repeatedly determines shortest paths between S start and S goal as the edge costs of a graph change.

Path Planning changed eight-connected gridworld LPA* S start LPA* repeatedly determines shortest paths between S start and S goal as the edge costs of a graph change.

Path Planning changed eight-connected gridworld LPA* S start LPA* repeatedly determines shortest paths between S start and S goal as the edge costs of a graph change.

LPA* LPA*A* A* LPA* is an incremental version of A* that applies to the same finite path-planning problems as A*. A* heuristics h(s) s It shares with A* the fact that it uses non-negative and consistent heuristics h(s) that approximate the goal distances of the vertices s to focus its search. Consistent heuristics obey the triangle inequality: h(s goal ) = 0 – h(s goal ) = 0 – h(s) ≤ c(s, s’) + h(s’)s – h(s) ≤ c(s, s’) + h(s’); for all vertices s ∈ S and sss s’ ∈ succ(s) with s ≠ s goal.

D* Lite LPA* S start LPA* repeatedly determines shortest paths between S start and S goal as the edge costs of a graph change. D* Lite S current D* Lite repeatedly determines shortest paths between the current vertex S current of the robot and S goal as the edge costs of a graph change, while the robot moves towards S goal.

D* Lite LPA* S start LPA* repeatedly determines shortest paths between S start and S goal as the edge costs of a graph change. D* Lite S current D* Lite repeatedly determines shortest paths between the current vertex S current of the robot and S goal as the edge costs of a graph change, while the robot moves towards S goal. D* Lite goal-directed navigation problems unknown terrains D* Lite is suitable for solving goal-directed navigation problems in unknown terrains.

Free space Assumption

Free space assumption Move the robot on a shortest potentially unblocked path towards the goal.

Using the free space assumption in path planning Search from the Goal towards the robot’s current location -This allows one to re-use parts of the search tree after the robot has moved. -This allows one to use heuristics to focus the search; thereby not requiring to search the entire graph.

Using the free space assumption in path planning Search from the Goal towards the robot’s current location incremental search efficient -This makes incremental search efficient.

Variables S S finite set of vertices successors set of successors of s predecessors set of predecessors of s Cost of moving from vertex s to vertex s’ Start vertex Start vertex – has no predecessors Goal vertex Goal vertex – has no successors

LPA* Variables Start distance shortest path Start distance = length of the shortest path from S start to S g(s) = estimate of the Start distance g*(s) g(s) = estimate of the Start distance g*(s)

D* Lite Variables Goal distance shortest path S goal Goal distance = length of the shortest path from S to S goal g(s) = estimate of the Goal distance g*(s) g(s) = estimate of the Goal distance g*(s)

LPA* Rhs-value rhs-values The rhs-values are one-step look-ahead values, based on the g-values; and thus, potentially better informed than the g-values

D* Lite Rhs-value rhs-values The rhs-values are one-step look-ahead values, based on the g-values; and thus, potentially better informed than the g-values

D* Lite Rhs-value locally consistent g-value = rhs-value: cell is locally consistent locally inconsistent g-value ≠ rhs-value: cell is locally inconsistent locally overconsistent g-value > rhs-value: cell is locally overconsistent locally underconsistent g-value < rhs-value: cell is locally underconsistent priority queue the priority queue contains exactly the locally inconsistent vertices km their priority is [min(g(s),rhs(s)) + h(s,s start ) + km; min(g(s),rhs(s))] smaller priorities first, according to a lexicographic ordering rhs-values The rhs-values are one-step look-ahead values, based on the g-values and thus potentially better informed than the g-values

Shortest Path alllocally consistent If all vertices are locally consistent, – g(s) == g*(s) ; for all vertices s S goal – one can trace back the shortest path from S start to S goal. From vertex s, find a successor s’ that minimises g(s’) + c(s, s’). Ties can be broken arbitrarily. S goal Repeat until S goal is reached. S goal from S start to S goal

Selective Update locally consistent D*Lite does not make all vertices locally consistent after some edge costs have changed heuristics It uses heuristics to focus the search g-values It updates only the g-values that are relevant for computing the shortest path

Priority priority queue keeping track of locally inconsistent vertices D*Lite maintains a priority queue for keeping track of locally inconsistent vertices – vertices that potentially needs their g-values updated to make them locally consistent Priority Priority of a vertex = key Key – vector with 2 components k(s)k 1 (s)k 2 (s) k(s) = [ k 1 (s); k 2 (s) ] k 1 (s)s start km k 1 (s) = min(g(s), rhs(s)) + h(s, s start ) + km k 2 (s) k 2 (s) = min(g(s), rhs(s))

Priority Priority of a vertex = key Key – vector with 2 components k(s)k 1 (s)k 2 (s) k(s) = [ k 1 (s); k 2 (s) ] k 1 (s)s start km k 1 (s) = min(g(s), rhs(s)) + + h(s, s start ) + km k 2 (s) k 2 (s) = min(g(s), rhs(s)) Key comparison Key comparison (lexicographic ordering): either(k 1 (s) <k 1 ‘(s) ) k(s) ≤ k’(s) iff either ( k 1 (s) < k 1 ‘(s) ) or(k 1 (s) ==k 1 ‘(s) ) and or( k 1 (s) == k 1 ‘(s) ) and (k 2 (s) ≤k 2 ‘(s) ) ( k 2 (s) ≤ k 2 ‘(s) ) smallest key The vertex with the smallest key is expanded first by D*Lite.

Heuristic Function (8-connected Gridworld) distance between two cells As an approximation of the distance between two cells, we use the maximum of the absolute differences of their x and y coordinates. eight-connected gridworlds These heuristics are for eight-connected gridworlds what Manhattan distances are for four-connected gridworlds. s goal goal Lifelong Planning A* h(s, s goal ) – calculated relative to the goal position s start start D*Lite h(s, s start ) – calculated relative to the start position

The pseudocode uses the following functions to manage the priority queue: U.TopKey() U.TopKey() - returns the smallest priority of all vertices in priority queue U. (If U is empty, then U.TopKey() returns [∞; ∞].) U.Pop() U.Pop() - deletes the vertex with the smallest priority in priority queue U and returns the vertex. U.Insert(s; k) U.Insert(s; k) - inserts vertex s into priority queue U with priority k. Update(s; k) 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.) U.Remove(s) U.Remove(s) - removes vertex s from priority queue U. Priority Queue Management

LPA* Pseudocode (just for comparison)

D* Lite Pseudocode gets satisfied when one of the edge costs change

Route-planning example: 4-connected gridworld Example, Step 1

Route-planning example: 4-connected gridworld Example, Step 2

Route-planning example: 4-connected gridworld Example, Step 3

Route-planning example: 4-connected gridworld Example, Step 4

Route-planning example: 4-connected gridworld Example, Step 5

After one cell gets blocked… C3 Blocked cell: C3 The previous search results are carried over to help solve this re-planning problem.

After one cell gets blocked… C3 Blocked cell: C3 Neighbours of C3: B3, C4, D3, C2 B3C2 ∞ Among the neighbours, Cells B3 & C2 calculates their rhs-values based on C3; therefore, set their g-values to ∞

Route-planning example: 4-connected gridworld Example, Step 6

Route-planning example: 4-connected gridworld Example, Step 7

Route-planning example: 4-connected gridworld Example, Step 8

Route-planning example: 4-connected gridworld Example, Step 9

Route-planning example: 4-connected gridworld Example, Step 10

Route-planning example: 4-connected gridworld Example, Step 11

Route-planning example: 4-connected gridworld Example, Step 12

Simulation Example D* Lite operating in a 4-Connected Gridworld km demonstration of the use of the km variable

Route-planning example #2: 4-connected gridworld Example, Step 1

Route-planning example #2: 4-connected gridworld Example, Step 2

Route-planning example #2: 4-connected gridworld Example, Step 3

Route-planning example #2: 4-connected gridworld Example, Step 4

Route-planning example #2: 4-connected gridworld Example, Step 5

C3 Cell C3 gets blocked…

Route-planning example #2: 4-connected gridworld Example, Step 6

Route-planning example #2: 4-connected gridworld Example, Step 7

Route-planning example #2: 4-connected gridworld Example, Step 8

Route-planning example #2: 4-connected gridworld Example, Step 9

Route-planning example #2: 4-connected gridworld Example, Step 10

Route-planning example #2: 4-connected gridworld Example, Step 11

Route-planning example #2: 4-connected gridworld Example, Step 12

Route-planning example #2: 4-connected gridworld Example, Step 13

Route-planning example #2: 4-connected gridworld Example, Step 14

Route-planning example #2: 4-connected gridworld Example, Step 15

Route-planning example #2: 4-connected gridworld Example, Step 16

Conclusions D* Lite D* Lite is a fast re-planning method for robot navigation in unknown terrain. It implements the same navigation strategies as D*, and is at least as efficient as D*.

References A complete navigation system for goal acquisition in unknown environments, Anthony Stentz and Martial Hebert A complete navigation system for goal acquisition in unknown environmentsAnthony StentzMartial Hebert