Beruflich Dokumente
Kultur Dokumente
FACULTAD DE INGENIERÍA
INGENIERÍA MECATRÓNICA
ROBÓTICA 1
ROBOT PUMA
Integrantes:
CINEMÁTICA DIRECTA
𝒊 𝒂𝒊 − 𝟏 а𝒊 − 𝟏 𝒅𝒊 𝜽𝒊
1 0 0 0 𝜃1
2 -90 0 0 𝜃2
3 0 а2 𝑑3 𝜃3
4 -90 а3 𝑑4 𝜃4
5 90 0 0 𝜃5
6 -90 0 0 𝜃6
Cos[θ1] −Sin[θ1] 0 0
0 Sin[θ1] Cos[θ1] 0 0
1𝑇 = [ ],
0 0 1 0
0 0 0 1
Cos[θ2] −Sin[θ2] 0 0
1 0 0 1 0
2𝑇 =[ ],
−Sin[θ2] −Cos[θ2] 0 0
0 0 0 1
Cos[θ3] −Sin[θ3] 0 a2
2 Sin[θ3] Cos[θ3] 0 0
3𝑇 = [ ],
0 0 1 d3
0 0 0 1
Cos[θ4] −Sin[θ4] 0 a3
3 0 0 1 d4
4𝑇 = [ ],
−Sin[θ4] −Cos[θ4] 0 0
0 0 0 1
Cos[θ5] −Sin[θ5] 0 0
4 0 0 −1 0
5𝑇 =[ ],
Sin[θ5] Cos[θ5] 0 0
0 0 0 1
Cos[θ6] −Sin[θ6] 0 0
5 0 0 1 0
6𝑇 = [ ].
−Sin[θ6] −Cos[θ6] 0 0
0 0 0 1
Se obtiene:
Dónde:
𝑟11 = Sin[θ1](Cos[θ5]Cos[θ6]Sin[θ4] + Cos[θ4]Sin[θ6]) + Cos[θ1](−Cos[θ6]Sin[θ2
+ θ3]Sin[θ5] + Cos[θ2 + θ3](Cos[θ4]Cos[θ5]Cos[θ6] − Sin[θ4]Sin[θ6]))
CINEMÁTICA INVERSA
Se invierte 01𝑇:
𝐶1 𝑆 0 0 𝑟11 𝑟12 𝑟13 𝑝𝑥
−𝑆1 𝐶1 0 0] [𝑟21 𝑟22 𝑟23 𝑝𝑦
[ ] = 16𝑇 (3)
0 0 1 0 𝑟31 𝑟32 𝑟33 𝑝𝑧
0 0 0 1 0 0 0 1
𝜙 = 𝐴𝑡𝑎𝑛2(𝑝𝑦 , 𝑝𝑥 )
𝑎3 𝐶3 − 𝑑4 𝑆3 = 𝐾 (11)
Donde
𝑝𝑥2 + 𝑝𝑦2 + 𝑝𝑧2 − 𝑎22 − 𝑎32 − 𝑑32 − 𝑑42
𝐾= (12)
2𝑎2
Ahora que la dependencia de 𝜃1 de la ecuación (11) ha sido removida se puede
resolver aplicando los mismos criterios que se usaron para resolver (4) ya que
poseen una forma similar y por tanto se obtiene
Por lo tanto tenemos también dos posibles soluciones para 𝜃1 . Si ahora volvemos
a tomar en cuenta a (1) podemos escribir el lado izquierdo en función de solo lo
que conocemos y 𝜃2 .
[ 03𝑇(𝜃2 )]−1 06𝑇 = 34𝑇(𝜃4 ) 45𝑇(𝜃5 ) 56𝑇(𝜃6 ) (14)
o como:
𝐶1 𝐶23 𝑆1 𝐶23 −𝑆23 −𝑎2 𝐶3 𝑟11 𝑟12 𝑟13 𝑝𝑥
−𝐶 𝑆 −𝑆1 𝑆23 −𝐶23 𝑎2 𝑆3 𝑟23 𝑝𝑦
[ 1 23 ] [𝑟21 𝑟22 ] = 36𝑇 (15)
−𝑆1 𝐶1 0 −𝑑3 𝑟31 𝑟32 𝑟33 𝑝𝑧
0 0 0 1 0 0 0 1
Obteniendo los valores resultan cuatro posibles valores de 𝜃23 debido a las
combinaciones de 𝜃1 y 𝜃3 , por tanto Se obtienen
𝜃2 = 𝜃23 − 𝜃3 (20)
Y 46𝑇 mediante la ecuación. Igualando los elementos (1,3) y (3,3) de ambos lados
de la ecuación (21) obtenemos
𝑟13 (𝑐1 𝑐23 𝑐4 + 𝑠1 𝑠4 ) + 𝑟23 (𝑠1 𝑐23 𝑐4 − 𝑐1 𝑠4 ) − 𝑟33 (𝑠23 𝑐4 ) = − 𝑠5
𝑟13 (−𝑐1 𝑠23 ) + 𝑟23 (−𝑠1 𝑠23 ) + 𝑟33 (−𝑐23 ) = 𝑐5 (22)
𝜃5 = 𝐴𝑡𝑎𝑛2(𝑠5 , 𝑐5 )
En donde
𝑐6 = 𝑟11 [(𝑐1 𝑐23 𝑐4 + 𝑠1 𝑠4 )𝑐5 − 𝑐1 𝑠23 𝑠5 ] + 𝑟21 [(𝑠1 𝑐23 𝑐4 − 𝑐1 𝑠4 )𝑐5 − 𝑠1 𝑠23
− 𝑟31 (𝑠23 𝑐4 𝑐5 + 𝑐23 𝑠5 ).
Debido a los signos positivo y negativo que aparecen en las ecuaciones (9) y (13),
estas cuatro soluciones. Además, hay cuatro soluciones adicionales que se
obtienen volteando la muñeca del manipulador. Para cada una de las cuatro
soluciones calculadas antes, obtenemos la solución inversa mediante.
𝜃′4 = 𝜃4 + 180°,
𝜃′5 = −𝜃5
𝜃′6 = 𝜃6 + 180°.
Una vez que se han calculado las ocho soluciones, habrá que descartar algunas
de ellas (o incluso todas) debido a violaciones en los límites de las articulaciones.
De cualquier solución válida restante, generalmente se selecciona la más cercana
a la configuración actual del manipulador.
DINÁMICA DEL ROBOT.
La dinámica del robot trata con las formulaciones matemáticas de las ecuaciones
de movimiento del brazo. Las ecuaciones de movimiento de un manipulador son
un conjunto de ecuaciones matemáticas que describen su conducta dinámica.
Tales ecuaciones son útiles para la simulación en computadora del movimiento del
robot, el diseño de ecuaciones de control apropiadas para el robot y la evaluación
del diseño y estructura del brazo.
En el presente documento se proporciona la dinámica del robot PUMA 560,
considerando su velocidad lineal y angular y las fuerzas que actúan sobre él,
resulta ser que el estudio de ambas velocidades y fuerzas estáticas nos lleva a
una matriz llamada jacobiano del manipulador, la cual se utilizará para determinar
la dinámica del robot mencionado.
En primera instancia se determinaron las posiciones de los eslabones del
manipulador con respecto al centro de masa del mismo. Y se dividió en cada una
de sus posiciones en X, Y y Z para su fácil manipulación.
Para determinar las posiciones de los eslabones que se encargan de posicionar el
efector final se utilizó el análisis geométrico considerando el ángulo 𝛳1 y las
distancias que se muestran en la Figura 4.
POSICION.
La posición del eslabón 1 es:
lc1 ∗ Cos[q1[𝑡]]
𝑃𝑐𝑔1 = [ lc1 ∗ Sin[q1[𝑡]] ]
0
Se agrega el siguiente eslabón de la estructura del robot para determinar la
posición del eslabón 2 como se muestra en la Figura 2.
hxx1 0 0
𝐼1 = [ 0 hyy1 0 ]
0 0 hzz1
hxx2 0 0
𝐼2 = [ 0 hyy2 0 ]
0 0 hzz2
hxx3 0 0
𝐼3 = [ 0 hyy3 0 ]
0 0 hzz3
hxx4 0 0
𝐼4 = [ 0 hyy4 0 ]
0 0 hzz4
hxx5 0 0
𝐼5 = [ 0 hyy5 0 ]
0 0 hzz5
hxx6 0 0
𝐼6 = [ 0 hyy6 0 ]
0 0 hzz6
𝑙𝑐22 𝐶𝑜𝑠[𝑞1[𝑡]]𝐶𝑜𝑠[𝑞2[𝑡]]𝑆𝑖𝑛[𝑞2[𝑡]]
𝐽𝑣212 = −
√𝑙12 + 𝑙𝑐22 𝐶𝑜𝑠[𝑞2[𝑡]]2
𝑙𝑐22 𝐶𝑜𝑠[𝑞2[𝑡]]𝑆𝑖𝑛[𝑞1[𝑡]]𝑆𝑖𝑛[𝑞2[𝑡]]
𝐽𝑣222 = −
√𝑙12 + 𝑙𝑐22 𝐶𝑜𝑠[𝑞2[𝑡]]2
𝐽𝑣232 = 𝑙𝑐2𝐶𝑜𝑠[𝑞2[𝑡]]
2
𝐽𝑉311 = −√l12 + (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]]) Sin[q1[𝑡]]
l2Cos[q1[𝑡]](l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])Sin[q2[𝑡]]
𝐽𝑉312 = −
√l12 + (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2
lc3Cos[q1[𝑡]](l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])Sin[q3[𝑡]]
𝐽𝑉313 = −
√l12 + (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2
l2(l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])Sin[q1[𝑡]]Sin[q2[𝑡]]
JV322 = −
√l12 + (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2
lc3(l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])Sin[q1[𝑡]]Sin[q3[𝑡]]
JV323 = −
√l12 + (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2
2
Jv421 = Cos[q1[𝑡]]√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])
𝐽𝑣512 =
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2 Cos[q1[𝑡] +
q5[𝑡]]
𝐽𝑣513 = 0
𝐽𝑣521
l2Cos[q1[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])Sin[q2[𝑡]]
= −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2
𝐽𝑣522
l2(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])Sin[q2[𝑡]]Sin[q1[𝑡] + q5[𝑡]]
=−
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2
𝐽𝑣523 = l2Cos[q2[𝑡]] + (l3 + lc4)Cos[q2[𝑡] + q3[𝑡]]
𝐽𝑣531
l3Cos[q1[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])Sin[q3[𝑡]]
= −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2
𝐽𝑣5532
l3(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])Sin[q3[𝑡]]Sin[q1[𝑡] + q5[𝑡]]
=−
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2
𝐽𝑣533 = (l3 + lc4)Cos[q2[𝑡] + q3[𝑡]]
𝐽𝑣541
l4Cos[q1[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])Sin[q4[𝑡]]
= −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2
𝐽𝑣542
l4(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])Sin[q4[𝑡]]Sin[q1[𝑡] + q5[𝑡]]
= −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2
𝐽𝑣543 = 0
𝐽𝑣551
lc5Cos[q1[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])Sin[q5[𝑡]]
= −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2
𝐽𝑣552
2
(l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]]) )Cos[q1[𝑡]]
=
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2
+
q5[𝑡]]−lc5(l2Cos[q2[𝑡]]+l3Cos[q3[𝑡]]+l4Cos[q4[𝑡]]+lc5Cos[q5[𝑡]])Sin[q5[𝑡]]Sin[q1[𝑡]+q5[𝑡]]
√l12 +(l2Cos[q2[𝑡]]+l3Cos[q3[𝑡]]+l4Cos[q4[𝑡]]+lc5Cos[q5[𝑡]])2
𝐽𝑣553 = 0
𝐽𝑣561 = 0
𝐽𝑣562 = 0
𝐽𝑣563 = 0
𝐽𝑣612
𝑙2(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝐶𝑜𝑠[𝑞1[𝑡] + 𝑞6[𝑡]]𝑆𝑖𝑛[𝑞2[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣613
𝑙3(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝐶𝑜𝑠[𝑞1[𝑡] + 𝑞6[𝑡]]𝑆𝑖𝑛[𝑞3[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣614
𝑙4(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝐶𝑜𝑠[𝑞1[𝑡] + 𝑞6[𝑡]]𝑆𝑖𝑛[𝑞4[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣615
𝑙5(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝐶𝑜𝑠[𝑞1[𝑡] + 𝑞6[𝑡]]𝑆𝑖𝑛[𝑞5[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣616
−𝑙𝑐6(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝐶𝑜𝑠[𝑞1[𝑡] + 𝑞6[𝑡]]𝑆𝑖𝑛[𝑞6[𝑡]] − (𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2 )𝑆𝑖𝑛[𝑞1[𝑡] + 𝑞6[𝑡]]
=
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣621
= 𝐶𝑜𝑠[𝑞1[𝑡]]√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣622
𝑙2(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝑆𝑖𝑛[𝑞1[𝑡]]𝑆𝑖𝑛[𝑞2[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣623
𝑙3(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝑆𝑖𝑛[𝑞1[𝑡]]𝑆𝑖𝑛[𝑞3[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣624
𝑙4(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝑆𝑖𝑛[𝑞1[𝑡]]𝑆𝑖𝑛[𝑞4[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣625
𝑙5(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝑆𝑖𝑛[𝑞1[𝑡]]𝑆𝑖𝑛[𝑞5[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
𝐽𝑣626
𝑙𝑐6(𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])𝑆𝑖𝑛[𝑞1[𝑡]]𝑆𝑖𝑛[𝑞6[𝑡]]
=−
√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2
1 1 1 0 0 0
Jw3 = 1 1 1 0 0 0
0 1 1 0 0 0
1 1 1 1 0 0
Jw4 = 1 1 1 1 0 0
0 1 1 1 0 0
1 1 1 1 1 0
Jw5 = 1 1 1 1 1 0
0 1 1 0 0 0
1 1 1 1 1 1
Jw6 = 1 1 1 1 1 1
0 1 1 1 0 1
Por lo que tenemos todo lo necesario para poder calcular la matriz de masas e
Inercias de cada eslabón y cuya suma da como resultado la Matriz de masas e
Inercias de todo el Robot.
M211 M212 0 0 0 0
M221 M222 0 0 0 0
0 0 0 0 0 0
M2 =
0 0 0 0 0 0
0 0 0 0 0 0
[ 0 0 0 0 0 0]
lc22 m2 1 2
M211 = hxx2 + hyy2 + l12 m2 + + lc2 m2Cos[2q2[𝑡]]
2 2
M221 = hxx2 + hyy2
M212 = hxx2 + hyy2
2
M411 = hxx4 + hyy4 + m4 (l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]]) )
2
M422 = hxx4 + hyy4 + hzz4 + m4((l2Cos[q2[𝑡]] + l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]])
l22 Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q2[𝑡]]2
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l22 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q2[𝑡]]2
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
𝜕𝑞𝑀
𝑑𝑞3 =
𝜕𝑞3(𝑡)
𝜕𝑞𝑀
𝑑𝑞4 =
𝜕𝑞5(𝑡)
𝜕𝑞𝑀
𝑑𝑞5 =
𝜕𝑞5(𝑡)
𝜕𝑞𝑀
𝑑𝑞6 =
𝜕𝑞6(𝑡)
Finalmente se aplica la siguiente fórmula para poder obtener el vector de fuerzas
centrípetas y de coriolis.
dq1
𝑑𝑞2
1 𝑑𝑞3
𝑉 = dM − ∗
2 𝑑𝑞4
𝑑𝑞5
[ ( (𝑑𝑞6))]
Por tanto, lo último que queda por calcular es el vector de Gravedad
0
𝑔𝑣 = ( 0 )
𝑔
Donde se aplica la siguiente formula
𝐴 = m1 ∗ Jv1 + m2 ∗ Jv2 + m3 ∗ Jv3 + m4 ∗ Jv4 + m5 ∗ Jv5 + m6 ∗ Jv6
𝐺𝑖𝑛 = −𝑔𝑣 𝑇 ∗ 𝐴
Y se obtiene al final el vector de gravedad.
𝐺 = 𝐺𝑖𝑛𝑇
BIBIOGRAFÍA
Craig, John J. (2006). Robótica.Pearson Educación. México.
Spong W. Mark., Hutchinson S., & Vidyasagar, M. (2004). Robot Dynamics and
Control.
Tsai, Lung-Wen. (1999). Robot analysis: the mechanics of serial and parallel
manipulators. Wiley.
http://personal.us.es/jcortes/Material/Material_archivos/Articulos%20PDF/RobotPU
MA.pdf Visitado 17 Noviembre de 2014.