В настоящее время я пытаюсь реализовать алгоритм D * Lite для планирования пути (см. Также здесь), чтобы разобраться в этом. Я нашел две реализации в Интернете, обе для C / C ++, но почему-то не смог полностью следовать идеям, поскольку они, кажется, отличаются от псевдокода в официальных документах больше, чем ожидалось. Я особенно использую следующие две статьи: Кенинг, С.; Лихачев, М. - D * Lite, 2002 г. Кениг и Ликкачев, Быстрое перепланирование для навигации в Неизвестная местность, IEEE Transactions on Robotics, Vol. 21, No. 3, июнь 2005 г.
Я попытался реализовать оптимизированную версию D * Lite из первого технического документа (стр. 5, рис. 4), а для «отладки» я использую пример, как показано и объяснено во втором техническом документе (стр. 6, рис. 6 и рис. .7). Вся работа выполняется в MatLab (проще обмениваться кодом с другими).
Пока что я запустил код, чтобы найти начальный кратчайший путь, запустив computeShortestPath () один раз. Но теперь я застрял на строках {36 ''} и {37 ''} псевдокода:
{36”} Scan graph for changed edge costs;
{37”} if any edge costs changed
Как мне обнаружить эти изменения? Я почему-то не понимаю, как это обнаруживается? В своей реализации до сих пор я в основном использовал 3 матрицы. Одна матрица того же размера, что и карта сетки, содержащая все значения rhs. Одна матрица одинакового размера, содержащая аналогично все g-значения. И одна матрица с переменным количеством строк для очереди с приоритетом, где первые два столбца являются ключами приоритета, а третья и четвертая строки - координатами x и y.
Сравнивая свои результаты, я получаю тот же результат для первого запуска computeShortestPath () на Шаге 5, как показано во втором техническом документе, стр.6 Рис.6. Переместить робота на один шаг тоже не проблема. Но я действительно понятия не имею, как должен быть реализован следующий шаг сканирования измененных затрат на границу.
Спасибо за любые подсказки, советы и / или помощь !!!
