CC BY
166
5. Shestakov A. A. Obobshchennyy pryamoy metod Lyapunova dlya sistem s raspredelennymi parametrami. Moscow, Nauka, 2007. (in Russian).
6. Kirichenko V. F., Samarkin P. A. Kachestvennyy analys evolucionnih uravneniy v neklassicheskoy teorii obolochek c nachalnymy nepravilnostyami [Qualitative analysis of the evolution equations in nonclassical theory of shallow shells with initial irregularities]. Vestnik Saratov. gos. tekhn. univ., 2011, no. 3 (57). iss. 1, pp. 33-40 (in Russian).
7. Kirichenko V. F., Samarkin P. A. Ispolzovanie norm iz fazovogo prostranstva pri issledovanii dinamicheskoy ustoychivosti pologih obolochek [Application of the phase space norms in the analysis of dynamic buckling of shallow shells]. Vestnik Saratov. gos. tekhn. univ., 2011, no. 4 (60), iss. 2, pp. 70-76 (in Russian).
8. Lions J. L. Quelques méthodes de résolution des
problèmes aux limites non linéaires [Some methods for solving nonlinear boundary value problems, in French]. Paris, Dunod, 1969.
9. Ladyzhenskaya O. A. The Boundary Value Problems of Mathematical Physics. Applied Mathematical Sciences, 1985.
10. Rektoris K. Variacionnye metody v matematicheskoj fizike i tehnike [Variational Methods in Mathematical Physics and Engineering]. Moscow, Mir, 1985 (in Russian).
11. Ciarlet P. G., Rabier P. Les Equations de von Karman [Von Karman equations]. Springer, 1980 (in French).
12. Vorovich I. I. Matematicheskie problemy nelineynoy teorii pologih obolochek [Mathematical problems of nonlinear theory of Shallow Shells]. Moscow, Nauka, 1989 (in Russian).
УДК 531.38, 681.5
ДУАЛЬНЫЕ МАТРИЧНЫЕ И БИКВАТЕРНИОННЫЕ МЕТОДЫ РЕШЕНИЯ ПРЯМОЙ И ОБРАТНОЙ ЗАДАЧ КИНЕМАТИКИ РОБОТОВ-МАНИПУЛЯТОРОВ НА ПРИМЕРЕ СТЭНФОРДСКОГО МАНИПУЛЯТОРА. I
Е. И. Ломовцева1, Ю. Н. Челноков2
1 Аспирант кафедры математического и компьютерного моделирования, Саратовский государственный университет им. Н. Г. Чернышевского, [email protected]
2Доктор физико-математических наук, профессор кафедры математического и компьютерного моделирования, Саратовский государственный университет им. Н. Г. Чернышевского, [email protected]
На примере стэнфордского манипулятора рассматривается методология решения прямой задачи кинематики роботов-манипуляторов с использованием винтовых методов механики (матриц дуальных направляющих косинусов, бикватернионов Клиффорда), выводятся кинематические уравнения движения манипулятора, необходимые для решения обратной задачи кинематики манипулятора с использованием бикватернионной теории кинематического управления.
Ключевые слова: робот-манипулятор, прямая задача кинематики, матрица дуальных направляющих косинусов, бикватер-нион, кватернион, кинематические уравнения.
1. КИНЕМАТИЧЕСКАЯ СХЕМА И СИСТЕМЫ КООРДИНАТ
Стэнфордский манипулятор [1] представляет собой манипулятор с шестью степенями свободы: пятью вращательными и одной поступательной. В качестве обобщенных координат выступают углы ^ (г = 1, 2,4, 5,6) поворота г-го звена относительно (г — 1)-го и величина — линейное поступательное перемещение 3 звена относительно 2. Схема манипулятора и вводимые системы координат приведены на рис. 1. На нем Х0У0— система координат, связанная с основанием манипулятора, X,У— система координат, связанная с г-м звеном манипулятора, ось zi направлена вдоль оси г-го сочленения; ось х, перпендикулярна оси zi-1 и направлена от нее; ось у дополняет оси х,, zi до правой декартовой системы координат.
Относительное положение звеньев стэнфордского манипулятора может быть описано с помощью трех соответствующих каждому звену конструктивных геометрических параметров в,, а,, ^, приведенных в таблице.
Параметры систем координат звеньев стэнфордского манипулятора
Рис. 1. Схема стэнфордского манипулятора
В таблице Ог — присоединенный угол (угол, на который надо повернуть ось хг-1 вокруг оси хг-1 чтобы она стала сонаправлена с осью хг); — расстояние между пересечением оси хг-1 с осью хг и началом (г — 1)-й системы координат, отсчитываемое вдоль оси хг-1; ¿ь ¿2, — постоянные велиины, — переменная величина; аг — угловое смещение (угол, на который надо повернуть ось хг-1 вокруг оси хг, чтобы она стала сонаправлена с осью ).
Обобщенная координата — угол поворота г-го звена относительно (г — 1)-го звена вокруг оси хг-1, обобщенная координата — поступательное перемещение 3 звена относительно 2 вдоль оси х2(х3). Отметим, что обобщенные координаты отсчитываются вокруг тех же осей, что и конструктивные параметры . На рис. 1 Фг = &г + .
Сочленение г ©г аг йг
1 -90° -90° й\
2 -90° 90° й2
3 -90° 0° йз
4 0° -90° 0
5 0° 90° 0
6 0° 0° йб
2. РЕШЕНИЕ ПРЯМОЙ ЗАДАЧИ КИНЕМАТИКИ С ИСПОЛЬЗОВАНИЕМ МАТРИЦ ДУАЛЬНЫХ НАПРАВЛЯЮЩИХ КОСИНУСОВ И БИКВАТЕРНИОНОВ КОНЕЧНЫХ ПЕРЕМЕЩЕНИЙ
Прямая задача кинематики состоит в определении линейного положения и ориентации схвата манипулятора относительно абсолютной системы координат Х0УоZo по известному вектору обобщенных координат = (^1 (£),ф2(£),..., Зб(£)) = (¿),^з(£),^4(¿),^5^6(^)) и заданным геометри-
ческим параметрам звеньев.
Для решения прямой задачи кинематики используется следующая схема конечных перемещений звеньев манипулятора:
Хоад ——— Х1У Zl -—-—и Х2^2 -———— ХзУзZз ———— Х4^4 Х5У5Z5 -————> Хбад,
с л
Х0 У0 Z0 —'—> Х6У6 Z6 •
Здесь Ог и Лг — матрица дуальных направляющих косинусов и бикватернион конечного перемещения г-го звена манипулятора относительно (г — 1)-го, О и Л — матрица дуальных направляющих косинусов и бикватернион конечного перемещения выходного звена манипулятора относительно основания.
Нахождение матрицы О и бикватерниона Л в виде функций обобщенных координат манипулятора с помощью формул сложения конечных перемещений [2,3] и составляет предмет решения прямой задачи кинематики. Уравнения для нахождения матрицы О и бикватерниона Л в случае стэнфордского манипулятора имеют вид
О = с + йс0 =
012 °13\ / с11
О21 О22 О23 I = I С21
О31 О32 О33 с31
= Об О5 О4 О3 О2О1,
о
с12 с22 с32
с13 с23 с33
+ й
О г = Сг + ЙС0,
'С?1
С21
0
31
с0 с12
с022
32
с0103
с023
с0 с33
(1)
Л = Ло + Л111 + Л212 + Лз 1з = Л + йл0 = Ло + А111 + Л212 + Аз 1з + й(Л0 + Л011 + Л212 + А31з) = = (Ло + йЛ0 ) + (А1 + йЛ0 )11 + (Л2 + йЛ0 )12 + (Лз + йЛ0 )1з =
= Л1 о Л2 о Лз о Л4 о Л5 о Лб,
лг = лг + йл0.
(2)
Здесь 11, 12, 1з — векторные мнимые единицы Гамильтона; й — символ (комплексность) Клиффорда, обладающая свойством й2 = 0; о — символ кватернионного умножения; Огк (г, к = 1,2,3) и Лj (? = 0,1, 2,3) — дуальные направляющие косинусы и дуальные параметры Родрига-Гамильтона (Эйлера), характеризующие угловое и линейное положение схвата манипулятора в основной системе координат ХоУо; сгк (г, к = 1, 2,3) и Лj (? = 0,1, 2,3) — вещественные направляющие косинусы и вещественные параметры Родрига-Гамильтона (Эйлера), характеризующие ориентацию схвата в системе координат ХоУо(главные части дуальных величин Огк и Лj), с®к и Л® — моментные части дуальных величин Огк и Лj, характеризующие линейное положение схвата в системе координат ХоУос и л — вещественная матрица направляющих косинусов и кватернион поворота, описывающие ориентацию схвата в системе координат ХоУо; с0 и л0 — вещественная матрица и кватернион, описывающие поступательное перемещение схвата в системе координат ХоУо; сг(лг) и с0 (л0) — главная и моментная части матрицы дуальных направляющих косинусов Ог (бикватерниона Лг), характеризующие угловое и линейное положение г-го звена (системы координатХгУгZг) в системе координат Хг-1 Уг-1 , связанной с (г — 1)-м звеном.
Конечное перемещение г-го звена относительно (г — 1)-го, описываемое матрицей Ог или бикватернионом Лг, представляет собой композицию двух перемещений: перемещения на дуальный угол Фг + = (©г + ) + вокруг оси гг-1 (рис. 2) и поворота на угол аг вокруг оси хг, полученной из оси хг—1 в результате первого перемещения (рис. 3).
Схема конечных перемещений имеет следующий вид:
Хг — 1 У—1 Zг_1
С1 ,Л1
» X1 У1 ^ ^
Хг Уг Zг.
Рис. 2. Схема первого перемещения г-го звена относительно (г — 1)-го
Матрица дуальных направляющих косинусов О1 и бикватернион Л1 первого перемещения г-го звена относительно (г — 1)-го имеют вид
008((вг + у )+ ) 81п((©г + у )+ ) 0' Ог1 = ( — 8т((©г + у) + С08((©г + у) + ) 0
0 0 1.
А1 (©г + Уг , (¿\ , . /©г + у г (Л
Л =С0Ч+ 5 2) +81Ч+ 5~2 )
1з.
Второе конструктивное перемещение г-го звена относительно (г — 1)-го представляет собой поворот на угол аг вокруг оси хг (см. рис. 3). За счет отсутствия линейного перемещения дуальные углы обра-
щаются в обычные. Матрица направляющих косинусов С? и кватернион Л? второго конструктивного перемещения г-го звена относительно (г — 1)-го в соответствии с рис. 3 имеют вид
с? — c2 —
л? = л? =
а о о
о cos ai sin а
^о — sin ai cos а,
2 J ' VT
cos( f)+sin( f)11 •
Матрица дуальных направляющих косинусов и бикватернион конечного перемещения г-го звена манипулятора относительно (г — 1)-го находятся с использованием формул сложения конечных перемещений [2,3]:
Рис. 3. Схема второго перемещения г-го звена относительно (г — 1)-го
_ ^2^1 _
i — Ci Ci —
( cos((©i + Vi) + sdi) sin((©i + Vi) + sdi) 0
— cos ai sin((Qi + ) + sdi) cos ai cos((©i + v) + sdi) sin ai y sin ai sin((Qi + ^i) + sdi) — sin ai cos((©i + v) + sdi) cos ai
(3)
Л1 — Л1 о Л? — cos [ — ) cos
Qi + ■
2
+4)+sin( ?) cos(
'Qi + № di — + s 2 111 +
. . /ал . (Qi + Vi , di\. /ал .
+ sin ( — ) sin ( ----+ s— ) 12 + co^ — j sin
Qi + Vi . d.
+ s f) 13 *
(4)
2 J \ 2 2 у V 2 7 \ 2
При подстановке в уравнения (3), (4) геометрических параметров манипулятора, приведенных в таблице, получаем следующие матрицы дуальных направляющих косинусов и бикватернионы относительных конечных перемещений звеньев манипулятора:
^sin + sd1 cos — cos + sd1 sin 0^ C1 — 0 0 —1 , (5)
ycos — sd1 sin sin + sd1 cos 0 J
л 1 ( i d1 \ 1 ( d^ \ . 1 ( d1 \ 1 ( dx . .
Л1 — ^ P1 + s—01 — ñ P1 + s^H 11 + о 01 — ^pn 12 + - —?! + ^рн 13,
2
2
2
2
2
2
2
2
P1 — cos — +sin—,
.
q1 — cos--sin —,
^ 2 2 '
C? —
( sin v2 + sd2 cos v2 — cos v2 + sd2 sin v2 0^ 0 0 1 y— cos v2 + sd2 sin v2 — sin v2 — sd2 cos v2 0y
(6) (7)
1
2
d2
2
1
Л2 — - P2 + s —02 + о P2 + s —02 11 + - —02 + s—P? 1? + о —02 + s—P2 1з
2
d2
2
1
2
V2 . . V2
P2 — cos — +sin—,
V2 . V2 02 — cosy — sin—,
sd3 1 0
C3 —
sd3 0 01
Л
— -U1 + s^ + - (—1 + sd3
V2
C4 —
<h
2
cos V4 0
x/2 V
sin 0
13,
0
y— sin v4 cos v4 0
a 1 V4 1 V4 . 1 . V4. . 1 . V4 . Л4 — —= cos —---= cos —11--= sin —12 +--— sin —13,
4 V2 2^2 2 1 V2 2 2 V2 2 3'
C5 —
(cos V5 0
y sin V5
sin v5 0^ 01 — cos v5 0y
(8) (9) (10) (11) (12) (13)
1
0
А 1 У* . 1 Рб. . 1 . Рб. . 1 . ^5. /Мч
Лб = 72cos Т + cos Т11 + sl" Т12 + 71sl" Т13' (14)
( cos ре — sd6 sin ре sin ре + sd6 cos
С« =
(15)
— sin — sd6 cos ре cos ре — sd6 sin 0
v 0 0 V
А 1 ( ^ . ^ • рЛ , 1 Л ^ . ^ <го . . Лб = cos — + s — sin — +—— sin — + s — cos — 13. (1b)
л/2\ 2 2 2 У ^2 V 2 2 2
Матрица дуальных направляющих косинусов С и бикватернион конечного перемещения Л выходного звена манипулятора относительно неподвижного основания находятся через приведенные матрицы и бикватернионы по формулам (1), (2):
С = С6С5С4С3С2С1, (17)
Л = Л1 о Л2 о Л3 о Л4 о Л5 о Л6. (18)
Подстановка выражений (3)-(16) в (17), (18) дает выражение матрицы дуальных направляющих косинусов и бикватерниона конечного перемещения выходного звена манипулятора относительно неподвижного основания через обобщенные координаты , , «3) в явном виде. Формулы (17), (18) и (3)—(16) являются алгоритмами решения прямой задачи кинематики стэнфордкого манипулятора в дуальных матрицах направляющих косинусов и в бикватернионах конечных перемещений.
3. КИНЕМАТИЧЕСКИЕ УРАВНЕНИЯ ДВИЖЕНИЯ
Кинематические уравнения движения манипулятора получаются из векторных выражений для линейной и угловой скоростей движения выходного звена путем проектирования их на оси системы координат Х6У6^6 , связанной с выходным звеном манипулятора.
В соответствии с кинематической схемой (см. рис. 1) вектор ш угловой скорости выходного звена манипулятора будет иметь вид
Ш = ко + 02к1 + ^4 *3 + ^5 к4 + ^6к5, (19)
Здесь кг — орт оси гг системы координат, связанной с г-м звеном манипулятора, ^ — обобщенные координаты манипулятора, верхняя точка обозначает производную по времени.
Радиус-вектор г начала 06 системы координат, связанной с выходным звеном, в соответствии с кинематической схемой (см. рис. 1) определяется соотношением
г = ОО1 + О102 + 0203 + 0304 + 0405 + 05 06, (20)
где 0 — начало системы координат, связанной с основанием манипулятора, 01 — начало г-й системы координат.
Дифференцирование уравнения (20) по времени и подстановка в него заданных геометрических параметров манипулятора дает выражение для вектора линейной скорости начала системы координат Х6У6связанной с выходным звеном, относительно неподвижного основания:
^2 ^3 ^6
V = —:--1--:--1--:--1--— ; di = аг кг.
Ж
Векторы d1, d2, d6 постоянны по модулю, поэтому использование формулы Эйлера для постоянного по модулю вектора и дифференцирование переменного вектора d3 дает следующее выражение:
V = ш1 X dl + (ш1 + ш2) х d2 + «3к2 + (ш1 + ш2) х («3к2) + (ш1 + ш2 + ш4 + ш5 + ш6) X d6, (21)
где х — символ векторного произведения, шг — относительные угловые скорости звеньев, которые выражаются через обобщенные координаты и орты осей гг следующим образом:
Шг = ^гкг-1 (г = 1, 2, 4, 5, 6).
Для получения кинематических уравнений необходимо спроектировать векторные выражения (19) и (21) на оси системы координат Х6У6связанной с выходным звеном манипулятора. Необходимые для проектирования вещественные матрицы направляющих косинусов обозначим следующим образом:
С = С6С5 С4С3 С2С1, С61 = Сб С5С4 С3С2, С62 = С6 С5 С4С3, Сб3 = Сб С5С4, Сб4 = Сб С5, Сб5 = Се-
Здесь сг — вещественная матрица направляющих косинусов поворота ¿-го звена манипулятора относительно (г — 1)-го.
Тогда переход от одной системы координат к другой, учитывающий лишь повороты систем координат, осуществляется в соответствии со схемой поворотов:
Найдем проекции единичных векторов кг на оси системы координат Х6У6^6, связанной с выходным звеном. Обозначим через сг^ и с6к (г, ^ = 1, 2,3; к = 1, 2,3,4, 5) элементы матриц с и с6к, а через 16, .Ь, к6 — орты системы координат Х6У6^6. Тогда будем иметь:
ко = с1316 + с2. + С33к6, кг = с6316 + с23.6 + с33к6 (г = 1, 2,3,4, 5). (22)
Запишем векторное уравнение (19) в матричном виде, учитывая соотношения (22) и обозначая через , , проекции вектора ш на оси системы координат Х6. Получим:
/шЛ ( 01 с13 + 02 c6f + 04 С(33 + 05 Cj34 + 06 C^f^
(23)
Ш1
Ш2
w
01 с23 + 02 c61 + 04 C23 + 05 с23 + 06 C2f C33 + 02 c6i + 04 C33 + 05 C33 + 06 C3f )
Найдем проекции векторов d^ на оси системы координат X6Y6Z6, связанной с выходным звеном. Имеем:
d1 = d1c13 i6 + d1C23j6 + d1 с33 k6, di = diC13-1i6 + d^-j + d c33-1k6 (i = 2,3,6). (24) Запишем в координатной форме векторные произведения в уравнении (21): w1 х d1 = 0, так как векторы коллинеарны,
X d2 = i6(d2^1(c23C33 - C33C61 )) - j6(d2^1(c13C33 - C33cJ3)) + k6(d201(c13с23 - C23с63)),
w2 х d2 = 0, так как векторы коллинеарны,
d3w1 X k2 = i6(d301 (с23C33 - с33с62)) - j6(d301 (с13с33 - с33с12)) + k6(d301 (с13с63 - с23с62)),
d3^2 X k2 = i6 (d302 (с2?с33 - с33с23)) - j6 (¿302 (с63cjg - C33C13)) + k6 (¿302 (с63c62 - с63с63)),
wi X d6 = i6(d601(C23C3f - с33с65)) - j6(d601(C13C3f - C33C13)) + k6(d601(C13C23 - с23с65)), Ш2 X d6 = i6(d602(C63C33 - C33с63)) - j6(d602(C13C33 - C31 C13)) + k6(402(c63C65 - с63C13)), w3 x d6 = 0, так как w3 = 0,
w4 x d6 = i6(d604(c63c33 - с33c6f)) - j6(d604(c33c33 - с33с13)) + k6(d604(c63с65 - с63с13)), w5 x d6 = i6(d60f(c63c33 - с33с63)) - j6(d605(сi4с33 - с34с13)) + k6(d60f(c63с63 - с63с13)), w6 x d6 = 0, так как векторы коллинеарны.
Проектирование векторного уравнения (21) на оси системы координат X6Y6Z6 с учетом приведенных выше соотношений для векторных произведений дает 3 скалярных уравнения для компонент v вектора v линейной скорости выходного звена манипулятора относительно основания в системе координат XoY0Z0, имеющих вид
V1 = d201(c23c33 - с33с63)) + ¿3с63+
+¿3 01 (с23 с63 — с33с23) + 4<Ыс63 е32 — с33 с23) + ¿6 0 1 (с23 с! — с33с23)+ +¿6 002 (с63 с65 — с63с23) + ¿б004(с63с63 — с33 с65) + ¿6 05 (с64 с65 — с^с23),
^2 = —(¿201(с13с63 — с33 с61)) + ¿3 с63 +
+¿3 1 (с13 с63 — с33с13) + ¿300 2(с63 с62 — с33 с13) + ¿6 001 (с13 с63 — с33с13)+ +¿6 002 (с61 с65 — с63с63) + ¿600 4(с63с63 — с63 с63) + ¿6 005 (с6| с33 — с33с63)),
(25)
^3 = ¿2001(с13с23 — с23 с61)) + ¿3 с63+ +¿3 001 (с13 с23 — с23с13) + ¿3002(с63 с62 — с33 с13) + ¿6 001 (с13 с! — с23с13)+
+¿6 002 (с63 с65 — с23с15) + ¿604(с63с63 — с23 с65) + ¿6 005 ^4 с65 — с24с13).
Объединяя соотношения (23) и (25), получим выражения для проекций угловой и линейной скоростей выходного звена манипулятора на связанные с ним координатные оси:
Ы
(
= 01
13 23
33
\
¿2 (с23 с33 — с33с23)) + ¿3 (с23 с62 — с33 с22) + ¿6(с23с63 — с33с25)
л (с13с33 с33с13)) л (с13с33 с33с13) л (с13с33 с33с13) ¿2(с с61 с с61)) ¿3(с с62 с с62) ¿6(с с65 с с65)
V ¿2(с13с23 — с23с13)) + ¿3(с13с23 — с23с12) + ¿6(с13с23 — с23с25) )
+
(
+ 00 2
с13 с61 с23 с61 с33 с61
\
23 33 33 23 23 33 33 23 ¿3(с61с62 с61с62) + ¿6(с61с65 с61с65) -¿о (с13с33 _ с33с13) _ л (с13с33 _ с33с13)
¿3(с61с62 с61с62) ¿6(с61с65 с61с65) ¿ (с13с23 с23с13) + ¿ (с13с23 с23с13)
\ ¿3(с61с62 с61с62) + ¿6(с61с65 с61с65)/
+ ¿03
+ 00 4
13 63 23 63 33 63
\
л (с23с33 _ с33с23)
¿6(с63с65 с63с65)
¿6 (
13 33 33 13 с63с65 с63с65)
(
+ 00 5
V ¿6(сб3с65 с23с65) )
с13 с64 с23 с64 с33 с64
л (с23с33 _ с33с23)
¿6(с64с65 с64с65)
— ¿6(
13 33 33 13 с64с65 с64с65)
¿ (с13с23 с23с13) \ ¿6(с64с65 с64с65) /
/0\ 0
0
с13 с62 с23 с62
с3632
+
+ 00 6
с1635
с23 с65 с33 с65
0
0 0
(26)
Ведем обозначение
А = ||а,
(27)
где
а11 = с
13
П21 = с
23
13
п = с13 п12 = с61,
п = с13
п14 = с63,
п = с13
п15 = с64,
п = с23,
п22 = с61, п = с33,
п32 = с61,
п = с23,
п24 = с63, п = с33,
п34 = с63,
п13 = 0, п23 = 0,
п31 = с , п32 = с61, п33 = 0,
п = л /с23с33 с33 с23 \ + л /с23 с33 с33 с23\ + л /с23с33 с33с23\ п41 = ¿2 с с61 с с61 + ¿3 с с62 с с62 + ¿6 с с65 с с65 ,
п = л /с23 с33 с33 с23 \ + л /с23 с33 с33с23\ п = с13
п42 = ¿3 (с61 с62 — с61 с62^ + ¿6 (С61 с65 — с61с6^ 5 п43 = с62>
п = л /с23с33 _ с33с23\ п = л /с23с33 _ с33с23\ п = 0.
п44 = ¿6 ус63с65 с63с65/ , п45 = ¿6 ^с64с65 с64с65/ , п46 = 0.
„ = л /с13с33 с33с13\ л /с13с33 с33с13\ л /с13с33 с33с13\
п51 = ¿2 с с61 с с61 ¿3 с с62 с с62 ¿6 с с65 с с65 ,
п = с13.
п16 = с65.
п = с23 п = с23. п25 = с64, п26 = с65.
п = с33 п = с33. п35 = с64, п36 = с65.
Пго = _¿0 Гс13с33 _ с33с13А Л /с13с33 _ с33с13\ п52 = ¿3 ^с61 с62 с61 с62^ ¿6 ^с61 с65 с61с65^ ,
п = с23 , п53 = с62,
п54 = — ¿6 (с63с63 — с1с65) 5 п55 = — ¿6 (с64с65 — с24с65) 5 п56 = 0;
п = л /с13с23 с23с13\ + л /13с23 с23с13\ + л /с13с23 с2313\ п61 = ¿2 с с61 с с61 + ¿3 с с62 с с62 + ¿6 с с65 с с65 ,
13 23 23 13 13 23 23 13
п62 = ¿3 (с61 с62 — с61 с62^ + ¿6 (с61 с65 — с61с6^ >
33
п63 = с62,
аб4 = ¿6 (с13 сб3 — с23сб5), аб5 = ¿6 (44 с65— с23 с65) , «66 = о.
Разрешая уравнение (26) относительно обобщенных скоростей с учетом обозначения (27), получим матричное уравнение:
(ф 1 <¿>2 (¿3 ф4 ф5 (¿6) = А-1 ^2 ^3 VI г>2 у^ , (28)
где Т — символ транспонирования.
Полученные уравнения (28), (27) — кинематические уравнения движения стэнфордского робота-манипулятора. Они представляют собой систему 6 нелинейных нестационарных обыкновенных дифференциальных уравнений относительно переменных ф1, ф2, (3, ф4, ф5, ф6, являющихся обобщенными координатами манипулятора. Эти уравнения и полученные в п. 2 бикватернионные соотношения будут использованы для решения обратной задачи кинематики с использованием бикватернионной теории кинематического управления [4].
Работа выполнена при финансовой поддержке РФФИ (проект 12-01-00165).
Библиографический список
1. Фу К., Гонсалес P., Ли К. Робототехника. М. : Мир, 1989. 621 с.
2. Челноков Ю. Н. Кватернионные и бикватернионные модели и методы механики твердого тела и их приложения. Геометрия движения. Саратов : Изд-во Сарат. ун-та, 2006. 236 с.
3. Челноков Ю. Н. Кватернионные и бикватернионные модели и методы механики твердого тела и их прило-
жения. Геометрия и кинематика движения. М. : Физ-матлит, 2006. 512 с.
4. Челноков Ю. Н. Бикватернионное решение кинематической задачи управления движением твердого тела и его приложение к решению обратных задач кинематики роботов-манипуляторов // Изв. РАН. Механика твердого тела. 2013. № 1. С. 38-58.
Dual Matrix and Biquaternion Methods of Solving Direct and Inverse Kinematics Problems of Manipulators, for Example Stanford Robot Arm. I
E. I. Lomovtseva, Yu. N. Chelnokov
Saratov State University, Russia, 410012, Saratov, Astrahanskaya st., 83, [email protected], [email protected]
The methology of solving the direct kinematics problem of manipulators by using screw mechanics methods (dual direction cosine matrices, Clifford biquaternions) is shown on the example of Stanford robot arm. Kinematic equations of motion of the manipulator are found. These equations will be used for solving the inverce kinematics problem with the help of biquaternion theory of kinematic control.
Keywords: robot-manipulator, direct kinematics problem, dual direction cosine matrix, biquaternion, quaternion, kinematic equations.
References
1. Fu K. S., Gonzalez R. C., Lee C. S. G. Robotics : Control, Sensing, Vision and Intelligence. McGraw-Hill, Inc, 1987, 580 p.
2. Chelnokov Yu. N. Kvaternionnye i bikvaternionnye modeli i metody mehaniki tverdogo tela i ih prilozhenija. Geometrija dvizhenija [Quaternion and Biquaternion Models and Methods of Mechanics of a Rigid Body and their Applications. Geometry of Motion]. Saratov, Izd-vo Saratov. Univ., 2006, 236 p. (in Russian).
3. Chelnokov Yu. N. Kvaternionnye i bikvaternionnye modeli i metody mehaniki tverdogo tela i ih prilozhenija.
Geometrija i kinematika dvizhenija [Quaternion and Biquaternion Models and Methods of Mechanics of a Rigid Body and their Applications. Geometry and Kinematics of Motion] Moscow, Fizmatlit, 2006, 512 p. (in Russian).
4. Chelnokov Yu. N. Biquaternion Solution of the Kinematic Control Problem for the Motion of a Rigid Body and Its Application to the Solution of Inverse Problems of Robot-Manipulator Kinematics. Mechanics of Solids [Izv. RAN. Mehanika tverdogo tela], 2013, vol. 48, no. 1. pp. 31-46.
