Compendio2 Rev1

UNIVERSIDAD NACIONAL DEL CALLAO
ESCUELA DE POSGRADO
UNIDAD DE POSGRADO DE LA FACULTAD DE INGENIERÍA ELÉCTRICA Y
ELECTRÓNICA
MAESTRIA EN CIENCIAS DE LA ELECTRÓNICA, MENCIÓN CONTROL Y

AUTOMATIZACIÓN
CURSO: EB-309 - ROBOTICA
TRABAJO N° 2
TEMA:
APLICACIÓN DE LA DINAMICA DE UN ROBOT DE 4 GRADOS DE
LIBERTAD
DOCENTE: MG. ING. JOSÉ MACHUCA MINES
INTEGRANTES DEL GRUPO:

 LUCERO GUILLEN, PABLO
 GALINDEZ ROMERO, HENRY
 RIVERA CORRALES, MARCOS
 LAU MEJIA, JUAN
Callao, 11 de Noviembre de 2018

TRABAJO N° 2
TEMA:
APLICACIÓN DE LA DINAMICA DE UN ROBOT DE 4 GRADOS DE LIBERTAD
INDICE
1. DEFINICION DEL PROBLEMA

2. DETERMINACION DE LA ENERGÍA CINETICA DEL SISTEMA
3. DETERMINACION DE LA ENERGÍA POTENCIAL DEL SISTEMA
4. DETERMINACION DE LA EXPRESION DE LAGRANGE DEL SISTEMA
5. DETERMINACION DE LAS ECUACIONES DE LA DINAMICA INVERSA
DEL SISTEMA
6. SIMULACION MEDIANTE SOFTWARE MATLAB DE LA DINAMICA DEL
ROBOT
1. DEFINICION DEL PROBLEMA
El trabajo tiene por objetivo encontrar los parámetros dinámicos de un robot manipulador
de 4 Grados de Libertad de cadena cinemática serial, de los cuales 3 son rotacionales
y 1 es prismático.
La figura N° 1 muestra la estructura del robot en mención.
Figura N° 1 – Estructura analítica del robot
Objeto Ixx Iyy Izz

Cilindro o 3𝑅2 + 𝐻 2 3𝑅2 + 𝐻 2 𝑅2
disco 𝑀 𝑀 𝑀
12 12 2
Cascarón cilíndrico muy delgado 6𝑅2 + 𝐻 2 6𝑅2 + 𝐻 2 𝑀𝑅2
𝑀 𝑀
12 12
Cilindro muy delgado 𝐻2 𝐻2 0
𝑀 𝑀
12 12
Prisma o paralelepipedo 𝐴2 + 𝐻 2 𝐴2 + 𝐻 2 𝐴2
𝑀 𝑀 𝑀
12 12 6
Eslabon 1
0
̅̅̅̅̅ 0
Δ𝐶1 = [ 𝐻1]
𝑞1 −
2
𝐼1𝑋𝑋 0 0
𝐼1 = [ 0 𝐼1𝑌𝑌 0 ]
0 0 𝐼1𝑍𝑍
̅̅̅
𝐷1 (𝑞) = 0𝑅1 𝐼1 1𝑅0
𝐴2 + 𝐻12
𝑀 0 0
𝐼1𝑋𝑋 0 0 12
2 2
̅̅̅1 (𝑞) = [ 0 𝐴 + 𝐻1
𝐷 𝐼1𝑌𝑌 0 ]= 0 𝑀 0
0 0 𝐼1𝑍𝑍 12
𝐴2
[ 0 0 𝑀
6]
Sabemos que:
̅̅̅̅̅̅̅
𝐶1 (𝑞) = 𝑃1 (𝑞) + 0𝑅1 ̅̅̅̅̅
∆𝐶1
0
0 1 0 0
̅̅̅̅̅̅̅ 0
𝐶1 (𝑞) = [ 0 ] + [0 1 0] [ 𝐻1]
𝐻1 0 0 1 𝑞1 −
2
0
̅̅̅̅̅̅̅ 0
𝐶1 (𝑞) = [𝐻1 ]
+ 𝑞1
2
Hallamos la matriz jacobiana V:
𝜕𝐶1𝑥 (𝑞) 𝜕𝐶1𝑥 (𝑞) 𝜕𝐶1𝑥 (𝑞) 𝜕𝐶1𝑥 (𝑞)
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4
𝜕𝐶1𝑦 (𝑞) 𝜕𝐶1𝑦 (𝑞) 𝜕𝐶1𝑦 (𝑞) 𝜕𝐶1𝑦 (𝑞)
𝐽𝑣1 (𝑞) =
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4
𝜕𝐶1𝑧 (𝑞) 𝜕𝐶1𝑧 (𝑞) 𝜕𝐶1𝑧 (𝑞) 𝜕𝐶1𝑧 (𝑞)
[ 𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4 ]
0 0 0 0
𝐽𝑣1 (𝑞) = [0 0 0 0]
1 0 0 0
Hallamos la matriz jacobiana W:
1 (𝑞)
𝐽𝑤 = [𝜉1 𝑍𝑜 0 0 0]
Para este caso: 𝜉1 = 0
0 0 0 0
1 (𝑞)
𝐽𝑤 = [ 0 0 0 0]
0 0 0 0
𝐷1 (𝑞) = [𝐽𝑣1 (𝑞)]𝑇 𝑀1 [𝐽𝑣1 (𝑞)] + [𝐽𝑤
1 ̅̅̅1 (𝑞)[𝐽𝑤
(𝑞)]𝑇 𝐷 1
(𝑞)]
𝑀1 0 0 0
𝐷1 (𝑞) = [ 0 0 0 0]
0 0 0 0
0 0 0 0
Eslabón 2
0
̅̅̅̅̅2 = [ 0
Δ𝐶 𝐻2]
𝑑2 −
2
𝐼2𝑋𝑋 0 0
𝐼2 = [ 0 𝐼2𝑧𝑧 0 ]
0 0 𝐼2𝑦𝑦
6𝑅 2 + 𝐻22
𝑀2 0 0
12
𝐼2 = 0 𝑀2𝑅 2 0
6𝑅 2 + 𝐻22
[ 0 0 𝑀2 ]
12
̅̅̅
𝐷2 (𝑞) = 0𝑅 2 𝐼2 2𝑅0
𝐶2 0 −𝑆2 𝐼2𝑋𝑋 0 0 𝐶2 𝑆2 0
̅̅̅2 (𝑞) = [𝑆2
𝐷 0 𝐶2 ] [ 0 𝐼2𝑧𝑧 0 ][ 0 0 −1]
0 −1 0 0 0 𝐼2𝑥𝑥 −𝑆2 𝐶2 0
𝐼2𝑋𝑋 0 0
̅̅̅2 (𝑞) = [ 0
𝐷 𝐼2𝑥𝑥 0 ]
0 0 𝐼2𝑧𝑧
Sabemos que:
̅̅̅̅̅̅̅
𝐶2 (𝑞) = 𝑃2 (𝑞) + 0𝑅 2 ̅̅̅̅̅
∆𝐶2
0
𝐿1 𝐶2 0 −𝑆2
̅̅̅̅̅̅̅ 0
𝐶2 (𝑞) = [ 0 ] + [𝑆2 0 𝐶2 ] [ 𝐻2]
𝐻1 0 −1 0 𝑑2 −
2
𝐻2
𝐿1 − 𝑆2 (𝑑2 − )
2
̅̅̅̅̅̅̅
𝐶2 (𝑞) = 𝐻2
𝐶2 (𝑑2 − )
2
[ 𝐻1 ]
𝜕𝐶2𝑥 (𝑞) 𝜕𝐶2𝑥 (𝑞) 𝜕𝐶2𝑥 (𝑞) 𝜕𝐶2𝑥 (𝑞)
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4
𝜕𝐶2𝑦 (𝑞) 𝜕𝐶2𝑦 (𝑞) 𝜕𝐶2𝑦 (𝑞) 𝜕𝐶2𝑦 (𝑞)
𝐽𝑣2 (𝑞) =
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4
𝜕𝐶2𝑧 (𝑞) 𝜕𝐶2𝑧 (𝑞) 𝜕𝐶2𝑧 (𝑞) 𝜕𝐶2𝑧 (𝑞)
[ 𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4 ]
𝐻2
0 −𝐶2 (𝑑2 − )
𝑄
2 0 0
𝐽𝑣 (𝑞) = 0 𝐻2 0 0
0 −𝑆2 (𝑑2 − 2 ) 0 0
[ 0 ]
1 (𝑞)
𝐽𝑤 = [𝜉1 𝑍𝑜 𝜉2 𝑍1 0 0]
Para este caso: 𝜉1 = 0, 𝜉2 = 1
0 0 0 0
1 (𝑞)
𝐽𝑤 = [ 0 0 0 0]
0 1 0 0
𝐷2 (𝑞) = [𝐽𝑣2 (𝑞)]𝑇 𝑀2 [𝐽𝑣2 (𝑞)] + [𝐽𝑤
2 ̅̅̅2 (𝑞)[𝐽𝑤
(𝑞)]𝑇 𝐷 2
(𝑞)]
0 0 0 𝐻2
𝐻2 𝐻2 0 −𝐶2 (𝑑2 − 2 ) 0 0
−𝐶2 (𝑑2 − ) − 𝑆2 (𝑑2 − )0
𝐷2 (𝑞) = 2 2 𝑀2 0 𝐻2 0 0
0 0 0 0 −𝑆 (𝑑2 − )
2
2 0 0
[0 0 0] [ 0 ]
0 0 0 𝐼 0 0 0 0 0 0
2𝑋𝑋
0 01
+[ ][ 0 𝐼2𝑥𝑥 0 ] [ 0 0 0 0]
0 0 0
0 0 𝐼2𝑧𝑧 0 1 0 0
0 0 0
0 0 0 0
𝐻2 2
𝐷2 (𝑞) = 0 𝑀2 (𝑑2 − ) + 𝐼2𝑧𝑧 0 0
2
0 0 0 0
[0 0 0 0]
Eslabón 3
0
̅̅̅̅̅3 = [ 0
Δ𝐶 𝐻3]
𝐿2 −
2
𝐼3𝑥𝑥 0 0
𝐼3 = [ 0 𝐼3𝑧𝑧 0 ]
0 0 𝐼3𝑥𝑥
6𝑅 2 + 𝐻32
𝑀3 0 0
12
𝐼3 = 0 𝑀3𝑅2 0
6𝑅2 + 𝐻32
[ 0 0 𝑀3 ]
12
̅̅̅
𝐷3 (𝑞) = 0𝑅 3 𝐼3 3𝑅0
𝐶2 𝐶3 −𝑆2 𝐶2 𝑆3 𝐼3𝑥𝑥 0 0 𝐶2 𝐶3 𝐶3 𝑆2 −𝑆3

̅̅̅
𝐷3 (𝑞) = [𝐶3 𝑆2 𝐶2 𝑆2 𝑆3 ] [ 0 𝐼3𝑧𝑧 0 ] [ −𝑆2 𝐶2 0 ]
−𝑆3 0 𝐶3 0 0 𝐼3𝑥𝑥 𝐶2 𝑆3 𝑆2 𝑆3 𝐶3
(𝐼3𝑥𝑥 − 𝐼3𝑧𝑧 )
𝐼3𝑥𝑥 𝐶32 + 𝐼3𝑧𝑧 𝑆32 𝑆23 0
2
̅̅̅
𝐷3 (𝑞) = (𝐼3𝑥𝑥 − 𝐼3𝑧𝑧 )
𝑆23 𝐼3𝑥𝑥 𝑆32 + 𝐼3𝑧𝑧 𝐶32 0
2
[ 0 0 𝐼3𝑥𝑥 ]
Sabemos que:
̅̅̅̅̅̅̅
𝐶3 (𝑞) = 𝑃3 (𝑞) + 0𝑅 3 ̅̅̅̅̅
∆𝐶3
𝐶2 𝐶3 −𝑆2 𝐶2 𝑆3 0
𝐿1
̅̅̅̅̅̅̅ 0
𝐶3 (𝑞) = [ 𝑑2 ] + [𝐶3 𝑆2 𝐶2 𝑆2 𝑆3 ] [ 𝐻3]
𝐻1 −𝑆3 0 𝐶3 𝐿2 −
2
𝐻3
𝐿1 + 𝐶2 𝑆3 (𝐿2 −
)
2
̅̅̅̅̅̅̅ 𝐻3
𝐶3 (𝑞) = 𝑑2+𝑆2 𝑆3 (𝐿2 − )
2
𝐻3
[ 𝐻1 + 𝐶3 (𝐿2 − 2 ) ]
𝜕𝐶3𝑥 (𝑞) 𝜕𝐶3𝑥 (𝑞) 𝜕𝐶3𝑥 (𝑞) 𝜕𝐶3𝑥 (𝑞)
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4
𝜕𝐶3𝑦 (𝑞) 𝜕𝐶3𝑦 (𝑞) 𝜕𝐶3𝑦 (𝑞) 𝜕𝐶3𝑦 (𝑞)
𝐽𝑣3 (𝑞) =
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4
𝜕𝐶3𝑧 (𝑞) 𝜕𝐶3𝑧 (𝑞) 𝜕𝐶3𝑧 (𝑞) 𝜕𝐶3𝑧 (𝑞)
[ 𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4 ]
𝐽𝑣3 (𝑞) = [ 0, 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) ∗ (𝐻3/2 − 𝐿2), −𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) ∗ (𝐻3/2 − 𝐿2), 0]
[ 0, −𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) ∗ (𝐻3/2 − 𝐿2), −𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2) ∗ (𝐻3/2 − 𝐿2), 0]
[ 0, 0, 𝑠𝑖𝑛(𝑞3) ∗ (𝐻3/2 − 𝐿2), 0]
3 (𝑞)
𝐽𝑤 = [𝜉1 𝑍𝑜 𝜉2 𝑍1 𝜉3 𝑍2 0]
Para este caso: 𝜉1 = 0, 𝜉2 = 1, 𝜉3 = 1
0 0 −𝑆2 0
3 (𝑞)
𝐽𝑤 = [0 0 𝐶2 0]
0 1 0 0
𝐷3 (𝑞) = [𝐽𝑣3 (𝑞)]𝑇 𝑀3 [𝐽𝑣3 (𝑞)] + [𝐽𝑤
3 ̅̅̅3 (𝑞)[𝐽𝑤
(𝑞)]𝑇 𝐷 3
(𝑞)]
0 0 0 0
0 𝐷322 0 0
𝐷3 (𝑞) = [ ]
0 0 𝐷333 0
0 0 0 0
𝐷322 = 𝐼3𝑥𝑥 + (𝐻3^2 ∗ 𝑀3 ∗ 𝑠𝑖𝑛(𝑞3)^2)/4 + 𝐿2^2 ∗ 𝑀3 ∗ 𝑠𝑖𝑛(𝑞3)^2 − 𝐻3 ∗ 𝐿2 ∗ 𝑀3 ∗ 𝑠𝑖𝑛(𝑞3)^2
𝐷333 = (𝑀3 ∗ 𝐻3^2)/4 − 𝑀3 ∗ 𝐻3 ∗ 𝐿2 + 𝑀3 ∗ 𝐿2^2 + 𝐼3𝑧𝑧
Eslabón 4
0
̅̅̅̅̅ 0
Δ𝐶4 = [ 𝐻4]
𝐷4 −
2
𝐼4𝑥𝑥 0 0
𝐼4 = [ 0 𝐼4𝑧𝑧 0 ]
0 0 𝐼4𝑥𝑥
𝐻42
𝑀4 0 0
12
𝐼4 = 0 0 0
𝐻42
[ 0 0 𝑀4
12 ]
̅̅̅
𝐷4 (𝑞) = 0𝑅 4 𝐼4 4𝑅0
𝐶2 𝐶3 𝐶4 − 𝑆2 𝑆4 −𝐶4 𝑆3 −𝐶2 𝑆4 − 𝐶3 𝐶4 𝑆2 𝐼4𝑥𝑥 0 0 𝐶2 𝐶3 𝐶4 − 𝑆2 𝑆4 𝐶2 𝐶3 𝐶4 + 𝑆2 𝐶4 −𝐶2 𝑆3
̅̅̅
𝐷4 (𝑞) = [𝐶2 𝐶3 𝐶4 + 𝑆2 𝐶4 −𝑆3 𝑆4 𝐶2 𝑐4 − 𝐶3 𝑆2 𝑆4 ] [ 0 𝐼4𝑧𝑧 0 ][ −𝐶4 𝑆3 −𝑆3 𝑆4 −𝐶3 ]
−𝐶2 𝑆3 −𝐶3 𝑆2 𝑆3 0 0 𝐼4𝑥𝑥 −𝐶2 𝑆4 − 𝐶3 𝐶4 𝑆2 𝐶2 𝑐4 − 𝐶3 𝑆2 𝑆4 𝑆2 𝑆3
Pero 𝐼4𝑧𝑧 = 0
𝐼4𝑥𝑥 𝑆32 𝑆44
𝐼4𝑥𝑥 (𝑆32 𝑆42 − 𝐶32 ) − −𝐼4𝑥𝑥 𝐶3 𝐶4 𝑆3
2
̅̅̅
𝐷4 (𝑞) = − 𝐼4𝑥𝑥 𝑆32 𝑆44
−𝐼4𝑥𝑥 (𝑆32 𝑆42 − 1) −𝐼4𝑥𝑥 𝐶3 𝑆3 𝑆4
2
−𝐼4𝑥𝑥 𝐶3 𝐶4 𝑆3 −𝐼4𝑥𝑥 𝐶3 𝑆3 𝑆4 −𝐼4𝑥𝑥 𝑆32
[ ]
Sabemos que:
̅̅̅̅̅̅̅
𝐶4 (𝑞) = 𝑃4 (𝑞) + 0𝑅 4 ̅̅̅̅̅
∆𝐶4
0
𝐿1 𝐶2 𝐶3 𝐶4 − 𝑆2 𝑆4 −𝐶4 𝑆3 −𝐶2 𝑆4 − 𝐶3 𝐶4 𝑆2
̅̅̅̅̅̅̅ 0
𝐶4 (𝑞) = [ 𝑑2 ] + [ 2 𝐶3 𝐶4 + 𝑆2 𝐶4
𝐶 −𝑆3 𝑆4 𝐶2 𝑐4 − 𝐶3 𝑆2 𝑆4 ][ 𝐻4]
𝐻1 + 𝐻3 −𝐶2 𝑆3 −𝐶3 𝑆2 𝑆3 𝐷4 −
2
𝐻4
𝐿1 − (𝐶2 𝑆4 + 𝐶3 𝐶4 𝑆2 )(𝐷4 −
)
2
̅̅̅̅̅̅̅ 𝐻4
𝐶 4 (𝑞) = 𝑑2 + (𝐶2 𝐶4 − 𝐶3 𝑆2 𝑆4 )(𝐷4 − )
2
𝐻4
[ 𝐻1 + 𝐻3 + 𝑆2 𝑆3 (𝐷4 − 2 ) ]

𝜕𝐶4𝑥 (𝑞) 𝜕𝐶4𝑥 (𝑞) 𝜕𝐶4𝑥 (𝑞) 𝜕𝐶4𝑥 (𝑞)
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4
𝜕𝐶4𝑦 (𝑞) 𝜕𝐶4𝑦 (𝑞) 𝜕𝐶4𝑦 (𝑞) 𝜕𝐶4𝑦 (𝑞)
𝐽𝑣4 (𝑞) =
𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4
𝜕𝐶4𝑧 (𝑞) 𝜕𝐶4𝑧 (𝑞) 𝜕𝐶4𝑧 (𝑞) 𝜕𝐶4𝑧 (𝑞)
[ 𝜕𝑞1 𝜕𝑞2 𝜕𝑞3 𝜕𝑞4 ]
𝐽𝑣4 (𝑞) = [ 0, −(𝐷4 − 𝐻4/2) ∗ (𝐶2 ∗ 𝐶4 − 𝐶3 ∗ 𝑆2 ∗ 𝑆4), 𝐶2 ∗ 𝑆3 ∗ 𝑆4 ∗ (𝐷4 − 𝐻4/2), (𝐷4 − 𝐻4/2) ∗ (𝑆2 ∗ 𝑆4 − 𝐶2 ∗ 𝐶3 ∗ 𝐶4)]
[ 0, −(𝐷4 − 𝐻4/2) ∗ (𝐶4 ∗ 𝑆2 + 𝐶2 ∗ 𝐶3 ∗ 𝑆4), 𝑆2 ∗ 𝑆3 ∗ 𝑆4 ∗ (𝐷4 − 𝐻4/2), −(𝐷4 − 𝐻4/2) ∗ (𝐶2 ∗ 𝑆4 + 𝐶3 ∗ 𝐶4 ∗ 𝑆2)]
[ 0, 0, 𝐶3 ∗ 𝑆4 ∗ (𝐷4 − 𝐻4/2), 𝐶4 ∗ 𝑆3 ∗ (𝐷4 − 𝐻4/2)]

4 (𝑞)
𝐽𝑤 = [𝜉1 𝑍𝑜 𝜉2 𝑍1 𝜉3 𝑍2 𝜉4 𝑍3 ]
Para este caso: 𝜉1 = 0, 𝜉2 = 1, 𝜉3 = 1, 𝜉4 = 1
0 0 −𝑆2 𝐶3 𝑆2
4 (𝑞)
𝐽𝑤 = [0 0 𝐶2 𝑆2 𝑆3 ]
0 1 0 𝐶3
𝐷4 (𝑞) = [𝐽𝑣4 (𝑞)]𝑇 𝑀4 [𝐽𝑣4 (𝑞)] + [𝐽𝑤
4 ̅̅̅4 (𝑞)[𝐽𝑤
(𝑞)]𝑇 𝐷 4
(𝑞)]
0 0 0 0
0 𝐷422 𝐷423 𝐷424
𝐷4 (𝑞) = [ ]
0 𝐷432 𝐷433 𝐷434
0 𝐷442 𝐷443 𝐷444
Donde:
𝐷422 = 𝐼4𝑥𝑥 ∗ 𝑆3^2 ∗ 𝑆4^2 + 𝑀4 ∗ (𝐷4 − 𝐻4/2)^2 ∗ (𝐶4 ∗ 𝑆2 + 𝐶2 ∗ 𝐶3 ∗ 𝑆4)^2 + 𝑀4 ∗ (𝐷4 − 𝐻4/2)^2 ∗ (𝐶2 ∗ 𝐶4 − 𝐶3 ∗ 𝑆2
∗ 𝑆4)^2 + 𝐶4^2 ∗ 𝐼4𝑥𝑥 ∗ 𝑆3^2
𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝐷423 = −
4
𝐶3 ∗ 𝑀4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝐷424 = (
4
𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝐷432 = 𝐷423 == −
4
𝐷433 = 𝐼4𝑥𝑥 + (𝐷4^2 ∗ 𝑀4)/2 + (𝐻4^2 ∗ 𝑀4)/8 − (𝐷4 ∗ 𝐻4 ∗ 𝑀4)/2 − (𝐷4^2 ∗ 𝑀4 ∗ 𝑐𝑜𝑠(2 ∗ 𝑞4))/2 − (𝐻4^2 ∗ 𝑀4 ∗ 𝑐𝑜𝑠(2 ∗ 𝑞4))/8 + (𝐷4 ∗ 𝐻4
∗ 𝑀4 ∗ 𝑐𝑜𝑠(2 ∗ 𝑞4))/2
𝐷434 = 0
𝐶3 ∗ 𝑀4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝐷442 = 𝐷424 = (
4
𝐷443 = 𝐷434 = 01
𝐷444 = (𝑀4 ∗ (2 ∗ 𝐷4 − 𝐻4)^2)/4
El tensor de inercia total neto del sistema respecto de los ejes de rotación o traslación relativos al referencial
base se obtiene sumando los tensores de cada elemento como
𝐷(𝑞) = 𝐷1(𝑞) + 𝐷2(𝑞) + 𝐷3(𝑞) + 𝐷1(𝑞)
𝑀1 0 0 0
0 𝑑11 𝑑12 𝑑13
𝐷(𝑞) = [ ]
0 𝑑21 𝑑22 𝑑23
0 𝑑31 𝑑32 𝑑33
𝑑11 = 𝑀2 ∗ 𝑅^2 + (𝑀3 ∗ (3 ∗ 𝐻3^2 ∗ 𝑆3^2 + 𝐻3^2 − 12 ∗ 𝐻3 ∗ 𝐿2 ∗ 𝑆3^2 + 12 ∗ 𝐿2^2 ∗ 𝑆3^2 + 6 ∗ 𝑅^2))/12 + 𝑀2 ∗ 𝑆2^2
∗ (𝐷2 − 𝐻2/2)^2 + 𝑀4 ∗ (𝐷4 − 𝐻4/2)^2 ∗ (𝐶4 ∗ 𝑆2 + 𝐶2 ∗ 𝐶3 ∗ 𝑆4)^2 + 𝑀4 ∗ (𝐷4 − 𝐻4/2)^2 ∗ (𝐶2
∗ 𝐶4 − 𝐶3 ∗ 𝑆2 ∗ 𝑆4)^2 + 𝐶2^2 ∗ 𝑀2 ∗ (𝐷2 − 𝐻2/2)^2 + (𝐶4^2 ∗ 𝐻4^2 ∗ 𝑀4 ∗ 𝑆3^2)/12 + (𝐻4^2 ∗ 𝑀4
∗ 𝑆3^2 ∗ 𝑆4^2)/12
𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝑑12 = −
4
𝐶3 ∗ 𝑀4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝑑13 =
4
𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝑑21 = −
4
𝑑22
𝑀3 ∗ (𝐻32 − 4 ∗ 𝐻3 ∗ 𝐿2 + 4 ∗ 𝐿22 + 4 ∗ 𝑅 2 )
=
4
𝑀4 ∗ (12 ∗ 𝐷42 ∗ 𝑐𝑜𝑠(2 ∗ 𝑞4) + 3 ∗ 𝐻42 ∗ 𝑐𝑜𝑠(2 ∗ 𝑞4) − 12 ∗ 𝐷42 − 5 ∗ 𝐻42 + 12 ∗ 𝐷4 ∗ 𝐻4 − 12 ∗ 𝐷4 ∗ 𝐻4 ∗ 𝑐𝑜𝑠(2 ∗ 𝑞4))
−
24
𝑑23 = 0
𝐶3 ∗ 𝑀4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝑑31 =
4
𝑑32 = 0
𝑀4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
𝑑33 =
4
Energía cinética
1 𝑇
𝐸𝐶 (𝑞, 𝑞̇ ) = 𝑞̇ 𝐷(𝑞)𝑞̇
2
1 1 1 1 1 1 1 1
𝐸𝐶 (𝑞, 𝑞̇ ) = 𝑀1𝑞̇ 12 + 𝑑11 𝑞̇ 22 + 𝑑21 𝑞2̇ 𝑞3̇ + 𝑑31 𝑞2̇ 𝑞4̇ + 𝑑12 𝑞3̇ 𝑞2̇ + 𝑑22 𝑞̇ 32 + 𝑑32 𝑞3̇ 𝑞4̇ + 𝑑13 𝑞4̇ 𝑞2̇
2 2 2 2 2 2 2 2
1 1 2
+ 𝑑23 𝑞4̇ 𝑞3̇ + 𝑑33 𝑞̇ 4
2 2
Siendo
𝑝𝑎𝑟𝑎 𝑖 = 𝑗 𝑎𝑐𝑒𝑙𝑒𝑟𝑎𝑐𝑖ó𝑛 𝑐𝑒𝑛𝑡𝑟𝑖𝑝𝑒𝑡𝑎

𝑞̇ 𝑖 𝑞̇ 𝑗 = {
𝑝𝑎𝑟𝑎 𝑖 ≠ 𝑗 𝑎𝑐𝑒𝑙𝑒𝑟𝑎𝑐𝑖ó𝑛 𝑑𝑒 𝑐𝑜𝑟𝑒𝑜𝑙𝑖𝑠𝑖𝑠
𝐸𝐶 (𝑞, 𝑞̇ ) = (𝑀1 ∗ 𝑑𝑞1^2)/2 + 𝑑𝑞4 ∗ ((𝑀4 ∗ 𝑑𝑞4 ∗ (2 ∗ 𝐷4 − 𝐻4)^2)/8 + (𝐶3 ∗ 𝑀4 ∗ 𝑑𝑞2 ∗ (2 ∗ 𝐷4
− 𝐻4)^2)/8) − 𝑑𝑞3 ∗ ((𝑑𝑞3 ∗ ((𝑀4 ∗ (12 ∗ 𝐷4^2 ∗ 𝐶44 + 3 ∗ 𝐻4^2 ∗ 𝐶44 − 12
∗ 𝐷4^2 − 5 ∗ 𝐻4^2 + 12 ∗ 𝐷4 ∗ 𝐻4 − 12 ∗ 𝐷4 ∗ 𝐻4 ∗ 𝐶44))/24 − (𝑀3 ∗ (𝐻3^2 − 4
∗ 𝐻3 ∗ 𝐿2 + 4 ∗ 𝐿2^2 + 4 ∗ 𝑅^2))/4))/2 + (𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ 𝑑𝑞2 ∗ (2 ∗ 𝐷4
− 𝐻4)^2)/8) + 𝑑𝑞2 ∗ ((𝑑𝑞2 ∗ (𝑀2 ∗ 𝑅^2 + (𝑀3 ∗ (3 ∗ 𝐻3^2 ∗ 𝑆3^2 + 𝐻3^2 − 12
∗ 𝐻3 ∗ 𝐿2 ∗ 𝑆3^2 + 12 ∗ 𝐿2^2 ∗ 𝑆3^2 + 6 ∗ 𝑅^2))/12 + 𝑀2 ∗ 𝑆2^2 ∗ (𝐷2 − 𝐻2/2)^2
+ 𝑀4 ∗ (𝐷4 − 𝐻4/2)^2 ∗ (𝐶4 ∗ 𝑆2 + 𝐶2 ∗ 𝐶3 ∗ 𝑆4)^2 + 𝑀4 ∗ (𝐷4 − 𝐻4/2)^2 ∗ (𝐶2
∗ 𝐶4 − 𝐶3 ∗ 𝑆2 ∗ 𝑆4)^2 + 𝐶2^2 ∗ 𝑀2 ∗ (𝐷2 − 𝐻2/2)^2 + (𝐶4^2 ∗ 𝐻4^2 ∗ 𝑀4
∗ 𝑆3^2)/12 + (𝐻4^2 ∗ 𝑀4 ∗ 𝑆3^2 ∗ 𝑆4^2)/12))/2 + (𝐶3 ∗ 𝑀4 ∗ 𝑑𝑞4 ∗ (2 ∗ 𝐷4
− 𝐻4)^2)/8 − (𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ 𝑑𝑞3 ∗ (2 ∗ 𝐷4 − 𝐻4)^2)/8)
DONDE:
𝑑𝑞𝑖 = 𝑞𝑖̇
Energía potencial
Sabemos que:.
4
𝐸𝑃 (𝑞) = − ∑ 𝑔𝑇 𝑀𝑖 ̅̅̅̅̅̅̅
𝐶𝑖 (𝑞)
𝑖=1
0
Como: 𝑔𝑇 = ( 0 )
−𝑔𝑜
Resolviendio
𝐸𝑃 (𝑞) = 𝐻1 ∗ 𝑀2 ∗ 𝑔𝑜 + 𝑀4 ∗ 𝑔𝑜 ∗ (𝐻1 + 𝐻3 + 𝑠𝑖𝑛(𝑞3) ∗ 𝑠𝑖𝑛(𝑞4) ∗ (𝐷4 − 𝐻4/2)) + 𝑀1 ∗ 𝑔𝑜

∗ (𝐻1/2 + 𝑞1) + 𝑀3 ∗ 𝑔𝑜 ∗ (𝐻1 − 𝑐𝑜𝑠(𝑞3) ∗ (𝐻3/2 − 𝐿2))
El lagrangeano se define como:
𝐿(𝑞, 𝑞̇ ) = 𝐸𝐶 (𝑞, 𝑞̇ ) − 𝐸𝑃 (𝑞)
𝐿(𝑞, 𝑞̇ ) = (𝑀1 ∗ 𝑑𝑞1^2)/ 2 + 𝑑𝑞4 ∗ ((𝑀4 ∗ 𝑑𝑞4 ∗ (2 ∗ 𝐷4 − 𝐻4)^2)/8 + (𝐶3 ∗ 𝑀4 ∗ 𝑑𝑞2 ∗ (2 ∗ 𝐷4

− 𝐻4)^2)/8) − 𝑑𝑞3 ∗ ((𝑑𝑞3 ∗ ((𝑀4 ∗ (12 ∗ 𝐷4^2 ∗ 𝑐𝑜𝑠(2 ∗ 𝑞4) + 3 ∗ 𝐻4^2 ∗ 𝑐𝑜𝑠(2
∗ 𝑞4) − 12 ∗ 𝐷4^2 − 5 ∗ 𝐻4^2 + 12 ∗ 𝐷4 ∗ 𝐻4 − 12 ∗ 𝐷4 ∗ 𝐻4 ∗ 𝑐𝑜𝑠(2 ∗ 𝑞4)))/24
− (𝑀3 ∗ (𝐻3^2 − 4 ∗ 𝐻3 ∗ 𝐿2 + 4 ∗ 𝐿2^2 + 4 ∗ 𝑅^2))/4))/2 + (𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4
∗ 𝑑𝑞2 ∗ (2 ∗ 𝐷4 − 𝐻4)^2)/8) + 𝑑𝑞2 ∗ ((𝑑𝑞2 ∗ (𝑀2 ∗ 𝑅^2 + (𝑀3 ∗ (3 ∗ 𝐻3^2 ∗ 𝑆3^2
+ 𝐻3^2 − 12 ∗ 𝐻3 ∗ 𝐿2 ∗ 𝑆3^2 + 12 ∗ 𝐿2^2 ∗ 𝑆3^2 + 6 ∗ 𝑅^2))/12 + 𝑀2 ∗ 𝑆2^2
∗ (𝐷2 − 𝐻2/2)^2 + 𝑀4 ∗ (𝐷4 − 𝐻4/2)^2 ∗ (𝐶4 ∗ 𝑆2 + 𝐶2 ∗ 𝐶3 ∗ 𝑆4)^2 + 𝑀4 ∗ (𝐷4
− 𝐻4/2)^2 ∗ (𝐶2 ∗ 𝐶4 − 𝐶3 ∗ 𝑆2 ∗ 𝑆4)^2 + 𝐶2^2 ∗ 𝑀2 ∗ (𝐷2 − 𝐻2/2)^2 + (𝐶4^2
∗ 𝐻4^2 ∗ 𝑀4 ∗ 𝑆3^2)/12 + (𝐻4^2 ∗ 𝑀4 ∗ 𝑆3^2 ∗ 𝑆4^2)/12))/2 + (𝐶3 ∗ 𝑀4 ∗ 𝑑𝑞4 ∗ (2
∗ 𝐷4 − 𝐻4)^2)/8 − (𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ 𝑑𝑞3 ∗ (2 ∗ 𝐷4 − 𝐻4)^2)/8) − 𝑀3 ∗ 𝑔𝑜
∗ (𝐻1 − 𝐶3 ∗ (𝐻3/2 − 𝐿2)) − 𝑀4 ∗ 𝑔𝑜 ∗ (𝐻1 + 𝐻3 + 𝑆3 ∗ 𝑆4 ∗ (𝐷4 − 𝐻4/2)) − 𝐻1
∗ 𝑀2 ∗ 𝑔𝑜 − 𝑀1 ∗ 𝑔𝑜 ∗ (𝐻1/2 + 𝑞1)
Para determinar la dinámica del robot se aplica:

𝑑 𝜕 𝜕
𝐿(𝑞, 𝑞̇ ) − 𝐿(𝑞, 𝑞̇ ) + 𝑏𝑖 (𝑞̇ ) = 𝜏𝑖
𝑑𝑡 𝜕𝑞̇ 𝑖 𝜕𝑞𝑖
La ecuación de energía cinetica se puede expresar

4 4
1
𝐸𝐶 (𝑞, 𝑞̇ ) = ∑ ∑ 𝐷𝑘𝑗 (𝑞) 𝑞̇ 𝑘 𝑞𝑗
2
𝑘=1 𝑗=1
La ecuación de energía potencial se puede expresar

3 4
𝐸𝑝 (𝑞) = − ∑ ∑ 𝑔𝑘 𝑚𝑗 𝑐𝑘𝑗
𝑘=1 𝑗=
Luego el lagrangeano será:

4 4 3 4
1
𝐿(𝑞, 𝑞̇ ) = ∑ ∑ 𝐷𝑘𝑗 (𝑞) 𝑞̇ 𝑘 𝑞𝑗 + ∑ ∑ 𝑔𝑘 𝑚𝑗 𝑐𝑘𝑗
2
𝑘=1 𝑗=1 𝑘=1 𝑗=
Reemplazando y reduciendo:
4 4 4 4 4 3 4
𝜕𝐷𝑖𝑗 (𝑞) 1 𝜕𝐷𝑘𝑗 (𝑞) 𝑖
∑ 𝐷𝑖𝑗 (𝑞)𝑞𝑗̈ + ∑ ∑ 𝑞̇ 𝑘 𝑞̇ 𝑗 − ∑ ∑ 𝑞̇ 𝑘 𝑞̇ 𝑗 + ∑ ∑ 𝑔𝑘 𝑚𝑗 𝐽𝑣𝑘𝑗 + 𝑏𝑖 (𝑞̇ ) = 𝜏𝑖
𝜕𝑞𝑘 2 𝜕𝑞𝑘
𝑗=1 𝑘=1 𝑗=1 𝑘=1 𝑗=1 𝑘=1 𝑗=𝑖
Efectuando en MATLAB:
clear all
clc
%dinamica
syms A go b1 b2 b3 b4 q1 q2 q3 q4 d1 D2 D3 D4 L1 L2 L3 L4 real
syms dq1 dq2 dq3 dq4 d2q1 d2q2 d2q3 d2q4 M1 M2 M3 M4 R H1 H2 H3
H4 real
syms I1xx I1zz I2xx I2zz I3xx I3zz I4xx real
DTJQv=sym('x',[3 4]);
DTJQw=sym('y',[3 4]);
ST=(sym('y',[1 4]));
warning off;
A1=0;A2=L1;A3=L2;A4=0;
a1=0;a2=-90;a3=90;a4=-90;
d1=q1;d2=D2;d3=0;d4=D4;
Q1=0;Q2=q2;Q3=q3;Q4=q4;
T01=[cosd(Q1) -cosd(a1)*sind(Q1) sind(a1)*sind(Q1) A1*cosd(Q1);

sind(Q1) cosd(a1)*cosd(Q1) -sind(a1)*cosd(Q1) A1*sind(Q1);
0 sind(a1) cosd(a1) d1;
0 0 0 1];
0 0 0 1];
0 0 0 1];
0 0 0 1];
X=T01;
X=subs(X,'cos((pi*(q1))/180)','cos(q1)');
X=subs(X,'sin((pi*(q1))/180)','sin(q1)');
T01=X
X=T12;
T12=X
X=T23;
T23=X
X=T34;
T34=X
T02=simplify(T01*T12);
T03=simplify(T01*T12*T23);
T04=simplify(T01*T12*T23*T34);
R01=T01(1:3,1:3);
R02=T02(1:3,1:3);
R03=T03(1:3,1:3);
R04=T04(1:3,1:3);
P01=T01(1:3,4);
P02=T02(1:3,4);
P03=T03(1:3,4);
P04=T04(1:3,4);
I1=[M1*(6*A^2+H1^2)/12 0 0;0 M1*(6*A^2+H1^2)/12 0 ;0 0

M1*A^2/6];
D1T=R01*I1*R01'
C1=[0;0;H1]+R01*[0;0;q1-H1/2]
Jv1=jacobian(C1,[q1 q2 q3 q4])
Jw1=[0 0 0 0;0 0 0 0;0 0 0 0]
D1T=Jv1'*M1*Jv1+Jw1'*D1T*Jw1
I2=[M2*(6*R^2+H2^2)/12 0 0;0 M2*R^2 0 ;0 0 M2*(6*R^2+H2^2)/12];

D2T=R02*I2*R02'
C2=[L1;0;H1]+R02*[0;0;d2-H2/2]
Jv2=jacobian(C2,[q1 q2 q3 q4])
Jw2=[0 0 0 0;0 0 0 0;0 1 0 0]
D2T=Jv2'*M2*Jv2+Jw2'*D2T*Jw2
I3=[M3*(6*R^2+H3^2)/12 0 0;0 M3*R^2 0 ;0 0 M3*(6*R^2+H3^2)/12];

D3T=R03*I3*R03';
C3=[L1;d2;H1]+R03*[0;0;L2-H3/2];
Jv3=jacobian(C3,[q1 q2 q3 q4]);
Jw3=[0 0 -sin(q2) 0;0 0 cos(q2) 0;0 1 0 0];
D3T=simplify(Jv3'*M3*Jv3+Jw3'*D3T*Jw3);
I4=[M4*H4^2/12 0 0;0 0 0 ;0 0 M4*H4^2/12];

D4T1=R04*I4*R04';
C4=[L1;d2;H1+H3]+R04*[0;0;D4-H4/2];
Jv4=jacobian(C4,[q1 q2 q3 q4]);
Jw4=[0 0 -sin(q2) cos(q2)*sin(q3);0 0 cos(q2) sin(q2)*sin(q3);0
1 0 cos(q3)];
D4T=simplify(Jv4'*M4*Jv4+Jw4'*D4T1*Jw4);
I4xx=0
Dtotal=simplify(D1T+D2T+D3T+D4T);
CT=[C1 C2 C3 C4]
Q=[q1;q2;q3;q4];
dQ=[dq1;dq2;dq3;dq4]; %Derivadas de qi
d2Q=[d2q1;d2q2;d2q3;d2q4]; %Derivadas segunda de qi
g=[0 0 go]
%Energia cinetica
Ec=(1/2)*dQ'*Dtotal*dQ
%Energia potencial
Ep=[0 0 go]*M1*C1+[0 0 go]*M2*C2+[0 0 go]*M3*C3+[0 0 go]*M4*C4
%Lagrangiano
L=Ec-Ep
JT=[Jv1 Jv2 Jv3 Jv4]
MT=[M1 M2 M3 M4]
bT=[b1 b2 b3 b4];
for i=1:4
S1=0;S2=0;S3=0;S4=0;SS2=0;SS3=0;SS4=0;
for j=1:4
S1=S1+Dtotal(i,j)*d2Q(j);
end
for k=1:4
S2=S2+SS2;
for j=1:4
SS2=SS2+diff(Dtotal(i,j),Q(k))*dQ(k)*dQ(j);
end
end
for k=1:4
S3=S3+SS3;
for j=1:4
SS3=SS3+diff(Dtotal(k,j),Q(i))*dQ(k)*dQ(j);
end
end
S3=(-1/2)*S3;
for k=1:3
S4=S4+SS4;
for j=i:4
JTI=JT(:,4*i-3:4*i);
SS4=SS4+g(k)*JTI(k,j)*MT(j);
end
end
AUX=simplify(S1+S2+S3+S4+bT(i));
ST(i)=AUX;
end
X=ST;
X=subs(X,'cos(q1)','C1');
X=subs(X,'sin(q1)','S1');
X=subs(X,'cos(2*q1)','C11');
X=subs(X,'sin(2*q1)','S11');
X=subs(X,'cos(2*q2)','C22');
X=subs(X,'sin(2*q2)','S22');
X=subs(X,'cos(2*q3)','C33');
X=subs(X,'sin(2*q3)','S33');
X=subs(X,'cos(2*q4)','C44');
X=subs(X,'sin(2*q4)','S44');
disp('la dinamica esta dada por')
ST=X
𝑏1 + 𝑀1 ∗ 𝑑2𝑞1 = 𝑓1
𝑏2 + 𝑑2𝑞2
𝑀3 ∗ (3 ∗ 𝐻32 ∗ 𝑆32 + 𝐻32 − 12 ∗ 𝐻3 ∗ 𝐿2 ∗ 𝑆32 + 12 ∗ 𝐿22 ∗ 𝑆32 + 6 ∗ 𝑅2 )
∗ (𝑀2 ∗ 𝑅2 +
12
2 2
𝐶2 ∗ 𝑀2 ∗ (2 ∗ 𝐷2 − 𝐻2) 𝑀2 ∗ 𝑆2 ∗ (2 ∗ 𝐷2 − 𝐻2)2
2
+ +
4 4
𝑀4 ∗ (𝐶4 ∗ 𝑆2 + 𝐶2 ∗ 𝐶3 ∗ 𝑆4)2 ∗ (2 ∗ 𝐷4 − 𝐻4)2
+
4
𝑀4 ∗ (𝐶2 ∗ 𝐶4 − 𝐶3 ∗ 𝑆2 ∗ 𝑆4)2 ∗ (2 ∗ 𝐷4 − 𝐻4)2 𝐶42 ∗ 𝐻42 ∗ 𝑀4 ∗ 𝑆32
+ +
4 12
2 2 2
𝐻4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4
+ )
12
𝑆33 ∗ 𝑑𝑞2 ∗ 𝑑𝑞3 ∗ (6 ∗ 𝐻32 ∗ 𝑀3 − 12 ∗ 𝐷42 ∗ 𝑀4 − 𝐻42 ∗ 𝑀4 + 24 ∗ 𝐿22 ∗ 𝑀3 + 12 ∗ 𝐷4 ∗ 𝐻4 ∗ 𝑀4 − 24 ∗ 𝐻3 ∗
+
24
𝐶3 ∗ 𝑀4 ∗ 𝑑2𝑞4 ∗ (2 ∗ 𝐷4 − 𝐻4)2 𝑀4 ∗ 𝑆3 ∗ 𝑑𝑞3 ∗ 𝑑𝑞4 ∗ (2 ∗ 𝐷4 − 𝐻4)2
+ −
4 4
𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ 𝑑2𝑞3 ∗ (2 ∗ 𝐷4 − 𝐻4)2 𝐶3 ∗ 𝐶4 ∗ 𝑀4 ∗ 𝑆4 ∗ 𝑑𝑞32 ∗ (2 ∗ 𝐷4 − 𝐻4)2
− − = 𝜏2
4 4
𝑏3 − 𝑑2𝑞3
𝑀4 ∗ (12 ∗ 𝐶44 ∗ 𝐷42 + 3 ∗ 𝐶44 ∗ 𝐻42 − 12 ∗ 𝐷42 − 5 ∗ 𝐻42 + 12 ∗ 𝐷4 ∗ 𝐻4 − 12 ∗ 𝐶44 ∗ 𝐷4 ∗ 𝐻4)
∗(
24
𝑀3 ∗ (𝐻32 − 4 ∗ 𝐻3 ∗ 𝐿2 + 4 ∗ 𝐿22 + 4 ∗ 𝑅2 )
− )
4
𝑆33 ∗ 𝑑𝑞22 ∗ (6 ∗ 𝐻32 ∗ 𝑀3 − 12 ∗ 𝐷42 ∗ 𝑀4 − 𝐻42 ∗ 𝑀4 + 24 ∗ 𝐿22 ∗ 𝑀3 + 12 ∗ 𝐷4 ∗ 𝐻4 ∗ 𝑀4 − 24 ∗ 𝐻3 ∗ 𝐿2 ∗ 𝑀
−
24
𝑀4 ∗ 𝑆3 ∗ 𝑑𝑞2 ∗ 𝑑𝑞4 ∗ (2 ∗ 𝐷4 − 𝐻4)2 𝐶4 ∗ 𝑀4 ∗ 𝑆3 ∗ 𝑆4 ∗ 𝑑2𝑞2 ∗ (2 ∗ 𝐷4 − 𝐻4)2
+ −
4 4
𝐶3 ∗ 𝐶4 ∗ 𝑀4 ∗ 𝑆4 ∗ 𝑑𝑞2 ∗ 𝑑𝑞3 ∗ (2 ∗ 𝐷4 − 𝐻4)2
+ = 𝜏3
8
𝑀4∗𝑑2𝑞4∗(2∗𝐷4 – 𝐻4)2 𝐶3∗𝑀4∗𝑑2𝑞2∗(2∗𝐷4 – 𝐻4)2 𝑀4∗𝑆44∗𝑑𝑞32 ∗(2∗𝐷4 – 𝐻4)2 𝑀4∗𝑆32 ∗𝑆44∗𝑑𝑞22 ∗(2∗𝐷4 – 𝐻4)2
𝑏4 + + − + −
4 4 8 4
𝑀4∗𝑆3∗𝑑𝑞2∗𝑑𝑞3∗(2∗𝐷4 – 𝐻4)2 3∗𝐶44∗𝑀4∗𝑆3∗𝑑𝑞2∗𝑑𝑞3∗(2∗𝐷4 – 𝐻4)2
+ = 𝜏4
4 8
Donde
𝑏𝑗 = 𝑠𝑔𝑛(𝑞𝑗̇ )𝑏𝑗𝑠 + 𝑏𝑗𝑣 𝑞𝑗̇ 𝑐𝑜𝑛 𝑗 = 1 ,2,3, 4

Compendio2 Rev1

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Compendio2 Rev1

Cargado por

Copyright:

Formatos disponibles

UNIVERSIDAD NACIONAL DEL CALLAO

MAESTRIA EN CIENCIAS DE LA ELECTRÓNICA, MENCIÓN CONTROL Y

CURSO: EB-309 - ROBOTICA

DOCENTE: MG. ING. JOSÉ MACHUCA MINES

INTEGRANTES DEL GRUPO:

Callao, 11 de Noviembre de 2018

1. DEFINICION DEL PROBLEMA

La figura N° 1 muestra la estructura del robot en mención.

Figura N° 1 – Estructura analítica del robot

Objeto Ixx Iyy Izz

𝐶2 𝐶3 −𝑆2 𝐶2 𝑆3 𝐼3𝑥𝑥 0 0 𝐶2 𝐶3 𝐶3 𝑆2 −𝑆3

𝐷333 = (𝑀3 ∗ 𝐻3^2)/4 − 𝑀3 ∗ 𝐻3 ∗ 𝐿2 + 𝑀3 ∗ 𝐿2^2 + 𝐼3𝑧𝑧

Hallamos la matriz jacobiana V:

[ 0, 0, 𝐶3 ∗ 𝑆4 ∗ (𝐷4 − 𝐻4/2), 𝐶4 ∗ 𝑆3 ∗ (𝐷4 − 𝐻4/2)]

Hallamos la matriz jacobiana W:

𝐷444 = (𝑀4 ∗ (2 ∗ 𝐷4 − 𝐻4)^2)/4

𝐷(𝑞) = 𝐷1(𝑞) + 𝐷2(𝑞) + 𝐷3(𝑞) + 𝐷1(𝑞)

𝑝𝑎𝑟𝑎 𝑖 = 𝑗 𝑎𝑐𝑒𝑙𝑒𝑟𝑎𝑐𝑖ó𝑛 𝑐𝑒𝑛𝑡𝑟𝑖𝑝𝑒𝑡𝑎

𝐸𝑃 (𝑞) = 𝐻1 ∗ 𝑀2 ∗ 𝑔𝑜 + 𝑀4 ∗ 𝑔𝑜 ∗ (𝐻1 + 𝐻3 + 𝑠𝑖𝑛(𝑞3) ∗ 𝑠𝑖𝑛(𝑞4) ∗ (𝐷4 − 𝐻4/2)) + 𝑀1 ∗ 𝑔𝑜

El lagrangeano se define como:

𝐿(𝑞, 𝑞̇ ) = 𝐸𝐶 (𝑞, 𝑞̇ ) − 𝐸𝑃 (𝑞)

𝐿(𝑞, 𝑞̇ ) = (𝑀1 ∗ 𝑑𝑞1^2)/ 2 + 𝑑𝑞4 ∗ ((𝑀4 ∗ 𝑑𝑞4 ∗ (2 ∗ 𝐷4 − 𝐻4)^2)/8 + (𝐶3 ∗ 𝑀4 ∗ 𝑑𝑞2 ∗ (2 ∗ 𝐷4

Para determinar la dinámica del robot se aplica:

La ecuación de energía cinetica se puede expresar

La ecuación de energía potencial se puede expresar

Luego el lagrangeano será:

T01=[cosd(Q1) -cosd(a1)*sind(Q1) sind(a1)*sind(Q1) A1*cosd(Q1);

I1=[M1*(6*A^2+H1^2)/12 0 0;0 M1*(6*A^2+H1^2)/12 0 ;0 0

I2=[M2*(6*R^2+H2^2)/12 0 0;0 M2*R^2 0 ;0 0 M2*(6*R^2+H2^2)/12];

I3=[M3*(6*R^2+H3^2)/12 0 0;0 M3*R^2 0 ;0 0 M3*(6*R^2+H3^2)/12];

I4=[M4*H4^2/12 0 0;0 0 0 ;0 0 M4*H4^2/12];

𝑏𝑗 = 𝑠𝑔𝑛(𝑞𝑗̇ )𝑏𝑗𝑠 + 𝑏𝑗𝑣 𝑞𝑗̇ 𝑐𝑜𝑛 𝑗 = 1 ,2,3, 4

También podría gustarte

T01=[cosd(Q1) -cosd(a1)sind(Q1) sind(a1)sind(Q1) A1*cosd(Q1);

I1=[M1(6A^2+H1^2)/12 0 0;0 M1(6A^2+H1^2)/12 0 ;0 0

I2=[M2(6R^2+H2^2)/12 0 0;0 M2R^2 0 ;0 0 M2(6*R^2+H2^2)/12];

I3=[M3(6R^2+H3^2)/12 0 0;0 M3R^2 0 ;0 0 M3(6*R^2+H3^2)/12];

I4=[M4H4^2/12 0 0;0 0 0 ;0 0 M4H4^2/12];