Для проекта робототехники, с которым я работал, я хочу использовать D * Lite (оптимизированная версия) из бумаги Koenig, 2002 для dynamici c планирование пути для изменяющейся сетки занятости / карты затрат. Идея алгоритма поиска D * Lite, как описано в статье, заключается в том, что он работает, в основном, выполняя поиск A * в обратном порядке, начиная с цели и пытаясь вернуться к началу. Затем решатель выдает текущее решение и ожидает какого-то изменения в весах или препятствиях, с которыми оно связано. В отличие от повторного поиска A *, алгоритм D * Lite избегает перепланировки с нуля и постепенно восстанавливает путь, сохраняя свои модификации локальными вокруг позы робота.
Моя проблема
I реализовали алгоритм в python с симуляцией в pygame для проверки производительности. Но у меня есть проблема, связанная с псевдокодом. Я реализовал алгоритм, как оптимизированной, так и неоптимизированной версии, теперь три раза, и я все еще получаю ошибку, заключающуюся в том, что когда алгоритм сталкивается с некоторой конфигурацией препятствий (крестообразных или больших вертикальных препятствий), алгоритм внезапно застревает в бесконечно l oop внутри while-l oop (процедура Main, псевдокод строки 32) и перемещается назад и вперед между двумя вариантами для вершины s_start (процедура Main, строка 34). Я несколько раз сравнивал мою python реализацию с псевдокодом, и я не могу найти никаких отклонений от псевдокода, которые могли бы вызвать эту ошибку.
Мое временное "исправление"
Теперь, чтобы избежать застревания алгоритма в бесконечном l oop, я отступил computeShortestPath () в строке 48 в процедуре Main () на одну слева, чтобы Я выходит за рамки if в строке 37 в процедуре Main ().
Когда я это делаю, алгоритм почти всегда способен вычислить новый кратчайший путь при обнаружении новых препятствий на своем пути.
Мои вопросы
Прежде всего, у кого-нибудь есть идея, почему алгоритм иногда застревает в бесконечности, пока l oop и как это исправить?
Я полагаю, что мое временное "исправление" снова сочтет алгоритм вычислительно более дорогим, и поэтому большая часть алгоритма D * Lite состоит в том, чтобы сейчас нет. Какова вычислительная сложность моего исправления отступа по сравнению с исходным алгоритмом?
Теперь, с моим временным исправлением, код фактически отличается от вычислений по сравнению с динамическим c A * где вы пересчитываете все для каждого повторного планирования нового пути?
Иногда при встрече с большими сложными лабиринтами, когда нужно много исследовать новые вершины, мой "временный" исправленный код иногда становится довольно медленным Но разве это не произойдет даже в некотором роде в исходном коде?
Моя реализация
Если вы хотите запустить Напишите код и попробуйте алгоритм самостоятельно, вы можете проверить мою реализацию здесь , запустив python main.py.
В эту конкретную реализацию включено мое "временное исправление". Но если вы хотите испытать это без, вы можете go в строку 156 в d_star_lite.py и сделать отступ compute_shortest_path () один вправо, так что он идет внутри 'if' в строке 132.
Псевдокод
Вот оригинальный псевдокод для алгоритма 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 **