Математическое описание кинематики и динамики исполнительных механизмов роботов с древовидной кинематической структурой

№ 11

2008

621.391.535.8

МАТЕМАТИЧЕСКОЕ ОПИСАНИЕ КИНЕМАТИКИ. И ДИНАМИКИ ИСПОЛНИТЕЛЬНЫХ МЕХАНИЗМОВ РОБОТОВ С ДРЕВОВИДНОЙ КИНЕМАТИЧЕСКОЙ

СТРУКТУРОЙ

Канд. техн. наук. доц. А.К. КОВАЛЬЧУК, ст. препод. Д.Б. КУЛАКОВ. канд. техн. наук, доц. С.Е. СЕМЕНОВ

Представлен универсальный способ математического описания кинематики и динамики древовидных исполнительных механизмов роботов. Предложено математическое описание кинематической структуры с использованием элементов теории графов и специальных индексных функций. Приведены рекуррентные уравнения кинематики и динамики древовидных исполнительных механизмов роботов.

В настоящее время во всем мире активно ведутся работы по созданию различных средств экстремальной робототехники, исполнительные механизмы (ИМ) которых, как правило, не закреплены на неподвижном основании и часто имеют древовидную структуру. Наиболее яркими представителями роботов с такими сложными исполнительными механизмами являются шагающие роботы. Актуальной является задача математического обеспечения этих разработок. Необходимы адекватные и эффективные с вычислительной точки зрения математические модели, алгоритмы управления. Важно, чтобы и форма записи соответствующих математических выражений была компактной, наглядной и удобной.

Как правило, для математического описания ИМ роботов используют специальные системы координат Денавита-Хартеберга и рекурсивные алгоритмы,что идеально приспособлено для промышленных роботов и различных манипуляторов, обладающих кинематической структурой типа "цепь", когда каждое звено имеет одно предыдущее и не более одного следующего звена. В этом случае каждое звено может быть однозначно

обозначено своим порядковым номером в цепи, начиная от основания, а рекурсивные алгоритмы соответствуют простому перебору номеров в прямом и обратном порядке.

Матричное уравнение динамики таких ИМ можно представить в следующем виде:

I -J J О

7Л/.Л ГН\

+

ч * / А

где

I ~ матрица инерции ИМ;

вектор обобщенных ускорений; Н- матрица-столбец приведенных центробежных, кориолисовых и внешних сил; ъ - вектор шарнирных моментов и сил,

матрица Якоби наложенных на ИМ дополнительных внешних связей, И - вектор реакций дополнительных внешних связей, Р - вектор-столбец уравнений связей.

Однако многие роботы (в том числе шагающие) содержат несколько кинематических цепей. Их число не менее числа конечностей. Соединены они, как правило, в структуру типа "дерево". При этом утрачивает смысл простой порядковый номер, а структура рекурсивных алгоритмов становится значительно более сложной. Опыт авторов, с 1985 г. ведущих НИР в области создания двуногих шагающих роботов (ДШР), показывает, что не только компактность и "читаемость" уравнений кинематики и динамики робота, но и трудоемкость построенных на их основе алгоритмов существенно зависит от способа, которым обозначается каждое звено, а также соответствующие ему переменные и константы.

Авторами предложена система нумерации звеньев и обозначения переменных, позволяющая не только сделать запись математических выражений более компактной, но и использовать при формировании соответствующих алгоритмов элементы теории графов.

Представим исполнительный механизм как корневое ориентированное дерево. Звенья в таком графе являются вершинами, а соединяющие их сочленения - ребрами. Ребра ориентированы от корня к листьям (конечным звеньям цепей, составляющих кинематическое дерево). Корнем дерева будем считать какую-либо точку, фиксированную в абсолютной системе координат. В случае, если исполнительный механизм не закреплен на неподвижном основании, искусственно введем фиктивную кинематическую цепь

(ствол дерева), привязывающую какое-либо звено реального механизма к условной неподвижной стойке. Ствол состоит из пяти невесомых звеньев, соединенных между собой, со стойкой и ИМ тремя поступательными и тремя вращательными кинематическими парами пятого класса. Соответствующие шарнирные силы и моменты заведомо равны нулю. Таким образом, наличие ствола не оказывает влияния на движение ИМ, но позволяет "привязать" движение ИМ к абсолютной системе координат, используя традиционные для робототехники методы.

Для работы с ИМ как с деревом используем следующие принятые в теории графов функции и термины:

dg+(i) - полустепень исхода звена (вершины) г\ определяет количество звеньев-сыновей звена i;

Dnum(i) - определяет глубинный номер звена i (т.е. его порядковый номер при поиске в глубину от корня дерева);

D - матрица достижимости. Если звено i достижимо из звена j\ т.е. имеется ориентированный путь из вершины j в вершину /, df/=l, иначе d//=0. Звено i достижимо из самого себя, т. е. по диагонали матрицы достижимости стоят 1. Обозначим: L={1, 2, N} - неупорядоченное множество, элементами которого являются номера звеньев ИМ;

Г(/) - кортеж номеров звеньев, являющихся для звена i следующими (звеньями-сыновьями); кортеж содержит dg+(/) элементов.

ai £ {ОД} множитель, определяющий тип сочленения звена i со звеном-отцом;

<7. = 1 - тип сочленения звеньев - вращательный шарнир;

<у{ =0 - тип сочленения звеньев - телескопический шарнир.

Введем следующие функции: Опыт k(i) - глубинный номер звена i при поиске в глубину от звена к\ s(i,k) - определяет номер звена, являющегося км следующим звеном {к-й вершиной-сыном) для звена i;

/(f) - номер звена, являющегося предыдущим (вершиной-отцом) для звена г;

ns(i) - определяет, каким по счету следующим звеном (из числа непосредственно

присоединенных) является звено / для своего предыдущего звена;

nan(i) - определяет количество предков звена /;

an(ti) ~ определяет номер у-го от корня предка звена i;

Эти функции будем использовать для определения порядка обхода дерева при организации вычислений, а также в качестве индексов при обозначении физических величин, относящихся к конкретным звеньям.

Для записи уравнений механики исполнительных механизмов в робототехнике принято с каждым звеном связывать свою систему координат, и чаще всего это системы координат Денавита-Хартенберга. Однако при описании древовидных механизмов одной связанной системы координат может оказаться не достаточно. Если звено ветвящееся, то к нему посредством сочленений присоединено несколько следующих звеньев. В соответствии с правилами Денавити-Хартенберга ось ъ\ должна совпадать с осями этих сочленений, поэтому свяжем с ветвящимся звеном столько систем координат, сколько следующих звеньев у него есть (но не менее одной). Первую из них назначим основной, остальные - вспомогательными (рис.1).

Рис Л. Обозначение при помощи индексных функций номеров звеньев, присоединенных к звену /

Компактность и наглядность записи уравнений существенно зависит от способа индексации. В робототехнике известен способ, когда у каждой переменной может быть три индекса. Правый нижний указывает номер звена, к которому относится данная величина, левый верхний - номер системы координат, в которой она представлена, а правый

№11

2008

верхний - специальный символ (например знак транспонирования). Если требуется сослаться на предыдущее или последующее звенья, то пишут, например, Тм или 7м. В случае древовидного исполнительного механизма такая запись теряет смысл, так как у звена может быть несколько следующих звеньев. Некоторые исследователи вводят дополнительные символы для того, чтобы указать, какой именно конечности робота соответствует данная кинематическая цепь. Записывают столько рекуррентных выражений, сколько кинематических цепей имеется у рассматриваемого робота. В любом случае структура уравнений становится значительно более сложной, чем для простой кинематической цепи, хотя физические явления, которым эти уравнения соответствуют, остаются теми же. Существенно, что программы, написанные по таким уравнениям, могут наследовать избыточную сложность, из-за чего становятся более громоздкими и менее быстродействующими. Важно, что становится невозможной универсальная запись уравнений для всех видов роботов с древовидной кинематикой и построение на их основе компактного универсального программного обеспечения. Использование предложенной авторами системы индексации совместно с элементами теории графов позволяет преодолеть перечисленные трудности.

В целях упрощения записи уравнений и улучшения их наглядности предлагается:

- условной стойке присвоить номер О,

- звенья ствола (фиктивной кинематической цепи), если робот не закреплен на неподвижном основании, пронумеровать от стойки последовательно от 1 до 5;

- первые три степени ствола - поступательные, остальные вращательные;

реальные звенья кинематических цепей, составляющих дерево ИМ нумеровать без пропусков в возрастающем порядке от корня; при этом собственный номер звена всегда меньше номера любого звена-потомка;

номер обобщенной координаты, как и номер соответствующего сочленения, такой же, как и у звена, присоединяемого этим сочленением к предыдуще-

Обобщенными координатами исполнительных механизмов роботов принято считать углы во вращательных и перемещения в поступательных сочленениях. Номер обобщенной координаты, как и номер соответствующего сочленения, такой же, как и у звена, присоединяемого этим сочленением к предыдущему звену. Теперь, для того чтобы обозначить какую-либо величину х, относящуюся к предыдущему или последующему по дереву звену, используем записи х/а> и где номер звена, по отношению к

му.

№11

2008

которому задаются последующее и предыдущее, а у - порядковый номер последующего звена в списке непосредственно присоединенных к звену / следующих звеньев; у€Г(/).

Векторные и тензорные величины могут задаваться в различных СК. В какой именно СК задана данная величина, указывает его левый верхний индекс. Если этот индекс отсутствует или нулевой, то величина задана в базовой СК; если указано некоторое целое отличное от нуля число ¿, то вектор задан в основной СК звена номером которого является указанное число. Если в качестве индекса стоит с, то он задан в основной связанной системе, относящейся к звену, к которому относится данная величина. Если два числа разделенные запятой - 1']Х , то вектор задан в р)и вспомогательной СК звена г (у = 2..^+(/)).

Как и принято в робототехнике, с каждым звеном г свяжем матрицу А,- перехода к системе координат предыдущего звена и матрицу Т, перехода к абсолютной системе. Но если сохранить традиционные правила вычисления матриц А/, то в случае, если предыдущее по дереву звено является ветвящимся, то при помощи этой матрицы произойдет переход к одной из нескольких систем координат, связанных с предыдущим звеном. Возможно, что это будет вспомогательная система. Чтобы перейти к основной системе предыдущего звена, надо использовать дополнительную постоянную матрицу Мд, где у - номер предыдущего звена, а к ~ порядковый номер звена г в списке следующих для звенау звеньем. Таким образом, с учетом введенных выше индексных функций

Эта запись будет универсальной, если для всех звеньев, в том числе и не ветвящихся,

А

М,Х=Е

В блочном виде

/

\

'¿.«¿

0 0 0 1

г

\

4 =

0 0 0 1

№ И

2008

— Тцк) 'МГ(ки,,(к) • Ак

Ч

х0 0 0 1

Запишем основные кинематические соотношения для исполнительного механизма в целом.

В абсолютной системе координат

= + 1и. + (1 - ; = 1 + (О

"и = «/

ае1=а1 + Цхс1 + Щх(Щхс1) ] =

В связанных системах координат

хСЩх-%), ] = 1-Д?+(0,

с +с£,.хг с;+''Щ х(с х( с)) где 11Л - вектор, соединяющий начало соответствующей звену г СК предыдущего звена, с началом к~и СК звена / (к 6 {\,2,...dg+{Г)}), с, - вектор положения центра масс звена, Щ. - угловая скорость звена I, - угловое ускорение звена ¡', V, . - скорость начала у'-й системы координат звена /, я(- ускорение началау'-й системы координат звена г, аы - ускорение центра масс звена г.

№ 11

В этих выражениях звено с номером 0

i = Опыт'1 (п)\п = 1

где N- полное число степеней подвижности ИМ.

В порядке обратной рекурсии вычисляются силы и моменты, приложенные к звеньям, а также нагрузки на приводы.

В абсолютных системах координат сила, действующая со стороны звена / на предыдущее звено

fi = Е fslU) + (8-~ äci )mi + Feni

Момент, действующий со стороны звена / на предыдущее звено

_ dg+(i)_ _ _

Mi = Е м*т + кк х Л«.« ~ Л Д- ~ Щ х ¿сД + м«>« + к-1

где Fan - внешние силы, действующие на звено

Мвн - моменты внешних сил относительно центра тяжести звена i и действующие на это звено.

Моменты (силы), развиваемые приводами,

В связанных системах координат сила, действующая со стороны звена / на предыдущее звено, и выраженная в соответствующей звену / вспомогательной системе координат звенаДО (система ДО,ns(0) вычисляется следующим образом:

(<tg* а) \

f(i),ns(i) г _/(/),ns(i)p V*P , / г)7 — с— V

\ k=l J

Момент, действующий со стороны звена i на предыдущее

_ dg +

ш\та)м fd),nmR(Y(iR ijcjr cj ,

Ar—1 '

Ч, 4: ~^xcJcl сщ +cMeHi+% x {{Rjg~cäci )mi VFeili))

Моменты (силы), развиваемые приводами,

Т. = ~/7  (от. Я'^'-'м, + (1 - (7,- ) м0 "лт/;),

где *=(0Д1)Т

Эти вычисления производятся в обратном порядке перечисления глубинных номеров

I = Опшп~] {п)\п =

Элементы матрицы инерции вычислим как шарнирные моменты (силы), вызывающие единичные ускорения по соответствующим обобщенным координатам. Матрица инерции механизма симметричная, однако в случае древовидного исполнительного механизма невозможно вычислить просто одну из треугольных подматриц, т.к. звенья механизма и соответствующие обобщенные координаты пронумерованы не по порядку.

Для расчета г-го столбца положим /-е обобщенное ускорение равным 1, а остальные равными 0. Обобщенные скорости также примем равными 0.

Далее, используя уравнения прямой рекурсии, рассчитаем необходимые угловые и линейные ускорения для звеньев.

...../>

где е,;,-- элемент с номерами

+

Если использовать свойство симметричности матрицы инерции, то порядок перебора индексов следующий:

/ = 1,.. ЛУ, ] = Опигщх{п) - 1,...тах Впипц

В обратной рекурсии вычисляются силы и моменты, действующие на звенья, а также шарнирные моменты (силы).

Сила, действующая со стороны звена / на предыдущее (/'( /)) звено, и выраженная в системе/(Ода (/), вычисляется следующим образом:

\

Момент, действующий со стороны звена / на предыдущее (/"(/) ) звено, и выраженный, как и сила, в соответствующей звену / вспомогательной системе координат звена/(0, будет

/(

А-1

-Ус,с£Гсс.хсас1т{)

Тогда элементы матрицы инерции

ц = -Л • (¿т, "'^М, + (1 - <гУи)**и)1),

Эти вычисления производятся в обратном порядке перечисления глубинных номеров, с учетом симметричности матрицы инерции

] = Вшт~х{п\ п =

Далее значения вычисленных элементов матрицы инерции присваиваются симметричным им элементам, оставшиеся не вычисленными элементы обнуляются.

Для расчета элементов матрицы-столбца Н центробежных, кориолисовых и внешних сил в прямой рекурсии необходимо найти угловые и линейные ускорения звеньев при равных нулю обобщенных ускорениях.

'а^Щ+'е^+'Щ х(сЩхсс,) ; = +

Порядок перечисления индексов г = Впит(и);п = 1,...#,

В порядке обратной рекурсии вычисляются силы и моменты, приложенные к звеньям, а также шарнирные моменты (силы), которые и равны элементам матрицы Я.

Я0./м(0 £ _/(0.™(0

V *=1 ;

Л( = -Л  (<т; . + (1 - ст.) /»^(/^) 5

Порядок перечисления индексов I = Опыт ' (п), П = N

Предложенный подход позволяет также вычислить элементы матриц уравнений внешних связей и может быть использован при создании алгоритмов управления роботами с древовидными ИМ.

Если исполнительный механизм не закреплен к неподвижному основанию, то шарнирные силы и моменты в стволе кинематического дерева заведомо равны нулю и структура матричного уравнения динамики древовидных исполнительных механизмов примет следующий вид;

I,, Гп, \ / •• Л <7/ Г (Г

1т -к Чк + — Т

Л 0 ) ^р) к

где

/// - часть матрицы инерции, соответствующая фиктивной кинематической цепи (стволу дерева),

„ часть матрицы инерции, определяющая взаимовлияние фиктивных и реальных степеней подвижности механизма,

/ю* - часть матрицы инерции, соответствующая реальной части дерева, 7/ и /и - части матрицы уравнений внешних связей,

<?/ и части вектора обобщенных ускорений механизма, соответствующие фиктивной кинематической цепи и реальному механизму,

Я7 и Як - части матрицы-столбца приведенных центробежных, кориолисовых, весовых и внешних сил, действующих на звенья механизма, г - вектор сил и моментов, развиваемых приводами.

Авторы использовали данный подход при создании лабораторного ДШР. Была разработана математическая модель исполнительного механизма, оснащенного электрогидравлическими следящими приводами. В данной модели порядок рекурсивных вычислений формируется автоматически по заданной структуре механизма. Это позволяет использовать ее для шагающих машин с различными кинематическими схемами. Также разработано программное обеспечение системы управления робота, позволяющее ему реализовать статическую и динамическую ходьбу, а также стабилизировать движение по сигналам системы ориентации и адаптироваться к отклонениям опорной поверхности от прогнозируемой формы.

№11

2008

СПИСОК ЛИТЕРАТУРЫ

1. Фу К., Гоисалес Р., Ли К Робототехника: Пер. с англ. под ред. Б.Г. Градецкого. М.: Мир, 1989.

2. Зенкевич СЛ., Ющенко ЛС. Основы управления манипуляционными роботами: Учеб, для вузов. - М. Издательство МГТУ им. Н.Э. Баумана, 2004 г.

3. Ковальчук А.К, Семенов С.Е, Кинематический алгоритм управления движением двуногого шагающего робота //Вестник МГТУ. Приборостроение - М.: Изд. МГТУ. 1996 г. № I.