Рекурсивный метод поиска базовых инерционных параметров манипуляционных механизмов Текст научной статьи по специальности «Математика»

МЕХАТРОНИКА И РОБОТОТЕХНИКА ¡

УДК 531.396, 681.5.015

С. Л. Крутиков

РЕКУРСИВНЫЙ МЕТОД ПОИСКА БАЗОВЫХ ИНЕРЦИОННЫХ ПАРАМЕТРОВ МАНИПУЛЯЦИОННЫХ МЕХАНИЗМОВ

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

E-mail: serrk@yandex.ru

Ключевые слова: базовые инерционные параметры, динамическая модель манипулятора, уравнения движения в форме Лагранжа, манипулятор PUMA 560.

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

Управление, а зачастую и моделирование, является задачей реального времени, т.е. необходимо выполнять достаточно непростые расчеты десятки, а то и сотни раз в секунду. Использование менее мощных вычислительных устройств, способных выполнить те же расчеты за требуемое время, позволит упростить и удешевить систему. Также существуют задачи, при решении которых не обойтись без идентификации. Вот лишь некоторые из них: создание программных имитаторов существующих манипуляторов2 [1], разработка новых систем управления для старых роботов, моментное управление манипулятором с адаптацией его динамической модели.

1 Имеются в виду 10 независимых параметров для каждого звена: масса, координаты центра масс и осевые и центробежные моменты инерции. В дальнейшем будем называть их элементарными инерционными параметрами.

2Такие системы могут применяться, например, для тестирования системы управления роботом или для обучения операторов.

Для разрешения неоднозначности вводится множество базовых инерционных параметров, которое представляет собой наименьший набор параметров, определяющих уравнения динамики и соответствующих им взаимно-однозначно3, и в дальнейшем используется запись этих уравнений в терминах именно базовых параметров. Отдельной и весьма непростой задачей является поиск такого множества параметров и их выражение через обычные инерционные параметры, а также запись уравнений движения с помощью базовых инерционных параметров. Этим вопросам посвящено немало работ, например [2-5]. Автором был предложен альтернативный подход к решению задачи поиска базовых инерционных параметров [6], имеющий ряд преимуществ по сравнению с методами, описанными в указанных работах.

В [6] рассмотрены лишь теоретические аспекты предложенного метода. Настоящая статья посвящена вопросам его реализации, эффективной с точки зрения вычислений.

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

где N — число звеньев манипулятора; р — некоторый инерционный параметр. В дальнейшем, если некоторая функция f (не обязательно скалярная) линейно зависит от ряда параметров р (г = 1 ,...,п), то частные производные вида дf/дpi будем называть коэффициентами влияния соответствующего параметра на эту функцию. Уравнение динамики манипулятора может быть записано с помощью уравнений Лагранжа 2-го рода

где я и с} — векторы обобщенных координат и скоростей, Q — вектор обобщенных сил; Ь = К — П — функция Лагранжа. Обозначим вектор-строку, составленную из коэффициентов влияния элементарных инерционных параметров на функцию Лагранжа, как то^, а вектор-столбец, состоящий из самих этих параметров, как р. Тогда уравнение движения примет следующий вид:

3При неизменных кинематических параметрах. 4Подробно материал данного параграфа изложен в [6].

dt

d

Выражение в квадратных скобках представляет собой матрицу размера N х 10N, элементы которой зависят от обобщенных координат, скоростей и ускорений. Обозначим ее W. Можно показать, что коэффициенты влияния на функцию Лагранжа являются векторами в бесконечномерном линейном пространстве функций q и q. Однако эти векторы оказываются линейно зависимыми [4], а следовательно, линейно зависимыми будут и столбцы матрицы W, являющиеся по сути коэффициентами влияния на левую часть уравнения движения. Последнее и приводит к проблеме неоднозначности.

Отметим, что система векторов wL задает некоторое конечномерное подпространство бесконечно-мерного пространства функций. Очевидно, что функция Лагранжа L принадлежит этому подпространству. Пусть имеется вектор-строка WL размерностью 1 х r (r < 10N), элементы которой являются линейно независимыми векторами упомянутого бесконечно-мерного пространства такими, что образуют базис L{wl}. Тогда базовые инерционные параметры можно определить как коэффициенты разложения вектора L по этому базису. Обозначая вектор, составленный из них, как p, можно записать L = WLp. Вместе с тем L = wLp. Пусть Y — матрица размера r х 10N координат векторов wL в базисе WL, т.е. wL = WLY. Из условия инвариантности функции Лагранжа (а следовательно, и уравнения движения) следует, что справедливо следующее равенство p = Yp. Таким образом, число базовых инерционных параметров равно рангу системы векторов wL, а их выражение через элементарные инерционные параметры определяется матрицей Y.

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

Теорема (о базисном множестве). Система векторов wL принадлежит конечномерному линейному пространству размерности (N2 + + N) ■ 5V ■ 3N-v/2 + 3V ■ 2N-v, задаваемому следующим множеством базисных векторов:

в = ((Q12 u Q21) * Y * ■■■ * Yn ) и (X * ... * XN), где Qi2= {q2/2, i=1 ...N}, Q2i= im, i=2,...,N, j = l,...,i - 1},

{1, cos qi, sin qi} cos 2qi, sin 2qi}, вращ.; {1,Qi,Qi}, поступ.;

Под операцией "*" понимается следующее: если A = {a, i =

= 1 , ...,n} и B = {bj, j = 1,...,m}, то A * B = {abj, i = 1,...,n, j = 1,...,m}.

Благодаря этой теореме базовые инерционные параметры могут быть найдены с помощью следующего простого алгоритма:

• поиск матрицы координат Z векторов системы wL в конечномерном пространстве C{¡3};

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

• составление матрицы координат Z базисной системы векторов Wl из базисных столбцов исходной матрицы координат Z;

• вычисление матрицы Y по формуле Y = Z+Z, где операция «+» означает псевдообращение.

Последнее соотношение следует из условия инвариатности функции Лагранжа. Пусть b — вектор-строка, составленная из элементов множества в так, чтобы wL = bZ и WL = bZ. Тогда справедливо равенство Zp = Zp, которое можно интерпретировать как неоднородную систему линейных алгебраических уравнений относительно p. Нетрудно заметить, что основная матрица этой системы Z имеет максимальный ранг, поэтому неизвестные определяются единственным образом по формуле p = Z+Zp. Сравнивая с p = Yp, получаем искомое соотношение.

Изначально предполагалась прямая реализация предложенного алгоритма в части нахождения координат векторов wL (i = 1... 10N):

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

• раскрытие скобок, упрощение и приведение подобных слагаемых в этих выражениях (расширение выражения);

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

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

Рекурсивное вычисление проекций. Из кинематики манипуляторов известно, что матрица перехода от системы координат к-го звена к системе координат (к — 1)-го звена, если они построены по алгоритму

Денавита-Хартенберга5, имеет вид

Ak =

cos qk sin qk 0 0

- sin qk cos ak cos qk cos ak sin ak 0

sin qk sin ak

cos qk sin ak cos ak 0

ak cos qk ak sin qk dk 1

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

в виде

A rot Ak

6

0 0 0 0

0 0 0 0

0 Sak Сак dk

0 0 0 1

• 1 +

1 0 0 ak

0 Сак ~Sak 0

0 0 0 0

0 0 0 0

+

а для поступательного как

Atr — Ak —

ч +

0 1 0 0

Сак sak 0

0 0 ak

0 0 0

0 0 0

"Qk'

CQk SQk Ca.k SQk Sak ak CQk 0 0 0 0

SQk CQk Ca.k CQk sak ak Sqk • 1 + 0 0 0 0

0 Sak Сак 0 0 0 0 1

0 0 0 1 0 0 0 0

d

k ■

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

' k

Ak — / , Akxk,

= 1

(1)

где xlk G Xk, mk = \Xk |, Xk = {1, cos qk, sin qk} в случае вращательного звена и Xk = {1,dk} в случае поступательного звена, а Alk — матрицы при соответствующих xlk. Скалярные коэффициенты xlk в выражении (1) можно рассматривать как векторы в линейном пространстве не-

5 Подробнее о кинематическом описании манипуляционных роботов с помощью однородных координат см. [7].

6Буквами c и s для краткости обозначены функции cos и sin. Нижний индекс является их аргументом.

прерывных функций, определенных на Я. Очевидно, что эти векторы линейно независимы и образуют базис подпространства С{Хк}.

Матрица перехода от системы координат к-го звена к абсолютной системе координат определяется соотношением Тк = А1 ...Лк .С учетом выражения (1) это соотношение запишется в виде

т \ I тк

Тк = (Е А1X1 ... ЕАк хк

VI=1 / \г»=1

Раскрывая скобки, получаем

mi mk

( Ali Л1Л jk Ji

Tk — £Aii1 ...Ä

11=1 ik=i

/у k /у

1k I xk ■■■x1

Обозначим выражение в круглых скобках как Т1к"'1к. Тогда имеем

ГП1 тк

Тк = Е ...^Тк1-1" хкк ...х!1. (2)

11=1 1к=1

Из определения матрицы перехода Тк следует, что имеет место рекуррентное соотношение Тк = Тк_ 1Ак. Тогда с учетом выражений (1) и (2) это соотношение запишется в виде

(т,1 тк-1 \ / тк

Е- Е тт-"-1 .. х?) ЕА[кхкк

11=1 1к-1=1 ) \1к=1

Раскрыв скобки, получим

т1 тк-1 тк

'к / у ... /

f ^ V V fh-lk-1 Alk) xlk xlk-1 xli

fk — ■■■ V k—1 ÄkJXkX k-1 ■■■x1 ■

l1=1 lk 1=1 lk=1

Сравнивая полученное выражение с (2), можно заметить, что

Ткь..1к-11к = Т£:1гк-1 Ак, к = 1,...,Ж (3)

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

То = Е.

Теперь запишем выражение для коэффициента влияния 1-го инерционного параметра к-го звена на потенциальную энергию механизма. Напомним, что рассматривается только потенциальная энергия сил веса [6] N

п = - 0]ЕТкНк[ о 001 ]т.

к=1

Здесь Нк — матрица инерции к-го звена манипулятора7, а g — вектор ускорения свободного падения.

7Подробнее об описании инерционных свойств звеньев манипуляторов см. [7].

Учитывая соотношение (2), имеем

mi mk

дП = "£ ...£ ([gT 0] TlrlkDH{[ 0 0 0 1 ]т) xk ...x, (4)

k

k ii=i ik=i

где pk — 1-й элементарный инерционный параметр k-го звена манипулятора; DHlk = dHk/dplk. Очевидно, что DHl = DHj Vi,j G N, поэтому в дальнейшем будем опускать индекс звена. Обозначая выражение в круглых скобках как DPщк-i)+l, а также имея ввиду, что x1 = 1 (i = 1,...,N), получаем

mi mk

l1...lk 1 k

10(k-1)+l X1 ...x Xk + 1 ...XN

li = 1 lk = 1

l1 ...lk

дП - V Dpl1-lk xli xlk x1 x1

dpi — Z^ ... Z^ DP10(k-1)+l x1 ...xk xk+1 ...xN.

Таким образом, величина ОРщк- 1)+г есть проекция на соответствующий базисный вектор коэффициента влияния (10(к — 1) + /)-го элементарного инерционного параметра на потенциальную энергию механизма.

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

N

= ^2 №икгНк) , г,] =

к=тах(г,])

где величины и и являются частными производными вида дТк/дд^. Поскольку матрица квадратичной формы является симметрической, ее элементы ау и а^ равны. Тогда можно вычислить только коэффициенты ау, лежащие на главной диагонали и ниже нее (т.е. с индексами г = 1,...,Ы, ] = 1,...,г). С учетом сказанного кинетическая энергия механизма может быть записана в виде

1 N N г-1

К = 2 ^2 аг$ + ^2 ^ а'Ч дд, (5)

о / j ~~iin ' / j / j ij ■

i=1 i=2 j=1

причем

N

aij — J2 tr (UTJ UkiHk) > i — 1,...,Ni j — 1,...,i. (6)

k=i

Рассмотрим величины Uki = dTk/dgi. Поскольку Tk = A1 ...Ak и Ai = Ai(gi), то справедливо следующее:

0, k < i;

A1 ...dAi/dgi ...Ak, k > i.

Uki —

Из кинематики известно, что дАг/дяг = ОгАг [7], где

) =

Г) — ) Drot, 15 п —

Di — \ n п Drot —

Dtr 5 — 0

0 -10 0

10 0 0

0 0 0 0

0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0

Значение аг определяет тип сочленения между г-м и (г — 1)-м звеньями: единица соответствует вращательной паре, нуль — поступательной. Тогда с учетом (1) имеем

дА,

9q,t

Е М

X i ■

li=1

Подставляя это соотношение в выражение для ик,г и раскрывая скобки, получаем:

т1 тк

икг ^ ^ ..^ ^^ (а^-1 . ..ОгА£ ... Ак 1 ХС ,...,Х-

11 = 1 кк = 1

Обозначим выражение в круглых скобках как икгк. Тогда последнее соотношение запишется в виде

т1 тк

икг = £ ...Т.иыЛк хк ...X11. (7)

к1=1 кк=1

С учетом того, что Тк = Тк_1(дь...,дк-1)Ак (як), справедливы рекуррентные соотношения

Uki —

Uk—1iÄk, i < k; fk- 1Dk Äk, i — k-

(8)

Действуя аналогично процедуре вывода соотношений (3), получаем

U

11 -lk — Utt-1 Ukk, i<k,

Ul1 ",lk — rul1---lk-1 D /U lk i — k — 1 DkÄk 5 1 — k■

(9a) (96)

Рассмотрим теперь матричное произведение икг. Воспользовавшись равенством (7), будем иметь

m1

mk

Ufcj Uki J ^

EUi-

lk xlk

k

lk=1

E

l1=1 lk=1

mk

Ul1 ...lk lk

Ul1 k

/ v Uki rk ,

JA

Раскрыв скобки, получим

т1 т1 тк тк

ц?,и» = ЕЕ-ЕЕ

11=1 к1=1 кк=1 кк=1

UU

l1 '"lkN\ Т TUl1'"lk

kj

ki

J'k rlk rl1 rl1 (10) rk rk r1 ■

l1 = 1

Можно видеть, что система векторов Xk = {x\ • xJk : x\,x3k G Xk, i, j = 1,...,mk} линейно зависима. Действительно, хотя бы ввиду коммутативности рассматриваемой операции умножения множество Xk будет содержать одинаковые элементы, что неизбежно ведет к линейной зависимости. Пусть система векторов Yk образует базис пространства L{X2}. Тогда векторы этого пространства могут быть представлены в виде линейной комбинации базисных векторов yk G Yk, т.е.

rk

¿¿(a,, в,) 4 xk = ¿ YmyT, (11)

l'=i i=1 m=1

где rk = \Yk|. Легко показать, что в качестве базисного множества пространства £{X|} может использоваться Yk = {1, cos qk, sin qk, cos2qk, sin2qk} в случае вращательного звена и Yk = {1,dk, d2} в случае поступательного звена.

Найдем, как выражаются координаты Ym произвольного вектора пространства L{X|} в базисе Yk через коэффициенты ау и в,. Выполняя суммирование, подставляя вместо xlk, xlk и ym соответствующие элементы Xk и Yk и приводя подобные слагаемые, получаем следующие соотношения:

Yi = a^i + 2 a2@2 + 1 аз вз;

Y2 = aiß2 + a2ß\\ 7э = aiß3 + 03^1;

Y4 = 1 а2р2 - 1 а3р3; 75 = 1 а.2вз + 1 азв2

(12a)

для вращательного сочленения;

Yi = aiei;

Y2 = aie2 + a2ei; (12б)

Y3 = а2в2

— для поступательного сочленения.

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

Yk = {1, cos qk, sin qk, cos2 qk, sin qk cos qk) .

Это связано с тем, что с точки зрения быстродействия эффективнее перемножить уже вычисленные значения синуса и косинуса, нежели заново вычислять эти функции от двойного аргумента. В этом случае коэффициенты преобразования (11) будут вычисляться следующим образом:

71 = + азвз;

72 = + озА;

73 = а1вз + «зА;

74 = «2 А - азвз;

75 = азвз + азвз-

Можно показать, что подобные правила справедливы и в случае матричных коэффициентов:

mk mk rk

J'xl „l

£ Е (А СВ) Хк Хк = Е °тУк■ (13)

1'=1 1=1 т=1

В этом случае слагаемые вида аувг необходимо заменить на Аг,СБ;. Также следует помнить о некоммутативности матричного умножения.

Применяя преобразование (13) в равенстве (10) последовательно к раз, получаем

Г1 Тк

иы = £ ■■■Т.йк^ Ук ■■У ■ (14)

11=1 1к=1

Замечание. Приведение равенства (10) к форме (14) необходимо для соответствия теореме о базисном множестве, ведь исходное представление не является линейной комбинацией линейно независимых векторов.

В соответствии с соотношением (5) выражение для коэффициента влияния 1-го инерционного параметра к-го звена на кинетическую энергию механизма, очевидно, имеет вид

1 N ^ N г—1 О

дК 1 оац ,2 ^^ да^ . _

~ г 1г + -тгт т ■

dplk 2 ^ dplk г j=1 dpk

Введем множество V = ф12иф21. Нетрудно заметить, что ¡V | = N (И+ + 1)/2. Вместе с тем V = Пусть множество V упорядочено так, что если к = г(г — 1)/2 + ], то Ук = дг2/2 при ] = г и ук = при ] = г. Тогда

dK v^ v^ да

dpk 1=1 dpk V

j Vi(i-l)/2+j

Проанализировав (6), можно сделать вывод о том, что да^/дрк = 0

при к < г. Учитывая выражение (14) и принимая во внимание, что у1 = 1 (г = при к = г,..., N будем иметь

да-- ¿К / * \

j _ \ Л \ Л +„, I TTl1---lk n zuM „,l

tr (jDH^ y1 ...ykyi+1 ...yN.

кз

°Рк 11=1 г*=1

Обозначая след матричного выражения в круглых скобках как ЩКО^'—^, окончательно получаем

дК N г П гк

др

l -££ £ - £ DK ^-Vl1 ...ykk yi+1 ...yN vi(i—1)/2+

i=1 j=1 ll = 1 lk = 1

k

Таким образом, величина ЩКг0(к,'_гк)+1 является проекцией на соответствующий базисный вектор коэффициента влияния (10(к — 1) + /)-го элементарного инерционного параметра на кинетическую энергию манипулятора.

Замечание. В дальнейшем для единообразия будем полагать, что индекс %Ц1...IN полностью определяет любой базисный вектор из множества в. Причем при г = 1,...,^3 = 1,...,г этот индекс задает вектор у1° у1 ,...,уу (10 = г (г — 1)/2 + 3), соответствующий кинетической энергии, а при г = 3 = 0 — вектор ж!1 ,...,х1у], соответствующий потенциальной энергии. Ясно, что 1к (к = 1,...,^ принимает значения в диапазоне от 1 до |Ук | в первом случае и от 1 до \Хк | во втором случае.

Получим теперь рекуррентные соотношения для величин и1кЗ':Лк. Учитывая выражение (8), можно записать

( АкЩ_1зик-1,гАк, г < к; Щ3икг = { АкЩ-^Тк_ЩАк, г = к, з < к; (15)

[ А?ЩТ^Тк-ЩАк, г = к, з = к.

Таким образом, рекурсивное вычисление матриц икг через матрицы Щ_1з ик-1,г возможно лишь в случае г < к. Получим эти соотношения, записав первое из равенств (15) с учетом (14) и (1):

тк

из икг = | £ ^ хкк I X

Г1 Гк-1

х ' £ . " £ 1,к-1 yk-1 ...y11 , A-k xk

,ii=1 lk-1 =1

lk=1

Раскрывая скобки и меняя порядок суммирования, получаем

Г1 Гк-1 / тк тк т

т,££ 4 У—,яц

xl'kxlk | ylk-i „h xk xk yk — 1 ■

1-1 = 1 1к-1 = 1 \1'к = 1--к = 1 Сравнивая последнее выражение с (13), можно сделать вывод, что

Гк тк тк

1-к„, ;-к

E%Jk yk = ££К ütü-1 Ä

ik=1 ik=1 ik=1

k

xk xk •

Тогда, в соответствии с матричным вариантом правил (12) нахождения коэффициентов преобразования (13), матрицы и,"'1к (к=1,...,И, г = 1,...,к — 1, ] = 1,...,г) могут быть рекурсивно найдены следующим образом:

Ту11...1к-11 = / дЛ т ЦУ-1-1'-1 Д1 + ( яА т тт-1-1'-1 Дз.

ик,г = ^Лк} ик — 1,,г Ак + ик—1,,г Ак;

Ц-1...-'-12 = / дЛт иу-1...-'-1 Д2 + яЛт иУ-1...-'-1 Д1.

иУ^-к-1* = (Ак) т ад-1 ^з + (А1) т ад-1 ¿к. (16а)

ту]-1...1к-14 _ ( я^ т Ту 1-1...1к-1 Я 2 ^ яА т гтг1...1к-1 яз. ик,г = ^ик — 1,,г Ак ик—1,,г Ак;

ту 1-1...1к-15_ / яЛ т Ту-1 ...-к-1 яз I / яз\ т ТУ-1...1к-1 я2

и к,г = ^Тк — 1,,г Ак + ^ ^ Т к— 1,,г Ак

в случае вращательного звена;

т

ylx...lk-i1_ / лМ fjli—lk-i

/kji — l A kj Uk-1,ji A k;

,к-12 = (т ТТк-Й-1 ^к + (Ак)т и::,1 Ак. (1бб)

ТУ-1 ...-к-1з _ ( яА т тУ1- 1...;-к-1 Д2

ик,г = ^Лк) ик—1,,г Ак ■

- в случае поступательного звена.

Чтобы найти рекуррентные соотношения для матриц и1,.1' (к =

- г = к, ] = 1,---,к) перепишем равенство (10) с учетом (9)

т1 тк ик, икк = £ £ ■■■

mk mk

И! •••DjAI •• Akl ^ ••• Ajj •••DkA

lk ^k l1 ll xk xk •••x1 x1 •

l'k=1 lk=1

li=1 li=1

т

к

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

ГПк ГПк

UjUkk = Ш)'I £

mk-1 mk-1

lk = 1 lk = 1

Vlk-1 = 1 lk-1 = 1

ЕЕ d,rn E E

mj—1 mj—1

J', = 1 j = 1

Jj—1=1j—1=1

m1 m1 т

\ Л \ Л i Al1 i nil^l1 ™l1

I \ \ ' I Al1\ Al1 xl1 Xl1 I I X' xlj 1 1 D Alk X'k xlk ... 2-^! 2-^! V A1 j A1 x1 x1 .. xj xj ..4 Dk Ak xk xk ■

Ji = 1 l1 = 1

(17)

В соответствии с преобразованием (13) двойная сумма в круглых скобках, находящаяся на самом высоком уровне вложенности, может быть представлена как

т1 т1 Г1

£Е(А?) ' -А'.1 х11 хЧ = £ АЧ^,

11 = 1 1.1 = 1 11 = 1

причем матричные коэффициенты А^к определяются по соответствующим правилам (12). Подставляя в соотношение (17) вместо левой части последнего выражения его правую часть, раскрывая скобки и меняя порядок суммирования, получаем

Г1 / mk mk

UjUkk = £ £ J2 (A

l1 = 1 \lk = 1 lk = 1

l' lk

X

mk—1 mk—1

X

E E -ЕЕ(d, , E E

j',=1 j=1

mj—1 m,—1

' 1 = 1 lj —1 = 1

i l2 = 112 = 1

ЕЕ № j

l2^\ ü l1 л l2 xl2 xl2 2 x2 x1

A j x ' x ' 1

П Alk xlk xlk | yl1

DkAk xk xk I y1

Повторив эту процедуру еще (к — 1) раз, придем к следующему результату:

Г1 Гк

.1■■■1к„к „.Н

щ,Ukk = £...^2,yk...yi1

l1=1 lk=1

mm

jj

mm

jj

l' =1 lk 1=1

Сравнив его с равенством (14), можно заметить, что Ujlk = A'1'' J'k

kjk

Матрицы Aj'' (i = l,...,k, j = 1,...,k) определяются с помощью

следующей рекурсивной процедуры:

AI 1. . .Ii-11 Aijk

D1A ) At j 1D2A1 + D1 A1)T Ai-'llj-1 D2A2 +

Dl A1)T Ä-j D2A +

A A A A

h...lk-l2

ijk

ijk

ll...lk-i4

ijk

ll...lk-15

= (DlA,2) Ail:--1 щлц -

l1...li- 1 П2 42

ijk

= (DlA2) Ail:--1 d2A43 +

jk

l1...li- 1 n2 43

D1A3

D1A2

DiA3

D1A3

D1A3

£}...к-1-П2 43.

A

i-1,jk

l1...li-i-1,jk

D2A3;

"A41;

A

l1...li-1 i-1,jk

A41; (18a)

Al1...li-1 A2 A3

ii Ai-^lj-1 D2A2

в случае вращательного звена;

A

l1...lk-11 ■ijk

d1A1

Ah..^-1 A2 A1; Ai-1,jk Di Ai;

A

■ijk

D^AAT AJ-ä-1 A2A + Га1А42х т

A

i- 1,jk

D2A41; (186)

A

■ijk

D1A2

A-j d2A2

в случае поступательного звена, причем

'e,

Drot^j + Dir (l — a),

D1 =

d2 =

(E

[Drot^k + Dir(1 - Uk), i = k.

г = j, « =

г = k.

Для инициализации процедуры следует положить A0jk = E.

Практическая реализация. Итак, соотношения (3), (16) и (18) определяют рекуррентный алгоритм вычисления проекций. Однако эта обратная рекурсивная схема вычислений (от N-го звена к первому) оказывается неэффективной, поскольку одни и те же матрицы Т, U и A рассчитываются множество раз. Гораздо эффективнее применение прямой рекурсии, которая позволяет вычислять указанные матрицы только один раз. Правда в этом случае требуется дополнительно хра-

N

нить в оперативной памяти 2 + ^^(k(k + 1)/2 + k + 2) матриц размера

k=1

4 х 4 (на максимальной глубине рекурсии). Например, для N = 6 это значение равно 56. Если элементы матриц являются числами с плавающей точкой двойной точности (согласно IEEE 754), то дополнительное потребление памяти составит около 7 кБ. При этом для хранения матрицы координат Z в случае всех вращательных сочленений

T

T

T

T

T

i1

для любого способа необходимо около 150 МБ. Таким образом, при использовании прямой рекурсии потребление памяти несущественно превышает аналогичный показатель в случае обратной рекурсии. Выигрыш же в быстродействии оказывается весьма существенным. Так, время работы программы, вычисляющей все элементы матрицы координат Z, построенной на базе обратной рекурсии, превышает аналогичный показатель для прямой рекурсии почти в 20 раз.

Рассмотрим теперь реализацию прямого рекурсивного алгоритма вычисления проекций. Обозначим преобразования (18) как r^s, где индексы f, s и а принимают значения нуль или единица. Если f или s равны нулю, то D1 или D2 соответственно равны единичной матрице. В противном случае они определяются указанными ранее соотношениями. При а = 1 используются соотношения для вращательного звена, при а = 0 — для поступательного. С учетом введенных обозначений очевидна справедливость следующих соотношений:

= i Д1:^) , j = 1... k - 1; (19a)

ik,j,k+1 ~ '00 I^k-1,j,k+1

As^.k+l = T1o\ A k-1 fk.fc+l f" ; (196)

Afc.k+l.k+l _ T00 1 Ak-1 ,k+1,k+1 ( • (19в)

\к...1к _ та \ лк---1к- 1

1к,к+1,к+1 _ '00 ^Ак-1,к+1,к+1

Легко проверить, что матрицы А1^'к'1г (П _ я + 1,...,к) одинаковы,

поэтому далее будем их обозначать А!'10к'г. Индекс 0 выбран не случайно, ибо для этой матрицы В1 _ В2 _ Е. Кроме того, проанализировав преобразования (18), можно заметить, что для любых П справедливо равенство Ак-'1 к)Ъ 1 _ Ак-1кк+р С учетом сказанного соотношения (19) запишутся в виде

А11-1 к _ та I А11"Лк-1\ п _1 к — 1-Ак,),к+1 _ т00 |Ак-1,к,к ( , п _ 1,...,к 1-

А ' 1к _ та I А' 1к-1

Ак,к,к+1 _ т10 ^ Ак-1,0,к

А1'1...1'к _ та I А' 1...' к-1 Ак,0,к+1 _ т00 ^ Ак-1,0,к ,

или, совмещая первое и третье равенства, получаем

А'1...'к _ та / А11...1к-11 п _ о к — 1 Ак,к,к+1 _ т00 ^Ак-1),к \ , п _ °,...,к 1-

А11...1 к _ та Г АЬ-Лк-1 к,к,к+1 _ т10 к-1,0,к

На основе вычисленных матриц можно определить величины и)1 к _

_ к (к _ 1,...,И, П _ 1,...,к) с помощью следующих преобразований:

л'1...'к _ та Г Аь..^■ _1 к — 1

Ак)к _ т01 1 Ак-1,),к ¡, п _1,...,к 1-

<7 I л11---1к — 1

ДЧ"лк _ т7 JA1 Дккк _ '111 Дк- 1,0,к

Таким образом k матриц Ulkj'k'lk (j = l,...,k) вычисляются с помо-

щью к матриц Д-Й- ^ = 0,...,к — 1), полученных на предыдущем, (к — 1)-м шаге. С помощью соотношений, аналогичных (17), можно показать, что преобразования (16) являются частным случаем преобразований (18) при { > к. Тогда

j _ т<о{Ütj^ (i _i,...,k - 1, j _1,...,i).

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

function ProjKinEnergy (list, prev_u, prev_a, DH, coor_matr) //list — список, состоящий из элементов индекса l1 ...lk //prev_u — массив, содержащий матрицы Uk-'/:-1, г = 1,...,k — ^ j = 1,...,i

//prev_a — массив, содержащий матрицы Ak—jk1 ,j = 0,...,k — 1 n := число звеньев манипулятора; k := номер текущего звена;

Создать массив cur_u матриц 4 х 4 из size(prev_u)+k элементов;

//cur_u содержит матрицы Ulkj'^'lk ,i = 1,...,k, j = 1,...,i

Создать массив cur_a матриц 4 х 4 из k+1 элемента;

//cur_a содержит матрицы A^'k^ 1,j = 0,...,k

sigma := TRUE, если ak = 1, и FALSE, если ak = 0;

m := \Yk\;

for l := 1 to m do

Добавить l в конец list;

r := 0;

//Вычисление Uj'lk ,i = 1..k — 1,j = 1..i for i := 1 to k-1 do for j := 1 to i do

cur_u[r] := tau (l, prev_u[r], FALSE, FALSE, sigma); r :=r+ 1; end for end for

//Вычисление Ulk:jkk,j = 1 ...k for i := 1 to k-1 do

cur_u[r] := tau (l, prev_a[i], FALSE, TRUE, sigma); r :=r+ 1; end for

cur_u[r] = tau (l, prev_a[0], TRUE, TRUE, sigma);

//Вычисление Akj>k+1,j = 0..k for i := 0 to k-1 do

cur_a[i] := tau (l, prev_a[i], FALSE, FALSE, sigma); end for

cur_a[k] := tau (l, prev_a[0], TRUE, FALSE, sigma); //Вычисление проекций r := 0;

for i := 1 to k do for j := 1 to i do

for p := 1 to 10 do

row := idx_to_row_number (list, i, j); col := 10 * (k - 1) + p;

coor_matr[row, col] := trace (cur_u[r] * DH[p]); end for r :=r + 1; end for end for

//Рекурсивный вызов функции, если текущее звено не

последнее if k != n

ProjKinEnergy (list, cur_u, cur_a, DH, coor_matr); end if

Убрать из list последний элемент; end for end func

Функция idx_to_row_number() определяет строку матрицы координат Z, соответствующую текущему базисному вектору, задаваемому индексом ijl1 ...lk 1... 1. Мы не будем останавливаться на деталях реализации этой функции, поскольку это несущественно с точки зрения принципа работы рассматриваемого алгоритма. Псевдокод функции tau(), осуществляющей преобразование r^s, не приводится ввиду его очевидности.

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

function ProjPotEnergy (list, prev_t, DH, coor_matr) //list — то же, что и ранее, prev_t — матрица т1к1_.'11к-1

= число звеньев манипулятора; = номер текущего звена;

= [9x 9y 9z °j; = [0 0 0 1]T;

m:= |Xk |;

for l := 1 to m do

Добавить l в конец list; a := %;

//Вычисление T^-lk cur_t := prev_t * a; //Вычисление проекций for p := 7 to 10 do

row := idx_to_row_number (list, 0, 0); col := 10 * (k - 1) + p;

coor_matr[row, col] := g * cur_t * DH[p] * u; end for

//Рекурсивный вызов функции, если текущее звено не

последнее if k != n

ProjPotEnergy (list, cur_t, DH, coor_matr); end if

Убрать из list последний элемент; end for end func

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

function CalcProjections (coor_matr)

Создать пустой список list целых чисел; Создать пустой массив u матриц размера 4 х 4; Создать массив a матриц размера 4 х 4 из одного элемента; Создать массив DH матриц размера 4 х 4 из 10 элементов; a[0] := E4;

t := e4;

for l := 1 to 10 do

DH[l] := DHl; end for

ProjKinEnergy (list, u, a, DH, coor_matr); ProjPotEnergy (list, t, DH, coor_matr); end func

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

для пятизвенного механизма с вращательными сочленениями это значение равно 47118 х 50. Для приведения матрицы координат к ступенчатому виду применяется модифицированный алгоритм исключения Гаусса с частичным выбором ведущего элемента7.

Из определения матриц А следует, что они содержат много нулевых элементов. Это, очевидно, справедливо и для остальных матричных коэффициентов, вычисляемых на основе А. Поэтому еще одним источником повышения быстродействия является специализированное представление всех этих разреженных матриц, ориентированное, главным образом, на минимизацию времени выполнения операции умножения матриц. В данной реализации использовалось упрощенное представление в виде ассоциативного массива, содержащего в качестве данных ненулевые элементы матрицы, а в качестве ключей — соответствующие им линейные индексы. Под линейным индексом здесь понимается натуральное число, вычисляемое как I = гп + ], где г — номер строки, ] — номер столбца, а п — число столбцов матрицы. Очевидно, что при известном размере матрицы номер строки и столбца определяется однозначно для каждого значения линейного индекса:

г = 1/п, ] = /%п,

где под операцией «/» понимается целочисленное деление, а под операцией «%» — остаток от целочисленного деления. Отметим, что индексы г, I отсчитываются от нуля. Это позволяет исключить избыточные операции при пересчете матричных индексов в линейный и наоборот. Недостатком этой реализации является необходимость постоянного извлечения матричных индексов элемента из его линейного индекса при выполнении операции матричного умножения. Но, поскольку такая реализация применяется к разреженным матрицам небольшого размера (и соответственно с небольшим числом ненулевых элементов), заметного понижения быстродействия не происходит. Однако, если такое допущение не справедливо, необходима полноценная реализация механизма разреженных матриц с хранением обоих ин-дексов8.

Пример работы приложения. Автором разработана программа на языке С++, осуществляющая поиск базовых инерционных параметров манипулятора, если заданы его кинематические параметры9. Создан-

7Этот алгоритм является стандартным и широко распространен в матричных вычислениях, поэтому в настоящей статье он не описывается. Подробнее об алгоритме см, например, в работах [8, 9].

8См., например, [10].

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

Кинематическая схема манипулятора PUMA 560

ное приложение основано на методе проекций и использует предложенный в данной статье рекурсивный подход к их вычислению. Эта часть программы полностью реализована согласно описанному алгоритму. Проиллюстрируем корректность работы приложения на примере хорошо известного манипулятора PUMA 560. Кинематическая схема робота представлена на рисунке, а геометрические параметры — в таблице.

Геометрические параметры манипулятора PUMA 560

№ qi ai, град ai, мм di, мм

1 1 var -90 0 0

2 1 var 0 431,8 150

3 1 var 90 -20,3 0

4 1 var -90 0 433,1

5 1 var 90 0 0

6 1 var 0 0 70

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

параметры10

pi

p2 p3 p4 p5 p6 pT p8 p9 p10 Р ii Р i2 p 13

p 14 = J2 yy p 15 = J2 xy p16 = J2 yz p 1т S 2 p 18 = J3 xz p 19 - J3 yz

p 20 = J4 XX p 21 = J4 xy p 22 = J4 xz p 23 = J4 yz p 24 — SX p 25 - J5 XX

p 2б - J5 xy p 2т = J5 xz p 28 = J5 yz p 29 — SX p 30 = J6 XX p 31 - Jy6y yy

p 32 = J6 xy p 33 = J6 xz p 34 = J6 yz p 35 — SX p36 - S6 - Sy

Число базовых инерционных параметров и их выражения через элементарные инерционные параметры соответствуют результатам, полученным в работах [2, 3 5].

Заключение. Итак, в настоящей работе были получены соотношения, позволяющие вычислять проекции коэффициентов влияния с помощью рекурсивной процедуры. На основе этих соотношений предложен алгоритм и разработана программа, осуществляющая поиск базовых инерционных параметров в соответствии с оригинальным методом [6]. Необходимо отметить высокую сложность реализации предлагаемого алгоритма. Однако рекурсивный подход позволяет добиться высокого быстродействия при использовании метода проекций. Так полное время работы созданного приложения для робота PUMA 560 составило около 200 мс на персональном компьютере средней мощности. Это значение сравнимо с аналогичным показателем численных способов поиска базовых инерционных параметров,

10Коэффициенты при статических моментах и массах имеют размерности мм и

2

мм2 соответственно.

jXX + 4 + i2z + 4 + 3oo(s2 + S3) +

+22500(Ш2 + Ш3 + m4 + m5 + m6)

+ 4, - 866, 2 S4 + 187576(m4 + m5 + m6) JXX - 186451(m 2 + m3 + m4 + m5 + m6) SX + 431, 8(m2 + m3 + m4 + m5 + m6) JXX - 412, 09(m3 + m4 + m5 + m6) S3 - S4 + 433,1(m4 + m5 + m6) SX - 20,3(m3 + m4 + m5 + Ш6)

+ & + 140 S6 + 4900 Шб JXz + 431,8(S2 + Sy3)

5б

S5 + Sz6 + 70 Шб

JXy - 20, 3 Sy

J4 + J5

zz yy s4 + s5

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

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

1.ZenkevichS., NazarovaA., NagyS. Computer-based arm // Proc. of 23rd International Symposium on Industrial Robots (1992), p. 116-121.

2. G a u t i e r M. Numerical calculation of the base inertial parameters of robots // IEEE Transactions on Robotics and Automation. - 1990. Vol. 6. No. 3. - P. 368-373.

3. Gautier M., Khalil W. Direct calculation of minimum set of inertial parameters of serial robots // IEEE Transactions on Robotics and Automation. -1990. - Vol. 6, No. 3. - P. 368-373.

4. Mayeda H., Yoshida K., Osuka K. Base parameters of manipulator dynamic models // IEEE Trans. on Robotics and Automation. - 1990. - Vol. 6. No. 3. - P. 312-320.

5. S h e u S. -Y., W a l k e r M. W. Basis sets for manipulator inertial parameters // Proc. of IEEE Conf. on Robotics and Automation (1989), p. 1517-1522.

6. Крутиков С. Л. Базовые инерционные параметры манипуляционных роботов. Вестник МГТУ им. Н.Э. Баумана. Сер. Приборостроение. - 2010. № 1. -C. 10-20.

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

8. Г о л у б Д ж., Ч. ВанЛоун. Матричные вычисления. - М.: Мир, 1999.

9. B e e z e r R. A. A first course in linear algebra [Электронный документ]. (2004). 10. B a r r e t R., B e r r y M. et al. Templates for the solution of linear systems: Building

blocks for iterative methods. PA Philadelphia, SIAM (1994).

Статья поступила в редакцию 26.01.2011

Сергей Леонидович Крутиков родился в 1985 г., окончил МГТУ им. Н.Э. Баумана в 2008 г., получил степень магистра техники и технологии по направлению "Автоматизация и управление". Асотстент кафедры "Робототехнические системы" МГТУ им. Н.Э. Баумана. Автор двух научных работ в области робототехники.

S.L. Krutikov (b. 1985) graduated from the Bauman Moscow State Technical University in 2008, Assistant of "Robotic Systems" department of the Bauman Moscow State Technical University. Author of 2 publications in the field of robotics.