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

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

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

Наши цели
Вместе со студентом магистратуры мы пытаемся заменить некоторые пакеты по умолчанию из библиотеки ROS, которые использует этот робот. Таким образом мы изучали различные алгоритмы, которые используются в типичном стеке робота. Мне, как человеку, который работает над планированием и обучением с подкреплением, но не над робототехникой, было сложно с этим справиться. В первую очередь роботу требуется уметь локализовать себя в окружающей среде, затем понять своё местоположение, построить карту на лету (если ее еще нет), а также избежать препятствий, которые появляются случайно. Более того, роботу желательно управлять двигателями для изменения скорости или направления, придумать план решения задачи и так далее.
Мой робот Pumpkin

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

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

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

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

  • Планирование движения должно работать на роботе. К примеру, робот по плану совершает поворот под острым углом, что технически невозможно, исходя из его функциональности (как и у автомобиля). Следовательно, подобный маршрут неприемлем.
  • Чем ближе к оптимальному, тем лучше. Безусловно, хорошо найти любое движение, в процессе которого робот из начальной точки 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, которая в конечном итоге генерирует оптимальный путь.

Источники

ЛУЧШИЕ СТАТЬИ ПО ТЕМЕ

matyushkin
07 апреля 2020

ТОП-15 книг по Python: от новичка до профессионала

Книги по Python (и связанным с ним специальным темам) на русском языке. Рас...
admin
14 июля 2017

Пишем свою нейросеть: пошаговое руководство

Отличный гайд про нейросеть от теории к практике. Вы узнаете из каких элеме...