UNIVERSIDAD NACIONAL DE

INGENIERÍA
FACULTAD DE INGENIERÍA MECÁNICA

INFORME N°2:
DINAMICA Y JACOBIANOS DE UN MANIPULADOR DE
2 G.D.L CONSIDERANDO LA DINAMICA DEL ROTOR

Curso:
Análisis y Control de Robots (MT-571)
Docente:
Calle Flores, Iván Arturo
Autor:
Berrios Rojas, William 20140004C
Castro Martinez, Alexander 20134010E
Hurtado Duarez, Johan Antonio 20093501J
Lopez Rumrill,Jorge Alejandro 20144052B

Lima - 2017
1. CINEMATICA DIRECTA
1.1 DETERMINACIÓN DE LOS SISTEMAS COORDENADOS

Figura 1. Sistemas coordenados del robot planar

1.2 DETERMINACIÓN DE LOS PARÁMETROS DE DENAVIT HARTENBERG

Tabla 1. Cuadro con los parámetros D.H

de 2GDLӨi
JUNTA di ai αi
1 Ө1 L2 L1 0

2 Ө2 0 L3 0

Tabla 2. Valores de los parámetros

L1 de 2GDL 117.5 mm
L2 66.3mm
L3 80 mm

Figura 2. Parámetros D.H para nuestro

manipulador de 2GDL
1.3 DETERMINACIÓN DE LAS MATRICES DE TRANSFORMACIÓN:

𝐴𝑖−1
𝑖 (𝑞𝑖 ) = 𝑅𝑜𝑡𝑧,𝜃𝑖 . 𝑇𝑟𝑎𝑛𝑠𝑧,𝑑𝑖 . 𝑇𝑟𝑎𝑛𝑠𝑥,𝑎𝑖 . 𝑅𝑜𝑡𝑥,𝑎𝑖

C 𝜃𝑖 −𝑆𝜃𝑖 . 𝐶𝛼𝑖 𝑆𝜃𝑖 . 𝑆𝛼𝑖 𝑎𝑖 . 𝐶𝜃𝑖

𝑆𝜃𝑖 𝐶𝜃𝑖 . 𝐶𝛼𝑖 −𝐶𝜃𝑖 . 𝑆𝛼𝑖 𝑎𝑖 . 𝑆𝜃𝑖 …(1)
𝐴𝑖−1
𝑖 (𝑞𝑖 ) = [ ]
0 𝑆𝛼𝑖 𝐶𝛼𝑖 𝑑𝑖
0 0 0 1

Hallando las matrices homogéneas:

𝐶𝜃1 𝑆𝜃1 0 𝐿1 . 𝐶𝜃1

𝑆𝜃 𝐶𝜃1 0 𝐿1 . 𝑆𝜃1
𝑇10 = [ 1 ] …(2)
0 0 1 𝐿2
0 0 0 1

𝐶𝜃12 −𝑆𝜃12 0 𝐿3 . 𝐶𝜃12 + 𝐿1 . 𝐶𝜃1

𝑆𝜃 𝐶𝜃12 0 𝐿3 . 𝑆𝜃12 + 𝐿1 . 𝑆𝜃1
𝑇20 = [ 12 ] …(3)
0 0 1 𝐿2
0 0 0 1
2. CINEMATICA INVERSA

Calculando el ángulo 𝜃2 por el teorema de cosenos

𝑥 2 + 𝑦 2 − 𝐿1 2 − 𝐿3 2
𝑐𝑜𝑠𝜃2 = =𝐷
2𝐿1 . 𝐿3

𝜃2 = cos−1 𝐷

Sin embargo esta relación no especifica en que cuadrante se encuentra 𝜃2 , por

tanto se prefiere:

sin 𝜃2 = ±√1 − 𝐷2

Por tanto la fórmula adecuada para definir el ángulo 𝜃2 es de la forma

sin 𝜃2 ±√1 − 𝐷2
tan 𝜃2 = =
𝑐𝑜𝑠𝜃2 𝐷

Figura 3. Cálculo del ángulo 𝜃2

Luego despejando 𝜃2 :

±√1 − 𝐷2
𝜃2 = tan−1 𝐸𝑛 𝑀𝑎𝑡𝑙𝑎𝑏: 𝜃2 = 𝑎𝑡𝑎𝑛2( ±√1 − 𝐷2 , 𝐷)
𝐷

En donde el signo positivo se refiere a la cinemática con codo hacia arriba y el

signo negativo a la cinemática con el codo hacia abajo

Para el cálculo del ángulo 𝜃1 se realizan las proyecciones del link 2 sobre el eje
paralelo al link 1

Luego por simple resta de ángulos calculamos 𝜃1 :

𝑦 𝐿3 . sin 𝜃2
𝜃1 = tan−1 ( ) − tan−1 ( )
𝑥 𝐿1 + 𝐿3 . cos 𝜃2

𝐸𝑛 𝑀𝑎𝑡𝑙𝑎𝑏: 𝜃1 = 𝑎𝑡𝑎𝑛2( 𝑦, 𝑥) − 𝑎𝑡𝑎𝑛2(𝐿3 . sin 𝜃2 , 𝐿1 + 𝐿3 . cos 𝜃2 )

Figura 4. Cálculo del ángulo 𝜃1

3. JACOBIANOS
3.1 DEDUCCION DE JACOBIANOS
Considerando un manipulador de n-links con juntas q=(q1. q2,...,qn )T y con matriz
de homogénea
0
𝑇𝑛0 (𝑞) = [𝑅𝑛 𝑃𝑛0 ] …(4)
0 1
La cual representa la transformación del sistema del efector final a sistema base.
Con el movimiento del robot las variables de las juntas (qi) y el efector final 𝑂𝑛0 y
la orientación 𝑅𝑛0 son funciones del tiempo.
Se define:
𝑣𝑛0 = 𝐽𝑣 𝑞̇ …(5)
𝑤𝑛0 = 𝐽𝑤 𝑞̇ …(6)

Donde 𝐽𝑣 y 𝐽𝑤 son matrices de 3xn. Combinando las dos expresiones anteriores

obtenemos el vector de velocidades angulares y lineales.
𝑣𝑛0
𝜀 = 𝐽𝑞̇ = [ ] …(7)
𝑤𝑛0
𝐽
𝐽 = [ 𝑣]
𝐽𝑤
Cálculo de jacobianos para el link ‘i’:
(𝑙 ) (𝑙𝑖 ) (𝑙 ) (𝑙𝑖 )
𝐽𝑣 𝑖 = [𝐽𝑣1 𝐽𝑣2𝑖 … … 𝐽𝑣𝑖 0 …… 0]
(𝑙 ) (𝑙𝑖 ) (𝑙 ) (𝑙𝑖 )
𝐽𝑤 𝑖 = [𝐽𝑤1 𝐽𝑤2𝑖 … … 𝐽𝑤𝑖 0 …… 0]

Donde cada columna de este vector se calcula a partir de:

(𝑙 ) 𝑧 × (𝑝𝑛 − 𝑝𝑗−1 ) 𝑗𝑢𝑛𝑡𝑎𝑠 𝑐𝑖𝑙𝑖𝑛𝑑𝑟𝑖𝑐𝑎𝑠

𝐽𝑣𝑗𝑖 = { 𝑗−1 …(8)
𝑧𝑗−1 𝑗𝑢𝑛𝑡𝑎𝑠 𝑝𝑟𝑖𝑠𝑚á𝑡𝑖𝑐𝑎𝑠

(𝑙 ) 𝑧𝑗−1 𝑗𝑢𝑛𝑡𝑎𝑠 𝑐𝑖𝑙𝑖𝑛𝑑𝑟𝑖𝑐𝑎𝑠

𝐽𝑤𝑗𝑖 = { …(9)
0 𝑗𝑢𝑛𝑡𝑎𝑠 𝑝𝑟𝑖𝑠𝑚á𝑡𝑖𝑐𝑎𝑠
Para los motores, su velocidad lineal se representa de la siguiente forma:
𝑚
𝑝̇𝑚𝑖 = 𝐽𝑣 𝑖 𝑞̇

El Jacobiano para los motores se representa de la siguiente manera:

𝑚 𝑚 𝑚 𝑚 𝑖
𝐽𝑣 𝑖 = [𝐽𝑣1𝑖 𝐽𝑣2𝑖 …. 𝐽𝑣,𝑖−1 0 … 0]

Calculo de las columnas:

(𝑚) 𝑧𝑗−1 × (𝑝𝑚 − 𝑝𝑗−1 ) 𝑗𝑢𝑛𝑡𝑎𝑠 𝑐𝑖𝑙𝑖𝑛𝑑𝑟𝑖𝑐𝑎𝑠 …(10)

𝐽𝑣𝑗 = {
𝑧𝑗−1 𝑗𝑢𝑛𝑡𝑎𝑠 𝑝𝑟𝑖𝑠𝑚á𝑡𝑖𝑐𝑎𝑠

Para los motores, su velocidad angular se representa de la siguiente forma:

𝑚
𝑤𝑚𝑖 = 𝐽𝑤 𝑖 𝑞̇
𝑚 𝑚 𝑚 𝑚
𝐽𝑤 𝑖 = [𝐽𝑤1𝑖 𝐽𝑤2𝑖 … . 𝐽𝑤,𝑖𝑖 0 … 0]

Calculo de las columnas:

(𝑙 )
(𝑚 ) 𝐽𝑤𝑗𝑖 𝑗 = 1,2, … … . 𝑖 − 1 …(11)
𝐽𝑤𝑗 𝑖 ={
𝑘𝑟𝑖 𝑧𝑚𝑖 𝑗=𝑖

Figura 5. Posicionamiento de un motor en los manipuladores

3. 2 CÁLCULO DE JACOBIANOS DE LOS CENTROS DE MASA DE CADA
LINK

Tabla 3. Cuadro con los parámetros D.H

JUNTA Өi
de 2GDL di ai αi
1 Ө1 L2 L1 0
2 Ө2 0 L3 0

Tabla 4. Valores de los parámetros

de L1
2GDL 117.5 mm
L2 66.3mm
L3 80 mm

Figura 6. Parámetros D.H para nuestro

manipulador

de 2GDL
A partir de la cinemática, calculamos las coordenadas del centro de masa de los links-
1 y 2:

Figura 7. Vista de perfil del manipulador de 2 G.D.L

 Cálculo de la posición del CM1:

𝑟𝑐𝑚1 0 = 𝑇10 . 𝑟𝑐𝑚

1
1

−(𝐿1 − 𝐿𝑐𝑚1 )
1
𝑟𝑐𝑚 =[ 0 ]
1
−𝐿𝑐𝑚1𝑧
1

𝐶𝜃1 𝑆𝜃1 0 𝐿1 . 𝐶𝜃1 −(𝐿1 − 𝐿𝑐𝑚1 ) 𝐿𝑐𝑚1 𝐶𝜃1

0
𝑟𝑐𝑚
0
= 𝑇1 . 𝑟1𝑐𝑚1 = 𝑆𝜃1 𝐶𝜃1 𝐿 .
0 1 1 .𝑆𝜃 0 = [ 𝐿𝑐𝑚1 𝑆𝜃1 ]
1
0 0 1 𝐿2 0 𝐿2
[ 0 0 0 1 ] [ 1 ] 1

 Cálculo de la posición del CM2:

𝑟𝑐𝑚2 0 = 𝑇20 . 𝑟𝑐𝑚

2
2

−(𝐿3 − 𝐿𝑐𝑚2 )
2
𝑟𝑐𝑚 =[ 0 ]
2 𝐿2
1

𝐶𝜃12 −𝑆𝜃12 0 𝐿3 . 𝐶𝜃12 + 𝐿2 . 𝐶𝜃1 −(𝐿3 − 𝐿𝑐𝑚2 )

0
𝑟𝑐𝑚
0
= 𝑇2 . 𝑟2𝑐𝑚2 = 𝑆𝜃12 𝐶𝜃12 0 𝐿3 . 𝑆𝜃12 + 𝐿2 . 𝑆𝜃1 . 0
2
0 0 1 𝐿2 0
[ 0 0 0 1 ] [ 1 ]

𝐿𝑐𝑚2 𝐶𝜃12 + 𝐿1 . 𝐶𝜃1

𝐿 𝑆𝜃 + 𝐿1 . 𝑆𝜃1
= [ 𝑐𝑚2 12 ]
𝐿2
1

Cálculo de los jacobianos con ayuda de (8), (9), (10) y (11)

−𝐿𝑐𝑚1 𝑆𝜃1 0 −𝐿𝑐𝑚2 𝑆𝜃12 − 𝐿1 . 𝑆𝜃1 −𝐿𝑐𝑚2 𝑆𝜃12

𝐽𝑣𝑐𝑚1 = [ 𝐿𝑐𝑚1 𝐶𝜃1 0] 𝐽𝑣𝑐𝑚2 = [𝐿𝑐𝑚2 𝐶𝜃12 + 𝐿1 . 𝐶𝜃1 𝐿𝑐𝑚2 𝐶𝜃12 ]
0 0 0 0

0 0 0 0
𝐽𝑤1 = [ 0 0] 𝐽𝑤2 = [ 0 0]
1 0 1 1
3.3 CÁLCULO DE JACOBIANOS DE LOS CENTROS DE MASA DE CADA
MOTOR

Figura 8. Posición de los centros de masa de cada motor

 Posición del centro de masa del motor1:

0
0
𝑟𝐶.𝑚𝑜𝑡𝑜𝑟1
=[ 0 ]
𝐿𝑚𝑜𝑡1

 Posición del centro de masa del motor2:

𝐿1 . 𝐶𝜃1
0
𝑟𝐶.𝑚𝑜𝑡𝑜𝑟2
= [ 𝐿1 . 𝑆𝜃1 ]
𝐿2 − 𝐿𝑚𝑜𝑡1
Cálculo de los Jacobianos con ayuda de (8), (9), (10) y (11)

0 0
𝐽𝑣_𝑚𝑜𝑡𝑜𝑟1 = [0 0]
0 0

−𝐿1 . 𝑆𝜃1 0
𝐽𝑣_𝑚𝑜𝑡𝑜𝑟2 = [ 𝐿1 . 𝐶𝜃1 0]
0 0

0 0
𝐽𝑤_𝑚𝑜𝑡𝑜𝑟1 =[ 0 0]
𝑘𝑟1 0

0 0
𝐽𝑤_𝑚𝑜𝑡𝑜𝑟1 = [0 0 ]
1 𝑘𝑟1
4. DINÁMICA
4.1 CALCULO DE INERCIAS Y CENTROS DE MASA DE LOS ESLABONES
4.1.1 CALCULO DE CENTROS DE MASA
 Link1

Figura 9. Vista isométrica del link-1 Figura 10. Vista frontal del link-1

Tabla 5. Valores de los parámetros del link-1

de 2GDL
Lcm1z 3.31 mm

Lcm1 54 mm
Masa link1 18.61 gr.

 Link2

Figura 11. Vista de perfil del link-2

Tabla 6. Valores de los parámetros del link-2

de 2GDL
Lcm2 36.39 mm

Masa link 2 6.85 gr.

4.1.2 CALCULO DE INERCIAS
 LINK 1

Figura 12. Link-1 conformado por los acoples y la barra principal

Figura 13. Cálculo del tensor de Inercia del link-1 mediante el

uso del software SolidWorks 2017

 LINK 2

Figura 15. Cálculo del tensor de Inercia del link-2

mediante el uso del software SolidWorks 2017

Figura 14. Link2 en SolidWorks

4.2 MÉTODO DE LAGRANGE

El análisis del modelo dinámico puede ser útil para diseño y cálculo de las fuerzas y
pares requerido para la ejecución de movimientos típicos, proporciona información útil
para diseñando juntas, transmisiones y actuadores. Para ello usaremos el método que
se basa en la formulación de Lagrange y es conceptualmente simple y sistemático.

El modelo dinámico de un manipulador proporciona una descripción de la relación entre

los pares de accionamiento del conjunto y el movimiento de la estructura. Con la
formulación de Lagrange. El Lagrangiano del sistema mecánico se puede definir como:

𝐿 =𝑇−𝑈

Donde T y U denotan la energía cinética y potencial del sistema. La ecuación de

Lagrange puede ser expresada por:

𝑑 𝜕𝐿 𝜕𝐿
− 𝜕𝑞 = 𝜀𝑖 para 𝑖 = 1,2 … , 𝑛
𝑑𝑡 𝜕𝑞𝑖̇ 𝑖

Donde 𝜀𝑖 es la fuerza generalizada asociada con el movimiento generalizado 𝑞𝑖 , esta

ecuación puede ser escrita de manera compacta de la forma:

𝑑 𝜕𝐿 𝑇 𝜕𝐿 𝑇 …(12)
( ) −( ) =𝜀
𝑑𝑡 𝜕𝑞̇ 𝜕𝑞

Las contribuciones a las fuerzas generalizadas están dadas por las fuerzas no
conservativas, es decir, la articulación, Pares de torsión del actuador, pares de fricción
de la junta, así como pares de torsión inducidos por fuerzas efectores finales en el
contacto con el medio ambiente. La ecuación anterior establece la relación existente
entre el vector de fuerzas aplicadas al manipulador y las posiciones de las
articulaciones, las velocidades y aceleraciones. Por lo tanto, permiten la derivación del
modelo dinámico del manipulador a partir de la determinación de la energía cinética y
potencial del sistema mecánico.
4.2.1 Calculo de la Energía Cinética

Considerando un manipulador con n juntas, la energía cinética total está dada por:

𝑇 = ∑(𝑇𝑙𝑖 + 𝑇𝑚𝑖 ) …(13)

𝑖=1

Donde 𝑇𝑙𝑖 es la energía cinética de cada link i y 𝑇𝑚𝑖 , es la energía cinética de los motores
de las juntas i. Luego la contribución de la energía cinética de los links es:

Figura 16. Relación cinemática del eslabón “i”

1 1
𝑇𝑙𝑖 = 𝑚𝑙𝑖 . 𝑝𝑙̇ 𝑖 𝑇 . 𝑝̇ 𝑙𝑖 + 𝑤𝑖 𝑇 . 𝑅𝑖 . 𝐼 𝑖 𝑙𝑖 . 𝑅𝑖 𝑇 . 𝑤𝑖
2 2
Además:

𝑝𝑙̇ 𝑖 = 𝐽(𝑙𝑖) 𝑣 . 𝑞̇
𝑤̇ 𝑖 = 𝐽(𝑙𝑖) 𝑤 . 𝑞̇

Donde 𝐽(𝑙𝑖) 𝑣 y 𝐽(𝑙𝑖) 𝑤 son los jacobianos del link i.

 Figura 17. Relación cinemática del motor “i”

Luego la energía cinética del rotor i es:

1 1
𝑇𝑚𝑖 = 𝑚𝑚𝑖 . 𝑝𝑚̇ 𝑖 𝑇 . 𝑝𝑚̇ 𝑖 + 𝑤𝑚𝑖 𝑇 . 𝐼𝑚𝑖 . 𝑤𝑚𝑖 …(14)
2 2

Donde 𝑚𝑚𝑖 es la masa del rotor y 𝑝𝑚̇ 𝑖 es la velocidad linear del centro de masa del rotor,
𝐼𝑚𝑖 es el tensor de inercia de el rotor relativo al centro de masa y 𝑤𝑚𝑖 es la velocidad
angular de el rotor. Además:

𝑝̇𝑚𝑖 = 𝐽(𝑚𝑖 ) 𝑣 . 𝑞̇
𝑤̇𝑚𝑖 = 𝐽(𝑚𝑖 ) 𝑤 . 𝑞̇

Donde 𝐽(𝑙𝑖) 𝑣 y 𝐽(𝑙𝑖) 𝑤 son los jacobianos del link i.

4.2.2 Calculo de la Energía Cinética para eslabones:

La energía cinética de cada eslabón se representa de la siguiente manera:

1 𝑇 1 𝑇 …(15)
𝑇𝑙𝑖 = 𝑚𝑙𝑖 𝑞̇ 𝑇 𝐽𝑣 (𝑙𝑖) 𝐽𝑣 (𝑙𝑖) 𝑞̇ + 𝑚𝑙𝑖 𝑞̇ 𝑇 𝐽𝑤 (𝑙𝑖) 𝑅𝑖 𝐼𝑙𝑖 𝑖 𝑅𝑖 𝑇 𝐽𝑤 (𝑙𝑖) 𝑞̇
2 2
4.2.3 Calculo de la Energía Cinética para motores:

Asimismo la energía cinética contribuida por cada motor se representa de la siguiente

manera:

1 𝑇 1 𝑇
𝑇𝑚𝑖 = 𝑚𝑚𝑖 𝑞̇ 𝑇 𝐽𝑣 (𝑚𝑖) 𝐽𝑣 (𝑚𝑖) 𝑞̇ + 𝑚𝑚𝑖 𝑞̇ 𝑇 𝐽𝑤 (𝑚𝑖) 𝑅𝑚𝑖 𝐼𝑚𝑖 𝑚𝑖 𝑅𝑚𝑖 𝑇 𝐽𝑤 (𝑚𝑖) 𝑞̇ …(16)
2 2

Matriz de Rotación para llevar la Inercia de los motores al sistema base

Figura 18. Matriz de Transformación del momento de inercia del motor “mi”

Se debe tener en cuenta de que la matriz de rotación del motor con respecto a tierra es
diferente a la matriz de rotación del link ‘i’ con respecto a tierra, es decir:

0
𝑅𝑚𝑖 ≠ 𝑅𝑙𝑖0

Para encontrar la matriz de rotación adecuada se plantea:

𝑖−1
𝑅𝑚𝑖 = 𝑅𝑜𝑡𝑧,𝜃𝑚𝑖 = 𝑅𝑜𝑡𝑧, 𝑞𝑖
𝑘𝑟𝑖

Para finalmente por una composición respecto al eje actual se obtendrá:

0 0 𝑖−1 0 …(17)
𝑅𝑚𝑖 = 𝑅𝑖−1 . 𝑅𝑚𝑖 = 𝑅𝑖−1 . 𝑅𝑜𝑡𝑧, 𝑞𝑖
𝑘𝑟𝑖
Luego de la aclaración anterior finalmente calculamos con la suma de las contribuciones
de cada link y de cada rotor, la energía cinética total del manipulador está definida con
la siguiente forma cuadrática

𝑛 𝑛
1 1
𝑇 = ∑ ∑ 𝑑𝑖𝑗 (𝑞)𝑞̇ 𝑖 𝑞̇ 𝑗 = 𝑞̇ 𝑇 𝐷(𝑞)𝑞̇
2 2
𝑖=1 𝑗=1

Donde:

𝑛
𝑇 𝑇 𝑇
𝐷(𝑞) = ∑(𝑚𝑙𝑖 𝐽𝑣 (𝑙𝑖) 𝐽𝑣 (𝑙𝑖) + 𝐽𝑤 (𝑙𝑖) 𝑅𝑖 𝐼𝑙𝑖 𝑖 𝑅𝑖 𝑇 𝐽𝑤 (𝑙𝑖) + 𝑚𝑚𝑖 𝐽𝑣 (𝑚𝑖) 𝐽𝑣 (𝑚𝑖)
𝑖=1
…(18)
𝑇
+ 𝐽𝑤 (𝑚𝑖) 𝑅𝑚𝑖 𝐼𝑚𝑖 𝑚𝑖 𝑅𝑚𝑖 𝑇 𝐽𝑤 (𝑚𝑖) )

4.2.4 Calculo de la Energía Potencial

De la misma manera que se vio en la energía cinética, la energía potencial será la suma
de 2 contribuciones, una relativa al link y otra relativa al rotor.

𝑈 = ∑𝑛𝑖=1(𝑈𝑙𝑖 + 𝑈𝑚𝑖 ) …(19)

Asumiendo links rígidos, la contribución debida solo a la fuerza gravitacional es

definida como

𝑈𝑙𝑖 = − ∫ 𝑔𝑜 𝑇 𝑝∗ 𝑖 𝜌𝑑𝑉 = −𝑚𝑙𝑖 𝑔𝑜 𝑇 𝑝𝑙𝑖

𝑣𝑙 𝑖

De la misma manera para la contribución de los rotores

𝑈𝑚𝑖 = −𝑚𝑚𝑖 𝑔𝑜 𝑇 𝑝𝑚𝑖

Entonces la energía potencial total será:

𝑈 = − ∑(𝑚𝑙𝑖 ∗ 𝑔0𝑇 ∗ 𝑝𝑙𝑖 + 𝑚𝑚𝑖 ∗ 𝑔0𝑇 ∗ 𝑝𝑚𝑖 ) …(20)

𝑖=1

Podemos notar que esta es solo una función de las posiciones q, y no de las velocidades
q’.
4.3 Ecuaciones del movimiento

Entonces remplazando nuestros valores en el Lagrangiano:

𝐿(𝑞, 𝑞̇ ) = 𝑇(𝑞, 𝑞̇ ) − 𝑈(𝑞)

Aplicamos el método de Euler - Lagrange y tendremos:

𝑑 𝜕𝐿 𝜕𝐿
( )− = 𝜀𝑘
𝑑𝑡 𝜕𝑞𝑘̇ 𝜕𝑞𝑘
Primer término:

𝑛 𝑛 𝑛 𝑛 𝑛
𝑑 𝑑𝐿 𝑑 𝑑𝑇 𝑑𝐷𝑘𝑗 (𝑞) 𝑑𝐷𝑘𝑗 (𝑞)
( )= ( ) = ∑ 𝐷𝑘𝑗 (𝑞). 𝑞𝑗̈ + ∑ . 𝑞̇ 𝑗 = ∑ 𝐷𝑘𝑗 (𝑞). 𝑞𝑗̈ + ∑ ∑ . 𝑞̇ 𝑖 𝑞̇ 𝑗 …(21)
𝑑𝑡 𝑑𝑞𝑘̇ 𝑑𝑡 𝑑𝑞𝑘̇ 𝑑𝑡 𝑑𝑞𝑖
𝑗=1 𝑗=1 𝑗=1 𝑖=1 𝑗=1

Segundo término:

𝑛 𝑛
𝑑𝑇 1 𝑑𝐷𝑖𝑗 (𝑞)
( ) = ∑∑ . 𝑞̇ 𝑖 𝑞̇ 𝑗
𝑑𝑞𝑖̇ 2 𝑑𝑞𝑘
𝑗=1 𝑘=1

𝑛 𝑛
𝑑𝑈 𝑑𝑝𝑙𝑗 𝑑𝑝𝑚𝑗 (𝑙𝑗 ) (𝑚𝑗 )
( ) = − ∑ (𝑚𝑙𝑗 . 𝑔0 𝑇 + 𝑚𝑚𝑗 . 𝑔0 𝑇 ) = − ∑ (𝑚𝑙𝑗 . 𝑔0 𝑇 𝐽𝑣𝑖 + 𝑚𝑚𝑗 . 𝑔0 𝑇 𝐽𝑤𝑖 ) = 𝑔𝑘 (𝑞)
𝑑𝑞𝑘̇ 𝑑𝑞𝑘 𝑑𝑞𝑘
𝑗=1 𝑗=1

𝑛 𝑛
𝑑𝐿 1 𝑑𝐷𝑖𝑗 (𝑞) …(22)
( ) = ∑∑ . 𝑞̇ 𝑖 𝑞̇ 𝑗 + 𝑔𝑘 (𝑞)
𝑑𝑞𝑘̇ 2 𝑑𝑞𝑘
𝑗=1 𝑘=1

Finalmente remplazando en ():

𝑛 𝑛 𝑛
…(23)
∑ 𝐷𝑘𝑗 (𝑞). 𝑞𝑗̈ + ∑ ∑ ℎ𝑘𝑗𝑖 (𝑞). 𝑞̇ 𝑖 𝑞̇ 𝑗 + 𝑔𝑘 (𝑞) = 𝜀𝑘 𝑘 = 1,2, … . , 𝑛
𝑗=1 𝑖=1 𝑗=1
Donde:
𝑑𝐷𝑘𝑗 (𝑞) 1 𝑑𝐷𝑖𝑗 (𝑞)
ℎ𝑘𝑗𝑖 (𝑞) = −
𝑑𝑞𝑖 2 𝑑𝑞𝑘

El langrangiano puede ser escrito de la siguiente forma despreciando la fricción y

fuerzas de contacto externas.

𝐷(𝑞). 𝑞⃛ + 𝐶(𝑞, 𝑞̇ )𝑞̈ + 𝑔𝑘 (𝑞) = 𝜏 …(24)

Donde 𝐶(𝑞, 𝑞̇ ) se calcula de la siguiente forma:

𝑛

𝐶𝑘𝑗 (𝑞) = ∑ 𝐶𝑖𝑗𝑘 . 𝑞̇ 𝑖 …(25)

𝑖=1

1 𝑑𝐷𝑘𝑗 𝑑𝐷𝑘𝑖 𝑑𝐷𝑖𝑗

𝐶𝑖𝑗𝑘 = [ + − ]
2 𝑑𝑞𝑖 𝑑𝑞𝑗 𝑑𝑞𝑘

4.4 Dinámica de nuestro manipulador de 2GDL.

Tenemos los siguientes parámetros del manipulador de 2GDL.

Figura 19. Vista de frontal de nuestro robot de 2 G.D.L

Figura 20. Vista isométrica de nuestro robot de 2 G.D.L
 Tabla 7. Parámetros del robot de 2.G.D.L

Variable de 2GDL
Descripción Magnitud
L1 Longitud L1 117.5 mm

L2 Longitud L2 66.3mm

L3 Longitud L3 80 mm

Kr1 Relación de transmisión del motor 1 74.83:1

Kr2 Relación de transmisión del motor 2 29.86:1

Lcm1 Longitud del CM1 54 mm.

Lcm1z Longitud del CM1z 3.31 mm.

Lcm2 Longitud del CM2 36.39mm

m1 Masa del link-1 18.61 gr.

m2 Masa del link-2 6.85 gr.

Mm1 Masa motor 1 104 gr

Mm2 Masa motor 2 9.5 gr

IL1X INERCIA Ixx link1 978 gr.mm2

IL1Y INERCIA Iyy link1 39758.65 gr.mm2

IL1Z INERCIA Izz link1 400458.53 gr.mm2

ILXZ INERCIA Ixz link1 -132.9 gr.mm2

IL2X INERCIA Ixx link2 147.2 gr.mm2

IL2Y INERCIA Iyy link2 4805.86 gr.mm2

IL2Z INERCIA Izz link2 4934.79 gr.mm2

De los cálculos de la sección anterior sobre jacobianos:

Para el link 1:

−𝐿𝑐1 ∗ Sθ1 0
𝐽𝑣𝑐1 = [ 𝐿𝑐1 ∗ Cθ1 0]
0 0

0 0
𝐽𝑤𝑐1 = [0 0]
1 0

Para el link 2:

−𝐿1 ∗ Sθ1 − 𝐿𝑐2 ∗ Sθ12 −𝐿𝑐2 ∗ Sθ12

𝐽𝑣𝑐2 = [ 𝐿1 ∗ Cθ1 + 𝐿𝑐2 ∗ Cθ12 𝐿𝑐2 ∗ Cθ12 ]
0 0
0 0
𝐽𝑤𝑐2 = [0 0]
1 1

 Para los motores

Para el motor 1:

0 0
𝐽𝑣𝑚1 = [0 0]
0 0
0 0
𝐽𝑤𝑚1 = [ 0 0]
𝑘𝑟1 0

Para el motor 2:
−𝐿1 ∗ Sθ1 0
𝐽𝑣𝑚2 = [ 𝐿1 ∗ Cθ1 0]
0 0
0 0
𝐽𝑤𝑚2 = [0 0 ]
1 𝑘𝑟2
Luego calcularemos la matriz de inercias, las cuales serán calculadas en dos partes:

 Componente Traslacional

 Para los links

Para el link 1:
𝐷1𝑣 = 𝑚1 ∗ 𝐽𝑣𝑐1 𝑇 ∗ 𝐽𝑣𝑐1
2
𝐷1𝑣 = [𝐿𝑐1 ∗ 𝑚1 0]
0 0

Para el link 2:
𝐷2𝑣 = 𝑚2 ∗ 𝐽𝑣𝑐2 𝑇 ∗ 𝐽𝑣𝑐2

(𝐿 2 + 2 ∗ Cθ2 ∗ 𝐿1 ∗ 𝐿𝑐2 + 𝐿𝑐2 2 ) ∗ 𝑚2 𝐿𝑐2 ∗ 𝑚2 ∗ (𝐿𝑐2 + 𝐿1 ∗ Cθ2 )

𝐷2𝑣 = [ 1 ]
𝐿𝑐2 ∗ 𝑚2 ∗ (𝐿𝑐2 + 𝐿1 ∗ Cθ2 ) 𝐿𝑐2 2 ∗ 𝑚2

 Para los motores

Para el motor 1:
𝐷𝑚1𝑣 = 𝑚𝑚1 ∗ 𝐽𝑣𝑚1 𝑇 ∗ 𝐽𝑣𝑚1

0 0
𝐷𝑚1𝑣 = [ ]
0 0

Para el motor 2:
𝐷𝑚2𝑣 = 𝑚𝑚2 ∗ 𝐽𝑣𝑚2 𝑇 ∗ 𝐽𝑣𝑚2
2
𝐷𝑚2𝑣 = [𝐿1 ∗ 𝑚𝑚2 0]
0 0

 Componente Rotacional.

 Para los links

Para el link 1:
𝐼𝑙1𝑥 0 𝐼𝑙𝑥𝑧
𝐼1 = [ 0 𝐼𝑙1𝑦 0 ]
𝐼𝑙𝑥𝑧 0 𝐼𝑙1𝑧

𝐼𝐼1 = 𝑅1 ∗ 𝐼1 ∗ 𝐼1 𝑇

𝐼𝑙1𝑥 ∗ Cθ1 2 + 𝐼𝑙1𝑦 ∗ Sθ1 2 S2θ1 ∗ (𝐼𝑙1𝑥 − 𝐼𝑙1𝑦 )/2 𝐼𝑙𝑥𝑧 ∗ Cθ1
𝐼𝐼1 = [ S2θ1 ∗ (𝐼𝑙1𝑥 − 𝐼𝑙1𝑦 )/2 2 2
𝐼𝑙1𝑦 ∗ Cθ1 + 𝐼𝑙1𝑥 ∗ Sθ1 𝐼𝑙𝑥𝑧 ∗ Sθ1 ]
𝐼𝑙𝑥𝑧 ∗ Cθ1 𝐼𝑙𝑥𝑧 ∗ Sθ1 𝐼𝑙1𝑧

𝐷1𝑤 = 𝐽𝑤1 𝑇 ∗ 𝐼𝐼1 ∗ 𝐽𝑤1

𝐼 0
𝐷1𝑤 = [ 𝑙1𝑧 ]
0 0
Para el link 2:
𝐼𝑙2𝑥 0 0
𝐼2 = [ 0 𝐼𝑙2𝑦 0 ]
0 0 𝐼𝑙2𝑧

𝐼𝐼2 = 𝑅2 ∗ 𝐼2 ∗ 𝐼2 𝑇

𝐼𝑙2𝑥 ∗ Cθ12 2 + 𝐼𝑙2𝑦 ∗ Sθ12 2 Cθ12 ∗ Sθ12 ∗ (𝐼𝑙2𝑥 − 𝐼𝑙2𝑦 ) 0

𝐼𝐼2 = [ Cθ12 ∗ Sθ12 ∗ (𝐼𝑙2𝑥 − 𝐼𝑙2𝑦 ) 2 2
𝐼𝑙2𝑦 ∗ Cθ12 + 𝐼𝑙2𝑥 ∗ Sθ12 0 ]
0 0 𝐼𝑙2𝑧

𝐷2𝑤 = 𝐽𝑤2 𝑇 ∗ 𝐼𝐼2 ∗ 𝐽𝑤2

𝐼𝑙2𝑧 𝐼𝑙2𝑧
𝐷2𝑤 = [ ]
𝐼𝑙2𝑧 𝐼𝑙2𝑧

 Para los motores

Para el motor 1:
𝐼𝑚1𝑥𝑥 0 0
𝐼𝑚1 = [ 0 𝐼𝑚1𝑦𝑦 0 ]
0 0 𝐼𝑚1𝑧𝑧

𝐼𝐼𝑚1 = 𝑅1 ∗ 𝐼𝑚1 ∗ 𝐼𝑚1 𝑇

𝐼𝑚1𝑥𝑥 ∗ Cθ1 2 + 𝐼𝑚1𝑦𝑦 ∗ Sθ1 2 S2θ1 ∗ (𝐼𝑚1𝑥𝑥 − 𝐼𝑚1𝑦𝑦 )/2 0

𝐼𝐼𝑚1 = [ S2θ1 ∗ (𝐼𝑚1𝑥𝑥 − 𝐼𝑚1𝑦𝑦 )/2 2 2
𝐼𝑚1𝑦𝑦 ∗ Cθ1 + 𝐼𝑚1𝑥𝑥 ∗ Sθ1 0 ]
0 0 𝐼𝑚1𝑧𝑧

𝐷𝑚1𝑤 = 𝐽𝑤𝑚1 𝑇 ∗ 𝐼𝐼𝑚1 ∗ 𝐽𝑤𝑚1

2
𝐷1𝑤 = [𝐼𝑚1𝑧𝑧 ∗ 𝑘𝑟1 0]
0 0

Para el motor 2:
𝐼𝑚2𝑥𝑥 0 0
𝐼𝑚2 = [ 0 𝐼𝑚2𝑦𝑦 0 ]
0 0 𝐼𝑚2𝑧𝑧

𝐼𝐼𝑚2 = 𝑅2 ∗ 𝐼𝑚2 ∗ 𝐼𝑚2 𝑇

𝐼𝑚2𝑥𝑥 ∗ C(θ1 + 𝑘𝑟2 ∗ θ2 )2 + 𝐼𝑚2𝑦𝑦 ∗ S(θ1 + 𝑘𝑟2 ∗ θ2 )2 S(θ1 + 𝑘𝑟2 ∗ θ2 ) ∗ (𝐼𝑚2𝑥𝑥 − 𝐼𝑚2𝑦𝑦 )/2 0
𝐼𝐼𝑚2 =[ S(θ1 + 𝑘𝑟2 ∗ θ2 ) ∗ (𝐼𝑚2𝑥𝑥 − 𝐼𝑚2𝑦𝑦 )/2 𝐼𝑚2𝑦𝑦 ∗ C(θ1 + 𝑘𝑟2 ∗ θ2 )2 + 𝐼𝑚2𝑥𝑥 ∗ S(θ1 + 𝑘𝑟2 ∗ θ2 )2 0 ]
0 0 𝐼𝑚1𝑧𝑧

𝐷𝑚2𝑤 = 𝐽𝑤𝑚2 𝑇 ∗ 𝐼𝐼𝑚2 ∗ 𝐽𝑤𝑚2

𝐼𝑚2𝑧𝑧 𝐼𝑚2𝑧𝑧 ∗ 𝑘𝑟2

𝐷𝑚2𝑤 = [ ]
𝐼𝑚2𝑧𝑧 ∗ 𝑘𝑟2 𝐼𝑚2𝑧𝑧 ∗ 𝑘𝑟22
Luego en base a estos hallaremos la matriz de inercias totales, que es la suma
de los valores calculados anteriormente.

𝐷 = 𝐷1𝑣 + 𝐷2𝑣 + 𝐷𝑚1𝑣 + 𝐷𝑚2𝑣 + 𝐷1𝑤 + 𝐷2𝑤 + 𝐷𝑚1𝑤 + 𝐷𝑚2𝑤

𝐷11 = 𝐿𝑐1 2 ∗ 𝑚1 + (𝐿1 2 + 2 ∗ Cθ2 ∗ 𝐿1 ∗ 𝐿𝑐2 + 𝐿𝑐2 2 ) ∗ 𝑚2 + 𝐿1 2 ∗ 𝑚𝑚2 + 𝐼𝑙1𝑧 + 𝐼𝑙2𝑧 + 𝐼𝑚1𝑧𝑧 ∗ 𝑘𝑟12 + 𝐼𝑚2𝑧𝑧

𝐷12 = 𝐷21 = 𝐿𝑐2 ∗ 𝑚2 ∗ (𝐿𝑐2 + 𝐿1 ∗ Cθ2 ) + 𝐼𝑙2𝑧 + 𝐼𝑚2𝑧𝑧 ∗ 𝑘𝑟2

𝐷22 = 𝐿𝑐2 2 ∗ 𝑚2 + 𝐼𝑙2𝑧 + 𝐼𝑚2𝑧𝑧 ∗ 𝑘𝑟22

Para calcular la matriz C, tenemos que usar la siguiente formula:

𝑛

𝑐𝑖𝑗 = ∑ 𝑐𝑖𝑗𝑘 ∗ 𝑞𝑘̇

𝑘=1

Donde:
1 𝜕𝐷𝑖𝑗 𝜕𝐷𝑖𝑘 𝜕𝐷𝑗𝑘
𝑐𝑖𝑗𝑘 = ( + − )
2 𝜕𝑞𝑘 𝜕𝑞𝑗 𝜕𝑞𝑖
Entonces:

−𝐿1 ∗ 𝐿𝑐𝑚2 ∗ 𝑚2 ∗ 𝑆θ2 ∗ 𝑞2̇ −𝐿1 ∗ 𝐿𝑐𝑚2 ∗ 𝑚2 ∗ 𝑆θ2 ∗ (𝑞1̇ + 𝑞2̇ )

𝐶=[ ]
−𝐿1 ∗ 𝐿𝑐𝑚2 ∗ 𝑚2 ∗ 𝑆θ2 ∗ (𝑞1̇ + 𝑞2̇ ) 0

Por último, calcularemos el efecto de la energía potencial en la dinámica del

manipulador:

𝑔1 = 𝑔 ∗ 𝑚2 ∗ (𝐿𝑐𝑚2 ∗ 𝐶θ12 + 𝐿1 ∗ 𝐶θ1 ) + 𝑔 ∗ 𝑚𝑚2 ∗ 𝐿1 ∗ 𝐶θ1 + 𝐿𝑐𝑚1 ∗ 𝑔 ∗ 𝑚1 ∗ 𝐶θ1

𝑔2 = 𝐿𝑐𝑚2 ∗ 𝑔 ∗ 𝑚2 ∗ 𝐶θ12

Luego:
𝑔 ∗ 𝑚2 ∗ (𝐿𝑐𝑚2 ∗ 𝐶θ12 + 𝐿1 ∗ 𝐶θ1 ) + 𝑔 ∗ 𝑚𝑚2 ∗ 𝐿1 ∗ 𝐶θ1 + 𝐿𝑐𝑚1 ∗ 𝑔 ∗ 𝑚1 ∗ 𝐶θ1
𝑔=[ ]
𝐿𝑐𝑚2 ∗ 𝑔 ∗ 𝑚2 ∗ 𝐶θ12

Observaciones:
 Se observó en el cálculo de la dinámica que esta no depende de IL1X , IL1y
IL2X, IL2y, Im1xx, Im2xx, Im1yy, Im2yy.
5. CODIGOS EN MATLAB

5.1 CINEMATICA DIRECTA

%CINEMATICA DEL ROBOR ROBOTICA 2

clc;clear all;close all;
%------------------------%
%DEFINICION DE VARIABLES SIMBOLICAS%
%-------------------------%
syms theta1 d1 a1 alpha1
syms theta2 d2 a2 alpha2
syms q1 q2
syms L1 L2 L3
%-----------------------%
%2.DEFINICION DE SISTEMAS COORDENADOS%
%2.1 Sistema Coordenado 1
theta1=q1;
d1=L2;
a1=L1;
alpha1=0;
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
%2.2 Sistema coordenado 2
theta2=q2;
d2=0;
alpha2=0;
a2=L3;
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
%---------------------%
%CINEMATICA%
%--------------------%
T01=A1;
T02=simplify(A1*A2);
%MATRIZ DE TRANSFORMACION T01
disp('matriz T1')
pretty(T01)
%MATRIZ DE TRANSFORMACION T01
disp('matriz T2')
pretty(T02)
Figura 21. Vista frontal del manipulador
5.2 SIMULACION CINEMATICA DIRECTA (EJES ROTADOS)

%% CON EJES ROTADOS

%SIMULACION CINEMÁTICA DIRECTA
clc;clear all;close all
%------------------------%
%DEFINICION DE PARAMETROS
%-------------------------%
l1=1;
l2=1;
l3=1;%PONER VALORES
LCM1x=0.5;
LCM2x=0.5;
LCM1z=0.5;
%TG=matriz_homogenea_DH(theta1,d1,a1,alpha1);
TG=[-1 0 0 0;0 0 1 0;0 1 0 0;0 0 0 1];
%DEFINICION DEL SISTEMA INERCIAL
%1.1 MATRIZ HOMOGENEA
T0=TG';
%1.2 PLOTEO DEL SISTEMA
figure('name','Cinematica Directa','NumberTitle','off')
title('SIMULACIÓN CINEMÁTICA DIRECTA')
plot_frame(T0,'color','k','view','auto')
axis([-4 4 -4 4 0 3]);
hold on
grid on
xlabel('x(m)')
ylabel('y(m)')
%-----------------------%
%2.DEFINICION DE SISTEMAS COORDENADOS%
%2.1 Sistema Coordenado
theta1=0;
d1=l2;
alpha1=0;
a1=l1;
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
T01=A1;
T01=TG'*T01;
h1=plot_frame(T01,'frame','1','color','b');

%2.2 Sistema coordenado 2

theta2=0;
d2=0;
alpha2=0;
a2=l3;
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
T02=A1*A2;
T02=TG'*T02;
h2=plot_frame(T02,'frame','2','color','g');

disp(' Presione una tecla para esperar la simulacion!!!')

pause
%------------------------
%%LAZO DE SIMULACION
%------------------------
MOVE_JUNTA1 = 1;
MOVE_JUNTA2 = 1;
STEPS = 15;

for i=1:STEPS
if(MOVE_JUNTA1)
theta1 = theta1 + pi/100;
end
if(MOVE_JUNTA2)
theta2 = theta2 + pi/100;
end
A1 = matriz_homogenea_DH(theta1,d1,a1,alpha1);
A2 = matriz_homogenea_DH(theta2,d2,a2,alpha2);

T01 = A1;
T01=TG'*T01;
T02 = A1*A2;
T02=TG'*T02;
plot_frame(h1, T01);
plot_frame(h2, T02);
pause(0.1)
end

Figura 22. Simulación cinemática directa

5.3. JACOBIANOS

% Tema: Dinamica de un manipulador planar de 2GDL

clc; clear all; close all
%---------------------------------------------------------------------
-%
% 1. DEFINICION DE VARIABLES SIMBOLICAS
%---------------------------------------------------------------------
-%
syms theta1 d1 a1 alpha1
syms theta2 d2 a2 alpha2
syms q1 q2
syms L1 L2 L3
syms m1 m2
syms Lcm1 Lcm2 lcm1z
syms Lcmot1 Lcmot2 %centros de maza de los motores con respecto a su sistema
coordenado
syms kr1 kr2 %relacion de transmicion

%----------------------------------------------------------%
% 2. SISTEMAS COORDENADOS Y CINEMATICA
%---------------------------------------------------------------------
-%
% 2.1. SISTEMA COORDENADO 1
% -> Parametros D-H
theta1 = q1;
d1 = L2;
a1 = L1;
alpha1 = 0;
% -> Matriz A
A1 = matriz_homogenea_DH(theta1, d1,a1,alpha1);
% -> MATRIZ T
T01 = A1;

% 2.2. SISTEMA COORDENADO 2

% -> Parametros D-H
theta2 = q2;
d2 = 0;
a2 = L3;
alpha2 = 0;
% -> Matriz A
A2 = matriz_homogenea_DH(theta2, d2,a2,alpha2);
% -> MATRIZ T
T02 = T01*A2;
T02 = simplify(T02);

%---------------------------------------------------------------------
-%
% 3. CALCULO JACOBIANOS
%---------------------------------------------------------------------
-%
% 3.1. PARAMETROS PARA HALLAR LOS JACOBIANOS
% -> Origenes de coordenadas
p0 = [0;0;0];
p1 = T01(1:3,4);
% -> Vectores unitarios
z0 = [0;0;1];
z1 = T01(1:3,3);
%% EXPLICAR CADA UNO EL CALCULO SIMPLE Y EL CALCULO PROGRAMADO
% 3.2. JACOBIANO PARA ESLABON 1
% -> Centro de masa del eslabon 1
rc1 = [Lcm1*cos(theta1); Lcm1*sin(theta1); L2-lcm1z];
% -> Jv para el centro de masa
Jvc1 = [ cross(z0, (rc1-p0)) zeros(3,1) ];
% -> Jw para el cuerpo
Jw1 = [ z0 zeros(3,1) ];

% 3.3. JACOBIANO PARA ESLABON 2

% -> Centro de masa
rc2 = [L1*cos(theta1) + Lcm2*cos(theta1+theta2);
L1*sin(theta1) + Lcm2*sin(theta1+theta2);
L2];
% -> Jv para el centro de masa
Jvc2 = [ cross(z0, (rc2-p0)) cross(z1, (rc2-p1)) ];
% -> Jw para el cuerpo
Jw2 = [ z0 z1 ];

%JACOBIANO DE LOS MOTORES

%jacobiano del motor 1

pm1=[0;Lcmot1;0];
Jvm1=[zeros(3,1) zeros(3,1)];
Jwm1=[kr1*z0 zeros(3,1)];
%jacobiano del motor 2
pm2=T01*[0;0;-Lcmot2;1]; %demostrar graficamente
pm2=pm2(1:3);
Jvm2=[cross(z0,(pm2-p0)) zeros(3,1)];
Jwm2=[z0 kr2*z1];

%-------------------------------------------
%IMPRESION DE RESULTADOS
%-------------------------------------
disp('JACOBIANOS DEL LOS LINKS')
disp('%%%%%%%%%%%%%%%%%%%%%%%%%%')
disp('LINK 1:')
disp(' ')
disp('Jvcm1')
disp(Jvc1)
disp('Jwcm1')
disp(Jw1)
disp('LINK 2:')
disp(' ')
disp('Jvcm2')
disp(Jvc2)
disp('Jwcm2')
disp(Jw2)

disp('JACOBIANOS DEL LOS MOTORES')

disp('%%%%%%%%%%%%%%%%%%%%%%%%%%')
disp('MOTOR 1:')
disp(' ')
disp('JvM1')
disp(Jvm1)
disp('JwM1')
disp(Jwm1)
disp('MOTOR 2:')
disp(' ')
disp('JvM2')
disp(Jvm2)
disp('JwM2')
disp(Jwm2)

RESULTADOS:
5.4. DINAMICA

% Tema: Dinamica de un manipulador planar de 2GDL

clc; clear all; close all
%----------------------------------------------------------------------%
% 1. DEFINICION DE VARIABLES SIMBOLICAS
%----------------------------------------------------------------------%
syms theta1 d1 a1 alpha1
syms theta2 d2 a2 alpha2
syms q1 q2
syms dq1 dq2
syms L1 L2 L3
syms m1 m2
syms Lcm1 Lcm2 lcm1z
syms Lcmot1 Lcmot2 %centros de maza de los motores con respecto a su sistema
coordenado
%----------------------------------------------------------
%INERCIAS DE LOS LINK--> SE CONSIDERA TODOS LOS VALORES DIFERENTES DE CERO
%SEGUN NUESTRO DISEÑO EN SOLIDWORKS

syms IL1X IL1Y IL1Z ILXZ %INERCIA DEL LINK 1

syms IL2X IL2Y IL2Z %INERCIA DEL LINK 2

%--------------------------------------------------------
syms Im1zz Im2zz % Inercia de los rotores a lo largo del eje de giro Z
I=1/2*m*r^2
syms g
syms kr1 kr2 %relacion de transmicion
syms Mm1 Mm2 % masas de los rotores
%INERCIA DE LOS MOTORES
syms Im1xx Im2xx Im1yy Im2yy % Imxx=Imyy=1/12*m(3*r^2+Lm^2)
q = [q1; q2];
dq = [dq1; dq2];
n = 2;

%----------------------------------------------------------------------%
% 2. SISTEMAS COORDENADOS Y CINEMATICA
%----------------------------------------------------------------------%
% 2.1. SISTEMA COORDENADO 1
% -> Parametros D-H
theta1 = q1;
d1 = L2;
a1 = L1;
alpha1 = 0;
% -> Matriz A
A1 = matriz_homogenea_DH(theta1, d1,a1,alpha1);
% -> MATRIZ T
T01 = A1;

% 2.2. SISTEMA COORDENADO 2

% -> Parametros D-H
theta2 = q2;
d2 = 0;
a2 = L3;
alpha2 = 0;
% -> Matriz A
A2 = matriz_homogenea_DH(theta2, d2,a2,alpha2);
% -> MATRIZ T
T02 = T01*A2;
T02 = simplify(T02);

%----------------------------------------------------------------------%
% 3. CALCULO JACOBIANOS
%----------------------------------------------------------------------%
% 3.1. PARAMETROS PARA HALLAR LOS JACOBIANOS
% -> Origenes de coordenadas
p0 = [0;0;0];
p1 = T01(1:3,4);
% -> Vectores unitarios
z0 = [0;0;1];
z1 = T01(1:3,3);
%% EXPLICAR CADA UNO EL CALCULO SIMPLE Y EL CALCULO PROGRAMADO

% 3.2. JACOBIANO PARA ESLABON 1

% -> Centro de masa del eslabon 1
rc1 = [Lcm1*cos(theta1); Lcm1*sin(theta1); L2-lcm1z];
% -> Jv para el centro de masa
Jvc1 = [ cross(z0, (rc1-p0)) zeros(3,1) ];
% -> Jw para el cuerpo
Jw1 = [ z0 zeros(3,1) ];

% 3.3. JACOBIANO PARA ESLABON 2

% -> Centro de masa
rc2 = [L1*cos(theta1) + Lcm2*cos(theta1+theta2);
L1*sin(theta1) + Lcm2*sin(theta1+theta2);
L2];
% -> Jv para el centro de masa
Jvc2 = [ cross(z0, (rc2-p0)) cross(z1, (rc2-p1)) ];
% -> Jw para el cuerpo
Jw2 = [ z0 z1 ];

%JACOBIANO DE LOS MOTORES

%jacobiano del motor 1

pm1=[0;Lcmot1;0];
Jvm1=[zeros(3,1) zeros(3,1)];
Jwm1=[kr1*z0 zeros(3,1)];
%jacobiano del motor 2
pm2=T01*[0;0;-Lcmot2;1]; %demostrar graficamente
pm2=pm2(1:3);
Jvm2=[cross(z0,(pm2-p0)) zeros(3,1)];
Jwm2=[z0 kr2*z1];

%----------------------------------------------------------------------%
% 4. CALCULO DE LA MATRIZ DE INERCIA - COMPONENTE TRANSLACIONAL
%----------------------------------------------------------------------%
% 4.1. ESLABON "1"
D1v = m1*transpose(Jvc1)*Jvc1;
D1v = simplify(D1v);
% 4.2. ESLABON "2"
D2v = m2*transpose(Jvc2)*Jvc2;
D2v = simplify(D2v);

% 4.3. MOTOR "1"

Dm1v = Mm1*transpose(Jvm1)*Jvm1;
Dm1v = simplify(Dm1v);
% 4.4. MOTOR2 "2"
Dm2v = Mm2*transpose(Jvm2)*Jvm2;
Dm2v = simplify(Dm2v);

%----------------------------------------------------------------------%
% 5. CALCULO DE LA MATRIZ DE INERCIA - COMPONENTE ROTACIONAL
%----------------------------------------------------------------------%
% 5.1. ESLABON "1"
% -> Tensor de inercia en el centro de masa
I1 = [IL1X 0 ILXZ;0 IL1Y 0; ILXZ 0 IL1Z];
% -> Tensor de inercia en el sistema inercial
R1 = T01(1:3, 1:3);
II1 = R1*I1*transpose(R1);
II1 = simplify(II1);
% -> Componente "D"
D1w = transpose(Jw1)*II1*Jw1;
D1w = simplify(D1w);
% 5.2. ESLABON "2"
% -> Tensor de inercia en el centro de masa
I2 = [IL2X 0 0;0 IL2Y 0; 0 0 IL2Z];
% -> Tensor de inercia en el sistema inercial
R2 = T02(1:3, 1:3);
II2 = R2*I2*transpose(R2);
% -> Componente "D"
D2w = transpose(Jw2)*II2*Jw2;
D2w = simplify(D2w);

%MODELANDO LOS MOTORES COMO CILINDROS->PRODUCTOS DE INERCIA SON 0

% 5.3. MOTOR1 "1"
% -> Tensor de inercia en el centro de masa
Im1 = [Im1xx 0 0;0 Im1yy 0; 0 0 Im1zz];
% -> Tensor de inercia en el sistema inercial
R1 = T01(1:3, 1:3);
IIm1 = R1*Im1*transpose(R1);
IIm1 = simplify(IIm1);
% -> Componente "D"
Dm1w = transpose(Jwm1)*IIm1*Jwm1;
Dm1w = simplify(Dm1w);

% 5.3. MOTOR1 "2"

% -> Tensor de inercia en el centro de masa
Im2 = [Im2xx 0 0;0 Im2yy 0; 0 0 Im2zz];
% -> Tensor de inercia en el sistema inercial
R2 = T01(1:3, 1:3)*rotz(theta2*kr2);
IIm2 = R2*Im2*transpose(R2);
IIm2 = simplify(IIm2);
% -> Componente "D"
Dm2w = transpose(Jwm2)*IIm2*Jwm2;
Dm2w = simplify(Dm2w);

%----------------------------------------------------------------------%
% 6. CALCULO DE LA MATRIZ "D"
%----------------------------------------------------------------------%
D = D1v + D2v + D1w + D2w + Dm1v + Dm2v + Dm1w +Dm2w;
D = simplify(D);
disp('**********')
disp(' MATRIZ D ')
disp('**********')
for i=1:n
for j=1:n
fprintf('D%d%d: \n', i, j)
pretty( D(i,j))
end
end

%----------------------------------------------------------------------%
% 7. CALCULO DE LA MATRIZ "C"
%----------------------------------------------------------------------%
% 7.1. METODO AUTOMATICO
cijk = cell(n,n,n);
for i=1:n % Fila de C
for j=1:n % Columna de C
for k=1:n % Indice de la sumatoria
 term_01 = diff(D(k,j), q(i));
term_02 = diff(D(k,i), q(j));
term_03 = diff(D(i,j), q(k));

cijk{i,j,k} = 1/2*( term_01 + term_02 - term_03);

end
end
end

% 7.2. MATRIZ "C"

disp('**********')
disp(' MATRIZ C ')
disp('**********')
C = cell(n,n);
for k=1:n
for j=1:n
% Calculo de "ckj"
sum = 0;
for i=1:n
sum = sum + cijk{i,j,k}*dq(i);
end
C{k,j} = simplify(sum);
% Mensajes
fprintf('C%d%d:\n', k, j)
pretty( sum)
end
end

%----------------------------------------------------------------------%
% 8. CALCULO DE LA MATRIZ "g"
%----------------------------------------------------------------------%
% 8.1.CALCULO DE LA ENERGIA POTENCIAL
P1L = m1*g*Lcm1*sin(q1);
P2L = m2*g*(L1*sin(q1) + Lcm2*sin(q1 + q2));
P1M=-Mm1*[0;-g;0].'*pm1;
P2M=-Mm2*[0;-g;0].'*pm2;%OJO .'
P = P1L + P2L+ P1M+ P2M;

% 8.2. HALLAMOS "gi"

disp('**********')
disp(' MATRIZ g ')
disp('**********')
for i=1:n
a = diff(P, q(i));
a = simplify(a);
% Mensajes
fprintf('g%d: \n', i)
pretty( a)
end
%%
Matriz “D”

Matriz “C”

Matriz “g”
 ANEXOS

Motor de corriente continua

Figura 23. Diagrama simplificado de un motor CC

Relaciones eléctricas y mecánicas

𝑑𝑖
𝑉 = 𝐼. 𝑅 + 𝐿 + 𝐸𝑔 …(26)
𝑑𝑡

𝐸𝑔 = 𝑘𝑏 𝑤 …(27)

𝜏𝑚 = 𝐽𝛼 + 𝑏𝑤 + 𝜏𝑠 …(28)

𝜏𝑚 = 𝑘𝑡 𝐼 …(29)

Y además por conservación de la energía a la entrada del eje del motor

𝑃𝑜𝑡 = 𝐸𝑔 𝐼 = 𝜏𝑚 𝑤

𝑘𝑡 = 𝑘𝑏 = 𝑘 …(30)
CALCULO DE LA RESISTENCIA DE ARMADURA PARA CADA MOTOR
Prueba de rotor bloqueado
Motor 1:

Tabla 8. Voltaje vs Corriente motor 1

V(voltios)
de 2GDLI(mA) R(ohms)
0.073 7.95 9.182
0.132 14.45 9.135
0.165 18.06 9.136
0.185 20.17 9.172
0.215 23.63 9.099
0.277 30.45 9.097
0.294 32.3 9.102
0.329 36.1 9.114
0.406 44.5 9.124
0.465 51 9.118
0.506 55.4 9.134
0.558 61.2 9.118
0.581 63.7 9.121
0.593 65 9.123
0.621 67.9 9.146
0.748 81.7 9.155
Resistencia Prom. 9.130

VOLTAJE VS CORRIENTE MOTOR 1

90
y = 109.52x
80 R² = 1
70
corriente (mA)

60
50
40
30
20
10
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
Voltaje(V)

Figura 24. Voltaje vs Corriente motor 1

de 2GDL
Motor 2:

Tabla 9. Voltaje vs Corriente motor 2

de 2GDL I(mA)
V(voltios) R(ohms)
0 0 -
0.106 6.19 17.1244
0.156 9.1 17.1429
0.212 12.38 17.1244
0.285 16.67 17.0966
0.295 17.26 17.0915
0.309 18.09 17.0813
0.352 20.54 17.1373
0.405 23.67 17.1103
0.44 25.74 17.0940
0.5 29.19 17.1292
0.351 20.46 17.1554
0.563 32.9 17.1125
0.618 36 17.1667
Resistencia prom. 17.1205

VOLTAJE VS CORRIENTE MOTOR 2

40
y = 58.393x
35
R² = 1
30
Corriente(mA)

25
20
15
10
5
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7
Voltaje(V)

Figura 25. Voltaje vs Corriente motor 2

de 2GDL
CALCULO DE LA CONSTANTE DE VELOCIDAD PARA CADA MOTOR
MOTOR 1

Tabla 10. Cálculo de la constante de velocidad para el motor 1

de 2GDL MOTOR
VOLTAJE(v) CORRIENTE(mA) Eg(V) w(rad/s) w_salida(rad/s) Kb
(RPM)
1.057 81 0.317 1920 201.06 2.687 0.00158
1.352 95.5 0.480 2880 301.58 4.030 0.00159
1.508 102.9 0.569 3489 365.36 4.882 0.00156
1.751 112.3 0.726 4410 461.80 6.171 0.00157
2.088 122.7 0.968 5760 603.17 8.061 0.00160
2.385 133.8 1.163 6960 728.83 9.740 0.00160
2.726 145.7 1.396 8415 881.19 11.776 0.00158
3.035 155 1.620 9765 1022.56 13.665 0.00158
3.397 164.8 1.892 11325 1185.92 15.848 0.00160
3.767 173.7 2.181 13020 1363.41 18.220 0.00160
4.117 182.2 2.454 14494 1517.76 20.283 0.00162
4.42 182 2.758 16395 1716.83 22.943 0.00161
4.919 191 3.175 18990 1988.57 26.574 0.00160
5.182 192.5 3.424 20525 2149.31 28.723 0.00159
5.517 189 3.791 23325 2442.52 32.641 0.00155
5.851 192.2 4.096 25075 2625.77 35.090 0.00156
6.08 190.2 4.343 26440 2768.71 37.000 0.00157
Kb=0.0016
MOTOR 2

Tabla 11. Cálculo de la constante de velocidad para el motor 2

VOLTAJE CORRIENTE
de 2GDL MOTOR(RPM) Eg w_motor(rad/s) w_salida(rad/s) kb
0.682 22.85 3210 0.2908 168.065 5.628 0.00173
1.004 24.125 6120 0.5910 320.423 10.731 0.00184
1.286 25.5 8880 0.8494 464.927 15.570 0.00183
1.464 26.25 10395 1.0146 544.248 18.227 0.00186
1.826 28.15 13950 1.3441 730.376 24.460 0.00184
2.008 28.6 15660 1.5184 819.905 27.458 0.00185
2.37 30.2 19680 1.8530 1030.379 34.507 0.00180
2.501 30.5 20910 1.9788 1094.778 36.664 0.00181
2.88 32.2 24900 2.3287 1303.681 43.660 0.00179
3.134 33.2 27510 2.5656 1440.332 48.236 0.00178
3.439 34 30675 2.8569 1606.041 53.786 0.00178
4.019 35.5 36330 3.4112 1902.118 63.701 0.00179
4.386 36.6 40050 3.7594 2096.885 70.224 0.00179
4.747 36.5 43860 4.1221 2296.363 76.904 0.00180
5.132 37.4 47685 4.4917 2496.628 83.611 0.00180
5.663 39.1 53445 4.9936 2798.202 93.711 0.00178
6 40 57090 5.3152 2989.042 100.102 0.00178
0.00180