寻路算法创建循环

Question

我已经实现了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() 

---

编辑：

我已经成功地创建了测试用例足见其erronous行为。与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：更多进展。如果我用U != Ø（U不为空）代替ComputeShortestPath中的while循环中的中断条件，则会找到路径！它的速度很慢，因为它总是检查图中的每个节点，这不应该是必需的。此外，由于我使用无向图，我添加了一些代码到{40"}来更新u和v。

Answer 1

你的代码至少有两个问题（不包括typename，为了编译它，我必须预先写入像std::vector<TemplateParameter>::iterator这样的结构）。

1. 由于对角线的费用为1但长度为√2，因此您使用了不可接受的启发式。这可以防止第二次致电ComputeShortestPath做任何事情。

2. 你正在使用的堆的更新方法（按照惯例是私有的，因此显然没有记录）仅支持键减少。 D * Lite也需要重点增加。

Answer 2

不幸的是，发布伪是不是真的有用，因为这里的伪代码可能是正确的，但在实际执行可能发生故障。

一般来说，在寻路算法，如果你的节点之间的循环再有就是一个很好的机会，该算法没有从该组潜在的路径节点删除访问节点。这通常是通过在节点上遍历节点时设置一个标志来完成的，并且在您重新搜索树时重置该标志。

Answer 3

问题出在UpdateVertex函数中。

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

您可以通过使用-Wfloat平等编译测试这个对GCC（甚至更好-Werror =浮动相等），我有你同样的问题也

Answer 4

。我想我得到了原因，也许在这个时候你找到了解决问题的办法，并且可以给我一些提示。

我觉得问题来自U列表。

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

然后当这两个条件不保持while stop时，程序停止扩展新的单元格。

当你去计算路径，沿着路径连续使用一个最小化g(s)+c(u,s)的单元，结果在一个单元上仍然具有无限的成本（因为它在while循环中没有被扩展）。

这就是为什么如果更改条件，使用U!=0算法工作，这会强制程序展开U列表中的所有顶点。 （但你绝对失去了动态算法的优点）。

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