CC BY
24
КОМПЬЮТЕРНОЕ ЗРЕНИЕ И ПЛАНИРОВАНИЕ ДВИЖЕНИЯ РОБОТОВ В ЗАДАЧАХ МАНИПУЛИРОВАНИЯ
COMPUTER VISION AND ROBOT MOVEMENT PLANNING IN MANIPULATION TASKS
УДК 004.021
DOI: 10.17586/0021-3454-2022-65-3-185-193
АЛГОРИТМ
МОДИФИЦИРОВАННОГО ИНТЕЛЛЕКТУАЛЬНОГО ДВУНАПРАВЛЕННОГО СЛУЧАЙНОГО ДЕРЕВА
ДЛЯ ПЛАНИРОВАНИЯ ДВИЖЕНИЯ АНТРОПОМОРФНЫХ МАНИПУЛЯТОРОВ
И. С. Довгополик1*, К. Артемов1, О. И. Борисов1, С. Забихифар2, А. Н. Семочкин2
1 Университет ИТМО, Санкт-Петербург, Россия isdovgopolik@itmo.ru
2 Сбербанк, Москва, Россия
Аннотация. Рассматривается алгоритм планирования движения многозвенной робототехнической системы в пространстве с препятствиями. Основные требования, предъявляемые к данной задаче, — высокое быстродействие и эффективное использование памяти в процессе работы. Представлен алгоритм планирования пути на основе метода двунаправленного быстроисследующего случайного дерева с использованием подхода, исключающего добавление новых вершин к дереву, если по их расположению в пространстве можно однозначно определить нецелесообразность их использования для построения пути. Эта модификация позволяет ускорить планирование движения и уменьшить объем памяти, необходимой для хранения данных об анализе пространства.
Ключевые слова: планирование пути, быстроисследующие случайные деревья, исключение неиспользуемых вершин, СиЬ
Ссылка для цитирования: Довгополик И. С., Артемов К., Борисов О. И., Забихифар С., Семочкин А. Н. Алгоритм модифицированного интеллектуального двунаправленного случайного дерева для планирования движения антропоморфных манипуляторов // Изв. вузов. Приборостроение. 2022. Т. 65, № 3. С. 185—193. Б01: 10.17586/0021-3454-2022-65-3-185-193.
MODIFIED INTELLIGENT BIDIRECTIONAL RANDOM TREE ALGORITHM FOR PLANNING THE MOVEMENT OF ANTHROPOMORPHIC MANIPULATORS
I. S. Dovgopolik1*, K. Artemov1, O. I. Borisov1, S. Zabihifar2, A. N. Semochkin2
1 ITMO University, St. Petersburg, Russia isdovgopolik@itmo.ru 2 Sberbank, Moscow, Russia
Abstract. An algorithm for planning the movement of a multi-link robotic system in an environment with obstacles is considered. The main requirements for this task are high performance and efficient memory usage during operation. An algorithm for path planning based on the method of a bidirectional fast-investigating random tree is presented. An approach is used which excludes addition of new vertices to the tree if their location in space can unambiguously conclude that it is inappropriate to use them in the path construction. This modification makes it possible to speed up movement planning and reduce the amount of memory needed to store the environment analysis data.
© Довгополик И. С., Артемов К., Борисов О. И., Забихифар С., Семочкин А. Н., 2022
Keywords: path planning, rapidly-exploring random trees, exclusion of unused vertices, iCub
For citation: Dovgopolik I. S., Artemov K., Borisov O. I., Zabihifar S., Semochkin A. N. Modified intelligent bidirectional random tree algorithm for planning the movement of anthropomorphic manipulators. Journal of Instrument Engineering. 2022. Vol. 65, N 3. P. 185—193 (in Russian). DOI: 10.17586/0021-3454-2022-65-3-185-193.
Введение. Проблема планирования движения составляет большинство задач, выполняемых роботами. Эта проблема актуальна как для мобильных роботов, так и для многозвенных, которые выполняют поиск пути для взаимодействия с объектами окружающей среды, для захвата объектов и манипулирования ими. Задача планирования движения в первую очередь заключается в определении траектории перемещения, к которой предъявляются три основных требования [1]. Во-первых, траектория должна проходить от заданной начальной точки к заданной конечной точке; во-вторых, она должна проходить только через свободную часть рабочего пространства — какие-либо пересечения с зонами, занятыми препятствиями, недопустимы; в-третьих, полученная траектория должна быть оптимальной по параметру, который определяется спецификой задачи.
Для многих роботов, в частности антропоморфных, важным требованием является автономность, что накладывает значительные ограничения на вычислительную мощность и объем доступной для работы внутренней памяти. Задачу планирования движения можно назвать рутинной для робототехнической системы, поэтому она должна выполняться быстро и не требовать применения значительных ресурсов. В настоящей статье предлагается решение данной задачи, для чего необходимо:
— исследовать линейку алгоритмов быстроисследующих случайных деревьев (Rapidly Exploring Random Trees — RRT);
— провести моделирование работы линейки алгоритмов и оценить эффективность использования памяти;
— усовершенствовать лучший по результатам моделирования алгоритм и использовать его для сокращения объема памяти;
— оценить эффективность работы модифицированного алгоритма.
Алгоритм RRT и его модификации. Алгоритм, предложенный в [2], работает следующим образом. Исходными данными являются начальная и целевая конфигурации робота и данные о пространстве. Первая вершина дерева — это начальная конфигурация. В пространство добавляется случайная точка на расстоянии, равном шагу алгоритма от текущей точки. Если проверка пройдена, вершина и линия добавляются в дерево. Если случайная и целевая точки совпадают, построение траектории выполнено. В противном случае действия повторяются. Структурная схема алгоритма представлена на рис. 1. В этом методе не используются алгоритмы оптимизации при определении траектории.
В [3] представлен оптимальный вариант RRT, при котором траектория строится таким образом, чтобы сумма стоимостей перехода между вершинами дерева от начальной точки к конечной была минимальной. Когда количество случайных точек стремится к бесконечности, этот подход обеспечивает асимптотически оптимальное решение, понимаемое как решение, наиболее близкое к оптимальному по сравнению с другими решениями [4].
Для ускорения процесса поиска траектории было предложено двунаправленное быстро-исследующее случайное дерево [5]. В этом алгоритме инициализируются два дерева, одно — из начальной точки траектории, второе — из конечной точки. Если какие-либо вершины первого и второго деревьев находятся на расстоянии, равном или меньшем, чем шаг алгоритма, и между этими вершинами нет областей ограничения, деревья соединяются. Структурная схема этого алгоритма представлена на рис. 2. Такой подход позволяет увеличить скорость по-
строения траектории и уменьшить объем требуемой памяти, поскольку уменьшается необходимое количество итераций для успешного определения пути.
Начало
Рис. 1
Рис. 2
В работах [6, 7] предложен модифицированный подход, в котором реализован способ выбора вершин для присоединения. Модификация получила название „интеллектуальное двунаправленное быстроисследующее случайное дерево". Алгоритм определяет, к какому из деревьев целесообразно добавить случайную точку, и оценивает расстояние между нею и ближайшей вершиной дерева. Вершина присоединяется к ближайшему дереву. Структурная схема алгоритма представлена на рис. 3.
Подход, предусматривающий ограничение области поиска для увеличения скорости планирования и уменьшения объема используемой памяти, представлен в [8]. Подпространство для поиска пути предложено определять как фигуру эллипсоидальной формы, размеры которой определяются линейным расстоянием между начальной и конечной точками траектории и длиной первого пути, который найден в пространстве посредством оригинального алгоритма. Такое подпространство должно уменьшаться, постепенно „складываясь" в прямую, соединяющую начало и конец траектории, что необходимо для поиска оптимального решения. Во избежание необходимости поиска решения стандартным алгоритмом ЯЯТ, в [9] был предложен подход к поиску траектории путем инициализации случайных точек
в пространстве группами. В данном случае построение подпространства для поиска опирается только на линейное расстояние между начальной и конечной точками траектории. Случайные точки инициализируются группами, каждая из которых содержит п точек. Если после инициализации всех точек не был найден ни один путь, подпространство поиска расширяется и вновь инициализируются п точек. После того как первый путь был найден, подпространство начинает сужаться после инициализации каждой группы точек. Такой подход имеет выраженные недостатки — необходимость расширения области поиска и вероятность отсутствия пути в подпространстве из-за ограниченного количества инициализированных точек.
Начало
Рис. 3
Методы на основе ЯЯТ приобретают широкое распространение [1, 10, 11]. Для определения траектории не нужно анализировать все пространство, достаточно лишь часть, соответственно не требуется большой объем памяти. С увеличением размерности пространства сложность вычислений почти не возрастает, что особенно важно для многозвенных робото-технических систем.
Моделирование работы алгоритмов КИТ. Эксперимент проводился для двух вариантов начальной и конечной точек траектории робота в конфигурационном пространстве. Показателем используемого объема памяти является количество вершин. Траектории, построенные с использованием алгоритмов двунаправленного ЯЯТ и интеллектуального двунаправленного ЯЯТ, представлены на рис. 4, а, б соответственно (здесь д1, д2, д3 — углы поворота сочленений робота).
а)
Чз,
б)
100 80 60 40 20 0
160
120
80
™ ° д3:, ■..
100
80
60
40
20
Чъ ■ 0
0 200
-50
-100
40
-100
Рис. 4
о
о
Анализ рисунков показывает, что каждый алгоритм создает большое количество ветвей дерева, которые можно назвать заведомо неиспользуемыми. Подразумевается, что в процессе поиска пути в графе они никогда не будут востребованы в качестве составляющих частей траектории, поскольку построены не в пространстве между начальной и конечной точками траектории. Таким образом, часть памяти, используемая для хранения дерева, может быть освобождена путем исключения этих ветвей.
Модифицированное интеллектуальное двунаправленное КИТ. Для сокращения используемого объема памяти необходимо организовать добавление к дереву вершин, которые располагаются в пространстве между начальной и конечной точками траектории. Это геометрическая задача, рассмотрим ее для двухмерного пространства. Предположим, что новая случайная конфигурация расположена в пространстве между начальной и конечной точками пути, для четкого определения ее положения введем критерий оценки. Рассмотрим рис. 5, а, где qstart7 и д81аг!2 — деревья, инициализированные из начальной и конечной точек траектории соответственно, qrand — случайная конфигурация, qnear7 и qnear2 — ближайшие к случайной конфигурации вершины инициализированных деревьев. Соединив qnear7, qnear2 и qrand в треугольник, получим углы а1 и а2, которые являются острыми.
Рис. 5
Рассмотрим другой вариант (рис. 5, б) — новая случайная конфигурация расположена вне пространства между инициализированными деревьями. Если повторить аналогичные предыдущему варианту операции, получим углы а1 и а2, при этом а1 — тупой угол, а2 — острый. Также из рисунка следует, что в случае присоединения случайной точки к дереву qstart2 новая ветвь будет построена в пространстве между инициализированными деревьями, но если присоединить точку к дереву qstart7, то новая ветвь будет построена в сторону от пространства между начальной и конечной точками траектории, т.е. такая ветвь будет заведомо ложной. Таким образом, если в качестве критерия оценки расположения случайной точки выбирать углы а1 и а2, то можно однозначно оценивать целесообразность ее присоединения к дереву и исключать заведомо ненужные вершины.
Структурная схема предлагаемого модифицированного алгоритма представлена на рис. 6. Осуществляется инициализация деревьев из начальной и конечной точек траектории. Затем определяется угол расширения дерева, по умолчанию равный 90°, что позволяет обходить вертикальные препятствия перед деревом. Однако необходимо проверить, не окружено ли дерево препятствиями так, что обойти их, учитывая ограничение на значение этого угла, невозможно.
Поиск пути соединения деревьев
Нет
Путь найден?
Да
Построение траектории
Инициализация случайной конфигурации
Поиск ближайших соседей
• 1
Определение углов а,-
Добавление вершины к дереву
Да , L
Угол а,>0 и меньше угла расширения
Нет
Рис. 6
Для проверки предлагается следующий алгоритм (рис. 7). От стартовой точки дерева qstart опускаются две перпендикулярные к линии qstart-qgoal и друг к другу прямые. Выполняется поиск пересечения этих прямых с препятствиями. Если есть пересечение, то выполняется поиск крайних точек сфер и сравнение их отдаленности от qstart. На расстоянии, равном шагу алгоритма от самой удаленной точки, вводится другая точка и осуществляется построение треугольника abc, что позволяет определить угол расширения а^т, который вычисляется с использованием теоремы косинусов по формуле
f b2
а
lim
= arceos
2 2 ^ c - a
2bc
90.
Шаг
qgoal -•
Рис. 7
Следующий этап модифицированного алгоритма — проверка возможности соединения деревьев. Если соединение возможно, то происходит построение траектории, если же невозможно, то инициализируется случайная точка, выполняется поиск ближайших соседей и оп-
ределяются углы аг-. Если угол аг- не превышает значения угла расширения, то точка добавляется как вершина дерева и запускается следующая итерация.
Представленный модифицированный алгоритм исключает построение лишних ветвей дерева, что позволяет ускорить процесс планирования движения и снизить объем используемой памяти. Для определения степени улучшения этих показателей проводятся моделирование и сравнительный анализ алгоритмов.
Сравнение алгоритмов. Сцена для проведения эксперимента представляет собой антропоморфного робота ЮиЬ, стену с окном, стол с предметами (рис. 8).
Рис. 8
Задача робота — переместить руку с захваченным предметом через препятствие (горизонтальное или вертикальное) без столкновений с объектами окружающей среды. Для решения задействуется три сочленения робота: два в плече и одно в локте. Это необходимо для визуализации конфигурационного пространства, в котором происходит планирование, поскольку пространство размерностью выше трех визуализировать невозможно, а управления этими тремя сочленениями достаточно для перемещения руки по всему рабочему пространству. Каждый алгоритм запускается 10 раз для каждой задачи. На рис. 9, а—в представлены несколько траекторий, полученных по результатам работы алгоритмов: а — двунаправленного ЯЯТ; б — интеллектуального двунаправленного ЯЯТ; в — модифицированного интеллектуального двунаправленного ЯЯТ.
а)
Чз,
100
80
60
40
20
0
100 80
б)
qз,
100 80 60 40 20
?50 100
80 60
40
20 0
Ч2,
-100
-80
-60 Чи •
-40 0
50
-100
-60
-20
0 Ч1,
в)
О
о
о
0
о
о
Чз, •.. 100 80 60 40 20 0
Ч1, •••
150
100
Ч2, •••
50
0
-40 -80
0
Рис. 9
Проанализировав представленные рисунки, можно сделать вывод, что алгоритм модифицированного интеллектуального двунаправленного быстроисследующего случайного дерева выполняет построение траектории без лишних ветвей в отличие от других алгоритмов, которые заполняют ими пространство. Показатели работы алгоритмов для задачи с горизонтальным (horisont obstacle — HO) и вертикальным (vertical obstacle — VO) препятствиями представлены в таблице.
Алгоритм Среднее время планирования, с Среднее число вершин Количество успешных реализаций, % Длина пути в конфигурационном пространстве, о.е.
НО VO НО VO НО VO НО VO
RRT 938 703 3724 2343 50 50 640 691
Двунаправленное RRT 41 35 580 457 90 90 570 541
Интеллектуальное
двунаправленное RRT 55 71 604 633 90 90 530 623
Модифицированное
интеллектуальное двунаправленное КЯТ 13 5 147 53 100 100 500 411
Анализ таблицы показывает, что модифицированный алгоритм имеет лучшие показатели, чем любой другой из рассмотренных. Этот алгоритм позволяет быстрее находить траекторию и использует меньший объем памяти. Показатель стоимости (длины) пути означает, что для заданного числа случайных точек алгоритм находит более короткое решение.
Заключение. Представлен новый алгоритм планирования пути для робототехнических систем — модифицированное интеллектуальное двунаправленное быстроисследующее случайное дерево; используется подход, исключающий добавление лишних вершин к деревьям на основе оценивания их положения в пространстве планирования. Данный алгоритм превосходит предыдущие модификации по скорости обнаружения пути и эффективности использования памяти.
СПИСОК ЛИТЕРАТУРЫ
1. Лю В. Методы планирования пути в среде с препятствиями (обзор) // Математика и математическое моделирование. 2018. № 1. С. 15—58. DOI: 10.24108/mathm.0118.0000098.
2. LaValle S. М. Rapidly-exploring random trees : A new tool for path planning // The Annual Research Report. Iowa State University, 1998. October.
3. Karaman S., Frazzoli E. Sampling-based algorithms for optimal motion planning // Intern. Journal of Robotics Research. 2011. Vol. 30. P. 846—894.
4. Левин Б. Р. Теоретические основы статической радиотехники. М.: Радио и связь, 1989. 656 с.
5. Jordan M., Perez A. Optimal bidirectional rapidly-exploring random trees // Tech. Rep. MITCSAIL-TR-2013-021. Massachusetts Institute of Technology, Cambridge, MA. 2013. August.
6. Qureshi A. H., Ayaz Y. Intelligent bidirectional rapidly-exploring random trees for optimal motion planning in complex cluttered environments // ArXiv. 2015. Vol. abs/1703.08944.
7. Tahir Z., Qureshi A. H., Ayaz Y., Nawaz R. Potentially guided bidirectionalized RRT* for fast optimal path planning in cluttered environments // Robotics Auton. Syst. 2018. Vol. 108. P. 13—27.
8. Gammell J., Srinivasa S., Barfoot T. Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic // IEEE/RSJ Intern. Conf. on Intelligent Robots and Systems. 2014. P. 2997—3004.
9. Burget F., Bennewitz M., Burgard W. Bi2RRT*: An efficient sampling-based path planning framework for task-constrained mobile manipulation // IEEE/RSJ Intern. Conf. on Intelligent Robots and Systems (IROS). 2016. P. 3714—3721.
10. Kingston Z., Moll M., Kavraki L. Exploring implicit spaces for constrained sampling-based planning // Intern. Journal of Robotics Research. 2019. Vol. 38. P. 1151—1178.
11. Hauser K. Motion and Path Planning. Berlin, Heidelberg: Springer, 2020. P. 1—11.
Илья Сергеевич Довгополик
Кирилл Артемов
Олег Игоревич Борисов
СейедХассан Забихифар Александр Николаевич Семочкин
Сведения об авторах магистр; Университет ИТМО, факультет систем управления и робототехники, лаборатория биомехатроники и энергоэффективной робототехники; инженер; E-mail: isdovgopolik@itmo.ru аспирант; Университет ИТМО, факультет систем управления и робототехники, лаборатория биомехатроники и энергоэффективной робототехники; инженер-исследователь; E-mail: kaartemov@itmo.ru канд. техн. наук; Университет ИТМО, факультет систем управления и робототехники, лаборатория биомехатроники и энергоэффективной робототехники; доцент; E-mail: borisov@itmo.ru канд. техн. наук; ПАО „Сбербанк", лаборатория робототехники; инженер-разработчик; E-mail: zabikhifar.s@sberbank.ru канд. физ.-мат. наук, доцент; ПАО „Сбербанк", лаборатория робототехники; гл. инженер-разработчик; E-mail: Semochkin.A.N@sberbank.ru
Поступила в редакцию 28.12.21; одобрена после рецензирования 10.01.22; принята к публикации 18.01.22.
REFERENCES
1. Liu W. Mathematics and Mathematical Modelling, 2018, no. 01, pp. 15-58, DOI: 10.24108/mathm.0118.0000098.
2. LaValle S. TR 98-11, Computer Science Dept., Iowa State University, October 1998.
3. Karaman S. and Frazzoli E. The International Journal of Robotics Research, 2011, vol. 30, pp. 846-894.
4. Levin B.R. Teoreticheskiye osnovy staticheskoy radiotekhniki (Theoretical Foundations of Static Radio Engineering), Moscow, 1989, 656 p. (in Russ.)
5. Jordan M. and Perez A. Tech. Rep. MITCSAIL-TR-2013-021, Computer Science and Artificial Intelligence Laboratory, Massachusetts Institute of Technology, Cambridge, MA, August 2013.
6. Qureshi A.H. and Ayaz Y. ArXiv, 2015, vol. abs/1703.08944.
7. Tahir Z., Qureshi A.H., Ayaz Y., and Nawaz R. Robotics Auton. Syst., 2018, vol. 108, pp. 13-27.
8. Gammell J., Srinivasa S., and Barfoot T. 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2014, pp. 2997-3004.
9. Burget F., Bennewitz M., and Burgard W. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2016, pp. 3714-3721.
10. Kingston Z., Moll M., and Kavraki L. The International Journal of Robotics Research, 2019, vol. 38, pp. 1151-1178.
11. Hauser K. Motion and Path Planning, Berlin, Heidelberg, Springer, 2020, pp. 1-11.
Data on authors
MSc; ITMO University, Faculty of Control Systems and Robotics, International Laboratory of Biomechatronics and Energy-Efficient Robotics; Engineer; E-mail: isdovgopolik@itmo.ru
Post-Graduate Student; ITMO University, Faculty of Control Systems and Robotics, International Laboratory of Biomechatronics and Energy-Efficient Robotics; Engineer-Researcher; E-mail: kaartemov@itmo.ru
PhD; ITMO University, Faculty of Control Systems and Robotics, International Laboratory of Biomechatronics and Energy-Efficient Robotics; Associate Professor; E-mail: borisov@itmo.ru
PhD; Sberbank, Robotics Laboratory; Engineer-Designer; E-mail: zabikhifar.s@sberbank.ru
PhD, Associate Professor; Sberbank, Robotics Laboratory; Chief Engineer-Designer; E-mail: Semochkin.A.N@sberbank.ru
Ilya S. Dovgopolik
Kirill Artemov
Oleg I. Borisov
Seyedhassan Zabihifar Aleksandr N. Semochkin
Received 28.12.21; approved after reviewing 10.01.22; accepted for publication 18.01.22.
