Pathfinding algorithm creating loops

I've implemented the D*-Lite algorithm (here's a description, it's an algorithm for doing pathfinding when edge costs change over time), but I'm having problems with doing the edge cost updates. It works mostly, but sometimes it gets stuck in a loop, going back and forth between two vertices. I'm trying to create a test case which exhibits this behaviour, at the moment it happens for some cases when used in a large application, which makes it difficult to debug.

I'll get a test case up as soon as I can, but maybe someone can spot the error I've done going from pseudo to C++ right away.(There is a test case included below) The article presents an optimized version, Figure 4, which is the one I've implemented. The pseudocode is pasted below.

The source for my implementation is availible here.

If it helps, I'm using these types in my code:

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;

If any more information about usage is needed just ask, there's just too much to paste.

Pseudo code for D*-Lite:

procedure CalculateKey(s)
{开发者_JAVA百科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()

EDIT:

I've succeeded in creating a test-case which shows the erronous behaviour. Running this along with the code in the pastebin, it will hang up in the last get_path call, going back and forth between nodes 1 and 2. It seems to me it is because the node 3 is never touched, and so going that way has an infinite cost.

#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;
}

EDIT 2: More progress. If I replace the break condition in the while loop in ComputeShortestPath with just U != Ø (U is not empty), the path is found! It's quite slow though, since it always examines every node in the graph, which is not supposed to be neccessary. Also, since I use undirected graphs, I added some code to {40"} to update both u and v.

There are at least two problems with your code (not including the typenames I had to prepend to constructs like std::vector<TemplateParameter>::iterator in order to compile it).

1. You're using a non-admissible heuristic, since the diagonals cost 1 but have length √2. This prevents the second call to ComputeShortestPath from doing anything at all.

2. The update method of the heap you're using (which is private by convention to Boost and thus apparently undocumented) supports key decreases only. D* Lite needs key increases as well.

Unfortunately, posting pseudocode isn't really useful here since the pseudocode may be correct but the actual implementation may be at fault.

Generally, in path finding algorithms, if you're cycling between nodes then there is a really good chance that the algorithm is not removing visited nodes from the set of potential route nodes. This is usually done by setting a flag on the node as you traverse through it and reset the flag as you back step up the search tree.

The problem is in the UpdateVertex function.

The psuedo-code was written assuming that the comparisons are on integers (which they are in the papers). In your implementation you're doing comparisons on floating point values. You need to add a tolerance if you are dealing with non-integer costs.

You can test this on GCC by compiling with -Wfloat-equal (or even better -Werror=float-equal)

I'm having your same problem too. I think I got the cause, maybe in this time you find the solution to your problem and can give me some tips.

I think the problem come from the U list.

Since probably some key of every vertex have a value higher than the key of the s_start. So ComputeKey(s)<ComputeKeu(s_start) is not satisfied (first condition of the while in ComputePath), the second condition rhs(s_start)>g(s_start) is not satisfied since when you move along the path you move through cells that are being made consistent.

Then when this two conditions don't hold the while stop, so the program stops expanding new cells.

When you go calculating the path, using as successive along the path the one that minimize g(s)+c(u,s) you end up on a cell the still has and infinite g cost (cause it has not been expanded in the while cycle).

This is the reason even to the fact that if you change condition, using while U!=0 the algorithm works, this force the program to expand all the vertices in the U list. (But you definitely lost the advantages of dynamical algorithm).

Now I hope I've helped you, if you don't need this help no more maybe you can help me.

I had this issue myself when implementing both the D* Lite (regular version) and the optimized version. I am a bit unsure why it happens in the first place, but I seems to be triggered for some arrangement of obstacles (cross shaped or tall vertical obstacles) where the algorithm suddenly is unable to explore more options and end up jumping back and forth between two options in an infinite loop. I already created a post on this earlier here and how I bypassed the infinite loop issue, however at the cost of the algorithm probably becoming a little slower.