modelo dinámico formulación de lagrange-euler

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.


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:


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.



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



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:



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

]



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



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

]



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:



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’)



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 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.’)))



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/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)]



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).



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)



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


modelo dinámico formulación de lagrange-euler

Documents