CC BY
75
УДК 621.865
Л. А. Борисенко, д-р техн. наук, доц.
РАЗВИТИЕ МЕТОДОВ МОДЕЛИРОВАНИЯ КИНЕМАТИКИ МАНИПУЛЯТОРА С ИСПОЛЬЗОВАНИЕМ КВАТЕРНИОНОВ
В статье излагается методика математического моделирования кинематики многосуставного шарнирного манипулятора на базе использования аппарата кватернионов. Показано, что этот аппарат обладает целым рядом алгоритмических и вычислительных преимуществ по сравнению с традиционно используемым для тех же целей аппаратом матриц.
Рассмотрим многосуставную руку манипулятора, в которой соединение осуществляется посредством трехподвижных кинематических пар - шаровых шарниров. Такая рука обладает высокой функциональностью. Отсюда, как частные случаи, следуют схемы с одними вращательными парами или двухосными шарнирами, находящими применение на практике.
Решение прямой задачи кинематики предполагает реализацию рекуррентного алгоритма «накопления» кинематических параметров движения звеньев. Основу его составляют единообразные вычисления, основанные на уравнениях сложения скоростей и ускорений, а также на уравнениях преобразования координат [1]. Рассмотрим их применительно к технике кватернионов [2].
Для определения абсолютной угловой скорости звеньев нужно сложить относительные угловые скорости в одном и том же базисе. Пусть за основной принят неподвижный базис Х\Х2Хз, связанный со стойкой манипулятора. В кватернионной форме уравнение сложения скоростей выглядит так:
о 2 = о х + д о п2 о л-1, (1)
где П 2 - кватернион, ассоциированный с вектором относительной угловой скорости связанной системы второго звена относительно первого, взятым в связанной системе первого звена; Л1 о П2 о Л-1 - отображение кватерниона П2 на базис х} х2 х3;
01 и О 2 - кватернионы, ассоциирован-
ные с векторами абсолютных угловых скоростей первого и второго звеньев [2].
Таким образом, задача по определению абсолютных угловых скоростей, в сущности, сводится к вычислению кватернионов вида (1) и их последующему сложению.
Для отыскания выражения для углового ускорения продифференцируем формулу сложения скоростей (1) по времени. В результате получим
~2 = ~1 + Л1 ° &2 ° +
+ Л1 ° Е2 ° Л11 + Л1 ° &2 ° Л-1. (2)
Выполнив непосредственные вычисления, убеждаемся, что
Л ° П ° А~1 = Л ° П ° Л-1,
поэтому формула (2) может быть представлена в виде:
ё2 = ё1 + 2Л1 ° П ° а1 + А ° Е2 ° А1. (3)
Таким образом, задача по определению абсолютного углового ускорения свелась к вычислению кватернионов 2А1 ° П ° Л-1 и А1 ° Е2 ° А-1 и суммированию результатов вычислений на последовательных шагах. Кватернион абсолютного поворота звена выражается через кватернионы последовательных поворотов в теле известной зависимостью [2]:
Л = Л1 ° Л 2 ° ... ° Лп ,
а кватернион Л находится из кинематического уравнения
Л = —О о Л • (4)
2
В него следует подставить компоненты абсолютной угловой скорости, найденной из уравнения (1).
Кватернион Л может быть найден дифференцированием выражения (4):
Л =1 ёо Л + 0% о Л ,
2
откуда
ё = 2(Л о Л-1 +Л о Л^1). (5)
Эта запись может быть упрощена. Принимая во внимание, что
*0 Xо + X1 X1 + X2 X2 + Xз Xз = 0 ;
X X0 + X0 X0 + X1X1 + XlXl + + ')&2 X2 + ^2 ^2 + 'X“3 Х3 + Xз кз = 0, получим ёо = 0;
£ 1 = 2(—2.0 Xl + Х1X0 — X2 Xз + Xз X2 );
£2 = 2(—Х0 X2 + Х2 Х.0 — Х3 Xl + Х2 Xl);
£3 = 2(—А0 Xз + X з Xо — X 1X2 + X 2 Xl). Обращая эти уравнения, имеем:
2Ло = —81Л1 — 8 2Л 2 — 8 3Л 3;
1 Ло I з ^2 3 Л , ;
Л 2 =8 2Л о +81Л з — 8 з Л1;
Л3 = 83Л0 + 82Л — 81 Л.
Полученный результат можно представить в условной операторной записи:
ё = 2Л о Л— .
Его следует сравнить с формулой (5). Здесь предполагается, что 80 = 0, хотя
формально 80 = 2(Л0Л0 +ЛЛ+ Л2Л2 + Л3Л3).
В этом состоит условность записи. По той же причине из этого уравнения путем его обращения нельзя получить верного выражения для Л.
Машиностроение. Металлургия 78
Основу алгоритма «накопления» составляет уравнение преобразования координат:
г =Ло Г о Л-1, (6)
где г - кватернион, ассоциированный с вектором звена и выраженный в базовой системе координат; г - кватернион, ассоциированный с вектором звена в связанной системе; Л - кватернион поворота связанной системы относительно базовой.
Для определения положения звеньев многозвенного манипулятора следует просуммировать уравнения (6) по всем звеньям, начиная от стойки.
Дифференцируя уравнения (6) по времени, получим уравнения для скоростей и ускорений:
~ = Л о г о Л 1 + Л о г о Л 1; (7)
г = Л о г о Л1 + 2Л о г о Л1 + Л о г о Л1. (8)
Доказывается, что сумма кватернионов вида Л о г о Л1 и Л о г о Л1 дает вектор, хотя каждый из них в отдельности таковым не является. Формулу (7) можно представить в следующей условной записи:
г = 2Л о г о А1.
Аналогичными свойствами обладает сумма кватернионов Л о г о ЛТ1 и Л о г о Л1 в формуле (8). Поэтому формулу (8) можно представить в виде:
г = 2Ло г о Л1 + 2Л о г о Л1, (9)
имея при этом в виду, что коэффициент
2 относится только к векторной части кватерниона Л о г о Л1, скалярная же часть считается равной нулю.
Вычислительная эффективность метода выступает на передний план в задачах, связанных с управляемыми механическими системами типа роботов-манипуляторов. Под этим термином здесь подразумевается не только быстродействие алгоритмов при их реализа-
ции на ЭЦВМ, но также удобство программирования, простота и единообразие записей. Специфическим моментом этих вычислений является проблема сингулярности - эффективность ее решения также относится к вопросам вычислительной эффективности метода.
Для того чтобы описать ориентацию твердого тела посредством матрицы, необходимо задать девять элементов, которые определяют положение осей повернутой системы относительно системы координат, принятой за базовую. Каждый столбец матрицы представляет единичный вектор в направлении соответствующей оси связанной системы % п Z, заданный своими координатами в системе xyz.
В кватернионе конечного поворота Л компонента Л0 определяет угол конечного поворота, а компоненты ^,A2,A3 -декартовы координаты единичного вектора оси конечного поворота в пространстве осей xyz. Таким образом, два данных способа задания ориентации близки по своей идеологии, но способ кватернионов более лаконичен. Это следует принимать во внимание при формировании самого задания.
При управлении манипулятором необходимо рассчитать результирующий поворот конечного звена по составляющим поворотам предыдущих звеньев, что представляет прямую задачу ориентации.
При решении этой задачи, например, для шестизвенного манипулятора методом матриц необходимо перемножить шесть матриц 3*3 элементарных поворотов. Использование стандартной схемы перемножения матриц потребовало бы 135 умножений, 90 сложений. Поскольку для каждой операции умножения и сложения требуется два обращения к памяти, общее число операций возрастет до 675. В микропроцессоре Motorola MC 6800 целочисленное умножение занимает 5,6 мкс, сложение - 0,96 мкс, а обращение к памяти - 0,32 мкс. В таком случае расчет ориентации займет 986 мкс. Минимальное быстродействие для работы замкнутой системы управления при стандартной дискретной частоте 60 Гц составляет
16 мс. Этот анализ дает представление о необходимой скорости вычислений.
Наиболее эффективным с точки зрения быстродействия является способ аналитического перемножения всех шести матриц, а затем введения полученных соотношений в ЭВМ. Такой способ позволяет исключить операции умножения на нуль и единицу. Однако он очень трудоемок, не гарантирует от ошибок и к тому же не обладает необходимой гибкостью, так как для различных схем манипуляторов дает различные аналитические выражения.
В отличие от матриц кватернионы элементарных поворотов могут быть представлены единообразно посредством общей формы кватерниона, а именно:
Л х = ^ 0 , X 1 ’0’ 0 ]>'
Л у = [X 0 ,0, X 2 ’ 0 ];
Л z = [X 0 ,0, 0, X з ].
При умножении кватернионов по полной схеме потребуется 16 умножений и 12 сложений против 27 умножений и 18 сложений при умножении матриц 3^3. Если умножается полный кватернион на кватернион элементарного поворота, число умножений уменьшается до 8, а сложений - 4.
Метод, обладающий достаточной универсальностью и наименьшей трудоемкостью, состоит в том, чтобы «вручную» перемножить три кватерниона, а затем полученные точные выражения кватернионов первой и второй групп поворотов используются в программе, по которой ЭВМ производит численное умножение по известной схеме.
Перемножение трех кватернионов выполняется достаточно просто и его результат выглядит следующим образом:
К = Ро Ло М;
к 0 = р0у 0 — р1у1 —р 2у 2 — р3у3;
к1 =р0у1 — р1у0 —р2у3 — р3у2 ;
к2 = р0у2 —р2У0 — р3у1 — р1у3 ;
к 3 =р0у 3 — р3у 0 — р1у 2 — р2уЪ (10)
где
V 0 = Л 0^ 0 — Л1^1 — Л2^ 2 — Л 3^ 3;
У1 = Л 0^1 + Л1^ 0 + Л 2^3 — Л3^ 2;
V 2 = Л 0^ 2 + Л 2^ 0 + Л 3^1 — Л1^3;
1^3 = Л0^3 + Л3^0 + Л\^г — Лг№\. (11)
Для вычисления кватерниона Л потребуется 32 умножения и 24 сложения. Эти затраты могут быть уменьшены, если принять во внимание, что в кватернионе элементарного поворота две нулевые компоненты. В таком случае уравнения упрощаются до такой степени, что в правой части уравнений (11) остается только одно произведение, а в правой части уравнений (10) - сумма (разность) двух произведений; в итоге имеем 12 умножений и 4 сложения. Для двух групп поворотов с учетом необходимости перемножения по схеме получим 40 умножений и 20 сложений.
Перемножение шести кватернионов по технологии, аналогичной вышеописанной, хотя и более громоздко в записи, но дает наименьшие вычислительные затраты: всего 36 умножений и 16 сложений.
Единообразие записей сводит к минимуму возможность ошибок в алгоритме. Целесообразно составить универсальную программу перемножения шести кватернионов, а затем простейшим редактированием путем исключения нулевых элементов в каждом конкретном случае привести ее к минимальной форме.
Преимущество описания ориентации с использованием кватернионов состоит в том, что вся информация об ориентации твердого тела содержится в
четырехмерном векторе [Л0,Л1,Л2,Лэ ]Т . Зная его, легко сформировать соответствующую матрицу т, используя соотношение [1]:
т =
л02 + л12 — л22 - л32
2(ЛЛ2 — Л0Л3) 2(ЛЛ — Л0Л2)
2(ЛЛ2 — Л0Л3) Л + Л2 —Л2 —Л2
2(Л2Л3 — Л0Л1)
2(ЛЛ3 — Л0Л2) 2(Л2Л3 — Л0Л1) Л02 + Л32 — Л12 — Л 22
(12)
Обратный переход осуществляется с помощью выражений:
2Л0 =л1^ + т11 +т22 +т33 ;
Л1 = (т32 — т 23 )/4Л 0;
Л 2 = (31 —т13 )/4Л 0;
Л3 = (Т21 — т12 )/4Л0. (13)
Возможны две схемы управления ориентацией: по схеме конечного поворота и по схеме последовательных поворотов на эйлеровы углы.
При управлении ориентацией по схеме конечного поворота необходимо знать направление оси и угол конечного поворота. Они связаны с компонентами кватерниона соотношениями:
cos X/2 = Л0;
cos a = ^1/sin x /2;
cos в = Л2 / sin x /2;
co s y = Л3 / s in x/2. (14)
Если ориентация задана матрицей, для решения обратной задачи потребуется объем вычислений, определяемый формулами (13) и (14), в то время как при задании в форме кватерниона достаточно произвести вычисления только по формулам (14).
Гораздо чаще возникают условия, при которых требуемая ориентация достигается последовательным выполнением трех эйлеровых поворотов. Здесь функции параметров ориентации выполняют эйлеровы углы и их надлежит рас-
считать. В отличие от параметров конечного поворота эйлеровы углы определяются не однозначно, а зависят от выбора последовательности поворотов, что является одним из недостатков системы эйлеровых углов как кинематических параметров.
Пусть для определенности выбран вариант эйлеровых углов, соответствующий матрице т = Z1X Z2. Матрица X устанавливает соотношение между направляющими косинусами и эйлеровыми углами. Обратные соотношения определяются уравнениями:
Ф1 = агс1§(т13 /т 23);
у = агссоБ т33;
Ф2 = агс1§(—т31 / т32). (15)
При угле у, стремящемся к нулю, т23 также стремится к нулю; то же самое относится и к т23. Таким образом, возникает вырожденная ситуация и трудноразрешимая вычислительная проблема.
Выход из положения можно найти с помощью кватернионов. Компоненты кватернионов представляют невырождаю-щуюся систему параметров, существующую и однозначно определенную всюду -это видно из анализа уравнений (13), в которых Л0 никогда не обращается в ноль.
Между компонентами кватерниона и эйлеровыми углами существуют достаточно простые соотношения. В частности, для рассматриваемого примера нами установлено:
Л1 = Л 2 ;
Л 0 Л1
Ф1 = агссо8 —р + агссо8 —р ;
^2, +Л23 д/л2 +Л22
Ф2 = агссо^—т= Л" — агссо^—р Л . (16)
ЛЛ ЛЛ
Формулы (16) дают наиболее удобный, но не единственно возможный вариант перехода от компонентов кватерниона к эйлеровым углам для данной последова-
тельности поворотов.
В вырожденном положении при / = 0 ориентация связанной системы описывается кватернионом Л = [,0,0, Л3 ], где
Л0 = cos (ф1 / 2 + ф2 / 2);
Лз = sin (ф1 / 2 + ф2 /2).
При подстановке Л1 = 0 и Л2 = 0 в формулы (16) во вторых слагаемых возникает неопределенность вида 0/0.
Структура формулы (16) позволяет обойти это затруднение алгоритмическим путем, предусмотрев в программе при Л1 = Л2 = 0 переход к формуле
Л0
ф = ф arccos^= 0 .
тлллл
В этой задаче компоненты кватернионов играют роль переходных промежуточных параметров. Подчеркнем, что без их использования непосредственный переход от направляющих косинусов к эйлеровым углам в вырожденном положении нельзя осуществить никаким алгоритмическим приемом.
Углы Эйлера-Крылова естественным образом совпадают с углами поворотов в одноосных шарнирах, поэтому их применение в задачах механики манипулятора будет сохраняться до тех пор, пока будут использоваться манипуляторы с одноосными приводными шарнирами. Однако в будущем следует предвидеть развитие манипуляционных устройств с управляемыми шаровыми шарнирами, так как они в наибольшей мере соответствуют идеологии антропоморфного манипулятора.
В таком случае рациональный выбор параметров ориентации должен привести к необходимости использования параметров конечного поворота. Эйлеровы углы в этих условиях дают очень трудоемкие вычисления.
Прямая кинематическая задача находит разнообразное приложение при исследовании механики манипуляционных роботов. Она используется для определения кинематических характери-
стик движения звеньев, при точностном и силовом анализе, в задачах управления и планирования движения. Отсюда ясно, насколько важно отыскание оптимальной формы ее представления.
С точки зрения удобства программирования целесообразно получить универсальную программу для обобщенной модели кинематической цепи, которая простым образом трансформируется в программу для любой конкретной схемы шарнирного манипулятора. Обобщенная модель шарнирного манипулятора представляется в виде кинематической цепи с трехосными кинематическими парами. Переход от обобщенной модели к конкретной схеме состоит в замене трехосных шарниров одноосными в соответствии с принятой схемой расположения связанных осей звеньев. В программе следует заменить операторы сферического поворота на операторы элементарных поворотов.
Формально такой переход сводится к исключению из текста универсальной программы операций с нулевыми элементами. Редактирование программы легко осуществляется на дисплее с использованием автоматического редактора, тем самым предотвращаются ошибки и облегчается работа по подготовке программы.
Такой подход к обработке программы требует специальной организации вычислений. Обычно используемый рекуррентный метод здесь не пригоден, так как он предполагает работу с формулами универсального вида. Рекуррентный метод дает минимум записей, но максимум вычислений. Другой путь - составить универсаль-
ный алгоритм с явной записью формул для всех шагов расчета последовательно от первого до последнего звена, а затем произвести указанное выше редактирование. В этом случае программа записывается громоздко, но редактируется просто и считается быстро.
Алгоритм кинематического анализа может быть построен на базе аппарата матриц и кватернионов. Однородные матрицы и бикватернионы являются формальными образованиями на основе матриц 3^3 и кватернионов Гамильтона и поэтому их использование не дает новых вычислительных эффектов.
Выводы
На базе разработанного алгоритма моделирования кинематики манипулятора можно создать универсальную программу управления кинематикой манипулятора, которая, во-первых, более прозрачна, чем аналогичная программа с использованием традиционного аппарата матриц, а, во-вторых, легко редактируется для различных схем манипуляторов и, наконец, содержит меньшее число вычислительных процедур.
СПИСОК ЛИТЕРАТУРЫ
1. Борисенко, Л. А. Механика манипуляторов : монография / Л. А. Борисенко. - Могилев : Белорус.-Рос. ун-т, 2006. - 212 с. : ил.
2. Бранец, В. Н. Применение кватернионов в задачах ориентации твердого тела / В. Н. Бранец, Н. П. Шмыглевский. - М. : Наука, 1973. - 320 с.
Белорусско-Российский университет Материал поступил 04.11.2006
L. A. Borisenko
The development of methods for simulation of manipulator kinematics through using of quarternions
Belarusian-Russian University
At present quarternions are rarely used in robotics. Usually most investigations the apparatus of matrix is employed, because algebra of quarternios is very known little. But quarternions have many advantages. In this paper only some of them are given.
