CC BY
7
УДК 681.5 DOI: 10.14529/^сг210405
ФОРМАЛИЗМ ВЫПИСЫВАНИЯ УРАВНЕНИЯ ДИНАМИКИ МАНИПУЛЯТОРОВ
А.И. Телегин
Южно-Уральский государственный университет, филиал в г. Миассе, г. Миасс, Россия
Решается проблема громоздкости уравнений динамики манипуляционных систем промышленных роботов (манипуляторов), состоящих из поступательных и вращательных сочленений. Предложен новый формализм выписывания уравнений динамики манипуляторов в направляющих косинусах, из которых легко получить уравнения динамики в других параметрах относительной или абсолютной ориентации тел. Приведены примеры выписывания уравнений динамики манипуляторов в направляющих косинусах, из которых путем применения их свойств получены уравнения динамики в относительных углах поворота тел. Рассмотренные манипуляторы имеют от трех до шести степеней свободы. В их уравнениях динамики явно выражены геометрические, кинематические, статические и инерционные параметры. Множители при ускорениях и произведениях скоростей в выписанных уравнениях динамики являются оптимальными в смысле минимума арифметических операций (сложений и умножений), необходимых для их вычислений. Предложен JS-код и методика верификации уравнений динамики манипуляторов, записанных в аналитическом виде. Проблема в том, что при ручном выписывании уравнений возможны ошибки и описки в промежуточных записях и конечном результате. Поэтому необходимо проверять результаты выписывания на отсутствие ошибок, т. е. выполнять верификацию формул вычисления составляющих уравнений динамики. Для этого можно использовать ПО, предназначенное для вычисления обобщенных движущих сил манипуляторов, т. е. решать первую задачу динамики. В качестве такого ПО здесь предлагается web-приложение, в котором JS-функция используется для верификации уравнений динамики манипуляторов. Разработана методика верификации формул вычисления обобщенных сил тяжести и множителей (коэффициентов) при обобщенных ускорениях и произведениях обобщенных скоростей в уравнениях динамики. Приведен пример верификации уравнений динамики универсального манипулятора с шестью степенями свободы в пространстве. Целью исследования является разработка формализма выписывания аналитического вида уравнений динамики манипуляторов в направляющих косинусах главных осей связанных систем координат тел, коэффициенты которых содержат минимальное количество арифметических операций. Методы исследования относятся к векторной и аналитической механике систем абсолютно твердых тел, к векторной алгебре, а также к системному анализу и программированию на скриптовых языках. Результаты исследования содержат два доказанных утверждения, формулы которых и методика их использования позволили вручную выписать уравнения динамики манипуляторов с тремя и шестью степенями подвижности как в направляющих косинусах, так и в обобщенных координатах. В обоих случаях упростить полученные уравнения невозможно. Заключение. Записанные аналитические виды уравнений динамики занимают несколько строк. По известным классическим формализмам (Лагранжа, Аппеля, Нильсена, Ньютона - Эйлера и т. д.) практически невозможно получить аналогичные результаты из-за большого количества сложных математических операций в их реализации и громоздкости получаемых формул.
Ключевые слова: промышленный робот, уравнения динамики, формализм выписывания, направляющие косинусы, повторное использование формул, JS-функция, верификация уравнений.
Введение
Основной проблемой практического использования уравнений динамики (УД) манипуляционных систем (МС) является их громоздкость. Это следствие неэффективности классических формализмов вывода УД МС. В частности, поэтому в научных статьях в качестве примеров демонстрируются, как правило, плоские двухзвенники [1-5]. До сих пор встречаются научные статьи, в которых по формализму Лагранжа на нескольких страницах выводятся УД простейших
МС [6, 7], в то время как современные формализмы позволяют выписывать УД таких МС в нескольких строках [8, 9].
Из-за громоздкости аналитических видов УД разрабатываются пошагово-алгоритмические методы исследования МС. Известны различные формулы вычисления коэффициентов УД МС и им соответствующее ПО. Рассматриваются как конкретные МС [10-14], так и классы МС [15]. Решаются задачи динамики [16] и управления движением тел МС [17]. Однако такие методы не позволяют эффективно решать задачи синтеза МС с заданными динамическими свойствами, а также синтезировать адаптивные ПИД-регуляторы программных движений тел МС или решать задачи оптимального управления МС в реальном масштабе времени.
Постановка задачи. Разработать простой формализм выписывания УД МС в аналитическом виде с минимальным числом арифметических операций и продемонстрировать его на примерах выписывания аналитических видов УД МС, имеющих от трех до шести подвижных тел.
1. Формальное описание МС
Присвоим неподвижному телу МС (стойке, станине) нулевой номер и свяжем с ним неподвижную систему координат (НСК) Охуг. Здесь х - орт оси абсцисс, направленной горизонтально вправо, у - орт оси ординат, направленной вертикально вверх, г - орт оси аппликат, направленной так, что НСК - правая. Следующие за НСК подвижные тела занумеруем числами 1, 2, 3, ..., М, где N - количество подвижных тел МС. С i-м телом свяжем точку и назовем ее полюсом /-го тела. Полюс вращающегося тела выбираем на оси его относительного вращения и полюса тел максимально совмещаем. Тогда максимальное число межполюсных радиус-векторов ^ = 0¿_10¿ обнулится.
Введем следующие обозначения: - орт оси вращения /-го тела относительно (£ — 1)-го тела; рг - орт оси поступательного движения /-го тела относительно (£ — 1)-го тела; т01 - масса /-го тела; сь - орт, направленный из полюса /-го тела в его центр масс (ЦМ) Сь; 0¿x¿y¿z¿ - главная система координат /-го тела (ГСК(/)); т^ - масса /-й подсистемы МС, состоящей из /-го (корневого) тела и следующих за ним тел.
В процессе выписывания УД МС используются проекции на оси НСК статических моментов подсистем относительно полюсов их корневых тел. Векторы статических моментов можно выписать по обратной рекуррентной формуле [18]
т{ = а1с1 + т1+1Я1+1 + т1+1,_ __(1)
где £ = Ы, N — 1,... ,1; тк+1 = = 0; аг = тО(|0(С\| - модуль статического момента /-го тела относительно точки . Для проекций на оси НСК векторов т^, К; и орт С; введем следующие обозначения: т^, т^, т? - проекции вектора тг на оси НСК; Я*, К(у, Щ - проекции вектора на оси НСК; с*, с(у, с? - направляющие косинусы (НК) орта с^ = 01С1/О^С^ в НСК.
Для выписывания УД будем использовать формальное описание МС в виде таблиц. Основная таблица составляется на основе вербального описания, включающего кинематическую схему (КС) МС и, при необходимости, дополнительное словесное описание. На КС полюс /-го тела изображается точкой и надписью О;, а ЦМ - звездочкой и надписью С;. Точкой 0О1 изображается положение полюса /-го поступательного тела в случае ^ = 0, где ^ - величина относительного поступательного перемещения /-го тела вдоль оси 0¿p¿. На КС /-е поступательное тело изображается в положении, когда орт р1 = 0o¿0¿/q¿ указывает на положительное направление перемещения. На КС /-е вращательное тело изображается в исходном относительном положении, т. е. когда д,- = 0, где д,- - угол поворота /-го тела вокруг оси 0^. относительно (у — 1)-го тела. Если смотреть навстречу оси , то поворот против хода стрелки часов считается положительным.
На КС достаточно изобразить начало НСК, а также положения точек О¿, С;, 0О1, так как положение осей НСК и ориентация в пространстве векторов К;, С;, р1 следует из их определений. В дополнительном словесном описании нуждаются только положения орт и углы поворота д,. Когда углы поворота всех тел равны нулю, тогда, по умолчанию, ~Ц = у, если ось вращения /-го
тела вертикальна, = х, если ось вращения горизонтальна, = г, если ось вращения смотрит на наблюдателя. Угол поворота откладывается от исходного до текущего положения выбранного орта, который жестко связан с j-м телом.
Формальное описание МС начинается с разложения орт , р1 (/ = 1, 2, ..., Ы) по осям НСК.
Затем определяются проекции на оси НСК межполюсных радиус-векторов В.^ = и стати-
ческих моментов т.1 подсистем. В качестве примеров на рис. 1-3 приведены КС и в табл. 1-3 им соответствующие формальные описания рассматриваемых МС. В заголовке таблиц использованы введенные обозначения. Имена д/р*, ц/р![, я/р? означают, что в 1-й строке записываются НК , Чь орта ^ в НСК, если /-е сочленение вращательное, иначе записываются НК р1*, р?7, р? орта Гр1 в НСК.
Если из КС МС проекции векторов , р1, В.^ на оси НСК не очевидны, то в таблице записываются их обозначения. Иначе записываются их выражения через ^, 5у = Су = соз(ду) и геометрические параметры тел.
Проекции , ш^, rn.fr вектора тк выписываются по формуле (1).
Рассмотрим примеры заполнения таблиц формального описания МС.
Рис. 1. МС с декартовой СК Fig. 1. MS with cartesian CS
Рис. 2. МС с цилиндрической СК Fig. 2. MS with cylindrical CS
Рис. 3. МС с шестью степенями свободы Fig. 3. MS with six freedom degree
Таблица 1
Параметры МС на рис. 1
Table 1
MS parameters in Fig. 1
i 4/Vxi ч/vl Rf «Г «f mf У mf mf
1 /0 1 0 0 Ял 0 m2g2 + a3c3 - a2 a3S3 - ai 0
2 /1 0 0 42 0 0 a3C3 — a2 a3s3 0
3 0 0 1 0 0 0 a3c3 a3s3 0
Таблица 2
Параметры МС на рис. 2
Table 2
MS parameters in Fig. 2
i Rfri q/PiV 9/Pf Щ «Г mf У mi mf
1 0 1 0 0 0 0 mf mf mf
2 /С1 0 0 -a2c1 a3-m3q3 a2Si
3 /0 -1 0 0 -Чз 0 0 a3 0
Таблица 3
Параметры МС на рис. 3
Table 3
MS parameters in Fig. 3
i 4/Pf 4/pf 4/vt fif Rf mf У mi mf
1 0 1 0 0 0 0 mf mf mf
2 0 1 0 L1c1 0 -■Ml bc12 - aqf mq -bs12 - aq|
3 0 1 0 0 -^2S12 -aq% mq -aqi
4 /0 -1 0 0 Ч4 0 -aq£ — &C5 -aqi
5 0 Cl3 0 0 0 -aq% -acs -aqi
6 c5 0 0 0 -a6q£ -a6c5 -a6qZ
Пример 1. Из КС на рис. 1 видно, что N = 3, рг = у, р2 = х, q3 = г, Яг = ООх = цгу, Я2 = Ог02 = ц2х, Я3 = 0203 = 0. Эта очевидная информация отражена в первых шести столбцах табл. 1. Угол q3 откладывается от орта х до орта с3.
По формуле (1) и из рис. 1 получим т3 = а3с3 = а3(с3х + 53у), что отражено в трех последних столбцах 3-й строки табл. 1.
Для I = 2 по формуле (1) и рис. 1 получим
~т2 = а2с2 + т3Д3 + т3 = а2(-х) + т3 = (а3с3 - а2)х + а353у.
Для I = 1 по формуле (1) и рис. 1 получим
~тг = а1с1 + т2Я2 + т2 = а1(—у) + т2ц2х + т2= (т2ц2 + а3с3 - а2)х + (а353 - ах)у.
Коэффициенты полученных разложений векторов т2, тг по осям НСК записаны соответственно во 2-й и 1-й строках табл. 1.
Пример 2. Из КС на рис. 2 видно, что N = 3, цг=у, р3 = —у, Яг = 001 = 0, Я3 = 0203 = = —■ЧзУ. Угол q1 откладываем от орта х до орта р2, направленного по определению из точки Оо2 в точку 02. Тогда р2 = с1х — и Я2 = 0102 = ц2с^х — д2з1г. Следовательно, р% = сг, р2 =0, р| = — Я2 = q2c1, =0, #2 = —что отражено во 2-й строке табл. 2.
По формуле (1) и рис. 2 получим т3 = а3с3 = а3у.
Для £ = 2 по формуле (1) и рис. 2 получим
т2 = а2с2 + т3Д3 + т3 = а2(—р2) — т3д3у + а3у =
= —а2(с1ж — — т3д3у + а3у = —а^х + (а3 — т3д3)у + а^!
Пример 3. Из КС на рис. 3 видно, что N = 6, цг = ц2 = ц2 = у, р4 = —у, д5 = 513х + с12г, где 513 = sm(q13), с13 = cos(q1з), д13 = цг + ц2 + д3. Цв = + с5у + Здесь использованы обозначения для НК так как формулы их вычисления не очевидны. Их мы найдем после
выписывания УД.
Для межполюсных векторов очевидны следующие разложения по осям НСК: Яг = 0, Я2 = Ь1с1х — Ь^г, Я3 = Ь2с12х — М^, _ _ где 512 = sin(q12), с12 = cos(q12), д12 = + ц2, Я4 = —цАу, Я5 = Я6 = 0. По формуле (1) и рис. 3 получим т6 = а6с6 = —а6д6 = —ав(ц£х + с5у + др). Для I = 5 по формуле (1) и рис. 3 получим
т5 = а5с5 + т6Я6 + т6 = а5(—д6) + т6 = — а5д6 — а6ц6 = —сщ6 = —а(ц£х + с5у + ц%г), где а = а5 + а6.
Для I = 4 по формуле (1) и рис. 3 получим
тА = а4с4 + т5Я5 + т5 = аАу + т5 = —ац£х + (а4 — ас5)у — ац^г. Для 1 = 3 по формуле (1) и рис. 3 получим
т3 = а3с3 + тАЯА + тА = а3(—у) — тАцАу + тА = —ац£х + тяу — ац^г, где тд = а4 — ас5 — а3 — т4д4.
Для 1 = 2 по формуле (1) и рис. 3 получим:
т2 = а2с2 + т2Я2 + т2 = а2(с12х — з12г) + т2Ь2(с12х — з12г) — ац£х + тду — ац1г = = [(а2 + т3Ь2)с12 — ац%]х + тчу — [(а2 + т312)512 + =
= (Ьс12 — ац£)х + тчу — (Ьз12 + ац1)г, где Ь = а2+ т3Ь2.
Коэффициенты полученных разложений векторов тг по осям НСК записаны в последних трех столбцах табл. 3.
2. Формализм выписывания УД МС
Ручное выписывание УД состоит из последовательности формальных действий, осуществляемых на основе таблицы формального описания и КС МС по формулам утверждений 1, 2. Доказательство утверждений приведено в конце статьи. Здесь на примерах МС с тремя и шестью степенями подвижности демонстрируется формализ выписывания УД в НК и переход к записи УД в относительных углах поворота тел вращательных сочленений.
Утверждение 1. Для/-го поступательного сочленения движущая сила
^ = р* (т^ + т?) + ру (щк*. + ту) + р) (т^ + т?) + т]дру, (2)
где ^ = Й=1 , Яуу = £{=1 Яу, ^ = !{=1 Щ.
Для /-го вращательного сочленения момент движущей силы М} = — тЩ + М*+1) + ц^т'к^ — т*^ + АГ/+1) +
— т^Щ + М*+1) + % + С], (3)
где М*+1 = = = 0,
М?+1 = 7$=}+1 [тк(ЯукЁгк — ЯгкЯук) + тукЯгк — Ягкту + Яутгк — тгкЯук],
Му+1 = — КЧ) + — «М + — <Ч],
М*+1 = [тк(Я£Яу — ЯукЯхк) + ткЯк — Яутхк + Яхктук — тукк%]. _
Скорость кинетического момента тел/-й подсистемы относительно оси О^ц. вычисляется по формуле к) = К]к, где
К]к = + Акшукш1) + + Вкш£ш2к) + + Вкш1шук), (4)
1ок, /о/с, ¡ок. - моменты инерции k-го тела относительно осей Окхк, Окук, Окгк ГСК(k) соответственно; Ак = ]*к —]Уок; Вк = Л^к —]*к; ^ = ]Уок —ЛЦк; шу, - проекции абсолютной угловой скорости k-го тела (вектора шк) на оси ГСК(^); Ч*к = Ч) • *к, Ч*к = Ч} ■ Ук, Ч)к = ^ • гк.
Момент силы тяжести, приведенный к оси вращения /-го тела:
С, = — ц*т*). (5)
Пример 1 (продолжение). МС с декартовой СК в вертикальной плоскости (см. рис. 1). Формальное описание этой МС представлено в табл. 1. Следовательно, р* = р1 = 0, ру = 1 и формула (2) для у = 1 принимает вид Рг = т^^Ё^ + ту + тгд, где Ё.^ = Ёу. Отсюда по табл. 1 получим Рг = т1ц1 + а3$3 + тгд. Для } = 2 в формуле (2) имеем р2 = 1, р% = р% = 0 и, следовательно, Р2 = т2Ё%2 + ^2, где Ё%2 =^1+^2. Отсюда по табл. 1 получим Р2 = ш2ц2 + а3с3. Для } = 3 в формуле (3) имеем = = 0, Ц3 = 1 и, следовательно, М3 = тх3Ёу13 — туЁ?3 + К3 + С3, Ёу3 = Ёу, Щ3 = £?=1 Я*, где по формуле (5) и табл. 1 получим С3 = д(ц3т3 — Ц$т3) = да3т% = да3с3. Теперь, используя табл. 1, получим М3 = а3с3ц1 — а3с3ц2 + ЛГ3 + да3с3, где по формуле (4) с учетом равенств 9зз = д33 = 0, 4§з = 1, = 0, = 0, выпишем К3 = .
Таким образом, УД в НК для МС на рис. 1 имеют следующий вид: т1ц1 + а3$3 + т1д = Рг, т2д2 + а3с3 = Р2, |Уоз^з + а3(с3цг — 53ц2) + да3с3 = М3.
Видно, что УД 1-го и 2-го тел интегрируются. Известны формулы [19]
= ЗкУк + 21у1 + ЯкУю = + xfe¿fe + ШI = + ууху + (6)
где хк,х^,хк - НК орта хк оси абсцисс ГСК(^) в НСК; ук,Ук,Ук - НК орта ук оси ординат ГСК(^) в НСК; гк, - НК орта ~гк оси аппликат ГСК(k) в НСК. Из третей формулы получим
а>§ = у3х3 + у3ху + у3х3. Угол ц3 откладывается от горизонтальной оси до главной оси 3-го тела 03х3. Следовательно, у3* = — 53, х£ = с3, у3у = с3, = 53, у3 = 0 и = —з3с3 + с3з3 = ц3.
Для перехода от УД в НК к УД в относительных углах поворота тел используются следующие свойства простых НК:
^ = с^ — siq?, с1 = — — ciq?, ед — 5гсг = ^ (7)
— = —41, ЗД + С1С1 = —¿[¡. (8)
Эти формулы легко проверить непосредственным вычислением производных. Из предшествующих УД для МС на рис. 1, используя формулы (7), получим УД в углах поворота тел:
т1ц1 + а3(с3ц3 — з3ф + тгд = Рх,
™2Й2 — а303д3 + с3ц1) = Р2, |УозЧз + а3(с3цг — 53ц2) + да3с3 = М3.
Пример 2 (продолжение). МС с цилиндрической СК (см. рис. 3). Формальное описание этой МС представлено в табл. 2. Следовательно, в формуле (2) для ] = 2 имеем: Рг = С1, р2 =0, р% = —б1 и Р2 = сг(т2Ё%2 + т%) — з1(т2Ё^2 + т%), где $12 = + $2, ^12 = + . Отсюда по табл. 2 получим Р2 = с1[т2(д2с1)"2 — а2с1] — з1[т2(—д2з1)"2 + а^] = = с1[т2(ц2с1 + ц2с^)'1. — а2с1] + з1[т2(с[251 + — а^] =
= с1[т2(ц2с1 + 2ц2с1+ц2с1) — а2с^] + з1[т2(с[251 + 2ц2б1 + д2И1) — а2з1] = = т2с[2 + 2т2ц2(с1сг + з-^) + т2ц2(с1с1 + — а2(с1'с1 + 5!^!) = = т2ц2 + (т2ц2 — а2)(с1с1 + з-^). Для ] = 3 по формуле (2) и по табл. 2 получим
^з = —тзК^з —т3 — т3д = —т3 Ё* — — т3д = т3ц3—т3д. Для у = 1 в формуле (3) имеем = 1, ц* = ц1 = 0 и, следовательно, М1 = т\Ё1х — т^Ё^ + N¡1 + ^ + Съ где по формуле (5) = д(ц*т* — = 0.
Из табл. 2 имеем Я* = Я* = 0, Я$ = Я§ = 0. Следовательно, М1 = М2 + Къ где М^ = т2(Я%Я% - Я£Я|) + тЩ - Я%т! + Я%т% - тЩ = = (т2Я% + т2)Я% - (т2Я% + тх2)Ц - Я%т*2 + Я1тх2 и по табл. 2 получим
= [т2(-ч2з1) + а2з1](ч2с1)'1: - (т2Ч2с1 - а2с1)(-ч2з1)[ - а2ц2с1з1 + а2ц2з1с1 = = -(Щ2Ч2 - «гЛ^г^ - с^я^] + а2ц2(з1с1 - с^О = = -(т2ч2 - а2)[з1(ц2с1 + ц2с1) - с1(ц2з1 + Ч2з1)]+а2ц2(51с1 - с^О = = (а2 - т2ц2)ц2(5^с1 - с^ + а2ц2(з1с1 - с^ = (2а2 - т2ц2)ц2(з1с1 - с&У С учетом равенств шк = ц-^у, цхк = q[k = 0, = 1, = = 0, к = 1,2,3 по формуле (4) выпишем Кг = +Уо2^2 + /оз^з' = , где ]УГЗ = ]УоГ + ]%2 + ]%3. По второй формуле (6) с
учетом равенств х* = съ х% = -г* = г? = сг получим = ххг? + ху¿у + = = с-^ - 51с1, где I = 1,2,3. Следовательно,
= /13(^1 и
М1 = [(1а2 - т2ц2)ц2(з1с1 - с^^ +] ^с^ - =
= [(Я3 - 2а2я2 + т2ц^)(с1з1 - 51с1)]£.
Таким образом, УД рассматриваемой МС в НК имеют следующий вид: [(/13 + - ^^Х^! - = М1,
т2с[2 + (т2ц2 - а2)(с1с1 + з-^) = Р2, ^зЧз - т3д = Р3.
Для представления этих УД в углах вращательных сочленений используем формулы (7), (8). Тогда получим следующие УД:
[(/13 + т242 - 2а2ц2)с{1]'1: = М1, т2ц2 + (а2 - т2ц2)ц1 = Р2, т3ц3-т3д = Р3.
Для упрощения процесса выписывания УД МС с параллельными осями вращательных сочленений рекомендуем использовать Утверждение 2. Если 1 = , то
М]_1 = М} + дЫ^т?^ - т?) - Ц^т^ - т*)] + +
+Ях[т](ЯуЯ* - Я/Я/Х + Я?т? - + (т*^ - т'Щ^ - (т?_1 - т^Я^] + +цу)[т](Я*Щ - Я?Щ)[ + Я]Ш* - Я*Щ + (т?_1 - - (т?^ - т?)^^] +
+Ч][т](Я]хЯ? - Я]Я*)[ + Я^т] - Я?т? + (т^ - т?)^ - (т^_1 - т^Щ.,].
Пример 3 (продолжение). МС с КС на рис. 3. Формальное описание этой МС представлено в табл. 3. Следовательно, в формуле (2) для ] = 4 имеем: Р4 = -1, Р4 =р£ = 0 и РА = -тАя1А -туА- т4д, где = Щ. Отсюда с учетом табл. 3 получим РА = тАцА + ас5 - тАд. Для у = 6 по формуле (3) и табл. 3 выпишем
Мв = Чхв [шу6 (Я! + Я!) - тЩ] + с5[т*6(Щ + Я%) - тх6(Я% + Щ)] + +Ч*6[тх6Щ - ту6 (Щ + Я3*)] +К6 + С6 = а6(Чх6цг6 - -
-а6(с5ЧI - с5Ч1)№ + Щ) + а6(с5Ч% - Ч£с5)(Я! + Я%) + К6 + С6 = К6 + С6, где по формуле (5) получим Св = д(ц%т% - ц£т1) = д[ц1(-авц1)-ц1(-авц1)] = 0. Тогда М6 = К6, где по формуле (4) с учетом равенств у£6 = у|6 = 0, у£6 = 1 получим М6 = к6 = ]у06шу6 + В6шёш1
Для у' = 5 по формуле (3) с учетом равенств Я% = Я^ = Я§ = 0 выпишем М5 = Ч^[ту(Я2 + Я§) - т$Яу] + я![тхвЯуА - ту(Щ + Щ)] + Кв + Св = = (4§шхв - Ч^ШвЩ + шу5 №(Я1 + Я!) - ч!(Щ + Я3Х)] +Ъ + С5,
где по формуле (5) и табл. 3 получим
= 9(ч1™- 5 — тр = дкв^—ачр — Ч5(—щ1)] = —ад(я§41 — ч£ч!). Учитывая равенства д5 = г5, ц6 = у5 и формулу
х = — ^ф* + — + ^ — ^фг (9)
векторного произведения орт СК, получим
Ч5Ч6 — Ч5Ч6 = г1У5 — у| = х у5 • у = —х5 • у = —ху = —cos(90O — д5) = — 55. Таким образом, используя табл. 3, получим
М5 = —а(ц1ц£ — ц^ц&Щ — ас5[513(—— — С^^С! + Ь2с12)] + + од^ =
= —аэ^'ц 4 + ас^^з«?! + <^¿1) + 12(513512 + С13С12)] + + а#55, где по формуле (4) с учетом равенств д5 = г5, ^55 = = 0, д|5 = 1, г£6 = —з6, гу6 = 0, г|6 = с6 получим
= Уо5^5 + — + ^б^б^б) + Сб(Уо6^6 + ).
По формуле (3) и табл. 3 для ] = 3 имеем М3 = т|(Я2* + Щ) — тх2{Щ + Ё§) + Му + К3 + С3, где Му = 0 и по формуле (5) С3 = д(ц3т3 — Ц3т3) = 0. Отсюда, используя табл. 3, получим
М3 = —сщКЬ^ + Ьг'Сц) — ац^Ь^+Ь^) + К3. Учитывая равенства у3*3 = у|3 = 0, уу = 1, = = 0, у3*4 = у|4 = 0, уу4 = 1, = д3, = = 0, у3 = 55х5 + с5у5, у|5 = 0, по формуле (4) получим
Ъ = ^ +/0>зу + + л5ш*ш1) + с5дуо5ыу5 + в5^|) +
+Ч36(У26^6 + А6^6'ш6) + 9зб(/о6Й6 + + X
где с учетом равенства ~Ц3=у имеем д36 = у • х6 = х^, ду6 = у^ = с5, д|6 = . Таким образом,
М3 = —аЬ1(ц^с^ + д^) — аЬ2(д^с12 + д£<т12) + £3. Учитывая равенство ц2 = ц2= у, по утверждению 2 получим
М2=М3 + К22 + т3(Я|Яз — ЯЖ + Яз*™з — + — ^з2)^ — (тх2 — тх3)Ё?2,
где по формуле (4) с учетом равенств д22 = д|2 = 0, д22 = 1, ш2 = ш2 = 0 получим К22 = ;>у + = У>у. Теперь по табл. 3 получим
м2 = М3 + У„2^2 + т3Ь22(—512с12 + С12512)£ + «¿2(512Чб + Сц^ — Й51211С1 + ЬС121151. Учитывая равенство Ч^ = Ч2 = У, по утверждению 2 получим
М1 = М2 + Кг1 + т2(Яг2Щ — + — Я2*т| + (т? — т!)^ — (т? — т^,
где по формуле (4) с учетом равенств = = 0, дуг = 1, шх = = 0 получим Кг1 = + В-^ш^ш^ = Теперь по табл. 3 получим
М1 = М2 +]уо1(Ъ1 + т^К—Б^ + с^Х + (—Ь151)(Ьс12 — ас&) + 11с1(Й512 + По формуле (6) имеем
У _ % • х У ' У % • 2 _ • • ^ _ X ' X ^ • ^ 2 • 2 _ • •
^^ — + Х^ + — — + ^2 +^2 ^2 — ^12 ^12 ^12 ^12.
Следовательно,
м2 = М3 + аЬ^^ + с12ц%) + ы1(с1251 — 512С1),
Мг = М2 +]1<Ь{ + [а(с1Ц^ + + Ь(С!512 — 51С12)], где ;у = ;у + т31?2, ;у = ;у + т,!2!.
Если в исходном положении МС на рис. 3 массы тел 3-й подсистемы распределены симметрично оси 03у, то /о; = /о;, т. е. = 0, А( = —!)г, где £ = 3,4, 5, 6. Следовательно,
= Уоб^б ,
=Уо5^5 —Уоб(5б^б — С6Й|) + + 56ш|),
где У1У3=/0Уз+/04.
Таким образом, если = 12 = I, то УД МС на рис. 3 в НК имеют вид:
]1й>1 + Ь1[а(с1ц£ + + Ь(с1§12 - = Мг- М2,
]у2шу2 + а12(512с[% + с12(Ц) + Ы1(с12з1 - з12с1) = М2- М3,
/з4ЙЭ + + /о5С5^ + /О6(*6^£ + гК) +]У0&Сь(Ь1 -
-аЬ[ц^(с^ + с12) + 0?! + <т12)] - + - ху6ш1) = М3,
" тАцА + асв - тАд = РА,
-Уоб(5б^б - Сб^б) + аЬс^^ + <т12) + с13(с! + с12)] - аз5(}4 + + + 56ш£) + адя5 = М5,
= М6.
<
Для перехода к УД в углах поворота тел выпишем формулы вычисления проекций шк через углы относительного поворота тел и их производные по времени. Для этого используем формулу сложения угловых скоростей шк = шк_1 + цк~цк. Тогда последовательно получим = ку = ц1у1, ш2 = ш1 + д2У2 = д12У2, й3 = ш2 + д3у3 = д13у3, = ш3 + д5д5 = = Ч13У3 + . Из рис. 3 видно, что У3 = С5у5 + 55Х5. Тогда = Ч13(с5у5 + з5х5) + ц5г5 и, следовательно, = 55д13, = с5д13, ш§ = д5. Отсюда = з5д13 + с5д13д5 = з5д13 + шуд5, = с5д13 - 55д13д5 = с5д13 -
Аналогично ш6 = ш5 + ц6у6 = ^х5 + ад|у6 + + ц6у6, х5 = с6х6 + 5616, г5 = -56х6 + с616, ш6 = ш%(с6х6 + 56г6) + (^+'д6)у6 + ц5(-з6х6 + с6г6), = - = шу + д6,
= + с6д5, = - 56д5 - - с6д5д6 = - 56д5 - о>£ = + д6,
о>£ = ¿¿ш^ + с6д5 + с6д6о)£ - 56д5д6 = +
В табл. 3 неизвестны выражения НК Их можно выписать по общим формулам или
вывести путем элементарных рассуждений на основе КС на рис. 3. Действительно, из рис. 3 видно, что Ч6=У5, У5 = с5у - 55х3, х3 = с13х - 513г. Следовательно, д6 = с5у - 55(С13х - 5132),
т. е. = У5 = -c13s5, Чб =У52 = 51з55. Отсюда Чб = (-¿13%-С1355Х = -55с13 - с1355 + +2<т13с5д13д5, = (51355 + 51355)£ = 55513 + 51355 + 2с13с5с[13с[5. Теперь для перехода от УД в НК к УД в относительных углах поворота тел достаточно воспользоваться свойствами (7), (8) простых НК, а также следующими свойствами: + саср = sin(a - 0)0 - ^(а - Р)$2, Са'зр - За'ср = ^(а - - sin(a - Р)$2, где 5а = sin(a),...,ср = cos(P). Эти формулы проверяются путем элементарных вычислений.
После перехода к относительным углам поворота тел и их производным по времени получим следующие УД МС на рис. 3:
' + ^Ь(С2'Ч 12 - 52Ч|2) + ¿«[^5(^23413 - 52зЧ|з) + $23(С5$5 - 5вЧв) +
+2с23С$Ц13Ц$] =Мх- М2,
2^12 + ^ь(с2'Ч 1 + + ¿а[55(СзЧ13 - 53д|3) + 53(с5(}5 - З^) + +2с3свс[13д5] =М2- М3,
¿«55(^23^1 + с3ц 12 + 523д! + 53д|2) +]уоЬСъЦв + (/Зс6 + ¡16с1)с[13 + +55(1еС5С[13 - 1ас[6)с[5 = М3, тАцА - а(55с}5 + с5д|) - тАд = РА,
¿^5(523^1 + 53Ч12 - СгэЧ1! - с3с[^2) + - 5$[ацА - (0С5С[13 + 1ас[6)с[13] +
+адя5 = М5,
Уоб(С5Ч13 + Чб - 5541345) = М6,
<
где использованы следующие обозначения констант: Ьь = ЬЬ, Ьа = Ьа,
!<11 = 1о! + m2^2, 1(12 = Уо2 + m3^2, ^36 = /оз + }о-4 + ^^ ^56 = 1о5 + Jo6, ^56 = #5 + /об, ^56 = /5У6 - ^56, ¡а = 06 + ]Хо6, 0=05+ Я6, /е = -/5С6 - О. Дальнейшие упрощения этих УД практически невозможны.
3. Верификация УД
В ручном и автоматизированном (с применением ПЭВМ) режимах выписывания УД МС возможны ошибки и описки. Поэтому выписанные формулы необходимо верифицировать. Для этого мы используем JS-функцию СФСТОВ вычисления силовых факторов для систем тел с одной открытой ветвью. В случае определенного сочетания нулевых и отличных от нуля входных списков обобщенных координат (ОК), обобщенных скоростей (ОС) и обобщенных ускорений (ОУ) эта функция позволяет верифицировать УД МС. Функция СФСТОВ возвращает списки числовых значений обобщенных движущих сил (ОДС) [Qlt Q2,..., Qw], а также проекций динамических реакций в сочленениях на оси НСК.
JS-код функции СФСТОВ представлен в листинге 1. Из первой строки кода видно, что функция СФСТОВ имеет десять входных параметров. Девять параметров являются списками и последний - скаляром g (модуль ускорения свободного падения). Скалярные, векторные и матричные величины хранятся в списках. До комментария /* умножение матрицы на вектор */ запрограммированы формулы вычисления ОДС. Эти формулы опубликованы в статье [20]. Из двух последних строк видно, что функция СФСТОВ возвращает три списка. Первый их них содержит ОДС. После комментария /* умножение матрицы на вектор */ представлены очевидные коды вычисления матричных и векторных операций.
Перед обращением к функции СФСТОВ необходимо заполнить следующие списки постоянных входных параметров: коды кинематических пар (КП), где КП(/) = 1, если i-е тело вращательное, и КП(/) = 0, если i-е тело - поступательное; направляющие косинусы осей КП (НКОКП) в ССК тел КП; координаты баз полюсов тел (КБПТ) в ССК предшествующих тел; координаты центров масс (ЦМ) тел в их ССК; тензоры инерции тел (ТИТ) в центральных ССК этих тел. Для МС на рис. 3 в листинге 2 приведен пример заполнения этих списков.
Для верификации УД МС можно использовать функцию СФСТОВ следующим образом. Если для всех k положить qk = qk = 0, то Qk = Gk.
Листинг 1
JS-функция решения первой задачи динамики МС
Listing 1
JS-function for solving the first problem of dynamics of manipulation systems
function СФСГОВ(КП,НКОКП,КБПТ,МТ,ЦМ,ТИТ,ОК,ОС,ОУ^){ var N = miength-l, i, qx, qy, qz, c, s, cl, МП=[], МП0=[,[]], МПТ=[], КПТ=[], АУЦМ=[], АУС=[], АУУ = [], АУП = [], АКГВС = [], АКГМС = [], ОДС = [] , F = [], M = []; for(var i = 0; i <= N; i++){ МЩ] = []; /* Вычисление МП */ ЩКЩ] === 0) { МЩ]=[[1,0,0],[0,1,0],[0,0,1]] } else {qx = НКОКЩ][0]; qy = НКОКЩЩ]; qz = НКОКПИ[2]; c = МаШ.^(ОКЩ); s = МаШ^ш(ОКЩ); c1 = 1 - c; МП[i]=[[qx*qx*c1+c, qx*qy*c1-qz*s, qx*qz*c1+qy*s], [qx*qy*c1+qz*s, qy*qy*c1+c, qy*qz*c1-qx*s],
[qx*qz*c1-qy*s, qy*qz*c1+qx*s, qz*qz*c1+c]] }; МПТЩ = трансп(МП[1]) }; /* Вычисление АУС, АУУ, АУП */
АУС[0] = ЧВ(КЩ0]=ЮС[0], НКОКП[0]); АУУ[0] = ЧВ(КП[0]=ЮУ[0], НКОКП[0]); АУП[0] = ЧВ((1-КП[0])=ЮУ[0], НКОКП[0]);
АУЦМ[0] = СВ(ВП(АУУ[0],ЦМ[0]), ВП(АУС[0],ВП(АУС[0],ЦМ[0]))); МП0[0] = МПТ[0]; for(i = 1; i <= N; i++){ КПТ[1] = СВ(КБПТ[1], ЧВ^-КЩфЮКИ, НКОКП[1])); АУС[1] = МВ(МПТ[1], СВ(АУС[1-1], ЧВ^ИЮОД, НКОКП[1]))); АУП[1]=МВ(МПТ[1],СВ(АУП[1-1],ВП(АУУ[1-1],КПТ[1]),ВП(АУС[1-1], ВП(АУС[1-1],КПТ[1])), ЧВ((1-КП[1]), СВ(ЧВ(ОУ[1], НКОКП[1]), ЧВ(2*ОСИ, ВП(АУС[1-1], НКОКП[1])))))); АУУ[1] = МВ(МПТ[1], СВ(АУУ[1-1], ЧВ(КП[1],(СВ(ЧВ(ОУ[1], НКОКП[1]),
ЧВ(ОС[1], ВП(АУС[1-1], НКОКП[1]))))))); АУЦМ[1] = СВ(ВП(АУУ[1], ЦМ[1]), ВП(АУС[1], ВП(АУС[1], ЦМ[1]))); МП0[1] = УМ(МПТЩ, МП0[1-1]) }; /* Вычисление сил и моментов сил */ F[N] = ЧВ(МТр\Т], СВ(АУП[N], РВ(АУЦМр\Т], МВ(МП0[N], [0,-g,0])))); M[N]=СВ(МВ(ТИТ[N],АУУ[N]), ВП(АУС[N], МВ(ТИТр\Т],АУСрЧ])), ВП(ЦМ[N],F[N])); for(i = N-1; i >= 0; i--){ F[i]=СВ(ЧВ(МТ[i], СВ(АУП[1], РВ(АУЦМ[1], МВ(МП0[1], [0,-g,0])))), МВ(МП[1+1], F[i+1]));
M[i] = СВ(МВ(ТИТ[i],АУУ[i]), ВП(АУС[i],МВ(ТИТ[i],АУС[i])), ВЩЦМЩ, F[i]),
Bn(PB(KnT[i+1], ЦМ[i]), MB(Mn[i+1], F[i+1])), MB(Mn[i+1], M[i+1])) }; /* В^гчисление ОДС */
for(i = 0; i <= N; i++) { if(Kn[i] === 0) { ОДС[i] = Cn(HKOKn[i], F[i]) } else { ОДСр] = Cn(HKOKn[i], M[i]) } } return [ОДС,F,M] }; /* умножение матрицы на вектор */
function MB(A, b) {var C = [0,0,0]; for(var i = 0; i <= 2; i++) { for(varj = 0; j <= 2; j++) { C[i] += A[i][j]*b[j] } } return C }; /* умножение матрицы на число */ function M4(A, b) { for(var i = 0; i <= 2; i++) { for(varj = 0; j <= 2; j++) { A[i][j] = A[i][j]*b } } return A }; function YM(A,B) { var C = [[0,0,0],[0,0,0],[0,0,0]]; /* умножение матриц */ for (var k = 0; k <= 2; k++){ for (var i = 0; i <= 2; i++) { for (var j=0; j<=2; j++){ C[i][k]+=A[i][j]*B[j][k] }}} return C }; /* скалярное произведение векторов */ function Cn(a, b) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] }; function Bn(a, b) { /* векторное произведение */ return [a[1]*b[2]-b[1]*a[2], a[2]*b[0]-b[2]*a[0], a[0]*b[1]-b[0]*a[1]] }; /* умножение числа на вектор */
function 4B(num, a) { return [num*a[0], num*a[1], num*a[2]] }; /* сумма векторов */
function CB(){ for(var i = 0, x = 0, y = 0, z = 0; i < arguments.length; i++){ x+=arguments[i][0]; y+=arguments[i][1]; z+=arguments[i][2] } return [x,y,z] }; /* разность двух векторов */
function PB(a, b) { return [a[0]-b[0], a[1]-b[1], a[2]-b[2]] }; function трансп(matrix) { /* транспонирование матрицы */ return matrix[0].map((col, i) => matrix.map(row => row[i])) }
Листинг 2
Верификация УД МС на рис. 3
Listing 2
Verification of the equations of dynamics of manipulation systems in Fig. 3
<html> <head> <script 5ГС="вычислениеОДСо5"> </script> <script> onload = function() { // Bычисление ОДС с помощью ПО var m01=10, m02=8, m03=5, m04=3, m05=2, m06=1, // массы тел // главные центральные моменты инерции тел Icx1=0.6, Icy1=0.9, Icx2=0.4, Icy2=0.8, Icx3=0.6, Icy3=0.3, Icx4=0.5, Icy4=0.2, Icx5=0.4, Icy5=0.1, Icx6=0.3, Icy6=0.1, // расстояния от полюсов тел до их центров масс
A1=0.2, A2=0.3, B3=0.4, B4=0.2, B5=0.1, B6=0.07, L1=0.5, L2=0.5, // обобщенные ускорения, скорости и координаты qtt1=2.3, qtt2=3.2, qtt3=1.3, qtt4=2.1, qtt5=1.1, qtt6=2.1, g=9.81, qt1=3.2, qt2=2.2, qt3=4.1, qt4=2.1, qt5=4.1, qt6=2.1, q1=3.2, q2=2.2, q3=4.1, q4=2.1, q5=1.1, q6=2.1, q12=q1+q2; q13=q12+q3; q23=q2+q3; qt12=qt1+qt2; qt13=qt12+qt3; qtt12=qtt1+qtt2; qtt13=qtt12+qtt3; // списки входных параметров var КП = [1,1,1,0,1,1], MI = [m01,m02,m03,m04,m05,m06], НКОКП = [[0,1,0],[0,1,0],[0,1,0],[0,-1,0],[0,0,1],[0,1,0]], КБПТ = [[0,0,0], [L1,0,0], [L2,0,0], [0,0,0], [0,0,0], [0,0,0]], qM = [[A1,0,0],[A2,0,0],[0,-B3,0],[0,B4,0],[0,-B5,0],[0,-B6,0]], ТИТ=[[[Icx1,0,0],[0,Icy1,0],[0,0,Icy1]], [[Icx2,0,0],[0,Icy2,0],[0,0,Icy2]], [[Icx3,0,0],[0,Icy3,0],[0,0,Icx3]], [[Icx4,0,0],[0,Icy4,0],[0,0,Icx4]], [[Icx5,0,0],[0,Icy5,0],[0,0,Icx5]], [[Icx6,0,0],[0,Icy6,0],[0,0,Icx6]]], OK=[q1,q2,q3,q4,q5,q6], OC=[qt1,qt2,qt3,qt4,qt5,qt6], OY=[qtt1,qtt2,qtt3,qtt4,qtt5,qtt6];
var ответ = СФСТ^КП, НКОКП, КБПТ, MТ, qM, ТИТ, ОК, ОС, ОУ, g);
// Bычисление движущих сил и моментов сил по выписанным формулам m5=m05+m06; m4=m04+m5; m3=m03+m4; m2=m02+m3; // массы подсистем a=m05*B5+m06*B6; b=m02*A2+m3*L2; // параметры статических моментов Ix6=Icx6+m06*B6*B6; Ix5=Icx5+m05*B5*B5; Ic34=Icy3+Icy4; // моменты инерции
^5=1x5+1x6; D5=Icy5-Ix5; D6=Icy6-Ix6; Ы2=1су2+т02*А2*Л2+т3:|Ъ2:,Ъ2; Id1=Icy1+m01*A1*A1+m2*L1*L1; La=L1*a; 1x56=1x5+1x6; D=D5+D6; Id=D6+Ix6; 1у56=1су5+1су6; Ie=Ix56-Iy56-D5-D6; If=D6+Ix6; c1=Math.cos(q1); s1=Math.sin(q1); // косинусы и синусы углов поворота тел с2=МаШ.^№); s2=Math.sin(q2); с3=МаШ.^([3); s3=Math.sin(q3); c5=Math.cos(q5); s5=Math.sin(q5); c6=Math.cos(q6); s6=Math.sin(q6); ^=№.^.^([12); s12=Math.sin(q12); c13=Math.cos(q13); s13=Math.sin(q13); c23=Math.cos(q2з); s23=Math.sin(q2з); // движущие моменты сил
M3=La*s5*(c23*qtt1+c3*qtt12+s23*qt1*qt1+s3*qt12*qt12)+Icy6*c5*qtt6+ (Ic34+Ix56*s5*s5+Iy56*c5*c5)*qtt13+Ie*s5*c5*qt13*qt5-If*s5*qt5*qt6; M2=M3+Id2*qtt12+b*L1*(c2*qtt1+s2*qt1*qt1)+
a*L2*(s5*(c3*qtt13-s3*qt13*qt13)+s3*(c5*qtt5-s5*qt5*qt5)+2*c3*c5*qt13*qt5);
M1=M2+Ы1*qtt1+b*L1*(c2*qtt12-s2*qt12*qt12)+a*L1*(s5*(c23*qtt13-
s23*qt13*qt1з)+s23*(c5*qtt5-s5*qt5*qt5)+2*c23*c5*qt13*qt5);
M5=La*c5*(s23*qtt1+s3*qtt12-c23*qt1*qt1-c3*qt12*qt12)+Ix56*qtt5
-s5*(a*qtt4-(D*c5*qt13+Id*qt6)*qt13)+g*a*s5;
F4=m4*qtt4-a*(s5*qtt5+c5*qt5*qt5)-m4*g; M6=Icy6*(c5*qtt13+qtt6-s5*qt13*qt5); document.getElemenffiyЫ("вывод").innerHTML= // результат верификации "Обобщенные движущие силы:<Ьг/>"+ответ[0] jom("<br/>")+"<br/>M1="+M1+ "<br/>M2="+M2+"<br/>M3="+M3+"<br/>F4="+F4+"<br/>M5="+M5+"<br/>M6="+M6; } <^отрР- </head> <body> <div id="вывод"></drv> </body> </html>
Следовательно, должно совпасть значение Qk, вычисленное функцией СФСТОВ, и значение Ск, вычисленное по верифицируемой формуле. Так проверяется на отсутствие ошибок формула вычисления Ск. Если необходимо проверить формулу вычисления Нк^, то в ^м УД достаточно положить qj > 0 и тогда Qk = + Ск. Для проверки, например, инерционных сил ^го УД,
достаточно считать отличными от нуля только скорости, и так далее. Код верификация УД на рис. 3 приведен в листинге 2.
4. Доказательство утверждений
С целью сокращения записей будем использовать символы ц, принимающие значения на множестве {х,у,г) имен осей СК. Тогда НК орта р1 в НСК будем записывать как р^, где ^ е {х, у, г), а скалярное произведение орта на орт оси 0кцк ГСК(к) будем записывать как
д^. Смешанное произведение орт осей НСК будем коротко записывать в виде ^ х ^ • ( ,
где - символ Леви-Чивиты [19].
Записи формул значительно сокращаются, если использовать знаки суммирования по немым символьным индексам ц, (. Например, формулы (6) можно коротко записать в виде = Ъ ,
К = Ъ , < = Ъ Ук4. _
Из утверждений 2 и 4 статьи [18] следует, что силу действующую на /-е тело со стороны (/ - 1)-го тела, можно вычислить по формуле
Р] = Щ Ц=1 К1 + Щ - ЩЗ.
Тогда, используя разложения ~д = -ду, = Ъ ^Л, Щ = Ъ и определение = р. • получим
Р] = V] • Щ Ъ£=1 Ъ ЩЩ + Р,- • Ъ т^Щ + т]др] • у.
Отсюда с учетом обозначений р1] = р. • л, Е1]. = Ъ\-л следует формула
] ] 111
р] = Ъ + т]) + щдр*.
Развертывая в ней сумму по получим искомую формулу (2).
Из утверждений 1 и 7 статьи [18] следует, что момент силы Mj, действующий на /-е тело со стороны (/-1)-го тела, можно вычислять по формуле
М; = щ х Ъ=1 + Ък=]+1 (ткЁк хЁк + Ккхтк+Шк Х~Ёк) + К} + С},
где Kj = Zk=j (Jok • ~^k)'t; Gj =~g x mj; ]ok - тензор инерции k-го тела в его ГСК с началом в полюсе Ок; шк - абсолютная угловая скорость k-го тела. Для слагаемых формулы вычисления движущего момента силы Mj = ~q. • Mj выведем искомые представления. Для этого разложим векторы qj, тк, Rk по ортам НСК, приведем подобные при ортах НСК и используем символ Леви-Чивиты. Тогда получим
4j • Щ x li=1 Ri = Z<; qj? • Ъп rfv x Jji=1 Zf R¡!, = Z^ q) • ?rnf jji=1 ßf =
= Z<ZV Z? ^q]mi¡R¡.
Аналогично
4j • (™kRk xRk + Rkxmk + mkx Rk) =
= 4j • {шк Zv x % Rll + Ъч Rlri x % + % x Zv Rfii) = = Z^ qj? • (шк % Rl % Rlvxf + % R¡ Zf mlñx^ + % mf Zv R^ xrj) =
= Zv Z? Z< ql(™kRvA + - xГ J.
Подставим эти выражения в формулу Mj = ~q. • Mj. Тогда получим
Mj = Zv % Z< E^ q][m] + !Nk=j+1 (mkRlRl + Rvkrñí-m^kR^k)] + Kj + Gj.
Если в этой формуле развернуть суммы по немым символьным индексам ц, учесть значения символов Леви-Чивиты и обозначения М*+1, Mj+1, Mz+1, то получим искомые формулы (3).
Приведем вектор Kj к оси Ojq., используем диадное представление тензора инерции k-го тела в его ГСК, разложим вектор шк на оси этой ГСК, внесем орты НСК под знак производной по
— ~_ t
времени и используем равенство • Zr¡ шкЦк = шк. Тогда получим
¿7 = 4j •JH=J (Jok • ük)'t_ = 4j • Efe=7 Of /Ш* • Zv_ = = Z< • ZNk=j Z? Jl(fk4yt = % q] ZNk=j Z? Zofe« • fk4rt = ZNk=j Zff Ы(?A)'f
После вычисления производной получим Kj = Zk=j (JokQ'j^k^k + JokQj^k^k). Разложим орт (ffc ГСК(к) по ортам НСК. Тогда получим = Zr¡ ¡ffi. Отсюда имеем
v ^ V— ~7 V С V
= Lr, EVfv шкук^ = Lr, EVfv укШк.
Следовательно,
Z« ^Ife^^fe^f = Zf Zofe^f Z< qj^fc = Zf Zofe^l Z< qj Z, EvSv Vfe^fe =
= Zf Zofc^f ^fc Z< = Eviv qvjku>l = Z¡. ]\кш\ Z
Теперь, учитывая представление = 4j • = ^7fe, получим
^7 = Zfc=7 Zf ¿fc^ffc^f + ^fc ч]кшк).
Раскроем две последние суммы по ^ и ^. Тогда получим
ZfJÍko>lZ EVS<; qffc^fc =
= JZkMkZr! Ецх^ ч]кШк + Jlk^kZV EVVZ Ч}кШк + Jok^kZri Er\z^ ч]кШк =
7x xi У z z У i jУ У f z x x z*\ i
= Jok^k yEzxy Ч]кшк +Eyxz Ч]кшк J + Jok^k\Exyz 4jkMk +Ezyx Ч]кшк) +
+ Jok^ki^yzx 4]kK + Exzy Ч]кШк)ч*к{]%кшкшк - ^к^к^^кШк^кЫк -1%кшкшк) + +Ч)к{]У0кшК -JZk^y)qXk(jzk -Jyok)uykuzk + q^U^J^tofaZ + qjbUZk -JZkX^l
Отсюда с учетом обозначений Ак, Вк, Dk получим
¿7 = Zfc=7 (Z jL^jk^l + +АкЧ*кЫУкш1 + Bkqykuluzk + DkqzjkшiшУk), что после развертывания сумм по ^ совпадает с формулой
Kj = Zfc=; fajfc(/ífcü>fc + Акш%ш*к) + qyk(Jyok(byk + Bkuxkuzk)+qzjk(Jzok(bzk + Д^^)].
Если воспользоваться введенными в утверждении 1 обозначениями, то формула вычисления Kjk примет искомый вид (4).
Приведем момент силы тяжести Gj к оси Oj~q.. Тогда получим
gJ = 4j ■ = 4j-9 xщ = -зЪ qj?-yxlv rfv = д^Ъц 4*jy x 1'
7 ~
т. е. Gj = дЪ$ Ъ Е-у^ц 4jmj . Если в этой формуле развернуть суммы по немым символьным индексам и учесть значения символов Леви-Чивиты, то получим искомую формулу (5). Если = то по формуле (3) получим
Mj-i = - + tif) + qy{m^_1Rxlj_1 - m^R^ + Му) +
+qZj(m:f_1Rlj_1 - т/.Л*-! + **?) + Kj-i + Gj_1.
Прибавим и отнимем в этом выражении величины, необходимые для выделения в нем слагаемых правой части формулы вычисления Mj. Тогда получим:
MJ.1 = q*[(rny + ту_х - ту)(Щ - Щ) - (т? + - т*)Щ-Ъу) + М*+1 + +Mxj] + qy[(m? + т*_г - т?)Щ - R*) - (mf+m?^ - т?)Щ- - Rf) + Му+г + Myj] + +q][(m? + - m_?)(Ryr'Ry) - (my + m]_x - - R?) + M*+1 + Mzj] + Kj +
+Kj_1j_1+Gj + Gj_t - Gj,
где
Mx] = m.j(RyR? - R*Ry) + myRf - Rfm? + Rymf - mfRy, Myj = mj(RjZRX - RfRf) + mfRf - Rfm* + Rfmf - mfRf, Mzj = mj(R*Ry - RyR?) + m?Ry - Rym* + R*my - туЦ. Таким образом,
Mj_1 = Mj + qX[-myRjZ + (ту_± - ту)Щ - Щ) + m*Ry -
-(mj-i - - Щ) + MXJ] + q?[-m?R? + - - R*) +
+m?R? - - m?)(RZj - R;z) + Му]]+Ч][-т? Ry + - m^)(Rylj - Ry) + +туЩ - (ту_г - т*)(Щ - R?) + Mzj] + + GJ.1 - Gj.
Отсюда после подстановки выражений MXj, Myj, MZj, Gj-1, Gj и очевидных сокращений получим формулу утверждения 2.
Заключение
В данной работе впервые для МС с шестью степенями свободы в пространстве выписаны УД в аналитическом виде с явно выраженными геометрическими, кинематическими, статическими и инерционными параметрами тел. Причем сократить количество математических операций в этих УД практически невозможно, т. е. они оптимальны в смысле быстродействия вычислений. Еще недавно такой результат был практически не достижимым. Но методы математического моделирования МС развиваются и предоставляют новые возможности аналитического исследования МС.
Работа выполнена при поддержке РФФИ (проект 20-41-740019).
Литература
1. Adaptive Fractional PID Controller for Robot Manipulator / H. Delavari, R. Ghaderi, A.N. Ranjbar et al. // Proceedings of FDA '10. The 4th IFAC Workshop Fractional Differentiation and its Applications. Badajoz, Spain, October 18-20. - 2010. - P. 1-7.
2. Jafarov, E.M. A new variable structure PID-controller for robot manipulators with parameter perturbations: an augmented sliding surface approach / Elbrous M. Jafarov, Yorgo Istefanopulos, M.N. Alpaslan Parlakci // 15th Triennial World Congress, Barcelona, Spain. - 2002. - P. 365-370.
3. Xu, J. Robust Adaptive PID Control of Robot Manipulator with Bounded Disturbances/ Jian Xu, Lei Qiao // Hindawi Publishing Corporation Mathematical Problems in Engineering. - 2013. -Vol. 2013. - Article ID 535437. - P. 1-13. DOI: 10.1155/2013/535437
4. Chunqing, H. PID feedback for mixed Hi/Hco tracking control of robotic manipulators / Huang Chunqing, Shi Songjiao // Journal of Systems Engineering and Electronics. - 2004 - Vol. 15, no. 4. -P. 579-585.
5. Velocity Control of a Spherical Rolling Robot Using a Grey-PID Type Fuzzy Controller With an Adaptive Step Size / Erkan Kayacan, Erdal Kayacan, Herman Ramon, Wouter Saeys // 10th IFAC Symposium on Robot Control International Federation of Automatic Control. Dubrovnik, Croatia. -2012. - P. 863-868.
6. Ghaleb, N.M. Modeling and Control of 2-DOF Robot Arm / Nasr M. Ghaleb, Ayman A. Aly // International Journal of Emerging Engineering Research and Technology. - 2018. - Vol. 6, iss. 11. -P. 24-31.
7. Amin, A.T.M. Adaptive controller algorithm for 2-DOF humanoid robot arm / Adam Tan Mohd Amin, Abdul Hakim Ab Rahim, Cheng Yee Low //Procedia Technology. - 2014. - Vol. 15. - P. 765-774. DOI: 10.1016/j.protcy.2014.09.049
8. Телегин, А.И. Уравнения математических моделей механических систем. - Челябинск: Изд-во ЮУрГУ, 1999. - 181 с.
9. Телегин, А.И. Алгоритмы решения первой задачи динамики для плоских рычажных механизмов / А.И. Телегин, М.В. Тимощенко // Вестник ЮУрГУ. Серия «Машиностроение». -2007. - Вып. 10, № 25 (97). - С. 12-22.
10. Elshabasy, M. Power optimization of planar redundant manipulator moving along constrained-end trajectory using hybrid techniques / M. Elshabasy, K.T. Mohamed, A.A. Ata // Alexandria Engineering Journal. - 2017. - vol. 56, iss. 4. - P. 439-447. DOI: 10.1016/j.aej.2017.01.040
11. Dynamic characterization and simulation of two-link soft robot arm with pneumatic muscles / A. Hosovsky, J. Pitel, K. Zidek et al. //Mechanism and Machine Theory. - 2016. - No. 103. - P. 98-116. DOI: 10.1016/j .mechmachtheory. 2016.04.013
12. Korayem, M.H. Dynamic modeling of nonholonomic wheeled mobile manipulators with elastic joints using recursive Gibbs-Appell formulation / M.H. Korayem, A.M. Shafei, H.R. Shafei // Scientia Iranica. - 2012. - vol. 19, iss. 4. - P. 1092-1104. DOI: 10.1016/j.scient.2012.05.001
13. Propulsion Effect Analysis of 3Dof Robot under Gravity / A. Shala, R. Likaj, M. Bruqi, X. Bajrami //Procedia Engineering. - 2015. - Vol. 100. - P. 206-212.
14. Sadati, S.M.H. An Automatic Algorithm to Derive Linear Vector Form of Lagrangian Equation of Motion with Collision and Constraint / S.M.H. Sadati, S.E. Naghibi, M. Naraghi // Procedia Computer Science. - 2015. - Vol. 76. - P. 217-222. DOI: 10.1016/j.procs.2015.12.345
15. Fontes, J.V. On the dynamic performance ofparallel kinematic manipulators with actuation and kinematic redundancies / J.V. Fontes, M.M. da Silva // Mechanism and Machine Theory. - 2016. -Vol. 103. - P. 148-166. DOI: 10.1016/j.mechmachtheory.2016.05.004
16. Amin, A.T.M. Adaptive controller algorithm for 2-DOF humanoid robot arm / A.T.M. Amin, A.H.A. Rahim, C.Y. Low // Procedia Technology. - 2014. - Vol. 15. - P. 765-774.
17. Lewis, F.L. Robot Manipulator Control: Theory and Practice / F.L. Lewis, D.M. Dawson, C.T. Abdallah. - Marcel Dekker, Inc., New York, 2004. - 614 p.
18. Телегин, А.И. Новый векторный вид уравнений динамики систем тел / А.И. Телегин // Вестник ЮУрГУ, серия «Машиностроение». - 2014. - Т. 14, № 1. - С. 33-40.
19. Лурье, А.И. Аналитическая механика / А.И. Лурье. - М. : Физматгиз, 1961. - 824 с.
20. Войнов, И.В. Maxima-код вывода динамических реакций и обобщенных движущих сил в сочленениях роботов-манипуляторов / В.И. Войнов, А.И. Телегин, Д.Н. Тимофеев // IVМеждународная научно-техническая конференция «Пром-Инжиниринг». - 2018. - C. 265-269.
Телегин Александр Иванович, д-р физ.-мат. наук, профессор, профессор кафедры автоматики, Южно-Уральский государственный университет, филиал в г. Миассе, г. Миасс, teleginai@susu.ru.
Поступила вредакцияю 28 апреля 2021 г.
DOI: 10.14529/ctcr210405
FORMALISM OF WRITING OUT OF MANIPULATORS DYNAMIC EQUATION
A.I. Telegin, teleginai@susu.ru
South Ural State University, Miass, Russian Federation
The problem of cumbersome equations of dynamics for manipulation systems of industrial robots (manipulators) with translational and rotational joints is solved. A new formalism for writing out the equations of dynamics of manipulators by using of guide cosines is proposed. Examples of writing out equations of dynamics of manipulators with guid cosines are given. The equations of dynamics in relative angles of rotation of bodies are obtained with the help of these guide cosines by applying their properties. These manipulators have from three to six degrees of freedom. In their equations of dynamics the geometric, kinematic, static and inertial parameters are explicit. The multipliers for accelerations and products of velocities are optimal in the sense of the minimum of arithmetic operations (additions and multiplications) that are needed for their calculations in the written out equations of dynamics. JS-code and method for verification of the equations of dynamics of manipulators written in analytical form are proposed. The problem is that when the equations are written out manually, errors and oversights in the intermediate entries and the final result are possible. Therefore it is necessary to check the results of writing out for absence of errors, i.e. to perform verification of formulas for calculation of constitutive equations of dynamics. To do this, we can use software designed to calculate the generalized driving forces of manipulators, i.e. to solve the first problem of dynamics. Such software is offered as a web-application, in which JS-function is used for verification of the equations of dynamics of manipulators. The method of verification of formulas to calculate the generalized forces of gravity and multipliers (coefficients) for generalized accelerations and products of generalized velocities in the equations of dynamics is developed. An example of verification of the equations of dynamics of the universal manipulator with six degrees of freedom in space is given. Aim. The aim of research is to develop a formalism for writing out the analytical form of the equations of the manipulators' dynamics in the guide cosines of the principal axes of the coupled body coordinate systems, whose coefficients contain the minimum number of arithmetic operations. Research methods. The methods of research refer to vector and analytic mechanics of absolutely solid systems, to vector algebra, and to systems analysis and programming in scripting languages. Results. The results contain two proved statements, in which there are the formulas and the methodology that allow us to write manually the equations of dynamics of manipulators with three and six degrees of mobility both in guiding cosines and in generalized coordinates. In both cases it is impossible to simplify the obtained equations. Conclusion. The offered analytical types of the equations of dynamics occupy several lines. By the known classical formalisms (Lagrange, Appel, Nielsen, Newton-Euler, etc.) it is practically impossible to obtain similar results because of the large number of complex mathematical operations in their implementation and the cumbersome-ness of the resulting formulas.
Keywords: industrial robot, equations of dynamics, prescription formalism, guid cosines, formula reuse, JS-function, equations verification.
References
1. Delavari H., Ghaderi R., Ranjbar N.A., HosseinNia S.H., Momani S. Adaptive Fractional PID Controller for Robot Manipulator. Proceedings of FDA'10. The 4th IFAC Workshop Fractional Differentiation and its Applications. Badajoz, Spain, October 18-20, 2010, pp. 1-7.
2. Jafarov Elbrous M., Istefanopulos Yorgo, Parlakci Mehmet Nur Alpaslan. A new variable structure PID-controller for robot manipulators with parameter perturbations: an augmented sliding surface approach. 15th Triennial World Congress, Barcelona, Spain, 2002, pp. 365-370.
3. Xu Jian, Qiao Lei. Robust Adaptive PID Control of Robot Manipulator with Bounded Disturbances. Hindawi Publishing Corporation Mathematical Problems in Engineering, 2013, vol. 2013, article ID 535437, pp. 1-13. DOI: 10.1155/2013/535437
4. Chunqing Huang, Songjiao Shi. PID feedback for mixed Hi/Hco tracking control of robotic manipulators. Journal of Systems Engineering and Electronics, 2004, vol. 15, no. 4, pp. 579-585.
5. Kayacan Erkan, Kayacan Erdal, Ramon Herman, Saeys Wouter. Velocity Control of a Spherical Rolling Robot Using a Grey-PID Type Fuzzy Controller With an Adaptive Step Size. 10th IFAC Symposium on Robot Control International Federation of Automatic Control. Dubrovnik, Croatia, September 5-7, 2012, pp. 863-868.
6. Nasr M. Ghaleb, Ayman A. Aly. Modeling and Control of 2-DOF Robot Arm. International Journal of Emerging Engineering Research and Technology, 2018, vol. 6, iss. 11, pp. 24-31.
7. Amin Adam Tan Mohd, Ab Rahim Abdul Hakim, Cheng Yee Low. Adaptive controller algorithm for 2-DOF humanoid robot arm. Procedia Technology, 2014, vol. 15, pp. 765-774. DOI: 10.1016/j.protcy.2014.09.049
8. Telegin A.I. Uravneniya matematicheskikh modeley mekhanicheskikh sistem [Equations of mathematical models of mechanical systems]. Chelyabinsk, South Ural St. Univ. Publ., 1999. 181 p.
9. Telegin A.I., Timoshchenko M.V. Algoritmy resheniya pervoy zadachi dinamiki dlya ploskikh rychazhnykh mekhanizmov [Algorithms for solving the first problem of dynamics for flat lever mechanisms]. Bulletin of the South Ural State University. Ser. Mechanical engineering industry, 2007, iss. 10, no. 25 (97), pp. 12-22. (in Russ.)
10. Elshabasy M.M.Y.B., Mohamed K.T., Ata A.A. Power optimization of planar redundant manipulator moving along constrained-end trajectory using hybrid. Alexandria Engineering Journal, 2017, vol. 56, iss. 4, pp. 439-447. DOI: 10.1016/j.aej.2017.01.040
11. Hosovsky A., Pitel' J., Zidek K., Tothova M., Sarosi J., Cveticanin L. Dynamic characterization and simulation of two-link soft robot. Mechanism and Machine Theory, 2016, no. 103, pp. 98-116. DOI: 10.1016/j.mechmachtheory.2016.04.013
12. Korayem M.H., Shafei A.M., Shafei H.R. Dynamic modeling of nonholonomic wheeled mobile manipulators with elastic joints using recursive Gibbs-Appell formulation. Scientia Iranica, 2012, vol. 19, iss. 4, pp. 1092-1104. DOI: 10.1016/j.scient.2012.05.001
13. Shala A., Likaj R., Bruqi M., Bajrami X. Propulsion Effect Analysis of 3Dof Robot under Gravity. Procedia Engineering, 2015, vol. 100, pp. 206-212.
14. Sadati S.M.H., Naghibi S.E., Naraghi M. An Automatic Algorithm to Derive Linear Vector Form of Lagrangian Equation of Motion with Collision and Constraint. Procedia Computer Science, 2015, vol. 76, pp. 217-222. DOI: 10.1016/j.procs.2015.12.345
15. Fontes J.V., da Silva M.M. On the dynamic performance of parallel kinematic manipulators with actuation and kinematic redundancies. Mechanism and Machine Theory, 2016, vol. 103, pp. 148-166. DOI: 10.1016/j.mechmachtheory.2016.05.004
16. Amin A.T.M., Rahim A.H.A., Low C.Y. Adaptive controller algorithm for 2-DOF humanoid robot arm. Procedia Technology, 2014, vol. 15, pp. 765-774.
17. Lewis F.L., Dawson D.M., Abdallah C.T. Robot Manipulator Control: Theory and Practice, Marcel Dekker, Inc., New York, 2004. 614 p.
18. Telegin A.I. [A new vector form of the equations of dynamics of body systems]. Bulletin of the South Ural State University. Ser. Mechanical engineering industry, 2014, vol. 14, no. 1, pp. 33-40. (in Russ).
19. Lurie A.I. Analiticheskaya mekhanika [Analytical Mechanics]. Moscow, Fizmatgiz Publ., 1961. 824 p.
20. Voinov I.V., Telegin A.I., Timofeev D.I. Maxima-Code for Derivation of Dynamic Reactions and Generalized Driving Forces in the Joints of Robotic Manipulators. IVInternational Conference on Industrial Engineering, 2018, pp. 265-269. (in Russ.)
Received 28 April 2021
ОБРАЗЕЦ ЦИТИРОВАНИЯ
FOR CITATION
Телегин, А.И. Формализм выписывания уравнения динамики манипуляторов / А.И. Телегин // Вестник ЮУрГУ. Серия «Компьютерные технологии, управление, радиоэлектроника». - 2021. - Т. 21, № 4. -С. 52-68. DOI: 10.14529Ма210405
Telegin A.I. Formalism of Writing Out of Manipulators Dynamic Equation. Bulletin of the South Ural State University. Ser. Computer Technologies, Automatic Control, Radio Electronics, 2021, vol. 21, no. 4, pp. 52-68. (in Russ.) DOI: 10.14529/ctcr210405
