问题
For a robotics project I've been working with I want to use the D* Lite (optimized version) from paper Koenig, 2002 for dynamic path planning for a changing occupancy grid / cost map. The idea of the D* Lite search algorithm, as described in the paper, is that it works by basically running A* search in reverse starting from the goal and attempting to work back to the start. The solver then gives out the current solution and waits for some kind of change in the weights or obstacles that it is presented with. As opposed to repeated A* search, the D* Lite algorithm avoids replanning from scratch and incrementally repair path keeping its modifications local around robot pose.
My problem
I have implemented the algorithm in python with simulation in pygame to test the performance. But I have an issue related to the pseudo code. I have implemented the algorithm, both the optimized version and the non-optimized version, now three times and I still get the bug that when the algorithm face some configuration of obstacles (cross shaped or large vertical obstacles) the algorithm suddenly get stuck in a infinite loop inside the while-loop (Procedure Main, line 32 pseudo code), and goes back and forth between two options for the vertex s_start (Procedure Main, line 34). I have compared my python implementation to the pseudo code several times now, and I am unable to find any deviation from the pseudo code that could cause this bug.
My temporary "fix"
Now, to avoid the algorithm getting stuck in a infinite loop, I indented the computeShortestPath() at line 48 in Procedure Main() one to the left, so that I goes outside the scope of the if at line 37 in Procedure Main().
When I do this, the algorithm is almost always able to compute a new shortest path when discovering new obstacles in it's path.
My questions
First of all, anyone have any idea why the algorithm sometimes get stuck in an infinite while loop and how to fix it?
I guess my temporary "fix" will deem the algorithm computationally more expensive again, and therefore a lot of the purpose of the D* Lite algorithm is now gone. What is the computational complexity of my indentation fix vs the original algorithm?
Now with my temporary fix, is the code actually any different computation wise as compared to a dynamic A* where you recompute everything for each time you have to replan a new path?
Sometimes when encountering large complex mazes when there is a lot of exploration of new vertices to be done, my "temporary fixed" code sometimes get kinda slow. But wouldn't that also have happend even in the original code to some degree?
My implementation
If you would like to run the code and experience the algorithm yourself, you can test my implementation here by running python main.py.
This particular implementation has my "temporary fix" included. But if you want to experience it without, you can go to line 156 in d_star_lite.py and indent compute_shortest_path() one to the right, so that it goes inside the 'if' at line 132.
Pseudo code
Here is the original pseudo code for the D* Lite (optimized version) algorithm.
procedure CalculateKey(s)
{01”} return [min(g(s), rhs(s)) + h(s_start, s) + km;min(g(s), rhs(s))];
procedure Initialize()
{02”} U = ∅;
{03”} km = 0;
{04”} for all s ∈ S rhs(s) = g(s) = ∞;
{05”} rhs(s_goal) = 0;
{06”} U.Insert(s_goal, [h(s_start, s_goal); 0]);
procedure UpdateVertex(u)
{07”} if (g(u) != rhs(u) AND u ∈ U) U.Update(u,CalculateKey(u));
{08”} else if (g(u) != rhs(u) AND u /∈ U) U.Insert(u,CalculateKey(u));
{09”} else if (g(u) = rhs(u) AND u ∈ U) U.Remove(u);
procedure ComputeShortestPath()
{10”} while (U.TopKey() < CalculateKey(s_start) OR rhs(s_start) > g(s_start))
{11”} u = U.Top();
{12”} k_old = U.TopKey();
{13”} k_new = CalculateKey(u));
{14”} if(k_old < k_new)
{15”} U.Update(u, k_new);
{16”} else if (g(u) > rhs(u))
{17”} g(u) = rhs(u);
{18”} U.Remove(u);
{19”} for all s ∈ Pred(u)
{20”} if (s != s_goal) rhs(s) = min(rhs(s), c(s, u) + g(u));
{21”} UpdateVertex(s);
{22”} else
{23”} g_old = g(u);
{24”} g(u) = ∞;
{25”} for all s ∈ Pred(u) ∪ {u}
{26”} if (rhs(s) = c(s, u) + g_old)
{27”} if (s != s_goal) rhs(s) = min s'∈Succ(s)(c(s, s') + g(s'));
{28”} UpdateVertex(s);
procedure Main()
{29”} s_last = s_start;
{30”} Initialize();
{31”} ComputeShortestPath();
{32”} while (s_start != s_goal)
{33”} /* if (g(s_start) = ∞) then there is no known path */
{34”} s_start = argmin s'∈Succ(s_start)(c(s_start, s') + g(s')); <--- ** jumps between two solutions of s_start**
{35”} Move to s_start;
{36”} Scan graph for changed edge costs;
{37”} if any edge costs changed <--- ** this if ****
{38”} km = km + h(s_last, s_start);
{39”} s_last = s_start;
{40”} for all directed edges (u, v) with changed edge costs
{41”} c_old = c(u, v);
{42”} Update the edge cost c(u, v);
{43”} if (c_old > c(u, v))
{44”} if (u != s_goal) rhs(u) = min(rhs(u), c(u, v) + g(v));
{45”} else if (rhs(u) = c_old + g(v))
{46”} if (u != s_goal) rhs(u) = min s'∈Succ(u)(c(u, s') + g(s'));
{47”} UpdateVertex(u);
{48”} ComputeShortestPath() <--- ** this calculation **
来源：https://stackoverflow.com/questions/61467693/d-lite-search-algorithm-for-robot-path-planning-gets-stuck-in-infinite-loop-wh