upiita mecatronica 9 comparación de la simulación y animación de un robot "r" y...

Unidad Profesional Interdisciplinaria en Ingeniería y Tecnologías Avanzadas.

MECATRONICA IX

ROBOTICA II

� Implementación del método de Euler para resolver ecuaciones diferenciales.� Implementación del método de Runge-Kutta de 4to orden para resolver ecua-ciones diferenciales.

� Comparación de la simulación y la animación de un mecanismo "R" de 1GDL.

� Comparación de la simulación y la animación de un mecanismo "RR" de 2GDL.

Responsable de la asignatura: Dr. Miguel Gabriel Villarreal Cervantes

Alumno: Rodríguez Ramírez Juan de DiosGrupo: 9MV1

México, D.F.29 de septiembre ,2010.

Indice

1 Implementación del método de Euler para resolver ecuaciones diferenciales. 3

2 Implementación del método de Runge-Kutta de 4to orden para resolver ecuaciones diferen-ciales. 3

3 Conclusión de las secciones 1 y 2. 4

4 Comparación de la simulación y la animación de un mecanismo "R" de 1 GDL. 44.1 Obtención del modelo dinámico de un mecanismo "R" de 1 GDL . . . . . . . . . . . . . . . . . . . . 4

4.1.1 Asignar el sistema de coordenadas a cada eslabón (según las normas de D-H) y obtener losparámetros de D-H. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

4.1.2 Obtener ii+1T; para todo i = 0; :::; n, donde n es el número de eslabones . . . . . . . . . . . . 54.1.3 Establecer condiciones iniciales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54.1.4 Cálculo de las velocidades y aceleraciones. Propagación hacia delante . . . . . . . . . . . . . 54.1.5 Aceleración lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54.1.6 Aceleración lineal en el centro de masa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64.1.7 Cálculo de las fuerzas y pares. Propagación hacia atrás . . . . . . . . . . . . . . . . . . . . . 64.1.8 Par o fuerza resultante . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64.1.9 Modelo dinámico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

4.2 Animación un mecanismo "R" de 1GDL . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74.3 Comparación del modelo dinámico obtenido y las gra�cas de la animación. . . . . . . . . . . . . . . . 7

5 Obtención del modelo dinámico de un mecanismo "RR" de 2 GDL 85.1 Asignar el sistema de coordenadas a cada eslabón (según las normas de D-H) y obtener los parámetros

de D-H . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85.2 Obtener para todo i = 0; :::; n, donde n es el número de eslabones . . . . . . . . . . . . . . . . . . . . 85.3 Establecer condiciones iniciales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

5.3.1 Obtener iPCi ;CiIi; mi y n+1fn+1;

n+1nn+1 para todo i = 1; :::; n: . . . . . . . . . . . . . . . . 85.3.2 Sistema base: 0w0 = 0; 0 _w0 = 0; 0 _v0 = g 0XY Z0;

0XY Z0 : Es un vector unitario en ladirección opuesta de la gravedad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

5.3.3 Sistema base: 0w0 = 0; 0 _w0 = 0; 0 _v0 = g 0XY Z0;0XY Z0 : Es un vector unitario en la

dirección opuesta de la gravedad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95.4 Cálculo de las velocidades y aceleraciones. Propagación hacia delante . . . . . . . . . . . . . . . . . . 9

5.4.1 Velocidad y aceleración angular . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95.4.2 Aceleración lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

5.5 Cálculo de las fuerzas y pares. Propagación hacia atrás . . . . . . . . . . . . . . . . . . . . . . . . . 105.5.1 Fuerza y momento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

5.6 Par o fuerza resultante . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115.6.1 Si la unión i es de tipo rotacional . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115.6.2 Modelo dinamico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

6 Animación un mecanismo "RR" de 2GDL 12

7 Comparación del modelo dinámico obtenido y las gra�cas de la animación 13

8 Conclusiones: 13

9 Anexo 139.1 Programa en Matlab para solucionar la ecuación diferencial _y = sin(x) con el metodo de Euler . . . 139.2 Programa en Matlab para solucionar la ecuación diferencial _y = sin(x) con el metodo de Runge-Kutta

de 4to orden . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149.3 Programa en Matlab para solucionar la ecuación diferencial del modelo dinámico de un mecanismo

"R" de 1GDL con el método de Runge-Kutta de 4to orden . . . . . . . . . . . . . . . . . . . . . . . . 149.4 Función en Matlab para evaluar numéricamente el modelo dinámico de un mecanismo "RR" de 2 GDL 149.5 Programa en matlab para solucionar la ecuacion diferencial del modelo dinamico de un mecanismo

"RR" de 2GDL con el metodo de Runge-Kutta de 4to orden . . . . . . . . . . . . . . . . . . . . . . . 152

1 Implementación del método de Euler para resolver ecuaciones difer-enciales.

Se necesita implementar el método de Euler para resolver la ecuación diferencial: dydx = cos(x) , para lo cual se utilizóel programa No.1 mostrado en el anexo , y se obtuvo lo siguiente:

numero de pasos=100, intervalo de x0 = 0 a xf = 4� numero de pasos=300, intervalo de x0 = 0 a xf = 4�

numero de pasos=1000, intervalo de x0 = 0 a xf = 4�

2 Implementación del método de Runge-Kutta de 4to orden para re-solver ecuaciones diferenciales.

Se necesita implementar el método de Runge-Kutta de 4to orden para resolver la ecuación diferencial: dydx = cos(x) ,para lo cual se utilizo el programa mostrado en el anexo , y se obtuvo lo siguiente:

numero de pasos=100, intervalo de x0 = 0 a xf = 4�

3

numero de pasos=300, intervalo de x0 = 0 a xf = 4� numero de pasos=1000, intervalo de x0 = 0 a xf = 4�

3 Conclusión de las secciones 1 y 2.

En las gra�cas se puede observar que el método de Euler se acerca mucho a la gra�ca de sin(x), sin embargo es muynotable que se requiere un numero de pasos mayor, cuando el numero de pasos fue igual a 1000 la aproximación fuemuy precisa. Aplicando el método de Runge-Kutta se hace evidente que la aproximación a la gra�ca real es mayorque con el método de Euler, y con un número menor de pasos, sin embargo el método de Runge-Kutta requiere demás operaciones para cada iteración.

4 Comparación de la simulación y la animación de un mecanismo "R"de 1 GDL.

4.1 Obtención del modelo dinámico de un mecanismo "R" de 1 GDL

4.1.1 Asignar el sistema de coordenadas a cada eslabón (según las normas de D-H) y obtener losparámetros de D-H.

D =

264 i �i�1 ai�1 di �i

1 0 0 0 �1 ��

22 0 L1 0 0

375

4

4.1.2 Obtener ii+1T; para todo i = 0; :::; n, donde n es el número de eslabones

T (i) =

2664cos(Di+1;5) � sin(Di+1;5) 0 Di+1;3

sin(Di+1;5) cos(Di+1;2) cos(Di+1;5) cos(Di+1;2) � sin(Di+1;2) � sin(Di+1;2)Di+1;4sin(Di+1;5) sin(Di+1;2) cos(Di+1;5) sin(Di+1;2) cos(Di+1;2) cos(Di+1;2)Di+1;4

0 0 0 1

3775

T(1)=

2664cosD2;5 � sinD2;5 0 D2;3

cosD2;2 sinD2;5 cosD2;2 cosD2;5 � sinD2;2 � (sinD2;2)D2;4sinD2;2 sinD2;5 cosD2;5 sinD2;2 cosD2;2 (cosD2;2)D2;4

0 0 0 1

3775 =2664

sin �1 cos �1 0 0� cos �1 sin �1 0 00 0 1 00 0 0 1

3775T(2)=

2664cosD3;5 � sinD3;5 0 D3;3


0 0 0 1

3775 =26641 0 0 L10 1 0 00 0 1 00 0 0 1

3775R1 =

24 sin �1 cos �1 0� cos �1 sin �1 00 0 1

35 P1 =

24 000

35R2 =

24 1 0 00 1 00 0 1

35 P2 =

24 L100

354.1.3 Establecer condiciones iniciales

Obtener iPCi ;CiIi; mi y n+1fn+1;

n+1nn+1 para todo i = 1; :::; n:Sistema base: 0w0 = 0; 0 _w0 = 0; 0 _v0 = g 0XY Z0;

0XY Z0 : Es un vector unitario en la dirección opuesta dela gravedad.

1PC1 = PC =

24 L00

35 C1I1 = I1 =

24 Ix 0 00 Iy 00 0 Iz

35 2f2 = f2 =

24 000

35 2n2 = n2 =

24 Mx

My

Mz

350w0 = w0 =

24 000

35 _w0 = w00 =

24 000

35 0 _v0 = v00 =

24 0g0

35Método de Newton-Euler

4.1.4 Cálculo de las velocidades y aceleraciones. Propagación hacia delante

Para i = 0; :::; n� 1Velocidad y aceleración angular:Si la unión i+ 1 es de tipo rotacionali+1wi+1 =

i+1i Riwi + _�i+1

i+1Zi+1i+1

_wi+1 =i+1i Ri _wi +

i+1i Riwi � _�i+1 i+1Zi+1 + ��i+1 i+1Zi+1

1w1 =10 R

0w0 + _�11Z1 = R

T1 w0 +

24 00�01

35 = w1 =24 00�01

351 _w1 =

10 R

0 _w0 +10 R

0w0 � _�1 1Z1 + ��1 1Z1 = RT1 w00 +RT1 w0 �

24 00�01

35+24 00�001

35 = w01 =24 00�001

354.1.5 Aceleración lineal

Si la unión i+ 1 es de tipo rotacionali+1 _vi+1 =

i+1i R

�i _vi +

i _wi �i Pi+1 +i wi ��iwi �i Pi+1

��_v1 =

10 R

�0 _v0 +

0 _w0 �0 P1 +0 w0 ��0w0 �0 P1

��= RT1 (v

00 + w

00 � P1 + w0 � (w0 � P1)) = v01 =

24 �g cos �1g sin �10

355

4.1.6 Aceleración lineal en el centro de masai+1 _vCi+1 =

i+1 _vi+1 +i+1 _wi+1 �i+1 PCi+1 +i+1 wi+1 �

�i+1wi+1 �i+1 PCi+1

�1 _vC1 =

1 _v1+1 _w1�1PC1+1w1�

�1w1 �1 PC1

�= v01+w

01�PC+w1� (w1�PC) = v0C =

24 �L(�01)2 � g cos �1L�001 + g sin �1

0

35Método de Newton-Euler

4.1.7 Cálculo de las fuerzas y pares. Propagación hacia atrás

Para i = n; :::; 1Fuerza y momento:ifi =

i Fi +ii+1 R

i+1f ii+1ni =

i Ni +ii+1 R

i+1ni+1 +i PCi �i Fi +i Pi+1 �ii+1 Ri+1fi+1

donde:iFi = mii _vci y

iNi =ciIi

i _wi +i wi �Ci Ii iwi

1F1 = m11 _vc1 = m1v

0C = F1 =

24 �m1

�L(�01)

2 + g cos �1�

m1

�L�001 + g sin �1

�0

351f1 =

1 F1 +12 R

2f2 = F1 +R2f2 = f1 =

24 �m1

�L(�01)

2 + g cos �1�

m1

�L�001 + g sin �1

�0

351N1 =

c1I11 _w1 +

1 w1 �C1 I1 1w1 = I1w01 + w1 � (I1w1) = N1 =

24 00�001Iz

351n1 =

1 N1 +12 R

2n2 +1 PC1 �1 F1 +1 P2 �12 R2f2 = N1 +R2n2 + PC � F1 + P2 � (R2f2) =

n1 =

24 Mx

My

Mz + �001Iz + Lm1

�L�001 + g sin �1

�35

4.1.8 Par o fuerza resultante

Si la unión i es de tipo rotacional� i =

i nTiiZi

�1 =1 nT1

1Z1 = nT1

24 001

35 =Mz + �001Iz + Lm1

�L�001 + g sin �1

�4.1.9 Modelo dinámico

Mz + �001Iz + Lm1

�L�001 + g sin �1

�= �001m1L

2 + gm1 (sin �1)L+Mz + �001Iz

M = m1L2

C = 0G = gm1 (sin �1)L+Mz + �

001Iz

�1 =M�001 + C�

01 +G

6

4.2 Animación un mecanismo "R" de 1GDL

Se utilizo working model, que permite simular un modelo dinámico virtual de un objeto, en este caso una barra"R" unida a un motor:

El objeto tiene una masa de 10 kg , una longitud de 6 m, el centro de masa esta a la mitad del objeto, y tieneun momento de inercia de 30.833 kg �m2

4.3 Comparación del modelo dinámico obtenido y las gra�cas de la animación.

Haciendo uso de el método de Runge-Kutta implementado en Matlab, se soluciono numéricamente la ecuacióndiferencial del modelo dinámico del mecanismo (línea roja y verde) , y se comparo con el resultado obtenido de laanimación en Working Model (puntos azules).

7

5 Obtención del modelo dinámico de un mecanismo "RR" de 2 GDL

5.1 Asignar el sistema de coordenadas a cada eslabón (según las normas de D-H) yobtener los parámetros de D-H

D =

2664i �i�1 ai�1 di �i1 0 0 0 �12 0 L1 0 �23 0 L2 0 0

37755.2 Obtener para todo i = 0; :::; n, donde n es el número de eslabones

T (i) =

2664cos(Di+1;5) � sin(Di+1;5) 0 Di+1;3

sin(Di+1;5) cos(Di+1;2) cos(Di+1;5) cos(Di+1;2) � sin(Di+1;2) � sin(Di+1;2)Di+1;4sin(Di+1;5) sin(Di+1;2) cos(Di+1;5) sin(Di+1;2) cos(Di+1;2) cos(Di+1;2)Di+1;4

0 0 0 1

377501T = T (1) =

2664cosD2;5 � sinD2;5 0 D2;3


0 0 0 1

3775= T1=2664cos �1 � sin �1 0 0sin �1 cos �1 0 00 0 1 00 0 0 1

377501R = R1 =

24 cos �1 � sin �1 0sin �1 cos �1 00 0 1

35 0P1 = P1 =

24 000

3512T = T (2) =

2664cosD3;5 � sinD3;5 0 D3;3


0 0 0 1

3775= T2=2664cos �2 � sin �2 0 L1sin �2 cos �2 0 00 0 1 00 0 0 1

377512R = R2 =

24 cos �2 � sin �2 0sin �2 cos �2 00 0 1

35 1P2 = P2 =

24 L100

3523T = T (3) =

2664cosD4;5 � sinD4;5 0 D4;3


0 0 0 1

3775 = T3 =26641 0 0 L20 1 0 00 0 1 00 0 0 1

377523R = R3 =

24 1 0 00 1 00 0 1

35 2P3 = P3 =

24 L200

355.3 Establecer condiciones iniciales

5.3.1 Obtener iPCi ;CiIi; mi y n+1fn+1;

n+1nn+1 para todo i = 1; :::; n:

5.3.2 Sistema base: 0w0 = 0;0 _w0 = 0;

0 _v0 = g0XY Z0;

0XY Z0 : Es un vector unitario en la direcciónopuesta de la gravedad

1Pc1 = P01 =

24 P11P120

35 2Pc2 = P02 =

24 P21P220

35 C1I1 = I1 =

24 I11 0 00 I12 00 0 I13

35C2I2 = I2 =

24 I21 0 00 I22 00 0 I23

35 3f3 = f3 =

24 f31f32f33

35 3n3 = n3 =

24 000

35

8

5.3.3 Sistema base: 0w0 = 0;0 _w0 = 0;

0 _v0 = g0XY Z0;

0XY Z0 : Es un vector unitario en la direcciónopuesta de la gravedad

0w0 = w00 =

24 000

35 0 _w0 = w000 =

24 000

35 0 _v0 = v000 =

24 �g00

355.4 Cálculo de las velocidades y aceleraciones. Propagación hacia delante

5.4.1 Velocidad y aceleración angular

Si la unión i+ 1 es de tipo rotacional

i+1wi+1 =i+1i Riwi + _�

i+1

i+1Zi+1

i+1 _wi+1 =i+1i Ri _wi +

i+1i Riwi � _�

i+1

i+1Zi+1 +��i+1

i+1Zi+1

Para fi = 0g

1w1 =10 R

0w0 + _�1

1 Z1 = RT1 � w00 +

24 00�01

35 = w11 =24 00�01

351 _w1 =

10 R

0 _w0 +10 R

0w0 � _�1 1Z1 + ��1 1Z1 = RT1 � w000 +�RT1 � w00

��

24 00�01

35+24 00�001

35 = w011 =24 00�001

35Para fi = 1g

2w2 =21 R

1w1 + _�2

2 Z2 = RT2 � w11 +

24 00�02

35 = w22 =24 0

0�01 + �

02

352 _w2 =

21 R

1 _w1+21 R

1w1� _�2

2 Z2+��2

2 Z2 = RT2 �w011+

�RT2 � w11

��

24 00�02

35+24 00�002

35 = w022 =24 0

0�001 + �

002

355.4.2 Aceleración lineal

Si la unión i+ 1 es de tipo rotacional

i+1 _vi+1 =i+1i R

�i _vi +

i _wi �i Pi+1 +i wi ��iwi �i Pi+1

��Aceleración lineal en el centro de masa:

i+1 _vCi+1 =i+1 _vi+1 +

i+1 _wi+1 �i+1 PCi+1 +i+1 wi+1 ��i+1wi+1 �i+1 PCi+1

�Para fi = 0g

1 _v1 =10 R

�0 _v0 +

0 _w0 �0 P1 +0 w0 ��0w0 �0 P1

��= RT1 (v

000+w

000�P1+w00�(w00�P1)) = v011 =

24 �g cos �1g sin �10

351 _vC1 =

1 _v1+1 _w1�1PC1+1w1�

�1w1 �1 PC1

�= v011+w

011�P 01+w11�(w11�P 01) = C1 =

24 �P11(�01)2 � g cos �1 � �001P12�P12(�01)2 + g sin �1 + �001P11

0

35

9

Para fi = 1g2 _v2 =

21 R

�1 _v1 +

1 _w1 �1 P2 +1 w1 ��1w1 �1 P2

��= RT2 (v

011 + w

011 � P2 + w11 � (w11 � P2)) =

v022 =

24 (sin �2)�g sin �1 + �

001L1

�� (cos �2)

�L1(�

01)2 + g cos �1

�(cos �2)

�g sin �1 + �

001L1

�+ (sin �2)

�L1(�

01)2 + g cos �1

�0

352 _vC2 =

2 _v2 +2 _w2 �2 PC2 +2 w2 �

�2w2 �2 PC2

�= v022 + w

022 � P 02 + w22 � (w22 � P 02) =

C2 =

264 (sin �2)�g sin �1 + �

001L1

�� P22

��001 + �

002

�� P21

��01 + �

02

�2 � (cos �2) �L1(�01)2 + g cos �1�P21

��001 + �

002

�+ (cos �2)

�g sin �1 + �

001L1

�� P22

��01 + �

02

�2+ (sin �2)

�L1(�

01)2 + g cos �1

�0

3755.5 Cálculo de las fuerzas y pares. Propagación hacia atrás

5.5.1 Fuerza y momento

ifi =i Fi +

ii+1 R

i+1fi+1

ini =i Ni +

ii+1 R

i+1ni+1 +i PCi �i Fi +i Pi+1 �ii+1 Ri+1fi+1

donde: iFi = mii _vci y iNi =

CiIii _wi +

i wi �Ci Ii iwi

Para fi = 2g2F2 = m2

2 _vc2 = m2 � C2 =

F2 =

2664�m2

�P22

��001 + �

002

�� (sin �2)

�g sin �1 + �

001L1

�+ P21

��01 + �

02

�2+ (cos �2)

�L1(�

01)2 + g cos �1

��m2

�P21

��001 + �

002

�+ (cos �2)

�g sin �1 + �

001L1

�� P22

��01 + �

02

�2+ (sin �2)

�L1(�

01)2 + g cos �1

��0

37752N2 =

C2I22 _w2 +

2 w2 �C2 I2 2w2 = I2 � w022 + w22 � (I2 � w22) = N2 =

24 00

I23��001 + �

002

�35

2f2 =2 F2 +

23 R

3f3 = F2 + (R3 � f3) =

f2 =

2664f31 �m2

�P22

��001 + �

002

�+ (sin �2)

�g sin �1 � �001L1

�+ P21

��01 + �

02

�2 � (cos �2) �g cos �1 � (�01)2L1��f32 �m2

�(cos �2)

�g sin �1 � �001L1

�� P21

��001 + �

002

�+ P22

��01 + �

02

�2+ (sin �2)

�g cos �1 � (�01)2L1

��f33

37752n2 =

2 N2 +23 R

3n3 +2 PC2 �2 F2 +2 P3 �23 R3f3 = N2 +R3n3 + P 02 � F2 + P3 � (R3f3) =

n2 =

24 0�L2f33

I23��001 + �

002

�+ L2f32 +A+B

35Donde:A = P21m2

�P21

��001 + �

002

�+ (cos �2)

�g sin �1 + �

001L1

�� P22

��01 + �

02

�2+ (sin �2)

�L1(�

01)2 + g cos �1

��B = P22m2

�P22

��001 + �

002

�� (sin �2)

�g sin �1 + �

001L1

�+ P21

��01 + �

02

�2+ (cos �2)

�L1(�

01)2 + g cos �1

��Para fi = 1g

1F1 = m11 _vc1 = m1 � C1 = F1 =

24 �m1

�P11(�

01)2 + g cos �1 + �

001P12

�m1

��P12(�01)2 + g sin �1 + �001P11

�0

351f1 =

1 F1 +12 R

2f2 = F1 +R2f2 =

f1 =

24 C �D �m1

�P11(�

01)2 + g cos �1 + �

001P12

�E + F +m1

��P12(�01)2 + g sin �1 + �001P11

�f33

3510

Donde:

C = (cos �2)�f31 �m2

�P22

��001 + �

002

�+ (sin �2)

�g sin �1 � �001L1

�+ P21

��01 + �

02

�2 � (cos �2) �g cos �1 � (�01)2L1��D = (sin �2)

�f32 �m2

�(cos �2)

�g sin �1 � �001L1

�� P21

��001 + �

002

�+ P22

��01 + �

02

�2+ (sin �2)

�g cos �1 � (�01)2L1

��E = (cos �2)

�f32 �m2

�(cos �2)

�g sin �1 � �001L1

�� P21

��001 + �

002

�+ P22

��01 + �

02

�2+ (sin �2)

�g cos �1 � (�01)2L1

��F = (sin �2)

�f31 �m2

�P22

��001 + �

002

�+ (sin �2)

�g sin �1 � �001L1

�+ P21

��01 + �

02

�2 � (cos �2) �g cos �1 � (�01)2L1��1N1 =

C1I11 _w1 +

1 w1 �C1 11 1w1 = I1 � w011 + w11 � (I1 � w11) = N1 =

24 00

�001I13

351n1 =

1 N1 +12 R

2n2 +1 PC1 �1 F1 +1 P2 �12 R2f2 = N1 + (R2n2) + (P 01 � F1) + P2 � (R2f2) =

n1 =

24 L2f33 sin �2�L1f33 � L2f33 cos �2

I23��001 + �

002

�+ �001I13 + L2f32 + L1 (I + J) +K + P11m1

��P12(�01)2 + g sin �1 + �001P11

�+G+H

35Donde:G = P21m2

�P21

��001 + �

002

�+ (cos �2)

�g sin �1 + �

001L1

�� P22

��01 + �

02

�2+ (sin �2)

�L1(�

01)2 + g cos �1

��H = P22m2

�P22

��001 + �

002

�� (sin �2)

�g sin �1 + �

001L1

�+ P21

��01 + �

02

�2+ (cos �2)

�L1(�

01)2 + g cos �1

��I = (cos �2)

�f32 �m2

�(cos �2)

�g sin �1 � �001L1

�� P21

��001 + �

002

�+ P22

��01 + �

02

�2+ (sin �2)

�g cos �1 � (�01)2L1

��J = (sin �2)

�f31 �m2

�P22

��001 + �

002

�+ (sin �2)

�g sin �1 � �001L1

�+ P21

��01 + �

02

�2 � (cos �2) �g cos �1 � (�01)2L1��K = P12m1

�P11(�

01)2 + g cos �1 + �

001P12

�5.6 Par o fuerza resultante

5.6.1 Si la unión i es de tipo rotacional

� i =i nTi

iZi

�2 =2 nT2

2Z2 = nT2 �

24 001

35 = �2 = I23 ��001 + �002�+ L2f32 +A+B =�001I23+�

002I23+L2f32+�

001P

221m2+�

001P

222m2+�

002P

221m2+�

002P

222m2+gP22m2 cos (�1 + �2)+gP21m2 sin (�1 + �2)+

�001L1P21m2 cos �2 � �001L1P22m2 sin �2 + (�01)2L1P22m2 cos �2 + (�

01)2L1P21m2 sin �2

�1 =1nT1

1Z1 = nT1 �

24 001

35 = �1 = I23 ��001 + �002�+�001I13+L2f32+L1 (I + J)+K+P11m1

��P12(�01)2 + g sin �1 + �001P11

�+

G+H =�001I13 + �

001I23 + �

002I23 +L2f32 + �

001L

21m2 + �

001P

211m1 + �

001P

212m1 + �

001P

221m2 + �

001P

222m2 + �

002P

221m2 + �

002P

222m2 +

L1f32 cos �2+L1f31 sin �2+gP22m2 cos (�1 + �2)+gP21m2 sin (�1 + �2)+gP12m1 cos �1+gL1m2 sin �1+gP11m1 sin �1+2�001L1P21m2 cos �2+�

002L1P21m2 cos �2�2�001L1P22m2 sin �2��002L1P22m2 sin �2�(�02)2L1P22m2 cos �2�(�02)2L1P21m2 sin �2�

2�01�02L1P22m2 cos �2 � 2�01�02L1P21m2 sin �2

11

5.6.2 Modelo dinamico��1�2

�=

�L M

I23 + P221m2 + P

222m2 + L1P21m2 cos �2 � L1P22m2 sin �2 I23 + P

221m2 + P

222m2

� ��001�002

�+�

�2�02L1P22m2 cos �2 � 2�02L1P21m2 sin �2 �(�02)L1P21m2 sin �2 � (�02)L1P22m2 cos �2(�01)L1P22m2 cos �2 + (�

01)L1P21m2 sin �2 0

� ��01�02

�+�

gP22m2 cos (�1 + �2) + gP21m2 sin (�1 + �2) + gP12m1 cos �1 + gL1m2 sin �1 + gP11m1 sin �1gP22m2 cos (�1 + �2) + gP21m2 sin (�1 + �2)

�+�

L2f32 + L1f32 cos �2 + L1f31 sin �2L2f32

�Donde:L = I13 + I23 + L

21m2 + P

211m1 + P

212m1 + P

221m2 + P

222m2 + 2L1P21m2 cos �2 � 2L1P22m2 sin �2

M = I23 + P221m2 + P

222m2 + L1P21m2 cos �2 � L1P22m2 sin �2

6 Animación un mecanismo "RR" de 2GDL

Se utilizo working model, que permite simular un modelo dinámico virtual de un objeto, en este caso una barra"RR" unidas por motores:

Los eslabones son identicos, tienen una masa de 4 kg , una longitud de 4 m, el centro de masa esta a la mitaddel objeto, y tienen un momento de inercia de 5.667 kg �m2

12

7 Comparación del modelo dinámico obtenido y las gra�cas de la an-imación

Haciendo uso de el método de Runge-Kutta implementado en Matlab, se soluciono numéricamente la ecuacióndiferencial del modelo dinamico del mecanismo (línea roja y verde) , y se comparo con el resultado obtenido de laanimación en Working Model (puntos azules) para cada eslabón (�las).

8 Conclusiones:

En la comparación de la simulación con la animación del robot "R" se puede apreciar que las gra�cas son muysimilares, por lo que se comprobó que el modelo dinámico obtenido es correcto.En la comparación de la simulación con la animación del robot "RR" se puede apreciar que las gra�cas son muy

similares, aunque en la animación existen pequeñas fuerzas de fricción, por lo cual se puede llegar a la conclusiónde que estas fuerzas se hacen prácticamente despreciables cuando los pares en los motores son muy grandes.

9 Anexo

9.1 Programa en Matlab para solucionarla ecuación diferencial _y = sin(x) conel metodo de Euler

clc;clear all;close all;%rango de integracion:xi=0;xf=4*pi;rang=xf-xi;%numero de diviciones(pasos):

np=1000;step=(xf-xi)/np;%condiciones iniciales:xi=0;yi=0;sn=0;i=1;t=xi:step:xf;for x=0:step:rangecdif=cos(x+step);sn=sn+ecdif*step;y(i)=sn;

13

i=i+1;end% solucion realyr=sin(t);plot(t,y,�r�)hold on;plot(t,yr,�g�)axis([xi xf min(y)+min(y)/10 max(y)+max(y)/10])

9.2 Programa en Matlab para solucionarla ecuación diferencial _y = sin(x) conel metodo de Runge-Kutta de 4to or-den

clc;clear all;close all;xi=0;xf=4*pi;rang=xf-xi;np=200;step=(xf-xi)/np;xi=0;sn=0;i=1;f=@(x) cos(x);for x=xi:step:xfk1=f(x);k2=f(x+step/2);k3=f(x+step/2);k4=f(x+step);sn=sn+(1/6)*(k1+2*k2+2*k3+k4)*step;y(i)=sn;i=i+1;endts=xi:step:xf;tr=xi:0.1:xf;yr=sin(tr);plot(ts,y,�r�) %solucion numerica en rojohold on;plot(tr,yr,�g�) %solucion numerica en verdeaxis([xi xf min(y)+min(y)/10 max(y)+max(y)/10])

9.3 Programa en Matlab para solucionarla ecuación diferencial del modelodinámico de un mecanismo "R" de1GDL con el método de Runge-Kutta de 4to orden

clc;clear all;close all;ti=0;tf=10;np=1000;

Iz=30.833; m=10; lc=3; k=Iz+m*lc^2; tau=100;g=9.81; i=1;y=[0;0];f1=@(t,y) [y(2);(tau/k)-((lc*m*g)/k)*sin(y(1))];h=(tf-ti)/np;for t=ti:h:(tf-h)k1=f1(t,y(:,i));k2=f1(t+h/2,y(:,i)+h*k1/2);k3=f1(t+h/2,y(:,i)+h*k2/2);k4=f1(t+h,y(:,i)+h*k3);y(:,i+1)=y(:,i)+h/6*(k1+2*(k2+k3)+k4);i=i+1;endt=[ti:h:tf];subplot(1,2,1)plot(t,y(1,:),�r�)hold on[tg,rot,vel]=WM1gdl;grid onplot(tg,rot,�.�)title(�Posición angular�)axis([ti tf min(y(1,:))+min(y(1,:))/10 max(y(1,:))

+max(y(1,:))/10])subplot(1,2,2)plot(t,y(2,:),�g�)hold onplot(tg,vel,�.�)grid ontitle(�Velocidad angular�)axis([ti tf min(y(2,:))+min(y(2,:))/10 max(y(2,:))

+max(y(2,:))/10])

9.4 Función en Matlab para evaluarnuméricamente el modelo dinámicode un mecanismo "RR" de 2 GDL

function [Y]=val2gdl(t,y)P=[2,0,0;2,0,0;4,4,0];I=[0,0,5.667;0,0,5.667];T=[50;50];m=[4,4];m1=m(1,1);m2=m(1,2);I13=I(1,3);I23=I(2,3);p11=P(1,1);p12=P(1,2);p21=P(2,1);p22=P(2,2);L1=P(3,1);L2=P(3,2);M=@(t,y) [I13 +I23 +L1^2*m2 +m1*p11^2

+m1*p12^2 +m2*p21^2 +m2*p22^2 +2*L1*m2*p21*cos(y(3)) -2*L1*m2*p22*sin(y(3)),I23

+ m2*p21^2+m2*p22^2 +L1*m2*p21*cos(y(3))-L1*m2*p22*sin(y(3));

14

I23 +m2*p21^2 +m2*p22^2 +L1*m2*p21*cos(y(3))-L1*m2*p22*sin(y(3)),I23+m2*p21^2+m2*p22^2];C=@(t,y) [-2*y(4)*L1*m2*p22*cos(y(3)) -

2*y(4)*L1*m2*p21*sin(y(3)), -y(4)*L1*m2*p22*cos(y(3))-y(4)*L1*m2*p21*sin(y(3));y(2)*L1*m2*p22*cos(y(3)) +y(2)*L1*m2*p21*sin(y(3)),0];G=@(t,y) [9.81*m2*p22*cos(y(1) +y(3)) +9.81*m2*p21*sin(y(1) +y(3)) +9.81*m1*p12*cos(y(1))

+9.81*m1*p11*sin(y(1)) +9.81*L1*m2*sin(y(1));9.81*m2*p22*cos(y(1) +y(3)) +9.81*m2*p21*sin(y(1)

+y(3))];theta=((M(t,y))^-1)*(T-C(t,y)*[y(2);y(4)]-G(t,y));Y=[y(2);theta(1,1);y(4);theta(2,1)];

9.5 Programa en matlab para solucionarla ecuacion diferencial del modelodinamico de un mecanismo "RR"de 2GDL con el metodo de Runge-Kutta de 4to orden

clc;clear all;close all;ti=0;tf=10;np=1000;y=[0;0;0;0];h=(tf-ti)/np;i=1;for t=ti:h:(tf-h)k1=val2gdl(t,y(:,i));k2=val2gdl(t+h/2,y(:,i)+h*k1/2);k3=val2gdl(t+h/2,y(:,i)+h*k2/2);k4=val2gdl(t+h,y(:,i)+h*k3);y(:,i+1)=y(:,i)+h/6*(k1+2*(k2+k3)+k4);i=i+1;

end%posición angular1t=[ti:h:tf];subplot(2,2,1)plot(t,y(1,:),�r�)hold on[tg,rot,vel]=WM2gdl1;grid onplot(tg,rot,�.�)title(�Posición angular�)axis([ti tf min(y(1,:)) + min(y(1,:))/10 max(y(1,:))

+ max(y(1,:))/10])%Velocidad angular1subplot(2,2,2)plot(t,y(2,:),�g�)hold onplot(tg,vel,�.�)grid ontitle(�Velocidad angular�)axis([ti tf min(y(2,:)) + min(y(2,:))/10 max(y(2,:))

+ max(y(2,:))/10])%posición angular2t=[ti:h:tf];subplot(2,2,3)plot(t,y(3,:),�r�)hold on[tg,rot,vel]=WM2gdl2;grid onplot(tg,rot,�.�)title(�Posición angular�)axis([ti tf min(y(3,:)) + min(y(3,:))/10 max(y(3,:))

+ max(y(3,:))/10])%Velocidad angular2subplot(2,2,4)plot(t,y(4,:),�g�)hold onplot(tg,vel,�.�)grid ontitle(�Velocidad angular�)axis([ti tf min(y(4,:)) + min(y(4,:))/10 max(y(4,:))

+ max(y(4,:))/10])

15

upiita mecatronica 9 comparación de la simulación y animación de un robot "r" y...

Documents