Нелинейные динамические системы

ПРОБЛЕМЫ МЕХАНИКИ И УПРАВЛЕНИЯ
Нелинейные динамические системы
Вып. 43
Межвузовский сборник научных трудов
2011
УДК 621.865.8-5
Р.Г. Мухарлямов, Н.В. Абрамов
г. Москва
г. Самара
УПРАВЛЕНИЕ ДИНАМИКОЙ МАНИПУЛЯТОРА
С ПРОГРАММНЫМИ СВЯЗЯМИ1
Предлагается метод построения уравнений динамики
манипуляционных систем с учетом динамики приводов.
Целью управления является обеспечение движений манипулятора в соответствии с заданными программными
связями. Определяется структура уравнений программных связей и уравнений возмущений связей, обеспечивающих устойчивость интегрального многообразия, соответствующего уравнениям связей.
Введение
Процесс управления исполнительными органами роботаманипулятора предполагает решение совокупности разнообразных задач, связанных с осуществлением необходимых движений исполнительного органа и отдельных звеньев манипулятора. Постановка этих задач вполне естественно следует из анализа требований, предъявляемых к работе манипулятора, и возможностей, обусловленных его техническим оснащением. Анализ динамики робота-манипулятора с учетом приводов как
сложной управляемой механической системы с большим числом
степеней свободы и определение управляющих воздействий,
© Мухарлямов Р. Г., Абрамов Н. В., 2011
1
Работа выполнена при финансовой поддержке РФФИ, проект № 1001-00381.
90
Р.Г. Мухарлямов, Н.В. Абрамов. Управление динамикой манипулятора …
осуществляющих программное движение манипулятора, представляют самостоятельную задачу.
На первый взгляд кажется, что простейшим способом программирования движений робота-манипулятора является задание
законов изменения положения его основных частей. Однако даже
определение законов движения отдельных звеньев в соответствии
с внешними условиями, накладывающими ограничения на кинематические показатели движения, не обходится без решения системы
конечных уравнений. С другой стороны, динамика манипулятора
описывается посредством обобщенных координат, и для определения управляющих воздействий, обеспечивающих движение по
заданному закону, желательно программу движений выражать
также через обобщенные координаты. Рассматривая манипуляционные роботы как механические системы, составленные из связанных твердых тел, программу движений этой системы приходится
задавать как уравнения голономных и неголономных связей, наложенных на обобщенные координаты и скорости.
Совокупность уравнений динамики манипулятора и соответствующих уравнений связей составляет систему дифференциально-алгебраических уравнений, которые в последнее
время являются объектом интенсивных исследований [1, 2]. Основной проблемой в этих исследованиях является обеспечение
асимптотической устойчивости интегрального многообразия,
соответствующего уравнениям связей. В работе [3] для обеспечения устойчивости движения по интегральному многообразию
было предложено использовать уравнения программных связей
и их возмущений.
В настоящей работе описывается метод построения
уравнений динамики манипуляционных систем с программными
связями. Предлагается алгоритм решения задачи управления с
учетом динамики приводов при постоянно действующих возмущениях. Составляются уравнения возмущений связей, обеспечивающих устойчивость программного многообразия.
1. Определение положения многозвенного
манипулятора
Рассмотрим манипулятор, состоящий из n звеньев, соединения между которыми допускают прямолинейное поступатель91
ПРОБЛЕМЫ МЕХАНИКИ И УПРАВЛЕНИЯ – 2011
ное движение последующего звена по отношению к предыдущему звену или вращения последующего звена вокруг оси, связанной с предыдущим звеном. Пусть для i-звена ось Oi z i системы координат Oi xi y i z i , связанной с i-звеном манипулятора,
направлена вдоль оси сочленения, соединяющего i-звено с i+1звеном, Oi xi -ось, направлена вдоль общего перпендикуляра к
осям сочленений звеньев с направлением от Oi 1 zi 1 к Oi zi ,
i  0,1,  , n . Обозначим через  i угол между осями Oi 1 xi 1 и
Oi xi , si – расстояние от Oi1 до точки Ci1 пересечения оси
Oi 1 zi 1 с общим перпендикуляром к осям Oi 1 zi 1 и Oi zi , ai=Ci1Oi,  i – угол между осями Oi 1 zi 1 и Oi zi .
Введем
промежуточную
систему
координат
Oi xi 1,i yi 1,i zi 1,i . Если определить положение точки в трехмерном евклидовом пространстве Oi xi yi zi вектором проективного
пространства
Ri   xi ,1 , xi ,2 xi ,3 , xi ,4  ,
где
xi ,1  xi xi , 4 ,
xi , 2  y i xi , 4 , xi ,3  zi xi ,4 , xi , 4  1 , то переходу от Oi 1 xi 1 yi 1zi 1
к Oi xi 1,i yi 1,i zi 1,i и от Oi xi 1,i yi 1,i zi 1,i к Oi xi yi zi будут соответствовать матрицы
Ai 1,i

 cos i  sin i


 , A   sin i cos i
 i 1,i  0
0


0

 0
0
ai
1 0

0 cos  i  sin  i 0

 0 sin  i cos  i 0

0
1
0 0
0 0

0 0
.
1 si 

0 1
В зависимости от вида соединения обобщенной координатой qi, определяющей положение i-звена относительно преды-




дущего i-1-звена, является  i si  0 или si  i  0 . Положение i-звена относительно неподвижного пространства Oxyz
определяется матрицей Si  A01 A1 A12 A2 Ai 1,i Ai . Программу
движения манипулятора можно представить в виде зависимостей, определяющих значения отдельных координат звеньев ма92
Р.Г. Мухарлямов, Н.В. Абрамов. Управление динамикой манипулятора …
нипулятора, которые выражаются через обобщенные координаты, либо положения j1 ,..., jk – звеньев относительно i1 ,..., ik
звеньев, либо в виде связей между координатами отдельных
звеньев и их производными, выраженные через обобщенные
координаты и скорости. Обобщенные координаты qi ,введенные
таким образом, позволяют описывать поступательное движение iзвена манипулятора, движение центра схвата по заданной кривой
или поверхности, вращательные движения схвата и различные
неголономные программы, соответствующие его движениям.
2. Уравнения динамики манипуляционной системы
Для описания динамики манипуляционных роботов обычно используют уравнения Лагранжа второго рода. Представим
лагранжиан в виде разности L  K  q, v   P  q  . Кинетическая


энергия 2K  qT H  q  q , H  q   hij  q  , i , j  1,..., n , и потенциальная энергия P  Pq  манипулятора вычисляются как
суммы энергий всех звеньев:
n
n
K   Kk ,
P   Pk .
k 1
k 1
Кинетическая энергия k-звена определяется выражением
   R R dm V   tr V M V
2K k  tr Vk
k
T
k
k
T
k
k
T
k
k
,
Fk
Ek
mk xci 
  Bk  Ck  Ak  / 2


Fk
Dk
mk yck 
 Bk Ck  Ak  / 2

M k 
,
Ek
Dk
 Bk Ck  Ak  / 2 mk zck 


mk xck
mk yck
mk zck
1 

где Ak, Bk, Ck – моменты инерции относительно осей Okxk, Okyk,
Okzk; Ek, Fk, Dk –- центробежные моменты, xck, yck, zck – координаты центра масс в системе Okxkykzk, mk – масса k-звена.
Матрица Vk определяется выражением: Vk 
Uki  A01 A1 A12 A2
Ai1QAi 1,i Ai Ai 1
k
U
i 1
q,
ki i
Ak ,
93
ПРОБЛЕМЫ МЕХАНИКИ И УПРАВЛЕНИЯ – 2011
где матрица Q  Q1 для сочленения вращательного типа и
Q  Q2 для призматического подвижного соединения,
0 0 0 0 
0  1 0 0 
0 0 0 0 .
1 0 0 0  ,



Q2  
Q1 
0 0 0 1 
0 0 0 0 




0 0 0 0 
0 0 0 0 
Если потенциальная энергия манипулятора определяется
только однородным полем силы тяжести и положение центра
масс k-звена задается вектором Rck   xck , yck , zck , 1 , то
n
P    mk GSk Rck , G  0, 0, g , 0  .
k 1
3. Уравнения динамики манипулятора с учетом
динамики приводов
Динамика манипулятора с n степенями свободы описывается уравнениями Лагранжа второго ода [1, 2]:
dqi
 qi ,
dt
d  L  L
 wi ,


dt  qi  qi
i  1,
,n ,
(1)
где qi , qi – соответственно обобщенные координаты и скорости,
L – лагранжиан, L  K  P , K – кинетическая энергия, P –
потенциальная энергия манипулятора, wi – обобщенная сила,
действующая на звено с номером i . Полагая 2K  qT H  q  q ,


где H  q   hij  q  , i, j  1,
, n , – положительно-определен-
ная симметричная матрица, после проведения процедуры дифференцирования в левых частях уравнений (1) представим уравнения динамики в виде dq  q, H  q  q  C  q, q  q  p  q   w .
dt
Здесь q  Q0  Rn – вектор обобщенных координат, определяющих положение манипулятора, q  Q1  Rn – вектор обоб-
щенных скоростей; C  q, q  – матрица центростремительных и
94
Р.Г. Мухарлямов, Н.В. Абрамов. Управление динамикой манипулятора …
кориолисовых сил, зависящих от обобщенных координат и скоростей, причем компоненты вектора C  q, q  q  col C1 , , Cn 
представляют собой квадратичные формы относительно обобщенных скоростей; p  q   col  p1 , , pn  – вектор внешних
сил тяготения; w  Rn – вектор обобщенных сил или моментов
w1 , , wn , развиваемых исполнительным устройством.
Будем рассматривать задачу управления манипулятором, в
качестве исполнительных устройств которых используются электродвигатели постоянного тока. Для составления уравнений динамики двигателя запишем уравнения Кирхгофа для цепи якоря:
L
di  t 
dt
 Ri  t   e  t   u  t  ,
w  t   Ka iв  i  t  ,
(2)
где u – напряжение, L – индуктивность, R – сопротивление, i –
ток в цепи якоря, e – электродвижущая сила, пропорциональная
d
угловой скорости
вращения вала двигателя,

dt
e  t   K b  t    t  , Kb  t  – коэффициент пропорциональности,
w  t  – момент, развиваемый на валу, K a  iв  – коэффициент
пропорциональности. Ток в обмотке возбуждения
iв
и коэффи-
циент K a  iв  будем считать постоянными. С учетом сделанных предположений перепишем уравнение (2) относительно
момента m , развиваемого двигателем:
w
K K
K
R
w a b   a u.
L
L
L
(3)
Угол вращения двигателя  связан с угловым перемещением нагрузки  н отношением н  t   n  t  . Вводя обозначения R  a  0 , K a K b  d  0 , K a  b  0 , уравнение (3) можно
nL
L
L
представить в виде:
dw
 aw  dv  bu .
dt
95
ПРОБЛЕМЫ МЕХАНИКИ И УПРАВЛЕНИЯ – 2011
Окончательно уравнения динамики манипулятора с учетом
динамики приводов могут быть записаны следующим образом:
dq
dq
 q, H  q   C  q , q  q  p  q   w   ,
dt
dt
(4)
dw
  Aw  Dq  Bu.
dt
Здесь вектор  t   Rn , оценивающий неопределенные, но ограниченные вместе с производными внешние возмущения, A, D, B –
диагональные матрицы с положительными постоянными коэффициентами передачи ai , d i , bi , u – вектор напряжений ui , i  1,..., n
якорей электродвигателей, которые являются управляющими воздействиями, приложенными к звеньям манипулятора.
4. Управление программным движением
манипулятора
Представим систему уравнений (4) в виде, разрешенном
относительно производных
dq
 q,
dt
dq
  H 1  q   C  q, q  q  p  q   w    ,
dt
dw
  Aw  Dq  Bu.
dt
(5)
Рассмотрим следующую задачу. Пусть движения манипулятора ограничены уравнениями голономных и неголономных
связей, наложенных на обобщенные координаты и скорости
(6)
f  q, t   0 ,
f   q, q, t   0 ,
f   f 1,
, f m  , f    f m 1 ,
1
1
, f m m  , m1  m2  n .
1
2
Уравнения (6) могут определять, например, траекторию схвата
манипулятора и зависимости между прямоугольными координатами в трехмерном пространстве, определяющими положение
его центра, и проекциями скоростей этой точки, выраженными
96
Р.Г. Мухарлямов, Н.В. Абрамов. Управление динамикой манипулятора …
через обобщенные координаты. В частности, уравнениями (6)
можно задавать семейство траекторий центра схвата из начальной точки в конечную точку с обходом препятствий [4]. Вектор
u , задающий законы управления приводами манипулятора,
должен быть определен так, чтобы обобщенные координаты q и
скорости q удовлетворяли уравнениям связей (6), если начальные условия
(7)
q  t0   q0 ,
q  t0   q0
выбраны
соответствующим
f  q0 , t0   0 ,
образом:
f  q0 , q0 , t0   0 , и обеспечивалась устойчивость выполнения
уравнений связей, когда условия (6) нарушаются.
Для оценки отклонений от уравнений связей (6) используем уравнения программных связей [2]:
(8)
f q t  , t  y t  , f  q t  , q t  , t  y t  .




Правые части y  t  , y   t  в равенствах (8) составляют векторфункции, удовлетворяющие уравнениям возмущений связей:
d3y
d2y
dy
dy 
 K11 2  K12  K13 y  K11
 K12 y   0,
3
dt
dt
dt
dt
(9)
2
2
d y
d y
dy
dy 
 K 21 2  K 22  K 23 y  K 21
 K 22 y   0.
2
dt
dt
dt
dt
 при переменных y, y  и их
Здесь коэффициенты K11 ,..., K 22
производных в общем случае представляют собой матрицы соответствующих размерностей, элементы которых представляют
собой функции, зависящие от векторов q,q,w , определяющих
состояние системы (5). Условия, которые следует наложить на
эти матрицы определяются последующими требованиями,
накладываемыми на динамику манипулятора. К ним относятся
требование устойчивости по отношению к уравнениям связей,
стабилизация связей при численном решении уравнений динамики манипулятора и другие. Так, если матрицы коэффициентов
системы уравнений (9) при переменных y, y  и их производных
являются постоянными, то для асимптотической устойчивости
97
ПРОБЛЕМЫ МЕХАНИКИ И УПРАВЛЕНИЯ – 2011
ее тривиального решения y  0, y   0 достаточно потребовать,
чтобы корни характеристического уравнения системы имели
отрицательные действительные части. При этом сохраняется
устойчивость по отношению к уравнениям связей при постоянно действующих возмущениях. В случае, когда элементы матриц коэффициентов зависят от обобщенных координат и скоростей манипулятора и приводов, условия устойчивости определяются методом функций Ляпунова.
Введение уравнений программных связей позволяет
сформулировать условия устойчивости движения манипулятора
по отношению к уравнениям связей (6). Пусть начальные условия (7), неконтролируемые внешние возмущения   t  и их
производные   t  ограничены неравенствами
f  q0 , t0    , f  q0 , q0 , t0     ,   t    ,   t     .
Требуется определить управляющий вектор u , обеспечивающий выполнение условий y  t    , y  t    при всех
t  t0 .
Для определения вектора управления следует вычислить
производную вектора y до третьего порядка, производную вектора y  до второго порядка и подставить полученные выражения в равенства (9). Искомые производные определяются из выражений (8):
y  f  q, t  , f  f s , s  1, , m1 ,
 
2
d y
dq T
dy
 q f q q q  2 f qt q  f tt ,
 fq q  ft , 2  fq
dt
dt
dt
d3y
d 2q
dq

f
 3 qT f q q  f qt
 qT f q q q q 
q
3
2
q
dt
dt
dt
T



T


T
3 qT f q q q  3 f qtt q  f ttt
T
t
s
f s ,
f q   f i s  , f t   f t s  , f i s  f , f t s 
t
qi
98

Р.Г. Мухарлямов, Н.В. Абрамов. Управление динамикой манипулятора …
 n s 
f q q    f j q j  , f q q   f ijs  , f qt   f its  , f tt   f tts  ,
 j 1

 n s

 n s  T
T
,
q
f
q

q f q q    f ij qi 
  f ij qi q j  ,
q q
 i 1

 i , j 1

2
s
 f
 n

2 f s ,
s
,
f qt q    f jts q j  , f jt 
f ijs 

q

t
qi q j
j
 j 1

T
T
T
q
T
fq qq
T

q
 n

 n

   f ijks qi q j  ,  qT f q q q     f ijks qi q j qk  ,
q
 i , j 1

 i , j ,k 1

T
3 f s ,
3 f s ,
f q qt   f jkts  , f jkts 
f 
qi q j qk
q j qk t
s
ijk
T
f qtt   f jtts  , f jtts 
3 s
3 f s ,
s
, fs  f .
f

f


ttt
ttt
ttt
t 3
q j t 2
Аналогично определяются производные вектора
y   f   q, q, t  , f    f
h
, h  m
1
 1,
y :
, m1  m2 ,
dy 
dq
 f q  f qq  f t ,
dt
dt
T
2
 dq
d y
d 2 q   dq 
T






f

f

2q
f

2
f

f


q
qt
q
 
q q
 dt
dt 2
dt 2   dt  q q

T
 q f q q q  2 f qt q  f tt
T
T
T
f q   f jh  ,
f q   f i h  ,
 
 
f t   f t h  ,
f h ,
f j 
q j
 
 
h
f h ,
fi 
qi
h
 
 
h
h
h
f h , f q q  f ij , f q q  f ij , f qt  f it ,
ft 
t f   f h , f   f h , f   f h ,
ij
qt
it
tt
tt
q q
T
h
T
T
99
ПРОБЛЕМЫ МЕХАНИКИ И УПРАВЛЕНИЯ – 2011
2 f h
2 f h
2 f h
h
h
f ij 
, f ij 
, f it 
,
qi q j
qi q j
qi t
h
2 f h
2 f h
2 f h
h
h
f 
, f it 
, f it 
,
qi q j
qi t
t 2
h
ij
Для составления уравнения относительно управляющего
вектора
u
необходимо вычислить
ки (5) с учетом выражения
 
n
H  hij , hij  
d 2q
. Из уравнений динамиdt 2
d 1
H   H 1 HH 1 , H   hij  ,
dt
hij
qk , следует равенство

q
i 1
k
d 2q
 r  H 1 Bu  M   H 1 ,
2
dt
1
r  H H  C H 1  Cq  p  w  



(10)

 C q   C  H 1  Cq  p  w    q  
q 
 q

,
H  
   Aw  Dq   p



1
1
1
1
M   H  H  C  H   H Cq  H    q 
1
 H 1  H  C  H 1   H 1Cq q  H 1   ,
M  H 1  H  C  Cq q  H 1.
M   mij  , H   hij  , H 1   h jk  ,
n


mij   hik hkl  ckl hlj 
kl 1
n
h
k ,l , s 1
ik
ckl ,s h sj ql
Перепишем уравнения (9) с учетом проведенных вычислений:
100
Р.Г. Мухарлямов, Н.В. Абрамов. Управление динамикой манипулятора …
f q H 1 Bu    M   f q H 1  0,
f qH 1 Bu     M    T H 1 f q q H 1 
T
 f qH 1  0,

(11)

  f q r  3  qT f q q  f qt   K11 f q  K11 f q H 1 
T




  Cq  p  w   qT f q q q q  3 qT f q q q 
T

q
T

t
3 f qtt q  f ttt  K11 qT f q q q  2 f qt q  f tt 
T
  f qq  f    K 12
 f   0,
 K12  f q q  f t   K13 f  K 11
t
Cq  p  w  
 f   H 1  Cq  p  w  
  2qT f q q  2 f qt  f q  K 21
   f qr  H
Cq  p  w 
1
T
f q q H
1
T
T


qT f q q q  2 f qt q  f tt  K 21 qT f q q q  2 f qt q  f tt 
T
T
  f qq  f t   K 22
 f ,
 K 22  f q q  f t   K 23 f  K 21


 3 qT f q q  f qt  K11 f q  K11
 f q  
 H 1 ,
M 

1
  f H H  C  C q

q
 q

T
 f  
 2qT f q q  2 f qt  f q  K 21
 H 1 ,
M  
T

1
 2H  Cq  p  m  f 

q q


T
T
Представим систему уравнений (11) в виде
SBu  s  L   T A  S  ,
(12)
 fq 
 
M 
S  FH 1 , F    , s    , L    ,
 f 
 M 
 q 
 q
0

A   1
 H f  H 1q 
q q


T
101
ПРОБЛЕМЫ МЕХАНИКИ И УПРАВЛЕНИЯ – 2011
s  s  q, q, w, t  , F  F  q, q, t  , S  S  q, q, t  ,
L  L  q, q, t  , A  A  q, q, t  .
В случае, когда m1  m2  n и матрица F является квадратной, полагая, что определитель матрицы не обращается в
нуль, можно получить решение системы (12):
u   B 1 S 1  s  L   T A   B 1  .
Если тривиальное решение y  0, y   0 уравнений системы (9) асимптотически устойчиво, внешние возмущения 
вместе с производными  являются ограниченными функциями
и матрицы F 1 , H 1 , S 1 , L, f qq ограничены по норме, то систе-

ма уравнений динамики с управлением u   B 1 S 1 s  

обеспечивает устойчивость движений манипулятора по отношению к уравнениям связей (6).
Библиографический список
1. Rentrop P., Strehmel K. and Weiner R. Ein Uberblick uber
Eincshrittverfaren zur numerischen Integration in der technischen Simulation // GAMM-Mitteilungen. Band 19. Heft 1.
1996. P.9–43.
2. Мухарлямов Р.Г. О построении систем дифференциальных уравнений движения механических систем //
Дифф. уравнения. 2003. Вып. 39, № 3. С.343–353.
3. Мухарлямов Р.Г. Стабилизация движений механических систем на заданных многообразиях фазового пространства // ПММ. 2006. Т. 70, № 2. С.236–249.
4. Мухарлямов Р.Г. Уравнения движения механических
систем. М.: Изд-во РУДН, 2001. 99 с.
102