My crude implementation of D* Lite in C++.
诸行无常，诸法无我，涅槃寂静。
There are two brutal truths that I learned in my early adult life:
1. The world abounds with differences and changes, although they are often abstracted away by our minds;
2. The so-called “free will”, just as many other concepts in the mental world, doesn’t exist in reality.
I know the second one is rather controversial and I will get into that in another post. As an implication of the first one, we know that we can never make perfect plans. For one thing, our plans are based on our limited knowledge of the objective world at a certain time and may overlook facts that turn out to be relevant. For another, the world is constantly changing and the “truth” we know may soon become outdated.
Thus one should always embrace differences and changes. On the one hand, one needs to eagerly dive into practical problems, special circumstances and concrete instances of a generic concept, instead of resting on abstract models and general theories. In this way we could get a more thorough understanding about the circumstances and get to make more robust plans. On the other hand, one should always expect changes and quickly adapt to new circumstances as changes happen. By doing so we will lend ourselves to a state of anti-fragility, such that we don’t just survive unexpected challenges but also thrive in them.
That also applies to path planning. Apologies for the philosophical gibberish so far… But changes in circumstances motivate incremental search algorithms, the theme of this post. One of the most popular incremental search algorithms is D* Lite¹, which is based on Lifelong Planning A* (LPA*) designed by the same authors earlier. It leverages the idea of heuristic search to update the path that an online robot is actively following as information about graph changes come in. As a combinatorial planning algorithm, D* Lite is guaranteed to give optimal solutions, as opposed to sampling-based alternatives like RRT. Despite being named similarly, D* Lite is algorithmically different from D*, an earlier pioneer of incremental planning. Performance-wise D* Lite is as good as D*, but implementation-wise D* Lite is far more concise.
Graph changes occur in two different ways. Occasionally, we get some delightful surprise — the cost of an edge may decrease, or two previously nonadjacent vertices may start to connect. If that’s the case, we only need to maintain the cost-to-go values of all the vertices (i.e. the distance from each vertex to the goal vertex) and expand impacted vertices in the increasing order of cost-to-go after we “relax” the updated edges.
However, a more common case would be that a robot following a certain route meets some unexpected blockage, meaning the cost of some edge goes up or an edge no longer exists. In such a circumstance, it’s not so obvious what vertices need to be updated and in what order they should be updated. D* Lite addresses this problem by maintaining both g values (i.e. the cost-to-go function) and rhs values — one-step lookahead values based on the g values of successors and updated edge costs — and using both of them to determine the priority of a vertex in the queue. Besides, the heuristic value also comes into play so that the search is more guided towards the vertex where the robot currently stays. For further details, please refer to the original paper.
Last Sunday I spent an evening implementing this algorithm for the first time. I just want to use this post to share my premature code (before I dump it into garbage). The caveat is that I only tested it with a few simple cases and it will surely have rough edges. Offers of testing and bug reports would be appreciated.
To begin with, I wrote a min-priority queue based on the binary heap. This offers O(log n)-time mutations (enqueue, dequeue, update and removal) as well as O(1)-time query for the key with the minimum value.
Then I implemented D* Lite in a class template called Planner. The replan method is where the incremental planning takes place. Note that I made the heuristic estimator as a type parameter of Planner so that I could inject a mock dependency in my tests. I assume the estimator would somehow give us the heuristic distance between two vertices given that it knows the information of the graph.
