Перемещение робота в нужную точку векторной карты
|
Это незавершённая статья по робототехнике. Вы можете помочь проекту, исправив и дополнив её. |
Содержание
Классическая постановка задачи поиска пути на карте
При решении этой задачи обычно считается, что робот может поворачиваться в нужную сторону вокруг своей оси на месте (танковый поворот) и этим временем поворота обычно пренебрегают. Сама постановка классической задачи перемещения робота в нужную точку карты обычно формулируется следующим образом:
Дано: Карта с указанными, текущее положение робота и конечное положение, в которое нам нужно попасть.
Требуется: Составить маршрут (ломаную от текущей точки до нужной), позволяющий избегажать всех препятствий и приводящий в нужную точку карты за приемлимое или оптимальное время.
При этом под "положением робота" могут пониматься как "координаты робота", так и пара "координаты робота + направление робота".
Решение классической задачи поиска пути на карте
Переход к задаче поиска пути во взвешенном графе
Наиболее простой подход - составить граф допустимых путей, вершинами которого будут точки карты, а ребрами - доступная возможность проехать из 1 точку в другую напрямую, избежав препятствия. В случае растровой карты за вершины графа принимаются центры клеток, а за ребра - возможность проехать в соседние клетки. В случае же векторной карты, понятно, что вообще таких точек на карте бесконечное множество, но мы можем выбрать конечное число точек на карте, чтобы незначительно ограничить свои возможности и уже на этих точках искать оптимальный маршрут. Обязательное условие - в список вершин этого графа должны входить точки старта и финиша (где мы сейчас находимся и куда нам нужно попасть). Когда граф допустимых путей построен поиск пути на карте сводится к задаче поиска пути во взвешенном графе. А для этого есть очень эффективный "алгоритм Дейкстры".
Решение задачи поиска пути во взвешенном графе алгоритмом Дейкстры
Суть алгоритма сводится к тому, что в нем создается множество Q вершин графа в которые мы уже нашли путь из точки старта, но которые мы еще не "обработали". Для каждой вершины графа i при этом мы храним путь D[i] до неё, либо +бесконечность, если никакой путь до неё еще не нашли, признак обработанности Flag[i]. Длина ребра i,j будем считать извлекается функцией Len(i,j). Кроме того будем считать, что длины ребер ненулевые и, конечно же, неотрицательные.
| Входные данные и используемые функции: |
|---|
|
Инициализация алгоритма заключается в пометке всех вершин как необработанных (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).
W={finish}
i=finish
Пока i не равно start выполняем:
j=i
Для всех v из adj(i) пока j равно i выполняем:
Если D[i]=D[v]+len(v,i) тогда j=i
W={W,j}
i=j
Проблемы поиска оптимального пути по карте в реальности и способы их решения
В реальности существует масса различных факторов, которые не были учтены в рассматренной выше ситуации. Мы попробуем разобрать основные проблемы и указать возможные способы их решения:
Учет погрешности выполнения команд на перемещение роботом
Понятно, что это только в программе мы можем с нужной нам точностью посчитать столкновение робота с препятствием, а в реальности существуют погрешности исполнительных устройств, приводящие к отклонениям от запланированной траектории. Поэтому имеет смысл на карте провести границу вокруг всех препятствий на таком расстоянии SafeDistance, что вероятность отклонения шасси робота от запланированной траектории на большую чем SafeDistance величину не будет превышать допустимую. Разумеется при этом не стоит избегать использования систем защиты от столкновения, имеющих приоритеты над модулем навигации и не допускающих незапланированного столкновения робота с предметами окружения.
Учет времени, затрачиваемого на повороты на месте
В указанном выше алгоритме очевидным упущением является то, что не учитывается время, затрачиваемое на остановку, развороты робота в вершинах графа и разгон, требующиеся для движения в следующих направлениях. Здесь можно применить очень простой подход - ввести штраф за "возможно остановку и возможно требующийся поворот" в каждой промежуточной вершине, в этом случае просто все формулы вида D[j]=D[i]+Len(i,j) заменяться на D[j]=D[i]+Len(i,j)+СуммаШтрафа.
Понятно что при этом не учитывается сколько времени робот будет поворачиваться (на какой угол требуется поворот), но и это можно учесть предельно точно, сильно пожертвовав производительностью и объемом памяти требующимся для вычислений. Для этого достаточно "размножить" каждую вершину, начав считать "вершиной графа" пару "вершина и по какому ребру мы в неё пришли". Тогда СуммуШтрафа можно посчитать точно разложив её на составляющую штрафа за торможение и разгон и составляющую штрафа на поворот с направления X (откуда пришли в вершину) до направления Y (в какое ребро сейчас будем двигаться. Так же ясно, что этот подход позволяет учесть начальную ориентацию робота и конечную ориентацию, если её требуется достичь.
Оптимизация маршрута путем избегания поворотов "на месте"
Понятно, что если мы сможем проходить маршрут не останавливаясь на каждом повороте, то мы сможем делать это значительно быстрее. Один из наиболее простых вариантов - попробовать увеличить дистанцию безопасности SafeDistance, чтобы можно было повороты под небольшим углом проводить не снижая скорость до нулевой и заменяя участки движения "тормозим, поворачиваем, разгоняемся" на "чуть снижаем скорость, входим в поворот в движении, разгоняемся по следующему направлению". Основная проблема здесь - при значительном увеличении SafeDistance на карте может вообще не оказаться путей к целевой точке, либо они окажутся неоптимальными, потому что будет существовать более короткий маршрут, но со значительным снижением скорости. Имеет смысл попробовать перебрать несколько значений SafeDistance (от требующего повороты на месте до требующего малых снижений скоростей на поворотах) и сравнить найденные маршруты по общему времени прохождения. Кроме того можно пробовать комбинировать получившиеся при разных SafeDistance маршруты или применять более сложные смешанные алгоритмы.
