CC BY
145
УДК 621.38(62-52)
В.П. Глазков, С.К. Дауров
ОСОБЕННОСТИ РЕШЕНИЯ НЕКОТОРЫХ ЗАДАЧ КИНЕМАТИКИ В АППАРАТЕ КВАТЕРНИОНОВ
Рассматриваются особенности решения основных задач кинематики манипуляторов при использовании нетрадиционных кинематических параметров - кватернионов и их дуальных аналогов (бикватернионов). Предложен метод ускоренного умножения кватернионов, позволяющий снизить вычислительные затраты в 5-6 раз. Проведён сравнительный анализ эффективности предложенного метода решения обратной задачи кинематики и метода, использующего матрицы однородных координат.
V.P. Glazkov, S.K. Daurov FEATURES OF THE DECISION OF SOME PROBLEMS OF KINEMATICS
Features of the decision of the primary goals of kinematics of manipulators are considered at use of nonconventional kinematic parameters - quaternion and their dual analogues (biquaternion). The method of the accelerated multiplication кватернионов is assumed, allowing to lower computing expenses in 5-6 times. The comparative analysis of efficiency of the offered method of the decision of a return problem of kinematics is lead to comparison with a method, using matrixes of homogeneous coordinates.
Прямая задача
Приведем некоторые сведения решения прямой задачи манипулятора с использованием аппарата бикватернионов. Кинематическое звено многозвенного механизма соединяет два смежных цилиндрических шарнира, с каждым из которых связана своя система координат. При этом кинематическое звено характеризуется дуальным углом Вi конструктивных параметров и дуальным углом Qi обобщенной координаты.
Ві — ві + є Pi , Qi — qi + є qi ,
(1)
где Рь Рг° и д? - некоторые постоянные параметры г-го звена; дг - обобщенная координата г-го звена; є - символ Клиффорда.
Бикватернионы, соответствующие дуальным углам из выражения (1), будут иметь
вид:
Лів = ЛіоВ + іі Ліів , Лв = Люв + Лізв , а соответствующие бикватернионные матрицы:
mB =
ЛВо -Лі O O O O -Ла i3
лВі ЛВо O O , MQ = O Ла iO -Ла i3 о
O о Лв iO -ЛВі о Ла i3 Ла iO о
O O Лв І1 ЛВо Ла i3 O O Ла iO
Матрицы М8 и Мр могут быть представлены в ином виде:
М8 = т8 [Е + е т8 (Гу) / 2] , Мр = тр [Е + е тр (Гу) / 2] где т/8 и тр - матрицы вещественных членов бикватернионов Л8 и Лр:
тв =
Сві — і 0 0 С,і 0 0 —
5в. Сві 0 0 , т = 0 С,. — 5,і 0
0 0 Св — і 0 С,і 0
0 0 5в. Св 5,.- 0 0 С,
в которых % = 8Ш (в/), Ср/ = 008 (в/), = 8Ш (#/), Сд/ = 008 (#/).
В целом, /-е кинематическое звено описывается бикватернионной матрицей вида:
М/ = М 8 Мр = т1 [ Е + е т/ (Гу) / 2 ] ,
где тг = т8 ■ тр характеризует ориентацию /-го кинематического звена относительно ( /—1)-й системы координат, а
т/ (гу) = д/° Мз + тр (Р° М1) трТ определяет радиус-вектор / - го кинематического звена. Матрицы М1 и М3:
И, =
предназначены для указания осей координат, по которым осуществляется смещение на вели-
РО О
і и .
Введем традиционные для многозвенных механизмов обозначения:
М.==>1-1 Мі , ті==>1-1 ті , ті (гу)==>1-1 гі и определим результирующую бикватернион-ную матрицу:
0 Мк = 0 И, ■ 1 М2 ■ ■ ■ 1-1 Мі ■ ■ ■ к-1 Мк , которая распадается на два итерационных алгоритма:
0 —і 0 0 0 0 0 —і
і 0 0 0 = СП 0 0 —і 0
0 0 0 —і 0 і 0 0
0 0 і 0 і 0 0 0
і і і+1
тк = ті+і ■ тк ,
і і , і і+1 А \ Т
Гк = гі+і + ті+і ■ Гк ■ ( ті+і) , к = 00П8І , і = к-і, к-2, ..., і, 0 .
(2)
(3)
Ускоренное умножение кватернионов
Анализ выражений (2), (3) для решения прямой задачи показывает, что основной операцией является умножение кватернионов и, следовательно, разработка метода ускоренного перемножения кватернионов позволила бы существенно увеличить быстродействие решения многих задач кинематики.
Кватернион представляет собой гиперкомплексное число вида:
Л = X о + /1^1 + /'2^ 2 + /3Х з , (4)
где X0, Х1, X2, Х3 - действительные числа; /1, /2, /3 - орты гиперкомплексного пространства.
Произведение кватернионов можно представить в виде произведения кватернионных матриц Мх и Мц [1]:
м х о м ц =
Х0 — Хі — Х 2 3 Х — Ц0 — Ці — Ц 2 — Ц3
Хі Х0 — Х3 Х2 Ці Ц0 — Ц3 Ц 2
X 2 Х3 Х0 — Хі Ц2 Ц3 Ц0 — Ці
Х3 — Х 2 Хі Х0 Ц3 — Ц2 Ці Ц0
(5)
Операционный состав данного произведения - 64 операции умножения и 48 операций сложения, что соответствует произведению любых двух матриц размером (4x4). Однако анализ кватернионной матрицы показывает: матрица составлена из компонентов кватерниона с различными знаками, имеет определенную (кососимметричную) структуру, а элементы первого столбца точно соответствуют компонентам кватерниона. Всё это позволяет заменить операцию перемножения двух матриц на операцию умножения матрицы (4x4) на вектор (4x1), требующую 16 операций умножения и 12 - сложений. В результате получим только первый столбец результирующей кватернионной матрицы Мг. Используя свойства кососимметричных матриц, остальные элементы матрицы Мг восстанавливаются. Предложенный (основной) метод умножения кватернионных матриц увеличивает быстродействие выполнения этой операции в 4 раза.
При выполнении основного метода умножения кватернионных матриц можно предложить еще один (дополнительный) метод снижения операционных затрат [2].
Детально распишем произведение матрицы М% на первый столбец матрицы Мц.
(6)
Анализируя правую часть выражения (6), можно заметить, что все частичные произведения Хц, образуют полный набор частичных произведений при умножении двух четы-рехчленов (см. рис. 1, а). С другой стороны, левая часть выражения (6) представляет собой компоненты результирующего кватерниона Я:
Я =Л о М
Х 0 -Х1 -Х 2 -Х3 М0 Х0Ц0 - Х1Ц1 - Х2Ц2 - Х3М3
Х1 Х 0 -Х3 Х 2 М1 Х1Ц0 + Х 0Ц1 - Х3Ц 2 + Х 2М3
X 2 Хз Х0 -Х1 М 2 Х 2Ц0 + Х3М1 + Х0Ц2 - Х1М3
Хз -Х 2 Х1 Х0 М 3 Х3М - Х 2Ц1 + Х1Ц2 + Х 0Ц3
00 - Г1 - г2 - г0
Г0 + Г1 - Г12 + Г13 Г1
Г? + г1 + Г? - г3 '2 Г2
Г? - Г1 + Г32 + Г 3 г3 г3
Мо Мі М2 Мз
Хо ХоМо ХоМі Х0М2 Х0М3
Хі Х1Ц0 ХіМі Х1М2 Хі Мз
Х2 Х2М0 Х2М1 Х2М2 Х2Мз
Хз ХзМо ХзМі ХзМ2 ХзМз
Мо Мі М2 Мз
Хо Гоо Гіі Г 2 Гз3
Хі Гіо -Гоі Гз2 -Г2з
Х2 '2° -Гзі -Го2 Гіз
Хз Гзо Г2і -Гі2 -Гоз
а) б)
Рис. 1. Формальное представление: частичных произведений многочленов (а), составляющих компонентов Я (б)
Каждая компонента кватерниона Я определяется как алгебраическая сумма соответствующих частичных произведений, например:
Г0 = Х0М0 -Х1М1 -Х2М2 -Х3М3 = Г00 - Г0 - Г0 - Г03 •
Анализ таблицы формального распределения составляющих компонентов кватерниона Я с учетом знаков (рис. 1, б) позволяет заметить, что, если сложить две соседние компоненты в строках Х0+Х1 и в столбцах Цо+М-ь а затем выполнить умножение этих сумм, то получим:
(Х 0 + Х1)(М0 +М1) = Г0 + Г1 + Г1° + Г1 , откуда при известных значениях г00 и г1 определим:
г10 + Г1 = (Х0 + Х1)(М0 +М1) -г00 + Г1 •
Для определения алгебраической суммы оставшихся двух составляющих (см. рис. 1, б) - г12 + г13 необходимо использовать суммы Х2, Х3 и Ц2, Ц3. Так как составляющая г12 должна быть с отрицательным знаком, то это можно обеспечить, взяв с отрицательным знаком компоненту Х3, либо компоненту Ц2:
(Х2 -Х3) (Ц2 +Ц3) = г0 - г0 - г12 + г13 ,
откуда определим:
- Г12 + Г13 _ (Х2 -Х3) (М2 +М3) - Г0 + Г0 •
'0 1 '0
Окончательное значение г1 будет равно:
г1 = г10 + г1 - г12 + г13 = (Х0 +Х1) (Ц0 +Ц1) + (Х 2 -Х3) (Ц2 +Ц3) - г00 - г0 - г02 + г0 .
Для вычисления гх потребовались 2 операции умножения и 9 операций сложения вместо 4 операций умножения и 3 операций сложения.
Из порядка определения компоненты гх (рис. 1, б) можно сделать вывод о том, что составляющие определяемой компоненты должны быть в непосредственной близости от диа-
гонали, на которой находятся известные составляющие компоненты г0. Аналогично определим остальные компоненты результирующего кватерниона [2].
На основании полученных выражений сформируем алгоритм умножения кватернионов.
1. Определим составляющие г0:
г0 = Х0Ц0 , г0 = Х1Ц1 , г0 = Х2Ц2 , г0 = Х3Ц3 .
2. Значения гх, г2 и г3 определяются соответственно:
г1 = (Х0 +Х1) (Ц0 +Ц1) + (Х2 -Х3) (Ц2 +Ц3) - г00 - г0 - г0 + г0 .
г2 = (Х0 + Х 2 ) (Ц0 + Ц2) + (Х3 -Х1) (Ц3 + Ц1) - г00 + г01 - г0 - г03 . г3 = (Х1 -Х 2 ) (Ц1 +Ц1) + (Х3 +Х0) (Ц3 +Ц0) - г00 - г01 + г0 - г0 .
3. Суммируя составляющие г0 с определенными знаками, находим:
0 1 2 3
у — у ____ у1 _
г0 г0 г0 г0 г0 .
Операционный анализ показывает, что вспомогательный алгоритм для своей реализации требует 10 операций умножения и 30 операций сложения, то есть быстродействие возрастает более чем на треть.
Окончательно, используя основной и вспомогательный методы сокращения вычислительных затрат, быстродействие выполнения операции умножения кватернионов возрастает в 5-6 раз. Полученный результат нашел свое подтверждение при выполнении вычислительного эксперимента.
Формирование исходных данных для решения обратной задачи
Исходными данными для решения обратной задачи манипулятора являются пространственные координаты схвата и его ориентация. Можно утверждать, что задание координат схвата практически не зависит от вида математического аппарата, в котором решается обратная задача. Этого нельзя сказать в отношении ориентации. Например, в аппарате однородных координат ориентация схвата задается углами Эйлера ф, 0, у [1], с помощью которых определяется матрица ориентации размером [3x3]. Зная координаты схвата и матрицу его ориентации, легко сформировать однородную матрицу манипулятора, которая и является основой для решения обратной задачи.
В аппарате кватернионов ориентация схвата задается в виде кватерниона, представляющего собой совокупность трех направляющих косинусов и угла поворота. Направляющие косинусы определяют пространственное положение оси, вокруг которой осуществляется поворот, в результате чего реализуется преобразование из системы координат схвата в базовую систему координат манипулятора.
Необходимо определить порядок формирования кватерниона ориентации. Известно, что между матрицей ориентации и кватернионом существует однозначное соответствие, поэтому можно предложить процедуру, в которой вначале определяется матрица ориентации, а затем по элементам этой матрицы вычисляются компоненты кватерниона. Практическое применение данной процедуры требует больших вычислительных затрат, во-первых, при задании углов Эйлера и определении матрицы ориентации и, во-вторых, при использовании довольно громоздких формул пересчета элементов матрицы в компоненты кватерниона. Приведем здесь некоторые положения разработанного метода непосредственного определения кватерниона ориентации [3].
Связь между вектором в системе координат схвата У6 и вектором У0 в базовой системе координат определяется выражением:
:Л-У6 -ЛТ
(7)
где У6, Уо - известные координаты одного и того же вектора в системах координат схвата и стойки манипулятора; Л - кватернион поворота в полной и сокращенной форме:
Хп
Л:
Хо -Х1 -Х2 -X
Хі Хо -Х3 Х2
Х2 Х3 Х о -X
Х3 -Х 2 Х 2 Хо
или Л
Х 2 Ха
(8)
Формально, используя выражение (7), можно было бы определить компоненты кватерниона Л, однако система алгебраических уравнений, полученных из выражения (7), оказывается неполной, то есть недостаточной для решения задачи. Полную систему алгебраических уравнений получим, если используем выражение (7) для тройки ортогональных векторов, заданных в системе координат схвата (Х,У,Т)6:
У* = Л • У/-ЛТ
(9)
где У6Х, У6у, У6г - тройка ортогональных векторов, лежащих на соответствующих осях ОХ6, ОУ6, О76; У0Х, У/, У0г - положение этих же векторов в базовой системе координат.
Так как мы используем системы координат только одного типа (правые), то система уравнений (9) является избыточной и нам достаточно использовать, например, первое и третье уравнения. Представим их в следующем виде:
Уо* •Л
Л-У/
(1о)
где У6Х и У6г соответственно равны:
о о
о , Vх = *6
о 6 о
^6 о
V*
а значения координат этих же векторов У/, Уог в базовой СК определяются либо аналитически либо из сборочного чертежа объекта сборки. Для корректного решения задачи необходимо проверить ортогональность векторов У0Х, У0г, для чего составим скалярное произведение векторов и проверим его на равенство нулю.
Подставляя в приведенные выше выражения (10) известные значения векторов, найдем правые и левые части. Для выражения (10) эти части будут соответственно равны:
V 'А:
0 — Х* Х0 — У 0* — *0* К0
Х0 0 — *0* у0 К1
У0* *0 0 — Х* Х0 К2
*0 — У 0* Х* Х0 0 К3
Х0 К1 - У О К 2 - *0 ^3°
Х0 К 0 - *0* К 2 + У О V
У0К0 + *0К1 - Х0 К3°
*0К0 - У0К1 + Х0К2°
(11)
А-V
>> о о 0 о —К *6°
V 0 о К 2 *6°
К 2° 0° — К 2 *6°
>> о о *6 К 0 *6°
(12)
Из равенства приведенных выражений сформируем систему алгебраических уравнений:
- Х0К1 - У0К2 - *0К3 = - К3*6
у 0 2 ^0/''3 _ 'ч'3<-6
Х0К 0 — *0К2 + У0К3 = К 2 *6 У(з К0 + *0К1 — Х0К3 = — К1*6
*0К0 — У0К + Х0К2 = К0 *6 которые могут быть преобразованы к следующему виду:
'Х0 К1 — У0К2 — К3 (*0 — *6 ) = 0 Х0К0 + У0К3 — К2 (*0* + *6 ) = 0 у0К0 — Х0К3 + К1(оо + *6 ) = 0
У0К1 + Х0К2 + К0 (о0 — *6 ) = 0 используя первое и четвертое уравнения системы, выразим компоненты К3 и К0:
К
К1Х0 +К2 У0 К К1У0 — К2 Х --------;---------, К0 ------------;---------- •
Х0
*0 — *6
(13)
° 0 К 0° — К хХ А<1Х0 — К 2 у0 — К3 *0 ° К0° 0° — К1Х6°
О X о V К хХ /ъ0 Х0 — К 2 *0 + К3 У0Х ° , л-У6х — V Х6° К) Х6°
У0Х ° К 2° — К хХ /ъ3 Х0 + К 0 у0Х + К1*0 ° К 2° 0° К Х6°
* О X о К 3° К хХ ^2 Х0 1 1 У О Я + о * о х О К3° 0° — К 2 Х6°
Возвращаясь ко второму уравнению из выражения (10), повторим для него ту же процедуру. Правая и левая части выражения (10) будут соответственно равны:
Vх-Л —
Из равенства последних выражений сформируем систему алгебраических уравнений:
'К1(х0 — Х6 ) —К2 У0 — К3 *0 = 0 К0(х0 — Х6 )+К3У0 — К2*0 = 0 >
■ К3 (х0Х + Х6 ) + К0 У0Х + К1*0Х =0 > ’
К2 (Х0 + Х6 ) — К1У0 + К0 *0 = 0 из которых, используя первое и четвертое уравнения системы, выразим компоненты К и Ко:
) —К2 У0Х
К —
К у0 — К2(Х0+Х6)
(14)
Приравняем правые части значений К3 из выражений (13) и (14):
_Х1Х0+Х2ус[ = _МХ0 + Х6 )~Х 2 У0 *0 _ *6 *0
и после ряда преобразований найдем отношение:
К_
К
У0*0
(*0— *6 )У0Х
(*0 — *6 )(х0Х — Х6 )— Х0*0
с.
0
6
х
х
*
I
0
0
Используя одно из свойств кватерниона, можно задать, например: Кх=С, а К2=1, тогда из выражений (13) определяются остальные компоненты кватерниона:
с%1 + уо
А,п
В заключение выполним нормирование кватерниона, для чего разделим все его компоненты на величину ^К20 + А2! + К22 + К23 .
Исходными данными для решения обратной задачи являются:
- матрица радиуса-вектора схвата в полной и сокращенной форме:
СХ
0 - ХСХ Х - - 7 7СХ 0
Хсх 0 СХ N - Х , Усх - Хсх
Усх 7 7СХ 0 СХ Х - Х
Й N - у СХ Хсх 0 7 7СХ
X 0 -Х1 -Х 2 3 << - Х0
XI Х 0 -Х 3 Х 2 XI
X 2 X 3 Х 0 Хі , М X X 2
X 3 -Х 2 Хі Х 0 Xз
- кватернионная матрица ориентации схвата в полной и сокращенной форме:
Для удобства изложения и решения обратной задачи необходимо иметь в виду таблицу конструктивных параметров манипулятора (см. таблицу).
Особенностью данного манипулятора является совпадение начал трех последних систем координат СК4, СК5 и СК6. С другой стороны, заданные координаты схвата в базовой системе СКо являются координатами вектора с компонентами (0, 0, ц6°), находящегося в системе координат схвата СК6. Если теперь найти координаты начала системы СК6 в базовой системе, то мы тем самым перейдем к трехзвенному манипулятору.
Сочленение і і-1 А ц0 (мм) в0 (мм) Диапазон
1 9 О о Я1 і со о о 0 0 -160° -160°
2 0 Я2 0° 149,09 431,8 -225° -45°
3 9 О о Яз Ї2 0 -20,32 -45° -225°
4 0 04 0 = СУ0 - Х0 4 - ^6 433,07 0 -110° -170°
5 0 Я5 9 О о 0 0 -100° -100°
6 0 Яб 0° 56,25 0 -266° -266°
Для реализации этого зададим в кватернионной форме вектор вида:
0
V 6
о о
-4б°
и выполним операцию кватернионного преобразования:
V6
в результате получим координаты заданного вектора У06 в базовой СК. Сложив координаты этого вектора с координатами схвата:
Усх + Уо6
У
СК
получим координаты начала системы СК6. Используя упомянутую особенность манипулятора, можно записать:
0
У6 - У5 - У4 -
Х
7
(15)
Последовательно используя выражения (2) и (3), определим радиус-вектор трехзвен ного механизма:
Г-0г1 + ті т (1г4 }(т1ет]8) ,
Г4 -1Г2 + тЄ т2В ( 2Г4 )(тЄ т2В ^ ,
Г4-2г3 + т3Є т3в (3г4 )(тЄт3в )Т ,
- ■ М3 + тЄ(в4 ■ М1 )(тЄ)Г .
(16)
(17)
(18)
(19)
Начиная с последнего выражения, будем последовательно определять значения ради-
3 2 1 0
усов-векторов г4, г4, г4 и г4 [5]. В процессе определения радиусов-векторов будут определены следующие углы:
Ґ
41 - аг^
вій (^1) '
С°в (41 )у
aгctg
4% + X Л X 42 + У42 -4°)
Яг Х4 - У4л/ Х4 + У42 -(ц2 )
(20)
где
• ( ) & У4 ± X ^ X 42 + У42 -(420 )2
8іП (41 ) - ------X 2 + %42 ----------
( ) 40X4 ± У4УІX42 + У42 -(420 )2
С°8 (41) - ---ХЇ+УҐ---------
42 - aгctg
АдАу ± Ах^АХ + Ау? - А
VАА ±
2
(21)
где
Ах - 2Кху в2,
Ау - 2Кг в2,
Ад - КХУ + КІ2 + (в2 ) - (д4 ) - (в3 ) ,
КХУ — У4Сд1 Х 4 $41 ,
43 - aгctg
Г в01
-0 I- aгctg
Я4 У
(
К7С 42 КХУ $ Я-2
V КХУ С Я-2 + К7 $ Я-2 - в2
(22)
Переходим к определению следующей тройки обобщенных координат д4, д5 и д6. Кватернионная матрица ориентации в развернутом виде:
0 т6 - 0т1-1 т2-2 т3-3 т4-4 т5-5 т6
(23)
19
4
3
г
4
При известных углах дь д2 и д3 предыдущее выражение можно записать:
0 т6 - 0т3 -3 т6
откуда
(0 т3 )Т-0
(24)
Так как левая часть выражения известна, то для удобства дальнейших преобразований введем обозначение:
М и - (0 т3 )'
тЙ
Теперь это выражение представим в виде:
-3™ _3™ 4
М и- т6 - т4 • т5 • т6 .
(25)
Представим предыдущее выражение в развернутом виде, для этого необходимо определить кватернионные матрицы 3т4, 4т5 и 5т6. Используя общую форму записи кватернионной матрицы гтг+1 и конструктивные параметры манипулятора (см. таблицу) запишем для 3т4:
3т4 - т4Є т% - т\ тв
С,
44/2
0
0
$
4 412
Р4/2
в^2
0
0
в результате при условии, что ^4= -90° и, следовательно, Ср4/2—^2/2, а 5р4/2— -^2/2, приходим к:
тл
С44і2 ^ С45 С44/2
-С С44/2 • $45 _42 -С С44/2
- ^4^2 • $45 - 2 - $44/2
^4^2 ^ С45 ^44/2
Аналогичным образом найдем:
л/2
2
4512
45І2
45І2
45І2
т6-
С
46І2
0
01
$
46І2
Определим теперь в развернутом виде правую часть выражения (25):
3 4 5
т4 • т5 • т6 -
л/2
2
С44/2 ол/2 С45І2 С46І2
- С /2 4412 С45/2 0
- $4^2 0 2 $4^2 0
$44/2 $4^2 $46/2
После перемножения матриц и некоторых преобразований получим:
Ми- т6
ССС
С44/2С45/2С46/2
$ С $
°44/2С45/2 $46/2
- ^44/2$45/2С466/2 + С44П$45/2$46/2 С44І2 $45/2С46/2 + ^ 44/2 $45/2 $46/2 $44/2С45/2С46/2 + С44/2С45/2$ 46/2
3
т6 - т6 .
О
4
т5 -
2
Данное выражение может быть представлено в виде системы уравнений, которые с использованием тригонометрических формул сложения и вычитания могут быть существенно упрощены:
М0 = Сд5/2 (^/2^6/2 - $и/2 ^д6/2 ) = С45/2С( 44+46 )/2 ,
М-1 = $45/2 ((Сд^2$д^2 - ^/2^6/2 ))= ~^Я5/2$(44-46 )/2 , (26)
М2 = $й/2 (С?4/2С?6/2 + $4412 $ 46/2 | = $«/2^-® )/2 ,
М3 = Сд5/2 ($д4/2Сд6/2 + Си/2^/2 ) - С45/2$(44+46 V2 .
Разделим четвертое уравнение системы (26) на первое и получим:
44 + 46 = 2 • arctg ГМ3'] , (27)
IМ0 )
а используя второе и третье уравнения системы (26), находим:
44 - 46 = 2 • аг^ Г- -^0 . (28)
I М2 )
При сложении выражений (27) и (28) определим угол 44:
Г м31 ґ
44 - arctg — I + аг^
IМ0 )
Мі М 2
(29)
Если из выражения (27) вычесть выражение (29), то определим угол 46:
46 - arctg
Г М31 Г
— I - агС^
V М0
Из первого и третьего уравнений системы (26) определим значения:
М 0 о _ М2
-Мі I . (30)
М2 )
с -_______________ $ __________
С45/2 = с ’ $45/2 - с ■
с(44+46 )/2 с(44-46)/2
Составим отношение $45/2 / С45/2 и определим обобщенную координату 45:
45 - 2 • аг^
Ґ и с ^
Н-2с(44+46 V 2
V М0С(44-46 V2 )
(31)
Таким образом, найдены все обобщенные координаты манипулятора, т.е. обратная задача о положениях решена.
Сравнительный анализ методов решения обратной задачи
Решение обратной задачи манипулятора как в аппарате однородных матриц, так и в аппарате кватернионов состоит из этапа формирования исходных данных (ориентация и координаты схвата) и этапа решения обратной задачи (определение обобщенных координат) манипулятора.
Проведем сравнительный анализ трудоемкости (вычислительной сложности) выполнения этих этапов в упомянутых математических аппаратах.
Задание пространственных координат схвата практически не зависит от вида математического аппарата, в котором решается обратная задача, чего нельзя сказать в отношении ориентации.
Например, в аппарате однородных матриц ориентация схвата задается углами Эйлера ф, 0, у [1], определение которых представляет собой довольно трудоемкую задачу как в реальном рабочем пространстве манипулятора, так и на графическом представлении этого пространства. Далее, по найденным углам ориентации определяется матрица ориентации размером [3x3], для чего потребуется вычислить 6 тригонометрических функций, выполнить 16 операций умножения и 7 операций сложения. Теперь, зная координаты схвата и матрицу его ориентации, можно сформировать однородную матрицу манипулятора, которая и является основой для решения обратной задачи.
В аппарате кватернионов ориентация схвата задается в виде кватерниона, представляющего собой совокупность 3 направляющих косинусов, определяющих пространственное положение оси, и угла поворота вокруг этой оси.
Определение компонентов кватерниона выполнено ранее. Теперь перейдем к сравнительному анализу результатов решения обратной задачи в рассматриваемых математических аппаратах. Необходимо иметь в виду, что обратная задача в данном случае решается для манипулятора промышленного робота РМ-01 (РиМА-260), конструктивные параметры которого приведены ранее.
Процедуры решения обратной задачи манипулятора в аппарате однородных матриц и в аппарате кватернионов [3] во многом идентичны.
Например, первые три угла (обобщенные координаты) манипулятора 41, 42, 43 в обоих случаях были определены из одной и той же системы уравнений. Естественно, что они определяются по одним и тем же формулам.
В аппарате однородных матриц для определения последних 3 углов выражение будет иметь вид:
ч =
C4CsC6 - S 4S 6 - C 4C s S 6 - S 4C 6 C4Ss d6C4Ss nx Sx ax Px
S 4C sC 6 + C 4 S 6 - S 4C s S 6 + C 4C 6 S4Ss d6S4Ss ny Sy ay Py
- S sC 6 SsS6 Cs d6Cs + d4 nz Sz az Pz
0 0 0 1 0 0 0 1
(32)
в котором Si = sin (qi), Ci = cos (qO; d4 = q40, d6 = q60.
Так как компоненты правой матрицы 3A6 нам известны, то из выражения (32) определим обобщенные координаты q4, q5, q6:
(ay ,
q4 = arctg I — I, q6 = arctg
V ax
Sz
- nz
qs = arctg
ax + ay
(C 4 + S 4) • az,
(33)
Сравнивая операционный состав выражений (29)-(31) и (33), можно сделать вывод: решение обратной задачи манипулятора в аппарате кватернионов и в аппарате однородных матриц имеет практически одинаковую вычислительную сложность.
Однако, анализируя выражения (26) и (32), можно заметить малую (26) и значительную (32) степени зависимости вычисления обобщенных координат 44 и 46 от значения 45. Например, из выражения (26) следует, что при значении 45=0, значения м1 и М2 будут равны нулю, но это не помешает нам определить значения 44 по выражению (29) и 46 по выражению (30), которые в этом случае будут равны. Аналогичная ситуация (45=0) для выражения (32) приведет к тому, что 6 компонентов (ах, ау, щ, рх, РУ) из 12 обнуляются и определение значений 44 и 46 с использованием выражений (33) становится невозможным. Выход из этого положения возможен только в использовании остальных компонентов матрицы 3А6, которая в этом случае будет иметь вид:
С4Сб - Б 45 6 - С4Б 6 - Б 4Сб 0 0 С(4 + б) - Б(4 + б) 0 0
5 4Сб + С4Бб - 5456+С4Сб 0 0 Б (4 + б) С(4 + б) 0 0
0 0 1 й 4+й б 0 0 1 й 4+й б
0 0 0 1 0 0 0 1
3 А =
Из последней матрицы можно определить:
С Щ ^
д4 + д6 = arctg
V Пх.
или д4 = дб = аг^
Г Пу ^
I/2 .
V Пх )
(34)
Таким образом, в аппарате однородных матриц перед вычислением последних трех обобщенных координат необходимо вначале проверить значения ах и на их равенство нулю. Если выполняется условие ахФягФ0, то используется выражение (33); в противном случае (ах=^г=0) значение 45=0, а углы 44 и 46 определяются из выражения (34).
Суммируя изложенное, можно сделать вывод о предпочтительном использовании решения обратной задачи в аппарате кватернионов по следующим причинам:
- вычислительная сложность формирования исходных данных обратной задачи в аппарате кватернионов в 3 раза ниже, чем в аппарате однородных матриц;
- практически отсутствует взаимная зависимость углов 44, 45 и 46 в процессе их определения, что также снижает вычислительную сложность и повышает вычислительную надежность.
ЛИТЕРАТУРА
1. Перков Н.Ф. Применение бикватернионных матриц в кинематике пространственных механизмов / Н.Ф. Перков, Ю.Н. Челноков // Машиностроение. 1981. № 4. С. 60-66.
2. Глазков В.П. Метод ускоренного умножения кватернионов / В.П. Глазков, С.К. Дауров // Математические методы в технике и технологиях: сб. трудов Междунар. науч. конф. В 5 т. Т. 1 / Новгород. гос. ун-т. Великий Новгород, 1999. С. 91-94.
3. Планирование траекторий движения манипулятора с использованием кватернионов / В.П. Глазков, С.К. Дауров, В.А. Лобанов, В.А. Подлипалин // Математические методы в технике и технологиях: сб. трудов Междунар. науч. конф. В 5 т. Т. 3 / Новгород. гос. ун-т. Великий Новгород, 1999. С. 65-68.
4. Дауров С.К. Формирование исходных данных обратной задачи кинематики манипулятора / С.К. Дауров // Математические методы в технике и технологиях: сб. трудов Меж-дунар. науч. конф. В 7 т. Т. 2 / С.-Петербург. гос. технолог. ин-т. СПб., 2000. С. 58-60.
5. Дауров С.К. Решение обратной задачи манипулятора / С.К. Дауров // Проблемы управления и связи: материалы Междунар. науч.-техн. конф. Саратов: СГТУ, 2000. С. 76-82.
6. Дауров С.К. Анализ методов решений обратной задачи манипулятора / С.К. Дауров // Математические методы в технике и технологиях: сб. трудов Междунар. науч. конф. В 6 т. Т. 2. Секции 2, 5 / Смоленск. филиал Московск. энергет. ин-та. Смоленск, 2001.
С. 142-144.
Глазков Виктор Петрович -
кандидат технических наук, доцент,
заведующий кафедрой «Системы искусственного интеллекта» Саратовского государственного технического университета
Дауров Станислав Константинович -
кандидат технических наук,
доцент кафедры «Системы искусственного интеллекта» Саратовского государственного технического университета
