寻路算法创建循环答案

【问题标题】：Pathfinding algorithm creating loops寻路算法创建循环
【发布时间】：2011-08-30 14:58:28
【问题描述】：

我已经实现了 D*-Lite 算法（这里是 description，这是一种在边缘成本随时间变化时进行寻路的算法），但是我在进行边缘成本更新时遇到了问题。它主要工作，但有时它会卡在一个循环中，在两个顶点之间来回移动。我正在尝试创建一个展示这种行为的测试用例，目前在大型应用程序中使用时会发生某些情况，这使得调试变得困难。

~~我会尽快建立一个测试用例，但也许有人可以立即发现我从伪到 C++ 所做的错误。~~（有一个测试用例包括在下面） 这篇文章提供了一个优化版本，图 4，这是我已经实现的版本。伪代码粘贴在下面。

我的实现的源代码是可用的here。

如果有帮助，我将在我的代码中使用这些类型：

struct VertexProperties { double x, y; };
typedef boost::adjacency_list<boost::vecS, 
                              boost::vecS, 
                              boost::undirectedS,
                              VertexProperties,
                              boost::property<boost::edge_weight_t, double> > Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef DStarEuclidianHeuristic<Graph, Vertex> Heuristic; 
typedef DStarPathfinder<Graph, Heuristic> DStarPathfinder;

如果需要更多关于使用的信息，请直接询问，粘贴太多了。

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'));
{35”}   Move to s_start;
{36”}   Scan graph for changed edge costs;
{37”}   if any edge costs changed
{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()

编辑：

我已经成功创建了一个显示错误行为的测试用例。将其与 pastebin 中的代码一起运行，它将在最后一次 get_path 调用中挂起，在节点 1 和 2 之间来回切换。在我看来，这是因为节点 3 从未被触及，所以就这样方式的成本是无限的。

#include <cmath>
#include <boost/graph/adjacency_list.hpp>
#include "dstar_search.h"

template <typename Graph, typename Vertex>
struct DStarEuclidianHeuristic {
    DStarEuclidianHeuristic(const Graph& G_) : G(G_) {}

    double operator()(const Vertex& u, const Vertex& v) {
        double dx = G[u].x - G[v].x;
        double dy = G[u].y - G[v].y;
        double len = sqrt(dx*dx+dy*dy);
        return len;
    }

    const Graph& G;
};

struct VertexProp {
    double x, y;
};

int main() {
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
        VertexProp, boost::property<boost::edge_weight_t, double> > Graph;
    typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef boost::graph_traits<Graph>::edge_descriptor Edge;
    typedef DStarEuclidianHeuristic<Graph, Vertex> Heur;

    typedef boost::property_map<Graph, boost::edge_weight_t>::type WMap;

    Graph g(7);
    WMap weights = boost::get(boost::edge_weight, g);
    Edge e;
    // Create a graph
    e = boost::add_edge(0, 1, g).first;
    weights[e] = sqrt(2.);
    e = boost::add_edge(1, 2, g).first;
    weights[e] = 1;
    e = boost::add_edge(2, 3, g).first;
    weights[e] = 1;
    e = boost::add_edge(1, 4, g).first;
    weights[e] = 1;
    e = boost::add_edge(3, 4, g).first;
    weights[e] = 1;
    e = boost::add_edge(3, 5, g).first;
    weights[e] = sqrt(2.);
    e = boost::add_edge(2, 6, g).first;
    weights[e] = sqrt(2.);
    e = boost::add_edge(5, 6, g).first;
    weights[e] = 1;
    e = boost::add_edge(6, 7, g).first;
    weights[e] = 1;
    g[0].x = 1; g[0].y = 0;
    g[1].x = 0; g[1].y = 1;
    g[2].x = 0; g[2].y = 2;
    g[3].x = 1; g[3].y = 2;
    g[4].x = 1; g[4].y = 1;
    g[5].x = 2; g[5].y = 3;
    g[6].x = 1; g[6].y = 3;
    g[7].x = 1; g[7].y = 4;

    DStarPathfinder<Graph, Heur> dstar(g, Heur(g), 0, 7);
    std::list<std::pair<Edge, double>> changes;

    auto a =  dstar.get_path(); // Find the initial path, works well
    std::copy(a.begin(), a.end(), std::ostream_iterator<Vertex>(std::cout, ","));
    // Now change the cost of going from 2->6, and try to find a new path
    changes.push_back(std::make_pair(boost::edge(2, 6, g).first, 4.));
    dstar.update(changes);
    a = dstar.get_path(); // Stuck in loop
    std::copy(a.begin(), a.end(), std::ostream_iterator<Vertex>(std::cout, ","));

    return 0;
}

编辑 2： 更多进展。如果我将ComputeShortestPath 中的while 循环中的中断条件仅替换为U != Ø（U 不为空），则找到路径！虽然它很慢，因为它总是检查图中的每个节点，这不应该是必需的。另外，由于我使用无向图，我在{40"} 中添加了一些代码来更新u 和v。

【问题讨论】：

使用下划线：know->k_new、gold->g_old 等。它使人类更容易解析代码 - 我的大脑看到“黄金”并想到金属！
在您回复 fred 的 cmets 中，您写道曼哈顿距离是 abs(dx+dy)。你的意思是abs(dx)+abs(dy)，对吧？
错误，是的。我什至不能纠正我的错误，可以吗？但是，正如我在那里也指出的那样，曼哈顿距离大于（或等于）欧几里得距离。它起作用的唯一原因是因为我第一次完全忘记了abs，所以取消了。还是谢谢！

标签： c++ algorithm d-star

【解决方案1】：

您的代码至少存在两个问题（不包括 typenames，我必须在 std::vector<TemplateParameter>::iterator 之类的构造之前添加才能编译它）。

您正在使用不允许的启发式算法，因为对角线的成本为 1，但长度为 √2。这可以防止第二次调用 ComputeShortestPath 做任何事情。
您正在使用的堆的更新方法（按照约定是 Boost 私有的，因此显然没有记录）仅支持减少密钥。 D* Lite 也需要增加密钥。

【讨论】：

感谢您的反馈！我改变了对角线遍历的成本，这是思维失误的结果。我还阅读了 d-ary 堆，确实它只允许减少。我更改为boost::relaxed_heap，它（我认为，它也是无证的......）允许增加和减少。但是，它仍然不起作用。如果我改为曼哈顿启发式（dx+dy），它适用于这种情况，但不是我真正的问题。为什么会有帮助？根据论文，启发式必须是非负的，低于成本，并且满足三角形 ineq。我认为我的欧几里得距离呢？
更正，它不适用于正确实现的曼哈顿距离，即abs(dx+dy)（无论如何应该比欧几里得距离更长，不知道我在想什么）
事实证明，一个问题是我在“真实”问题中也有不可接受的启发式方法（它使用了动态加权，在某些情况下，乘法因子可能会降至 1 以下，导致不可接受）。如果已解决，则“非优化”版本实现工作，但仍然不是我上面链接的那个。但是，如果没有人弄清楚优化版本有什么问题，这个答案将得到赏金，因为它确实为我指明了正确的方向

【解决方案2】：

不幸的是，在这里发布伪代码并不是很有用，因为伪代码可能是正确的，但实际的实现可能有问题。

通常，在寻路算法中，如果您在节点之间循环，那么该算法很有可能不会从潜在路径节点集中删除访问过的节点。这通常是通过在遍历节点时在节点上设置一个标志来完成的，并在您返回搜索树时重置该标志。

【讨论】：

flag 方法对于 MT 使用来说并不是真正可扩展的，在这种情况下，您更愿意保留一组指向节点的指针，而不是“标记”节点。
@Skizz：我很确定这是错误的实现;)。我将它作为一个 pastebin 链接到它，在这里包含它有点太长了。我包含了伪代码，这样人们就不必从文章中挖掘出来了。
关于循环，算法计算到一些节点的距离，直到找到目标（由启发式引导）。当它试图通过遵循计算出的最短路径来提取路径时，就会出现问题，因为由于某种原因，某个节点具有一个节点中最便宜的替代方案，而该节点又是那个节点中最便宜的......（这个解释是' t 非常好，如果你有时间，这篇文章会做得更好）

【解决方案3】：

问题出在 UpdateVertex 函数中。

伪代码是在假设比较是在整数上的情况下编写的（它们在论文中）。在您的实现中，您正在对浮点值进行比较。如果您要处理非整数成本，则需要添加容差。

您可以通过使用 -Wfloat-equal（甚至更好的 -Werror=float-equal）进行编译，在 GCC 上对此进行测试

【讨论】：

【解决方案4】：

我也遇到了同样的问题。我想我找到了原因，也许在这个时候你找到了解决问题的方法，可以给我一些提示。

我认为问题来自U 列表。

因为可能每个顶点的某些键的值高于s_start 的键。所以 ComputeKey(s)<ComputeKeu(s_start) 不满足（ComputePath 中 while 的第一个条件），第二个条件 rhs(s_start)>g(s_start) 不满足，因为当您沿着路径移动时，您会通过正在保持一致的单元格。

那么当这两个条件不满足while stop时，程序就停止扩展新的单元格。

当您计算路径时，沿路径连续使用使g(s)+c(u,s) 最小化的路径，您最终会在一个单元格上仍然具有无限g 成本（因为它在while 循环中没有扩展） .

这就是如果你改变条件，使用 while U!=0 算法工作的原因，这会强制程序扩展 U 列表中的所有顶点。（但你肯定失去了动态算法的优势）。

现在我希望我已经帮助了你，如果你不再需要这个帮助，也许你可以帮助我。

【讨论】：

【解决方案5】：

我自己在实现 D* Lite（普通版） 和 优化版 时遇到了这个问题。我有点不确定为什么它首先会发生，但我似乎被一些障碍物（十字形或高垂直障碍物）的排列触发，其中算法突然无法探索更多选项并最终在两者之间来回跳跃无限循环中的两个选项。我已经在早先的here 和我如何绕过无限循环问题上创建了一个帖子，但代价是算法可能会变慢一些。

【讨论】：