Академический Документы
Профессиональный Документы
Культура Документы
Indice
1. Introduccion
1.1. Sistemas Mecanicos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1.1. Energa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2. Sistemas Electricos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
2
2
3
3
3
4
1 INTRODUCCION
1. Introduccion
El modelado de sistemas dinamicos puede ser realizado en muchos sentidos. Una forma es el uso las
ecuaciones estandar de movimiento (las leyes de Newton) para sistemas mecanicos, o ecuaciones de circuitos (Kirchoff o Ohm) para sistemas electricos. Un enfoque alternativo usa la nocion de la energa para
obtener las ecuaciones dinamicas. Aqu, nosotros consideraremos el enfoque de energa. Este enfoque es
a menudo u til, 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 ecuaciones dinamica de movimiento para un sistema. Para todos los sistemas mas simples, esto puede ser muy
complicado. Algunos metodos para obtener modelos son mas faciles para algunos sistemas, y no todos
los metodos son uniformemente faciles de aplicar.
Las ecuaciones de Lagrange son la base de la energa del sistema. Esto tambien incluye la energa (almacenada) potencial como a la energa cinetica (activa). Una vez que la energa del sistema es descrita,
el metodo de Lagrange puede ser usada para obtener las ecuaciones dinamicas. Este enfoque es usado
extensamente en el campo de la Robotica.
(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 energa potencial son significativas. Para Sistemas mecanicos
con resortes, estiramiento (o compresion) a una distancia x y constante de resorte K (no Ke ), la energa
potencial es dada por:
1
V = Kx2
2
(4)
Tambien tenemos la energa disipada en los sistemas. Esta es llamada la energa disipativa. Para sistemas
mecanicos, la energa esta generalmente disipada en la friccion deslizante. Si consideramos un objeto con
friccion B (o amortiguamiento), y la velocidad v,
la energa disipativa es dado por:
1
V = Bx2
2
(5)
Potencial
Mecanica
Masa/Inercia
1 2 1 2
mv / J
2
2
Gravedad
Electrica
Inductor
1 2 1 2
Liv / l q
2
2
Condensador
mgh
1 2
1 2
Cv =
q
2
2C
Ke
Disipativa
P
Resorte
1 2
Kx
2
Amortiguamiento/Friccion
1 2
1 2
Bv =
q
2
2C
Resistencia
1 2 1 2
Ri = Rq
2
2
2.1. El Lagraniano
El Lagraniano es una cantidad que describe el balance entre las energas no disipadas. En particular se
escribe:
L = Ke V
(6)
Donde Ke es la energa cinetica, y V es la energa potencial. Cuando el Lagraniano es Positivo, eso quiere
decir que hay mas energa cinetica que energa potencial, y cuando el Lagraniano es negativo, el contrario
es verdadero. Cuando L = 0, no quiere decir que no hay energa en el sistema, pero el balance es cero.
Considere un pendulo sin friccion. Como oscila de un lado a otro, hay una constante de intercambio entre
la energa cinetica y potencial. Cuando esta en la cima de su giro, V esta en su maximo, y Ke es cero ya
que la velocidad es cero. Por lo tanto, L esta en su mnimo. 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 del
sistema. Estos son el numero de cantidades independientes que deben especificados en los estados del
sistema para que sea definido. Estos son en general variables de estado del sistema, pero no son todos.
Cualquier conjunto u nico de tales cantidades son referidos como coordenadas generalizadas del sistema.
+
= Qi
(7)
dt qi
qi qi
Donde P es llamado la funcion de potencia (describir la disipacion de la energa en el sistema), Qi son
las entradas externas generalizadas (fuerza) que actuan sobre el sistema. Por lo tanto, hay tres coordenadas generalizadas, entonces seran tres ecuaciones como (7). Note que (7) es en realidad una ecuacion
diferencial de segundo orden en el tiempo. (Esto no necesariamente puede ser obvio).
La energa cinetica:
1
K = mx
2
(8)
La energa potencial:
1
P = kx2
2
La ecuacion de Lagrange referida a las energas:
(9)
4 MODELO DINAMICO
DEL ROBOT 2DOF
L = K P
(10)
L
q
L
=Q
q
(11)
(12)
L
x
L
= FEXT
x
(13)
Desarrollando derivadas:
L
= mx
x
( )
d L
= mx
dt x
L
x
= kx
(14)
(15)
(16)
4 MODELO DINAMICO
DEL ROBOT 2DOF
Algoritmo Lagrange-Euler
L-E 1: Se asignan los sistemas de referencia y parametros D-H segun la Figura 3 y la siguiente tabla:
i
1
0
Articulacion
1
2
di
0
d2
ai
0
0
i
0
-90
C1 0 S1
S1 0
C1
0
A1 =
0 1
0
0
0
0
0
0
;
0
1
0
1
A2 =
0
0
0
1
0
0
0 0
0 0
1 d2
0 1
Luego:
C1 0 S1 d2 S1
S1 0
C1
d2C2
0
A2 = 0 A1 1 A2 =
0 1
0
0
0
0
0
1
L-E 3: Matrices Ui j .
U11 =
U21 =
S1
0 A1
C1
=
0
1
0
S1
0
A2 C1
=
0
1
0
0 C1
0 S1
0
0
0
0
0
0
;
0
0
U12 =
0 C1 d2C1
0 S1 d2 S1
;
0
0
0
0
0
0
0 A1
= [0]
d2
0
0
A2 0
U22 =
=
0
d2
0
0
0
0
0
0 S1
0 C1
0
0
0
0
6
4 MODELO DINAMICO
DEL ROBOT 2DOF
L-E 4: Matrices Ui jk .
U111 =
U11
1
U121 =
U12
1
U211 =
U21
1
U221 =
U22
1
0 S1 0
0 C1 0
; U112 = U11 = [0]
0 0 0
d2
0 0 0
U12
= [0]; U122 =
= [0];
d2
C1 0 S1
d2 S1
0
S1 0 C1 d2C1
; U212 = U21 = 0
=
0
0
0
0
0
d2
0
0
0
0
0
0 0 0 C1
0 0 0 S1
; U222 = U22 = [0]
=
0 0 0
0
d2
0 0 0
0
C1
S1
=
0
0
0
0
0
0
0 C1
0 S1
0
0
0
0
Elemento 1: Para x1 = y1 = 0 y z1 = L1
x12 dm1
x y dm
x z dm
x dm
12 1 1 1 1 1 1 1
y1 x1 dm1 y1 dm1
y z dm
y dm
21 1 1 1 1
z1 x1 dm1 z1 y1 dm1 z1 dm1
z dm
1 1
x1 dm1
y1 dm1
z1 dm1
dm1
0 0
0
0
0 0
0
0
2
0 0 m1 L1 m1 L1
0 0 m1 L1 m1
J1 =
(17)
Elemento 2: Para x2 = y2 = z2 = 0
Similarmente al caso anterior, consideramos la masa concentrada en el centro de masas y el
origen del sistema de coordenadas del elemento 2 se toma en el mismo centro de masas, la
matriz J2 tomara la siguiente forma:
M.Sc. Ricardo Rodrguez Bustinza
4 MODELO DINAMICO
DEL ROBOT 2DOF
J2
0
0
=
0
0
0
0
0
0
0 0
0 0
0 0
0 m2
d11 =
(
)
Traza Uk1 Jk UTk1 = Tr(U11 J1 UT11 ) + Tr(U21 J2 UT21 )
k=max(1,1)
S1C1 L m1 S L m1 0 0
1
1 1
+ Tr S1C1 d2 m2 S1 d2 m2
= Tr
0
0
0
0
0 0
0
0
0
0
0 0
(
)
(
)
= C12 + S12 m1 L12 + C12 + S12 d2 m2 = m1 L12 + m2 d22
d12 =
S1C1 d2 m2
S12 d2 m2
C2 d2 m2 S1C1 d2 m2
1
= Tr
0
0
0
0
2
0
0
0
0
S1C1 d2 m2 C12 d2 m2
S2 d2 m2 S1C1 d2 m2
1
= Tr
0
0
0
0
2
0
0
= S1C1 d2 m2 S1C1 d2 m2 = 0
0
0
(
)
Traza Uk1 Jk UTk2 = Tr(U21 J2 UT22 )
k=max(2,1)
d22 =
0
0
0
0
(
)
Traza Uk2 Jk UTk1 = Tr(U22 J2 UT21 )
k=max(1,2)
d21 =
0
0
0
0
0
0
0
0
0
0
= S1C1 d2 m2 S1C1 d2 m2 = 0
0
0
(
)
Traza Uk2 Jk UTk2 = Tr(U22 J2 UT22 )
k=max(2,2)
S12 m2
S1C1 m2
S1C1 m2
C12 m2
= Tr
0
0
0
0
0
0
0
0
0
0
= S12 m2 +C12 m2 = m2
0
0
Luego:
[
D =
[
=
d11 d12
d21 d22
m1 L12 + m2 d22 0
0
m2
4 MODELO DINAMICO
DEL ROBOT 2DOF
h111 =
(
)
Traza U j11 J j UTj1 = Tr(U111 J1 UT11 ) + Tr(U211 J2 UT21 )
j=max(1,1,1)
0
0
0
0
0
S1C1 d22 m2 S12 d22 m2
2 2
0
S1C1 d22 m2
+ Tr C1 d2 m2
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
h112 =
(
)
Traza U j12 J j UTj1 = Tr(U212 J2 UT21 )
j=max(1,1,2)
C12 d2 m2 S1C1 d2 m2
S1C1 d2 m2 S2 d2 m2
1
= Tr
0
0
0
0
2
h121 =
0
0
0
0
0
0
= C12 d2 m2 + S12 d2 m2 = d2 m2
0
0
(
)
Traza U j21 J j UTj1 = Tr(U221 J2 UT21 )
j=max(1,2,1)
h211 =
(
)
Traza U j11 J j UTj2 = Tr(U211 J2 UT22 )
j=max(2,1,1)
S12 d2 m2 S1C1 d2 m2
S1C1 d2 m2 C2 d2 m2
1
= Tr
0
0
0
0
2
h122 =
0
0
0
0
0
0
= S12 d2 m2 C12 d2 m2 = d2 m2
0
0
(
)
Traza U j22 J j UTj1 = Tr(U222 J2 UT21 ) = 0
j=max(1,2,2)
2
h212 =
(
)
Traza U j12 J j UTj2 = Tr(U212 J2 UT22 )
j=max(2,1,2)
S1C1 m2 C12 m2
S2 m2 S1C1 m2
1
= Tr
0
0
0
0
2
h221 =
0
0
0
0
0
0
= S1C1 m2 C1 S1 m2 = 0
0
0
(
)
Traza U j21 J j UTj2 = Tr(U221 J2 UT21 )
j=max(2,2,1)
h222 =
(
)
Traza U j22 J j UTj2 = Tr(U222 J2 UT22 ) = 0
j=max(2,2,2)
4 MODELO DINAMICO
DEL ROBOT 2DOF
h1 =
k=1 m=1
h2 =
k=1 m=1
= d2 m2 12 + (0 + 0)1 d2 + 0(d22 ) = d2 m2 12
Luego:
[
H=
2d2 m2 1 d2
d2 m2 12
ci =
m j gU j1 j r j
j=1
y, j es el vector de coordenadas homogeneas de posicion del centro de masas del eslabon j expresado en el sistema {S j } (ver Figura 4).
1
r1 = [ 0, 0, L1 , 1 ]T ,
r2 = [ 0, 0, 0, 1 ]T
Luego.
2
c1 =
j=1
c2
)
m j gU j1 j r j = m1 gU11 1 r1 m2 gU21 2 r2
S1 0 C1 0
0
S1
C1 0 S1 0 0
C1
m2 g
= m1 g
0
0
0
0
0 L1
0
0
0
0
1
0
)
2 (
= m j gU j2 j r j = m1 gU12 1 r1 m2 gU22 2 r2
j=1
= m1 g[0]
L1 m2 g
1
0
0
0
0
0
0
0
0
0
0 S1
0 C1 0
0
0 0
1
0
0
0 C1 d2C1
0
0
0 S1 d2 S1
0
0
0
0
0
0
0
1
=0
=0
Por lo tanto:
[
T
C = [ci ] =
0
0
10
4 MODELO DINAMICO
DEL ROBOT 2DOF
= Dq + H + C
Reemplazando:
[
T1
F2
[
=
m1 L12 + m2 d22 0
0
m2
][
1
d2
[
+
2d2 m2 1 d2
d2 m2 12
[
+
0
0
Luego:
Sin embargo el vector C se vera afectado, pues, ahora el vector de gravedad g expresado en el sistema de
referencia de la base {S0 } sera:
g = [ g, 0, 0, 0 ]
Por tanto:
11
4 MODELO DINAMICO
DEL ROBOT 2DOF
C1 L1
C1 d2
S1 L1
S1 d2
m2 g
= m1 g
0
0
0
0
S1
C1
= m1 g[0] m2 g
0 = m2 gS1
0
c1
c2
= m1 gL1C1 + m2 gd2C1
Siendo la matriz C:
[
C=
m1 gL1C1 + m2 gd2C1
m2 gS1
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 eslab
on: 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 Eslab
on)
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 Eslab
on)
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 Rodrguez 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/eslab
on
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.- Ecuaci
on Din
amica del Robot)
disp(--------------------------------------------------------------------)
tau=simplify(D*[ddq1;ddq2]+H+C);
disp(El torque o par para la primera articulaci
on revoluta: )
disp(--------------------------------------------------------------------)
T1 = tau(1) % Juntura Revoluta
disp(--------------------------------------------------------------------)
disp(La fuerza para al articulaci
on prism
atica:)
disp(--------------------------------------------------------------------)
F2 = tau(2) % Juntura Prism
atica
disp(--------------------------------------------------------------------)
disp(Ecuaci
on Din
amica del Robot Manipulador RP:)
disp(--------------------------------------------------------------------)
tau=[tau(1); tau(2)]
14
4 MODELO DINAMICO
DEL ROBOT 2DOF
Ejercicio
Sea el sistema manipulador de coordenadas cilndricas que se muestra en la Figura 6. El cuerpo principal
es el eslabon 1, el eje y efector final es el eslabon 2. Se consideran a r1 y r2 como los centros de masa del
eje de rotacion (NOTE: r1 = constante, r2 = variable). Las masas m1 y m2 . Los momentos de inercia de la
masa centroidal J1 y J2 . El a ngulo gira desde el eje x positivo al eje radial ri de rotacion. T es el torque
del actuador rotatorio desde la tierra al cuerpo 1 medido positivo. F es la fuerza radial del actuador del
cuerpo 1 en el cuerpo 2 medido positivo. La gravedad g que actua a lo largo del eje de y negativo. Modelar
el sistema usando las ecuaciones dinamicas de energa de Lagrange.
Solucion
Eslabon 1 (r1 esconstante):
x1 = r1 cos
(18)
y1 = r1 sin
(19)
x1 = r1 sin
y1 = r1 cos
(20)
x2 = r2 cos
(22)
y2 = r2 sin
(23)
(21)
15
4 MODELO DINAMICO
DEL ROBOT 2DOF
x2 = r2 cos r2 sin
y2 = r2 sin + r2 cos
(24)
(25)
(26)
1
1
1
1
K = m1 r12 2 + m2 r22 2 + m2 r22 + (J1 + J2 ) 2
2
2
2
2
(27)
P = m1 y1 g + m2 y2 g
(28)
(29)
La energa potencial:
(30)
=T
dt
(31)
(32)
y
(
d
dt
L
r2
L
=F
r2
(33)
( )
(
)
d L
=
m1 r12 + m2 r22 + J1 + J2 + 2m2 r2 r2
dt
= g(m1 r1 + m2 r2 ) cos
(34)
(35)
(36)
Luego obtenemos:
(
)
T = m1 r12 + m2 r22 + J1 + J2 + 2m2 r2 r2 + g(m1 r1 + m2 r2 ) cos
M.Sc. Ricardo Rodrguez Bustinza
(37)
16
4 MODELO DINAMICO
DEL ROBOT 2DOF
L
= m2 r2
r2
(
)
d L
= m2 r2
dt r2
L
r2
(38)
(39)
= m2 r2 2 m2 g sin
(40)
Luego obtenemos:
F = m2 r2 m2 r2 2 + m2 g sin
(41)
m1 r12 + m2 r22 + J1 + J2 0
0
m2
][
r2
] [
][ ]
T
0 2m2 r2
=
+
F
m2 r2
0
r2
[
]
g(m1 r1 + m2 r2 ) cos
m2 g sin
(42)
Siendo:
[
m1 r12 + m2 r22 + J1 + J2 0
M =
0
m2
[
]
0
2m2 r2
C =
m2 r2
0
[
]
g(m1 r1 + m2 r2 ) cos
G =
m2 g sin
Tambien:
[
x =
r2
[
]
x =
r2
[
]
T
=
F
Reescribiendo (42) tenemos el modelo Lagraniano:
(
)
x = M1 (x) + C(x)x G(x)
(43)
Grupo ISCA
Especialistas en Control por Computadora
www.grupo-isca.com
17