Перемещение робота в нужную точку векторной карты

Классическая постановка задачи поиска пути на карте

При решении этой задачи обычно считается, что робот может поворачиваться в нужную сторону вокруг своей оси на месте (танковый поворот) и этим временем поворота обычно пренебрегают. Сама постановка классической задачи перемещения робота в нужную точку карты обычно формулируется следующим образом:

Дано: Карта с указанными, текущее положение робота и конечное положение, в которое нам нужно попасть.

Требуется: Составить маршрут (ломаную от текущей точки до нужной), позволяющий избегажать всех препятствий и приводящий в нужную точку карты за приемлимое или оптимальное время.

При этом под "положением робота" могут пониматься как "координаты робота", так и пара "координаты робота + направление робота".

Решение классической задачи поиска пути на карте

Переход к задаче поиска пути во взвешенном графе

Наиболее простой подход - составить граф допустимых путей, вершинами которого будут точки карты, а ребрами - доступная возможность проехать из 1 точку в другую напрямую, избежав препятствия. В случае растровой карты за вершины графа принимаются центры клеток, а за ребра - возможность проехать в соседние клетки. В случае же векторной карты, понятно, что вообще таких точек на карте бесконечное множество, но мы можем выбрать конечное число точек на карте, чтобы незначительно ограничить свои возможности и уже на этих точках искать оптимальный маршрут. (Подробнее о построении этого графа смотрите Построение графа допустимых путей для векторной карты). Обязательное условие - в список вершин этого графа должны входить точки старта и финиши (где мы сейчас находимся и куда нам нужно попасть). Когда граф допустимых путей построен поиск пути на карте сводится к задаче поиска пути во взвешенном графе. А для этого есть очень эффективный "алгоритм Дейкстры".

Решение задачи поиска пути во взвешенном графе алгоритмом Дейкстры

Суть алгоритма сводится к тому, что в нем создается множество Q вершин графа в которые мы уже нашли путь из точки старта, но которые мы еще не "обработали". Для каждой вершины графа i при этом мы храним путь D[i] до неё, либо +бесконечность, если никакой путь до неё еще не нашли, признак обработанности Flag[i]. Длина ребра i,j будем считать извлекается функцией Len(i,j). Кроме того будем считать, что длины ребер ненулевые и, конечно же, неотрицательные.

Входные данные:
V - множество вершин графа; adj(i) - множество смежных вершин для вершины i; len(i,j) - длина ребра между вершинами i и j; start - стартовая вершина finish - целевая вершина Q - рабочая структура данных - множество вершин W - рабочая структура данных - массив вершин найденного пути ExtractMin(Q) - извлечь вершину i из Q с минимальным D[i]

Инициализация алгоритма заключается в пометке всех вершин как необработанных (Flag[i]=0), указании для всех вершин длины пути +бесконечность (D[start]=max_len), кроме стартовой (для неё указываем D[start]=0) и помещении стартовой вершины start в очередь Q.

 Для всех вершин v из V выполняем:
   Flag[v]=0
   D[v]=+бесконечность

 D[start]=0
 Q={start}

Итерации основного цикла алгоритма повторяются до тех пор, пока множество Q не пусто. На каждом шаге мы извлекаем вершину i из множества Q, путь до которой D[i] минимален и обрабатываем её следующим образом: для всех еще не обработанных вершин, в которые мы можем попасть из неё делаем 2 операции:

• Если этой вершины еще нет в множестве Q, тогда добавляем её в это множество;
• Если D[i]+Len(i,j)<D[j], тогда запоминаем более короткий путь D[j]=D[i]+Len(i,j);

 Пока Q не пусто выполняем:
   i=ExtractMin(Q)
   Flag[i]=1
   Для всех v из adj(i) выполняем:
     Если Flag[v]=0 тогда Q={Q,v}
     Если D[v]<D[i]+len(i,v) тогда D[v]=D[i]+len(i,v)

По окончании основного цикла для всех вершинах, в которые мы можем попасть из стартовой в D[i] будет храниться +бесконечность, если в эту вершину нельзя пройти, либо минимальная длина пути, если в эту вершину пройти можно.

Чтобы извлечь теперь оптимальный маршрут из полученного массива оптимальных расстояний нам потребуется выполнить обратный пробег по ребрам из вершины путь куда требуется найти до стартовой вершины.

Для этого достаточно завести массив для сохранения в нем найденного пути, положив там первым элементом целевую вершину, положить в рабочую переменную i целевую вершину finish и пока в рабочей переменной не окажется стартовой вершины выполнять цикл, в котором находить вершину j из которой мы могли оптимальным путем прийти в текущую вершину i (D[i]=D[j]+Len(i,j)) и присваивать её текущей вершине (i=j) и добавлять найденную вершину в массив найденного пути. Тогда при завершении работы этого цикла в массиве окажется найденный оптимальный путь, только выписанный задом наперед (первая вершина - finish, последняя - start).