RRT-Connect - неоптимальное планирование траектории робота

В предыдущей статье по планированию траектории мобильного робота я рассмотрел Open Motion Planning Library. Теперь давайте рассмотрим как работает один из алгоритмов планирования траектории: RRT-Connect. При чтении этой статьи от читателя потребуется небольшое знание математики. Сначала я рассмотрю общую архитектуру систем планирования траектории, а потом перейду к рассмотрению конкретного алгоритма.


 
 

Алгоритмы планирования

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

 
 
 
 
 
 

Геометрическая модель пространства определяет расположений известных объектов в мире робота. Модуль определения столкновений определяет допустимость нахождения робота в конкретной точке пространства. Алгоритм планирования движения прокладывает траекторию робота.

Планирования в непрерывных пространствах

При данном подходе планирование траектории рассматривается как поиск в непрерывном пространстве состояний C, где q ∈ C определяет положение и ориентацию робота в окружающем пространстве. Метрика ρ задана на C. Cfree – подпространство пространства состояний, в котором робот не сталкивается с какими препятствиями. Препятствия представлены в геометрической модели, поэтому напрямую реализовать Cfree невозможно. Использую алгоритм определения столкновений, для q∈C можно выяснит q∈Cfree. Необходимо найти непрерывный путь из начального состояния qinit в целевой состояние qgoal.
В случае планирования траектории для робота в соревнованиях Eurobot 2012 C∈R2. q содержит две компоненты: x и y. Метрика ρ определяет расстояния по прямой между положениями робота в двух состояниях.

Быстро исследующие случайные деревья(Rapidly-Exploring Random Trees)

Быстро исследующие случайные деревья – динамическая структура данных, предназначенная для исследования пространства состояний. Ниже приведено изображение таких деревьев в случае двухмерного пространства состояний.

 
 
 
 
 
 
 
 

Данный алгоритм исследует пространство состояний, начиная из начальной конфигурации, пока не достигнет целевой конфигурации. Он содержит две базовые конструкции представленные ниже.


 
 
 
 
 
 
 
 
 

 
 
 
 
 
 
 
 
 
 
 
 
 

На каждой итерации быстро-исследующее случайное дерево (RRT) пытается расшириться, добавляя новые вершины. Функция EXTEND выбирает вершину, которая уже входит в состав дерева, ближайшую к целевой конфигурации q. Функция NEW_CONFIG приближается к q на некоторое фиксированное расстояние E и проверяет перемещение и состояние на допустимость. В результате выполнения функции возможно возникновение трёх ситуаций: Reached, q напрямую добавлено в дерево, потому что оно находится не дальше E от выбранной вершины и достижимо из неё; Advanced, новое состояние qnew не равное q добавлено в дерево; Trapped, предложенная вершина отклонена, т.к. она не принадлежит Cfree или недостижима из неё по кратчайшему пути.

 
 
 
 
 
 
 
 
 
 
 
 

Функция EXTEND

RRT-Connect

Алгоритм RRT-Connect был разработан для планирования траекторий с учётом только кинематики объектов. Алгоритм основан на двух идеях:
• используется эвристика, которая пытается перейти к цели по прямой, с минимальным количеством поворотов;
• создаётся два быстро-исследующих случайных дерева, одно начинает поиск из начального состояния qinit, другое - из конечного qgoal.
Эвристика алгоритма Connect представляет собой жадную функцию, которая может быть рассмотрена как альтернатива функции EXTEND. Эвристика Connect выполняет функцию EXTEND, пока не будет достигнуто целевое состояние q или пока не встретиться препятствие. Она позволяет гораздо быстрее достигать решения.
Ниже приведён алгоритм RRT-Connect. На каждой итерации одно из деревьев расширяется и пытается соединиться с ближайшей вершиной другого дерева. После чего роли деревьев меняются.

 
 
 
 
 
 
 
 
 

 
 
 
 
 
 
 
 
 
 
 
 
 

Ниже приведены этапы планирования траектории алгоритмом RRT-Connect.

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Оптимизация полученной траектории

Алгоритм RTT-Connect возвращает неоптимальную траекторию, поэтому её было бы хорошо дополнительно оптимизировать. Для этой цели подходит алгоритм вырезания углов. На траектории выбирается три смежных узла. Если перемещение по прямой из крайнего левого узла в крайний правый возможно, то средний узел удаляется. Процесс повторяется итеративно, пока не будут удалены все возможные узлы.

Leave a Reply

You must be logged in to post a comment.