Sei sulla pagina 1di 22

1.

Identificar qué aspectos referentes a la cinemática directa e inversa del robot, sabe y
cuales se desconocen. Seleccionar de los siguientes términos, los desconocidos y
buscar su significado en las referencias del entorno de conocimiento.

Concepto Definición
Se denomina cinemática directa a una técnica usada en
gráficos 3D por computadora, para calcular la posición de
Cinemática directa partes de una estructura articulada a partir de sus componentes
fijas y las transformaciones inducidas por las articulaciones de
la estructura.
En Robótica, la Cinemática inversa (IK) es la técnica que
permite determinar el movimiento de una cadena de
Cinemática inversa
articulaciones para lograr que un actuador final se ubique en
una posición concreta.
La matriz de traslación puede entenderse como movimientos
directos sin cambios de orientación, es decir, mantienen la
Matriz de traslación
forma y el tamaño de las figuras u objetos trasladados, a las
cuales deslizan según el vector
Matriz de rotación. Representa la rotación de θ grados del
plano en sentido anti horario. ... Algebraicamente, una matriz
Matriz de rotación
de rotación es una matriz ortogonal de determinante uno: Las
matrices de rotación son cuadradas y con valores reales.
Se define como matriz de transformación homogénea T a
una matriz de dimensión 4*4 que representa
Matriz de transformación la transformación de un vector de coordenadas homogéneas de
homogénea un sistema de coordenadas a otro. ... que representa la
orientación y posición de un sistema 0'UVW rotado y
trasladado con respecto al sistema de referencia 0XYZ.
Los parámetros Denavit-Hartenberg son los cuatro parámetros
Parámetros Denavit asociados con una convención particular para unir marcos de
Hartemberg referencia a los enlaces de una cadena cinemática espacial o
manipulador de robot

2. INVESTIGAR Y ANALIZAR DETALLADAMENTE, EL MODELO CINEMÁTICO DE UNO DE LAS


SIGUIENTES CONFIGURACIONES DE ROBOTS:

ROBOT CARTESIANO
ROBOT CILÍNDRICO
ROBOT BRAZO ARTICULADO
ROBOT ESFÉRICO
I. GENERALIDADES

II.I Especificaciones Técnicas del Robot


A. Aplicaciones
El robot Hyundai HR015 (ver la Fig.1.) es de un diseño compacto, no debe ser usado en
ambientes altamente explosivos, ni áreas contaminadas por aceite o materiales inflamables.

Este robot facilita la realización de distintas tareas, como son las de soldaduras, montajes,
cortes, fundiciones, dispensación, acabados; funciona además como máquina de carga y
descarga, máquina de operación para manejo de diferentes materiales, Paletizado, pulido,
sellado, entre muchas otras cosas más, las anteriormente nombradas son las aplicaciones más
usadas.

Fig. 1. Robot Hyundai HR015. Obtenida del Manual [1].


B. Dimensiones
El robot HR015 posee un controlador en la cual su dimensión en mm es de 580x990x460,
tiene un peso de 125 Kg (sin transformador), una capacidad de carga de 15 Kg (ver Fig.2),
es similar al HR010L, pero el HR015 lo supera en varias características, se puede visualizar
el diseño general del robot en la Fig. 2. Las dimensiones se mostraran en la sección de
Espacio de trabajo Fig.5.
Fig. 2. Robot Hyundai HR015 – Capacidad de carga. Obtenida del Manual [1].

Los componentes se van a visualizar en la Fig.3., Fig.4. En cuanto a la lista detallada se


muestra en la Tabla1.

Tabla1. Componentes del Robot Hyundai HR015. [1].


N. Componente N. Componente
1 Cuerpo Base 8 H Motor de eje
2 A2 Marco 9 V Motor de eje
3 Segundo Brazo 10 R1 Motor de eje
4 Link 11 B Motor de eje
5 Primer Brazo 12 R2 Motor de eje
6 Muñeca 13 S eje L. Switch
7 S Motor de eje
Fig. 3. Robot Hyundai HR015 – Componentes1. Obtenida del Manual [1].

Fig. 4. Robot Hyundai HR015 – Componentes2. Obtenida del Manual [1].


C. Tipo de Configuración
Según las características físicas que presenta el robot, este pertenece a un tipo de
Configuración Angular, puesto que presenta movimiento rotacional y angular, es un brazo
articulado.

D. Espacio de Trabajo
Las dimensiones que tienen los elementos del manipulador del robot, con los grados de
libertad que son 6 GDL o DOF, definen el espacio de trabajo del robot.

El espacio de trabajo está restringido por los límites de giro, y también el desplazamiento
existentes en cada una de las articulaciones.

En la Fig.5., Fig.6., se puede visualizar el espacio de trabajo que presenta el robot Hyundai
HR015.

Fig. 5. Robot Hyundai HR015 Dimensiones. Obtenida del Manual [1].

Fig. 6. Robot Hyundai HR015 Espacio de Trabajo. Obtenida del Manual [1].
E. Descripción de las Articulaciones
Este robot como ya se había dicho anteriormente presenta una configuración angular (ver
Fig.7), tiene 6 ejes o grados de libertad (GDL o DOF) y por ende 6 articulaciones, a
continuación se describe en la Tabla 2. Ver Fig. 4.

Tabla2. Espacio de Trabajo de Robot. [1].


Brazo - Swivel o 180
Giro (S)
Rango de Brazo - Horizontal +156 hasta
Movimientos (H) 90
(En Grados) Brazo - Vertical +176 hasta
(V) 155
Muñeca – +/-180
Rotación (R2)
Muñeca – Bending +/-140
/ Flexión (B)
Muñeca – +/-400
Twisting / Torsión
(R1)

Fig. 7. Robot Hyundai HR015 - Rango de Movimientos. Obtenida del Manual [1].
II. CINEMÁTICA DIRECTA DEL MANIPULADOR

III.I. Sistemas de Referencias

a. Sistema de referencia articulación 0

Es el primer sistema de coordenadas, se asume que puede ser rotado un ángulo alrededor de
z0, la cual se encuentra de manera vertical.

b. Sistema de referencia articulación 1

Este sistema de coordenadas rota alrededor de z1, después del sistema de referencia
articulación 0, en donde la vertical se tiene en z0, ahora se tiene en y1.

c. Sistema de referencia articulación 2

Este sistema de coordenadas se encuentra de manera que en la vertical esta x2, y la z2 es


similar al anterior, rota también alrededor de z2.

d. Sistema de referencia articulación 3

Este sistema de coordenadas sigue la verticalidad, z3 esta donde antes se encontraba y2, y
y3 se pasa dónde estaba z2; rota alrededor de z3.

e. Sistema de referencia articulación 4

Este sistema de coordenadas horizontalmente pasa de z3 a y4, el cual se puede rotar


alrededor de z4 también, x4 sigue vertical.

f. Sistema de referencia articulación 5

Este sistema de coordenadas cambia con respecto al anterior únicamente que z5 pasa a
ocupar el lugar donde estaba y4 y y5 donde estaba z4.

g. Sistema de referencia articulación 6

Este sistema de coordenadas por ser del efector, es igual al anterior.

A continuación, se realizan los sistemas de referencia sobre el modelo del robot encontrado
en su manual, ver Fig.8.
Fig. 8. Robot Hyundai HR015 Sistemas de Referencias. Obtenida del manual (Contiene algunas
modificaciones propias). [1].

III.II. Visualización General de los Sistemas de Referencias

La visualización de los marcos de referencia se puede apreciar en la Fig.9, el código se


encuentra en los anexos. A continuación:

Fig. 9. Robot Hyundai HR015 modelado con los Sistemas de Referencia. Elaborada en Matlab.
III.III. Tabla de parámetros Denavit Hartenberg

Nota: El ai que se encuentra en la tabla, está basado en las medidas encontradas en el manual
del robot, ver Fig5.

Tabla 3. Parámetros Denavit Hartenberg.


Articulación ai alfa i di Teta i
i
1 240 -90 640 θ1
2 640 0 0 θ2
3 125 -90 0 θ3
4 0 90 458 θ4
5 0 -90 0 θ5
6 0 0 110 θ6

III.IV. Cálculo de las matrices de transformación homogénea de cada articulación

𝑐𝜃1 −𝑐(−90)𝑠𝜃1 𝑠(−90)𝑠𝜃1 240𝑐𝜃1


𝑠𝜃1 𝑐(−90)𝑐𝜃1 −𝑠(−90)𝑐𝜃1 240𝑠𝜃1
0𝐴1 = [ ]
0 𝑠(−90) 𝑐(−90) 640
0 0 0 1

𝑐𝜃2 −𝑐0𝑠𝜃2 𝑠0𝑠𝜃2 640𝑐𝜃1


𝑠𝜃2 𝑐0𝑐𝜃2 −𝑠0𝑐𝜃2 640𝑠𝜃1
1𝐴2 = [ ]
0 𝑠0 𝑐0 0
0 0 0 1

𝑐𝜃3 −𝑐(−90)𝑠𝜃3 𝑠(−90)𝑠𝜃3 125𝑐𝜃3


𝑠𝜃3 𝑐(−90)𝑐𝜃3 −𝑠(−90)𝑐𝜃3 125𝑠𝜃3
2𝐴3 = [ ]
0 𝑠(−90) 𝑐(−90) 0
0 0 0 1

𝑐𝜃4 −𝑐(90)𝑠𝜃4 𝑠(90)𝑠𝜃4 0


𝑠𝜃4 𝑐(90)𝑐𝜃4 −𝑠(90)𝑐𝜃4 0
3𝐴4 = [ ]
0 𝑠(90) 𝑐(90) 458
0 0 0 1

𝑐𝜃5 −𝑐(−90)𝑠𝜃5 𝑠(−90)𝑠𝜃5 0


𝑠𝜃5 𝑐(−90)𝑐𝜃5 −𝑠(−90)𝑐𝜃5 0
4𝐴5 = [ ]
0 𝑠(−90) 𝑐(−90) 0
0 0 0 1
𝑐𝜃6 −𝑐0𝑠𝜃6 𝑠0𝑠𝜃6 0
𝑠𝜃6 𝑐0𝑐𝜃6 −𝑠0𝑐𝜃6 0
5𝐴6 = [ ]
0 𝑠0 𝑐0 110
0 0 0 1

Al hacer la operación queda como resultante:

𝑐𝜃1 0 −𝑠𝜃1 240𝑐𝜃1


𝑠𝜃1 0 𝑐𝜃1 240𝑠𝜃1
0𝐴1 = [ ]
0 −1 0 640
0 0 0 1

𝑐𝜃2 −𝑠𝜃2 0 640𝑐𝜃2


𝑠𝜃2 𝑐𝜃2 0 640𝑠𝜃2
1𝐴2 = [ ]
0 0 1 0
0 0 0 1

𝑐𝜃3 0 −𝑠𝜃3 125𝑐𝜃3


𝑠𝜃3 0 𝑐𝜃3 125𝑠𝜃3
2𝐴3 = [ ]
0 −1 0 0
0 0 0 1

𝑐𝜃4 0 𝑠𝜃4 0
𝑠𝜃4 0 −𝑐𝜃4 0
3𝐴4 = [ ]
0 1 0 458
0 0 0 1

𝑐𝜃5 0 −𝑠𝜃5 0
𝑠𝜃5 0 𝑐𝜃5 0
4𝐴5 = [ ]
0 −1 0 0
0 0 0 1

𝑐𝜃6 −𝑠𝜃6 0 0
𝑠𝜃6 𝑐𝜃6 0 0
5𝐴6 = [ ]
0 0 1 110
0 0 0 1

III.V. Solución al problema cinemático directo

La solución de la cinemática directa del robot de 6 grados de libertad es la siguiente.

0𝑇5 = 0𝐴1 ∗ 1𝐴2 ∗ 2𝐴3 ∗ 3𝐴4 ∗ 4𝐴5 ∗ 5𝐴6


𝑚11 𝑚12 𝑚13 𝑚14
𝑚21 𝑚22 𝑚23 𝑚24
0𝑇6 = [ ]
𝑚31 𝑚32 𝑚33 𝑚34
0 0 0 1

𝑚11 = 𝐶6(𝐶5(𝑆1𝑆4 + 𝐶4𝐶1𝐶23) − 𝑆5𝐶1𝑆23) − 𝑆6(𝑆4𝐶1𝐶23 − 𝐶4𝑆1)


𝑚12 = − 𝑆6(𝐶5(𝑆1𝑆4 + 𝐶4𝐶1𝐶23) − 𝑆5𝐶1𝑆23) − 𝐶6(𝑆4𝐶1𝐶23 − 𝐶4𝑆1)
𝑚13 = − 𝐶5𝐶1𝑆23 − 𝑆5(𝑆1𝑆4 + 𝐶4𝐶1𝐶23)
𝑚14 = 𝐶1(240 − 110𝐶5𝑆23 + 640𝐶2 + 125𝐶23 − 458𝑆23) − 110𝑆5(𝑆1𝑆4 + 𝐶4𝐶1𝐶23)
𝑚21 = 𝐶6(𝐶5(𝐶4𝑆1𝐶23 − 𝐶1𝑆4) − 𝑆5𝑆1𝑆23) − 𝑆6(𝑆4𝑆1𝐶23 + 𝐶1𝐶4)
𝑚22 = − 𝑆6(𝐶5(𝐶4𝑆1𝐶23 − 𝐶1𝑆4) − 𝑆5𝑆1𝑆23) − 𝐶6(𝑆4𝑆1𝐶23 + 𝐶1𝐶4)
𝑚23 = − 𝐶5𝑆1𝑆23 − 𝑆5(𝐶4𝑆1𝐶23 − 𝐶1𝑆4)
𝑚24 = 𝑆1(240 − 110𝐶5𝑆23 + 640𝐶2 + 125𝐶23 – 458𝑆23) − 110𝑆5(𝐶4𝑆1𝐶23 − 𝐶1𝑆4)
𝑚31 = 𝐶6(−𝑆5𝐶23 − 𝐶4𝐶5𝑆23) + 𝑆4𝑆6𝑆23
𝑚32 = 𝐶6𝑆4𝑆23 − 𝑆6(−𝑆5𝐶23 − 𝐶4𝐶5𝑆23)
𝑚33 = −𝐶5𝐶23 + 𝐶4𝑆5𝑆23
𝑚34 = −𝐶23(458 + 110𝐶5) – 640(𝑆2 − 1) + 𝑆23( 110𝐶4𝑆5– 125)

Al colocar los diferentes valores en la matriz queda así:

III.VI. Ejemplo de aplicación

Se van a tomar los ángulos mencionados en la Tabla 4, y a través de la cinemática directa se


van a conocer las posiciones finales de Px, Py, Pz, el cual hace una trayectoria. Ver Fig. 10.

Tabla 4. Ejemplo de aplicación de Cinemática Directa


T1 T2 T3 T4 T5 T6 PX PY PZ
20 30 1 5 8 22 -2,8219 700,9582 1,2787
20 30 2 5 8 22 -13,588 696,8787 1,2787
20 30 3 5 8 22 -24,2812 692,612 1,2787
20 30 4 5 8 22 -34,8983 688,1592 1,2787
20 30 5 5 8 22 -45,4361 683,5219 1,2787
20 30 6 5 8 22 -55,8914 678,7014 1,2787
20 30 7 5 8 22 -66,2609 673,6991 1,2787
20 30 8 5 8 22 -76,5416 668,5166 1,2787
20 30 9 5 8 22 -86,7302 663,1555 1,2787
20 30 10 5 8 22 -96,8237 657,6173 1,2787
20 30 11 5 8 22 -106,819 651,9039 1,2787
20 30 12 5 8 22 -116,713 646,0169 1,2787
20 30 13 5 8 22 -126,503 639,9581 1,2787
20 30 14 5 8 22 -136,1856 633,7294 1,2787
20 30 15 5 8 22 -145,758 627,3326 1,2787
20 30 16 5 8 22 -155,2174 620,7698 1,2787
20 30 17 5 8 22 -164,5607 614,0428 1,2787
20 30 18 5 8 22 -173,7853 607,1538 1,2787
20 30 19 5 8 22 -182,8882 600,1049 1,2787
20 30 20 5 8 22 -191,8667 592,8982 1,2787
20 30 21 5 8 22 -200,718 585,5359 1,2787
20 30 22 5 8 22 -209,4395 578,0202 1,2787
20 30 23 5 8 22 -218,0285 570,3535 1,2787
20 30 24 5 8 22 -226,4825 562,538 1,2787
20 30 25 5 8 22 -234,7987 554,5762 1,2787
20 30 26 5 8 22 -242,9747 546,4705 1,2787
20 30 27 5 8 22 -251,008 538,2233 1,2787
20 30 28 5 8 22 -258,8961 529,8371 1,2787
20 30 29 5 8 22 -266,6367 521,3146 1,2787
20 30 30 5 8 22 -274,2273 512,6583 1,2787
20 30 31 5 8 22 -281,6658 503,8708 1,2787
20 30 32 5 8 22 -288,9497 494,9549 1,2787
20 30 33 5 8 22 -296,0769 485,9131 1,2787
20 30 34 5 8 22 -303,0453 476,7484 1,2787
20 30 35 5 8 22 -309,8526 467,4634 1,2787
20 30 36 5 8 22 -316,4968 458,0611 1,2787
20 30 37 5 8 22 -322,976 448,5442 1,2787
20 30 38 5 8 22 -329,288 438,9157 1,2787
20 30 39 5 8 22 -335,4311 429,1785 1,2787
20 30 40 5 8 22 -341,4033 419,3356 1,2787
20 30 41 5 8 22 -347,2028 409,39 1,2787
20 30 42 5 8 22 -352,8278 399,3446 1,2787
20 30 43 5 8 22 -358,2766 389,2026 1,2787
20 30 44 5 8 22 -363,5477 378,9671 1,2787
20 30 45 5 8 22 -368,6393 368,6411 1,2787
20 30 46 5 8 22 -373,5499 358,2279 1,2787
20 30 47 5 8 22 -378,278 347,7305 1,2787
20 30 48 5 8 22 -382,8222 337,1522 1,2787
20 30 49 5 8 22 -387,1811 326,4962 1,2787
20 30 50 5 8 22 -391,3533 315,7658 1,2787
20 30 51 5 8 22 -395,3377 304,9641 1,2787
20 30 52 5 8 22 -399,1329 294,0946 1,2787
20 30 53 5 8 22 -402,7378 283,1601 1,2787
20 30 54 5 8 22 -406,1514 272,1652 1,2787
20 30 55 5 8 22 -409,3725 261,1119 1,2787
20 30 56 5 8 22 -412,4003 250,0042 1,2787
20 30 57 5 8 22 -415,2337 238,8452 1,2787
20 30 58 5 8 22 -417,872 227,6385 1,2787
20 30 59 5 8 22 -420,3142 216,3875 1,2787
20 30 60 5 8 22 -422,5598 205,0956 1,2787
20 30 61 5 8 22 -424,6079 193,7662 1,2787
20 30 62 5 8 22 -426,458 182,4028 1,2787
20 30 63 5 8 22 -428,1095 171,0088 1,2787
20 30 64 5 8 22 -429,5619 159,5878 1,2787
20 30 65 5 8 22 -430,8147 148,1431 1,2787
20 30 66 5 8 22 -431,8676 136,6783 1,2787
20 30 67 5 8 22 -432,7203 125,1969 1,2787
20 30 68 5 8 22 -433,3725 113,7023 1,2787
20 30 69 5 8 22 -433,8239 102,1981 1,2787
20 30 70 5 8 22 -434,0745 90,6878 1,2787
20 30 71 5 8 22 -434,1242 79,1749 1,2787
20 30 72 5 8 22 -433,9729 67,6629 1,2787
20 30 73 5 8 22 -433,6208 56,1552 1,2787
20 30 74 5 8 22 -433,0679 44,6555 1,2787
20 30 75 5 8 22 -432,3143 33,1671 1,2787
20 30 76 5 8 22 -431,3604 21,6937 1,2787
20 30 77 5 8 22 -430,2064 10,2386 1,2787
20 30 78 5 8 22 -428,8526 -1,1946 1,2787
20 30 79 5 8 22 -427,2995 -13 1,2787
20 30 80 5 8 22 -425,5476 -23,9813 1,2787
20 30 81 5 8 22 -423,5973 -35,328 1,2787
20 30 82 5 8 22 -421,4493 -46,6389 1,2787
20 30 83 5 8 22 -419,1042 -57,9105 1,2787
20 30 84 5 8 22 -416,5628 -69,1396 1,2787
20 30 85 5 8 22 -413,8258 -80,3225 1,2787
20 30 86 5 8 22 -410,894 -91,456 1,2787
20 30 87 5 8 22 -407,7684 -102,5367 1,2787
20 30 88 5 8 22 -404,4498 -113,5611 1,2787
20 30 89 5 8 22 -400,9394 -124,5259 1,2787
20 30 90 5 8 22 -397,2381 -135,4277 1,2787

Fig. 10. Ejemplo de aplicación para el Robot Hyundai HR015. Elaborada en Matlab.

III. CINEMÁTICA INVERSA DEL MANIPULADOR

IV.I. Cálculo de los ángulos de articulación

a. Cálculo de θ1

𝑐1 𝑠1 0 −240
0 0 −1 640
(0𝐴1 )−1 =[ ]
−𝑠1 𝑐1 0 0
0 0 0 1
𝑝𝑥
𝑝𝑦
0𝑝 = [ ]
𝑝𝑧
1

𝑐23 0 −𝑠23 640𝑐2 + 125𝑐23


𝑠23 0 𝑐23 640𝑠2 + 125𝑠23
1𝐴3 = [ ]
0 −1 0 0
0 0 0 1

0
0
3𝑝 = [ ]
𝑑4 = 458
1

𝑐1 𝑠1 0 −240 𝑝𝑥 𝑐23 0 −𝑠23 640𝑐2 + 125𝑐23 0


0 0 −1 640 𝑝𝑦 𝑠23 0 𝑐23 640𝑠2 + 125𝑠23 0
[ ][ ] = [ ][ ]
−𝑠1 𝑐1 0 0 𝑝𝑧 0 −1 0 0 𝑑4 = 458
0 0 0 1 1 0 0 0 1 1

𝑐1𝑝𝑥 + 𝑠1𝑝𝑦 − 240 640𝑐2 + 125𝑐23 − 458𝑠23


640 − 𝑝𝑧 640𝑠2 + 458𝑐23 + 125𝑠23
[ ]=[ ]
𝑐1𝑝𝑦 − 𝑠1𝑝𝑥 0
1 1

1) 𝑐1𝑝𝑥 + 𝑠1𝑝𝑦 − 240 = 640𝑐2 + 125𝑐23 − 458𝑠23

2) 640 − 𝑝𝑧 = 640𝑠2 + 458𝑐23 + 125𝑠23

3) 𝑐1𝑝𝑦 − 𝑠1𝑝𝑥 = 0

b. Cálculo de θ3

Al elevar las ecuaciones 1), 2) y 3) al cuadrado se obtiene:

1) (𝑐1𝑝𝑥 + 𝑠1𝑝𝑦 − 240)2 = (640𝑐2 + 125𝑐23 − 458𝑠23)2

1) 𝑐12 𝑝𝑥 2 + 𝑠12 𝑝𝑦 2 + 57600 + 2𝑐1𝑝𝑥𝑠1𝑝𝑦 − 480𝑐1𝑝𝑥 − 480𝑠1𝑝𝑦


= 409600𝑐22 + 15625𝑐232 + 209764𝑠232 +
160000𝑐2𝑐23 − 586240𝑐2𝑠23 − 114500𝑐23𝑠23

2) (640 − 𝑝𝑧)2 = (640𝑠2 + 458𝑐23 + 125𝑠23)2


2) 409600 − 1280 + 𝑝𝑧 2 = 409600𝑠22 + 209764𝑐232 +
15625𝑠232 + 586240𝑠2𝑐23 + 160000𝑠2𝑠23 + 114500𝑐23𝑠23

3) (𝑐1𝑝𝑦 − 𝑠1𝑝𝑥)2 = 0
3) 𝑐12 𝑝𝑦 2 − 2𝑐1𝑝𝑦𝑠1𝑝𝑥 + 𝑠12 𝑝𝑥 2 = 0

Al sumarlas:

𝑝𝑥 2 + 𝑝𝑦 2 + 𝑝𝑧 2 + 465920 − 480(𝑐1𝑝𝑥 + 𝑠1𝑝𝑦)


= 634989 + 160000𝑐3 − 586240𝑠3

𝑝𝑥 2 + 𝑝𝑦 2 + 𝑝𝑧 2 − 480(𝑐1𝑝𝑥 + 𝑠1𝑝𝑦) − 169069 = 160000𝑐3 − 586240𝑠3

𝑘1 = 𝑝𝑥 2 + 𝑝𝑦 2 + 𝑝𝑧 2 − 480(𝑐1𝑝𝑥 + 𝑠1𝑝𝑦) − 169069


𝑘2 = 160000
𝑘3 = 586240

𝑘1 = 𝑘2𝑐3 − 𝑘3𝑠3

1 − 𝑡3
𝑐3 =
1 + 𝑡3
2𝑡 3
𝑠3 =
1 + 𝑡3
𝜃3
𝑡3 = 𝑡𝑎𝑛
2

1 − 𝑡3 2𝑡 3
𝑘1 = 𝑘2( ) − 𝑘3( )
1 + 𝑡3 1 + 𝑡3
𝑘1(1 + 𝑡 3 ) = 𝑘2(1 − 𝑡 3 ) − 𝑘3(2𝑡 3 )

𝑘1(1 + 𝑡 3 ) − 𝑘2(1 − 𝑡 3 ) + 𝑘3(2𝑡 3 ) = 0

𝑡32 (𝑘1 + 𝑘2) + 2𝑘3𝑡3 + (𝑘1 − 𝑘2) = 0

𝑘𝑥 = 𝑘1 + 𝑘2

𝑘𝑦 = 𝑘1 − 𝑘2

𝜃3 −2𝑘3 ± √4𝑘32 − 4𝑘𝑥𝑘𝑦


=
2 2𝑘𝑥

c. Cálculo de θ2

𝑐1𝑝𝑥 + 𝑠1𝑝𝑦 − 240 =


𝑐2(125𝑐3 − 458𝑠3 + 640) − 𝑠2(125𝑠3 + 458𝑐3)

640 − 𝑝𝑧 =
𝑐2(458𝑐3 + 125𝑠3) + 𝑠2(640 + 125𝑐3 − 458𝑠3)

d. Cálculo de θ5, θ4, θ6

𝑐1𝑐23 𝑠1𝑐23 −𝑠23 −64000𝑐3 − 24000𝑐23 + 51500


𝑠1 −𝑐1 0 0
(0𝐴3 )−1 =[ ]
−𝑐1𝑠23 −𝑠1𝑠23 −𝑐23 640𝑠3 − 640𝑐23 + 240𝑠23
0 0 0 1
𝑟33 = −𝑊𝑧𝐶23 − 𝑊𝑥𝐶1𝑆23– 𝑊𝑦𝑆1𝑆23

𝑟23 = 𝑆1𝑊𝑥 − 𝐶1 ∗ 𝑊𝑦

𝑟13 = 𝑊𝑥𝐶1𝐶23 + 𝑊𝑦𝑆1𝐶23 − 𝑊𝑧𝑆23

𝑟32 = −𝑉𝑧𝐶23 − 𝑉𝑥𝐶1𝑆23 – 𝑉𝑦𝑆1𝑆23

𝑟31 = −𝑈𝑧𝐶23 − 𝑈𝑥𝐶1𝑆23 − 𝑆1𝑈𝑦𝑆23

IV.II. Trayectoria para aplicar cinemática inversa

El código de este se puede encontrar en los Anexos.


IV. CINEMÁTICA DIFERENCIAL DEL MANIPULADOR

V.I. Cálculo del Jacobiano

a. Cálculo del Jacobiano

Nota: En los Anexos se pueden encontrar los desarrollos matemáticos de las diferentes
derivadas propuestas para el jacobiano, además de sus resultados para las matrices
correspondientes.

Se retoma a la matriz calculada en la cinemática directa anteriormente, en la cual corresponde


a .

𝑚11 𝑚12 𝑚13 𝑚14


𝑚21 𝑚22 𝑚23 𝑚24
0𝑇6 = [ ]
𝑚31 𝑚32 𝑚33 𝑚34
0 0 0 1

Se calculan las velocidades lineales derivando la matriz de traslación inmersa dentro de la


anterior matriz en las posiciones qx, qy y qz como se presenta a continuación.

𝑞𝑥 = 𝐶1(240 − 110𝐶5𝑆23 + 640𝐶2 + 125𝐶23 − 458𝑆23)


− 110𝑆5(𝑆1𝑆4 + 𝐶4𝐶1𝐶23)

𝑞𝑦 = 240𝑆1 − 110𝐶5(𝐶2𝑆1𝑆3 + 𝐶3𝑆1𝑆2) − 110𝑆5(𝐶4


(𝐶2𝐶3𝑆1 − 𝑆1𝑆2𝑆3) − 𝐶1𝑆4) + 640𝐶2𝑆1 + 125𝐶2𝐶3𝑆1
− 458𝐶2𝑆1𝑆3 − 458𝐶3𝑆1𝑆2 − 125𝑆1𝑆2𝑆3

𝑞𝑧 = 458𝑆2𝑆3 − 640𝑆2 − 458𝐶2𝐶3 + 110𝐶5(𝑆2𝑆3 − 𝐶2𝐶3)


− 125𝐶2𝑆3 − 125𝐶3𝑆2 + 110𝐶4𝑆5(𝐶2𝑆3 + 𝐶3𝑆2) + 640
Luego se calcula la velocidad angular realizando las siguientes derivadas de la matriz de
rotación inmersa en la matriz presentada inicialmente. Las operaciones se muestran a
continuación:

Del resultado de la matriz anterior se encuentran los valores de la matriz de velocidad angular
así:
6 6 6
𝜕𝑢𝑦 𝑑𝜃𝑖 𝜕𝑣𝑦 𝑑𝜃𝑖 𝜕𝑤𝑦 𝑑𝜃𝑖
(∑ ) 𝑢𝑥 + (∑ ) 𝑢𝑦 + (∑ ) 𝑢𝑧 = 𝜔𝑧
𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡
𝑖=1 𝑖=1 𝑖=1

6 6 6
𝜕𝑢𝑥 𝑑𝜃𝑖 𝜕𝑣𝑥 𝑑𝜃𝑖 𝜕𝑤𝑥 𝑑𝜃𝑖
(∑ ) 𝑣𝑥 + (∑ ) 𝑣𝑦 + (∑ ) 𝑣𝑧 = −𝜔𝑧
𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡
𝑖=1 𝑖=1 𝑖=1

6 6 6
𝜕𝑢𝑥 𝑑𝜃𝑖 𝜕𝑣𝑥 𝑑𝜃𝑖 𝜕𝑤𝑥 𝑑𝜃𝑖
(∑ ) 𝑤𝑥 + (∑ ) 𝑤𝑦 + (∑ ) 𝑤𝑧 = 𝜔𝑦
𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡
𝑖=1 𝑖=1 𝑖=1

6 6 6
𝜕𝑢𝑧 𝑑𝜃𝑖 𝜕𝑣𝑧 𝑑𝜃𝑖 𝜕𝑤𝑧 𝑑𝜃𝑖
(∑ ) 𝑢𝑥 + (∑ ) 𝑢𝑦 + (∑ ) 𝑢𝑧 = −𝜔𝑦
𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡
𝑖=1 𝑖=1 𝑖=1

6 6 6
𝜕𝑢𝑧 𝑑𝜃𝑖 𝜕𝑣𝑧 𝑑𝜃𝑖 𝜕𝑤𝑧 𝑑𝜃𝑖
(∑ ) 𝑣𝑥 + (∑ ) 𝑣𝑦 + (∑ ) 𝑣𝑧 = 𝜔𝑥
𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡
𝑖=1 𝑖=1 𝑖=1

6 6 6
𝜕𝑢𝑦 𝑑𝜃𝑖 𝜕𝑣𝑦 𝑑𝜃𝑖 𝜕𝑤𝑦 𝑑𝜃𝑖
(∑ ) 𝑤𝑥 + (∑ ) 𝑤𝑦 + (∑ ) 𝑤𝑧 = −𝜔𝑥
𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡 𝜕𝜃𝑖 𝑑𝑡
𝑖=1 𝑖=1 𝑖=1

Finalmente se puede escribir la matriz del Jacobiano.

𝜕𝑓𝑥 𝜕𝑓𝑥 𝜕𝑓𝑥 𝜕𝑓𝑥 𝜕𝑓𝑥 𝜕𝑓𝑥


𝜕𝜃1 𝜕𝜃2 𝜕𝜃3 𝜕𝜃4 𝜕𝜃5 𝜕𝜃6 𝜃1̇
𝑥̇
𝜕𝑓𝑦 𝜕𝑓𝑦 𝜕𝑓𝑦 𝜕𝑓𝑦 𝜕𝑓𝑦 𝜕𝑓𝑦
𝑦̇ 𝜃2̇
𝑧̇ 𝜕𝜃1 𝜕𝜃2 𝜕𝜃3 𝜕𝜃4 𝜕𝜃5 𝜕𝜃6 𝜃3̇
𝜔̇𝑥 = 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜕𝑓𝑧
𝜃4̇
𝜔𝑦̇ 𝜕𝜃1 𝜕𝜃2 𝜕𝜃3 𝜕𝜃4 𝜕𝜃5 𝜕𝜃6
𝜔𝑥(𝜃1̇ ) 𝜔𝑥(𝜃2̇ ) 𝜔𝑥(𝜃3̇ ) 𝜔𝑥(𝜃4̇ ) 𝜔𝑥(𝜃5̇ ) 𝜔𝑥(𝜃6̇ ) 𝜃5̇
[𝜔̇𝑧 ] [𝜃6̇ ]
𝜔𝑦(𝜃1̇ ) 𝜔𝑦(𝜃2̇ ) 𝜔𝑦(𝜃3̇ ) 𝜔𝑦(𝜃4̇ ) 𝜔𝑦(𝜃5̇ ) 𝜔𝑦(𝜃6̇ )
[𝜔𝑧(𝜃1̇ ) 𝜔𝑧(𝜃2̇ ) 𝜔𝑧(𝜃3̇ ) 𝜔𝑧(𝜃4̇ ) 𝜔𝑧(𝜃5̇ ) 𝜔𝑧(𝜃6̇ ) ]

b. Cálculo del Jacobiano inverso


−1
𝜕𝑓𝑥 𝜕𝑓𝑥 𝜕𝑓𝑥 𝜕𝑓𝑥 𝜕𝑓𝑥 𝜕𝑓𝑥
𝜃1̇ 𝜕𝜃1 𝜕𝜃2 𝜕𝜃3 𝜕𝜃4 𝜕𝜃5 𝜕𝜃6
𝑥̇
𝜕𝑓𝑦 𝜕𝑓𝑦 𝜕𝑓𝑦 𝜕𝑓𝑦 𝜕𝑓𝑦 𝜕𝑓𝑦
𝜃2̇ 𝑦̇
𝜃3̇ 𝜕𝜃1 𝜕𝜃2 𝜕𝜃3 𝜕𝜃4 𝜕𝜃5 𝜕𝜃6 𝑧̇
= 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜕𝑓𝑧 𝜔𝑥̇
𝜃4̇
𝜕𝜃1 𝜕𝜃2 𝜕𝜃3 𝜕𝜃4 𝜕𝜃5 𝜕𝜃6 𝜔𝑦̇
𝜃5̇ 𝜔𝑥(𝜃1̇ ) 𝜔𝑥(𝜃2̇ ) 𝜔𝑥(𝜃3̇ ) 𝜔𝑥(𝜃4̇ ) 𝜔𝑥(𝜃5̇ ) 𝜔𝑥(𝜃6̇ )
[𝜃6̇ ] [ 𝜔̇𝑧 ]
𝜔𝑦(𝜃1̇ ) 𝜔𝑦(𝜃2̇ ) 𝜔𝑦(𝜃3̇ ) 𝜔𝑦(𝜃4̇ ) 𝜔𝑦(𝜃5̇ ) 𝜔𝑦(𝜃6̇ )
[ 𝜔𝑧(𝜃1̇ ) 𝜔𝑧(𝜃2̇ ) 𝜔𝑧(𝜃3̇ ) 𝜔𝑧(𝜃4̇ ) 𝜔𝑧(𝜃5̇ ) 𝜔𝑧(𝜃6̇ ) ]

Potrebbero piacerti anche