Планирование, моделирование и экспериментальное исследование типовой манипуляционной операции

8 / 14

А.Г. Лесков, В.В. Илларионов, И.А. Калеватых
64 ISSN 0236-3941. Вестник МГТУ им. Н.Э. Баумана. Сер. Машиностроение. 2016. № 4
  
т.0 ст т.1 ст
т.0 т.1
.
l l l l
h
l l
 


Ширина клапана сумки используется в качестве весового коэффициента
при изменении масштаба шаблона траектории движения пальца. Координаты
точек шаблона траектории после масштабирования 
i Р принимают значения:
  ,
i i Р hP i = 1, …, 4, где i P — радиус-векторы точек исходной траектории.
Поворот шаблона траектории. Направление маркера-стрелки описывается
кватернионом ст . q Этот кватернион задает новое направление оси Оу CК, в ко-
торой расположена исходная траектория. Шаблон нужно повернуть так, чтобы
он лежал в плоскости, проходящей через маркер-стрелку перпендикулярно
плоскости клапана. Этот поворот описывается с помощью вспомогательного
кватерниона , q содержащего в себе поворот от оси Oz после поворота, задан-
ного ст, q к нормали плоскости клапана сумки. Согласно определению кватер-
ниона, запишем
 
      sin( / 2) , sin( / 2) , sin( / 2) , cos( / 2) ,x y z q n n n
где  — угол, на который нужно довернуть повернутый кватернионом qст орт k
оси Oz вокруг вектора n, чтобы он стал сонаправлен с нормалью плоскости кла-
пана сумки N ,
       
1 1 1
ст ст ст ст ст ст arccos ,
x x y z
y z N q kq N q kq N q kq       
  
  
т.0 ст т.1 ст
т.0 ст т.1 ст
,
l l l l
N
l l l l
 

 
 
 
1
ст ст
1
ст ст
.
q kq N
n
q kq N

 
Иными словами, кватернион  q задает требуемое направление оси Оz СК с
исходной траекторией. Координаты точек исходной траектории, после масшта-
бирования с учетом заданного оператором направления движения конца пальца
i Р  рассчитываются по соотношению
    1
ст ст , 1, , 4.
i i Р q q hР q q i
     
Перенос графического образа шаблона. Программа планирования безопас-
ных траекторий движения в качестве входных параметров принимает коорди-
наты точек в инерциальной CК, через которые должен пройти схват МР, и его
ориентацию для каждой точки траектории. Поскольку палец схвата неподвижен
относительно схвата в процессе выполнения операции, то его траекторию мож-
но определить путем параллельного переноса траектории пальца на вектор, за-
данный в СК пальца сх , l соединяющий конец пальца и основание схвата. Этот
вектор определяется из конструкции схвата. Координаты точек расчетной тра-
ектории для раскрытия сумки 
i Р находят с учетом заданного оператором
положения первой точки траектории ст l и вектора сх l по следующему соотно-
шению: