Министерство науки и высшего образования Российской Федерации

Санкт-Петербургский политехнический университет Петра Великого

Институт машиностроения, материалов и транспорта
Высшая школа автоматизации и робототехники

Работа допущена к защите

Директор ВШАиР
О. Н. Мацко
«___» ______________ 2023 г.

ВЫПУСКНАЯ КВАЛИФИКАЦИОННАЯ РАБОТА

МАГИСТЕРСКАЯ ДИССЕРТАЦИЯ

РАЗРАБОТКА АЛГОРИТМА ВЗАИМНОЙ КООРДИНАЦИИ

ПРОМЫШЛЕННЫХ РОБОТОВ В ЗАДАЧЕ ЛОКАЛЬНОГО
ПЛАНИРОВАНИЯ ТРАЕКТОРИИ ДВИЖЕНИЯ

по направлению подготовки 15.04.06 «Мехатроника и робототехника»

направленность (профиль) 15.04.06_04 «Робототехника»

Выполнил
студент гр. 3341506/10401 __________ И. С. Паньков
(подпись)

Научный руководитель
доцент ВШАиР, к.ф.-м.н. __________ А. А. Семакова
(подпись)

Консультант
по нормоконтролю __________ А. С. Габриель
(подпись)

Санкт-Петербург
2023
 Министерство науки и высшего образования Российской Федерации
Санкт-Петербургский политехнический университет Петра Великого
Институт машиностроения, материалов и транспорта
Высшая школа автоматизации и робототехники

УТВЕРЖДАЮ
Директор ВШАиР
О. Н. Мацко
« 01 » декабря 2022 г.

ЗАДАНИЕ
на выполнение выпускной квалификационной работы

студенту Панькову Илье Сергеевичу, гр. 3341506/10401

(фамилия, имя, отчество (при наличии), номер группы)

1. Тема работы: Разработка алгоритма взаимной координации промышленных

роботов в задаче локального планирования траектории движения

2. Срок сдачи студентом законченной работы: июнь 2023 г.

3. Исходные данные по работе: Постановка задачи планирования траектории

движения в среде с двумя промышленными роботами в присутствии статических
и динамических препятствий

4. Содержание работы (перечень подлежащих разработке вопросов):

1) Аналитический обзор алгоритмов планирования движения, способных
работать в средах с несколькими роботами
2) Настройка окружения для симуляции работы алгоритма в виртуальной
среде с двумя роботами
3) Разработка и программная реализация алгоритма планирования движения
4) Тестирование алгоритма планирования движения
5. Перечень графического материала (с указанием обязательных чертежей):
Графический материал не предусмотрен

6. Консультанты по работе:

7. Дата выдачи задания: 01 декабря 2022 г.

Руководитель ВКР А. А. Семакова

(подпись) (инициалы, фамилия)

Задание принял к исполнению 01 декабря 2022 г.

(дата)

Студент И. С. Паньков
(подпись) (инициалы, фамилия)
 РЕФЕРАТ
78 страниц, 41 рисунок, 1 таблица, 33 источника, 3 приложения
КЛЮЧЕВЫЕ СЛОВА: ПРОМЫШЛЕННЫЙ РОБОТ, ПЛАНИРОВАНИЕ
ТРАЕКТОРИИ ДВИЖЕНИЯ, КООПЕРАЦИЯ РОБОТОВ, ROS, GAZEBO
Объектом исследования являются алгоритмы планирования траектории
движения, которые применяются в современных робототехнических системах
при координации движений нескольких промышленных роботов.
Целью работы является разработка алгоритма планирования движения,
способного работать в системах с двумя манипуляторами в присутствии стати-
ческих и динамических препятствий.
Рассмотрены существующие методы планирования движения, приведены
особенности их работы, а также преимущества и недостатки. На основании срав-
нения методов выбран метод эластичных лент для дальнейшей реализации в
среде ROS.
Осуществлена настройка окружения ROS для проведения моделирования.
Для проведения экспериментов выбран манипулятор KUKA KR 150 R3100-2 и
настроен пакет ROS для поддержки работы фреймворка MoveIt. Создан пакет
ROS для возможности запуска моделирования с несколькими роботами в симу-
ляторе Gazebo и отладки в средстве визуализации RViz.
Проанализированы существующие программные реализации алгоритмов
планирования движения в MoveIt, а также изучены реализации метода эластич-
ных лент для многоподвижных роботов. Написана собственная реализация алго-
ритма планирования движения по методу эластичных лент на языке C++ с ис-
пользованием среды ROS. Протестирована работа алгоритма в нескольких сце-
нариях.
 ABSTRACT
78 pages, 41 figures, 1 table, 33 sources, 3 appendices
KEY-WORDS: INDUSTRIAL ROBOT, TRAJECTORY PLANNING,
ROBOT COOPERATION, ROS, GAZEBO
The object of the research is the motion planning algorithms, which are used in
modern robotic systems for the motion coordination of several industrial robots.
The aim of the work is to develop a motion planning algorithm that is capable of
work in systems with two manipulators in the presence of static and dynamic obstacles.
The existing methods of motion planning are considered, the features of their
work, as well as their advantages and disadvantages are given. Based on the compari-
son of the methods, the elastic band method was selected for further implementation in
the ROS environment.
The setup of ROS environment for the simulation is done. KUKA KR 150
R3100-2 manipulator was selected for the experiments and ROS package was config-
ured to support the MoveIt framework. Created ROS package to be able to run multi-
robot simulation in the Gazebo simulator and debugging in the RViz visualization tool.
Existing software implementations of motion planning algorithms in MoveIt
have been analyzed and implementations of the elastic band method for multi-degree-
of-freedom robots have been studied. Novel realization of motion planning algorithm
based on elastic band method using C++ programming language and ROS environment
is written. The algorithm is tested in several scenarios.
 СОДЕРЖАНИЕ
Введение ....................................................................................................................... 6
1 Обзор алгоритмов планирования движения ........................................................ 8
1.1 Метод эластичных лент ................................................................................... 9
1.2 Метод потенциальных полей ........................................................................ 13
1.3 Метод на основе стратегии ........................................................................... 16
1.4 Ориентированный на задачу метод.............................................................. 19
1.5 Сравнение методов планирования движения ............................................. 21
2 Настройка рабочего окружения ROS ................................................................. 23
2.1 Средство настройки манипулятора MoveIt Setup Assistant ....................... 24
2.2 Средство визуализации RViz ........................................................................ 30
2.3 Симулятор Gazebo ......................................................................................... 31
3 Разработка алгоритма планирования движения................................................ 40
3.1 Существующие реализации алгоритмов планирования движения .......... 40
3.2 Существующие реализации метода эластичных лент ............................... 41
3.3 Особенности программной реализации метода эластичных лент ............ 42
3.4 Эксперименты и отладка в MATLAB .......................................................... 44
4 Реализация алгоритма в ROS и моделирование в Gazebo ............................... 49
4.1 Обнаружение препятствий ............................................................................ 50
4.2 Планирование движения ............................................................................... 52
4.3 Управление движением по траектории ....................................................... 56
4.4 Движение по пересекающимся траекториям .............................................. 60
4.5 Движение по параллельным траекториям................................................... 62
4.6 Движение по траекториям в среде с препятствиями ................................. 64
4.7 Анализ результатов моделирования ............................................................ 66
Заключение ................................................................................................................ 67
Список использованных источников ...................................................................... 69
Приложение А Файл загрузки модели робота ....................................................... 73
Приложение Б Файл загрузки параметров и узлов робота ................................... 74
Приложение В Файл конфигурации регуляторов приводов ................................ 77

5
 ВВЕДЕНИЕ

Современное производство можно охарактеризовать высокой степенью ав-

томатизации и механизации процессов. Это достигается за счет применения про-
мышленных роботов различного назначения при выполнении разнообразных
технологических операций [1]: транспортировка грузов, точечная и дуговая
сварка, токарная и фрезерная обработка, сборка изделий и др.
Многие задачи в робототехнике требуют кооперации нескольких роботов
для более эффективного их решения с точки зрения скорости исполнения. Дру-
гие задачи в принципе не могут быть решены одним роботом, поскольку пред-
полагают разделение ролей роботов в процессе выполнения таких задач. Приме-
ром первых и вторых задач может послужить транспортировка груза, слишком
тяжелого для одного манипулятора, или сварка изделия сложной формы, для
чего один из роботов должен держать заготовку и различным образом ее ориен-
тировать, а второй — наносить сварной шов [2]. Иллюстрация кооперации двух
роботов представлена на рисунке В.1.

а) б)

Рисунок В.1 — Кооперация двух роботов [16]: а) — для транспортировки

тяжелого объекта, б) — для сварки детали сложной формы

При кооперации нескольких роботов возникает необходимость взаимной

координации их движений с целью избежания столкновений друг с другом и про-
чими препятствиями. Данная задача хорошо известна исследователям, и суще-
ствует огромное количество как ранних [3], так и современных [4] работ,

6
посвященных таким алгоритмам планирования движения (в большинстве своем
для мобильных роботов).
При этом алгоритмам планирования движения и взаимной координации
«классических» промышленных манипуляторов отводится куда меньше внима-
ния, и потому частым явлением оказывается их «ручное» программирование. Та-
кой подход нельзя назвать гибким, поскольку он требует модернизации последо-
вательности команд даже при незначительных изменениях в постановке задачи,
и потому необходим метод, позволяющий автоматизировать процесс планирова-
ния движения при взаимодействии нескольких манипуляторов.
Целью данной работы является разработка алгоритма планирования траек-
тории движения для работы в робототехнических системе с двумя манипулято-
рами в присутствии статических и динамических препятствий.
Для достижения поставленной цели требуется решить следующие задачи:
− проанализировать научно-техническую литературу, посвященную ал-
горитмам планирования движения в среде с несколькими промышленными
роботами,
− классифицировать основные подходы для решения задачи планирова-
ния движения, выделить их преимущества и недостатки,
− определить перспективные методы планирования движения для даль-
нейшего исследования и реализации,
− программно реализовать собственный алгоритм в среде ROS и осуще-
ствить моделирование в симуляторе Gazebo.

7
 1 Обзор алгоритмов планирования движения

Ряд задач, возникающих в робототехнике, связаны с перемещением робота

из одной точки в другую, через последовательность точек или по определенной
траектории. Решение задачи по перемещению робота состоит из двух частей:
планирование движения (англ. Motion Planning) и управление движением (англ.
Motion Control) [5].
Задача планирования движения заключается в определении закона движе-
ния робота, который сможет обеспечить требуемое перемещение. Данную задачу
решает специальный алгоритм — планировщик движения (англ. Motion Planner).
Сперва планировщик движения решает подзадачу планирования пути
(англ. Path Planning), чтобы определить, какая последовательность перемещений
позволит роботу достичь целевой точки в пространстве и избежать столкновений
с препятствиями — как статическими, так и динамическими. Затем планировщик
движения определяет закон движения приводов робота для обеспечения переме-
щения с требуемыми кинематическими и/или динамическими ограничениями —
решает подзадачу планирования траектории (англ. Trajectory Planning). Совокуп-
ность этих двух процессов и называется планированием движения.
В дальнейшем желаемый закон движения передается системе управления
(англ. Control System), которая уже решает задачу управления движением робота
(а вернее, его приводами), формируя необходимые управляющие воздействия.
Однако рассмотрения систем управления находятся за рамками данной работы.
В ходе написания работы был изучен ряд статей [2–4, 6–16], посвященных
алгоритмам планирования движения в системах с несколькими роботами.
Можно выделить следующие подходы:
− метод эластичных лент (англ. Elastic Band Method),
− метод потенциальных полей (англ. Potential Field Method),
− метод на основе стратегии (англ. Strategy-Based Method),
− ориентированный на задачу метод (англ. Task-Oriented Method).
Далее будет рассмотрен подробно каждый из указанных подходов.

8
 1.1 Метод эластичных лент

Метод эластичных лент впервые описан в статье [6] С. Квинланом

(S. Quinlan) и О. Хатибом (O. Khatib) в 1993 году. С тех пор он использовался во
многих работах, но не претерпел существенных изменений.
Пример работы системы с планировщиком движения на основе метода эла-
стичных лент представлен на рисунке 1.1. В первую очередь в дело вступает гло-
бальный планировщик пути, который пытается найти путь в прореженном про-
странстве, как это можно видеть на рисунке 1.1, а).

а) б)

в) г)

Рисунок 1.1 — Работа планировщика на основе метода эластичных лент [6]:

а) — планирование пути, б) — оптимизация пути для сглаживания
траектории, в) и г) — деформация траектории препятствием

Полученный путь оказывается слишком ломаным и, скорее всего, не будет

удовлетворять кинематическим ограничениям. Поэтому затем запускается ло-
кальный планировщик, который «прикладывает» к кривой две силы: внутрен-
нюю сжимающую силу и внешнюю растягивающую силу. Сжимающая сила
«натягивает» кривую, сглаживая полученный путь, а растягивающая сила «от-
талкивает» ее подальше от препятствий для обеспечения безопасной области для
робота, как это показано на рисунке 1.1, б). Также динамические препятствия
дополнительно «деформируют» путь, что обрабатывается локальным планиров-
щиком в реальном времени, как это показано на рисунках 1.1, в) и 1.1, г).
9
 Стоит отметить, что данный метод эффективно работает только при незна-
чительных изменениях в рабочем пространстве робота. Если же динамические
препятствия переместились таким образом, что ранее найденный глобальным
планировщиком путь уже не существует, алгоритм должен быть запущен заново
для поиска и оптимизации нового пути.
Для проверки на этапе деформации кривой того, что путь действительно
лежит в свободном пространстве, конфигурационное пространство робота необ-
ходимо проверять на наличие коллизий. Построение конфигурационного про-
странства — вычислительно сложная задача, сложность которой растет экспо-
ненциально с увеличением числа степеней подвижности робота [7]. Для приме-
нения в реальных системах необходима более простая реализация.
С целью упрощения вычислений траектория представляется в виде после-
довательности «пузырей» — сферических примитивов с центрами, лежащими на
кривой, которая соответствует найденному пути. Каждый пузырь имеет опреде-
ленный радиус — этот радиус тем меньше, чем ближе к пузырю препятствие.
Расстояние между двумя соседними пузырями также зависит от их размеров —
чем меньше радиусы пузырей на участке пути, тем плотнее они расположены,
поскольку тем ближе к роботу будут препятствия на данном участке траектории.
Совокупность таких пузырей образует эластичную ленту. Пример результата ра-
боты метода для робота в двумерном пространстве представлен на рисунке 1.2.

Рисунок 1.2 — Эластичная лента из последовательности пузырей [6]

10
 С математической точки зрения каждый пузырь описывает некоторую об-
ласть пространства B , свободную от препятствий:
B(b) = q : d (b − q)  (b), (1.1)
где q — вектор обобщенных координат робота,
b — вектор обобщенных координат, соответствующий центру пузыря,
(q ) — функция расстояния от вектора q до ближайшего препятствия,
d (q − q) — функция расстояния между векторами q и q ; в простей-

шем случае d (q − q)  q − q .

На каждый i -й пузырь действует сжимающая сила fc со стороны (i − 1) -го

и (i + 1) -го пузырей, кроме пузырей, соответствующих начальной и конечной
точкам. Данная сила имеет следующий вид:
 b −b b −b 
f c = kc  i −1 i + i +1 i  , (1.2)
 bi −1 − bi bi +1 − bi 
где kc — коэффициент усиления сжимающей силы.
Также со стороны ближайшего препятствия на i -й пузырь действует рас-
тягивающая сила f r , принимающая следующий вид:

 
 k r ( 0 −  ) T ,    0
fr =  bi , (1.3)
 0,   0

где kr — коэффициент усиления растягивающей силы,
0 — максимальный радиус действия силы,

 q — градиент функции расстояния до ближайшего препятствия:

T

 (q − hq 0 ) − (q + hq 0 ) 
 1   q ij = qi , i = j
  , q   q i = 0, i  j , i = 0, N .
i
(1.4)
qT 2h  N   j
 (q − hq ) − (q + hq ) 
N

В оригинальной работе метод описан для мобильного робота в двумерном

пространстве, однако в более поздней публикации [8] О. Брока (O. Brock) и
О. Хатиба (O. Khatib) данный подход применялся уже для манипулятора

11
Mitsubishi PA-10. Эластичная лента из пузырей строилась не только вдоль траек-
тории рабочего органа, но и вдоль траектории каждого шарнира, образуя «эла-
стичный туннель», как это показано на рисунке 1.3.

Рисунок 1.3 — Эластичный туннель манипулятора [8]

Наличие эластичного туннеля позволяет нескольким манипуляторам взаи-

модействовать в одной рабочей ячейке без опасения, что они столкнутся друг с
другом, например, как это показано на рисунке 1.4. Ценой тому выступает более
высокая вычислительная сложность алгоритма при применении эластичных лент
для планирования пути каждого шарнира, которая растет также и с увеличением
количества взаимодействующих манипуляторов [9].

а) б)

в) г)

Рисунок 1.4 — Взаимодействие двух манипуляторов в рабочей ячейке [8]:

а) — найденный путь, б) и в) — деформация эластичного туннеля при
приближении роботов друг к другу, г) — возврат на ранее найденный путь
12
 1.2 Метод потенциальных полей

Метод потенциальных полей впервые представлен О. Хатибом (O. Khatib)

в публикации [10] в 1986 году и представляет из себя метод планирования дви-
жения в среде с препятствиями как для мобильных роботов, так и для манипуля-
торов.
Предполагается, что уравнение динамики робота описывается дифферен-
циальным уравнением
Μ ( x ) x + C ( x, x ) x + G ( x ) = F , (1.5)
где x — вектор положения робота
Μ — матрица инерции,
C — матрица центробежных и кориолисовых сил,
G — вектор гравитационных сил,
F — вектор сил, действующих на систему в рабочем пространстве.
При этом вектор сил F может быть преобразован к вектору движущих сил при-
водов робота Q следующим образом:
Q = J (q)T F , (1.6)
где q — вектор обобщенных координат робота,

J = x q — матрица Якоби системы.

T

Движение мобильного робота или рабочего органа манипулятора будет

описываться дифференциальным уравнением
F = Μ ( x) F  + C ( x, x) x + G ( x), (1.7)

где F  — направляющая «сила», «двигающая» робота в целевую точку.

Для формирование этой «силы» делается предположение, что робот нахо-
дится в искусственном потенциальном поле нескольких сил U ( x ) . Целевая точка
создает притягивающее поле, а препятствия создают отталкивающее поле, кото-
рое является суперпозицией полей всех препятствий в рабочем пространстве ро-
бота. Также на робот продолжает действовать поле гравитации:
U ( x ) = U xd ( x ) + U O ( x) + U G ( x), (1.8)

13
 где U xd ( x ) — притягивающее поле,

U O ( x) — отталкивающее поле,
U G ( x) — гравитационное поле.

Искусственная направляющая сила F  складывается из сил притяжения и

отталкивания:
F  = Fxd + FO , (1.9)

где Fxd = U xd ( x ) xT — притягивающая сила,

FO = U O ( x ) xT — отталкивающая сила.

Значение «энергии» притягивающего поля U xd ( x ) в точке задается про-

порциональным расстоянию до целевой точки

1
U xd ( x ) = k ( x d − x ) ( x d − x ) ,
T
(1.10)
2
благодаря чему притягивающая сила пропорциональна разнице векторов теку-
щего и желаемого положения робота:
Fxd ( x ) = −k ( x − xd ), (1.11)

где k — коэффициент усиления притягивающего поля.

В то же время отталкивающее поле U Oi ( x) i -го препятствия задано более

сложным образом:
1
( )
2
  1 Oi − 1 0 , Oi  0
U Oi =  2 , (1.12)
 0, Oi  0

где  — коэффициент усиления отталкивающего поля,
Oi — расстояние до i -го препятствия,

0 — максимальный радиус действия потенциального поля.

Тогда отталкивающая сила i -го препятствия может быть найдена как
 1 Oi

FOi = 
(1 Oi
− 1  0 )
O2 i xT
, Oi  0
. (1.13)
0, Oi  0

14
 Действие всех отталкивающих полей препятствий складывается, давая
суммарную отталкивающую силу:
U O = U Oi , (1.14)
i

FO =  FOi . (1.15)

i

В итоге задача планирования движения сводится к задаче нахождения ми-

нимума, например, методом градиентного спуска. Траектория будет представ-
лять собой интегральную кривую в потенциальном поле, а коэффициенты уси-
ления притягивающей и отталкивающей сил модифицировать представление
этого поля [11]. Например, коэффициент  влияет на область безопасного про-
странства робота, как это показано на рисунке 1.5.

Рисунок 1.5 — Работа планировщика на основе метода потенциальных

полей с разными коэффициентами усиления отталкивающего поля [11]

Несмотря на кажущуюся простоту метода, его вычислительная сложность

так же, как и в случае с методом эластичных лент, растет с увеличением числа
степеней подвижности робота, а также при использовании данного подхода в си-
стемах с несколькими роботами.
Однако главный недостаток метода потенциальных полей связан с отказом
алгоритма с препятствиями сложной формы, где может оказаться помимо гло-
бального минимума несколько локальных [9]. Данное обстоятельство требует
введения эвристик для корректной работы этого метода.
15
 1.3 Метод на основе стратегии

Метод на основе стратегии описала М. Медиавилла (M. Mediavilla) с соав-

торами в своих работах [12, 13]. Данный подход вводит понятие стратегии дви-
жения робота (англ. Motion Strategy) — набора правил, ограничивающих область
конфигурационного пространства робота, в котором алгоритм осуществляет пла-
нирование траектории движения.
Для конкретной задачи и заданной конфигурации робототехнической си-
стемы планировщик движения должен подобрать подходящую стратегию и убе-
диться, что ограниченная этой стратегией область пространства позволяет по-
строить путь без столкновения с препятствиями. По этой причине метод на ос-
нове стратегии разделен на два этапа: оффлайн- и онлайн-стадию, как это пока-
зано на рисунке 1.6.

Рисунок 1.6 — Структура планировщика движения на основе стратегии

Оффлайн-стадия начинается с того, что пользователь вводит информацию

о робототехнической системе: начальные положения роботов, конфигурации их
шарниров, вид рабочего пространства и присутствующих в нем препятствий.
16
Также пользователю необходимо обозначить задачу, которая будет стоять перед
каждым из роботов.
Затем планировщик движения анализирует введенную информацию и
определяет, какие проблемы могут возникнуть при движении роботов: столкно-
вения двух роботов друг с другом, столкновения робота со статическим препят-
ствием и др. Для каждой проблемы планировщик выдвигает стратегии движения,
которые позволяют избежать ее возникновения. Специальный алгоритм оценки
вероятности отказа (англ. Estimation of the Probability of Fault, EPF) выполняет
множество симуляций для каждой стратегии движения, после чего отбирает
лишь те, которые способны решить конкретную проблему согласно его оценке.
На следующем этапе осуществляется оптимизация выбранных стратегий
движения. Алгоритм выбирает стратегии движения для роботов среди ранее ото-
бранных и на основе нескольких симуляций определяет среднее время исполне-
ния задачи. После этого в качестве оптимальной выбирается стратегия с мини-
мальным временем исполнения.
Как ранее было указано, каждая стратегия движения описывает некоторую
область конфигурационного пространства робота SC — сокращенное подпро-
странство S R (англ. Reduced Subspace, R-subspace). Данное подпространство
описывается следующим образом:
S R =  q  SС q = q0 + 1u1 +  2u2 , qF = q0 + qF − q0 u1 , (1.16)

где q0 и qF — начальный и конечный векторы обобщенных координат,

u1 — единичный вектор, направленный вдоль прямой, соединяющей
векторы q0 и qF ,
u2 — единичный вектор, направление которого зависит от выбранной
стратегии движения,
1 и  2 — скалярные параметры.
Как можно видеть, пространство является линейным и двумерным, благодаря
чему планирование движения в нем упрощается.

17
 На онлайн-стадии метода на основе стратегии управление передается ло-
кальному планировщику, работающему в реальном времени. Благодаря тому,
что пространство является двумерным, поиск пути и генерация траектории про-
исходят с высокой скоростью даже для робототехнических систем с несколь-
кими манипуляторами [9]. Примеры работы планировщика движения с разными
выбранными стратегиями движения представлены на рисунке 1.7.

а) б)

Рисунок 1.7 — Работа планировщика на основе стратегий движения [13]:

а) — перемещение над препятствием, б) — перемещение перед препятствием

Несмотря на свои преимущества в виде нахождения оптимального времени

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

Также в ряде работ [14–16] упоминается ориентированный на задачу ме-

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

Рисунок 1.8 — Схема взаимодействия кооперативного планировщика

и локального планировщика отдельного манипулятора

Предварительно пользователю необходимо указать конфигурацию робото-

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

а) б)

Рисунок 1.9 — Конфигурации систем [14]: а) — с одним позиционирующим

роботом, б) — с несколькими позиционирующими роботами
19
 Манипуляторы могут выполнять различные роли в рабочей ячейке. Пози-
ционирующие роботы (англ. Positioners) держат заготовки, перемещают и ориен-
тируют их. Рабочие роботы (англ. Workers) выполняют разнообразные техноло-
гические операции по обработке заготовок. В зависимости от задачи пользова-
тель может выбирать один или несколько позиционирующих манипуляторов, а
остальные будут исполнять роли рабочих.
После определения конфигурации системы пользователь должен ввести
последовательность команд для решения задачи. Данные команды должны быть
подобраны на основе анализа поставленной задачи и включать информацию о
том, какие роботы должны выполнять те или иные действия в той или иной по-
следовательности — кооперативно или поодиночке.
Кооперативный планировщик анализирует введенную информацию и осу-
ществляет планирование пути и генерацию траекторий в кооперативном опера-
ционном пространстве для всех роботов. Полученные траектории сэмплируются
во времени и подаются на вход локальным планировщикам манипуляторов для
дальнейшей обработки.
Локальный планировщик получает траекторию в виде кривой в операци-
онном (декартовом) пространстве, после чего ему необходимо решить обратную
задачу кинематики для определения законов движения шарниров манипулятора
в пространстве конфигураций. Также планировщик осуществляет масштабиро-
вание траектории во времени, если на каких-то ее участках нарушаются кинема-
тические ограничения.
Однозначным преимуществом данного метода является то, что данный
подход позволяет решить практически любую по сложности задачу, обеспечивая
работу нескольких роботов в кооперации. Однако стоит помнить о том, что для
работы данного метода необходимо заранее распределять роли манипуляторов в
рабочей ячейке и разрабатывать последовательность команд для решения по-
ставленной задачи. Кроме того, как и в случае с предыдущим методом, данный
подход подразумевает наличие длительного этапа по предварительной обра-
ботке данных алгоритмом [16].

20
 1.5 Сравнение методов планирования движения

В предыдущих подразделах 1.1–1.4 в деталях были описаны методы пла-

нирования движения, а также приведены их основные преимущества и недо-
статки. Сравнение существующих подходов представлено в таблице 1.1.

Таблица 1.1 — Результаты сравнения методов планирования движения

Метод
Метод эластичных лент
(Elastic Band Method)
Метод потенциальных полей
(Potential Field Method)
Метод на основе стратегии
(Strategy-Based Method)
Ориентированный на задачу метод − надежность
(Task-Oriented Method)
Преимущества Недостатки
− обобщенность − высокая вычислительная
− простота идеи сложность в системах с
− простота реализации несколькими роботами
− обобщенность − отказ алгоритма в
− простота идеи локальных минимумах
− простота реализации
− надежность − длительная стадия
− высокая скорость предварительной обработки
работы на онлайн-этапе на оффлайн-этапе
− необходимость создания
базы стратегий движения
− длительная стадия
− высокая скорость предварительной обработки
работы на онлайн-этапе на оффлайн-этапе
− необходимость создания
последовательности
команд

Как можно видеть, последние два подхода имеют устойчивость к отказам

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

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

22
 2 Настройка рабочего окружения ROS

В настоящее время при проектировании манипуляторов используется не-

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

Рисунок 2.1 — Кинематическая схема манипулятора

Большинство промышленных манипуляторов от крупных производителей

робототехники также обладает аналогичной кинематической схемой. В качестве
модели возьмем манипулятор KUKA KR 150 R3100-2 QUANTEC производителя
KUKA Roboter, поскольку он обладает внушительным радиусом рабочей зоны
более трех метров и способен поднять груз массой свыше 150 килограммов [17].
Поддержка манипуляторов KUKA в ROS осуществляется с помощью
набора ROS-пакетов KUKA Experimental [18], являющегося частью проекта
ROS-Industrial. Среди указанного набора пакетов имеется в том числе и пакет
KUKA KR 150 Support для поддержки манипуляторов серии KR 150.

23
 2.1 Средство настройки манипулятора MoveIt Setup Assistant

Средство настройки MoveIt Setup Assistant предназначено для создания

ROS-пакета манипулятора с целью его взаимодействия с фреймворком MoveIt и
средством визуализации RViz [19]. Данное программное обеспечение позволяет:
− настроить кинематические группы (звенья и кинематические пары, об-
разующие открытые кинематические цепи), используемые при планировании
движения манипулятора (англ. Planning Groups),
− выбрать для кинематических групп планировщик, используемый при
планировании пути (англ. Path Planner),
− указать предпочтительный алгоритм решения обратной задачи кинема-
тики (англ. Inverse Kinematics Solver) и настроить его параметры,
− настроить регуляторы положения (англ. Position Controller), скорости
(англ. Velocity Controller), траектории (англ. Trajectory Controller),
− выбрать звенья манипулятора, относящиеся к рабочему инструменту
(англ. End Effector) и др.
Интерфейс программы представлен на рисунке 2.2.

Рисунок 2.2 — Интерфейс программы MoveIt Setup Assistant

24
 Создадим с помощью MoveIt Setup Assistant конфигурационный ROS-па-
кет kuka_kr150r3100_2_moveit_config, в котором осуществим настройку мани-
пулятора KUKA KR 150 R3100-2 на основе его URDF-описания (англ. Universal
Robot Description Format), представленного в пакете KUKA Experimental.
2.1.1 Матрица коллизий
Сгенерируем матрицу коллизий. Данная матрица определяет взаимосвязи
между подвижными звеньями манипулятора и позволяет осуществлять планиро-
вание движения, исключающее столкновение звеньев друг с другом. Фрагмент
матрицы коллизий представлен на рисунке 2.2.

Рисунок 2.2 — Генерация матрицы коллизий

2.1.2 Виртуальный шарнир

Для описания перехода из глобальной системы координат в систему коор-
динат манипулятора используется так называемый «виртуальный шарнир» (англ.
Virtual Joint). В действительности данный шарнир не является кинематической
парой и имеет тип «зафиксированный» (англ. Fixed). Создадим виртуальный
шарнир с именем fixed_joint, расположенный в глобальной системе координат
world, и укажем в качестве дочернего звена звено base_link — в дальнейшем это

25
понадобится для интеграции манипулятора в симулятор. Создание виртуального
шарнира показано на рисунке 2.3.

Рисунок 2.3 — Создание «виртуального шарнира»

2.1.3 Кинематические группы для планирования движения

Для решения задачи планирования движения и избегания взаимных столк-
новений во время перемещения звеньев планировщику движения необходимо
знать, какие кинематические пары (шарниры) и звенья манипулятора должны
участвовать в планировании. Данные звенья и шарниры образуют «кинематиче-
ские группы для планирования» (англ. Planning Groups).
Кинематические группы могут быть заданы различным образом: с помо-
щью набора только шарниров и звеньев, кинематических цепей (последователь-
ности из звеньев и шарниров) или ранее определенных кинематических групп.
Для каждой кинематической группы должно быть создано уникальное имя, а
также определены алгоритмы, которые будут использоваться при решении об-
ратной задачи кинематики (англ. Kinematics Solver) и планировании пути
(англ. Group Planner).
Создадим кинематическую группу arm, включающую в себя все шесть ки-
нематических пар манипулятора: первые три шарнира относятся к «руке»

26
манипулятора (англ. Arm) и отвечают за его позиционирование, а последние три
шарнира — к его «кисти» (англ. Wrist) и отвечают за ориентацию рабочего ин-
струмента. В качестве решателя обратной задачи кинематики укажем алгоритм
из библиотеки KDL, а в качестве планировщика пути — алгоритм на основе ран-
домизированных быстрорастущих деревьев RRTstar, модернизированный алго-
ритмом поиска A*. Параметры планирования оставим по умолчанию. Настройка
кинематической группы представлена на рисунке 2.4.

Рисунок 2.4 — Создание кинематической группы для планирования движения

2.1.4 Стандартные положения

Для манипулятора можно определить набор стандартных положений —
конфигурации обобщенных координат его кинематических пар. Это облегчает
задачу планирования движения, поскольку устраняется необходимость решения
обратной задачи кинематики для часто используемых положений.
Определим для манипулятора единственную стандартную конфигурацию
шарниров — «домашнее» положение home. В этом положении второй и третий
шарниры повернуты на плюс и минус 120 градусов относительно своего нуле-
вого положения соответственно, а все остальные имеют нулевые обобщенные
координаты. Создание домашнего положения показано на рисунке 2.5.

27
 Рисунок 2.5 — Создание «домашнего» положения

2.1.5 Рабочий инструмент

Когда рабочий инструмент (англ. End Effector) представляет из себя слож-
ное многостепенное устройство, можно создать для него отдельное описание.
Однако в нашем случае «инструментом» является фланец выходного звена. Со-
здадим инструмент с родительским звеном tool0, который состоит из группы
arm, и назовем его end_effector, что показано на рисунке 2.6.

Рисунок 2.6 — Создание рабочего инструмента

28
 2.1.6 Регуляторы приводов шарниров
Управление приводами манипулятора осуществляется с помощью различ-
ных регуляторов системы ROS (англ. ROS Controllers): регуляторов положения
(англ. Position Controller), скорости (англ. Velocity Controller), траектории (англ.
Trajectory Controller). Данные регуляторы могут быть разного типа и управлять
положениями приводов, их скоростями, ускорениями и/или моментами.
Создадим регулятор траектории arm_controller, который управляет поло-
жениями (углами поворота) шарниров кинематической группы arm манипуля-
тора при движении по траектории. Параметры регулятора настроим позднее. Со-
здание регулятора траектории представлено на рисунке 2.7.

Рисунок 2.7 — Настройка регуляторов

2.1.7 Завершение настройки пакета

На этом создание конфигурационного пакета можно завершить. Пакет бу-
дет сгенерирован в директории kuka_kr150r3100_2_moveit_config по указанному
пути. В поддиректории config будут созданы конфигурационные yaml-файлы
для кинематических ограничений (joint_limits.yaml и cartesian_limits.yaml),
решателя обратной задачи кинематики (kinematics.yaml), планировщиков дви-
жения (chomp_planning.yaml, ompl_planning.yaml и stomp_planning.yaml),

29
регуляторов (ros_controllers.yaml) и некоторые другие, а также SRDF-описа-
ние манипулятора (англ. Semantic Robot Description Format). В поддиректории
launch будут созданы launch-файлы для запуска узлов ROS, необходимых для
функционирования фреймворка MoveIt и осуществления планирования движе-
ния. В том числе будет создан файл demo.launch для запуска демонстрационного
проекта в средстве визуализации RViz.

2.2 Средство визуализации RViz

RViz — это средство визуализации, входящее в состав системы ROS, ко-

торое предназначено для отображения на трехмерной сцене компонентов робо-
тотехнической системы в реальном времени [20].
Созданный с помощью MoveIt Setup Assistant конфигурационный пакет
может быть использован для запуска демонстрационного проекта в RViz с целью
осуществления планирования движения в интерактивном режиме. Посредством
маркера можно перемещать рабочий инструмент и менять его ориентацию, с по-
мощью команд Plan и Execute — планировать и осуществлять движение к вы-
бранному положению соответственно. Интерфейс программы с демонстрацией
планирования движения к произвольной точке представлен на рисунке 2.8.

Рисунок 2.8 — Интерфейс программы RViz

30
 2.3 Симулятор Gazebo

Gazebo — это трехмерный робототехнический симулятор с физическим

движком, позволяющий осуществлять моделирование кинематики и динамики
манипуляторов и других роботов [21]. Симулятор Gazebo поддерживает интегра-
цию различных систем под управлением ROS, взаимодействуя с ROS-узлами
(англ. Nodes) через темы (англ. Topics) и сервисы (англ. Services), а также эмули-
руя физические процессы и работу датчиков.
Для управления робототехническими системами в ROS используются узлы
из пакета ROS Control [22, 23]. В частности, менеджер регуляторов (англ. Con-
troller Manager) осуществляет контроль над существующими регуляторами и че-
рез аппаратный интерфейс состояний и команд шарниров взаимодействует с
приводами робота. Плагин Gazebo ROS Control [24] симулирует работу аппарат-
ной части робота, моделируя движение приводов робота и работу датчиков угла
поворота, и посылает менеджеру регуляторов обратную связь в виде показаний
координат шарниров, что показано на рисунке 2.9.

Рисунок 2.9 — Схема взаимодействия системы ROS и симулятора Gazebo [24]

31
 Несмотря на то, что при создании конфигурационного пакета были со-
зданы launch-файлы, предназначенные для осуществления симуляции в Gazebo
(в частности, demo_gazebo.launch), при попытке их запуска произойдет ошибка,
и моделирование не сможет быть запущено. Это связано с тем, что URDF-
описание манипулятора, предоставляемое набором пакетов KUKA Experimental,
не содержит достаточной для симулятора информации о динамических, инерци-
онных и некоторых других характеристиках робота.
Кроме того, сгенерированные файлы предназначены для запуска симуля-
ции лишь с одним роботом. Для поддержки работы Gazebo с несколькими робо-
тами необходимо не только дополнить URDF-описание манипулятора, но еще и
должным образом сконфигурировать ROS-узлы, отвечающие за планирование и
управление движением.
Для этого создадим новый пакет moveit_gazebo: в директории urdf будут
находиться копии URDF-описаний роботов, чтобы не приходилось вносить
правки в оригинальные пакеты, и дополнительные настройки, специфичные для
Gazebo, а в директории launch — launch-файлы для запуска необходимых узлов
с нужными параметрами в правильном порядке.
2.3.1 Загрузка модели робота в симулятор
За загрузку URDF-описания манипулятора на сервер параметров ROS и по-
следующее добавление модели в симулятор Gazebo отвечает launch-файл
spawn_robot.launch. Данный файл принимает в качестве параметров модель ро-

бота robot_name, пространство имен, в котором будут запущены ROS-узлы ро-

бота robot_ns, положение (x, y, z) и ориентацию (R, P, Y) модели в глобальной
системе координат, а также углы поворота шарниров (q1…q6). Текст файла
spawn_robot.launch представлен в листинге А.1 (см. приложение A).

2.3.2 Дополнение URDF-описания манипулятора

Для того, чтобы дополнить URDF-описание, можно не вносить правки для
каждого нового манипулятора, а воспользоваться макросами Xacro (XML Mac-
ros) с различными параметрами. Для этих целей были написаны макросы
robot_urdf, robot_gazebo и robot_control.

32
 2.3.2.1 Массовые характеристики звеньев
Для поддержки моделирования движения манипулятора физическим
движком симулятора его звенья должны обладать массовыми, или инерцион-
ными (англ. Inertial), характеристиками — массами, центрами масс, тензорами
инерции. Изначально URDF-описание манипулятора из пакета KUKA Experi-
mental этой информации не содержит. В открытых источниках эту информацию
также невозможно найти.
В URDF-описании манипулятора геометрические характеристики звеньев
описываются полигональными сетками (англ. Mesh) в stl-файлах, а их визуаль-
ная составляющая — текстурами в dae-файлах. Современные CAD-системы спо-
собны преобразовывать замкнутые поверхности в твердые тела, а для твердых
тел доступен расчет всех необходимых массовых характеристик. Таким образом
можно сделать грубую оценку масс и тензоров инерции звеньев с помощью твер-
дых тел, полученных из stl-файлов, задав им необходимую плотность.
Исходя из технических характеристик манипулятора KUKA KR 150
R3100-2 QUANTEC [17], его масса составляет m = 1105 кг , что при объеме по-
лученных твердых тел V = 0,85 м 3 дает плотность порядка  = 1300 кг м3 . Тогда
для звеньев манипулятора получаем следующие массовые характеристики:
 −0,12   11,18 −0,65 −0,65 
mbase = 219,90 кг, pC base =  0,00  м, I C base =  −0,65 13,58 0,00  кг  м 2 , (2.1)
 0,14   −0,65 0,00 21,80 
   
 0,01   12,01 1,90 0,26 
mlink1 = 283,29 кг, pC link1 =  −0,04  м, I C link1 =  1,90 21,27 0,06  кг  м 2 , (2.2)
 
 −0,12   0,26 0,06 21,84 
   
 0,66   10,07 2,15 −1,16 
mlink2 = 348,84 кг, pC link2 =  −0,14  м, I C link2 =  2,15 72,10 −0,31 кг  м 2 , (2.3)
  
 0,01   −1,16 −0,31 21,84 
   
 0,10   4,58 −0,28 0,09 
mlink3 = 188,32 кг, pC link3 =  −0,02  м, I C link3 =  −0,28 17,31 0,03  кг  м 2 , (2.4)
 −0,04   0,09 0,03 16,81 
  

33
  0,98   0,35 0,03 0,00 
mlink4 = 32,36 кг, pC link4 =  0,00  м, I C link4 =  0,03 0,56 0,00  кг  м 2 , (2.5)
 0,00   0,00 0,72 
   0,00

 0,05   0,08 −0,01 0,00 

mlink5 = 13,91 кг, pC link5 =  0,02  м, I C link5 =  −0,01 0,13 0,00  кг  м 2 , (2.6)
 0,00   0,00 0,00 0,13 
  
 −0,02   0,01 0,00 0,00 
mlink6  
= 1,40 кг, pC link5 =  0,00  м, I C link5 =  0,00 0,00 0,00  кг  м 2 , (2.7)
 0,00   0,00 0,00 
   0,00
где mi — масса i -го звена манипулятора,
pC i — радиус-вектор центра масс i -го звена манипулятора,

IC i — центральный тензор инерции i -го звена манипулятора.

URDF-описание манипулятора представлено в виде Xacro-макроса

robot_urdf, который находится в файле robot_urdf_macro.xacro. Дополним опи-

сание звеньев в Xacro-макросе этими характеристиками. Фрагмент обновленного

URDF-описания звеньев на примере первого звена манипулятора представлен в
листинге 2.1.
Листинг 2.1 — URDF-описание первого звена
001 <link name="link_1">
002 <visual>
003 <origin xyz="0 0 0" rpy="0 0 0"/>
004 <geometry>
005 <mesh filename="package://kuka_kr150_support/meshes/kr150r3100_2/vis-
ual/base_link.stl"/>
006 </geometry>
007 <xacro:material_kuka_orange/>
008 </visual>
009 <collision>
010 <origin xyz="0 0 0" rpy="0 0 0"/>
011 <geometry>
012 <mesh filename="package://kuka_kr150_support/meshes/kr150r3100_2/colli-
sion/base_link.stl"/>
013 </geometry>
014 </collision>
015 <inertial>
016 <origin xyz="0.01 -0.04 -0.12" rpy="0.00 0.00 0.00"/>
017 <mass value="283.89"/>
018 <inertia ixx="12.01" ixy="1.90" ixz="0.26" iyy="21.27" iyz="0.06"
019 izz="21.84"/>
</inertial>
020 </link>

34
 2.3.2.2 Динамические характеристики шарниров
Для корректного моделирования работы приводов манипулятора симуля-
тору необходима информация о типе шарнира (поворотный или призматиче-
ский), его кинематических ограничениях (минимальном qmin и максимальном
qmax значении обобщенной координаты, максимальной обобщенной скорости
qmax ), а также динамических характеристиках и ограничениях. Информацию о
последних необходимо добавить в URDF-описание манипулятора.
Динамическим ограничением, которое необходимо указать для шарниров,
является предельная обобщенная движущая сила Qmax (англ. Effort). Для пово-
ротных шарниров она соответствует предельному моменту Tmax , Н  м , который
способен развивать привод.
Техническая документация манипулятора KUKA KR 150 R3100-2 QUAN-
TEC не содержит информацию о характеристиках приводов, однако грубую
оценку для моментов можно сделать, анализируя длины звеньев и величину пре-
дельно допустимой нагрузки: привод каждого звена должен выдерживать эту
нагрузку, а также нагрузку, которую создают последующие звенья.
Также для шарниров можно указать коэффициент вязкого трения (англ.
Damping) kdamping , Н  м  с и момент трения покоя (англ. Friction) M friction , Н  м . По-

скольку в шарнирах используются подшипники качения, будем считать, что в

покое трение отсутствует, а вязкое трение мало и прямо пропорционально ско-
рости вращения. Тогда получим следующие динамические характеристики:
Tmax1 = 1350 Н  м, kdamping1 = 1,0 Н  м  c M friction1 = 0,0 Н  м, (2.8)

Tmax 2 = 1350 Н  м, kdamping2 = 1,0 Н  м  c M friction2 = 0,0 Н  м, (2.9)

Tmax 3 = 800 Н  м, kdamping3 = 1,0 Н  м  c M friction3 = 0,0 Н  м, (2.10)

Tmax 4 = 500 Н  м, kdamping4 = 1,0 Н  м  c M friction4 = 0,0 Н  м, (2.11)

Tmax 5 = 200 Н  м, kdamping5 = 1,0 Н  м  c M friction5 = 0,0 Н  м, (2.12)

Tmax 6 = 200 Н  м, kdamping6 = 1,0 Н  м  c M friction6 = 0,0 Н  м, (2.13)

35
 Дополним описание шарниров в Xacro-макросе этими характеристиками.
Фрагмент обновленного URDF-описания шарниров на примере шарнира первого
звена представлен в листинге 2.2.
Листинг 2.2 — URDF-описание шарнира первого звена
001 <joint name="joint_a1" type="revolute">
002 <origin xyz="0 0 0.645" rpy="0 0 0"/>
003 <parent link="base_link"/>
004 <child link="link_1"/>
005 <axis xyz="0 0 -1"/>
006 <limit effort="1350" lower="${radians(-185)}" upper="${radians(185)}"
velocity="${radians(105)}"/>
007 <dynamics damping="1.0" friction="0.0"/>
008 </joint>

2.3.2.3 Дополнительные характеристики звеньев

Симулятор Gazebo имеет ряд своих параметров для физического модели-
рования поведения твердых тел. Для звеньев манипулятора достаточно указать
коэффициенты сухого трения 1 и 2 , а также текстуру материала звеньев. Для
всех звеньев примем коэффициенты трения равными
1 = 0,2, 2 = 0,2. (2.14)
Для базового звена (англ. Base Link) и крепежного фланца (англ. Flange) — пер-
вое и шестое звенья — в качестве текстуры материала выберем матовый черный
(англ. Flat Black), а для всех остальных — ярко-красный (англ. Red Bright).
Специфичные для Gazebo характеристики звеньев описаны в Xacro-
макросе robot_gazebo, который находится в файле robot_gazebo_macro.xacro.
Фрагмент URDF-описания дополнительных характеристик звеньев на примере
первого звена манипулятора представлен в листинге 2.3.
Листинг 2.3 — URDF-описание дополнительных характеристик первого звена
001 <gazebo reference="link_1">
002 <material>Gazebo/RedBright</material>
003 <mu1>0.2</mu1>
004 <mu2>0.2</mu2>
005 </gazebo>

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

глобальной системы координат симулятора, в описание робота должно быть до-
бавлено фиктивное звено world, а также виртуальный шарнир, связывающий ба-
зовое звено манипулятора с этим звеном. Фрагмент соответствующего URDF-
описания манипулятора представлен в листинге 2.4.
36
Листинг 2.4 — URDF-описание фиктивного звена и «виртуального шарнира»
001 <link name="world"/>
002 <joint name="world-base_link" type="fixed">
003 <parent link="world"/>
004 <child link="base_link"/>
005 <origin xyz="${x} ${y} ${z}" rpy="${R} ${P} ${Y}"/>
006 </joint>

2.3.2.4 Характеристики приводов звеньев

Кроме всего прочего, URDF-описание манипулятора должно быть допол-
нено описанием приводов звеньев: типом передаточного механизма, передаточ-
ным отношением, разновидностью регулятора, который управляет приводом.
Данная информация содержится в Xacro-макросе robot_control, который нахо-
дится в файле robot_control_macro.xacro. Фрагмент URDF-описания приводов
на примере привода первого звена представлен в листинге 2.5.
Листинг 2.5 — URDF-описание привода первого звена
001 <transmission name="$drive_a1">
002 <type>transmission_interface/SimpleTransmission</type>
003 <joint name="$joint_a1">
004 <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInter-
face>
005 </joint>
006 <actuator name="$motor_a1">
007 <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInter-
face>
008 <mechanicalReduction>1</mechanicalReduction>
009 </actuator>
010 </transmission>

Также необходимо дополнить URDF-описание манипулятора информа-

цией о плагине Gazebo ROS Control, в котором указать пространство имен мани-
пулятора и параметры режима симуляции. Фрагмент URDF-описания плагина
Gazebo ROS Control представлен в листинге 2.6.
Листинг 2.6 — URDF-описание плагина Gazebo ROS Control
001 <gazebo>
002 <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
003 <robotNamespace>/{robot_ns}</robotNamespace>
004 <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
005 </plugin>
006 </gazebo>

2.3.3 Загрузка параметров и запуск узлов

За загрузку параметров на сервер параметров ROS и запуск ROS-узлов, не-
обходимых для работы одного конкретного робота, отвечает launch-файл
load_robot.launch. Также данный файл включает и файл spawn_robot.launch.

37
 Данный файл настраивает пространство имен робота: все параметры и все
узлы будут запущены не в глобальном пространстве /, а в пространстве
/robot_ns, что позволяет загружать в симуляцию произвольное количество раз-

личных манипуляторов при условии, что они имеют разные пространства имен.
Кроме того, robot_ns используется в качестве префикса для систем коор-
динат в ROS-пакете TF, отвечающем за публикацию преобразований координат
между звеньями робота и глобальной системой координат. Для этих же целей
запускаются узлы static_transform_publisher, публикующий преобразование
координат для базового звена робота, и robot_state_publisher, публикующий
преобразования координат между остальными звеньями робота.
Далее на сервер параметров ROS загружаются различные параметры ро-
бота: универсальное (URDF) и семантическое (SRDF) описание робота, пара-
метры регуляторов и решателя обратной задачи кинематики, кинематические
ограничения и настройки для алгоритмов планирования движения. Также узел
controller_manager загрузит существующие регуляторы приводов шарниров.

Затем запускается узел move_group — главный узел, обеспечивающий функцио-

нирование робота при взаимодействии с фреймворком MoveIt. Текст файла
load_robot.launch представлен в листинге Б.1 (см. приложение Б).

2.3.4 Настройка параметров регуляторов приводов

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

ональный K P , интегральный K I и дифференциальный K D коэффициенты, а

также некоторые другие.
Кроме того, для взаимодействия регуляторов с фреймворком MoveIt, паке-
том ROS Control и симулятором Gazebo необходимо указать кинематические
группы для планирования движения, предварительно определенные положения,
список шарниров и список доступных регуляторов для менеджера регуляторов.
Текст конфигурационного файла ros_controllers.yaml с описанием регуляторов
представлен в листинге В.1 (см. Приложение В).
38
 2.3.5 Запуск симуляции
После того как внесены все необходимые изменения в существующие
файлы, может быть запущена симуляция в Gazebo. Для этого сначала необхо-
димо запустить карту с нужным виртуальным миром Gazebo, а затем с помощью
launch-файла load_robot.launch загрузить необходимую модель робота с нуж-

ными глобальным положением и ориентацией, углами поворота шарниров и др.

Для отладочных целей после этого может быть запущено средство визуа-
лизации RViz, а затем для каждого робота можно осуществлять планирование
движения с использованием инструментов фреймворка MoveIt. Пример запу-
щенной симуляции на пустой сцене Gazebo empty.world c двумя манипулято-
рами KUKA KR 150 R3100-2 показан на рисунке 2.10, а), а планирование движе-
ния с помощью MoveIt в RViz — на рисунке 2.10, б).

а)

б)

Рисунок 2.10 — Запущенная симуляция: а) — Gazebo, б) — RViz

39
 3 Разработка алгоритма планирования движения

3.1 Существующие реализации алгоритмов планирования движения

Прежде чем разрабатывать собственную реализацию алгоритма планиро-

вания движения на основе метода эластичных лент, были проанализированы су-
ществующие реализации алгоритмов планирования движения — как присутству-
ющих в составе ROS, так и сторонних.
В составе фреймворка MoveIt имеется несколько глобальных планировщи-
ков движения:
1 Open Motion Planning Library (OMPL) — открытая библиотека методов
планирования движения в среде с препятствиями на основе семплирования про-
странства — рандомизированных быстрорастущих деревьев (англ. Rapidly-ex-
ploring Random Tree, RRT), вероятностных дорожных карт (англ. Probabilistic
Road Map, PRM) и др. [25],
2 Covariant Hamiltonian Optimization for Motion Planning (CHOMP) — ал-
горитм многопараметрической оптимизации траектории в среде с препятствиями
на основе метода градиентного спуска [26],
3 Stochastic Trajectory Optimization for Motion Planning (STOMP) — алго-
ритм планирования движения в среде с препятствиями на основе стохастической
оптимизации траектории [27],
4 Sequential Convex Optimization for Motion Planning (TrajOpt) — алгоритм
планирования движения в среде с препятствиями на основе методе последова-
тельного выпуклого программирования [28].
Как можно видеть, среди реализованных в MoveIt алгоритмов планирова-
ния нет метода эластичных лент. Все представленные алгоритмы являются гло-
бальными планировщиками движения и не способны реагировать на локальные
изменения в рабочем пространстве робота, не осуществляя остановку робота и
повторное планирование пути. Таким образом, данные методы не могут быть ис-
пользованы в качестве реактивного локального планировщика движения.

40
 3.2 Существующие реализации метода эластичных лент

Существует не так много программных реализаций алгоритма планирова-

ния движения на основе метода эластичных лент для многоподвижных манипу-
ляторов: с ростом числа степеней подвижности системы растет размерность век-
торов обобщенных координат, повышается сложность представления препят-
ствий и звеньев робота, и, как следствие, определение расстояния между препят-
ствием и роботом становится вычислительно трудной задачей.
Первой попыткой адаптации метода эластичных лент для робота с боль-
шим числом степеней подвижности стала работа [29], в рамках которой О. Брок
(O. Brock) и О. Хатиб (O. Khatib) представили Elastic Strip Framework — фрейм-
ворк на основе метода эластичных полосок.
В рамках данной работы для упрощения вычислений препятствия были
представлены в виде ограничивающих параллелограммов, а звенья робота — в
виде ломаных линий, которые покрывают наборы пузырей различного размера
по аналогии с методом эластичных лент. Все вычисления сжимающих и растя-
гивающих сил производятся в операционном пространстве робота. В результате
работы алгоритма для робота строится эластичный туннель из набора ломаных
линий, покрытых защитными пузырями, как показано на рисунке 3.1.

а) б)

Рисунок 3.1 — Работа метода эластичных полосок [29]:

а) — визуализация эластичных полосок, покрытых пузырями,
б) — деформация эластичного туннеля препятствием (пузыри не показаны)
41
 Похожая идея предложена в недавней публикации Т. Кота (T. Kot) с соав-
торами [30]. Авторы использовали коллаборативный манипулятор с шестью сте-
пенями подвижности, звенья которого описываются сеткой из множества точек,
расположенных на поверхности робота. Препятствия в рабочем пространстве ро-
бота детектируются системой технического зрения и представляются в виде ре-
гулярной воксельной сетки, каждый воксель которой представляет из себя от-
дельное препятствие и вносит вклад в создание отталкивающей силы. Пример
представления робота и препятствий приведен на рисунке 3.2.

а) б)

Рисунок 3.2 — Метод эластичных лент для коллаборативных роботов [30]:

а) — представление робота, б) — представление препятствия

3.3 Особенности программной реализации метода эластичных лент

В целях отладки была написана собственная реализация метода эластич-

ных лент на языке MATLAB с использованием Robotics System Toolbox. Для мо-
делирования использовался манипулятор KUKA KR 150 R3100-2.
Звенья робота представляются в виде ломаной линии, в узлах которой
находятся шарниры, а вдоль этой линии расположены сферы различного ради-
уса, повторяющие поверхность звеньев. Таким образом для каждого i -го звена
определено множество сфер

Li = ( pil , il ) : pil  Li , il  ( pi , Li ) , VLi  VLi , (3.1)

где Li — множество точек, принадлежащих i -му звену,

pil — координаты центра l -й сферы,

li — радиус l -й сферы,

42
 ( p, L) — расстояние от точки p до поверхности области L .
В свою очередь препятствия представляются в виде множества вокселей

O j = ( poj , l oj ) : poj  O j  , VOj  VO j , (3.2)

где O j — множество точек, принадлежащих j -му препятствию,

p oj — координаты центра o -го вокселя,

l oj — размер o -го вокселя.

Путь описывается как набор положений pe k и ориентаций  e k рабочего

органа манипулятора в k -й точке рабочего пространства:

P =  xe k = ( pe k e k )T : xe k = f (qk ) , (3.3)

где xe k = f (qk ) — основное уравнение прямой кинематики робота.

В каждой k -й точке pe k определяется виртуальная сила f k , которая опре-

деляется как сумма растягивающей f r k и сжимающей f c k сил. Сжимающая сила

вычисляется как
 p − pk p − pk   pk −1 − pk pk +1 − pk 
f c k = kc  k −1 + k +1 +
 c 
d +  , (3.4)
p
 k −1 − pk pk +1 − pk  p
 k −1 − pk pk +1 − pk 

где kc — коэффициент усиления сжимающей силы,

d c — коэффициент демпфирования сжимающей силы.

Для каждого вокселя pojk определяется ближайшая сфера pil k для вычисле-

ния расстояния до вокселя

(
iolj k = min poj k − pilk − il ,
i
) (3.5)

а затем и растягивающая сила j -го вокселя, действующая на i -е звено:

 poj k − pil k
kr ( 0 − i j k ) o , io jl k  0 ,
ol

fr i j k = p j k − pi k
l
(3.6)

0, io jl k  0 ,

где kr — коэффициент усиления растягивающей силы,

0 — радиус действия растягивающей силы.
43
 Для каждого i -го звена усредняется сумма растягивающих сил вокселей,
благодаря чему получается результирующая сила, действующая на i -е звено

fr i k =
 j
fr i j k 1, x  0,
, H ( x) = 
( )
(3.7)
jH fr i j k 0, x  0.

Силы всех звеньев манипулятора затем приводятся к выходному звену манипу-

лятора, давая в сумме результирующую растягивающую силу f r k .

Сумма сжимающей и растягивающей силы обуславливает ускорение k -й

точки пути в рабочем пространстве:

fk = ( fr k + fc k ) ,
1 1
pe k = m  1. (3.8)
m m
Благодаря этому эластичная лента, описывающая путь для манипулятора, про-
должает двигаться до тех пор, пока не перейдет в состояние покоя.
Траектория манипулятора вычисляется за счет параметризации получен-
ного пути по времени. Определение положений, скоростей и ускорений шарни-
ров между узловыми точками пути осуществляется с помощью кусочно-линей-
ной интерполяции.

3.4 Эксперименты и отладка в MATLAB

Описанный в предыдущем разделе алгоритм был реализован средствами

MATLAB, после чего был проведен ряд экспериментов. В первую очередь была
проверена работа алгоритма в среде без препятствий, чтобы проверить устойчи-
вость эластичной ленты, что показано на рисунках 3.3 и 3.4.
Затем было решено перейти к экспериментам в среде с динамическими
препятствиями. На рисунках 3.5 и 3.6 представлена работа алгоритма с препят-
ствием, движущимся горизонтально в направлении робота, а на рисунках 3.7 и
3.8 приведена работа алгоритма с вертикально движущимся препятствием.
В последнем представленном эксперименте в среде находится два препят-
ствия, одно из которых ограничивает рабочее пространство робота сверху, а вто-
рое — движется вертикально вверх, уменьшая область для маневра манипуля-
тора, что показано на рисунках 3.9 и 3.10.
44
 Рисунок 3.3 — Траектория движения
промышленного робота в среде без препятствий

а) б)

в) г)

Рисунок 3.4 — Графики движения

промышленного робота в среде без препятствий:
а) — модуль скорости рабочего органа, б) — углы поворота шарниров,
в) — угловые скорости шарниров, г) — угловые ускорения шарниров

45
 Рисунок 3.5 — Траектория движения промышленного
робота в среде с горизонтально движущимся препятствием

а) б)

в) г)

Рисунок 3.6 — Графики движения промышленного

робота в среде с горизонтально движущимся препятствием:
а) — модуль скорости рабочего органа, б) — углы поворота шарниров,
в) — угловые скорости шарниров, г) — угловые ускорения шарниров

46
 Рисунок 3.7 — Траектория движения промышленного
робота в среде с вертикально движущимся препятствием

а) б)

в) г)

Рисунок 3.8 — Графики движения промышленного

робота в среде с вертикально движущимся препятствием:
а) — модуль скорости рабочего органа, б) — углы поворота шарниров,
в) — угловые скорости шарниров, г) — угловые ускорения шарниров

47
 Рисунок 3.9 — Траектория движения промышленного робота
в среде с вертикально движущимся и статическим препятствиями

а) б)

в) г)

Рисунок 3.10 — Графики движения промышленного робота

в среде с вертикально движущимся и статическим препятствиями:
а) — модуль скорости рабочего органа, б) — углы поворота шарниров,
в) — угловые скорости шарниров, г) — угловые ускорения шарниров

48
 4 Реализация алгоритма в ROS и моделирование в Gazebo

В состав фреймворка MoveIt входит несколько функциональных блоков,

обеспечивающих различные функции: решение прямой и обратной задачи кине-
матики для робота, планирование движения, обнаружение препятствий, детекти-
рование коллизий робота с препятствиями и собственными звеньями [31]. Дан-
ные блоки выполняются в виде динамических библиотек — плагинов, и загру-
жаются на этапе запуска узлов ROS. Подключаемые в виде плагинов функцио-
нальные блоки фреймворка MoveIt показаны на рисунке 4.1.

Рисунок 4.1 — Функциональные блоки фреймворка MoveIt [31]

Для обеспечения работы алгоритма с использованием фреймворка MoveIt

необходимо реализовать непосредственно планировщик движения на основе ме-
тода эластичных лент и регулятор траектории, способный реагировать на дефор-
мации эластичной ленты и нужным образом корректировать движения робота.
Планировщик и регулятор должны загружаться с помощью соответствующих
плагинов: менеджера планировщиков (англ. Planner Manager) и менеджера регу-
ляторов (англ. Controller Manager). Кроме того, необходимо настроить блок об-
наружения препятствий для планирования пути в среде со статическими и дина-
мическими препятствиями.
49
 4.1 Обнаружение препятствий

Для планирования движения в среде с препятствиями в фреймворке MoveIt

используется их представление в виде деревьев октантов (англ. Octree). В част-
ности, для робота строится трехмерная карта занятости (англ. Occupancy Map) из
вокселей разного размера, после чего при планировании пути планировщик осу-
ществляет детектирование коллизий с этими вокселями.
MoveIt использует фреймворк OctoMap [32] для построения и обновления
карты занятости на основе облаков точек (англ. Point Cloud) или изображений с
камер глубины (англ. Depth Image). Для этих целей в составе MoveIt есть два
соответствующих плагина — Point Cloud Occupancy Map Updater и Depth Image
Occupancy Map Updater [33].
В реальных задачах для детектирования препятствий рабочее простран-
ство робота оснащается системой технического зрения, состоящей из камеры
глубины или нескольких обычных камер. Однако создание и настройка такой си-
стемы представляет из себя нетривиальную задачу, а в случае проведения моде-
лирования в симуляторе еще и требует дополнительных вычислительных ресур-
сов. Поэтому более оптимальным решением в данной ситуации будет восполь-
зоваться априорной информацией о положениях и габаритах препятствий, взятой
напрямую из симуляции.
Симулятор Gazebo поддерживает интеграцию пользовательских плагинов
на языке программирования C++, которые взаимодействуют с ним через про-
граммный интерфейс (англ. Application Programming Interface, API). Плагины за-
гружаются на этапе запуска симуляции и позволяют выполнять какие-либо спе-
цифичные функции, в том числе получать информацию о состоянии мира, про-
изводить манипуляции над моделями, управлять работой датчиков и др. Кроме
того, благодаря тесной интеграции ROS и Gazebo, плагины могут осуществлять
коммуникацию с другими узлами ROS с помощью тем (топиков) и сервисов.
С целью получения информации об окружающих робота препятствиях был
разработан плагин наблюдателя препятствий (англ. Obstacle Monitor). Данный
плагин осуществляет мониторинг всех присутствующих в симуляции моделей и
50
строит облако точек, повторяющее контуры их поверхностей, на каждом шаге
моделирования, после чего публикует полученное облако точек в тему
/robot_ns/obstacles/points. Для оптимизации вычислительной нагрузки об-

лака точек для каждой модели кэшируются, а координаты точек хранятся в ло-
кальных системах координат моделей. Перед отправкой сообщений с результи-
рующим облаком точек над облаками каждой модели производятся необходи-
мые геометрические преобразования в глобальную систему координат.
Полученные сообщения поступают на вход фильтру препятствий (англ.
Obstacle Filter). Данный узел ROS осуществляет предварительную обработку об-
лаков точек. Облака точек, которые строит наблюдатель препятствий, имеют су-
щественно более высокую плотность по сравнению с разрешением карты заня-
тости, которую строит OctoMap. Поэтому фильтр должен разрядить облако то-
чек, осуществив их выравнивание по регулярной воксельной сетке, и отбросить
точки, слишком сильно удаленные от робота. Фильтр публикует отфильтрован-
ные точки в тему /robot_ns/filter/points, откуда те поступают на вход фрейм-
ворку MoveIt. На основании этой информации OctoMap строит трехмерную
карту занятости.
Пример работы системы обнаружения препятствий представлен на ри-
сунке 4.2. На виртуальной сцене расположен манипулятор (в целях демонстра-
ции работы также рассматривается как препятствие) и несколько простейших
геометрических форм: куб, сфера и цилиндр, что показано на рисунке 4.2, а).
Наблюдатель препятствий строит плотное облако точек с шагом 50 миллимет-
ров, повторяющее форму моделей, что можно видеть на рисунке 4.2, б). Стоит
отметить, что в силу ограничений возможностей плагина звенья робота (как и
прочие модели, которые описываются полигональными сетками) представля-
ются в виде ограничивающих параллелепипедов. Фильтр препятствий умень-
шает количество точек, выравнивая их по регулярной воксельной сетке с разме-
ров вокселя 100 миллиметров как представлено на рисунке 4.2, в). Наконец на
рисунке 4.2, г) по полученному облаку точек строится карта занятости.

51
 а) б)

в) г)

Рисунок 4.2 — Этапы построения карты занятости:

а) — исходная виртуальная сцена с препятствиями,
б) — плотное облако точек наблюдателя препятствий,
в) — облако точек фильтра препятствий, г) — карта занятости

4.2 Планирование движения

Алгоритм планирования движения, реализующий метод эластичных лент,

не претерпел существенных изменений по сравнению с ранее описанным в под-
разделе 3.3. Однако были произведены некоторые модификации для уменьшения
времени вычислений на каждой итерации.
При вычислении виртуальной сжимающей силы в k -ой точке f c k больше

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

экономятся операции деления и извлечения квадратного корня:
f c k = kc ( pk −1 − 2 pk + pk +1 ) + d c ( pk −1 − 2 pk + pk +1 ) , (4.1)

где kc — коэффициент усиления сжимающей силы,

d c — коэффициент демпфирования сжимающей силы.

52
 Также незначительные изменения претерпела виртуальная растягивающая
сила j -го вокселя в k -ой точке f r i j k , действующая на i -е звено, при вычислении

которой теперь используется квадрат расстояния до вокселя io jl 2k , а нормализация

разности векторов также не производится:

kr ( 02 − iolj 2k )( poj k − pilk ) − dr pk , iolj 2k  02 ,
fr i j k = (4.2)
0, iolj 2k  02 ,

где kr — коэффициент усиления растягивающей силы,

d r — коэффициент демпфирования растягивающей силы,
0 — радиус действия растягивающей силы.
При этом результирующая сила, действующая на i -е звено стала вычисляться
простым суммированием сил:
fr i k =  fr i j k . (4.3)
j

В качестве начального приближения для эластичной ленты используется

путь, который может быть предварительно найден с помощью одного из встро-
енных планировщиков MoveIt. В данной работе использовался планировщик
OMPL с применением алгоритма рандомизированных быстрорастущих деревьев
RRT*. В ходе экспериментов этот планировщик показал высокую надежность
работы в совокупности с небольшим временем планирования.
Пример работы планировщика показан на рисунке 4.3. На вход планиров-
щик получает путь, найденный глобальным планировщиком OMPL (зеленая ли-
ния). Путь состоит из 20–60 точек в зависимости от количества препятствий и
расстояния между начальным и целевым положениями. На основе этих точек
планировщик строит эластичную ленту (синяя линия) и деформирует ее сжима-
ющей силой, как это показано на рисунке 4.3, а). При добавлении в рабочее про-
странство препятствия эластичная лента деформируется растягивающей силой,
как можно видеть на рисунке 4.3, б). Появление вблизи эластичной ленты дви-
жущегося препятствия (например, другого робота) также заставляет ее деформи-
роваться, что представлено на рисунках 4.3, в) и 4.3, г).

53
а)

б)

в)

г)

Рисунок 4.3 — Пример работы планировщика движения:

а) — исходный вид эластичной ленты в среде без препятствий,
б) — деформация эластичной ленты появившимся препятствием,
в) и г) — деформация эластичной ленты движущимся роботом
54
 Настройка параметров планировщика движения по аналогии со встроен-
ными планировщиками MoveIt осуществляется с помощью конфигурационного
файла eband_planning.yaml, который загружается на сервер параметров при за-
пуске узлов фреймворка. В нем могут быть настроены следующие параметры:
− max_distance — радиус действия виртуальной растягивающей силы,
действующей со стороны препятствий, в метрах. Изменяя этот параметр можно
менять расстояние, на которое робот приближается к препятствиям,
− update_rate — частота обновления эластичной ленты, в герцах. Изме-
няя этот параметр можно повысить плавность деформации эластичной ленты и
повысить точность работы планировщика,
− internal_force_gain — коэффициент усиления виртуальной сжимаю-
щей силы, в ньютонах на метр. Изменяя этот параметр можно менять скорость
сжатия эластичной ленты,
− internal_force_damping — коэффициент демпфирования виртуальной
сжимающей силы, в ньютон-секундах на метр. Изменяя этот параметр можно
нейтрализовать колебательные процессы, возникающие при быстрой деформа-
ции эластичной ленты сжимающей силой,
− external_force_gain — коэффициент усиления виртуальной растягива-
ющей силы, в ньютонах на метр. Изменяя этот коэффициент можно увеличить
скорость и величину деформации эластичной ленты под влиянием движущихся
препятствий,
− external_force_damping — коэффициент демпфирования виртуальной
растягивающей силы, в ньютон-секундах на метр. Изменяя этот коэффициент
можно скомпенсировать скачки эластичной ленты при деформации движущи-
мися препятствиями.
Пример влияния изменения одного из параметров — радиуса действия рас-
тягивающей силы max_distance — на форму эластичной ленты представлен на
рисунке 4.4. Как можно видеть, увеличение радиуса действия силы заставляет
эластичную ленту деформироваться сильнее, кратно увеличивая безопасный за-
зор между препятствием и роботом.
55
 а)

б)

в)

Рисунок 4.4 — Пример изменения формы эластичной ленты

в зависимости от величины радиуса действия растягивающей
силы: а) — 0 = 0,3 м , б) — 0 = 0,4 м , в) — 0 = 0,5 м

4.3 Управление движением по траектории

За движение робота по траектории в фреймворке MoveIt отвечает мене-

джер исполнения траектории (англ. Trajectory Execution Manager). Данный функ-
циональный блок управляет регуляторами приводов через программный интер-
фейс менеджера регуляторов, устанавливая новую траекторию через команду
Push и давая команду на исполнение Execute.
56
 Для преобразования пути, найденного планировщиком движения на ос-
нове метода эластичных лент, в формат закона движения его шарниров был реа-
лизован собственный ROS-узел регулятора траектории. Ему на вход подается
путь в виде эластичной ленты
P =  xe k = ( pe k e k )T  , (4.4)

где pe k — положение рабочего органа манипулятора в k -й точке пути,

 e k — ориентация рабочего органа манипулятора в k -й точке пути.

Регулятор траектории определяет текущее положение и ориентацию рабо-

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


T = qk = ( q1 
qN ) : qk = f −1 ( xe k ) ,
T
(4.5)

где qk = f −1 ( xe k ) — основное уравнение обратной кинематики робота.

Помимо этого, для работы регуляторов приводов необходимо вычислить
значения производных по времени от обобщенных координат — обобщенные
скорости qk и ускорения qk шарниров. Они могут быть определены методом ко-
нечных разностей:
qk − qk −1
qk = , (4.6)
tk − tk −1
qk − qk −1
qk = , (4.7)
tk − tk −1
где tk — момент времени, в который рабочий орган манипулятора ока-
жется в k -й точке пути.
Для определения этих временных моментов необходимо осуществить па-
раметризацию траектории по времени, задав желаемый закон изменения поло-
жения рабочего органа. В зависимости от задачи могут быть использованы раз-
личные формы функции, описывающей закон движения, но одним из распро-
страненных решений является задание трапецеидальной функции изменения

57
скорости рабочего органа с постоянными значениями ускорения при разгоне и
торможении робота. В этом случае скорость рабочего органа манипулятора опи-
сывается следующим законом:
 2 s (t )a , s (t )  d acc. ,
 acc.

ve (t ) = ve des. , d acc.  s (t )  sF − d dec. , (4.8)


 2( sF − s (t ))adec. , sF − d dec.  s (t ),

где s (t ) — координата рабочего органа манипулятора вдоль траектории,

sF = s(tF ) — длина траектории,
vdes. — желаемая установившаяся линейная скорость рабочего органа
при движении по траектории,
aacc.  0 — линейное ускорение рабочего органа при разгоне,
adec.  0 — линейное ускорение рабочего органа при торможении,

dacc. = ve2des. 2aacc. — длина пути разгона,

ddec. = −ve2des. 2adec. — длина тормозного пути.

Зная координаты рабочего органа sk в точках пути можно вычислить не-

обходимую линейную скорость ve k для соблюдения закона движения, а затем

определить и моменты времени tk :

sk − sk −1
tk = tk −1 + 2 , t0 = 0, (4.9)
ve k + ve k −1

после чего вычислить значения обобщенных скоростей qk и ускорений qk .

Пример работы регулятора траектории при движении манипулятора в
среде с препятствием из точки x0 в точку xF с плавным изменением ориентации
рабочего органа представлен на рисунке 4.5, а на рисунке 4.6 представлены гра-
фики движения манипулятора. Использовались следующие параметры:
x0 = ( p0 0 ) , p0 = ( 0 −1,5 0,3) ,  0 = ( 0 0 −45 ) ,
T T T

x F = ( pF F ) , pF = ( 0 1,5 0,3) ,  F = ( 0 90 0 ) ,

T T T

vdes. = 0,5vemax = 0,5 м с, aacc. = 0,5aacc.

max
= 1,25 м с, adec. = 0,5adec.
max
= −3 м с.

58
 Рисунок 4.5 — Пример работы регулятора траектории при движении
по траектории с плавно меняющейся ориентацией рабочего органа

а) б)

в) г)

Рисунок 4.6 — Графики движения манипулятора под управлением

регулятора траектории: а) — модуль скорости рабочего органа,
б) — углы поворота шарниров, в) — угловые скорости шарниров,
г) — угловые ускорения шарниров

59
 4.4 Движение по пересекающимся траекториям

При моделировании в Gazebo были рассмотрены несколько распростра-

ненных сценариев взаимодействия роботов, в ходе которых необходимо произ-
водить координацию их движений. Один из таких случаев — движение по вза-
имно пересекающимся траекториям, когда существуют участки траектории, на
которых роботы потенциально могут столкнуться друг с другом.
На рисунке 4.7 представлен процесс движения двух манипуляторов по пе-
ресекающимся траекториям со следующими начальными и конечными точками:

x0(1) = ( p0(1) 0 ) , p0(1) = ( 0,1 −1,5 1,5 ) , 0(1) = ( 0 0 −45 ) ,

T
(1)
T T

x F(1) = ( pF(1) F ) , pF(1) = ( 0,1 1,5 1,5 ) , F = ( 0 0 45 ) ,

T
(1) (1)
T T

x0(2) = ( p0(2) 0 ) , p0(2) = ( −0,1 0 0,2 ) , 0 = ( 0 −45 180 ) ,

T
 (2) (2)
T T

x F(2) = ( pF(2) F ) , pF(2) = ( −0,1 0 2,7 ) , F = ( 0 45 180 ) ,

T
 (2) (2)
T T

а на рисунке 4.8 — графики, полученные в результате моделирования.

Рисунок 4.7 — Процесс движения манипуляторов по траекториям

60
а) д)

б) е)

в) ж)

г) з)

Рисунок 4.8 — Графики движения манипуляторов:

а), б), в), г) — первый робот, д), е), ж), з) — второй робот

61
 4.5 Движение по параллельным траекториям

Также были рассмотрены сценарии, при которых роботы осуществляют

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

x0(1) = ( p0(1)  0(1) ) , p0(1) = ( 0 −1,5 0,5 ) ,  0(1) = ( 0 0 −45 ) ,

T T T

x F(1) = ( pF(1) F ) , pF(1) = ( 0 2 1,5 ) , F = ( 0 0 45 ) ,

T
 (1)  (1)
T T

x0(2) = ( p0(2)  0(2) ) , p0(2) = ( 0 1,5 0,5 ) ,  0(2) = ( 0 −90 180 ) ,

T T T

x F(2) = ( pF(2) F ) , pF(2) = ( 0 −2 0,5 ) , F = ( 0 90 180 ) ,

T
 (2)  (2)
T T

а на рисунке 4.10 — графики, полученные в результате моделирования.

Рисунок 4.9 — Процесс движения манипуляторов по траекториям

62
а) д)

б) е)

в) ж)

г) з)

Рисунок 4.10 — Графики движения манипуляторов:

а), б), в), г) — первый робот, д), е), ж), з) — второй робот

63
 4.6 Движение по траекториям в среде с препятствиями

Кроме того, проводились эксперименты с движением манипуляторов в

сценариях, когда значительная часть рабочего пространства роботов занята ста-
тическими препятствиями. В этих случаях роботам необходимо не только избе-
гать столкновения друг с другом, но еще и осуществлять движение только в сво-
бодном от препятствий пространстве.
На рисунке 4.11 представлен процесс движения двух манипуляторов по па-
раллельным траекториям со следующими начальными и конечными точками:

x0(1) = ( p0(1)  0(1) ) , p0(1) = ( 0 −2 0,7 ) ,  0(1) = ( 0 0 −45 ) ,

T T T

x F(1) = ( pF(1) F ) , pF(1) = ( −0,3 2 0,3) , F = ( 0 0 45 ) ,

T
 (1)  (1)
T T

x0(2) = ( p0(2) 0 ) , p0(2) = ( 0 2 0,7 ) , 0 = ( 0 0 135 ) ,

T
(2) (2)
T T

x F(2) = ( pF(2) F ) , pF(2) = ( 0,3 −2 0,3) , (2)

F = ( 0 0 −135 ) ,
T
(2)
T T

а на рисунке 4.12 — графики, полученные в результате моделирования.

Рисунок 4.11 — Процесс движения манипуляторов по траекториям

64
а) д)

б) е)

в) ж)

г) з)

Рисунок 4.12 — Графики движения манипуляторов:

а), б), в), г) — первый робот, д), е), ж), з) — второй робот

65
 4.7 Анализ результатов моделирования

В результате экспериментов, выполненных в симуляторе Gazebo, установ-

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

66
 ЗАКЛЮЧЕНИЕ

В ходе выполнения выпускной квалификационной работы изучена научно-

техническая литература, посвященная задаче взаимной координации движений
нескольких промышленных роботов.
Проанализированы существующие подходы для планирования траектории
движения в системах с несколькими роботами: метод эластичных лент, метод
потенциальных полей, метод на основе стратегии и ориентированный на задачу
метод. Для каждого метода приведено описание особенностей работы, выявлены
главные преимущества и недостатки. Для дальнейшей программной реализации
выбран метод эластичных лент.
В качестве модели манипулятора для решения задачи планирования траек-
тории движения выбран робот KUKA KR 150 R3100-2 QUANTEC. Для данного
манипулятора настроен ROS-пакет, содержащий файлы конфигураций для
фреймворка MoveIt, который описывает параметры кинематических группы, ал-
горитма решения обратной задачи кинематики, глобального планировщика пути,
регуляторов приводов и др.
Сгенерированный ROS-пакет запущен в средстве визуализации RViz с воз-
можностью планирования движения в интерактивном режиме. URDF-описание
манипулятора и конфигурационные параметры регуляторов дополнены недоста-
ющей информацией для интеграции системы в симулятор Gazebo для моделиро-
вания физических процессов в реальном времени. Создан ROS-пакет, позволяю-
щий запускать симуляцию с несколькими роботами на одной виртуальной сцене.
Изучены существующие программные реализации алгоритмов планирова-
ния движения на основе метода эластичных лент. Для отладочных целей напи-
сана собственная реализация на языке MATLAB с использованием Robotics Sys-
tem Toolbox. В среде MATLAB проведена серия экспериментов по моделирова-
нию работы алгоритма.
Разработанный алгоритм перенесен из среды MATLAB в среду ROS с ис-
пользованием языка программирования C++ и адаптирован для интеграции с

67
фреймворком MoveIt. Описаны некоторые детали реализации алгоритма в ROS,
касающиеся обнаружения препятствий, планирования движения и работы регу-
лятора траектории.
Проведены эксперименты по моделированию движения манипуляторов в
симуляторе Gazebo. Представлены результаты моделирования движения по па-
раллельным и пересекающимся траекториям, а также в среде с препятствиями.
Осуществлен анализ результатов моделирования. На основании анализа
сделаны выводы о работоспособности алгоритма. Приведены основные направ-
ления для дальнейшего усовершенствования алгоритма.

68
 СПИСОК ИСПОЛЬЗОВАННЫХ ИСТОЧНИКОВ

1 Robotics: Modelling, Planning and Control / B. Siciliano, L. Sciavicco,

L. Villani, G. Oriolo. — Springer, 2009. — 632 p.
2 Williams, D. The Virtual Linkage: A Model for Internal Forces in Multi-
Grasp Manipulator / D. Williams, O. Khatib. — DOI 10.1109/ROBOT.1993.292110.
— Текст : электронный // Proceedings of the 1993 IEEE International Conference on
Robotics and Automation. — 1993. — Vol. 1. — P. 1025–1030.
3 Vehicle/Arm Coordination and Multiple Mobile Manipulator
Decentralized Cooperation / O. Khatib, K. Yokoi, K. Chang [et al.]. — DOI
10.1109/IROS.1996.570849. — Текст : непосредственный // Proceedings of the
IEEE/RSJ International Conference on Intelligent Robots and Systems. — 1996. —
Vol. 1. — P. 546–553.
4 Alonso-Mora, J. Multi-Robot Formation Control and Object Transport in
Dynamic Environments via Constrained Optimization / J. Alonso-Mora, S. Baker,
D. Rus. — DOI 10.1177/0278364917719333. — Текст : электронный // The Interna-
tional Journal of Robotics Research. — 2017. — Vol. 36. — P. 1000–1021.
5 Kröger, T. On-Line Trajectory Generation in Robotic Systems: Basic Con-
cepts for Instantaneous Reactions to Unforeseen (Sensor) Events / T. Kröger. —
Springer, 2010. — 230 p.
6 Quinlan, S. Elastic Bands: Connecting Path Planning and Control / S. Quin-
lan, O. Khatib. — 10.1109/ROBOT.1993.291936. — Текст : непосредственный //
Proceedings of the 1993 IEEE International Conference on Robotics and Automation.
— 1993. — Vol. 1. — P. 802–807.
7 Development of a Configuration Space Motion Planner for Robot
in Dynamic Environment / X.J. Wu, J. Tang, Q. Li, K.H. Heng. — DOI
10.1016/j.rcim.2007.04.004. — Текст : электронный // Robotics and Computer-Inte-
grated Manufacturing. — 2009. — Vol. 25. — P. 13–31.
8 Brock, O. Real-Time Obstacle Avoidance and Motion Coordination in a
Multi-Robot Workcell / O. Brock, O. Khatib. — DOI 10.1109/ISATP.1999.782971.

69
— Текст : электронный // Proceedings of the 1999 IEEE International Symposium on
Assembly and Task Planning. — 1999. — Vol. 1. — P. 274–279.
9 Comparative Analysis of Collision-Free Path-Planning Methods for Multi-
Manipulator Systems / J.C. Fraile, J. Perez-Turiel, J.L. Gonzales-Snachez [et al.]. —
DOI 10.1017/S0263574706002888. — Текст : электронный // Robotica. — 2006. —
Vol. 24. — P. 711–726.
10 Khatib, O. Real-Time Obstacle Avoidance for Manipulators and Mobile Ro-
bots / O. Khatib. — DOI 10.1109/ROBOT.1985.1087247. — Текст : электронный //
Proceedings of the 1985 IEEE International Conference on Robotics and Automation.
— 1996. — Vol. 1. — P. 500–505.
11 Rimon, E. Exact Robot Navigation Using Artificial Potential Functions /
E. Rimon, D.E. Koditschek. — DOI 10.1109/70.163777. — Текст : электронный //
IEEE Transactions on Robotics and Automation. — 1992. — Vol. 8. — P. 501–518.
12 Reactive Approach to On-Line Path Planning for Robot Manipulators in Dy-
namic Environment / M. Mediavilla, J.L. Gonzalez, J.C. Fraile, J.R. Peran. — DOI
10.1017/S0263574702004071. — Текст : электронный // Robotica. — 2002. —
Vol. 20. — P. 375–384.
13 Selection of Strategies for Collision-Free Motion in Multi-Manipulator
Systems / M. Mediavilla, J.L. Fraile, I.J. Galindo, M.T. Gonzalez. — DOI
10.1023/A:1026248112092. — Текст : электронный // Journal of Intelligent and Ro-
botic Systems. — 2003. — Vol. 38. — P. 85–104.
14 Task-Oriented Motion Planning for Multi-Arm Robotic Systems / F. Basile,
F. Caccavale, P. Chiacchio [et al.]. — DOI 10.1016/j.rcim.2012.02.007. — Текст :
электронный // Robotics and Computer-Integrated Manufacturing. — 2012. —
Vol. 28. — P. 569–582.
15 A Decentralized Kinematic Control Architecture for Collaborative and Co-
operative Multi-Arm Systems / F. Basile, F. Caccavale, P. Chiacchio [et al.]. — DOI
10.1016/j.mechatronics.2013.08.008. — Текст : электронный // Mechatronics. —
2013. — Vol. 23. — P. 1100–1112.

70
 16 Gan, Y. Off-Line Programming Techniques for Multirobot Cooperation Sys-
tem / Y. Gan, X. Dai, D. Li. — DOI 10.5772/56506. — Текст : электронный // Inter-
national Journal of Advanced Robotic Systems. — 2013. — Vol. 10. — P. 1–17.
17 KR QUANTEC: Industrial Robots : [сайт] / разработчик: KUKA Roboter.
— Augsburg, Bavaria, 2023. — URL: https://www.kuka.com/en-de/products/robot-
systems/industrial-robots/kr-quantec (дата обращения: 01.05.2023).
18 KUKA Experimental : [сайт]. — URL: http://wiki.ros.org/kuka_experi-
mental (дата обращения: 01.05.2023). — Электронная программа : электронная.
19 MoveIt Tutorials: MoveIt Setup Assistant : [сайт] — URL: https://ros-plan-
ning.github.io/moveit_tutorials/doc/setup_assistant/setup_assistant_tutorial.html (дата
обращения: 01.05.2023). — Электронная программа : электронная.
20 RViz : [сайт]. — URL: http://wiki.ros.org/rviz (дата обращения:
01.05.2023). — Электронная программа : электронная.
21 Gazebo : [сайт]. — URL: https://gazebosim.org/home (дата обращения:
01.05.2023). — Электронная программа : электронная.
22 ROS Control: A Generic and Simple Control Framework for ROS / S. Chitta,
E. Marder-Eppstein, W. Meeussen [et al.] — DOI 10.21105/joss.00456. — Текст :
электронный // The Journal of Open Source Software. — 2017. — Vol. 2. — P. 1–5.
23 ROS Control : [сайт]. — URL: http://wiki.ros.org/ros_control (дата обра-
щения: 01.05.2023). — Электронная программа : электронная.
24 Tutorial: ROS Control. — Текст : электронный // Gazebo : официальный
сайт. — URL: https://classic.gazebosim.org/tutorials?tut=ros_control (дата обраще-
ния: 01.05.2023).
25 Sucan, A.I. The Open Motion Planning Library / I.A. Sucan, M. Moll,
L.E. Kavraki. — DOI 10.1109/MRA.2012.2205651. — Текст : электронный // IEEE
Robotics & Automation Magazine. — 2012. — Vol. 19. — P. 72–82.
26 CHOMP: Covariant Hamiltonian Optimization for Motion Planning /
M. Zucker, N. Ratliff, A.D. Dragan [et al.]. — DOI 10.1177/0278364913488805. —
Текст : электронный // The International Journal of Robotics Research. — 2013. —
Vol. 32. — P. 1–45.

71
 27 STOMP: Stochastic Trajectory Optimization for Motion Planning / M. Kala-
krishnan, S. Chitta, E. Theodorou [et al.]. — DOI 10.1109/ICRA.2011.5980280. —
Текст : электронный. — // IEEE International Conference on Robotics and Automa-
tion. — 2011. — Vol. 1. — P. 4569–4574.
28 Finding Locally Optimal, Collision Free Trajectories with Sequential Convex
Optimization / J. Schulman, J. Ho, A. Lee [et al.]. — DOI 10.15607/RSS.2013.IX.031.
— Текст : электронный // Robotics: Science and Systems. —2013. — Vol. 9. —
P. 1–10.
29 Brock, O. Executing Motion Plans for Robots with Many Degrees of
Freedom in Dynamic Environment / O. Brock, O. Khatib. — DOI 10.1109/RO-
BOT.1998.676237. — Текст : электронный // IEEE International Conference on Ro-
botics and Automation. — 1998. — Vol. 1. — P. 1–6.
30 Using Elastic Bands for Collision Avoidance in Collaborative Robotics /
T. Kot, R. Wierbica, P. Oscadal [et al.]. — DOI 10.1109/ACCESS.2022.3212407. —
Текст : электронный // IEEE Access. — 2022. — Vol. 9. — P. 106972–106987.
31 MoveIt Docs: Plugin Interface. — Текст : электронный // MoveIt : офици-
альный сайт. — URL: https://moveit.ros.org/documentation/plugins/ (дата обраще-
ния: 01.05.2023).
32 OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Oc-
trees / A. Hornung, K.M. Wurm, M. Bennewitz [et al.]. — DOI 10.1007/s10514-012-
9321-0. — Текст : электронный // Autonomous Robots. — 2013. — Vol. 34. —
P. 189–206.
33 MoveIt Tutorials: Perception Pipeline Tutorial : [сайт] — URL: https://ros-
planning.github.io/moveit_tutorials/doc/perception_pipeline/perception_pipeline_tuto-
rial.html (дата обращения: 01.05.2023). — Электронная программа : электронная.

72
 ПРИЛОЖЕНИЕ А
Файл загрузки модели робота

Листинг А.1 — Загрузочный файл spawn_robot.launch

001 <?xml version="1.0"?>
002 <launch>
003 <arg name="robot_name" default=""/>
004 <arg name="robot_ns" default=""/>
005 <arg name="urdf_path" default="$(find moveit_gazebo)/urdf/robot.xacro"/>
006
007 <arg name="x" default="0"/>
008 <arg name="y" default="0"/>
009 <arg name="z" default="0"/>
010 <arg name="R" default="0"/>
011 <arg name="P" default="0"/>
012 <arg name="Y" default="0"/>
013
014 <arg name="q1" default="0"/>
015 <arg name="q2" default="0"/>
016 <arg name="q3" default="0"/>
017 <arg name="q4" default="0"/>
018 <arg name="q5" default="0"/>
019 <arg name="q6" default="0"/>
020
021 <arg name="world_pose" default="-x 0 -y 0 -z 0"/>
022 <arg name="robot_pose" default="-J joint_a1 $(arg q1) -J joint_a2 $(arg q2) -J
joint_a3 $(arg q3) -J joint_a4 $(arg q4) -J joint_a5 $(arg q5) -J joint_a6 $(arg
q6)"/>
023 <param name="robot_description" command="$(find xacro)/xacro $(arg urdf_path)
name:=$(arg robot_name) ns:=$(arg robot_ns) x:=$(arg x) y:=$(arg y) z:=$(arg z)
R:=$(arg R) P:=$(arg P) Y:=$(arg Y)"/>
024
025 <arg name="model" value="$(eval 'robot' if (robot_ns == '') else robot_ns)"/>
026 <node pkg="gazebo_ros" type="spawn_model" name="spawn_robot" args="-urdf -
param robot_description -model $(arg model) $(arg world_pose) $(arg robot_pose)"
respawn="false" output="screen"/>
027 </launch>
028

73
 ПРИЛОЖЕНИЕ Б
Файл загрузки параметров и узлов робота

Листинг Б.1 — Загрузочный файл load_robot.launch

001 <?xml version="1.0"?>
002 <launch>
003 <arg name="robot_name" default=""/>
004 <arg name="robot_ns" default=""/>
005 <arg name="robot_moveit_config" default="$(arg robot_name)_moveit_con-
fig"/>
006 <arg name="robot_moveit_config_path" default="$(eval find(arg('ro-
bot_moveit_config')))"/>
007
008 <arg name="x" default="0"/>
009 <arg name="y" default="0"/>
010 <arg name="z" default="0"/>
011 <arg name="R" default="0"/>
012 <arg name="P" default="0"/>
013 <arg name="Y" default="0"/>
014
015 <arg name="q1" default="0"/>
016 <arg name="q2" default="0"/>
017 <arg name="q3" default="0"/>
018 <arg name="q4" default="0"/>
019 <arg name="q5" default="0"/>
020 <arg name="q6" default="0"/>
021
022 <group ns="$(arg robot_ns)">
023 <param name="tf_prefix" value="$(arg robot_ns)"/>
024
025 <!-- Spawn robot in Gazebo world -->
026 <include file="$(find moveit_gazebo)/launch/spawn_robot.launch">
027 <arg name="robot_name" value="$(arg robot_name)"/>
028 <arg name="robot_ns" value="$(arg robot_ns)"/>
029 <arg name="x" value="$(arg x)"/>
030 <arg name="y" value="$(arg y)"/>
031 <arg name="z" value="$(arg z)"/>
032 <arg name="R" value="$(arg R)"/>
033 <arg name="P" value="$(arg P)"/>
034 <arg name="Y" value="$(arg Y)"/>
035 <arg name="q1" value="$(arg q1)"/>
036 <arg name="q2" value="$(arg q2)"/>
037 <arg name="q3" value="$(arg q3)"/>
038 <arg name="q4" value="$(arg q4)"/>
039 <arg name="q5" value="$(arg q5)"/>
040 <arg name="q6" value="$(arg q6)"/>
041 </include>
042
043 <!-- Static transform between robot base and world frame -->
044 <node pkg="tf" type="static_transform_publisher" name="fixed_joint_pub-
lisher" args="$(arg x) $(arg y) $(arg z) $(arg Y) $(arg P) $(arg R) world $(arg
robot_ns)/base_link 100"/>
045
046 <!-- Transforms between robot links -->
047 <node pkg="robot_state_publisher" type="robot_state_publisher" name="ro-
bot_state_publisher" respawn="true" output="screen">
048 <remap from="/joint_states" to="/$(arg robot_ns)/joint_states"/>
049 </node>
050

74
Продолжение листинга Б.1
051 <!-- Fix for error with RobotModel in RViz -->
052 <node pkg="tf" type="static_transform_publisher" name="world_publisher"
args="0 0 0 0 0 0 world $(arg robot_ns)/world 100"/>
053
054 <!-- Load joint controller parameters for Gazebo -->
055 <rosparam command="load" file="$(arg robot_moveit_config_path)/config/-
ros_controllers.yaml"/>
056 <rosparam command="load" file="$(arg robot_moveit_config_path)/config/-
gazebo_controllers.yaml"/>
057
058 <!-- Load kinematics parameters for MoveIt -->
059 <group ns="robot_description_kinematics">
060 <rosparam command="load" file="$(arg robot_moveit_config_path)/config/-
kinematics.yaml"/>
061 </group>
062
063 <!-- Load limit kinematic constraints for MoveIt -->
064 <group ns="robot_description_planning">
065 <rosparam command="load" file="$(arg robot_moveit_config_path)/config/-
joint_limits.yaml"/>
066 <rosparam command="load" file="$(arg robot_moveit_config_path)/config/-
cartesian_limits.yaml"/>
067 </group>
068
069 <!-- Load SRDF for MoveIt -->
070 <param name="robot_description_semantic" textfile="$(arg robot_moveit_con-
fig_path)/config/$(arg robot_name).srdf"/>
071
072 <!-- Load MoveIt Planners -->
073 <group ns="move_group/planning_pipelines">
074 <!-- OMPL -->
075 <include file="$(arg robot_moveit_config_path)/launch/planning_pipeline.
launch.xml">
076 <arg name="pipeline" value="ompl"/>
077 </include>
078
079 <!-- CHOMP -->
080 <include file="$(arg robot_moveit_config_path)/launch/planning_pipeline.
launch.xml">
081 <arg name="pipeline" value="chomp"/>
082 </include>
083
084 <!-- STOMP -->
085 <include file="$(arg robot_moveit_config_path)/launch/planning_pipeline.
launch.xml">
086 <arg name="pipeline" value="stomp"/>
087 </include>
088
089 <!-- Pilz Industrial Motion -->
090 <include file="$(arg robot_moveit_config_path)/launch/planning_pipeline.
launch.xml">
091 <arg name="pipeline" value="pilz_industrial_motion_planner"/>
092 </include>
093
094 <!-- Elastic Band Planner -->
095 <include file="$(arg robot_moveit_config_path)/launch/planning_pipeline.
launch.xml">
096 <arg name="pipeline" value="eband"/>
097 </include>
098 </group>

75
Продолжение листинга Б.1
099
100 <!-- Spawn Gazebo ROS controllers -->
101 <node pkg="controller_manager" type="spawner" name="gazebo_control-
ler_spawner" respawn="false" output="screen" args="joint_state_controller
arm_controller"/>
102
103 <!-- Trajectory Execution Functionality -->
104 <group ns="move_group">
105 <param name="ros_control_namespace" value="/$(arg robot_ns)/"/>
106 <include file="$(arg robot_moveit_config_path)/launch/trajectory_execu-
tion.launch.xml">
107 <arg name="moveit_manage_controllers" value="true"/>
108 <arg name="moveit_controller_manager" value="ros_control"/>
109 <arg name="fake_execution_type" value="interpolate"/>
110 </include>
111 </group>
112
113 <!-- Load move_group node for planning and trajectory execution -->
114 <node pkg="moveit_ros_move_group" type="move_group" name="move_group" res-
115 pawn="false" output="screen">
<env name="DISPLAY" value="$(optenv DISPLAY :0)"/>
116
117 <param name="capabilities" value=""/>
118 <param name="disable_capabilities" value=""/>
119 <param name="allow_trajectory_execution" value="true"/>
120 <param name="default_planning_pipeline" value="ompl"/>
121 <param name="monitor_dynamics" value="false"/>
122 <param name="sense_for_plan/max_safe_path_cost" value="1"/>
123
124 <param name="planning_scene_monitor/publish_state_updates"
value="true"/>
125 <param name="planning_scene_monitor/publish_planning_scene"
value="true"/>
126 <param name="planning_scene_monitor/publish_geometry_updates"
value="true"/>
127 <param name="planning_scene_monitor/publish_transforms_updates"
value="true"/>
128 </node>
129 </group>
130 </launch>
131

76
 ПРИЛОЖЕНИЕ В
Файл конфигурации регуляторов приводов

Листинг В.1 — Конфигурационный файл ros_controllers.yaml

001 # Simulation settings for using moveit_sim_controllers
002 moveit_sim_hw_interface:
003 joint_model_group: arm
004 joint_model_group_pose: home
005
006 # Settings for ros_control_boilerplate control loop
007 generic_hw_control_loop:
008 loop_hz: 1000
009 cycle_time_error_threshold: 0.01
010
011 # Settings for ros_control hardware interface
012 hardware_interface:
013 joints:
014 - joint_a1
015 - joint_a2
016 - joint_a3
017 - joint_a4
018 - joint_a5
019 - joint_a6
020 sim_control_mode: 0 # 0: position, 1: velocity
021
022 # Publish all joint states
023 # Creates the /joint_states topic necessary in ROS
024 joint_state_controller:
025 type: joint_state_controller/JointStateController
026 publish_rate: 1000
027
028 arm _controller:
029 type: position_controllers/JointTrajectoryController
030 joints:
031 - joint_a1
032 - joint_a2
033 - joint_a3
034 - joint_a4
035 - joint_a5
036 - joint_a6
037 constraints:
038 joint_a1:
039 goal: 0.001
040 trajectory: 0.001
041 joint_a2:
042 goal: 0.001
043 trajectory: 0.001
044 joint_a3:
045 goal: 0.001
046 trajectory: 0.001
047 joint_a4:
048 goal: 0.001
049 trajectory: 0.001
050 joint_a5:
051 goal: 0.001
052 trajectory: 0.001
053 joint_a6:
054 goal: 0.001
055 trajectory: 0.001

77
Продолжение листинга В.1
056 gains:
057 joint_a1:
058 p: 10000
059 d: 100
060 i: 0.01
061 i_clamp: 1
062 joint_a2:
063 p: 10000
064 d: 100
065 i: 0.01
066 i_clamp: 1
067 joint_a3:
068 p: 10000
069 d: 100
070 i: 0.01
071 i_clamp: 1
072 joint_a4:
073 p: 10000
074 d: 100
075 i: 0.01
076 i_clamp: 1
077 joint_a5:
078 p: 10000
079 d: 100
080 i: 0.01
081 i_clamp: 1
082 joint_a6:
083 p: 10000
084 d: 100
085 i: 0.01
086 i_clamp: 1
087
088 controller_list:
089 - name: arm_controller
090 action_ns: follow_joint_trajectory
091 default: true
092 type: FollowJointTrajectory
093 joints:
094 - joint_a1
095 - joint_a2
096 - joint_a3
097 - joint_a4
098 - joint_a5
099 - joint_a6
100

78