Algorithm 机器人路径规划的D*Lite搜索算法陷入无限循环。为什么我的修复工作，它是任何慢？

对于一个与我一起工作的机器人项目，我想使用论文中的D*Lite（优化版）动态路径规划用于不断变化的占用率网格/成本图。如本文所述，D*Lite搜索算法的思想是，它基本上是从目标开始反向运行一个*搜索，然后尝试返回到起点。然后，解算器给出当前解决方案，并等待其显示的权重或障碍物发生某种变化。与重复A*搜索相反，D*Lite算法避免从头开始重新规划，并逐步修复路径，使其修改保持在机器人姿势附近的局部位置

我的问题

我已经在python中实现了该算法，并在pygame中进行了仿真，以测试其性能。但是我有一个关于伪代码的问题。我已经实现了算法，包括优化版本和非优化版本，现在已经实现了三次，但我仍然发现，当算法遇到一些障碍物（十字形或大型垂直障碍物）时，算法突然卡在while循环内的无限循环中（主程序，第32行伪代码），并在顶点s_开始的两个选项之间来回移动（过程主线，第34行）。我已经将我的python实现与伪代码进行了多次比较，但我无法找到任何可能导致此错误的伪代码偏差

我的临时“修复”

现在，为了避免算法陷入无限循环，我将过程Main（）中第48行的computeShortestPath（）缩进到左边，这样就超出了过程Main（）中第37行的if的范围

当我这样做的时候，算法几乎总是能够在发现新的障碍物时计算出一条新的最短路径

我的问题

首先，有人知道为什么算法有时会陷入无限while循环，以及如何修复它吗

我想我的临时“修复”会再次认为该算法的计算成本更高，因此D*Lite算法的许多用途现在都消失了。与原始算法相比，缩进修复的计算复杂度是多少

现在有了我的临时修正，代码与动态a*相比，在计算方面是否有任何不同，在动态a*中，每次必须重新规划新路径时，都要重新计算所有内容

有时，当遇到大型复杂迷宫时，需要对新顶点进行大量探索，我的“临时修复”代码有时会变得有点慢。但在某种程度上，即使在原始代码中也会发生这种情况吗

我的实施

如果您想自己运行代码并体验算法，可以通过运行python main.py来测试我的实现

这个特定的实现包含了我的“临时修复”。但是如果你不想体验它，你可以转到d_star_lite.py中的第156行，将计算最短路径（）缩进右边的一个，这样它就进入了第132行的“if”中

伪代码

这是D*Lite（优化版）算法的原始伪代码

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

过程计算键
{01}返回[min（g（s），rhs（s））+h（s_起点，s）+km；min（g（s），rhs（s））]；
过程初始化（）
{02}U=∅;
{03}km=0；
{04“}适用于所有s∈ S rhs（S）=g（S）=∞;
{05}rhs（s_目标）=0；
{06”}U.Insert（s_目标，[h（s_开始，s_目标）；0]）；
程序UpdateVertex（u）
{07}如果（g（u）！=rhs（u）和u∈ U） 更新（U，计算键（U））；
{08”}如果（g（u）！=rhs（u）和u/∈ U） U.插入（U，计算键（U））；
{09}如果（g（u）=rhs（u）和u∈ U） 移除（U）；
过程计算ShortestPath（）
{10}while（U.TopKey（）g（s_start））
{11”}u=u.Top（）；
{12”}k_old=U.TopKey（）；
{13”}k_new=CalculateKey（u））；
{14}如果（k_旧rhs（u））
{17}g（u）=rhs（u）；
{18}U.删除（U）；
{19}适用于所有s∈ Pred（美国）
{20”}如果（s！=s_目标）rhs（s）=min（rhs（s），c（s，u）+g（u））；
{21}UpdateVertex（s）；
{22}其他
{23}g_old=g（u）；
{24}g（u）=∞;
{25}适用于所有s∈ Pred（美国）∪ {u}
{26}如果（rhs（s）=c（s，u）+g_old）
{27}如果（s！=s_目标）rhs（s）=min s'∈（c（s，s′）+g（s′）；
{28}UpdateVertex（s）；
主程序（）
{29”}s_last=s_start；
{30}初始化（）；
{31}计算短路径（）；
{32}while（s_开始！=s_目标）
{33}/*如果（g（s_开始）=∞) 那么就没有已知的路径了*/
{34”}s_start=argmin s'∈成功（SU启动）（c（SU启动，s'）+g（s'））；