Расчет областей пересечения поверхностей захватных устройств манипуляторов и деформируемых объектов при планировании и моделировании захвата

8 / 18

А.Г. Лесков, Е.В. Селиверстова
104 ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. Приборостроение. 2016. № 6
которого реализован захват объекта трехпалой кистью Schunk SDH, имеющей
семь степеней свободы и десять звеньев. Модель схвата создана на базе ROS в
формате Unified Robot Description Format (URDF), который содержит информа-
цию о кинематической цепи захватного устройства (ЗУ), типах шарниров, ори-
ентации осей шарниров и длинах звеньев.
Решив прямую позиционную задачу, подробно рассмотренную в [14], опре-
делим положение и ориентацию звеньев кисти в инерциальной СК.
Полагаем, что геометрические поверхности ОМ звеньев ЗУ заданы с помо-
щью полигональных моделей. Полигоны имеют форму треугольников, коорди-
наты вершин полигонов заданы в СК, совпадающей с СК объекта.
Алгоритм построения OBB. Моделирование захвата деформируемого объек-
та начинается с построения OBB звеньев ЗУМ и ОМ. Задача построения OBB для
полигональной модели, заданной набором точек  
1,..., ,
n P p p сводится к
нахождению матрицы поворота obb obj  СК объекта к СК параллелепипеда, задан-
ных в СК объекта координат ( )obj
obbobj l центра параллелепипеда, и трех векторов
( ) , obj
obb x ( ), obj
obb y ( ) , obj
obb z определяющих направления и длины ребер параллелепипеда.
Сначала строится матрица M ковариации точек , P собственные векторы ко-
торой используются в качестве направляющих для ребер:
   
1
1 ,
n T
i i
i
M p p p pn 
   
1
1 .n
i
i
p p
n 
 
Собственные векторы матрицы M соответствуют ортам векторов ( ) , obj
obb x ( ) , obj
obb y
( ). obj
obb z Они находятся путем решения характеристического уравнения вида
  0, M E  
где  — собственные числа матрицы М ; Е — единичная матрица;  — собствен-
ные векторы матрицы М , заданные в системе координат объекта.
Зная орты векторов, соответствующие направлению ребер OBB, нетрудно
получить матрицу поворота obb obj  из СК объекта в СК соответствующего ему
OBB по формуле:
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
obj obj obj
y x z obb obb obb
obj obj obj
obb obb obb
obj obj obj
y x z obb obb obb
obb obj obj obj obj
obb obb obb
obj obj obj
y x z obb obb obb
obj obj obj
obb obb obb
x x x
x x x
y y y
y y y
z z z
z z z
 
 
 
 
 
   
 
 
 
 
.
Если полигональная модель выпуклая и расстояния между ее соседними
вершинами приблизительно равны, то координаты центра OBB ( )obj
obb obj l совпада-