UNIVERSIDAD AUTÓNOMA DE YUCATÁN

FACULTAD DE INGENIERÍA

INGENIERÍA MECATRÓNICA

ROBÓTICA 1

ROBOT PUMA

Integrantes:

CÉSAR ALBERTO SOSA ZÚNIGA

EDUARDO ERIBERT ESCOBAR AQUINO
ANDRÉ JOSÉ NOVELO CELMO
CRISTOPHER CORTÉS SANCHEZ
ANGEL LLANEZ CABALLERO
JONATAN DEOLARTE MARTINEZ

MÉRIDA, YUCATÁN, MÉXICO

2014
Introducción
El robot PUMA (Programmable Universal Machine for Asambly) es un brazo
articulado con 6 articulaciones rotatorias que le proporcionan 6 grados de libertad
y le permiten posicionar y orientar su herramienta final. Ver figura 1.

Figura 1. Robot PUMA 560.

De manera más específica, las 3 primeras articulaciones (sistema Hombro-codo-
Muñeca) se utilizan para posicionar el efector final, mientras que las últimas tres
articulaciones sirven para orientarlo.
La cinemática directa del brazo articulado se formuló siguiendo la representación
de Denavit-Hartenberg, cuya descripción comprende la asignación de sistemas de
referencia y relación de parámetros asociados a elementos y articulaciones.
La cinemática inversa se realizó algebraicamente partiendo de las ecuaciones
proporcionadas de la cinemática directa.

CINEMÁTICA DIRECTA

En la figura 2 se muestra las asignaciones de tramas. La figura 3 muestra los

detalles del antebrazo del robot.
La trama {0} (que no se muestra) es coincidente con la trama {1} cuando 𝜃1 es
cero. Los ejes de las articulaciones 4, 5, y 6 se intersectan todos en un punto
común, y este punto de intersección coincide con el origen de las tramas {4}, {5} y
{6}, los ejes de articulación de 4, 5 y 6 son mutuamente ortogonales.
Los parámetros de Denavit-Hartenberg que corresponden a esta disposición de
tramas de vínculos se muestran en la tabla 1. El robot tiene una combinación de
engranajes en la muñeca del manipulador que acopla los movimientos de las
articulaciones 4, 5 y 6.

Figura 2. Asignaciones de tramas para el manipulador.

Figura 3. Asignaciones de tramas para el antebrazo del manipulador.

Tabla 1. Parámetros de Denavit-Hartenberg del robot PUMA.

𝒊 𝒂𝒊 − 𝟏 а𝒊 − 𝟏 𝒅𝒊 𝜽𝒊
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

Dada la ecuación general de transformación homogénea, para eslabones

arbitrarios:

Cos[𝜃𝑖 ] −Sin[𝜃𝑖 ] 0 𝑎𝑖−1

𝑖−1 Sin[𝜃𝑖 ]Cos[𝛼𝑖−1 ] Cos[𝜃𝑖 ]Cos[𝛼𝑖−1 ] −Sin[𝛼𝑖−1 ] −Sin[𝛼𝑖−1 ]𝑑𝑖
𝑖𝑇 = [ ]
Sin[𝜃𝑖 ]Sin[𝛼𝑖−1 ] Cos[𝜃𝑖 ]Sin[𝛼𝑖−1 ] Cos[𝛼𝑖−1 ] Cos[𝛼𝑖−1 ]𝑑𝑖
0 0 0 1

Sustituyendo los valores de la tabla de parámetros de vínculos del PUMA en la

ecuación general obtenemos cada una de las transformaciones del vínculo:

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

Multiplicando las transformadas de las tramas {0} a {6}

0
6𝑇 = 01𝑇 12𝑇 23𝑇 34𝑇 45𝑇 56𝑇

Se obtiene:

𝑟11 𝑟12 𝑟13 𝑝𝑥

0 𝑟21 𝑟22 𝑟23 𝑝𝑦
6𝑇 = [ 𝑟 𝑟32 𝑟33 𝑝𝑧
]
31
0 0 0 1

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

𝑟21 = −Cos[θ6]Sin[θ1]Sin[θ2 + θ3]Sin[θ5]

− Cos[θ1](Cos[θ5]Cos[θ6]Sin[θ4] + Cos[θ4]Sin[θ6])
+ Cos[θ2 + θ3]Sin[θ1](Cos[θ4]Cos[θ5]Cos[θ6] − Sin[θ4]Sin[θ6])

𝑟31 = −Cos[θ6](Cos[θ4]Cos[θ5]Sin[θ2 + θ3] + Cos[θ2𝑧 + θ3]Sin[θ5]) + Sin[θ2

+ θ3]Sin[θ4]Sin[θ6]𝑧

𝑟12 = Sin[θ1](Cos[θ4]Cos[θ6] − Cos[θ5]Sin[θ4]Sin[θ6]) + Cos[θ1](Sin[θ2

+ θ3]Sin[θ5]Sin[θ6] − Cos[θ2 + θ3](Cos[θ6]Sin[θ4]
+ Cos[θ4]Cos[θ5]Sin[θ6]))

𝑟22 = Cos[θ1](−Cos[θ4]Cos[θ6] + Cos[θ5]Sin[θ4]Sin[θ6]) + Sin[θ1](Sin[θ2

+ θ3]Sin[θ5]Sin[θ6] − Cos[θ2 + θ3](Cos[θ6]Sin[θ4]
+ Cos[θ4]Cos[θ5]Sin[θ6]))
𝑟32 = Cos[θ2 + θ3]Sin[θ5]Sin[θ6] + Sin[θ2 + θ3](Cos[θ6]Sin[θ4] + Cos[θ4]Cos[θ5]Sin[θ6])
𝑟13 = −Sin[θ1]Sin[θ4]Sin[θ5] − Cos[θ1](Cos[θ5]Sin[θ2 + θ3] + Cos[θ2 + θ3]Cos[θ4]Sin[θ5])
𝑟23 = Cos[θ1]Sin[θ4]Sin[θ5] − Sin[θ1](Cos[θ5]Sin[θ2 + θ3] + Cos[θ2 + θ3]Cos[θ4]Sin[θ5])
𝑟33 = −Cos[θ2 + θ3]Cos[θ5] + Cos[θ4]Sin[θ2 + θ3]Sin[θ5]
𝑝𝑥 = −d3Sin[θ1] + Cos[θ1](a2Cos[θ2] + a3Cos[θ2 + θ3] − d4Sin[θ2 + θ3])
𝑝𝑦 = d3Cos[θ1] + Sin[θ1](a2Cos[θ2] + a3Cos[θ2 + θ3] − d4Sin[θ2 + θ3])

𝑝𝑧 = −d4Cos[θ2 + θ3] − a2Sin[θ2] − a3Sin[θ2 + θ3]

CINEMÁTICA INVERSA

En base a las 6 transformaciones de los vínculos:

𝑟11 𝑟12 𝑟13 𝑝𝑥

0 𝑟21 𝑟22 𝑟23 𝑝𝑦
6𝑇 = [𝑟 𝑟32 𝑟33 𝑝𝑧
] (1)
31
0 0 0 1

Se pone la dependencia en 𝜃1 del lado izquierdo de la ecuación:

[ 01𝑇(𝜃1 )]−1 06𝑇 = 12𝑇(𝜃2 ) 23𝑇(𝜃3 ) 34𝑇(𝜃4 ) 45𝑇(𝜃5 ) 56𝑇(𝜃6 ) (2)

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

De las ecuaciones de posición se utiliza una simple técnica en la que se multiplica

cada lado de la ecuación de transformación por una inversa que es regularmente
usada para separar las variables de salida en la búsqueda de una ecuación con
solución.
Tomando los elementos (2,4) de los dos lados de la ecuación obtenemos:
−𝑆1 𝑝𝑥 + 𝐶1 𝑝𝑦 = 𝑑3 (4)
Para resolver estas ecuaciones necesitamos algunas relaciones trigonométricas.
𝑝𝑥 = 𝜌 ∗ 𝐶𝑜𝑠𝜙, (5)
𝑝𝑦 = 𝜌 ∗ 𝑆𝑒𝑛𝜙

𝜌 = √𝑝𝑥2 + 𝑝𝑦2 (6)

𝜙 = 𝐴𝑡𝑎𝑛2(𝑝𝑦 , 𝑝𝑥 )

Sustituyendo (5) en (4)

𝑑3
𝐶1 𝑆𝜙 −𝑆1 𝐶𝜙 = (7)
𝜌
Manipulando trigonométricamente:
𝑑3
𝑆𝑖𝑛(𝜙 − 𝜃1 ) = (8)
𝜌

𝜃1 = 𝐴𝑡𝑎𝑛2(𝑝𝑦 , 𝑝𝑥 ) − 𝐴𝑡𝑎𝑛2 (𝑑3 , ±√𝑝𝑥2 + 𝑝𝑦2 − 𝑑32 ) (9)

Ahora conocemos dos posibles soluciones de 𝜃1 y esto nos da la oportunidad de

poder tomar los elementos (1,4) y (3,4) de la ecuación (3) e igualamos ambos
lados.
𝐶1 𝑝𝑥 + 𝑆1 𝑝𝑦 = 𝑎3 𝐶23 − 𝑑4 𝑆23 + 𝑎2 𝐶2 , − 𝑝𝑧 = 𝑎3 𝑆23 + 𝑑4 𝐶23 + 𝑎2 𝑆2 (10)

Si elevamos al cuadrado (10) y (4) y sumamos las ecuaciones resultantes

podemos obtener

𝑎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

𝜃3 = 𝐴𝑡𝑎𝑛2(𝑎3 , 𝑑4 ) − 𝐴𝑡𝑎𝑛2 (𝐾, ±√𝑎32 + 𝑑42 − 𝐾 2 ) (13)

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

De donde la ecuación (15) se tiene ya de la cinemática directa. Ahora tomamos los

elementos (1,4) y (2,4) y los igualamos con los elementos correspondientes en
(15) y obtenemos:

𝐶1 𝐶23 𝑝𝑥 + 𝑆1 𝐶23 𝑝𝑦 − 𝑆23 𝑝𝑧 −𝑎2 𝐶3 = 𝑎3 (16)

−𝐶1 𝑆23 𝑝𝑥 − 𝑆1 𝑆23 𝑝𝑦 − 𝐶23 𝑝𝑧 +𝑎2 𝑆3 = 𝑑4

Estas ecuaciones pueden ser simultáneamente resueltas para 𝐶23 y 𝑆23 .

(−𝑎3 −𝑎2 𝐶3 ) 𝑝𝑧 + (𝐶1 𝑝𝑥 + 𝑆1 𝑝𝑦 ) (𝑎2 𝑆3 − 𝑑4 )

𝑆23 = (17)
𝑝𝑧2 + (𝐶1 𝑝𝑥 + 𝑆1 𝑝𝑦 ) 2
(𝑎2 𝑆3 − 𝑑4 )𝑝𝑧 + (𝑎3 +𝑎2 𝐶3 ) (𝐶1 𝑝𝑥 + 𝑆1 𝑝𝑦 )
𝐶23 = (18)
𝑝𝑧2 + (𝐶1 𝑝𝑥 + 𝑆1 𝑝𝑦 ) 2
Los denominadores son iguales y positivos, por tanto resolvemos para la suma de
𝜃2 𝑦 𝜃3 .

𝜃23 = 𝐴𝑡𝑎𝑛2[(−𝑎3 −𝑎2 𝐶3 )𝑝𝑧 − (𝐶1 𝑝𝑥 + 𝑆1 𝑝𝑦 )(𝑑4 − 𝑎2 𝑆3 ),

(𝑎2 𝑆3 − 𝑑4 )𝑝𝑧 − (𝑎3 +𝑎2 𝐶3 )( 𝐶1 𝑝𝑥 + 𝑆1 𝑝𝑦 )] (19)

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)

Siempre y cuando 𝑠5 ≠ 0, podremos resolver para 𝜃4 de la siguiente manera:

𝜃4 = 𝐴𝑡𝑎𝑛2(−𝑟13 𝑠1 + 𝑟23 𝑐1 , − 𝑟13 𝑐1 𝑐23 − 𝑟23 𝑠1 𝑐23 + 𝑟33 𝑠23 )

𝜃4 Se elige arbitrariamente y, cuando se calcule 𝜃6 posteriormente, se calculará

acorde con ello.
Se puede escribir la ecuación (3) de modo que todo el lado izquierdo sea una
función solamente de variables conocidas y 𝜃4

[ 04𝑇(𝜃4 )]−1 06𝑇 = 45𝑇(𝜃5 ) 56𝑇(𝜃6 ) (21)

En donde [ 04𝑇(𝜃4 )]−1 se da mediante

𝑐1 𝑐23 𝑐4 + 𝑠1 𝑠4 𝑠1 𝑐23 𝑐4 − 𝑐1 𝑠4 −𝑠23 𝑐4 −𝑎2 𝑐3 𝑐4 + 𝑑3 𝑠4 − 𝑎3 𝑐4

−𝑐 𝑐 𝑠 + 𝑠1 𝑐4−𝑠1 𝑐23 𝑠4 − 𝑐1 𝑐4 𝑠23 𝑠4 𝑎2 𝑐3 𝑠4 + 𝑑3 𝑐4 + 𝑎3 𝑠4
[ 1 23 4 −𝑠1 𝑠23 −𝑐23 ]
−𝑐1 𝑠23 𝑎2 𝑠3 − 𝑑4
0 0 0 1

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)

Por lo tanto podemos resolver para 𝜃5

𝜃5 = 𝐴𝑡𝑎𝑛2(𝑠5 , 𝑐5 )

En donde 𝑠5 𝑦 𝑐5 se obtienen mediante la ecuación (22)

Aplicando el mismo método calculamos ( 05𝑇)−1y escribimos la ecuación (2) como

sigue:
( 05𝑇)−1 06𝑇 = 56𝑇(𝜃6 )

Igualando los elementos (3,1) y (1,1) de ambos lados de la ecuación (21)

obtenemos
𝜃6 = 𝐴𝑡𝑎𝑛2(𝑠6 , 𝑐6 )

En donde

𝑠6 = −𝑟11 (𝑐1 𝑐23 𝑠4 − 𝑠1 𝑐4 ) − 𝑟21 (𝑠1 𝑐23 𝑠4 + 𝑐1 𝑐4 ) + 𝑟31 (𝑠23 𝑐4 ),

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

Figura 4.Plano X-Y.

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.

Figura 5.Plano X-Y.

Figura 5.Plano X-Z.

La posición del eslabón 2 es:

√((lc2 ∗ Cos[q2[𝑡]])^2 + l1^2) ∗ Cos[q1[𝑡]]

𝑃𝑐𝑔2 = [ √((lc2 ∗ Cos[q2[𝑡]])^2 + l1^2) ∗ Sin[q1[𝑡]] ]
lc2 ∗ Sin[q2[𝑡]]
La posición del eslabón 3 es:

√((l2 ∗ Cos[q2[𝑡]] + lc3 ∗ Cos[q3[𝑡]])^2 + l1^2) ∗ Cos[q1[𝑡]]

𝑃𝑐𝑔3 = [ √((l2 ∗ Cos[q2[𝑡]] + lc3 ∗ Cos[q3[𝑡]])^2 + l1^2) ∗ Sin[q1[𝑡]] ]
l2 ∗ Sin[q2[𝑡]] + lc3 ∗ lc3 ∗ Sin[q2[𝑡] + q3[𝑡]]

Se tienen la Inercia de 3 eslabones

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

Usando la matriz general Jacobiana

𝜕𝑓𝑥 𝜕𝑓𝑥
⋯
𝜕𝑞1 𝜕𝑞1
𝐽= ⋮ ⋱ ⋮
𝜕𝑓𝑥 𝜕𝑓𝑥
⋯
[𝜕𝑞1 𝜕𝑞1 ]
La cual consta de derivadas parciales, se aplica para cada eslabón
Jacobiana del primer eslabón.
−lc1Sin[q1[𝑡]] 0 0 0 0 0
𝐽𝑣1 = [ lc1Cos[q1[𝑡]] 0 0 0 0 0]
0 0 0 0 0 0

Jacobiana del segundo eslabón.

𝐽𝑣211 𝐽𝑣212 0 0 0 0
𝐽𝑣2 = [𝐽𝑣221 𝐽𝑣222 0 0 0 0]
0 𝐽𝑣232 0 0 0 0

𝐽𝑣211 = −√𝑙12 + 𝑙𝑐22 𝐶𝑜𝑠[𝑞2[𝑡]]2 𝑆𝑖𝑛[𝑞1[𝑡]]

𝑙𝑐22 𝐶𝑜𝑠[𝑞1[𝑡]]𝐶𝑜𝑠[𝑞2[𝑡]]𝑆𝑖𝑛[𝑞2[𝑡]]
𝐽𝑣212 = −
√𝑙12 + 𝑙𝑐22 𝐶𝑜𝑠[𝑞2[𝑡]]2

𝐽𝑣221 = 𝐶𝑜𝑠[𝑞1[𝑡]]√𝑙12 + 𝑙𝑐22 𝐶𝑜𝑠[𝑞2[𝑡]]2

𝑙𝑐22 𝐶𝑜𝑠[𝑞2[𝑡]]𝑆𝑖𝑛[𝑞1[𝑡]]𝑆𝑖𝑛[𝑞2[𝑡]]
𝐽𝑣222 = −
√𝑙12 + 𝑙𝑐22 𝐶𝑜𝑠[𝑞2[𝑡]]2

𝐽𝑣232 = 𝑙𝑐2𝐶𝑜𝑠[𝑞2[𝑡]]

Jacobiana del tercer eslabón.

𝐽𝑉311 𝐽𝑉312 𝐽𝑉313 0 0 0
3 = 𝐽𝑉321 𝐽𝑉322 𝐽𝑉323 0 0 0
0 𝐽𝑉332 𝐽𝑉333 0 0 0

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

JV321 = Cos[q1[𝑡]]√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

JV332 = l2Cos[q2[𝑡]] + lc3Cos[q2[𝑡] + q3[𝑡]]

JV333 = lc3Cos[q2[𝑡] + q3[𝑡]]

Jacobiana del cuarto eslabón

Jv411 Jv412 Jv413 Jv414 0 0
v4 = [Jv421 Jv422 Jv423 Jv424 0 0]
0 Jv423 Jv433 Jv434 0 0

Jv411 = −√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]

2
Jv421 = Cos[q1[𝑡]]√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])

l2Cos[q1[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])Sin[q2[𝑡]]

Jv412 = −
2
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])

l2(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])Sin[q1[𝑡]]Sin[q2[𝑡]]

Jv422 = −
2
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])

Jv423 = l2Cos[q2[𝑡]] + l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]]

 l3Cos[q1[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])Sin[q3[𝑡]]
Jv413 = −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l3(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])Sin[q1[𝑡]]Sin[q3[𝑡]]
Jv423 = −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2

Jv433 = os[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]]

lc4Cos[q1[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])Sin[q4[𝑡]]

Jv414 = −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
lc4(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])Sin[q1[𝑡]]Sin[q4[𝑡]]
Jv424 = −
√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2

Jv434 = lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]]

Jacobiana del quinto eslabón

𝐽𝑣511
= −√l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + l4Cos[q4[𝑡]] + lc5Cos[q5[𝑡]])2 Sin[q1[𝑡]]

𝐽𝑣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

Jacobiana del sexto eslabón

𝐽𝑣611 𝐽𝑣612 𝐽𝑣613 𝐽𝑣614 𝐽𝑣615 𝐽𝑣616
𝐽𝑣6 = [𝐽𝑣621 𝐽𝑣622 𝐽𝑣623 𝐽𝑣624 𝐽𝑣625 𝐽𝑣626 ]
0 𝐽𝑣632 𝐽𝑣633 0 0 0

𝐽𝑣611 = −√𝑙12 + (𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + 𝑙3𝐶𝑜𝑠[𝑞3[𝑡]] + 𝑙4𝐶𝑜𝑠[𝑞4[𝑡]] + 𝑙5𝐶𝑜𝑠[𝑞5[𝑡]] + 𝑙𝑐6𝐶𝑜𝑠[𝑞6[𝑡]])2 𝑆𝑖𝑛[𝑞1[𝑡] + 𝑞6[𝑡]]

𝐽𝑣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

𝐽𝑣632 = 𝑙2𝐶𝑜𝑠[𝑞2[𝑡]] + (𝑙3 + 𝑙𝑐4)𝐶𝑜𝑠[𝑞2[𝑡] + 𝑞3[𝑡]]

𝐽𝑣633 = (𝑙3 + 𝑙𝑐4)𝐶𝑜𝑠[𝑞2[𝑡] + 𝑞3[𝑡]]

Ya con todos los jacobianos calculados se obtuvo la Velocidad Angular

1 0 0 0 0 0
Jw1 = 1 0 0 0 0 0
0 0 0 0 0 0
1 1 0 0 0 0
Jw2 = 1 1 0 0 0 0
0 1 0 0 0 0

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.

M1 = m1 ∗ 𝐽v1𝑇 ∗ Jv1 + 𝐽v1𝑇 ∗ I1 ∗ Jv1

M2 = m2 ∗ 𝐽v2𝑇 ∗ Jv2 + 𝐽v2𝑇 ∗ I2 ∗ Jv2
M3 = m3 ∗ 𝐽v3𝑇 ∗ Jv3 + 𝐽v3𝑇 ∗ I3 ∗ Jv3
M4 = m3 ∗ 𝐽v4𝑇 ∗ Jv4 + 𝐽v4𝑇 ∗ I4 ∗ Jv4
M5 = m5 ∗ 𝐽v5𝑇 ∗ Jv5 + 𝐽v5𝑇 ∗ I5 ∗ Jv5
M6 = m6 ∗ 𝐽v6𝑇 ∗ Jv6 + 𝐽v6𝑇 ∗ I6 ∗ Jv6

Matriz de masas e inercias:

𝑀 = 𝑀1 + 𝑀2 + 𝑀3 + 𝑀4 + 𝑀5 + 𝑀6

hxx1 + hyy1 + lc12 m1 0 0 0 0 0

0 0 0 0 0 0
M1= 0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
[ 0 0 0 0 0 0]

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

lc22 (l12 + lc22 )m2Cos[q2[𝑡]]2

M222 = hxx2 + hyy2 + hzz2 +
l12 + lc22 Cos[q2[𝑡]]2
 𝑀311 hxx3 + hyy3 hxx3 + hyy3 0 0 0
hxx3 + hyy3 𝑀322 𝑀323 0 0 0
M3= hxx3 + hyy3 𝑀332 𝑀333 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
[ 0 0 0 0 0 0]

𝑀311 = hxx3 + hyy3 + m3(l12 + (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2 )

𝑀322 = hxx3 + hyy3 + hzz3 + m3((l2Cos[q2[𝑡]] + lc3Cos[q2[𝑡] + q3[𝑡]])2

l22 (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2 Sin[q2[𝑡]]2
+ )
l12 + (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2

𝑀332 = hxx3 + hyy3 + hzz3 + m3((l2Cos[q2[𝑡]] + lc3Cos[q2[𝑡] + q3[𝑡]])2

l22 (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2 Sin[q2[𝑡]]2
+ )
l12 + (l2Cos[q2[𝑡]] + lc3Cos[q3[𝑡]])2

𝑀323 = hxx3 + hyy3 + hzz3 + lc3m3(Cos[q2[𝑡] + q3[𝑡]](l2Cos[q2[𝑡]] +

l2(l2Cos[q2[𝑡]]+lc3Cos[q3[𝑡]])2 Sin[q2[𝑡]]Sin[q3[𝑡]]
lc3Cos[q2[𝑡] + q3[𝑡]]) + l12 +(l2Cos[q2[𝑡]]+lc3Cos[q3[𝑡]])2
)

𝑀333 =hxx3 + hyy3 + hzz3 + lc32 m3Cos[q2[𝑡] + q3[𝑡]]2 +

lc32 m3(l2Cos[q2[𝑡]]+lc3Cos[q3[𝑡]])2 Sin[q3[𝑡]]2
l12 +(l2Cos[q2[𝑡]]+lc3Cos[q3[𝑡]])2

M411 M412 M413 M414 0 0

M421 M422 M423 M424 0 0
M431 M432 M433 M434 0 0
𝑀4 =
M441 M442 M443 M444 0 0
0 0 0 0 0 0
[ 0 0 0 0 0 0]

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

M432 = hxx4 + hyy4 + hzz4 + m4((l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]])(l2Cos[q2[𝑡]]

+ l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]])
l2l3Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q2[𝑡]]Sin[q3[𝑡]]
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l2l3(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q2[𝑡]]Sin[q3[𝑡]]
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2

M442 = hxx4 + hyy4 + hzz4 + lc4m4(Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q2[𝑡] + q3[𝑡]]

+ lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]])
l2Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q2[𝑡]]Sin[q4[𝑡]]
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l2(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q2[𝑡]]Sin[q4[𝑡]]
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2

M423 = hxx4 + hyy4 + hzz4 + m4((l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]])(l2Cos[q2[𝑡]]

+ l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]])
l2l3Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q2[𝑡]]Sin[q3[𝑡]]
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l2l3(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q2[𝑡]]Sin[q3[𝑡]]
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2

M433 = hxx4 + hyy4 + hzz4 + m4((l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]])2

l32 Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q3[𝑡]]2
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l3 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q3[𝑡]]2
2
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
M443 = hxx4 + hyy4 + hzz4 + lc4m4(Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]](l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡]
+ q3[𝑡] + q4[𝑡]])
l3Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q3[𝑡]]Sin[q4[𝑡]]
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l3(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q3[𝑡]]Sin[q4[𝑡]]
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2

M424 = hxx4 + hyy4 + hzz4 + lc4m4(Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]](l2Cos[q2[𝑡]] + l3Cos[q2[𝑡] + q3[𝑡]]

+ lc4Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]])
l2Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q2[𝑡]]Sin[q4[𝑡]]
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l2(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q2[𝑡]]Sin[q4[𝑡]]
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
M434 = hxx4 + hyy4 + hzz4 + lc4m4(Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]](l3Cos[q2[𝑡] + q3[𝑡]] + lc4Cos[q2[𝑡]
+ q3[𝑡] + q4[𝑡]])
l3Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q3[𝑡]]Sin[q4[𝑡]]
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
l3(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q3[𝑡]]Sin[q4[𝑡]]
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2

M444 = hxx4 + hyy4 + hzz4 + lc42 m4(Cos[q2[𝑡] + q3[𝑡] + q4[𝑡]]2

Cos[q1[𝑡]]2 (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q4[𝑡]]2
+
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2
(l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2 Sin[q1[𝑡]]2 Sin[q4[𝑡]]2
+ )
l12 + (l2Cos[q2[𝑡]] + l3Cos[q3[𝑡]] + lc4Cos[q4[𝑡]])2

Una vez obtenida la matriz de masas, se sigue el siguiente procedimiento para

poder calcular el vector de fuerzas centrípetas y de coriolis, donde se calcula la
derivada con respecto del tiempo de cada elemento de la matriz de las variables
articulares
𝜕 𝑞1(𝑡)
𝜕𝑡
𝜕 𝑞2(𝑡)
𝜕𝑡
𝜕 𝑞3(𝑡)
𝐷𝑞 = 𝜕𝑡
𝜕 𝑞4(𝑡)
𝜕𝑡
𝜕 𝑞5(𝑡)
𝜕𝑡
𝜕 𝑞6(𝑡)
( 𝜕𝑡 )
Y el resultado se multiplica por la derivada con respecto al tiempo de la Matriz de
Masas e Inercias
𝜕𝑀
𝑑𝑀 = ∗ 𝐷𝑞
𝜕𝑡
Se calcula la transpuesta de la derivada de las variables articulares y se multiplica
por la matriz de Masas e Inercias y por la derivada con respecto al tiempo de las
variables articulares.
𝑞𝑀 = 𝐷𝑞 𝑇 ∗ 𝑀 ∗ 𝐷𝑞
Y a ese resultado se le deriva nuevamente con respecto del tiempo para cada una
de las variables articulares.
𝜕𝑞𝑀
𝑑𝑞1 =
𝜕𝑞1(𝑡)
 𝜕𝑞𝑀
𝑑𝑞2 =
𝜕𝑞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.