Beruflich Dokumente
Kultur Dokumente
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
de 2GDLӨi
JUNTA di ai αi
1 Ө1 L2 L1 0
2 Ө2 0 L3 0
𝐴𝑖−1
𝑖 (𝑞𝑖 ) = 𝑅𝑜𝑡𝑧,𝜃𝑖 . 𝑇𝑟𝑎𝑛𝑠𝑧,𝑑𝑖 . 𝑇𝑟𝑎𝑛𝑠𝑥,𝑎𝑖 . 𝑅𝑜𝑡𝑥,𝑎𝑖
𝑥 2 + 𝑦 2 − 𝐿1 2 − 𝐿3 2
𝑐𝑜𝑠𝜃2 = =𝐷
2𝐿1 . 𝐿3
𝜃2 = cos−1 𝐷
sin 𝜃2 = ±√1 − 𝐷2
sin 𝜃2 ±√1 − 𝐷2
tan 𝜃2 = =
𝑐𝑜𝑠𝜃2 𝐷
±√1 − 𝐷2
𝜃2 = tan−1 𝐸𝑛 𝑀𝑎𝑡𝑙𝑎𝑏: 𝜃2 = 𝑎𝑡𝑎𝑛2( ±√1 − 𝐷2 , 𝐷)
𝐷
Para el cálculo del ángulo 𝜃1 se realizan las proyecciones del link 2 sobre el eje
paralelo al link 1
𝑦 𝐿3 . sin 𝜃2
𝜃1 = tan−1 ( ) − tan−1 ( )
𝑥 𝐿1 + 𝐿3 . cos 𝜃2
JUNTA Өi
de 2GDL di ai αi
1 Ө1 L2 L1 0
2 Ө2 0 L3 0
de L1
2GDL 117.5 mm
L2 66.3mm
L3 80 mm
de 2GDL
A partir de la cinemática, calculamos las coordenadas del centro de masa de los links-
1 y 2:
−(𝐿1 − 𝐿𝑐𝑚1 )
1
𝑟𝑐𝑚 =[ 0 ]
1
−𝐿𝑐𝑚1𝑧
1
−(𝐿3 − 𝐿𝑐𝑚2 )
2
𝑟𝑐𝑚 =[ 0 ]
2 𝐿2
1
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
0
0
𝑟𝐶.𝑚𝑜𝑡𝑜𝑟1
=[ 0 ]
𝐿𝑚𝑜𝑡1
𝐿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
de 2GDL
Lcm1z 3.31 mm
Lcm1 54 mm
Masa link1 18.61 gr.
Link2
de 2GDL
Lcm2 36.39 mm
LINK 2
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.
𝐿 =𝑇−𝑈
𝑑 𝜕𝐿 𝜕𝐿
− 𝜕𝑞 = 𝜀𝑖 para 𝑖 = 1,2 … , 𝑛
𝑑𝑡 𝜕𝑞𝑖̇ 𝑖
𝑑 𝜕𝐿 𝑇 𝜕𝐿 𝑇 …(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:
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:
1 1
𝑇𝑙𝑖 = 𝑚𝑙𝑖 . 𝑝𝑙̇ 𝑖 𝑇 . 𝑝̇ 𝑙𝑖 + 𝑤𝑖 𝑇 . 𝑅𝑖 . 𝐼 𝑖 𝑙𝑖 . 𝑅𝑖 𝑇 . 𝑤𝑖
2 2
Además:
𝑝𝑙̇ 𝑖 = 𝐽(𝑙𝑖) 𝑣 . 𝑞̇
𝑤̇ 𝑖 = 𝐽(𝑙𝑖) 𝑤 . 𝑞̇
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:
𝑝̇𝑚𝑖 = 𝐽(𝑚𝑖 ) 𝑣 . 𝑞̇
𝑤̇𝑚𝑖 = 𝐽(𝑚𝑖 ) 𝑤 . 𝑞̇
1 𝑇 1 𝑇 …(15)
𝑇𝑙𝑖 = 𝑚𝑙𝑖 𝑞̇ 𝑇 𝐽𝑣 (𝑙𝑖) 𝐽𝑣 (𝑙𝑖) 𝑞̇ + 𝑚𝑙𝑖 𝑞̇ 𝑇 𝐽𝑤 (𝑙𝑖) 𝑅𝑖 𝐼𝑙𝑖 𝑖 𝑅𝑖 𝑇 𝐽𝑤 (𝑙𝑖) 𝑞̇
2 2
4.2.3 Calculo de la Energía Cinética para motores:
1 𝑇 1 𝑇
𝑇𝑚𝑖 = 𝑚𝑚𝑖 𝑞̇ 𝑇 𝐽𝑣 (𝑚𝑖) 𝐽𝑣 (𝑚𝑖) 𝑞̇ + 𝑚𝑚𝑖 𝑞̇ 𝑇 𝐽𝑤 (𝑚𝑖) 𝑅𝑚𝑖 𝐼𝑚𝑖 𝑚𝑖 𝑅𝑚𝑖 𝑇 𝐽𝑤 (𝑚𝑖) 𝑞̇ …(16)
2 2
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
𝑖−1
𝑅𝑚𝑖 = 𝑅𝑜𝑡𝑧,𝜃𝑚𝑖 = 𝑅𝑜𝑡𝑧, 𝑞𝑖
𝑘𝑟𝑖
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)
𝑇
+ 𝐽𝑤 (𝑚𝑖) 𝑅𝑚𝑖 𝐼𝑚𝑖 𝑚𝑖 𝑅𝑚𝑖 𝑇 𝐽𝑤 (𝑚𝑖) )
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.
Podemos notar que esta es solo una función de las posiciones q, y no de las velocidades
q’.
4.3 Ecuaciones del movimiento
𝑑 𝜕𝐿 𝜕𝐿
( )− = 𝜀𝑘
𝑑𝑡 𝜕𝑞𝑘̇ 𝜕𝑞𝑘
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
𝑛 𝑛 𝑛
…(23)
∑ 𝐷𝑘𝑗 (𝑞). 𝑞𝑗̈ + ∑ ∑ ℎ𝑘𝑗𝑖 (𝑞). 𝑞̇ 𝑖 𝑞̇ 𝑗 + 𝑔𝑘 (𝑞) = 𝜀𝑘 𝑘 = 1,2, … . , 𝑛
𝑗=1 𝑖=1 𝑗=1
Donde:
𝑑𝐷𝑘𝑗 (𝑞) 1 𝑑𝐷𝑖𝑗 (𝑞)
ℎ𝑘𝑗𝑖 (𝑞) = −
𝑑𝑞𝑖 2 𝑑𝑞𝑘
Variable de 2GDL
Descripción Magnitud
L1 Longitud L1 117.5 mm
L2 Longitud L2 66.3mm
L3 Longitud L3 80 mm
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:
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 el link 1:
𝐷1𝑣 = 𝑚1 ∗ 𝐽𝑣𝑐1 𝑇 ∗ 𝐽𝑣𝑐1
2
𝐷1𝑣 = [𝐿𝑐1 ∗ 𝑚1 0]
0 0
Para el link 2:
𝐷2𝑣 = 𝑚2 ∗ 𝐽𝑣𝑐2 𝑇 ∗ 𝐽𝑣𝑐2
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 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𝑧
𝐼 0
𝐷1𝑤 = [ 𝑙1𝑧 ]
0 0
Para el link 2:
𝐼𝑙2𝑥 0 0
𝐼2 = [ 0 𝐼𝑙2𝑦 0 ]
0 0 𝐼𝑙2𝑧
𝐼𝐼2 = 𝑅2 ∗ 𝐼2 ∗ 𝐼2 𝑇
𝐼𝑙2𝑧 𝐼𝑙2𝑧
𝐷2𝑤 = [ ]
𝐼𝑙2𝑧 𝐼𝑙2𝑧
Para el motor 1:
𝐼𝑚1𝑥𝑥 0 0
𝐼𝑚1 = [ 0 𝐼𝑚1𝑦𝑦 0 ]
0 0 𝐼𝑚1𝑧𝑧
Para el motor 2:
𝐼𝑚2𝑥𝑥 0 0
𝐼𝑚2 = [ 0 𝐼𝑚2𝑦𝑦 0 ]
0 0 𝐼𝑚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𝑧𝑧
𝐷11 = 𝐿𝑐1 2 ∗ 𝑚1 + (𝐿1 2 + 2 ∗ Cθ2 ∗ 𝐿1 ∗ 𝐿𝑐2 + 𝐿𝑐2 2 ) ∗ 𝑚2 + 𝐿1 2 ∗ 𝑚𝑚2 + 𝐼𝑙1𝑧 + 𝐼𝑙2𝑧 + 𝐼𝑚1𝑧𝑧 ∗ 𝑘𝑟12 + 𝐼𝑚2𝑧𝑧
Donde:
1 𝜕𝐷𝑖𝑗 𝜕𝐷𝑖𝑘 𝜕𝐷𝑗𝑘
𝑐𝑖𝑗𝑘 = ( + − )
2 𝜕𝑞𝑘 𝜕𝑞𝑗 𝜕𝑞𝑖
Entonces:
𝑔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
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
%----------------------------------------------------------%
% 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;
%---------------------------------------------------------------------
-%
% 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) ];
%-------------------------------------------
%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)
RESULTADOS:
5.4. DINAMICA
%--------------------------------------------------------
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;
%----------------------------------------------------------------------%
% 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
%----------------------------------------------------------------------%
% 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);
%----------------------------------------------------------------------%
% 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);
%----------------------------------------------------------------------%
% 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));
%----------------------------------------------------------------------%
% 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;
Matriz “C”
Matriz “g”
ANEXOS
𝐸𝑔 = 𝑘𝑏 𝑤 …(27)
𝜏𝑚 = 𝐽𝛼 + 𝑏𝑤 + 𝜏𝑠 …(28)
𝜏𝑚 = 𝑘𝑡 𝐼 …(29)
𝑘𝑡 = 𝑘𝑏 = 𝑘 …(30)
CALCULO DE LA RESISTENCIA DE ARMADURA PARA CADA MOTOR
Prueba de rotor bloqueado
Motor 1:
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)
de 2GDL
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
25
20
15
10
5
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7
Voltaje(V)
de 2GDL
CALCULO DE LA CONSTANTE DE VELOCIDAD PARA CADA MOTOR
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