🤖 Планирование маршрута роботом при помощи RRT

Робот должен четко определять свое местоположение в пространстве, уметь строить маршрут, находить цель и обходить препятствия. Всё это учитывалось в моей исследовательской работе над собственным мобильным роботом Pumpkin.

Текст публикуется в переводе, автор оригинальной статьи Justin Svegliato.

Построение маршрута

Как вы уже догадались, важнейшая составляющая робота – его способность планировать маршрут от одного места к другому с учетом карты окружающей среды. Зачем это нужно? Например, робот перемещается через комнату, чтобы доставить посылку или сопроводить человека в здание. В случае, когда роботу требуется добраться от начальной точки до конечной, чтобы выполнить задание, наш аппарат разрабатывает план перемещения.

В прошлом я написал несколько постов с красочными диаграммами и длинными объяснениями. Признаться, поскольку такие посты требуют много работы, в итоге ничего так и не было опубликовано. В моих планах писать посты без лишнего материала, более грубые и непринужденные. Мне не только легче написать пост, который укрепит собственное понимание концепции. Я также уверен, что такая подача материала будет столь же информативной без "колокольчиков и свистков", которые присутствовали в других моих постах. А теперь вернемся к нашей теме.

Некоторые тонкости, которые стоит держать в уме:

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

• Чем ближе к оптимальному, тем лучше. Безусловно, хорошо найти любое движение, в процессе которого робот из начальной точки A достигнет конечной точки B. Однако этого, к сожалению, недостаточно. Мы хотим получить более эффективный результат работы. Это не только поможет роботу выполнить задачу как можно быстрее, но и сэкономит драгоценный заряд батареи.

• Разумеется, маршрут исключает столкновения со стенами. Роботы – дорогое развлечение, а поломка никогда не хороша. Один только мой маленький робот обошелся более чем в тысячу долларов.

Какой алгоритм подойдет для нашей цели?

Из удовлетворяющих нашим условиям алгоритмов наиболее известным является Быстро Исследуемое Случайное Дерево (Rapidly-exploring Random Trees – RRT). Поскольку одна картина стоит тысячи слов, давайте посмотрим на диаграмму ниже. Предположим, что задача робота – добраться от начальной точки (красная точка) до цели (зеленая точка) по стандартной карте, без каких-либо препятствий. Начнем с дерева, корневой узел которого – начальная позиция робота. После этого постепенно наращиваем дерево. Каким образом? Возьмем кучу случайных образцов карты, создадим новый узел для каждого и вставим его в дерево. Как только в дереве появится достаточно близкий к целевому положению робота узел, наша работа по планированию закончена.

Детализация RRT

Знаю, эта формулировка проблемы кажется туманной. Давайте добавим к грубой идее некоторые детали. Для начала предлагаю рассмотреть каждый параметр, который мы будем передавать в RRT:

• Разделенная на области карта окружения с препятствиями и без них. Она будет выглядеть как показанная выше, где область с препятствиями – это все, что окрашено в серый цвет, а область без препятствий – окрашенное в белый пространство.

• Стартовая позиция робота в окружающей среде – красная точка на карте.
  

• Область цели: цель движения робота в среде – зеленая точка на карте.

• Количество итераций, выполняемых RRT.

Алгоритм

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

1. Выберите случайную позицию относительно препятствий в области карты.
2. Создайте связанный со случайной позицией узел.
   
3. Найдите в дереве узел, который находится ближе всего к случайной позиции.
   
4. Рассчитайте маршрут относительно случайной позиции до позиции узла. Маршрут, который будет выполним для робота.
   
5. Перейдите к следующей итерации, если в выбранном пути робота возникнут проблемы.
   
6. Вставьте связанный со случайной позицией узел в дерево с узлом (ближайшим к нему) в качестве родительского узла.
   
7. Верните дерево, как только случайная позиция окажется на некотором расстоянии относительно целевой позиции.
   

Можно ли повторно использовать дерево для новых целей на той же карте?

Конечно! Если в дереве возле новой цели уже присутствует узел, то ситуация под контролем. Однако, в случае, если рядом с новой целью еще ничего нет, мы продолжаем выборку, пока не найдем узел рядом с нашей целью. До тех пор, пока окружение не изменится, строим идентичное дерево для новых мест расположения цели.

Без лишних слов, вот вариант алгоритма для RRT:

function RRT(map, startPosition, goalRegion, numIterations):
    tree = initializeEmptyTree()
    
    insertRootNode(tree, startPosition)
    for i = 1 to numIterations:
        randomPosition = sample(map)
        randomNode = createNode(tree, randomPosition)
        nearestNode = findNearestNode(tree, randomPosition)
        path = calculatePath(nearestNode, randomNode)
        if (hasCollision(map, path)):
            continue
        
        insertNewNode(tree, nearestNode, randomNode)
        if (randomPosition is within goalRegion): 
            return tree
    return tree

RRT и оптимальность

Еще один момент, который важно осветить! Хотя это планирование будет работать на рассматриваемом роботе и избегать препятствий, будет ли оно оптимальным? К сожалению RRT не гарантирует этого. Не спешите расстраиваться! В следующих статьях цикла я расскажу о RRT* – улучшенной вариации RRT, которая в конечном итоге генерирует оптимальный путь.

Источники

• https://towardsdatascience.com/how-does-a-robot-plan-a-path-in-its-environment-b8e9519c738b