1. n.h.reyes@massey.ac.nz 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. n.h.reyes@massey.ac.nz

2. Mars Rover

3. 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

4. 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*)

5. 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*

6. 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.

7. 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.

8. 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.

9. 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.

10. 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.

11. 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.

12. 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.

13. Free space Assumption

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

18. 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.

19. 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.

20. 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

21. 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)

22. 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)

23. 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

24. 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

25. 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

26. 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

27. 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

28. 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))

29. 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.

30. 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

31. 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

32. LPA* Pseudocode (just for comparison)

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

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

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

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

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

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

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

41. 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 ∞

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

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

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

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

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

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

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

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

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

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

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

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

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

55. C3 Cell C3 gets blocked…

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

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

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

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

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

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

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

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

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

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

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

67. 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*.

68. References http://idm-lab.org/publications.html http://idm-lab.org/project-a.html http://homepages.dcc.ufmg.br/~lhrios/applet_d_lite/index.html 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