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

МЕХАНИКА

J

УДК 517.977

А. П. Крищенко, С. Б. Ткачев, Д. А. Фетисов

УПРАВЛЕНИЕ ПЛОСКИМ ПЕРЕМЕЩЕНИЕМ ДВУНОГОГО ПЯТИЗВЕННОГО РОБОТА ПО ЛЕСТНИЦЕ*

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

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

Движение двуногого робота по некоторой поверхности можно представить как последовательную смену двух фаз [1-4]:

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

2) фазы перехода робота с одной ноги на другую, во время которой нога, бывшая на предыдущей фазе переносимой, ударяется о поверхность и становится опорной, а нога, бывшая опорной, отрывается от поверхности и становится переносимой.

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

Модель перемещения двуногого робота на фазе одноопорного движения записывается в виде аффинной системы с векторным управлением [2-4]. В этих работах ставится задача нахождения обратной

^Работа выполнена при финансовой поддержке РФФИ (проект № 05-0100840) и программы "Государственная поддержка ведущих научных школ" (проект НШ-2094-2003.1).

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

В работе [2] предложено решение поставленной задачи для трех-звенного робота, перемещающегося по горизонтальной плоскости. Аффинная система шестого порядка с двумерным выходом приводится к так называемой нормальной форме [5], позволяющей свести задачу к построению обратной связи, глобально асимптотически стабилизирующей положение равновесия. Из множества решений полученной замкнутой системы можно выделить единственное, если задать начальные условия для переменных состояний в произвольно выбранный момент времени, например в момент перед ударом переносимой ноги о поверхность земли. Получающееся решение при любом начальном условии будет удовлетворять всем требованиям, заданным при конструировании управления. Недостаток такого подхода заключается в том, что невозможно заранее, зная лишь начальное условие на некотором шаге, предсказать поведение робота в последующие моменты времени: может оказаться, что от шага к шагу вектор состояния в момент перед ударом будет принимать разные значения и через определенное число шагов робот упадет. В связи с этим в работах [2, 3] ставится задача нахождения начальных условий, обеспечивающих периодическое движение робота. Один из методов решения этой задачи — метод подбора [2]: задается какое-либо начальное условие, находится соответствующее решение замкнутой системы и для него проверяется условие периодичности; если оно выполняется, задача решена (периодическое движение построено), иначе необходимо изменить начальное условие и продолжить поиск в соответствии с описанной процедурой. В работе [4] метод подбора применен для построения периодического движения пятизвенного двуногого робота по горизонтальной плоскости.

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

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

В работе [6] на основе исследования уравнений нулевой динамики решена задача построения периодического движения пятизвенного двуногого робота по горизонтальной плоскости.

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

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

U h

Рис. 1. Схема пятизвенного двуногого робота при движении по лестнице

голени — на расстоянии р от коленного сустава. Моменты инерции туловища, бедер и голеней относительно их центров масс равны 3т, 3р и 3 соответственно.

Определим параметры лестницы, по которой перемещается робот, задав длину 1Х и высоту ступени.

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

Модель движения подобного робота описана в работе [1]. Для целей настоящей работы удобно записать модель движения в виде, близком к виду, предложенному в работах [2- 4].

Во время фазы одноопорного движения конец опорной ноги находится в неподвижном контакте с поверхностью ступени, поэтому для задания положения робота достаточно пяти обобщенных координат 9ъ 9з1, 9з2, 941, 942, определяющих отклонение туловища, бедер и голеней переносимой и опорной ног от нормали к поверхности ступени (см. рис. 1). Примем направление отсчета этих углов против часовой стрелки за положительное. Ограничим область изменения каждого из углов 931, 932 , 941, 942 интервалом (п/2,3п/2).

Обозначим через и1 и и2 крутящие моменты в тазобедренных суставах, а через и3 и и4 - крутящие моменты в коленных суставах опорной и переносимой ног соответственно.

В декартовой системе координат Х2, Z2 с началом в конце опорной ноги координаты тазобедренного сустава (Хн, ZH) и конца переносимой ноги (Х1, Z1) выражаются следующим образом:

Использование уравнений Лагранжа второго рода позволяет получить систему уравнений, описывающую поведение робота на фазе одноопорного движения. Запишем ее в следующем виде [1]:

где 9 = (9ь 931, 932 , 941, 942)т, ^(9) — симметрическая положительно определенная матрица пятого порядка с элементами

^ц = ттрТ, ^12 = 0, ^13 = -ттрт ^032-1, = 0,

^15 = -ттРт ^42-1, ^22 = тр рр + т^2,

Xh = L(sin q42 + sin q32), ZH = -L(cos 542 + cos ^32), Xi = Xh - L(sin q3i + sin 541), Zi = Zh + L(cos 531 + cos 541).

(1)

D(q)q + C(q, q)q + G(q) = Bu,

(2)

D23 = -(TOFPf + míL)Lc32-3i, D24 = mípíLc3i-4i, D25 = -(m^PF + mtL)Lc42_3i, D33 = (mT + mF + mt)L2 + mF(L - Pf)2, D34 = -mtPtLc32_4i, D35 = [(mT + 2mF + mt)L2 - mFLpF]c32-42, D44 = mtpj2, D45 = -mtPtLc42-4i, D55 = (mT + 2mF + mt)L2 + mt(L - pt)2, = cos(q¿ - qj), i, j = 1, 31, 32, 41, 42, C(q, q) — квадратная матрица пятого порядка с элементами Cii = Ci2 = 0, Ci3 = mTpTLs32_i<q32, Cw = 0, Ci5 = mTpTLs42_iq42, C2i = C22 = 0, C23 = (mFPf+mtL)Ls32_3i^32, C24 = mtPtLs3i_4iq4i,

C25 = (mF Pf + míL)Ls42_3i'?42, C3i = -mTpT Ls32_i^i, C32 = (mF Pf + mtL)Ls32_3i^3i, C33 = 0, C34 = -mípíLs32_4i'?4i, C35 = [(mT+2mF+mt)L2-mF LpF ]s32_42q42,

C4i = 0, C42 = -mtPíLs3i_4i^3i, C43 = mtPt Ls32_4iq32, C44 = 0, C45 = mtPíLs42_4i^42,

C5i = - mT Pt Ls42_i^i, C52 = -(mFPF + mtL)Ls42_3i^3i, C53 = -[(mT + 2mF + mt)L2 - mFLpf]s32_42<?32,

C54 = -mtPíLs42_4iq4i, C55 = 0,

si_j = sin(qi - qj), i, j = 1, 31, 32, 41, 42, G(q) — вектор обобщенных сил тяжести с компонентами

Gi = - gmTpt sin qi, G2 = -g(mFpf + mtL) sin q3i, G3 = g[(mT + 2mF + mt)L - mF Pf ] sin q32, G4 = -gmtPt sin q4i, G5 = g[(mT + 2mF + 2mt)L - mtPt] sin q42, B — матрица коэффициентов при управлении:

-1 -1 0 0 1 0 0 1

B = 0 1 -10 0 0 0 1 \ 0 0 1 0 /

(3)

(4)

причем Cт(q, q)q = 0, поэтому систему (2) можно переписать в виде

d

- + G(q) = Bu, (5)

удобном для последующих преобразований.

Система (2) в пространстве состояний имеет вид

4

х = / + & , (6)

где х = (qт, т)т,

^^(С+ G(q))) , &^ = (D-1(q)Bj

©5х1 — нулевая матрица размера 5x1, Bj — ^'-й столбец матрицы В.

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

х- е 5, где 5 = (Ы)|ЗД) = , X)> Хд^)},

где — абсцисса точки соответствующей началу следующей

ступени, на которую должен подняться робот (см. рис. 1); Х1(() и — абсцисса и ордината конца переносимой ноги в системе координат Х2, задаваемые выражениями из (1).

Примем [2] следующие предположения относительно поведения робота на фазе перехода:

а) после удара нога, ставшая опорной, не проскальзывает и не отскакивает от поверхности;

б) за время удара положение робота не изменяется;

в) скорости всех звеньев робота меняются скачкообразно;

г) нога, ставшая переносимой, отрывается от поверхности без взаимодействия с ней.

В системе (6), описывающей движение робота во время первой фазы, углы (з2 и q42 определяют положение опорной ноги, а углы (31 и q41 — переносимой. Поэтому использование этой же модели для описания движения на следующем шаге возможно лишь после переобозначения координат, отражающего смену опорной ноги в момент удара. Для реализации этого переобозначения введем q- и q+ — значения q в моменты до и после удара. Тогда

q+ = , (8)

где

/ 1 0 0 0 0 \

0 0 10 0

U = 0 10 0 0

0 0 0 0 1

\ 0 0 0 1 0 /

Найдем связь между значениями q- и q+ скоростей звеньев робота в моменты до и после удара. Пусть (X+, Z+) — компоненты вектора скорости конца опорной ноги в момент после удара; F = (FX, FZ)т — сила, действующая в точке контакта на переносимую ногу и имеющая проекции FX и FZ на оси X2 и Z2 соответственно. Обозначим дт = ^т,Х2,^2)т, ri = (X1, Z1)т. Применение модели удара к контакту между переносимой ногой и поверхностью ступени позволяет получить соотношение для скачка скоростей при ударе:

D(q-)(|+ - qr) = E^q-)F, (9)

где D(q) — симметрическая положительно определенная матрица седьмого порядка, элементы Dj которой при i, j < 5 совпадают с элементами матрицы D(q), а остальные элементы определяются следующим образом:

D16 = -штРт cos q1, D17 = —mTpT sin q1, D26 = (шрpf + mtL) cos q31, D27 = — (mppp + mtL) sin q3b

D36 = [(mT + 2mF + mt)L — mFPf] cos дз2,

D37 = [(mT + 2mF + mt)L — mFPf] sin 332,

¿.о = —mtpt cos q4i, D47 = : —mtPt sin q4i,

¿56 = [(mT + 2mF + 2mt)L - - mtpt] cos 542,

D57 = [(mT + 2mF + 2mt)L • — mtPt] sin 542,

D66 = mT + 2mF + 2mt, D67 = 0, D77 = mT + 2mF + 2m;

e (q) = t =

0 —L cos q31 L cos q32 —L cos q41 L cos q42 1 0 0 —L sin q31 L sin q32 —L sin q41 L sin q42 0 1

Выражение (9) представляет собой систему семи линейных алгебраических уравнений, содержащую девять неизвестных: семь компонент вектора q+ и две составляющие FX и FZ силы F. Скорость

конца опорной ноги в момент перед ударом равна нулю, поэтому Х- = Z- = 0. Чтобы замкнуть систему (9), добавим к ней уравнения, отражающие тот факт, что переносимая нога после удара не проскальзывает и не отскакивает от поверхности ступени:

-Г) = дп = о

или

Е = 0. (10)

Перепишем уравнения (9) и (10) в матричном виде:

( D(q-) -Ет((-) ) ( ) = ( !)((-)Г

V E(q-) ©2x2 / U/ V ©2x1

Обозначим через A(q-) матрицу системы (11):

(11)

л(д-) Л D)(q-) -EVM

A(q ) V E(q-) ©2x2 /

) ©2х2

Пусть Л-1 — элемент матрицы Л-1^-) с индексами г, Запишем выражение для первых пяти компонент вектора д+:

7 5

= ^ ^^¿кС , г = .

¿=1 к=1

Чтобы найденные значения скоростей звеньев робота в момент после удара можно было использовать в качестве начальных условий системы (6), их необходимо переобозначить так же, как и координаты q. Тогда

q+ = Aq (q-)q-, (12)

где Aq(q-) = UAq(q-), Aq (q-) — квадратная матрица пятого порядка с элементами

7

(Aq)ik = ^2 A-j^DDjk, i, k = 1, 5.

j=i

Объединяя выражения (8) и (12), получим основное соотношение для фазы перехода робота с одной ноги на другую:

=(q+)=()=д(х-)- (13)

Суммарная модель движения двуногого робота с учетом соотношений (6) и (13) принимает следующий вид:

x =

/(x) + 5^ gj (x)u, x i S j=1

x+ = A(x-), x- i S.

(14)

Нормальная форма и уравнения нулевой динамики. Рассмотрим аффинную систему с т управлениями и т выходами:

x = / (x

f (x) + Y1 gj (x)u,

j=1

yi = hi(x),

(15)

ут ^т(х)

где х е Мп, /(х), & (х) — гладкие векторные поля, ^¿(х) — гладкие функции, определенные в некоторой области пространства . Будем обозначать через Lf Л,(х) производную Ли функции Л,(х) по векторному полю / (х), вычисляемую по формуле

Lf Мх) = дх/(х).

Система (15) имеет векторную относительную степень (г1, ..., гт) в точке х0, если выполнены следующие условия [5]:

1) LgjLk^¿(х) = 0 при г, = 1,т, к < г — 1 и при любых значениях х из некоторой окрестности точки х0;

2) квадратная матрица т-го порядка

A(x) =

( LgiLf1-1hi(x)

Lg„ Lr/-1h1(x) \

Jgm-

Jgm-

Lg„ L?-1h2(x)

V Lgi Lfm 1h

(x)

LgmL/m 1hm(x) /

не вырождена в точке х = х0.

Используя введенное определение, можно показать, что если система (15) имеет векторную относительную степень (г1, ... ,гт) в точке х0, то справедливо неравенство г1 + ... + гт < п. Обозначим

Ф1 (х) = ^г(х), ^2 (х) = Lf ^¿(х),

# i (x) = Lfi-1 hi(x).

Пусть также г = Г1 + ... + гт. Если г = п, то система (15) приводится к каноническому виду. Если выполняется строгое неравенство г < п, то всегда можно найти п — г дополнительных функций ^г+1(ж),..., ^п(х) таких, что матрица Якоби отображения

Ф(х) = ($(х),..., <(х),..., ^*(х),..., С, (х), ^(х),..., ^(х))т

не вырождена в точке х0. Следовательно, отображение Ф(х) задает локальную замену переменных в окрестности х0. Значения дополнительных функций в точке х0 могут быть выбраны произвольно. Кроме того, если распределение й = 8рап(дь... , дт} в окрестности х0 ин-волютивно, то функции ^г+1(х),..., ^п(х) всегда можно выбрать так, чтобы выполнялись условия

Ь9] ^(х) = 0 (16)

для любых значений г = г + 1, п, ] = 1, т при всех х из некоторой окрестности точки х0.

Введем обозначения

I £i \

£2

£ =

\£Г<

( ^i(x) \

V ^ (x)

i = 1, m,

£ =

( £1 \

£ 2

Vr7

n =

( ni \

П2

^ ^r+i(x) \ ^r+2(x)

) \ Mx) /

Тогда в переменных £, n система (15) примет следующий вид:

£ = £2,

£i _ £i

£ri-i = Sri,

m

£Гг = Ьг(х) + ^ а^ (x)uj, i = 1,m,

j=i

(17)

¿=1 у» = ?!,

где х = Ф-1(?, п). В случае инволютивности распределения С выполняются условия (16), что во введенных обозначениях эквивалентно

возможности записать подсистему, образованную последними п — г уравнениями системы (17), в виде

п = Я^П^

не содержащем в правой части управлений щ, ] = 1, т.

Система (17) называется нормальной формой системы (15) в окрестности точки ж0.

Система обыкновенных дифференциальных уравнений (п — г)-го порядка

п = ?(0, п)

называется системой уравнений нулевой динамики.

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

а) туловище человека составляет приблизительно постоянный и небольшой угол с вертикалью;

б) таз находится на одинаковом расстоянии от ступней по оси X;

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

Зададим выходы к системе (14) в виде

У =

/уЛ

У2 Уз \У4/

( hi(x) ^

V Мж) )

(

= h(x) =

qi - qf

di + d2 7d

\

Zh - ZdH(d2) v Zi - Zd(d2) y

(18)

где qd — некоторое желаемое постоянное значение угла наклона туло вища,

di = XH - Xi = L(sin q4i + sin q3i), d2 = Xh - X2 = L(sin q42 + sin q32),

(19)

^я №) — желаемое изменение высоты тазобедренного сустава в зависимости от величины ¿2, выбираемое в виде полинома второй степени:

4 / V 2

+ (ZHfin — ZH min)-J2

d2 - y) ^2 + y ZH (d2) = Zh min + (Zh max Zh min)-^72--ь

16

d2 - I d2 +

12

4

ZHmin, max, ZHfin — минимальное, максимальное и конечное значения высоты ZH в пределах шага в системе координат X2, Z2 с началом в конце опорной ноги; Zf(d2) — желаемое изменение высоты конца переносимой ноги в зависимости от величины d2, также выбираемое в виде полинома второй степени:

Zd(d2) = -1z

d2 - — ) ( d2 - ^

3iX

X

+ k-z

d2 - ) ( d2 + ^

3-X

+ -

d2 - -X ) ( d2 + ^

-X

Z"

-2

16 4

к > 1 — коэффициент, задающий максимальное значение к/^ высоты в пределах шага в системе координат Х2, .

Функция (¿2) построена так, чтобы высота ^я при точном воспроизведении траектории ^Я(¿2) достигала минимального значения т1П при = —/2, что соответствует положению робота в начальный момент фазы одноопорного движения, т. е. сразу после удара; максимального значения тах при = /4 и некоторого значения при = /2, что соответствует положению робота в момент окончания фазы одноопорного движения, т. е. в момент перед следующим ударом.

Функция выбрана так, чтобы выполнялись условия

-£) = --Z, Zfl^

2

X

4

= k-z, Z?( f) = -z.

Как видно из выражения (18), выход у 1 представляет собой отклонение угла д 1 от д^. Выход у2 выбран так, что выполнение условия у2 = 0 обеспечит расположение тазобедренного сустава в точности посередине между концами опорной и переносимой ног. Выходы у3 и у4 позволяют проследить изменение высоты тазобедренного сустава и высоты конца переносимой ноги в зависимости от величины d2 и сравнить их с желаемыми траекториями ^Я (¿2) и ^ (^2).

Приведем систему (6) с выходом (18) к нормальной форме. Поскольку все функции ^ (ж) зависят от переменных дк, к =1, 31, 32, 41, 42, и не зависят от переменных дк, то для любых значений вектора ж справедливы равенства

^ ^(ж) = 0, г,; = Т~4.

4

Можно показать, что матрица

( Lffl Lf hl(x) ...L94 Lf \

=

V Lg1 Lf ^(ж) ...Ьд4 Lf ^(ж) )

не вырождена. Следовательно, для системы (6) с выходом (18) определена векторная относительная степень, равная (2, 2, 2, 2), тогда как порядок п системы (6) равен 10. Это означает, что необходимо подобрать еще две функции 71(5, д) и 72(5, д) так, чтобы отображение

Ф(х) = (hi,Lf hi,h2,Lf h2,hs,L/h^h^L/71 (q, q), 72(9, q))T

задавало замену переменных. Можно показать, что распределение G(x) = span{g1(x),g2(x),g3(x),g4(x)} инволютивно. Следовательно, функции y1, y2 можно подобрать так, чтобы выполнялись условия Lgj 7г = 0, i = 1, 2, j = M.

Выберем функцию Y1(q, q), зависящую только от координат:

Yi(q,q) = XH (q) = L(sin 942 + sin 932). (20)

Для нахождения функции Y2(q, q) используем форму записи исходной системы в виде (5). Сложим все уравнения этой системы. Как следует из вида матрицы B, сумма ее элементов в каждом столбце равна нулю, поэтому

d 5 5 - ^(D(q)q)i + £ Сг(д) = 0

i=1 i=1

и, следовательно,

55

d

dt

4=1 i=1

55

+ Аз(д)дз2 + 1] Дг4(д)д41 + А5(д)^) = - X] (21)

г=1 г=1 г=1 ' г=1

Выберем выражение под знаком производной в равенстве (21) в качестве искомой функции 72(д, д). Перепишем его в виде

72(д,д)= (22)

где

= (^1(д),ш2(д),шз(д),ш4(д),ш5(д)) =

^ 5 5 5 5 5 \

^ Бц (д), ^ Б^д), ^ Бй(д), ^ Б«(д), ^ Б^д) . (23)

чг=1 г=1 г=1 г=1 г=1 /

Подставляя в правую часть равенства (21) вместо дк, к = 1, 31, 32, 41, 42, их выражения через новые переменные, получим правую часть последнего уравнения преобразованной системы (6).

Рассмотрим отображение £ = Ф(ж), задаваемое системой функций

С2г-1 = Мж), С2г = ^г(ж), г = 1, 4,

(24)

Со = хя (g), Сю = ^(д)д.

Обозначим О = (СъСз,С5,Сг,Со)т, С2 = (С2, С4, Се, Сз, С10Г и разрешим систему (24) относительно д и д. Для этого сначала решим систему уравнений

^¿(ж) = С2г-1, г = 1, 4, Хя (д) = Со

относительно дк, к = 1, 31, 32, 41, 42, учитывая область изменения переменных дк:

9 = Ф-1(С1) =

( Zi + qd

d

3п . 7(Сз - Z9)2 + w2(Z) . Сэ - Z9

— arcsin --—--arcsin

2 2L \/(Сз - С9)2 + w2(Z)

3п -У С2 + w2(Z) . С9

— arcsin —-—--arcsin

2 2L VC! + w2 (С)

П + . У(Сз - C9)2 + w2(C) . Сз - C9

—+ arcsin —----arcsin —. =

2 2L V(C3 - C9)2 + w2(C)

^ . VC92 + w2(C) . C9

— + arcsin —----arcsin

2 2L VC92 + w2(C) /

(25)

где

W1(C) = C7 - C5 + 0,5 (vo - vi) C92 + - ^i)C9 + (k0 -

W2(C) = -C5 + 0,5 vo C92 + ^oC9 + K0,

8 (^ 7 ^ ^ П

v0 = -ß ZH max - ZHfin - 3ZHmin^j , v1 = 3-2 (2k - 1),

fin — min 2lz

—--;-, — —j—,

lX lX

K0 — 1 ZH fin — 4 ZH max — 1 min, K1 — — 2 lZ (2k — 1).

2 3 6 3

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

Lf hi(x) — г — 1,4, (26)

^(q)q— Clo,

где q — Ф-1((д). Все выходы h¿(x), г — 1, 4, зависят только от переменных q, поэтому функции Lfh¿(x) линейны по q. Следовательно, система (26) представляет собой систему линейных алгебраических уравнений относительно q¿, i — 1, 31, 32, 41, 42. Обозначим матрицу этой системы через R(q). Можно убедиться в том, что в области изменения q матрица R(q) не вырождена. Кроме того, вектор правой части системы (26) совпадает с вектором £2. Поэтому решение этой системы имеет вид

q — Ф-1(С) — R-1(q)C2, q — Ф-1(0). (27)

Объединяя выражения (25) и (27), получим

x—(q)--1<<)—(Ф-1!!

Таким образом, система функций (24) задает замену переменных, и эта замена переменных приводит систему (6) с выходом (18) к нормальной форме:

C2Í-1 — ^

4

Z2i — bi(x) + (x)Uj,

¿ — j(Z), (28)

Óo — ^2(C), Vi — Z2i-1, i — 1, 4,

где

bi(x) — L2hi(x), aij(x) —LgjLffc(x), х—Ф-1(С), ^(Z)—P(Z)/Q(Z),

P(Z) — L^1(q)s31-41S32-42 C2 + í^4(q) sin q31 — W2(q) sin q41 ]s32-42 Z4 + + [^2 (q) S32—42 cos q41 + ^3(q)s31-41 cos q42 + ^4 (q) S42—32 cos q31 +

+ W5(g)S4l-31 cos q32] Сб + [^4(0) cos 031 - ^2(q)cOS 041^32-42 Cs +

+ LS41-31S32-42 C10, 0 = Ф-1(0), (29)

Q(Z) = W2(q)[sin 041 + ((V1 -Vo)Z9 + -^0) COS 041]s42-32 + + ^3(q)[sin 042 + (V0C9 + ^0) COS 042^31-41 + + W4(q)[sin 031 + ((V1 - V0K9 + -^0) cos 031]s32-42 + + W5(0)[sin 032 + (^0С9 + ^0) cos 032]s41-31, 0 = Ф-1(0), (30)

g

^2(z) = gmTPT sin(z1 + 01) + 2L [mF Pf + mt(L + Pt)]z3-

g

- g(mT + 2mF + 2mt)C9 + — [m^PF + mt(L - Pt)] x

x ,W1(C^ - c"+ «Ж) -1+W2(zVÄ -1 (31)

Как видно из выражений (29)-(31), функция ^(С) линейна по компонентам вектора £2, а функция ) зависит только от вектора (д.

Введем новое управление V = 6(ж) + А(ж)м, 6(ж) = (61 (ж),... . ..,64(ж))т. Поскольку матрица А(ж) не вырождена, это равенство, рассматриваемое как уравнение относительно м, может быть разрешено следующим образом:

м = А-1 (ж)^ - 6(ж)]. (32)

После введения нового управления подсистема из первых восьми уравнений системы (28) примет вид

С2г-1 = С2^ __(33)

¿г = V», г = 1,4.

Рассмотрим подсистему системы (33) с фиксированным номером г, г = 1,4, и выберем управление V» так, чтобы нулевое решение С2г-1 = 0, С2г = 0 этой подсистемы было асимптотически устойчивым. Выполнение этого условия обеспечит выбор V» в виде

V» = ^(С2г-1, С2г) = -К»,^ - Кг^г-Ъ (34)

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

<^-1 = C2г, (35)

^2» = —К»,1 С2» — К»,2 С2»-1

становится линейной, причем коэффициенты ее характеристического уравнения совпадают с числами К»,1, К»,2:

Л2 + К»,1 Л + К»,2 = 0.

В силу положительности величин К^д, все корни этого уравнения имеют отрицательные действительные части. Следовательно, все решения системы (35) сходятся к нулю экспоненциально.

Закон управления и(х) найдем, подставляя в соотношение (32) вместо V векторную функцию

и учитывая соотношения (24).

Построение периодического решения. Обозначим правую часть построенной замкнутой системы через /с1(х):

и запишем модель шагания двуногого робота с коленями в виде

Из множества решений системы (36) можно выделить единственное, если в произвольный момент времени £°, принятый за начальный, задать условия для координат д и угловых скоростей ш. Получающееся решение х(£, £°,х°) будет удовлетворять всем требованиям, заданным при конструировании управления: за конечный момент времени Т3 угол наклона туловища д1 приблизится к д^, траектории движения таза и конца опорной ноги станут параболическими, положение таза зафиксируется посередине между концами ног. Недостаток построенной модели заключается в том, что невозможно заранее, зная лишь начальное условие х(£°) = х°, предсказать поведение робота в последующие моменты времени: может оказаться, что условие х-(£) € Б перемены роли ног выполнится раньше, чем наступит момент £ = Тв, а это значит, что построенное управление и не успеет стабилизировать движение системы. Кроме того, в этом случае вектор х+(£), используемый в качестве начального условия для системы х = /с1(х), от шага к шагу будет принимать разные значения, и нет гарантии того, что через определенное число шагов х+ не станет таким, что выходящая из него траектория системы х = /с1(х) вообще не вернется на поверхность Б, т. е. следующего удара ноги о поверхность не будет и робот упадет.

Для устранения указанного недостатка поступим следующим образом: построим периодическое решение системы (36) и будем использовать в качестве начальных условий для этой системы значения вектора х в момент перед ударом ноги о поверхность Б.

/^l(Cl,C2) \

V ^4(Сг,Сз) /

fcl(x) = /(x) + g(X)M(X),

x(t) = /ci(x(t)), x-(t) £S, x+(t) = A(x-(t)), x— (t) e S.

(36)

(37)

Положив в (28) Z = 0, i = 1, 8, и обозначив 6 = £2 = Сю, получим уравнения нулевой динамики для пятизвенного робота, перемещающегося по лестнице, на фазе одноопорного движения:

6 = «(6)6, 6 = в (6),

где функции а (6) и в (6) вычисляются по формулам

a(£i) = Ls4i-3iS32-42/Qü(^i), (38)

Qo(6) = W2(q)[sín q4i + ((Vi - Vo)£i + Vi -^o) COS q4i]s42-32 + + ^3(q)[sin q42 + (Vo^i + ^o) cos q42]s3i-4i+ + W4(q)[sinq3i + ((Vi -Vo)£i + ^i-^o) cos q3i]s32-42+ + ^5(q)[sin532 + (Vo£i + ^o) cos q32]s4i-3i,

q = ф-(6), 6 = (0, 0,0, 0,6),

g

в(6) = 9ттPt sin qf-д(тт+2mF+2mt)(i + — [mppf+mt(L-pt)]x

x| (íl)l/-1+tt% (íl)v «í+4íte)-1 (39)

wi0(«i) = 0,5 (vo - vi) + (^o - + (ко - ki), W20 (6) = 0,5 Vo «2 + ^o«i + ко

Найдем, как в координатах «i, «2 запишется условие перехода робота с одной ноги на другую. С учетом ограничений нулевой динамики h (x) = 0, i = 1, 4, значение вектора q в момент перед ударом можно найти, решив систему нелинейных алгебраических уравнений

hi(x) = 0 i = м,

Zi(q) = lz . ( )

Обозначим ее решение через q- = (q-, q—, q-2, q—, д-2)т. Заметим, что при ограничениях нулевой динамики значение вектора q в момент после удара можно найти по формуле (13): q+ = Aqq-. Тогда условие наступления фазы перехода при использовании координат «i, «2 запишется в виде

= L(sin q-2 + sin q-2) = 0,5 lx.

С учетом переобозначения переменных q на фазе перехода значение «1 в момент после удара определяется по формуле

= L(sin q- + sin

и в силу условия = 0 получаем

£+ = -L(sin q-2 + sin = -0,5 .

Найдем закон изменения при ударе координаты £2. Значение в момент после удара в силу соотношений (12) и (22) можно выразить через вектор скоростей q- в момент до удара:

= ^(q+)Aq (Г)Г. (41)

Угловые скорости q-, k = 1, 31, 32, 41, 42, в момент перед ударом можно выразить через £-, решив систему линейных алгебраических уравнений

Lf л,г(/- ,q-) = 0, i = I~4, w(q-)(j- =

относительно q-, получающуюся из системы (26) при наложении на нее ограничений Z2i = 0, i = 1, 4. Пусть r-1(q-) — элемент матрицы R-1(q) с индексами j, /, а ^(q-) — вектор-столбец с компонентами 1 ), j = 1, 5. Тогда

Г = Д-1Г) е-1 = *(Г)&-. (42)

--^ / в4х1

<2

Объединив выражения (41) и (42), окончательно получим

<2+ = 5<-, (43)

где 5 = ш(д+)А4(д-)а(д).

Найдем значение <2 в момент перед ударом переносимой ноги о поверхность ступени, обеспечивающее периодичность решения системы (37) совместно с соотношением (43).

Обозначим через ¿0 и ¿1 два последовательных момента удара ноги о поверхность лестницы. Из проведенных рассуждений следует, что ^(£0) = -0,5, <^(¿0 = 0,5. Для того чтобы движение робота было периодическим, необходимо, чтобы функция <1(^) = (£) монотонно возрастала на интервале (¿0,^1). Тогда <;1 > 0 для любого £ € (¿0,£а) и система (37) эквивалентна обыкновенному дифференциальному уравнению первого порядка с разделяющимися переменными:

^ = . (44)

¿<1 а(<1)<2

Интегрируя уравнение (44) на отрезке [¿0,£1], получим

- <<+)2 + ? Ш ¿е. (45)

2 2 J а«)

-0,5lX

Обозначим

* й)=2 / ш *

-0,51х

Предположим, что в момент перед следующим ударом, т. е. при <1 = 0,5/х, выполняется условие периодичности <2 = <-. Подставив данные значения < 1 и < 2 в формулу (45) и учитывая соотношение (43), получим

(<2-)2 = 52(<-)2 + * (0,51х),

откуда имеем

(ST)' = ^ ■ (46)

Выражение (46) определяет значение i-, соответствующее периодическому движению робота, только в случае, когда правая часть равенства (45) положительна при всех ^ G [—0,5ZX, 0,5/X], т.е. выполняется условие

¿2(i-)2 + F(ii) > 0 Vi! G [—0,51x, 0,51X],

или

(i-)2 > max —= —1 min F(ii)■ (47)

Описанный способ вычислений позволяет найти (i-)2, но не i-. Тем не менее, величину i2- можно определить единственным образом, если потребовать, чтобы ^ в момент перед ударом была положительной:

= a(0,51x)i- > 0,

что соответствует движению робота слева направо. Таким образом,

'F (0,51X ) V/2

i- = Sign(a(0,5/x))

1 - ¿2

Численное моделирование. Проведем численное моделирование в соответствии с описанным алгоритмом. Выберем численные значения параметров: тт = 20 кг, = 3,2 кг, т_р = 6,8 кг, = 2,22 кг-м2, 7 = 0,93 кг-м2, = 1,08 к™2, Ь = 0,4 м, ЬТ = 0,625 м, р = 0,128 м, ря = 0,16 м, рт = 0,2 м.

Определим размеры лестницы, задав длину и высоту ступени: = 0,32 м, = 0,08 м.

Параметры, определяющие желаемые траектории тазобедренного сустава и конца переносимой ноги, выберем следующими: к = 1,2, тт = 0,7м, тах = 0,78 м, = 0,76 м. Желаемый угол наклона туловища примем = —п/30.

Рис. 2. Решение уравнения N(£i) = 0

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

При выбранных числовых параметрах решение системы уравнений (40) дает следующие значения переменных qi, г = 1, 31, 32, 41, 42, в момент перед ударом при наличии ограничений нулевой динамики:

= -п/30, д.- = 3,85, qз2 = 3,12, = 2,89, = 2,75.

Условие наступления фазы перехода в координатах , £2 примет вид £- = 0,16 м. Значение переменной в момент после удара равно -0,16 м. Величина 5, задающая скачок переменной £2 на фазе перехода, равна 0,81 .

Согласно формуле (46) имеем (£-)2 = 922,36. Значение £- принимаем равным -30,4, так как а(0,16) = -0,032 < 0.

Убедимся, что найденное значение £- задает периодическое движение. Для этого проверим выполнение условия (47). Как видно из графика, приведенного на рис. 2, функция

N )

ß (6) a(£i)

на отрезке [-0,16, 0,16] монотонно возрастает и имеет единственный нуль £ *, соответствующий точке минимума функции ^ (£1).

Численное решение уравнения N(£1) = 0 на отрезке [-0,16, 0,16] дает следующий результат:

£* « -0,048.

Соответственно, имеем

min F(£1) = F(£í) w -168,5, -F(|í) w 258,8, Ci e[-0,i6,0,16] v ' v 1y 52

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

Соответствующее решение системы (37) совместно с соотношением (43) изобразим в виде кривой на плоскости £1, £2 (рис. 3).

Точка A соответствует состоянию робота в момент после удара. На фазе одноопорного движения изменение переменных £1, £2 описывается дугой ABC, причем координаты точки C совпадают со значениями £1 и £2 в момент перед ударом. Отрезок CA, показанный на рис. 3 пунктиром, соответствует фазе перехода робота с одной ноги на другую. При этом переменная £1 скачком меняет свое значение от = 0,16 м до = -0,16 м, а £2 меняется в соответствии с соотношением (43).

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

х- = (-П, 3,85, 3,12, 2,89, 2,75, 0, 0,19, -1,07, 2,37, -1,48^т.

Чтобы построить само периодическое движение, необходимо решать систему х = /с1 (х), х(0) = Д(х-) до выполнения условий Z (q) = lZ, X1 (q) > Xq (q), означающих окончание фазы одноопорного движения. Непосредственные вычисления показывают, что для выбранных значений параметров это условие выполняется при t = 0,38 с. Изобразим на рис.4 последовательные положения робота на фазе

_321_I_I_I_I_I_I_I_I Рис.4. Последовательность

- 0 02 - ОД 0 ОД ~ г

1 состоянии робота при пе-

Рис. 3. Периодическое движение по лестнице риодическом движении по

при ограничениях нулевой динамики лестнице

одноопорного движения. Изменение выходов у1, у2, высоты тазобедренного сустава, высоты конца переносимой ноги и управлений щ, г = 1,4, в пределах пяти шагов показано на рис. 5-12.

Угол в начальный момент времени £ = 0 равен -п/30 рад. Из графика, приведенного на рис. 5, видно, что при изменении £ от 0 до 0,08 с отклонение туловища от вертикали увеличивается по абсолютной величине до 0,114 рад. В последующие моменты времени туловище возвращается в исходное положение и при £ = 0,38 с принимает значение, приближенно равное -п/30 рад. Аналогичная зависимость имеет место и на следующих четырех шагах.

рад

-0,106 -0,107 -0,108 -0,109 -0,11 -0,111 -0,112 -0,113

0 0,08 0,18 0,3 0,4 0,5 0,8 0,7 0,8 0,9 1 1,06 1,181,28 1,4 1,5 1,6 1,7 1,8

Рис. 5. Изменение выхода у\ в пределах пяти шагов

с11 + с12, м

0,045 0,04 0,035 0,03 0,025 0,02 0,015 0,01 0,005

0 0,08 0,18 0,28 0,38 0,5 0,6 0,7 0,8 0,9 1 1,06 1,16 1,26 1,38 1,48 1,58 1,68 1,78 1,9 I, С

Рис. 6. Изменение выхода у2 в пределах пяти шагов

m

0 0,08 0,18 0,28 0,38 0,48 0,58 0,68 0,78 0,88 0,98 1,08 1,18 1,28 1,38 1,48 1,58 1,68 1,78 1,88 t, С

Рис. 7. Изменение высоты ^и тазобедренного сустава в пределах пяти шагов

Zb м

0 0,08 0,18 0,28 0,38 0,48 0,58 0,68 0,78 0,88 0,98 1,08 1,18 1,28 1,38 1,5 1,6 1,7 1,8 1,9 t, С

Рис. 8. Изменение высоты конца переносимой ноги в пределах пяти шагов

Величина + ¿2, определяющая горизонтальное положение таза относительно концов опорной и переносимой ног, в момент времени £ = 0,08 с достигает максимального значения 0,048м (рис.6). При £ > 0,08 с величина + ¿2 убывает и в момент £ = 0,38 с достигает значения 0, 005 м. Заметим, что величина + ¿2 во все моменты времени остается положительной. Это означает, что таз робота не расположен строго посередине между концами опорной и переносимой ног, а смещен в сторону переносимой ноги.

Тазобедренный сустав при £ = 0 находится на начальной высоте (0) ~ 0, 7 м (рис. 7). В момент времени £ = 0,28 с он поднимается на максимальную высоту, приближенно равную 0,78 м. При £ = 0,38 с достигает конечного значения 0,76 м. В момент удара величина

Ui, H • м

190 180 170 160 150 140 130 120 110 100 90 80 70 60 50 40 30 20 10 0

0 0,06 0,16 0,26 0,36 0,46 0,56 0,66 0,76 0,86 0,96 1,06 1,16 1,26 1,36 1,46 1,56 1,66 1,76 1,86 t, С

Рис. 9. Изменение управления (t) в пределах пяти шагов

и2, Н• м

о -20 -40 -60 -80 -100 -120 -140 -160 -180 -200 -220

О 0,08 0,18 0,28 0,38 0,48 0,58 0,88 0,78 0,88 0,98 1,08 1,18 1,28 1,38 1,48 1,58 1,88 1,78 1,88 t, С

Рис. 10. Изменение управления u (t) в пределах пяти шагов

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

Конец переносимой ноги в начальный момент времени находится на предыдущей ступени, поэтому Zi(0) = —lZ = -0,08м (рис.8). С течением времени Z1 возрастает, пока не достигает при t = 0,28 с максимального значения, равного 0, 096 м. До окончания фазы одно-

и3, Н • м

70 65 60 55 50 45 40 35 30 25 20 15 10

0 0,06 0,16 0,26 0,36 0,46 0,56 0,66 0,76 0,66 0,96 1,06 1,16 1,26 1,36 1,46 1,56 1,66 1,76 1,66 t, С

Рис. 11. Изменение управления Нз(^) в пределах пяти шагов

и4, Н• м

15 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0 -1

0 0,06 0,16 0,26 0,36 0,46 0,56 0,66 0,76 0,86 0,96 1,06 1,16 1,26 1,36 1,46 1,56 1,66 1,76 1,86 t, С

Рис. 12. Изменение управления и4(£) в пределах пяти шагов

опорного движения переносимая нога опускается на поверхность следующей ступени и при £ = 0, 38 с принимает значение 0, 08 м, равное высоте ступени 12. В момент удара происходит скачкообразное изменение величины . Это объясняется изменением роли ног после удара и связанным с ним переобозначением переменных.

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

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

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

1. Формальский А. М. Перемещение антропоморфных механизмов. - М.: Наука, 1982.-368 с.

2. G r i z z l e J. W., Abba G., Plestan F. Asymptotically Stable Walking for Biped Robots: Analysis via Systems with Impulse Effects // IEEE Trans. on Automatic Control. - 2001. - V. 46, № 8. - P. 51-64.

3. W e s t e г v e 11 E. R., Grizzle J. W., K o d i t s c h e k D. E. Hybrid Zero Dynamics of Planar Biped Walkers // IEEE Trans. on Automatic Control. - 2003. -V. 48, № 1. - P. 42-56.

4. P l e s t a n F., G r i z z l e J. W., W e s t e r v e l t E. R., A b b a G. Stable walking of a 7-DOF biped robot // IEEE Trans. Robotics and Automation. - 2003. -V. 19, №4.-P.653-668.

5. I s i d o r i A. Nonlinear Control Systems. - Berlin: Springer-Verlag, 1995.

6. К р и щ е н к о А. П., Ткачев С. Б., Фетисов Д. А. Управление плоским перемещением двуногого пятизвенного робота // Нелинейная динамика и управление: Сб. статей / Под ред.С.В. Емельянова, С.К.Коровина. Вып. 3. -М.: Физматлит, 2003. - С. 201-216.

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

Александр Петрович Крищенко родился в 1948 г., окончил в 1972 г. МГУ им. М.В.Ломоносова. Д-р физ.-мат. наук, профессор, зав. кафедрой "Математическое моделирование" МГТУ им. Н.Э. Баумана. Лауреат премии Правительства РФ в области науки и техники, академик РАЕН. Автор 160 научных работ в области математической теории управления и дифференциальных уравнений.

A.P. Krishchenko (b. 1948) graduated from the Lomonosov Moscow State University in 1972. D. Sc. (Phys.-Math.), professor, head of "Mathematical Simulation" department of the Bauman Moscow State Technical University. Winner of the RF Government Prize in the field of science and technology, full member of the Russian Academy of Natural Sciences. Author of 160 publications in the field of mathematical control theory and differential equations.

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

S.B. Tkachov (b. 1961) graduated from the Bauman Moscow Higher Technical School in 1984. Ph. D. (Phys.-Math.), assoc. professor of "Mathematical Simulation" department of the Bauman Moscow State Technical University. Winner of the RF Government Prize in the field of science and technology. Author of 50 publications in the field of non-linear control theory.

Дмитрий Анатольевич Фетисов родился в 1980 г., окончил в 2003 г. МГТУ им. Н.Э. Баумана. Аспирант кафедры "Математическое моделирование" МГТУ им. Н.Э. Баумана. Автор научной работы в области нелинейной теории управления.

D.A. Fetisov (b. 1980) graduated from the Bauman Moscow State Technical University in 2003. Post-graduate of "Mathematical Simulation" department of the Bauman Moscow State Technical University. Author of a publication in the field of non-linear control theory.