路径查找算法创建循环



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

我会尽快得到一个测试用例,但也许有人可以立即发现我从pseudo到C++所做的错误 (下面包含一个测试用例(本文提供了一个优化版本,图4,这就是我实现的版本。伪代码粘贴在下面。

我的实现源代码可在此处获得。

如果有帮助的话,我会在代码中使用以下类型:

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版:更多进展。如果我将ComputeShortestPathwhile循环中的中断条件仅替换为U != Ø(U不为空(,则会找到路径!不过,它的速度相当慢,因为它总是检查图中的每个节点,而这并不必要。此外,由于我使用无向图,我在{40"}中添加了一些代码来更新uv

您的代码至少有两个问题(不包括typename,我必须为std::vector<TemplateParameter>::iterator等结构做准备才能编译它(。

  1. 您使用的是不可接受的启发式,因为对角线的成本为1,但长度为√2。这防止了对CCD_ 11的第二次调用执行任何操作。

  2. 您正在使用的堆的更新方法(根据约定,它对Boost是私有的,因此显然没有记录(只支持密钥减少。D*Lite也需要增加关键点。

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

一般来说,在路径查找算法中,如果你在节点之间循环,那么算法很有可能没有从潜在的路由节点集中删除访问节点。这通常是通过在遍历节点时在节点上设置标志来完成的,并在返回搜索树时重置标志。

问题出在UpdateVertex函数中。

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

您可以在GCC上测试这一点,方法是使用-Wfloat equal(甚至更好的-Werror=float equale(

我也遇到了同样的问题。我想我已经找到了原因,也许在这个时候你能找到解决问题的方法,并给我一些建议。

我认为问题来自U列表。

因为可能每个顶点的某个键的值都高于CCD_ 13的键。因此,ComputeKey(s)<ComputeKeu(s_start)不满足(ComputePath中while的第一个条件(,第二个条件rhs(s_start)>g(s_start)不满足,因为当您沿着路径移动时,您会在一致的单元格中移动。

然后,当这两个条件不成立时,while停止,因此程序停止扩展新的单元格。

当你计算路径时,使用沿着路径最小化g(s)+c(u,s)的路径作为连续路径,你最终会得到一个仍然具有和无限g成本的单元(因为它在while循环中没有扩展(。

这就是为什么如果您更改条件,使用while U!=0算法工作,这将迫使程序扩展U列表中的所有顶点。(但你肯定失去了动态算法的优势(。

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

在实现D*Lite(常规版本(优化版本时,我自己也遇到了这个问题。我有点不确定为什么会发生这种情况,但我似乎是被一些障碍物(十字形或高大的垂直障碍物(的排列所触发的,在这些障碍物中,算法突然无法探索更多的选项,最终在两个选项之间以无限循环来回跳跃。我早些时候已经在这里创建了一篇关于这一点的帖子,以及我如何绕过无限循环问题,但代价是算法可能会变慢一点。

相关内容

  • 没有找到相关文章

最新更新