CC BY
46
УДК 621.865:004.021
DOI: 10.18698/0236-3933-2016-6-97-114
РАСЧЕТ ОБЛАСТЕЙ ПЕРЕСЕЧЕНИЯ ПОВЕРХНОСТЕЙ ЗАХВАТНЫХ УСТРОЙСТВ МАНИПУЛЯТОРОВ И ДЕФОРМИРУЕМЫХ ОБЪЕКТОВ ПРИ ПЛАНИРОВАНИИ И МОДЕЛИРОВАНИИ ЗАХВАТА
А.Г. Лесков1
Е.В. Селиверстова1' 2
agleskov@rambler.ru feoktistovaev@mail.ru
1 Дмитровский филиал МГТУ им. Н.Э. Баумана, пос. Орево, Дмитровский р-н, Московская обл., Российская Федерация
2 МГТУ им. Н.Э. Баумана, Москва, Российская Федерация
Аннотация
Выполнен анализ методов обнаружения и расчета областей пересечения поверхностей захватных устройств манипуляторов и объектов манипулирования при решении задач моделирования и планирования захвата деформируемых объектов. Предложен алгоритм обнаружения и расчета областей пересечения, включающий в себя широкую и узкую фазы. Алгоритм широкой фазы основан на методе ограничивающих параллелепипедов и усовершенствован благодаря введению оригинального алгоритма обнаружения областей потенциального взаимодействия захватных устройств манипуляторов и объектов манипулирования. Алгоритм узкой фазы является оригинальным. В его основе — алгоритм поиска ближайших соседей с учетом направления движения взаимодействующих тел. Разработана компьютерная программа и приведены результаты применения предложенных алгоритмов для моделирования захвата объекта трехпалой кистью Schunk SDH
Ключевые слова
Захват, деформируемый объект, моделирование, пересечение полигональных моделей, Oriented Bounding Box, VP-дерево
Поступила в редакцию 27.05.2016 © МГТУ им. Н.Э. Баумана, 2016
Одна из наиболее распространенных манипуляционных операций — это автоматический захват объекта манипулирования (ОМ). Известны многочисленные источники [1, 2], в которых рассматриваются вопросы захвата ОМ, представляющих собой жесткие тела. В отличие от захвата жестких тел, захват деформируемых тел является менее изученным.
В процессе захвата деформируемого тела элемент захватного устройства манипулятора (ЗУМ) деформирует ОМ. В результате формируется пара пятен контакта, принадлежащих поверхностям ОМ и ЗУМ. Эти пятна имеют одинаковую форму. Эпюры сил, действующих на ОМ и ЗУМ, зеркально симметричны относительно поверхности пятна контакта. Если звено ЗУМ недеформируемое, то геометрия пятен контакта идентична геометрии области поверхности звена, которая находится в контакте с ОМ. В противном случае она зависит от формы ОМ и звена, а также от их упругих свойств.
При контакте деформируемых элементов ЗУМ с деформируемыми ОМ происходит изменение первоначальной формы контактирующих элементов. В настоящей работе контакт между деформируемыми элементами определяется как геометрическое пересечение поверхностей первоначальных (еще не деформированных) форм контактирующих элементов. Такое рассмотрение геометрически имитирует контактные деформации. Отметим, что приведенное понятие пересечения поверхностей является условным, его использование — общепринятым приемом при моделировании захвата тел [3].
Одним из способов синтеза захвата является офлайн-планирование по полигональным моделям ЗУМ и ОМ [1], обеспечивающее предварительную оценку возможных способов захвата. Моделирование происходит путем пошагового изменения положения элементов ЗУМ для перевода его в состояние «схват закрыт».
В настоящей статье рассматривается один из центральных вопросов, возникающих при моделировании захвата упруго деформируемых объектов, — обнаружение пересечений поверхностей ЗУМ и ОМ. При решении этой задачи необходимо установить факт пересечения и рассчитать его параметры: область пересечения, которая представляет собой фигуру, образованную в результате конъюнкции фигур ЗУМ и ОМ; глубину проникания ЗУМ в ОМ при захвате, т. е. минимальное расстояние, на которое нужно развести эти объекты, чтобы их поверхности перестали пересекаться; вектор, вдоль которого отсчитывается глубина проникания.
При моделировании захвата упругих тел перечисленные параметры используются для определения геометрии пятен контакта и эпюр нагрузки, которые участвуют в расчете сил контактного взаимодействия между ЗУМ и ОМ.
Методы и алгоритмы обнаружения пересечения объектов. При решении задачи обнаружения пересечения поверхности ЗУМ и ОМ представляются в виде многогранников (полигональных моделей). Полигональные модели задаются наборами вершин многогранников и соединяющих их ребер (рис. 1, а, в). Захватное устройство представляется в виде набора полигональных моделей, каждая из которых соответствует одному элементу разветвленной кинематической цепи схвата (рис. 1, б).
а б в
Рис. 1. Полигональные модели ОМ (а, в) и захватного устройства Schunk SDH (б)
Сложность детектирования пересечений ЗУМ и ОМ при полигональном моделировании заключается в том, что для обеспечения приемлемой точности необходимо вводить в рассмотрение подробные модели поверхностей объектов. Точность моделирования поверхности объекта тем выше, чем больше входящих в модель полигонов. Следовательно, для расчета пересечений в приемлемое время потребуются значительные вычислительные ресурсы, пропорциональные числу полигонов и вершин моделей. Необходимый компромисс между точностью и временем вычислений можно найти, учитывая следующий факт.
При выполнении захвата в начальный момент времени контакт между звеньями ЗУМ и ОМ отсутствует — между ними имеется зазор. В процессе смыкания пальцев ЗУМ зазор постепенно уменьшается, затем происходит пересечение поверхностей ЗУМ и ОМ и объект захватывается. Исходя из этого, можно снизить ресурсоемкость вычислений при достаточной точности путем разбиения процедуры обнаружения пересечений на две фазы. В первой фазе моделирования захвата можно использовать «грубые» и менее ресурсоемкие методы обнаружения пересечения. В этой фазе просто устанавливается факт столкновения ЗУМ и ОМ. Более точные методы расчета пересечения поверхностей ЗУМ и ОМ применяются во второй фазе операции — после обнаружения пересечения ЗУМ и ОМ в первой фазе. В литературе эти этапы получили названия широкой (быстрая и грубая оценка) и узкой (более медленная и точная) фаз обнаружения столкновений [4].
Анализ существующих методов и алгоритмов широкой фазы. Наиболее часто используемыми алгоритмами широкой фазы обнаружения пересечений являются методы ограничивающих объемов (ОО) — в виде сферы и параллелепипедов.
В методе ОО элементы конструкции ЗУМ и ОМ полагаются помещенными внутрь других фигур, имеющих более простую форму. Расчет расстояний между ограничивающими объемами ЗУМ и ОМ требует значительно меньших затрат времени. Потеря точности компенсируется при реализации узкой фазы расчета захвата.
Использование в качестве ОО сферы является самым простым, быстрым, но и самым грубым приемом обнаружения столкновений (рис. 2, а). Фигура в виде параллелепипеда более точно, чем сфера, повторяет форму объектов. При этом проверка на пересечение с использованием в качестве ОО параллелепипедов (ОП) выполняется также, как и в случае сферы, достаточно быстро. Поэтому ОП наиболее часто используют для проверки на наличие пересечения.
а б в
Рис. 2. Ограничивающие объемы
Используют два вида ОП — с привязкой к осям координат (Axis-Aligned Bounding Box, AABB) и ориентируемый ОП (Oriented Bounding Box, OBB). Ребрами, параллельными осям мировой системы координат, задается ААВВ. При вращении объекта AABB изменяет свои размеры, но всегда остается ориентированным по осям некоторой неподвижной системы координат (СК). Поэтому наличие пересечения проверяется сравнительно просто. Однако использование AABB часто приводит к появлению таких ОО, форма которых значительно отличается от реальных прототипов (рис. 2, б). Это снижает точность результатов.
Ориентируемый ОП (OBB) имеет ребра, изменяющие свою ориентацию в пространстве соответственно ориентации объекта. Соответствующим выбором параметров можно выбрать такой ОВВ, который плотно облегает объект (рис. 2, в). В отличие от AABB, OBB вращается вместе с объектом и не меняет своих размеров. Однако проверка на пересечение OBB осуществляется сложнее, чем в рассмотренных ранее методах ограничивающей сферы и AABB.
Решение о выборе между AABB или OBB зависит от условий конкретной задачи. Для построения OBB как такового требуется больше вычислительных ресурсов, чем для построения AABB. Параметры OBB рассчитываются один раз в начале операции захвата. В последующем OBB не изменяет свою форму (изменяется только его положение и ориентация в пространстве в соответствии с текущим положением и ориентацией объекта).
Поскольку в общем случае OBB более точно, чем AABB, повторяет форму объекта, точность определения факта пересечения OBB выше, чем AABB. Поэтому, если проверка на пересечение осуществляется между небольшим числом объектов, то целесообразно использовать OBB как более точный. Если же на каждом шаге могут появляться новые объекты, то применяют более быстрый расчет для AABB.
Модернизированный алгоритм широкой фазы. Поскольку поверхности ОО часто значительно выступают за пределы очертаний объектов, пересечение поверхностей ОО не всегда соответствует пересечению поверхностей ЗУМ и ОМ. Факт пересечения можно установить только после перехода к полигональному представлению их поверхностей. Однако, как было отмечено, это требует существенного увеличения объема вычислений. Объем вычислений можно сократить, если каким-либо образом ограничить список полигонов и вершин полигональной модели. Далее предложен алгоритм решения такой задачи.
Введем в рассмотрение некоторые области потенциального взаимодействия (ОПВ), каждая из которых соответствует одному из ОО. В качестве ОО примем ОВВ. Такой выбор обусловлен относительно небольшим числом участвующих в рассмотрении объектов.
Область потенциального взаимодействия имеет вид параллелепипеда, расположенного внутри соответствующего OBB и ограничивающего пространство проникновения другого OBB. Она строится таким образом, что ее ребра параллельны ребрам OBB, которому она соответствует и внутри которого находится. Область потенциального взаимодействия для ОП ОМ и ОП звена ЗУМ изображены на рис. 3, а зелеными и красными линиями.
а б
Рис. 3. Области потенциального взаимодействия ОМ с одним из звеньев ЗУМ
На рис. 3, б эти же ОПВ представлены отдельно. Синими точками обозначены вершины ОМ, находящиеся внутри ОПВ, соответствующей OBB ОМ, розовыми — вершины звена ЗУМ, расположенные в ОПВ OBB звена ЗУ. Видно, что число точек и полигонов, вершинами которых они являются, существенно меньше общего числа вершин и полигонов моделей поверхностей ОМ и ЗУМ.
Очевидно, что пересечение поверхностей ОМ и ЗУМ может происходить только внутри ОПВ. Причем для практики представляют интерес только те случаи, когда в ОПВ находятся одновременно вершины многогранников и ОМ, и ЗУМ.
Подход с использованием ОПВ увеличивает надежность определения факта пересечения, поскольку уменьшается число случаев, когда имеет место пересечение ОП, но пересечение между полигональными моделями отсутствует. Кроме того, использование ОПВ позволяет модернизировать алгоритм широкой фазы, а именно: расчеты в широкой фазе выполняются не только до момента пересечения ОП ОМ и ЗУМ, но и после состоявшегося пересечения этих ОП.
Основанием для перехода к узкой фазе служит факт обнаружения в ОПВ вершин многогранников поверхностей и ОМ, и ЗУМ.
Анализ существующих методов и алгоритмов узкой фазы. В узкой фазе определения пересечения поверхностей объектов с использованием их полигональных моделей чаще всего используются алгоритмы, базирующиеся на методе Gilbert — Johnson — Keerthi (GJK) [5], которые определяют характеристики пересечения полигонов поверхностей путем анализа разности Минковского для векторов, описывающих полигоны, и алгоритмы, опирающиеся на расчет взаимного расположения областей Вороного фрагментов многогранников: алгоритм Lin — Canny [6] и алгоритм V-Clip [7].
В работе [7] приводится сравнение быстродействия алгоритмов GJK, Lin — Canny и V — Clip. Показано, что скорость определения пересечения объектов алгоритма V — Clip значительно больше скорости других алгоритмов. Алгоритм V — Clip используется во многих физических движках — компьютерных программах, моделирующих в виртуальной среде поведение объектов реального мира. В качестве примера можно привести Bullet Physics Library [8] и
Open Dynamics Engine [9]. Но несмотря на явные достоинства, метод V-Clip сложен в реализации, требует предварительного расчета областей Вороного. Результат его работы нуждается в дополнительной проверке и фильтрации.
Авторами с помощью пакета Bullet Physics Library был проведен расчет граничных точек области пересечения (находящихся на границе области пересечения) поверхностей пальца кисти Schunk SDH и объектов при захвате ОМ в виде детской игрушки и яблока.
Результаты расчета приведены на рис. 4 (точки обозначены зеленым). Из рис. 4, а видно наличие ложных граничных точек области пересечения, расположенных вне поверхностей моделей ЗУМ и ОМ; рис. 4, б иллюстрирует случай неполного определения граничных точек. Результаты моделирования, выполненного с использованием Open Dynamics Engine, аналогичны и обладают такими же недостатками.
Рис. 4. Моделирование пересечения полигональных моделей звена ЗУ и ОМ с использованием Bullet Physics Library
Недостатком рассматриваемых алгоритмов является их применимость только к выпуклым полигональным моделям, что требует дополнительных вычислительных ресурсов для представления полигональной модели в виде некоторой декомпозиции, состоящей из выпуклых многогранников. Далее предлагается алгоритм решения задач узкой фазы, свободный от указанных недостатков.
Описание оригинального алгоритма узкой фазы. Рассмотрим случай пересечения поверхностей двух объектов. Обозначим эти объекты соответственно А и В. Будем считать, что объект В перемещается относительно объекта А в направлении вектора V (рис. 5). Вопрос о выборе вектора V будет рассмотрен далее.
Пусть Р = {р1,..., рп} — множество вершин полигональной модели тела А. Пусть также Р' = { р1,..., рП} — соответствующее множество проекций вершин Р на по-
Рис. 5. Проекции вершин полигональной модели тела А на поверхность полигональной модели тела В вдоль вектора V
верхность тела B. Проецирование осуществляется в направлении вектора v (см. рис. 5). Тогда, если вектор pipí, связывающий соответствующие элементы из множеств P и P', сонаправлен с вектором v, то вершина pi тела A находится в контакте с телом B, т. е. является граничной точкой области пересечения. Глубина проникновения для нее в направлении вектора v равна модулю вектора pipí, а вектор, вдоль которого отсчитывается глубина проникновения, сонаправлен с pip'.
Для нахождения проекции p', соответствующей вершине pi, предлагается из pi провести прямую, параллельную вектору v, и определить из множества полигонов тела B тот, который эта прямая пересекает. Координаты точки пересечения будут соответствовать проекции pi .
При решении задачи поиска полигона, на котором будет располагаться проекция pi, примем во внимание тот факт, что одна вершина (обозначим ее q¡ ) из множества вершин полигональной модели Q = {q1,..., qm} тела B обладает таким свойством, что угол между векторами p¡qj и v будет наименьшим среди углов между pi и любым другим вектором qk е Q. Данное свойство вытекает из того, что чем меньше угол pí p¡qj, тем меньше расстояние между p' и q¡. Тогда, отыскав эту пару вершин pi и qj, можно избежать перебора всех полигонов тела B в поисках проекции pi и осуществлять поиск только среди полигонов, в которых qj является одной из вершин, что существенно сократит время расчетов.
Таким образом, необходимо решить задачу поиска ближайшего соседа, которая заключается в отыскании среди множества элементов, расположенных в некотором метрическом пространстве, элементов, близких к заданному, согласно некоторой функции близости, определяющей это метрическое пространство. Полигональная модель в этом случае может рассматриваться как замкнутое метрическое пространство.
Среди алгоритмов поиска ближайших соседей можно выделить алгоритм разбиения пространства с помощью диаграммы Вороного [10], поиск в kD-деревьях [11], поиск в BSP-деревьях [12], поиск в VP-деревьях [13]. Сравнение перечисленных методов по таким параметрам, как вычислительная сложность, скорость поиска, сложность реализации, применимость в многомерном пространстве и адаптированность к замкнутым пространствам, показало, что наиболее применимым к решению задачи поиска ближайшего соседа в узкой фазе определения столкновения является алгоритм поиска в VP-деревьях.
Временнйя сложность алгоритма узкой фазы при использовании алгоритма поиска по VP-дереву вместо алгоритма Lin — Canny или V — Clip одинакова. Однако реализация алгоритма поиска по VP-дереву значительно проще. Кроме того, введение ОПВ сокращает время моделирования, так как в узкой фазе расчет проводится только для вершин, находящихся в ОПВ, а не для всех вершин полигональной модели.
Реализация алгоритма обнаружения пересечения объектов. На основе предложенного алгоритма разработано программное обеспечение, с помощью
которого реализован захват объекта трехпалой кистью Schunk SDH, имеющей семь степеней свободы и десять звеньев. Модель схвата создана на базе ROS в формате Unified Robot Description Format (URDF), который содержит информацию о кинематической цепи захватного устройства (ЗУ), типах шарниров, ориентации осей шарниров и длинах звеньев.
Решив прямую позиционную задачу, подробно рассмотренную в [14], определим положение и ориентацию звеньев кисти в инерциальной СК.
Полагаем, что геометрические поверхности ОМ звеньев ЗУ заданы с помощью полигональных моделей. Полигоны имеют форму треугольников, координаты вершин полигонов заданы в СК, совпадающей с СК объекта.
Алгоритм построения OBB. Моделирование захвата деформируемого объекта начинается с построения OBB звеньев ЗУМ и ОМ. Задача построения OBB для полигональной модели, заданной набором точек P = {p1,...,pn}, сводится к нахождению матрицы поворота iobb ob¡ СК объекта к СК параллелепипеда, заданных в СК объекта координат ¡[filj центра параллелепипеда, и трех векторов
>bj) ^obb >
y(obj) Jobb ,
7(obj) obb
, определяющих направления и длины ребер параллелепипеда. Сначала строится матрица М ковариации точек Р, собственные векторы которой используются в качестве направляющих для ребер:
1 п т 1 п
М=-£( -Р)( -Р) , Р=-£Р>.
п 1=1 п 1=1
Собственные векторы матрицы М соответствуют ортам векторов хОЫ ', Ум '1, г" . Они находятся путем решения характеристического уравнения вида
(М-Х-Е )а = 0,
где Х — собственные числа матрицы М; Е — единичная матрица; а — собственные векторы матрицы М, заданные в системе координат объекта.
Зная орты векторов, соответствующие направлению ребер ОВВ, нетрудно получить матрицу поворота т0ьь 0Ь] из СК объекта в СК соответствующего ему ОВВ по формуле:
<obí)
z
tobb obj~
Если полигональная модель выпуклая и расстояния между ее соседними вершинами приблизительно равны, то координаты центра ОВВ ¡""¡Ць] совпада-
ют со средним арифметическим вершин р. Если полигональная модель невыпуклая, а расстояния между ее соседними вершинами значительно разнятся, то 1оЬЬоЬ) находятся из соотношений
l(obj)
координаты l0i
l(obj) _ p + Pi<=-P lobb obj x ~ rx "f"
max {[ - pl} + min {[ - p\)
PiGP
_ max {[p>- p 1}+mp{[p - pl}
¡(0^) - р + 1оЬЬ оЬ) у ~ ГУ ^
+-р Д»)+рп {[р -р Д»)
¡оЬЬ оЬ) » — р» + 2 .
Длины векторов хо^Р, УоЬь^, »м^ определяются отысканием множества максимально удаленных от центра ОВВ точек Р по направлениям, соответствующим этим векторам
\(оЬ))11 —оЬ) •( р, _ ) )]х
obb
piGP iL
у°1)
/ obb
_ max
ptGP
z,
(obj) II _
obb
_ max
ptGP
{|^obb obj '(p! iobj obj )] y {|^^obb obj pi -bjobj )Jz
Координаты вершин я(ооЬЬ^)оЬЬ ОВВ в СК объекта будут иметь вид:
п(оЬ)) — ¡(оЬ)) _ х(оЬ]) - у (оЬ)) _ »(оЬ)) п(оЬ)) — ¡(оЬ)) + (оЬ)) _ у(оЬ)) — »(оЬ)) и1оЬЬ оЬЬ ~ 1оЬЬ оЬ) лоЬЬ УоЬЬ ¿оЬЬ > и2оЬЬ оЬЬ ~ 1оЬЬ оЬ) л оЬЬ УоЬЬ ^оЬЬ >
п(оЬ)) — ¡(оЬ)) + (оЬ)) + (оЬ)) _ »(оЬ)) п(оЬ)) — ¡(оЬ)) - х(оЬ)) + (оЬ)) _ »(оЬ)) и3оЬЬ оЬЬ ~ 1оЬЬ оЬ) лоЬЬ УоЬЬ ¿оЬЬ > и4оЬЬ оЬЬ ~ 1оЬЬ оЬ) лоЬЬ ' УоЬЬ *оЬЬ >
п(оЬ)) — ¡(оЬ)) _ (оЬ)) _ (оЬ)) + »(оЬ)) п(оЬ)) — ¡(оЬ)) + (оЬ)) _ УоЬ)) + »(оЬ))
и5оЬЬ оЬЬ ~ 1оЬЬ оЬ) лоЬЬ УоЬЬ оЬЬ ' и6оЬЬ оЬЬ ~ 1оЬЬ оЬ) лоЬЬ УоЬЬ оЬЬ > п(оЬ)) — ¡(оЬ)) + х(оЬ)) + , (оЬ)) + »(оЬ)) п(оЬ)) — ¡(оЬ)) _ х(оЬ)) + . (оЬ)) + »(оЬ))
и7оЬЬ оЬЬ ~ 1 оЬЬ оЬ) ^ лоЬЬ УоЬЬ ¿оЬЬ > и8оЬЬ оЬЬ ~ 1оЬЬ оЬ) лоЬЬ УоЬЬ ¿оЬЬ •
Результат построения ОВВ для ОМ и звеньев ЗУМ представлен на рис. 6.
Рис. 6. Ориентированные ОП, построенные для звеньев ЗУМ и ОМ
2
Алгоритм вычисления векторов направления относительного движения.
Этот алгоритм определяется особенностями конструкции ЗУМ — векторы соответствуют перемещениям, которые могут совершать звенья захватного устройства при захвате объектов.
Для каждого звена ЗУМ рассчитывается вектор направления движения, заданный в СК звена. С учетом того, что между звеном к и базовым звеном находятся другие звенья кинематической цепи, результирующий вектор ) рассчитывается из соотношения:
..(ззук) = ^ЪеЬ т, т г..(ззуО =^реЬ т, ..(ззуО У^к е1ЬТк0тг0 Уг е1Ь1 тк ¡у1 >
г=1 г=1
где е{ — индикатор наличия движения шарнира, приводящего в движение звено г (ег = 1, если шарнир, к которому прикреплено звено г, изменил свое положение на данном шаге моделирования, и е{ = 0, если приводящий в движение звено г шарнир остался неподвижен); Ьг — скалярная величина, равная модулю линейной скорости г-го звена, если шарнир, к которому прикреплено звено г, поступательного типа, или модулю угловой скорости, если шарнир вращательного типа; тг0 и тк0 — транспонированные матрицы поворота СК г-го и к-го звеньев; т^ — матрица поворота СК звена г относительно СК звена к;
у(ЗЗУг) — вектор направления движения г-го звена относительно предыдущего звена кинематической цепи (звена, к которому прикреплен шарнир, приводящий в движение звено г).
Возможны три варианта вектора у(ЗЗУг). Если шарнир, к которому прикреплено звено, поступательного типа, то вектор у(ЗЗУг) совпадает по направлению с осью шарнира и ||у(ЗЗУг)|| = 1. Если шарнир, к которому прикреплено звено, вращательного типа, то у(ЗЗУг) есть векторное произведение вектора оси шарнира и вектора длины звена ¡£1, который соединяет СК звена г и СК следующего звена кинематической цепи (в случае, когда угол между ¡\% и осью шарнира является малым, а значит ||у(ЗЗУг) || ^ 0, вектор направления движения принимается нулевым). Если звено не имеет шарнира (базовое звено кинематической цепи), то У(ЗЗУг) — нулевой.
Векторы у(ЗЗУг) определяются в начале моделирования, а векторы у!тЗкзук) рассчитываются на каждом шаге моделирования захвата.
В узкой фазе для отыскания проекций вершин полигональной модели звена к ЗУМ на полигональную модель ОМ используется вектор направления относительного движения, равный -у^3^, как если бы звено было неподвижно, а двигался бы объект. Для определения проекций вершин полигональной модели ОМ на полигональную модель к-го звена ЗУМ в качестве вектора относительного направления движения принимается вектор ук°М), заданный в СК ОМ:
(ОМ) т тт ..(ЗЗУк) т (ЗЗУк) Ук =тОМ0тк = том ',
где т0М0 и тк 0 — матрицы поворота из инерциальной СК в СК ОМ и к-го звеньев соответственно; тОМ к — матрицы поворота в СК ОМ из СК звена к.
Далее для простоты изложения вектор относительного перемещения будем обозначать как у.
Широкая фаза алгоритма обнаружения пересечения объектов. Пусть положение ОМ и звена ЗУМ на текущем шаге моделирования задано в инерци-альной СК с помощью векторов линейных смещений ¡ОМ0, ¡ззуо и матриц поворота т0М0, тЗЗУ0 соответственно. Положения параллелепипедов ОВВ относительно СК объектов (ОМ и звена ЗУМ) заданы с помощью векторов линейных смещений ¡¡Ом ОМ, ¡0ЬЗуЗУ ЗЗУ и матриц поворота относительно СК объектов тоььом ОМ, тоььззу ЗЗУ соответственно.
Для определения областей пересечения двух параллелепипедов необходимо координаты их вершин перевести из СК объектов в СК одного из ОВВ. Перевод из СК объектов в СК ОП ОМ осуществляется по формуле
„(оЬЬОМ) =т /тт (т „(оЬЬЗЗУ) ;(0) , ;(0) \\
«гоЬЬЗЗУОМ _ хоЬЬОМ ОМ ^ХОМ0 ^ХЗЗУ 0«гоЬЬЗЗУЗЗУ ЮМГ ¡ЗЗУ0 )),
«(оЬЬОМ) =т «(ОМ) : = 1 8
гоььОМ ОМ оььОМ ОМ гоььОМ ОМ,
Перевод из СК объектов в СК ОП звена ЗУМ:
„(оЬЬЗЗУ) =т /тт (т (оЬЬОМ) - (0) ,7 (0) \\
«оЬЬОМ ЗЗУ _ тоЬЬЗЗУ ЗЗУ ^ ХЗЗУ о ^ хОМ 0«оЬЬОМ ОМ 'ЗЗУ 0 + ¡ОМ 0 ^ ] >
„(оЬЬЗЗУ) =т (ЗЗУ) г = 1 8
«гоЬЬЗЗУ ЗЗУ _ хоЬЬЗЗУ ЗЗУ«оЬЬЗЗУ ЗЗУ , ' - •••> О.
При этом если выполняются неравенства для случая перевода в СК ОП ОМ:
т1п {«оьЬОмОм х) > тах {«оььззуом х), тах{я0ЬЬО°ММОм х} < т1п{«омЗзуОм х},
т1п {«(оЬЬОМ) К тах {«(оЬЬОМ) ) тах {«(оЬЬОМ) ) < т1.п {«(оЬЬОМ) ) т1п | «оЬЬОМ ОМ У | > тах \ «гоЬЬЗЗУОМ у), тах \ «гоЬЬОМ ОМ у ) < т1п | «оЬЬЗЗУ ОМ у /,
тах {«("¡¡ОМ) К х {«(оЬЬОМ) ) х {«(оЬЬОМ) ) < т1п {«(оЬЬОМ) )
тах | «¡оЬЬОМ ОМ г / > тах \ «гоЬЬЗЗУ ОМ г) , тах \ «гоЬЬОМ ОМ г / < т1п | «оЬЬЗЗУ ОМ г /
и для случая перевода в СК ОП звена ЗУМ
т1п {«(оЬЬЗЗУ) К х {«(оЬЬЗЗУ) ) х {«(оЬЬЗЗУ) ) < т1п {«(оЬЬЗЗУ) )
т1п | «гоЬЬЗЗУ ЗЗУ хтах \ «гоЬЬОМ ЗЗУ х /, тах \ «оЬЬЗЗУЗЗУ х}^ т1п | «оЬЬОМ ЗЗУ х /,
т1п {«(оЬЬЗЗУ) К тах {«(оЬЬЗЗУ) ) тах {«(оЬЬЗЗУ) ) < т1п {«(оЬЬЗЗУ) ) т1п \«гоЬЬЗЗУ ЗЗУ у / > тах \ «гоЬЬОМ ЗЗУ у /, тах \«гоЬЬЗЗУ ЗЗУ у ) < т1п |«оЬЬОМ ЗЗУ у /,
т1п {«(оЬЬЗЗУ) К тах {«(оЬЬЗЗУ) ) тах {«(оЬЬЗЗУ) ) < т1п {«(оЬЬЗЗУ) ) т1п |«гоЬЬЗЗУ ЗЗУ г / > тах \ «гоЬЬОМ ЗЗУ г /, тах \«гоЬЬЗЗУ ЗЗУ г / < т1п |«оЬЬОМ ЗЗУ г /,
то пересечения нет, устанавливается факт отсутствия контакта между ОМ и звеном ЗУМ на текущем шаге моделирования.
Если одно или несколько условий не выполняются, то в ОП выделяется область потенциального взаимодействия. Она формируется границами ОВВ, в СК которого осуществлялся перевод, а также плоскостями, заданными координатами вершин второго ОВВ, при проверке которых условия не выполнялись, и нормалями, соответствующими ортам осей СК первого ОВВ, по которым условия не были выполнены (см. рис. 3, а).
Затем устанавливется факт наличия в ОПВ вершин полигональной модели соответствующего объекта. Если ни одна из вершин не располагается внутри ОПВ, делается вывод, что на текущем шаге моделирования контакт между ОМ и звеном ЗУ отсутствует. В этом случае вычисления продолжаются в широкой фазе.
Если же в ОПВ обоих ОП находятся вершины полигональных моделей обоих объектов Р — {р1,...,ру}, Р с Р и 0 — {ц1,...,^}, 0 с Q (см. рис. 3, б), в рамках широкой фазы делается вывод о возможном наличии пересечения, и для этого шага моделирования выполняется узкая фаза определения пересечения, которая выявляет факт наличия или отсутствия пересечения, а также при выявлении пересечения рассчитывает точки, лежащие на поверхности пересечения, соответствующие им глубины и нормали проникания.
Узкая фаза алгоритма обнаружения пересечения объектов. Использование расстояния между точками р1 и ц) в качестве функции близости вершин проще в реализации и понимании, чем использование угла между векторами рц) и V. Очевидно, что угол между векторами рц) и V будет наименьшим, если проекции вершин р, и ц) на плоскость, заданную вектором V в качестве вектора нормали и проходящую через точку р1 вдоль направления относительного движения V, будут ближайшими соседями с точки зрения расстояния между ними. Проецирование множеств точек Р и (0 дает два множества точек — Р' — {р1,...,р'/} и 0' — {ц1,...,ц^ }, элементы которых удовлетворяют равенствам:
Для решения задачи поиска ближайшего соседа для множеств Р' и О' строятся УР-деревья. Построение УР-дерева является рекурсивной процедурой — на каждой итерации алгоритма статистически [13] выбирается опорная точка и вычисляется среднее расстояние от нее до всех остальных точек — радиус области узла Я. Затем остальные точки делятся на два подмножества, называемых
pi_ p i - v
Vx ( Pix - plx ) + Vу ({iy - p1 у ) + Vz {piz - plz )
Vx2 + Vy2 + Vz2
i _1,..., f,
q j _ q j - V
Vx ((jx - plx ) + Vy ((jy - p1 у ) + Vz ((jz - plz )
Vx2 + Vy2 + Vz2
, j _1,..., g.
поддеревьями. Если расстояние между вершиной и опорной точкой меньше Я, то она принадлежит внутреннему поддереву, иначе — внешнему. В каждом из получившихся подмножеств выбирается следующая опорная точка и рассчитывается новый радиус. Операция построения дерева продолжается до тех пор, пока размеры подмножеств не достигнут заданного предела — максимального размера листа [13].
Затем путем поиска по УР-деревьям для каждого элемента множеств Р' и ((' определяется ближайший сосед среди элементов множеств ((' и Р'. Поиск по УР-дереву осуществляется следующим образом — начиная с корневого узла дерева определяется расстояние между вершиной, для которой осуществляется поиск, и центром узла УР-дерева. Если расстояние меньше радиуса области узла, то поиск продолжается во внутреннем поддереве, иначе — во внешнем. По достижении листа дерева выполняется линейный поиск среди точек листа [13].
Далее, опираясь на индексы полученных пар ближайших соседей из множеств Р' и ((', восстанавливаются индексы для соответствующих точек из Р и (( и в итоге для Р и (. На этом этапе проводится дополнительная фильтрация — если угол между векторами р^ и V — тупой, то очевидно, что точка pi тела А не находится в контакте с телом В, и ее можно исключить из дальнейшего рассмотрения (вершины р и q1 на рис. 5).
Для конфигурации ЗУМ, изображенной на рис. 7, а, представлен результат поиска ближайшего соседа по УР-дереву для вершин полигональных моделей, попавших в ОПВ. На рис. 7, а зелеными линиями изображен ОПВ для ОП ОМ, красными — ОПВ для ОП звена ЗУМ. На рис. 7, б синими точками обозначены вершины ОМ, находящиеся внутри ОПВ, соответствующей ОВВ ОМ, розовыми — вершины звена ЗУМ, расположенные в ОПВ ОВВ звена ЗУ, синими и розовыми линиями показаны соответственно ближайшие соседи для вершин ОМ и звена ЗУ после фильтрации.
щ
а
Рис. 7. Этапы узкой фазы определения пересечения ОМ и звена ЗУ
Затем через pi строится прямая, параллельная вектору направления относительного движения v. Далее среди граней второго объекта отыскиваются полигоны, одной из вершин которых является q¡. Для каждого из этих полигонов рассчитывается точка пересечения построенной прямой с плоскостью, совпадающей с гранью, и устанавливается принадлежность полученной точки полигону.
Точка пересечения прямой с плоскостью pi, проходящей через k-й полигон с нормалью nk и вершинами q¡, q^, q¡2, находится с помощью соотношения
, _ _ nkx (pix _qjx ) + nky (piy _qjy ) + nkz (piz _qjz ) pi _ pi _ v ,
nkxVx + nkyVy + nkzVz
_ (j _ qj )x( qj2 _ qj) k ll(qji _ qj )x(qj2 _ qj Ж
Точка pi будет принадлежать полигону k, когда площадь треугольника Aqjqjiqj2 будет равна площадям треугольников Apiqjqj1, Apiqj1qj2 и Apiq¡ 2qj:
S&qjqjiqj2 _ SApiqjqji + SApiqjiqj2 + SAp¡qj2qj .
Если вершина pi принадлежит этому полигону, то она является проекцией точки p¡ на поверхность полигональной модели второго объекта B вдоль вектора направления относительного движения v второго тела относительного первого. Если вектор pipi сонаправлен с вектором v, то вершина p¡ лежит на границе области пересечения. Соответствующий точке p¡ вектор, вдоль которого отсчитывается глубина проникновения, совпадает по направлению с вектором pipi, а глубина проникновения равна модулю вектора pipi (рис. 7, в).
Проделав эти вычисления для каждой из вершин, расположенной в ОПВ, определяем точки, лежащие на границе области пересечения, соответствующие им глубины и нормали проникновения. Результат работы предложенного алгоритма для всех звеньев ЗУМ представлен на рис. 8. Справа на рисунке синими и
Рис. 8. Результат работы предложенного алгоритма
розовыми точками обозначены вершины ОМ и звеньев ЗУМ, находящиеся внутри пятен контакта, синие и розовые линии соответствуют векторам pip' и qq.
Заключение. Таким образом, результаты моделирования подтвердили правильность и эффективность предложенного алгоритма.
Введение областей потенциального взаимодействия повышает точность широкой фазы определения пересечения вследствие проверки наличия вершин полигональных моделей внутри соответствующих ОПВ, поскольку в процессе моделирования уменьшается число случаев, когда имеет место пересечение ОП, но пересечение между полигональными моделями отсутствует.
Показано, что использование в узкой фазе алгоритма поиска по VP-дереву вместо V-Clip или Lin-Canny незначительно повлияло на временную сложность метода. Но поскольку время вычислений уменьшается вместе с количеством точек, для которых происходят вычисления, введение ОПВ сокращает время моделирования, так как в узкой фазе расчет проводится только для вершин, находящихся в ОПВ, а не для всех вершин полигональной модели.
Существенным достоинством предложенного алгоритма является его применимость к невыпуклым полигональным моделям без необходимости их представления в виде некоторой декомпозиции, состоящей из выпуклых многогранников.
Форма области пересечения, глубины и нормали проникания могут быть использованы для определения геометрии пятен контакта и эпюр нагрузки, которые участвуют в расчете сил контактного взаимодействия между ЗУМ и ОМ. По известным силам контактного взаимодействия можно оценить качество захвата и предсказать поведение ОМ в процессе выполнения операции.
ЛИТЕРАТУРА
1. Аппаратно-программный комплекс для решения задач автоматического захвата объекта манипуляторами / А.Г. Лесков, В.В. Илларионов, И.А. Калеватых, С.Д. Мо-рошкин, К.В. Ба-жинова, Е.В. Феоктистова // Инженерный журнал: наука и инновации. 2015. Вып. 1. DOI: 10.18698/2308-6033-2015-1-1361 URL: http://engjournal.ru/catalog/pribor/robot/1361.html
2. Boivin E., Sharf I., Doyon M. Optimum grasp of planar and revolute objects with gripper geometry constraints // Proc. IEEE ICRA 2004. 2004. Р. 326-332.
DOI: 10.1109/R0B0T.2004.1307171
3. Pauly M., Pai D.K., Guibas L.J. Quasi-Rigid Objects in Contact // Eurographics/ACM SIGGRAPH Symposium on Computer Animation. 2004. Р. 109-119.
4. Ericson C. Real-time collision detection. San Francisco: Elsevier, 2005. 593 p.
5. Gilbert E.G., Johnson D.W., Keerthi S.S. A fast procedure for computing the distance between complex objects in three-dimensional space // IEEE Journal of Robotics and Automation. 1988. Vol. 4. No. 2. Р. 193-203. DOI: 10.1109/56.2083
6. Lin M.C. Efficient collision detection for animation and robotics. PhD dissertation. University of California, Berkeley, CA, USA, 1993. 159 p.
7. Mirtich B. V-Clip: fast and robust polyhedral collision detection // Journal ACM Transactions on Graphics. 1998. Vol. 17. No. 3. Р. 177-208.
8. Bullet Physics Library. URL: http://bulletphysics.org/ (дата обращения 25.12.2015).
9. Open Dynamics Engine. URL: http://www.ode.org/ (дата обращения 25.12.2015).
10. Препарата Ф., Шеймос М. Вычислительная геометрия. М.: Мир, 1989. 478 с.
11. Panigrahy R. An improved algorithm finding nearest neighbor using Kd-trees // Proc. 8th Latin American Symposium. Buzios, Brazil, 2008. Vol. 26. No. 4. Р. 387-398.
12. Maneewongvatana S., Mount D.M. An empirical study of a new approach to nearest neighbor searching // Proc. 3th International workshop on algorithm engineering and experimentation. Washington, DC, USA, 2001. Р. 172-187.
13. Yianilos P.N. Data structures and algorithms for nearest neighbor search in general metric spaces // Proc. 4th annual ACM-SIAM symposium on discrete algorithms. Austin, TX, USA, 1993. Р. 311-321.
14. Лесков А.Г., Бажинова К.В., Морошкин С.Д., Феоктистова Е.В. Построение моделей кинематики исполнительных механизмов манипуляционных роботов с использованием блочных матриц // Инженерный журнал: наука и инновации. 2013. Вып. 9.
DOI: 10.18698/2308-6033-2013-9-954 URL: http://engournal.ru/catalog/pribor/robot/954.html
Лесков Алексей Григорьевич — д-р техн. наук, профессор, директор Дмитровского филиала МГТУ им. Н.Э. Баумана (Российская Федерация, 141801, Московская обл., Дмитровский р-н, пос. Орево).
Селиверстова Елена Владимировна — инженер Дмитровского филиала МГТУ им. Н.Э. Баумана (Российская Федерация, 141801, Московская обл., Дмитровский р-н, пос. Орево), аспирантка кафедры «Робототехнические системы и мехатроника» МГТУ им. Н.Э. Баумана (Российская Федерация, 105005, Москва, 2-я Бауманская ул., д. 5).
Просьба ссылаться на эту статью следующим образом:
Лесков А.Г., Селиверстова Е.В. Расчет областей пересечения поверхностей захватных устройств манипуляторов и деформируемых объектов при планировании и моделировании захвата // Вестник МГТУ им. Н.Э. Баумана. Сер. Приборостроение. 2016. № 6. C. 97-114. DOI: 10.18698/0236-3933-2016-6-97-114
COMPUTATION OF INTERSECTION REGIONS OF ROBOTIC GRIPPER AND DEFORMABLE OBJECT SURFACES DURING GRАSP PLANNING AND SIMULATION
A.G. Leskov1 agleskov@rambler.ru
E.V. Seliverstova1,2 feoktistovaev@mail.ru
1 Dmitrov Branch of Bauman Moscow State Technical University, Orevo, Moscow Region, Russian Federation
2 Bauman Moscow State Technical University, Moscow, Russian Federation
Abstract Keywords
The study tested methods for detecting and computing the Grip, deformable object, simulat-
intersection regions of robotic gripper and deformable ing, intersection of polygonal
object surfaces during the grasp planning and simulation. models, Oriented Bounding Box,
We suggest an algorithm for detection and computation of VP-tree
the intersection regions comprising both broad and narrow phases. The broad phase algorithm is based on the bounding box method and is improved by introducing the original algorithm for detecting areas of potential interaction between the gripper and the object. The narrow phase algorithm is novel. It uses the nearest neighbor search methods and considers the movement direction of interacting bodies. As a result of our research, we developed a computer program and carried out experiments to analyze the efficiency of the proposed algorithms for the grasp planning with the 3-finger robotic hand Schunk SDH
REFERENCES
[1] Leskov A.G., Illarionov V.V., Kalevatykh I.A., Moroshkin S.D., Bazhinova K.V., Feok-tistova E.V. Hardware-software complex for solving the task of automatic capture of the object with manipulators. Inzhenernyy zhurnal: nauka i innovatsii [Engineering Journal: Science and Innovation], 2015, iss. 1 (in Russ.). DOI: 10.18698/2308-6033-2015-1-1361
Available at: http://engjournal.ru/ eng/catalog/pribor/robot/1361.html
[2] Boivin E., Sharf I., Doyon M. Optimum grasp of planar and revolute objects with gripper geometry constraints. Proc. ICRA 2004, 2004, pp. 326-332.
DOI: 10.1109/R0B0T.2004.1307171
[3] Pauly M., Pai D.K., Guibas L.J. Quasi-rigid objects in contact. Proc. 2004 ACM SIGGRAPH/Eurographics symposium on Computer animation, 2004, pp. 109-119.
[4] Ericson C. Real-time collision detection. San Francisco, Elsevier, 2005. 593 p.
[5] Gilbert E.G., Johnson D.W., Keerthi S.S. A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE Journal of Robotics and Automation, 1988, vol. 4, no. 2, pp. 193-203. DOI: 10.1109/56.2083
[6] Lin M.C. Efficient collision detection for animation and robotics. PhD dissertation. University of California, Berkeley, CA, USA, 1993. 159 p.
[7] Mirtich B. V-Clip: fast and robust polyhedral collision detection. Journal ACM Transactions on Graphics, 1998, vol. 17, no. 3, pp. 177-208.
[8] Bullet Physics Library. Available at: http://bulletphysics.org/ (accessed 25.12.2015)
[9] Open Dynamics Engine. Available at: http://www.ode.org/ (accessed 25.12.2015)
[10] Preparata F.P., Shamos M.I. Computational geometry: an introduction. New York, Spinger-Verlag. 412 p. (Russ. ed.: Vychislitelnaya geometriya. Moscow, Mir Publ., 1989. 478 p.)
[11] Panigrahy R. An improved algorithm finding nearest neighbor using Kd-trees. Proc. 8th Latin American Symposium, Büzios, Brazil, 2008, vol. 26, no. 4, pp. 387-398.
[12] Maneewongvatana S., Mount D.M. An empirical study of a new approach to nearest neighbor searching. Proc. third international workshop on algorithm engineering and experimentation, Washington, DC, USA, 2001, pp. 172-187.
[13] Yianilos P.N. Data structures and algorithms for nearest neighbor search in general metric spaces. Proc. fourth annual ACM-SIAM Symposium on Discrete algorithms, Austin, TX, USA, 1993, pp. 311-321.
[14] Leskov A.G., Bazhinova K.V., Moroshkin S.D., Feoktistova E.V. Modeling of trobotic arms kinematics by means of block matrixes. Inzhenernyy zhurnal: nauka i innovatsii [Engineering Journal: Science and Innovation], 2013, iss. 9, no. 21 (in Russ.). DOI: 10.18698/2308-6033-2013-9-954 Available at: http://engjournal.ru/eng/catalog/pribor/robot/ 954.html
Leskov A.G. — Dr. Sci. (Eng.), Professor, Director of Dmitrov Branch of Bauman Moscow State Technical University (Orevo, Dmitrovsky District, Moscow Region, 141801 Russian Federation).
Seliverstova E.V. — engineer of Dmitrov Branch of Bauman Moscow State Technical University (Orevo, Dmitrovsky District, Moscow Region, 141801 Russian Federation), post-graduate student of Robotic Systems and Mechatronics Department, Bauman Moscow State Technical University (2-ya Baumanskaya ul. 5, Moscow, 105005 Russian Federation).
Please cite this article in English as:
Leskov A.G., Seliverstova E.V. Computation of Intersection Regions of Robotic Gripper and Deformable Object Surfaces During Grasp Planning and Simulation. Vestn. Mosk. Gos. Tekh. Univ. im. N.E. Baumana, Priborostr. [Herald of the Bauman Moscow State Tech. Univ., Instrum. Eng.], 2016, no. 6, pp. 97-114. DOI: 10.18698/0236-3933-2016-6-97-114
Введение
в информационную безопасность автоматизированных систем
В.В. Бондарев
В Издательстве МГТУ им. Н.Э. Баумана вышло в свет учебное пособие автора В.В. Бондарева
«Введение в информационную безопасность автоматизированных систем»
Рассмотрена законодательная база информационной безопасности, приведен перечень возможных угроз, отражены основные подходы к созданию систем защиты информации, представлена классификация предупредительных мер, изучены вопросы, связанные с программно-аппаратными механизмами обеспечения информационной безопасности.
ш-
По вопросам приобретения обращайтесь:
105005, Москва, 2-я Бауманская ул., д. 5, стр. 1
+7 (499) 263-60-45
press@bmstu.ru
www.baumanpress.ru
