Modelo Dinamico Formulacion de Lagrange Euler

Algoritmo de LagrangeEuler
M.Sc. Ricardo Rodrguez Bustinza

robust@grupo-isca.com
Indice
1. Introduccion
1.1. Sistemas Mecanicos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1.1. Energa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2. Sistemas Electricos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
2
2
3
2. Ecuaciones de Movimiento de Lagrange

2.1. El Lagraniano . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2. La Ecuacion de Lagrange . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
3
4
3. Modelando con Ecuaciones de Lagrange
4. Modelo Dinamico del Robot 2DOF

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.
1.1. Sistemas Mecanicos

Examinaremos algunos conceptos basicos de la fsica que permitira que comprendamos mejor la dinamica
de sistemas mecanicos. En el enfoque Lagraniano, la energa es el asunto principal. Observaremos varias
formas de energa para sistemas mecanicos, as como las formas comparables de la energa para sistemas
electricos.
1.1.1. Energa
Aqu, consideramos varias formas de energa, y como aparecen. Para objetos en movimiento, nosotros
tenemos la energa cinetica. Para un objeto de masa m, con velocidad v, la energa cinetica es dada por:
1
Ke = mv2
(1)
2
Es siempre una cantidad escalar, y no un vector. La energa cinetica tambien puede ser asociada con los
giros de los objetos. Para un objeto con inercia rotacional J, y velocidad angular , la energa cinetica es
dada por:
1
Ke = J 2
(2)
2
La energa potencial de masa m con una altura h en un campo gravitacional, con una constante g es dado
por:
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 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)
1.2 Sistemas Electricos
2 ECUACIONES DE MOVIMIENTO DE LAGRANGE
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)
1.2. Sistemas Electricos

Para sistemas electricos, aca se presentan los homologos con los sistemas mecanicos. Puede haber tipos
de la energa cinetica, energa potencial, y energa disipativa. En la siguiente Tabla se muestran estos
homologos.
Energa
Cinetica
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. 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 especficas del sistema en un tiempo t1 y t2 es un
movimiento natural si, y solo si, la energa del sistema permanece constante.
Note que aunque se haya dado una relacion de Fuerza y Movimiento, cuyas las ideas son concernientes a la energa, son por lo tanto igualmente aplicable a sistemas electricos.
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.
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 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.
2.2. La Ecuacion de Lagrange

Finalmente, obtenemos la ecuacion de Lagrange que esta asociada con cada coordenada generalizada qi ,
es dada por:
(
)
d L
L P
+
= 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).
3. Modelando con Ecuaciones de Lagrange

Modelar el sistema masa-resorte que se muestra en la Figura 1 usando las ecuaciones dinamicas de energa
de Lagrange. En el sistema, x es la direccion del movimiento, x es la velocidad del movil, k es la constante
de elongacion del resorte, m es la masa del movil, y FEXT es la fuerza externa aplicada al movil para
realizar el movimiento en la direccion de x.
Figura 1: Sistema masa-resorte.
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)
La ecuacion de Lagrange generalizada es:

(
d
dt
L
q
L
=Q
q
(11)
De (8) y (9) en (10):

1
1
L = mx kx2
2
2
(12)
Reescribiendo (11) para el sistema:

(
d
dt
L
x
L
= FEXT
x
(13)
Desarrollando derivadas:
L
= mx
x
( )
d L
= mx
dt x
L
x
= kx
(14)
(15)
(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 de
libertad (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.


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:
i
1
0
Articulacion
1
2
di
0
d2
ai
0
0
i
0
-90
L-E 2: Matrices de transformacion 0 Ai .
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
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
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:

4 MODELO DINAMICO
DEL ROBOT 2DOF
J2
0
0
=
0
0
0
0
0
0
0 0
0 0
0 0
0 m2
L-E 6: Matrices de inercias D = [di j ].

2
d11 =
(
)
Traza Uk1 Jk UTk1 = Tr(U11 J1 UT11 ) + Tr(U21 J2 UT21 )
k=max(1,1)
C12 d22 m2 S1C1 d22 m2

C12 L12 m1 S1C1 L12 m1 0 0
2 2
2
2
2
2
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
(
)
k=max(1,2)
d21 =
0
0
0
0
0
0
0
0
0
0
= S1C1 d2 m2 S1C1 d2 m2 = 0
0
0
(
)
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
L-E 7: Terminos en hikm .

2
h111 =
(
)
Traza U j11 J j UTj1 = Tr(U111 J1 UT11 ) + Tr(U211 J2 UT21 )
j=max(1,1,1)
C1 S1 m1 L12 S12 m1 L12

C2 m1 L2
C1 S1 m1 L12
1
1
= Tr
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
= C1 S1 m1 L12 +C1 S1 m1 L12 d22 S1C1 m2 + d22 S1C1 m2 = 0

2
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
(
)
j=max(1,2,1)
= U221 = U212 h121 = h112 = d2 m2

2
h211 =
(
)
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 =
(
)
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
(
)
j=max(2,2,1)
= U221 = U212 h221 = h212 = 0

2
h222 =
(
)
Traza U j22 J j UTj2 = Tr(U222 J2 UT22 ) = 0
j=max(2,2,2)

4 MODELO DINAMICO
DEL ROBOT 2DOF
L-E 8: Matriz columna de fuerzas de Coriolis y centrpetas H = [hi ]T

2
h1 =
h1km qk qm = h111 12 + h112 1 d2 + h121 d2 1 + h122 d22
k=1 m=1
= 0(12 ) + (d2 m2 + d2 m2 )1 d2 + 0(d22 ) = 2d2 m2 1 d2

2
h2 =
h2km qk qm = h211 12 + h212 1 d2 + h221 d2 1 + h222 d22
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
L-E 9: Matriz columna de fuerzas de gravedad C = [ci ]T .

N
ci =
m j gU j1 j r j
j=1
donde, g es el vector gravedad expresado en el sistema de la base del robot {S0 }.

g = [ 0, 0, g, 0 ]
jr
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
L-E 10: La ecuacion dinamica del robot sera:
= 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:
T1 = (m1 L12 + m2 d22 )1 + 2d2 m2 1 d2

F2 = m2 d2 d2 m2 12
Donde T1 es el par motor efectivo (incluye rozamiento y otras perturbaciones) que actuan sobre la articulacion 1 y F2 es la fuerza motora efectiva que actua sobre la articulacion 2. El modelo dinamico responde
a un sistema de dos ecuaciones diferenciales de segundo orden no lineales y acopladas. Se observa que el
termino gravedad C no aparece en el modelo. Esto se debe a que la estructura horizontal del robot permite
que las fuerzas de gravedad se proyecten sobre los apoyos, no siendo necesario la aportacion de par o
fuerza para vencerla. Si consideramos al robot en la posicion horizontal (ver Figura 5), manteniendo la
definicion 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 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
De este modo las ecuaciones que corresponden al modelo dinamico seran:

T1 = (m1 L12 + m2 d22 )1 + 2d2 m2 1 d2 + (m1 gL1 + m2 gd2 ) cos 1
F2 = m2 d2 d2 m2 12 + m2 g sin 1
Algoritmo Lagrange Euler con Matlab

% ------------------------------------------------------------------------% Profesor: Ricardo Rodriguez Bustinza
% Grupo ISCA
% www.grupo-isca.com
% ------------------------------------------------------------------------disp(--------------------------------------------------------------------)
disp(DIN
AMICA 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 Par
ametros DH)
disp(-a---alpha---d---theta---)
DH=[
0
-pi/2
0
q1
0
0
q2
0
]
disp(--------------------------------------------------------------------)
disp(2.- Calculo de las matrices de transformaci
on: Aij)
disp(--------------------------------------------------------------------)
disp(Matriz del primer eslab
on m
ovil 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)
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)))
disp(--------------------------------------------------------------------)
disp(4.- Calculo de las matrices : Uijk)
disp(--------------------------------------------------------------------)
U111=simple(simplify(diff(U11,q1)))
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.)))
13

4 MODELO DINAMICO
DEL ROBOT 2DOF
d12=simple(simplify(trace(U22*J2*U21.)))
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.))
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.
Figura 6: Manipulador robotico cilndrico.

Considerar: q1 = , q2 = r2 , q1 = , q2 = r2 , Q1 = T , y Q2 = F.
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)
Derivando (18) y (19).
(21)
Eslabon 2 (r2 esvariable):
Derivando (22) y (23).

15

4 MODELO DINAMICO
DEL ROBOT 2DOF
x2 = r2 cos r2 sin
y2 = r2 sin + r2 cos
(24)
(25)
La energa cinetica (traslacional + rotacional):

1
1
1
1
K = m1 (x12 + y21 ) + m2 (x22 + y22 ) + J1 2 + J2 2
2
2
2
2
Operando terminos en (26):
(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)
P = m1 gr1 sin + m2 gr2 sin
(29)
La energa potencial:
Reemplazando (19) y (23) en (28)
La ecuacion de Lagrange referida a las energas:

L = K P
(30)
Reemplazando (27) y (29) en (30)

1
1
1
1
L = m1 r12 2 + m2 r22 2 + + m2 r22 + (J1 + J2 ) 2 g(m1 r1 + m2 r2 ) sin
2
2
2
2
La ecuacion de Lagrange generalizada es:
( )
d L
L
=T
dt
(31)
(32)
y
(
d
dt
L
r2
L
=F
r2
(33)
Desarrollando derivadas para obtener la ecuacion de T :

(
)
L
=
m1 r12 + m2 r22 + J1 + J2
( )
(
)
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
(37)
16

4 MODELO DINAMICO
DEL ROBOT 2DOF
Desarrollando derivadas para obtener la ecuacion de F:
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)
Finalmente de (37) y (41) obtenemos la ecuacion dinamica matricial:

[
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

Modelo Dinamico Formulacion de Lagrange Euler

Enviado por

Dados do documento

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

Modelo Dinamico Formulacion de Lagrange Euler

Enviado por

Direitos autorais:

Formatos disponíveis

Algoritmo de LagrangeEuler

M.Sc. Ricardo Rodrguez Bustinza

2. Ecuaciones de Movimiento de Lagrange

3. Modelando con Ecuaciones de Lagrange

4. Modelo Dinamico del Robot 2DOF

1.1. Sistemas Mecanicos

M.Sc. Ricardo Rodrguez Bustinza

1.2 Sistemas Electricos

2 ECUACIONES DE MOVIMIENTO DE LAGRANGE

1.2. Sistemas Electricos

2. Ecuaciones de Movimiento de Lagrange

M.Sc. Ricardo Rodrguez Bustinza

2.2 La Ecuacion de Lagrange

3 MODELANDO CON ECUACIONES DE LAGRANGE

2.2. La Ecuacion de Lagrange

3. Modelando con Ecuaciones de Lagrange

Figura 1: Sistema masa-resorte.

M.Sc. Ricardo Rodrguez Bustinza

La ecuacion de Lagrange generalizada es:

De (8) y (9) en (10):

Reescribiendo (11) para el sistema:

Reemplazando (14), (15) y (16) en (13) tenemos:

4. Modelo Dinamico del Robot 2DOF

Figura 2: Robot polar 2DOF.

Los sistemas de referencia para el robot se muestra en la Figura 3.

Figura 3: Sistemas de referencia del robot polar.

L-E 2: Matrices de transformacion 0 Ai .

M.Sc. Ricardo Rodrguez Bustinza

L-E 5: Matrices de pseudoinercia Ji . La Figura 4 muestra los elementos del robot.

Figura 4: Elementos del robot polar.

L-E 6: Matrices de inercias D = [di j ].

C12 d22 m2 S1C1 d22 m2

M.Sc. Ricardo Rodrguez Bustinza

L-E 7: Terminos en hikm .

C1 S1 m1 L12 S12 m1 L12

= C1 S1 m1 L12 +C1 S1 m1 L12 d22 S1C1 m2 + d22 S1C1 m2 = 0

= U221 = U212 h121 = h112 = d2 m2

= U221 = U212 h221 = h212 = 0

M.Sc. Ricardo Rodrguez Bustinza

L-E 8: Matriz columna de fuerzas de Coriolis y centrpetas H = [hi ]T

h1km qk qm = h111 12 + h112 1 d2 + h121 d2 1 + h122 d22

= 0(12 ) + (d2 m2 + d2 m2 )1 d2 + 0(d22 ) = 2d2 m2 1 d2

h2km qk qm = h211 12 + h212 1 d2 + h221 d2 1 + h222 d22

L-E 9: Matriz columna de fuerzas de gravedad C = [ci ]T .

donde, g es el vector gravedad expresado en el sistema de la base del robot {S0 }.

M.Sc. Ricardo Rodrguez Bustinza

L-E 10: La ecuacion dinamica del robot sera:

T1 = (m1 L12 + m2 d22 )1 + 2d2 m2 1 d2

Figura 5: Configuracion horizontal del robot polar.

M.Sc. Ricardo Rodrguez Bustinza

De este modo las ecuaciones que corresponden al modelo dinamico seran:

Algoritmo Lagrange Euler con Matlab

M.Sc. Ricardo Rodrguez Bustinza

Figura 6: Manipulador robotico cilndrico.

Derivando (18) y (19).

Eslabon 2 (r2 esvariable):

Derivando (22) y (23).

La energa cinetica (traslacional + rotacional):

P = m1 gr1 sin + m2 gr2 sin

Reemplazando (19) y (23) en (28)

La ecuacion de Lagrange referida a las energas:

Reemplazando (27) y (29) en (30)

Desarrollando derivadas para obtener la ecuacion de T :

Desarrollando derivadas para obtener la ecuacion de F: