удк 004.9 решение задач ходьбы антропоморфного механизма в

УДК 004.9
РЕШЕНИЕ ЗАДАЧ ХОДЬБЫ АНТРОПОМОРФНОГО МЕХАНИЗМА В
САГИТТАЛЬНОЙ ПЛОСКОСТИ С ИСПОЛЬЗОВАНИЕМ ПРОГРАММНОГО
ПАКЕТА MATLAB
Дынник Т.В., магистрант электротехнического факультета,
Дынник И.В., ст.преподаватель каф. ВМиП
Донецкий национальный технический университет
[email protected]
В данной работе рассматривается применение программного пакета
MATLAB для решения задачи динамической ходьбы антропоморфного механизма
без стоп на примере синтеза системы регулирования ходьбой в сагиттальной
плоскости.
Дана робота присвячена використанню програмного пакету MATLAB для
розв’язання задачі динамічної ходи антропоморфного механізму без стоп на
прикладі синтезу системи регулювання ходою в сагітальній площині.
This article describes application of the software suite MATLAB for researches
of a dynamic walking of anthropomorphic robot without feet, particularly a synthesis
of the control system in sagittal plane.
Ключевые слова: антропоморфный механизм, динамическая ходьба,
система управления, синтез, теория колебаний, моделирование, программный
пакет MATLAB.
Рассмотрим механическую систему, которая состоит из трех звеньев,
соответствующих двум ногам и корпусу, расположенных в плоскости и
шарнирно-соединенных. Система может находиться в одноопорной фазе, когда
опорная нога находится на контактной поверхности, а переносимая является
свободной, и в двухопорной фазе, когда обе ноги стоят на контактной
поверхности. В качестве критерия идентификации опорной ноги выбираем
величину реакций опоры, в случае же их равенства для обеих ног, принимаем
правую ногу за опорную по умолчанию. Точку контакта опорной ноги O
принимаем за начало координатной системы, в которой рассматривается
механическая система.
Рассматривая данную систему как колебательную, в качестве правила
разгона можно указать наклон корпуса относительно вертикали в сторону разгона
(по аналогии с маятниковыми движениями [1]).
(1)
Задачу управления целесообразно разбить на две плоскости –
фронтальную и сагиттальную, и составить управление для каждой плоскости
отдельно. Целью управления в сагиттальной плоскости является формирование
динамической ходьбы, представляющей из себя периодический процесс. При
этом формирование ходьбы подразумевает комплексное управление
электромеханической системой, включающее в себя управление коленным
суставом, межзвенным суставом и наклоном тела.
Для решения поставленной задачи рассмотрим пятизвенный механизм,
включающий корпус и 2 ноги (рис. 1), каждая из которых состоит из двух
звеньев. Система не закреплена на опоре, но к ней прикладываются реакции
опоры.
Рисунок 1 – Объект управления для системы формирования ходьбы
Обозначим углы как φ1 – угол опорной ноги относительно вертикали, φ2 –
межзвенный угол и φ4 – положение корпуса относительно опорной ноги, φb – угол
корпуса относительно вертикали, φk1 – угол коленного сустава опорной ноги, φk2 –
угол коленного сустава переносимой ноги.
Параметры рассматриваемого механизма: массы тел m1.1 = m1.2 = m2.1 = m2.2
= 2.2 кг, m4 = 12 кг; момент инерции относительно центра масс J1.1 = J1.2 = J2.1 =
J2.2 = 0.02 кг·м2, J4 = 0.001 кг·м2; длинны звеньев a1.1 = a1.2 = a2.1 = a2.2 = 0.35 м, a4
= 0.5 м; центры масс расположены по центру звеньев.
В модель объекта также входит расчет реакции опорных суставов в
зависимости от положения и скорости точки контакта. По известному вектору
состояний системы определяется величина реакций опоры для обеих ног.
Система управления строится по принципу децентрализованного
регулирования [2] путем введения дополнительных обратных связей.
При рассмотрении задачи управления межзвенным углом необходимо
описать задачу, которая выполняется системой управления. Задачей,
выполняемой приводом в межзвенном суставе является перенос ноги и ее
постановка с определенной, наперед заданной длиной шага. Для решения данной
задачи рассмотрим систему ног, предполагая, что коленные суставы не
принимают участия в переставлении ног – углы коленных суставов держатся в
нуле, а опорная нога шарнирно закреплена на контакте (что справедливо для
положительных реакций опоры). Рассмотрим полученный плоский двухзвенный
механизм – циркуль, состоящий из двух весомых звеньев. Из уравнения
Лагранжа получаем следующие выражения, описывающие динамику системы




M 1  b1  2b2  a2  cg 2 y  cos  cg 2 x sin    b3  b2  a2  cg2 y  cos  cg2 x sin   





+m g  cg

    cg
 2m a 
+m2 g cg 2 x cos (   )   a2  cg2 y  sin(   )  a1 sin( ) 
1x
1

cos   a1  cg1 y  sin   m2 a1 2

2 1
2x
cos   a2  cg2 y  sin


M 2  b3  b2  a2  cg 2 y  cos  cg 2 x sin    b3 






 m2 g cg 2 x cos(   )   a2  cg 2 y  sin(   )  m2 a1  a2  cg2 y  sin  cg2 x cos  2

 
2
2
b1  I1  I 2  m1  a1  cg1 y   cg1x 2  m2  a2  cg 2 y   a12  cg 2 x 2

b2  m2 a1

2
b3  ( I 2  m2  a2  cg 2 y   cg 2 x 2

(2)
Для свободного движения M1=0 и M2=0 решим систему уравнений
относительно старших производных с учётом матрицы инерции системы. Для
полученной системы дифференциальных уравнений с помощью программного
пакета MATLAB решается краевая задача, задаются симметричные
конфигурации до и после совершения шага. Межзвенная скорость равна нулю.
Это условие позволяет приходить в удар сваренным механизмом, тем самым
добиваясь меньшего влияния удара на систему. Неизвестными параметрами
являются скорость опорной ноги перед совершением шага и время шага. Для
полученных решений выбирается функция, реализующая полученную кривую:
k 
 2 s  frsi( 1 )   2 max  th( s 1 )
 2 max ,
(3)
где
φ2max – длина шага; ks – константа механизма.
Исходя из очевидности процесса ходьбы, запишем функцию колена:
(4)
Полученные функции управления коленом и межзвенным суставом
сформировали управление системой ног. Механизм способен переставлять ноги,
подгибая колено. Принцип управления ходьбой построен на основе расчета
полной энергии механической системы рассчитываемой по аналитическим
выражениям. Задание на угол корпуса:

 4 soll    2   4 soll
2
(5)
На основе рассчитанного значения энергии в системе строится управление
на основе градиентной методики [3]. Это приводит к записи регулятора в форме
V
 4 soll  u   ES  E 
  E K  E P  ES    4 max sign( 1 )
x
(6)
Рассмотрим полученную в итоге структуру. Исходная механическая
система, записанная матрично в виде системы дифференциальных уравнений
первого порядка, имеет следующие векторы состояний x и входа u:
x T  1  2  4  k 1  k 2 1  2 4  k 1  k 2 xO yO xO y O ,
u  M 1 M 2 M 4 M k 1 M k 2 R x1 R x 2 R y 1 R y 2


(7)
где Mi – вращающий момент в i-м суставе;
Rx1, Ry1 – силы, действующие на тело в точке опоры;
Rx2, Ry2 – силы, действующие на тело на конце переносимой ноги;
xO, yO – глобальные координаты точки опоры О.
Силы, действующие на механизм в концах ног, соответствуют реакциям
поверхности и зависят только от свойств поверхности и текущего вектора
состояний системы. Расчет этих реакций выполнен согласно [4]. Приложение
момента в точке опоры невозможно. Моменты в суставах реализуются
электроприводом. Каждый двигатель имеет управление по положению и передает
усилие через редуктор и шнековую передачу, за счет чего в каждом суставе
формируется жесткий контур положения с заранее известной динамикой.
Полученная система представляет собой фактический MIMO объект, с которым
должна справится система регулирования. После введения обратных связей по
управлению коленом и межзвенным углом, в системе остается вектор входа и
состояний
x T  1  2  4 1 2 4 ,
u  4 soll 
(8)
Структура системы управления представлена на рисунке 2.
Для моделирования работы системы управления использовался
программный пакет MATLAB. Для моделирования механического объекта
использовалась среда Simulink, позволяющая моделировать динамические
системы. Кроме базовых блоков расчета использовалась дополнительная
библиотека SimMechanics, которая предназначена для моделирования
механических систем. С использованием блоков этой библиотеки создана модель
пятизвенного
механизма,
описанного
ранее.
Согласно
изложенным
соображениям составлена система управления, которая осуществляет
регулирование ходьбой. В силу того, что в системе моделируются системы
электропривода, осуществляющие регулирование положения с известной
динамикой, для всех суставов кроме опорного производится расчет инверсной
динамики. Задавая известные траектории перемещения систем положения,
производится расчет прямой динамики в опорном суставе с учетом мгновенной
конфигурации системы в поле гравитации.
Рисунок 2 – Общая структура системы управления
Richtung vorwarts
152
E
Es
150
E, J
148
146
144
142
140
0
2
4
6
8
10
t, s
12
14
16
18
20
0
2
4
6
8
10
t, s
12
14
16
18
20
1, deg
20
0
-20
Рисунок 3 – Переходные процессы в системе (результат моделирования)
Рассмотрим результаты численного эксперимента. Графики, полученные
при ходьбе вперед и постоянной длине шага φ2max =20º, представлены на рис. 3.
Механизм начинает движение с места из вертикального положения и постепенно
набирает энергию, выходя на периодический режим работы, время шага
составляет 0.54 секунды (частота шагов 1.85 Гц, при длине шага 0.24м скорость
перемещения 0.45 м/с = 1.6 км/ч). При повышении задания система заметно
быстрее набирает энергию. Это объясняется приближенным значением прироста
за период, а с ростом частоты за то же самое время механизм успевает сделать
больше шагов. После повышения уровня энергии время шага составляет 0.49с
(частота шагов 2.04 Гц, при длине шага 0.24м скорость перемещения 0.98 м/с =
1.75км/ч). При задании уровня энергии меньшего, чем необходимо для
совершения шага, механизм снижает скорость и на 19-й секунде начинает
совершать шаги на месте.
20
10
-40
4, deg
1, deg/s
-20
-60
-80
-20
0
-10
-10
0
-20
-20
10
1, deg
-10
0
10
1, deg
20
20
4, deg
4, deg
10
0
0
-10
-20
0
-20
-80
-50
1, deg/s -100 -20
-60
-40
1, deg/s
-20
20
0
1, deg
Рисунок 4 – Фазовые траектории в стационарном режиме
Из переходного процесса выбран интервал, на котором явно виден
установившийся режим, и который соответствует 6 шагам. Фазовая траектория
при этом стремится к заданному аттракторному множеству. На рис. 4 отчетливо
виден установившийся режим работы.
Выводы. В данной работе представлены результаты синтеза и
моделирования системы управления ходьбой для электромеханической системы с
использованием программного пакета MATLAB, применяемого как при синтезе
системы управления, так и для моделирования динамики системы.
1. Kurt Magnus. Schwingungen. Eine Einfuhrung in die theoretische
Behandlung von Schwingungsproblemen. Tubner, 1986, s. 251.
2. Control system design / Graham C. Goodwin, Stefan F. Graebe, Mario E.
Salgado. - 2001. - 3000 copys.
3. Efimov D.V. Robust and adaptive control of nonlinear oscillations. – SPb.:
Nauka, 2005. – 314 p.
4. Contact processing in the simulation of CLAWAR. Juhasz Konyev, Rusin,
Schmucker. Proceedings of 10th International Conference(CLAWAR 2007),
Singapore, 16-18 July 2007.