Date post: | 25-Oct-2015 |
Category: |
Documents |
Upload: | ricardo-rodriguez-bustinza |
View: | 629 times |
Download: | 1 times |
Algoritmo de Lagrange–Euler
M.Sc. Ricardo Rodrıguez [email protected]
Indice
1. Introduccion 21.1. Sistemas Mecanicos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.1.1. Energıa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2. Sistemas Electricos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2. Ecuaciones de Movimiento de Lagrange 32.1. El Lagraniano . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2. La Ecuacion de Lagrange . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
3. Modelando con Ecuaciones de Lagrange 4
4. Modelo Dinamico del Robot 2DOF 5
1
1 INTRODUCCION
1. Introduccion
El modelado de sistemas dinamicos puede ser realizado en muchos sentidos. Una forma es el uso lasecuaciones estandar de movimiento (las leyes de Newton) para sistemas mecanicos, o ecuaciones de cir-cuitos (Kirchoff o Ohm) para sistemas electricos. Un enfoque alternativo usa la nocion de la energıa paraobtener las ecuaciones dinamicas. Aquı, nosotros consideraremos el enfoque de energıa. Este enfoque esa menudo util, particularmente porque los sistemas no lineales son relativamente complicados.
Para los propositos de diseno de control, es necesario tener un modelo matematico que revele las ecua-ciones dinamica de movimiento para un sistema. Para todos los sistemas mas simples, esto puede ser muycomplicado. Algunos metodos para obtener modelos son mas faciles para algunos sistemas, y no todoslos metodos son uniformemente faciles de aplicar.
Las ecuaciones de Lagrange son la base de la energıa del sistema. Esto tambien incluye la energıa (al-macenada) potencial como a la energıa cinetica (activa). Una vez que la energıa del sistema es descrita,el metodo de Lagrange puede ser usada para obtener las ecuaciones dinamicas. Este enfoque es usadoextensamente en el campo de la Robotica.
1.1. Sistemas Mecanicos
Examinaremos algunos conceptos basicos de la fısica que permitira que comprendamos mejor la dinamicade sistemas mecanicos. En el enfoque Lagraniano, la energıa es el asunto principal. Observaremos variasformas de energıa para sistemas mecanicos, ası como las formas comparables de la energıa para sistemaselectricos.
1.1.1. Energıa
Aquı, consideramos varias formas de energıa, y como aparecen. Para objetos en movimiento, nosotrostenemos la energıa cinetica. Para un objeto de masa m, con velocidad v, la energıa cinetica es dada por:
Ke =12
mv2 (1)
Es siempre una cantidad escalar, y no un vector. La energıa cinetica tambien puede ser asociada con losgiros de los objetos. Para un objeto con inercia rotacional J, y velocidad angular ω , la energıa cinetica esdada por:
Ke =12
Jω2 (2)
La energıa potencial de masa m con una altura h en un campo gravitacional, con una constante g es dadopor:
V = mgh (3)
En algunos casos, el origen, o posicion de potencial cero son tomados con respecto a una posicion relativa,ya que en general, solo las diferencias en la energıa potencial son significativas. Para Sistemas mecanicoscon resortes, estiramiento (o compresion) a una distancia x y constante de resorte K (no Ke), la energıapotencial es dada por:
V =12
Kx2 (4)
M.Sc. Ricardo Rodrıguez Bustinza 2
1.2 Sistemas Electricos 2 ECUACIONES DE MOVIMIENTO DE LAGRANGE
Tambien tenemos la energıa disipada en los sistemas. Esta es llamada la energıa disipativa. Para sistemasmecanicos, la energıa esta generalmente disipada en la friccion deslizante. Si consideramos un objeto confriccion B (o amortiguamiento), y la velocidad v, la energıa disipativa es dado por:
V =12
Bx2 (5)
1.2. Sistemas Electricos
Para sistemas electricos, aca se presentan los homologos con los sistemas mecanicos. Puede haber tiposde la energıa cinetica, energıa potencial, y energıa disipativa. En la siguiente Tabla se muestran estoshomologos.
Energıa Mecanica ElectricaCinetica Masa/Inercia Inductor
Ke12
mv2 /12
Jω2 12
Liv2/12
lq2
Potencial Gravedad Condensador
V mgh12
Cv2 =1
2Cq2
Resorte12
Kx2
Disipativa Amortiguamiento/Friccion Resistencia
P12
Bv2 =1
2Cq2 1
2Ri2 =
12
Rq2
2. Ecuaciones de Movimiento de Lagrange
El principio de las ecuaciones de Lagrange esta basado en una cantidad llamada Lagraniano, con ello,formulamos:
Para un sistema dinamico en el que el trabajo de todas las fuerzas es explicado para el Lagraniano,una movimiento admisible entre las configuraciones especıficas del sistema en un tiempo t1 y t2 es unmovimiento natural si, y solo si, la energıa del sistema permanece constante.
Note que aunque se haya dado una relacion de “Fuerza” y “Movimiento”, cuyas las ideas son con-cernientes a la energıa, son por lo tanto igualmente aplicable a sistemas electricos.
2.1. El Lagraniano
El Lagraniano es una cantidad que describe el balance entre las energıas no disipadas. En particular seescribe:
L = Ke −V (6)
Donde Ke es la energıa cinetica, y V es la energıa potencial. Cuando el Lagraniano es Positivo, eso quieredecir que hay mas energıa cinetica que energıa potencial, y cuando el Lagraniano es negativo, el contrarioes verdadero. Cuando L = 0, no quiere decir que no hay energıa en el sistema, pero el balance es cero.
M.Sc. Ricardo Rodrıguez Bustinza 3
2.2 La Ecuacion de Lagrange 3 MODELANDO CON ECUACIONES DE LAGRANGE
Considere un pendulo sin friccion. Como oscila de un lado a otro, hay una constante de intercambio entrela energıa cinetica y potencial. Cuando esta en la cima de su giro, V esta en su maximo, y Ke es cero yaque la velocidad es cero. Por lo tanto, L esta en su mınimo. Cuando el pendulo tiene su velocidad maxima,por lo tanto, Ke esta en su maximo mientras V es cero. En ese momento, L esta en su maximo.
Para presentar la ecuacion de Lagrange, es importante considerar primero los grados de libertad delsistema. Estos son el numero de cantidades independientes que deben especificados en los estados delsistema para que sea definido. Estos son en general variables de estado del sistema, pero no son todos.Cualquier conjunto unico de tales cantidades son referidos como coordenadas generalizadas del sistema.
2.2. La Ecuacion de Lagrange
Finalmente, obtenemos la ecuacion de Lagrange que esta asociada con cada coordenada generalizada qi,es dada por:
ddt
(∂L∂ qi
)− ∂L
∂qi+
∂P∂ qi
= Qi (7)
Donde P es llamado la funcion de potencia (describir la disipacion de la energıa en el sistema), Qi sonlas entradas externas generalizadas (fuerza) que actuan sobre el sistema. Por lo tanto, hay tres coorde-nadas generalizadas, entonces seran tres ecuaciones como (7). Note que (7) es en realidad una ecuaciondiferencial de segundo orden en el tiempo. (Esto no necesariamente puede ser obvio).
3. Modelando con Ecuaciones de Lagrange
Modelar el sistema masa-resorte que se muestra en la Figura 1 usando las ecuaciones dinamicas de energıade Lagrange. En el sistema, x es la direccion del movimiento, x es la velocidad del movil, k es la constantede elongacion del resorte, m es la masa del movil, y FEXT es la fuerza externa aplicada al movil pararealizar el movimiento en la direccion de x.
Figura 1: Sistema masa-resorte.
La energıa cinetica:
K =12
mx (8)
La energıa potencial:
P =12
kx2 (9)
La ecuacion de Lagrange referida a las energıas:
M.Sc. Ricardo Rodrıguez Bustinza 4
4 MODELO DINAMICO DEL ROBOT 2DOF
L = K −P (10)
La ecuacion de Lagrange generalizada es:
ddt
(∂L∂ q
)− ∂L
∂q= Q (11)
De (8) y (9) en (10):
L =12
mx− 12
kx2 (12)
Reescribiendo (11) para el sistema:
ddt
(∂L∂ x
)− ∂L
∂x= FEXT (13)
Desarrollando derivadas:
∂L∂ x
= mx (14)
ddt
(∂L∂ x
)= mx (15)
∂L∂x
= −kx (16)
Reemplazando (14), (15) y (16) en (13) tenemos:
mx+ kx = FEXT
4. Modelo Dinamico del Robot 2DOF
Aplicar el metodo de Lagrange-Euler para la obtencion del modelo dinamico del robot de 2 grados delibertad (θ1, d2) con base segun la Figura 2.
Figura 2: Robot polar 2DOF.
Los sistemas de referencia para el robot se muestra en la Figura 3.
M.Sc. Ricardo Rodrıguez Bustinza 5
4 MODELO DINAMICO DEL ROBOT 2DOF
Figura 3: Sistemas de referencia del robot polar.
Algoritmo Lagrange-Euler
L-E 1: Se asignan los sistemas de referencia y parametros D-H segun la Figura 3 y la siguiente tabla:
Articulacion θi di ai αi
1 θ1 0 0 02 0 d2 0 -90◦
L-E 2: Matrices de transformacion 0Ai.
0A1 =
C1 0 −S1 0S1 0 C1 00 −1 0 00 0 0 1
; 1A2 =
1 0 0 00 1 0 00 0 1 d20 0 0 1
Luego:
0A2 =0A1
1A2 =
C1 0 −S1 −d2S1S1 0 C1 d2C20 −1 0 00 0 0 1
L-E 3: Matrices Ui j.
U11 =∂ 0A1
∂θ1=
−S1 0 −C1 0C1 0 −S1 00 0 0 00 0 0 0
; U12 =∂ 0A1
∂d2= [0]
U21 =∂ 0A2
∂θ1=
−S1 0 −C1 −d2C1C1 0 −S1 −d2S10 0 0 00 0 0 0
; U22 =∂ 0A2
∂d2=
0 0 0 −S10 0 0 C10 0 0 00 0 0 0
M.Sc. Ricardo Rodrıguez Bustinza 6
4 MODELO DINAMICO DEL ROBOT 2DOF
L-E 4: Matrices Ui jk.
U111 =∂U11
∂θ1=
−C1 0 S1 0−S1 0 C1 0
0 0 0 00 0 0 0
; U112 =∂U11
∂d2= [0]
U121 =∂U12
∂θ1= [0]; U122 =
∂U12
∂d2= [0];
U211 =∂U21
∂θ1=
−C1 0 S1 d2S1−S1 0 −C1 −d2C1
0 0 0 00 0 0 0
; U212 =∂U21
∂d2=
0 0 0 −C10 0 0 −S10 0 0 00 0 0 0
U221 =∂U22
∂θ1=
0 0 0 −C10 0 0 −S10 0 0 00 0 0 0
; U222 =∂U22
∂d2= [0]
L-E 5: Matrices de pseudoinercia Ji. La Figura 4 muestra los elementos del robot.
Figura 4: Elementos del robot polar.
Elemento 1: Para x1 = y1 = 0 y z1 = L1
J1 =
∫
x21dm1
∫x1y1dm1
∫x1z1dm1
∫x1dm1∫
y1x1dm1∫
y21dm1
∫y1z1dm1
∫y1dm1∫
z1x1dm1∫
z1y1dm1∫
z21dm1
∫z1dm1∫
x1dm1∫
y1dm1∫
z1dm1∫
dm1
=
0 0 0 00 0 0 00 0 m1L2
1 m1L10 0 m1L1 m1
(17)
Elemento 2: Para x2 = y2 = z2 = 0
Similarmente al caso anterior, consideramos la masa concentrada en el centro de masas y elorigen del sistema de coordenadas del elemento 2 se toma en el mismo centro de masas, lamatriz J2 tomara la siguiente forma:
M.Sc. Ricardo Rodrıguez Bustinza 7
4 MODELO DINAMICO DEL ROBOT 2DOF
J2 =
0 0 0 00 0 0 00 0 0 00 0 0 m2
L-E 6: Matrices de inercias D = [di j].
d11 =2
∑k=max(1,1)
Traza(
Uk1JkUTk1
)= Tr(U11J1UT
11)+Tr(U21J2UT21)
= Tr
C2
1L21m1 S1C1L2
1m1 0 0S1C1L2
1m1 S21L2
1m1 0 00 0 0 00 0 0 0
+Tr
C2
1d22m2 S1C1d2
2m2 0 0S1C1d2
2m2 S21d2
2m2 0 00 0 0 00 0 0 0
=
(C2
1 +S21
)m1L2
1 +(
C21 +S2
1
)d2m2 = m1L2
1 +m2d22
d12 =2
∑k=max(1,2)
Traza(
Uk2JkUTk1
)= Tr(U22J2UT
21)
= Tr
S1C1d2m2 S2
1d2m2 0 0−C2
1d2m2 −S1C1d2m2 0 00 0 0 00 0 0 0
= S1C1d2m2 −S1C1d2m2 = 0
d21 =2
∑k=max(2,1)
Traza(
Uk1JkUTk2
)= Tr(U21J2UT
22)
= Tr
S1C1d2m2 −C2
1d2m2 0 0S2
1d2m2 −S1C1d2m2 0 00 0 0 00 0 0 0
= S1C1d2m2 −S1C1d2m2 = 0
d22 =2
∑k=max(2,2)
Traza(
Uk2JkUTk2
)= Tr(U22J2UT
22)
= Tr
S2
1m2 −S1C1m2 0 0−S1C1m2 C2
1m2 0 00 0 0 00 0 0 0
= S21m2 +C2
1m2 = m2
Luego:
D =
[d11 d12d21 d22
]=
[m1L2
1 +m2d22 0
0 m2
]
M.Sc. Ricardo Rodrıguez Bustinza 8
4 MODELO DINAMICO DEL ROBOT 2DOF
L-E 7: Terminos en hikm.
h111 =2
∑j=max(1,1,1)
Traza(
U j11J jUTj1
)= Tr(U111J1UT
11)+Tr(U211J2UT21)
= Tr
−C1S1m1L2
1 −S21m1L2
1 0 0C2
1m1L21 C1S1m1L2
1 0 00 0 0 00 0 0 0
+Tr
−S1C1d2
2m2 −S21d2
2m2 0 0C2
1d22m2 S1C1d2
2m2 0 00 0 0 00 0 0 0
= −C1S1m1L2
1 +C1S1m1L21 −d2
2S1C1m2 +d22S1C1m2 = 0
h112 =2
∑j=max(1,1,2)
Traza(
U j12J jUTj1
)= Tr(U212J2UT
21)
= Tr
C2
1d2m2 S1C1d2m2 0 0S1C1d2m2 S2
1d2m2 0 00 0 0 00 0 0 0
=C21d2m2 +S2
1d2m2 = d2m2
h121 =2
∑j=max(1,2,1)
Traza(
U j21J jUTj1
)= Tr(U221J2UT
21)
= U221 = U212 ⇒ h121 = h112 = d2m2
h211 =2
∑j=max(2,1,1)
Traza(
U j11J jUTj2
)= Tr(U211J2UT
22)
= Tr
−S2
1d2m2 S1C1d2m2 0 0S1C1d2m2 −C2
1d2m2 0 00 0 0 00 0 0 0
= S21d2m2 −C2
1d2m2 =−d2m2
h122 =2
∑j=max(1,2,2)
Traza(
U j22J jUTj1
)= Tr(U222J2UT
21) = 0
h212 =2
∑j=max(2,1,2)
Traza(
U j12J jUTj2
)= Tr(U212J2UT
22)
= Tr
S1C1m2 −C2
1m2 0 0S2
1m2 −S1C1m2 0 00 0 0 00 0 0 0
= S1C1m2 −C1S1m2 = 0
h221 =2
∑j=max(2,2,1)
Traza(
U j21J jUTj2
)= Tr(U221J2UT
21)
= U221 = U212 ⇒ h221 = h212 = 0
h222 =2
∑j=max(2,2,2)
Traza(
U j22J jUTj2
)= Tr(U222J2UT
22) = 0
M.Sc. Ricardo Rodrıguez Bustinza 9
4 MODELO DINAMICO DEL ROBOT 2DOF
L-E 8: Matriz columna de fuerzas de Coriolis y centrıpetas H = [hi]T
h1 =2
∑k=1
2
∑m=1
h1kmqkqm = h111θ 21 +h112θ1d2 +h121d2θ1 +h122d2
2
= 0(θ 21 )+(d2m2 +d2m2)θ1d2 +0(d2
2) = 2d2m2θ1d2
h2 =2
∑k=1
2
∑m=1
h2kmqkqm = h211θ 21 +h212θ1d2 +h221d2θ1 +h222d2
2
= −d2m2θ 21 +(0+0)θ1d2 +0(d2
2) =−d2m2θ 21
Luego:
H =
[2d2m2θ1d2−d2m2θ 2
1
]L-E 9: Matriz columna de fuerzas de gravedad C = [ci]
T .
ci =N
∑j=1
(−m jgU j1
jr j
)donde, g es el vector gravedad expresado en el sistema de la base del robot {S0}.
g = [0, 0, −g, 0 ]
y, jr j es el vector de coordenadas homogeneas de posicion del centro de masas del eslabon j expre-sado en el sistema {S j} (ver Figura 4).
1r1 = [ 0, 0, L1, 1 ]T , 2r2 = [ 0, 0, 0, 1 ]T
Luego.
c1 =2
∑j=1
(−m jgU j1
jr j
)=−m1gU11
1r1 −m2gU212r2
= −m1g
−S1 0 −C1 0C1 0 −S1 00 0 0 00 0 0 0
00L11
−m2g
−S1 0 −C1 −d2C1C1 0 −S1 −d2S10 0 0 00 0 0 0
0001
= 0
c2 =2
∑j=1
(−m jgU j2
jr j
)=−m1gU12
1r1 −m2gU222r2
= −m1g[0]
00L11
−m2g
0 0 0 −S10 0 0 C10 0 0 00 0 0 0
0001
= 0
Por lo tanto:
C = [ci]T =
[00
]
M.Sc. Ricardo Rodrıguez Bustinza 10
4 MODELO DINAMICO DEL ROBOT 2DOF
L-E 10: La ecuacion dinamica del robot sera:
τ = Dq+H+C
Reemplazando:[T1F2
]=
[m1L2
1 +m2d22 0
0 m2
][θ1d2
]+
[2d2m2θ1d2−d2m2θ 2
1
]+
[00
]Luego:
T1 = (m1L21 +m2d2
2)θ1 +2d2m2θ1d2
F2 = m2d2 −d2m2θ 21
Donde T1 es el par motor efectivo (incluye rozamiento y otras perturbaciones) que actuan sobre la articu-lacion 1 y F2 es la fuerza motora efectiva que actua sobre la articulacion 2. El modelo dinamico respondea un sistema de dos ecuaciones diferenciales de segundo orden no lineales y acopladas. Se observa que eltermino gravedad C no aparece en el modelo. Esto se debe a que la estructura horizontal del robot permiteque las fuerzas de gravedad se proyecten sobre los apoyos, no siendo necesario la aportacion de par ofuerza para vencerla. Si consideramos al robot en la posicion horizontal (ver Figura 5), manteniendo ladefinicion de los sistemas de referencia de la Figura 3, las expresiones de las matrices y vectores A, U, J,D y H resultaran iguales que el caso anterior.
Figura 5: Configuracion horizontal del robot polar.
Sin embargo el vector C se vera afectado, pues, ahora el vector de gravedad g expresado en el sistema dereferencia de la base {S0} sera:
g = [g, 0, 0, 0 ]
Por tanto:
M.Sc. Ricardo Rodrıguez Bustinza 11
4 MODELO DINAMICO DEL ROBOT 2DOF
c1 = −m1g
−C1L1−S1L1
00
−m2g
−C1d2−S1d2
00
= m1gL1C1 +m2gd2C1
c2 = −m1g[0]−m2g
−S1C100
= m2gS1
Siendo la matriz C:
C =
[m1gL1C1 +m2gd2C1
m2gS1
]De este modo las ecuaciones que corresponden al modelo dinamico seran:
T1 = (m1L21 +m2d2
2)θ1 +2d2m2θ1d2 +(m1gL1 +m2gd2)cosθ1
F2 = m2d2 −d2m2θ 21 +m2gsinθ1
Algoritmo Lagrange Euler con Matlab
% -------------------------------------------------------------------------
% Profesor: Ricardo Rodriguez Bustinza
% Grupo ISCA
% www.grupo-isca.com
% -------------------------------------------------------------------------
disp(’--------------------------------------------------------------------’)
disp(’DINAMICA DE UN ROBOT 2DOF USANDO EL METODO DE LAGRANGE - EULER’)
disp(’--------------------------------------------------------------------’)
syms q1 q2 dq1 dq2 ddq1 ddq2;
syms l1 d2;
syms g;
syms m1 m2 x1 x2 y1 y2 z1 z2
disp(’1.- Tabla de Parametros DH’)
disp(’-a---alpha---d---theta---’)
DH=[ 0 -pi/2 0 q1
0 0 q2 0 ]
disp(’--------------------------------------------------------------------’)
disp(’2.- Calculo de las matrices de transformacion: Aij’)
disp(’--------------------------------------------------------------------’)
disp(’Matriz del primer eslabon movil desde la base del robot’)
disp(’--------------------------------------------------------------------’)
A01=simple(simplify(denavit(DH(1,1),DH(1,2),DH(1,3),DH(1,4))))
A12=simple(simplify(denavit(DH(2,1),DH(2,2),DH(2,3),DH(2,4))));
% Matriz de transformacion de la base al efector final
disp(’--------------------------------------------------------------------’)
disp(’Matriz del efector final desde la base del robot’)
M.Sc. Ricardo Rodrıguez Bustinza 12
4 MODELO DINAMICO DEL ROBOT 2DOF
disp(’--------------------------------------------------------------------’)
A02=simple(simplify(A01*A12))
disp(’--------------------------------------------------------------------’)
disp(’3.- Calculo de las matrices : Uij’)
disp(’--------------------------------------------------------------------’)
U11=simple(simplify(diff(A01,q1)))
U12=simple(simplify(diff(A01,q2)))
U21=simple(simplify(diff(A02,q1)))
U22=simple(simplify(diff(A02,q2)))
disp(’--------------------------------------------------------------------’)
disp(’4.- Calculo de las matrices : Uijk’)
disp(’--------------------------------------------------------------------’)
U111=simple(simplify(diff(U11,q1)))
U112=simple(simplify(diff(U11,q2)))
U121=simple(simplify(diff(U12,q1)))
U122=simple(simplify(diff(U12,q2)))
U211=simple(simplify(diff(U21,q1)))
U212=simple(simplify(diff(U21,q2)))
U221=simple(simplify(diff(U22,q1)))
U222=simple(simplify(diff(U22,q2)))
disp(’--------------------------------------------------------------------’)
disp(’5.- Matrices de Pseudoinercia : Ji’)
disp(’--------------------------------------------------------------------’)
% Para el primer eslabon: J1[Kg.m^2]
% m1=1.4Kg
% Coordenadas del centro de gravedad 1 respecto al frame 1
% x1=0;y1=0;z1=l1=0.5m;
disp(’--------------------------------------------------------------------’)
disp(’Para el Primer Eslabon’)
disp(’--------------------------------------------------------------------’)
J1=[ 0 0 0 0
0 0 0 0
0 0 (m1*l1^2) (m1*l1)
0 0 (m1*l1) m1 ]
% Para el primer eslabon: J1[Kg.m^2]
% m2=1Kg
% Coordenadas del centro de gravedad 2 respecto al frame 2
% x2=0;y2=0;z2=0;
disp(’--------------------------------------------------------------------’)
disp(’Para el Segundo Eslabon’)
disp(’--------------------------------------------------------------------’)
J2=[ 0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 m2]
disp(’--------------------------------------------------------------------’)
disp(’6.- Calculo de las matrices de inercia : D = [dij]’)
disp(’--------------------------------------------------------------------’)
d11=simple(simplify(trace(U11*J1*U11.’)))+simple(simplify(trace(U21*J2*U21.’)))
M.Sc. Ricardo Rodrıguez Bustinza 13
4 MODELO DINAMICO DEL ROBOT 2DOF
d12=simple(simplify(trace(U22*J2*U21.’)))
d21=simple(simplify(trace(U21*J2*U22.’)))
d22=simple(simplify(trace(U22*J2*U22.’)))
D = [d11 d12;d21 d22]
disp(’--------------------------------------------------------------------’)
disp(’7.- Calculo del vector hikm’)
disp(’--------------------------------------------------------------------’)
h111=simplify(trace(U111*J1*U11.’))+simplify(trace(U211*J2*U21.’))
h112=simplify(trace(U212*J2*U21.’))
h121=simplify(trace(U221*J2*U21.’))
h122=simplify(trace(U222*J2*U21.’))
h211=simplify(trace(U211*J2*U22.’))
h212=simplify(trace(U212*J2*U22.’))
h221=simplify(trace(U221*J2*U22.’))
h222=simplify(trace(U222*J2*U22.’))
disp(’--------------------------------------------------------------------’)
disp(’8.- Calculo del vector de fuerza centrifuga y de coriolis:H=[hi]’)
disp(’--------------------------------------------------------------------’)
h1=h111*dq1^2+h112*dq1*dq2+h121*dq2*dq1+h122*dq2^2;
h2=h211*dq1^2+h212*dq1*dq2+h221*dq2*dq1+h222*dq2^2;
H=[h1;h2]
disp(’--------------------------------------------------------------------’)
disp(’9.- Calculo de la matriz columna de fuerzas de gravedad:
C=[ci]’)
disp(’--------------------------------------------------------------------’)
g1=[g 0 0 0]; % Segun el frame 0 , gravedad actuando en el eje X
% Vector de compuesto por coordenadas de posicion CM c/eslabon
r11=[0;0;l1;1]; % CG para el eslabon 1
r22=[0;0;0;1]; % CG para el eslabon 2
c1=-m1*g1*U11*r11 - m2*g1*U21*r22; c2=-m1*g1*U12*r11 -
m2*g1*U22*r22;
C=[c1;c2]
disp(’--------------------------------------------------------------------’)
disp(’10.- Ecuacion Dinamica del Robot’)
disp(’--------------------------------------------------------------------’)
tau=simplify(D*[ddq1;ddq2]+H+C);
disp(’El torque o par para la primera articulacion revoluta: ’)
disp(’--------------------------------------------------------------------’)
T1 = tau(1) % Juntura Revoluta
disp(’--------------------------------------------------------------------’)
disp(’La fuerza para al articulacion prismatica:’)
disp(’--------------------------------------------------------------------’)
F2 = tau(2) % Juntura Prismatica
disp(’--------------------------------------------------------------------’)
disp(’Ecuacion Dinamica del Robot Manipulador RP:’)
disp(’--------------------------------------------------------------------’)
tau=[tau(1); tau(2)]
M.Sc. Ricardo Rodrıguez Bustinza 14
4 MODELO DINAMICO DEL ROBOT 2DOF
Ejercicio
Sea el sistema manipulador de coordenadas cilındricas que se muestra en la Figura 6. El cuerpo principales el eslabon 1, el eje y efector final es el eslabon 2. Se consideran a r1 y r2 como los centros de masa deleje de rotacion (NOTE: r1 = constante, r2 = variable). Las masas m1 y m2. Los momentos de inercia de lamasa centroidal J1 y J2. El angulo θ gira desde el eje x positivo al eje radial ri de rotacion. T es el torquedel actuador rotatorio desde la tierra al cuerpo 1 medido positivo. F es la fuerza radial del actuador delcuerpo 1 en el cuerpo 2 medido positivo. La gravedad g que actua a lo largo del eje de y negativo. Modelarel sistema usando las ecuaciones dinamicas de energıa de Lagrange.
Figura 6: Manipulador robotico cilındrico.
Considerar: q1 = θ , q2 = r2, q1 = θ , q2 = r2, Q1 = T , y Q2 = F .
Solucion
Eslabon 1 (r1esconstante):
x1 = r1 cosθ (18)
y1 = r1 sinθ (19)
Derivando (18) y (19).
x1 = −r1θ sinθ (20)
y1 = r1θ cosθ (21)
Eslabon 2 (r2esvariable):
x2 = r2 cosθ (22)
y2 = r2 sinθ (23)
Derivando (22) y (23).
M.Sc. Ricardo Rodrıguez Bustinza 15
4 MODELO DINAMICO DEL ROBOT 2DOF
x2 = r2 cosθ − r2θ sinθ (24)
y2 = r2 sinθ + r2θ cosθ (25)
La energıa cinetica (traslacional + rotacional):
K =12
m1(x21 + y2
1)+12
m2(x22 + y2
2)+12
J1θ 2 +12
J2θ 2 (26)
Operando terminos en (26):
K =12
m1r21θ 2 +
12
m2r22θ 2 +
12
m2r22 +
12(J1 + J2)θ 2 (27)
La energıa potencial:
P = m1y1g+m2y2g (28)
Reemplazando (19) y (23) en (28)
P = m1gr1 sinθ +m2gr2 sinθ (29)
La ecuacion de Lagrange referida a las energıas:
L = K −P (30)
Reemplazando (27) y (29) en (30)
L =12
m1r21θ 2 +
12
m2r22θ 2 ++
12
m2r22 +
12(J1 + J2)θ 2 −g(m1r1 +m2r2)sinθ (31)
La ecuacion de Lagrange generalizada es:
ddt
(∂L∂ θ
)− ∂L
∂θ= T (32)
y
ddt
(∂L∂ r2
)− ∂L
∂ r2= F (33)
Desarrollando derivadas para obtener la ecuacion de T :
∂L∂ θ
=(
m1r21 +m2r2
2 + J1 + J2
)θ (34)
ddt
(∂L∂ θ
)=
(m1r2
1 +m2r22 + J1 + J2
)θ +2m2r2r2θ (35)
∂L∂θ
= −g(m1r1 +m2r2)cosθ (36)
Luego obtenemos:
T =(
m1r21 +m2r2
2 + J1 + J2
)θ +2m2r2r2θ +g(m1r1 +m2r2)cosθ (37)
M.Sc. Ricardo Rodrıguez Bustinza 16
4 MODELO DINAMICO DEL ROBOT 2DOF
Desarrollando derivadas para obtener la ecuacion de F :
∂L∂ r2
= m2r2 (38)
ddt
(∂L∂ r2
)= m2r2 (39)
∂L∂ r2
= m2r2θ 2 −m2gsinθ (40)
Luego obtenemos:
F = m2r2 −m2r2θ 2 +m2gsinθ (41)
Finalmente de (37) y (41) obtenemos la ecuacion dinamica matricial:
[m1r2
1 +m2r22 + J1 + J2 0
0 m2
][θr2
]=
[TF
]+
[0 −2m2r2θ
m2r2θ 0
][θr2
]− ·· ·
−[
g(m1r1 +m2r2)cosθm2gsinθ
](42)
Siendo:
M =
[m1r2
1 +m2r22 + J1 + J2 0
0 m2
]C =
[0 −2m2r2θ
m2r2θ 0
]G =
[g(m1r1 +m2r2)cosθ
m2gsinθ
]Tambien:
x =
[θr2
]x =
[θr2
]τ =
[TF
]Reescribiendo (42) tenemos el modelo Lagraniano:
x = M−1(x)(
τ +C(x)x−G(x))
(43)
Grupo ISCAEspecialistas en Control por Computadorawww.grupo-isca.com
M.Sc. Ricardo Rodrıguez Bustinza 17