Планирование траектории движения мобильного робота с использованием градиента функции исследования областей пространства конфигураций

Пыхтин Павел Сергеевич аспирант, , Волгоградский Государственный Технический Университет 400005, Россия, Волгоград, пр. Ленина,28. Pykhtin Pavel Sergeevich graduate student, Volgograd State Technical University 400005, Russia, Volgograd, pr. Lenina,28. pavel.pykhtin@gmail.com Камаев Валерий Анатольевич доктор технических наук профессор, кафедра Системы Автоматизированного Проектирования и Поискового Конструирования, Волгоградский Государственный Технический Университет Волгоградский Государственный Технический Университет. 400005, Волгоград, пр. Ленина,28 Kamaev Valerii Anatol'evich Doctor of Technical Science Volgogradskii Gosudarstvennyi Tekhnicheskii Universitet. 400005, Volgograd, pr. Lenina,28 kamaev@unix.cad.vstu.ru Крыжановский Анатолий Иванович аспирант, кафедра САПР и ПК, Волгоградский Государственный Технический Университет Волгоградский Государственный Технический Университет. 400005, Волгоград, пр. Ленина,28. Kryzhanovskii Anatolii Ivanovich Volgogradskii Gosudarstvennyi Tekhnicheskii Universitet. 400005, Volgograd, pr. Lenina,28. anatoly.kryzhanovsky@gmail.com Никляев Илья Юрьевич аспирант, кафедра САПР и ПК, Волгоградский Государственный Технический Университет Волгоградский Государственный Технический Университет. 400005, Волгоград, пр. Ленина,28. Niklyaev Il'ya Yur'evich Volgogradskii Gosudarstvennyi Tekhnicheskii Universitet. 400005, Volgograd, pr. Lenina,28. spirit.of.fire@mail.ru

Пыхтин Павел Сергеевич аспирант, кафедра САПР и ПК, Волгоградский Государственный Технический Университет Волгоградский Государственный Технический Университет Pykhtin Pavel Sergeevich Volgogradskii Gosudarstvennyi Tekhnicheskii Universitet pavel.pykhtin@gmail.com

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

Одним из популярных алгоритмов планирования движения, который хорошо зарекомендовал себя в данной области, является алгоритм RRT (Rapidly exploring Random Trees). Этот алгоритм обеспечивает высокую скорость исследования пространства конфигураций, его сходимость была доказана при условии увеличения числа итераций, а также он неплохо зарекомендовал себя при решении задач большой размерности. Однако, данный алгоритм, как и другие алгоритмы на основе вероятностных дорожных карт, обладает плохой сходимостью, в случае если области, обеспечивающие связность отдельных компонент пространства конфигураций, имеют малый размер. Описанный случай соответствует наличию в пространстве конфигураций узких проходов. Описываемая в статье модификация алгоритма RRT может быть эффективно применена для построения траектории в подобных условиях.

Алгоритм RRT включает следующие шаги:

1. Добавить в пустое дерево поиска начальную конфигурацию.
2. Выбрать конфигурацию `C_(n)` , лежащую в `C_(Free)` .
3. Найти в дереве `T` конфигурацию `C_(i)` , ближайшую к `C_(n)`.
4. Построить путь `P_(i,n)` от `C_(i)` к `C_(n)` .
5. Проверить на наличие коллизий путь `P_(i,n)` .
6. Если коллизии не были найдены, добавить `C_(n)` и `P_(i,n)` в дерево `T`.
7. Если `C_(n)` не является целевой конфигурацией, перейти к шагу 2.

На шаге 2 выбирается некоторая конфигурация из подмножества ` ` пространства конфигураций, не занятого препятствиями. Как правило, она выбирается случайным образом, что обеспечивает сходимость алгоритма в общем случае. Функция распределения вероятности, используемая для выбора новой конфигурации, может варьироваться. Классическим решением является использование равномерного распределения для выбора новых конфигураций. Очевидно, что в этом случае вероятность выбора новой конфигурации, такой, что она лежит в некоторой области `A` , будет равна отношению объемов областей `A` и `C_(Free)` . На рис. 1 приведен пример ситуации, когда две компоненты связности пространства разделены препятствиями с узким проходом между ними. Траектория, соединяющая начальную и конечную точки пути, может быть найдена только при условии, что новая конфигурация будет взята из пересечения пространств компоненты `B` и региона видимости конфигураций, лежащих в пространстве `A` . В этом случае вероятность успешного выбора новой конфигурации оказывается весьма низкой, что заметно увеличивает время работы алгоритма и может привести к ложно отрицательному результату.

Для решения описанной выше проблемы нами предложено использовать функцию распределения вероятности, которая сопоставляет конфигурациям в регионе `C` большие значения плотности вероятности, чем значениям в регионах `A` и `B` (рис. 1). В данной работе предлагается использование в качестве таковой функции от градиента исследования `F_(IG)`.

Рис. 1. Пространство с двумя компонентами связности A и B, соединенное узким проходом C.

Функция `F_(IG)` отображает величину градиента качества исследования `G_(I)` в некоторой точке `C_(I)` в значение плотности распределения случайной величины, используемой при выборе новых конфигураций, в этой же точке.

Функция `G_(I)(C_(i))` рассчитывается как:

где `I(C)` – функция качества исследования для некоторой конфигурации `C_(i)` . В общем случае функция `I(C)` отображает дерево поиска в поле скалярных значений. Под качеством исследования в данной статье понимается суммарная вероятность успешного добавления конфигурации `C` в дерево поиска `T` . Таким образом, она может быть описана как:

где `P_(path)(C_(T),C)` – функция, описывающая вероятность успешного соединения конфигурации `C_(T)` и конфигурации `C` . `C_(T)` – конфигурация дерева поиска `T` , т. е. `C in T` . Функция `P_(path)(C_(T),C)` не может быть выражена аналитически в подавляющем большинстве задач, однако можно сделать вывод о том, что она обратно зависит от длины ребра, соединяющего дерево `T` и конфигурацию `C` .

В данной работе для расчета функций `F_(IG)`, `G_(I)`, `I` применялась равномерная прямоугольная сетка – назовем ее «базовой» – размерности `N`, равной размерности пространства поиска, с шагом `M` по всем направлениям. Эта сетка разбивает исходное пространство конфигураций `bbbA` на множество областей `bbbA_(i)` . Таким образом, функции `F_(IG)`, `G_(i)`, `I`представлены тензорами ранга `N` и размерности `M`.

Как было замечено выше, необходимая для вычисления функции `I` функция вероятности успешного соединения новой конфигурации и конфигурации дерева поиска `P_(path)(C_(T),C)` не может быть формализована для многих прикладных задач. По этой причине она была заменена тензором весов `W` , который посредством свертки применяется к тензору `D` , описывающему распределение конфигураций дерева поиска по областям пространства конфигураций, соответствующим ячейкам базовой сетки. Таким образом, значение тензора `I` вычисляется как:

Для вычисления градиента исследования `G_(I)` был применен тензорный вариант оператора Собеля.

где `S_(i)` – тензор описывающий оператор Собеля для измерения `i` .

Однако на начальных этапах построения поискового дерева и при малых значениях размерности тензора весов, полученные таким образом данные о градиенте функции `I` оказываются сильно зашумленными. Чтобы уменьшить зашумленность данных, было предложено ввести тензор `hat I` , вычисляемый как:

где `B` – тензор гауссианы для случая `N`-мерного пространства, обеспечивающий размытие.

Таким образом, тензор градиента исследования вычисляется следующим образом:

Отдельно следует рассмотреть значения градиента на границе препятствий. В силу того, что конфигурация `C_(obst)` , лежащая внутри препятствия, является некорректной и не может быть добавлена в дерево поиска, значение `D(C_(obst))` всегда равно 0. Таким образом, по мере роста дерева поиска на границе препятствий будет возникать градиент исследования. Однако результат проверки на наличие препятствия сам по себе является некоторым результатом исследования, что позволяет считать граничные области препятствий исследованными, начиная с момента достижения их деревом поиска.

С учетом описанных требований к поведению функции качества исследования на границе препятствий, градиент может быть выражен как:

где `bbA_(text(border))` – некоторая область на границе препятствия.

Областью на границе препятствия считается любая область `bbA_(i) in bbA`, для которой справедливо `i=[i_(1), ... , i_(N)]` , `i_(m) in [1 .. M]` , а также `EE k in [1 .. N]` , при котором выполняется условие `(bbA_(i) in bbA_(Free) ^^ bbA_(i^text(neighbour)) in bbA_(obst)) vv (bbA_(i) in bbA_(obst) ^^ bbA_(i^text(neighbour)) in bbA_(Free))` где `i^text(neighbour)=<sup>[1, ..., i_(k)+-1, ..., i_(N)]</sup>` , а `bbA_(Free)` , `A_(obst)` – области, соответственно, содержащие и не содержащие препятствия.

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

где `j` , `k` ­- `N`-мерные вектора индексов элемента тензора, такие что `AA j,k=[i_(1), ..., i_(N)] | i_(m)=1 .. M` , а функция `f(x)` позволяет задать распределение весов для разных значений градиента.

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

Описанный в данной статье алгоритм был применен для решения задачи поиска пути на плоскости. Было произведено сравнение среднего количества итераций, необходимых для нахождения пути для предлагаемого алгоритма и классического алгоритма RRT. Были составлены два набора тестовых карт – с разреженными препятствиями и слабосвязанные (присутствуют области, соединяемые только лишь узкими проходами) и проведено моделирование работы алгоритмов на этих наборах (табл. 1). Как видно из результатов, число итераций, необходимое алгоритму GIRRT для нахождения пути, как правило, не превышает таковое для алгоритма RRT, а в случае слабосвязанных областей существенно ниже.

Таблица 1

Сравнение числа итераций поиска пути алгоритмами RRT и GIRRT.

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

Эффективность алгоритма GIRRT существенно зависит от параметра размера базовой сетки. Хотя были представлены достаточно четкие рекомендации по выбору значений этого параметра, приоритетным является реализация автоматического подбора его значений в зависимости от конкретной задачи. Кроме того, перспективным является направление работ по применению неравномерной базовой сетки, что позволило бы варьировать шаг сетки в соответствии с плотностью препятствий.

Наконец предложенный алгоритм GIRRT обладает ресурсом для распараллеливания, так как обновление вспомогательных тензоров может быть легко распараллелено.