Está en la página 1de 64

MODELADO GEOMÉTRICO DE LA INTERFAZ HÁPTICA GEOMAGIC® TOUCH ™

Presentado por:

Lusvin Javier Amado Forero

Trabajo de la Asignatura:

Sistemas Robóticos en Medicina

Docente:

Mg. SERGIO ALEXANDER SALINAS Ingeniero Electrónico

UNIVERSIDAD PONTIFICIA BOLIVARIANA SECCIONAL BUCARAMANGA

2017

CONTENIDO

1. Descripción del robot

2. Estructura geométrica

2.1. Esquema geométrico

2.2. Tabla de parámetros geométricos

3. Modelo geométrico

3.1. Modelo geométrico Directo

3.2. Modelo geométrico Inverso

4. Prueba de los modelos geométricos en Simulink

4.1. Pruebas en Simulink de los Modelos Geométricos

5. Tabla de parámetros inerciales

5.1. Calculo de los parámetros inerciales de las 3 primeras articulaciones

5.2. Tabla de parámetros inerciales completa

6. Piezas en SolidWorks del Robot

6.1. Diseño de piezas en Solidworks del robot

6.2. Tablas con los valores de las masas y parámetros de cada pieza del robot

7. Modelos Dinámicos

7.1.

Archivos en Matlab del Modelo dinámico Inverso

7.1.

Archivos en Matlab del Modelos dinámico Directo

8. Trayectoria de Simulación

8.1. Concepto de la colecistectomía laparoscópica

8.2. Trayectoria para la simulación

9. Bibliografía

1. Descripción del robot

La interfaz háptica geomagic® touch™ (ver figura 1) es un dispositivo háptico profesional de gama media (entre los menos costoso de la 3D Systems). Este dispositivo otorga al usuario una retroalimentación de fuerza, lo cual permite sentir objetos virtuales y producir sensaciones táctiles reales a medida que el usuario manipula objetos 3D en pantalla o en una interfaz gráfica[1] .

Es importante mencionar que este dispositivo otorgar una posición espacial en 3D (a lo largo de los ejes X, Y y Z) y una orientación de giro, inclinación y dirección del lápiz de mano (Ver figura 2). Esto es posible en función de su modelo, el cual le proporciona 6 grados de libertad (DOF). Por lo cual es utilizado en diversas aplicaciones tales como: la simulación, el entrenamiento, la evaluación de habilidades, ensayos, ensamblaje virtual, control robótico, detección de colisiones, diseño de interfaz de máquina, rehabilitación

etc.[1].

Figura 1. Interfaz háptica Geomagic® Touch™ La Geomagic® Touch™ requiere ciertos componentes mínimos de

Figura 1. Interfaz háptica Geomagic® Touch™

La Geomagic® Touch™ requiere ciertos componentes mínimos de hardware y software para poder funcionar correctamente como un dispositivo de transmisión en tiempo real. Los cuales se describen a continuación:

Procesador Intel® Core™ 2 Dúo.

Puerto RJ45, Ethernet y USB

Sistema operativo Windows XP, Windows 7 o Windows 8.

Memoria de 2 GB

2 Dúo. • Puerto RJ45, Ethernet y USB • Sistema operativo Windows XP, Windows 7 o
2 Dúo. • Puerto RJ45, Ethernet y USB • Sistema operativo Windows XP, Windows 7 o

Figura 2. Límites físicos del dispositivo

Las principales características de la interfaz háptica geomagic® touch™ se presentan a continuación en la tabla N°1:

Área de trabajo

~ 6,4 An x 4,8 Al x 2,8 P pulg.

~ 160 An x 120 Al x 70 P mm

Rango de movimiento

Movimiento de la mano con giro de la muñeca

Resolución de la posición nominal

> 450 dpi ~ 0,055 mm

Fuerza de esfuerzo máxima y valor par en la posición nominal

0,75 lbf/3,3 N

Rigidez

Eje X > 7,3 lb/pulg. (1,26 N/mm) Eje Y > 13,4 lb/pulg. (2,31 N/mm) Eje Z > 5,9 lb/pulg. (1,02 N/mm)

Retroalimentación de fuerza (6 grados de libertad)

x, y, z

Entrada/sensor de posición (6 grados de libertad)

x, y, z (codificadores digitales) [Giro, inclinación, dirección (± 5% de potenciómetros de linealidad)]

Interfaz

Puerto IEEE 1394 FireWire®: 6 pines a 6 pines

Tabla N°1. Características técnicas

2. Estructura geométrica

La interfaz Geomagic® Touch™ presenta una estructura establecida en la patente denominada “FORCE REFLECTING HAPTIC INTERFACEcon número US007411576B2 del 12 de agosto del 2008 (Ver figura 3).[3]

Figura 3. Diagrama de la patente US007411576B2 2.1. Esquema geométrico 2.2. Tabla de parámetros geométricos.

Figura 3. Diagrama de la patente US007411576B2

2.1. Esquema geométrico

de la patente US007411576B2 2.1. Esquema geométrico 2.2. Tabla de parámetros geométricos. J σ α d

2.2. Tabla de parámetros geométricos.

J

σ

α

d

θ

r

1

0

0

0

q1

0

2

0

-90

0

q2

0

3

0

0

D3

q3

0

4

0

90

0

q4

R4

5

0

-90

0

q5

0

6

0

90

0

q6

0

0 D3 q3 0 4 0 90 0 q4 R4 5 0 -90 0 q5 0

3. Modelo geométrico

3.1. Tabla de parámetros en Matlab

clc; clear all; close all; syms q1 q2 q3 q4 q5 q6 D3 R4;

 

%

sigma

alpha

d

theta

r

tabla = [

0

0

0

q1

0;

0

-pi/2

0

q2

0;

0

0

D3

q3

0;

0

pi/2

0

q4

R4;

0

-pi/2

0

q5

0;

0

pi/2

0

q6

0];

3.2. Hallando cada una de las matrices

Para realizar el cálculo de matrices con el fin de hallar el MGD se utilizó la función transf (Ver anexo 1) suministrada por el Profesor Sergio Alexander Salinas.

T01=transf(tabla,0,1);

T12=transf(tabla,1,2);

T23=transf(tabla,2,3);

T34=transf(tabla,3,4);

T45=transf(tabla,4,5);

T56=transf(tabla,5,6);

3.3. Hallando el modelo geométrico Directo:

0 T 1 =

cos(q1)

-sin(q1)

0

0

sin(q1)

cos(q1)

0

0

0

0

1

0

0

0

0

1

1

T

2 =

 

cos(q2)

-sin(q2)

0

0

0

0

1

0

-sin(q2)

-cos(q2)

1

0

0

0

0

1

2

T

3 =

 

cos(q3)

-sin(q3)

0

D3

sin(q3)

cos(q3)

0

0

0

0

1

0

0

0

0

1

3

T

4 =

 

cos(q4)

-sin(q4)

0

0

0

0

-1

R4

sin(q4)

cos(q4)

0

0

0

0

0

1

4

T

5 =

 

cos(q5)

-sin(q5)

0

0

0

0

1

0

-sin(q5)

-cos(q5)

0

0

0

0

0

1

5

T

6 =

 

cos(q6)

-sin(q6)

0

0

0

0

-1

0

sin(q6)

cos(q6)

0

0

0

0

0

1

MGD=transf(tabla,0,6);

%Cálculo de matrices

% U4=simplify(T45*T56);

% U3=simplify(T34*U4);

% U2=simplify(T23*U3);

% U1=simplify(T12*U2);

% U0=simplify(T01*U1);

Modelo Geométrico Directo obtenido en Matlab

MGD =

[ - sin(q6)*(cos(q4)*sin(q1) + cos(q2 + q3)*cos(q1)*sin(q4)) - cos(q6)*(cos(q5)*(sin(q1)*sin(q4) - cos(q2 + q3)*cos(q1)*cos(q4)) + sin(q2 + q3)*cos(q1)*sin(q5)), sin(q6)*(cos(q5)*(sin(q1)*sin(q4) - cos(q2 + q3)*cos(q1)*cos(q4)) + sin(q2 + q3)*cos(q1)*sin(q5)) - cos(q6)*(cos(q4)*sin(q1) + cos(q2 + q3)*cos(q1)*sin(q4)), sin(q2 + q3)*cos(q1)*cos(q5) - sin(q5)*(sin(q1)*sin(q4) - cos(q2 + q3)*cos(q1)*cos(q4)), cos(q1)*(R4*sin(q2 + q3) + D3*cos(q2))]

[ sin(q6)*(cos(q1)*cos(q4) - cos(q2 + q3)*sin(q1)*sin(q4)) + cos(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) - sin(q2 + q3)*sin(q1)*sin(q5)),

cos(q6)*(cos(q1)*cos(q4) - cos(q2 + q3)*sin(q1)*sin(q4)) - sin(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) - sin(q2 + q3)*sin(q1)*sin(q5)),

sin(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) + sin(q2 + q3)*cos(q5)*sin(q1), sin(q1)*(R4*sin(q2 + q3) + D3*cos(q2))]

[ sin(q2 + q3)*sin(q4)*sin(q6) - cos(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)), sin(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)) + sin(q2 + q3)*cos(q6)*sin(q4), cos(q2 + q3)*cos(q5) - sin(q2 + q3)*cos(q4)*sin(q5), R4*cos(q2 + q3) - D3*sin(q2)]

[ 0,

0,

0,

1]

3.4.

Hallando el Modelo Geométrico Inverso

syms

sx

sy

sz

nx

ny

nz

ax ay az

px py

pz

Uo = [ sx

nx

ax

px;

sy

ny

ay

py;

sz

nz

az

pz;

 

0

0

0

1];

3.4.1. Usando método de Paul

%Cálculo de matriz T10

T10=simplify(inv(T01));

U1 = T10*Uo;

T16=transf(tabla,1,6);

%Cálculo de matriz T21

T21=simplify(inv(T12));

U2 = T21*U1;

T26=transf(tabla,2,6);

%Cálculo de matriz T32

T32=simplify(inv(T23));

U3=T32*U2;

T36=transf(tabla,3,6);

%Cálculo de matriz T34

T34=simplify(inv(T34));

U4=T34*U3;

T46=transf(tabla,4,6);

%Cálculo de matriz T45

T45=simplify(inv(T45));

U5=T45*U4;

T56=transf(tabla,5,6);

Teniendo como resultado las siguientes matrices en Matlab

T10 =

[ cos(q1),

sin(q1),

0,

0]

[ -sin(q1),

cos(q1),

0,

0]

[

0,

0,

1,

0]

[

0,

0,

0,

1]

U1 =

[

sx*cos(q1) + sy*sin(q1),

nx*cos(q1) + ny*sin(q1),

ax*cos(q1) + ay*sin(q1),

px*cos(q1) + py*sin(q1)]

[

sy*cos(q1) - sx*sin(q1),

ny*cos(q1) - nx*sin(q1),

ay*cos(q1) - ax*sin(q1),

py*cos(q1) - px*sin(q1)]

[

sz,

nz,

az,

pz]

[

0,

0,

0,

1]

T16 =

 

[

- cos(q6)*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5)) - cos(q2 + q3)*sin(q4)*sin(q6),

sin(q6)*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5))

-

cos(q2

+

q3)*cos(q6)*sin(q4),

sin(q2

+

q3)*cos(q5)

+

cos(q2

+

q3)*cos(q4)*sin(q5),

R4*sin(q2

+

q3)

+

D3*cos(q2)]

[

cos(q4)*sin(q6) + cos(q5)*cos(q6)*sin(q4),

 

cos(q4)*cos(q6) - cos(q5)*sin(q4)*sin(q6),

sin(q4)*sin(q5),

 

0]

[

sin(q2 + q3)*sin(q4)*sin(q6) - cos(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)),

sin(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5))

+sin(q2+q3)*cos(q6)*sin(q4),

cos(q2+q3)*cos(q5)-sin(q2+q3)*cos(q4)*sin(q5),

R4*cos(q2+q3)-D3*sin(q2)]

[

0,

0,

0,

1]

T21 =

[cos(q2),

0,

-sin(q2),

0]

[-sin(q2),

0,

-cos(q2),

0]

[0,

1,

0,

0]

[0,

0,

0,

1]

U2 =

[cos(q2)*(sx*cos(q1)+sy*sin(q1))-sz*sin(q2),

cos(q2)*(px*cos(q1)+py*sin(q1))-pz*sin(q2)

[ - sin(q2)*(sx*cos(q1) + sy*sin(q1)) - sz*cos(q2),

pz*cos(q2)-sin(q2)*(px*cos(q1)+py*sin(q1))

[sy*cos(q1) - sx*sin(q1),

[

cos(q2)*(nx*cos(q1)+ny*sin(q1))-nz*sin(q2), cos(q2)*(ax*cos(q1)+ay*sin(q1))-az*sin(q2), ]

- nz*cos(q2) - sin(q2)*(nx*cos(q1) + ny*sin(q1)), - az*cos(q2) - sin(q2)*(ax*cos(q1) + ay*sin(q1)), - ]

py*cos(q1) - px*sin(q1)]

ny*cos(q1) - nx*sin(q1),

0,

ay*cos(q1) - ax*sin(q1),

0,

0,

1]

T26 = [- cos(q6)*(sin(q3)*sin(q5) - cos(q3)*cos(q4)*cos(q5)) - cos(q3)*sin(q4)*sin(q6),

cos(q5)*sin(q3)+cos(q3)*cos(q4)*sin(q5),

[ cos(q6)*(cos(q3)*sin(q5)+ cos(q4)*cos(q5)*sin(q3)) - sin(q3)*sin(q4)*sin(q6), - sin(q6)*(cos(q3)*sin(q5) + cos(q4)*cos(q5)*sin(q3)) - cos(q6)*sin(q3)*sin(q4),

D3+R4*sin(q3)]

sin(q6)*(sin(q3)*sin(q5) - cos(q3)*cos(q4)*cos(q5))- cos(q3)*cos(q6)*sin(q4),

cos(q4)*sin(q3)*sin(q5)-cos(q3)*cos(q5),

-R4*cos(q3)]

[cos(q4)*sin(q6) + cos(q5)*cos(q6)*sin(q4),

 

cos(q4)*cos(q6) - cos(q5)*sin(q4)*sin(q6),

sin(q4)*sin(q5),

0]

[

0,

0,

0,

1]

T32 =

[cos(q3),

sin(q3),

0,

-D3*cos(q3)]

[-sin(q3),

cos(q3),

0,

D3*sin(q3)]

[0,

0,

1,

0]

[0,

0,

0,

1]

U3 = [-cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) - sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2)), -cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) - sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1))), -cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) - sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1))), -cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) - sin(q3)*(pz*cos(q2) +

sin(q2)*(px*cos(q1)+py*sin(q1)))-D3*cos(q3)

[sin(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) - cos(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2)),

] sin(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1)

+ny*sin(q1))) - cos(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1))), sin(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) - cos(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1))), sin(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) - cos(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) +

]

D3*sin(q3)

[sy*cos(q1)-sx*sin(q1), ny*cos(q1)-nx*sin(q1), ay*cos(q1)-ax*sin(q1), py*cos(q1)-px*sin(q1)]

1]

[

0,

0,

0,

T36 =

[cos(q4)*cos(q5)*cos(q6)-sin(q4)*sin(q6),

-cos(q6)*sin(q4)-cos(q4)*cos(q5)*sin(q6),

cos(q4)*sin(q5),

0]

[cos(q6)*sin(q5),

-sin(q5)*sin(q6),

-cos(q5),

-R4]

[cos(q4)*sin(q6)+cos(q5)*cos(q6)*sin(q4),

cos(q4)*cos(q6)-cos(q5)*sin(q4)*sin(q6),

sin(q4)*sin(q5),

0]

[0,

0,

0,

1]

T34 =

[cos(q4),

0,

sin(q4),

0]

[-sin(q4),

0,

cos(q4),

0]

[0,

-1,

0,

-R4]

[0,

0,

0,

1]

U4 =

[sin(q4)*(sy*cos(q1) - sx*sin(q1)) - cos(q4)*(cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) + sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2))), sin(q4)*(ny*cos(q1) - nx*sin(q1)) - cos(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1)))), sin(q4)*(ay*cos(q1) - ax*sin(q1)) - cos(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))), sin(q4)*(py*cos(q1) - px*sin(q1)) - cos(q4)*(cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) +

sin(q3)*(pz*cos(q2)+sin(q2)*(px*cos(q1)+py*sin(q1)))+D3*cos(q3))

[sin(q4)*(cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) + sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2))) + cos(q4)*(sy*cos(q1) - sx*sin(q1)),

]

sin(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1)))) + cos(q4)*(ny*cos(q1) - nx*sin(q1)), sin(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))) +

cos(q4)*(ay*cos(q1) - ax*sin(q1)), sin(q4)*(cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) + sin(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1)))

+D3*cos(q3))+cos(q4)*(py*cos(q1)-px*sin(q1))

[cos(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2)) - sin(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))), cos(q3)*(nz*cos(q2)

]

+ sin(q2)*(nx*cos(q1) + ny*sin(q1))) - sin(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))), cos(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1))) - sin(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))), cos(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) - R4 -

sin(q3)*(pz*sin(q2)-cos(q2)*(px*cos(q1)+py*sin(q1)))-D3*sin(q3)

 

]

[0,

0,

0,

1]

T46 =

[cos(q5)*cos(q6),

-cos(q5)*sin(q6),

sin(q5),

0]

[sin(q6),

cos(q6),

0,

0]

[-cos(q6)*sin(q5),

sin(q5)*sin(q6),

cos(q5),

0]

[0,

0,

0,

1]

T45 =

[cos(q5),

0,

-sin(q5),

0]

[-sin(q5),

0,

-cos(q5),

0]

[0,

1,

0,

0]

[0,

0,

0,

1]

U5 = [cos(q5)*(sin(q4)*(sy*cos(q1) - sx*sin(q1)) - cos(q4)*(cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) + sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) +

sz*cos(q2))))-sin(q5)*(cos(q3)*(sin(q2)*(sx*cos(q1)+sy*sin(q1))+sz*cos(q2))-sin(q3)*(sz*sin(q2)-cos(q2)*(sx*cos(q1)+sy*sin(q1)))),

cos(q5)*(sin(q4)*(ny*cos(q1) - nx*sin(q1)) - cos(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) +

ny*sin(q1)))))-sin(q5)*(cos(q3)*(nz*cos(q2)+sin(q2)*(nx*cos(q1)+ny*sin(q1)))-sin(q3)*(nz*sin(q2)-cos(q2)*(nx*cos(q1)+ny*sin(q1)))),

-cos(q5)*(cos(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))) - sin(q4)*(ay*cos(q1) - ax*sin(q1))) - sin(q5)*(cos(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1))) - sin(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1)))), sin(q5)*(R4 -

cos(q3)*(pz*cos(q2)+sin(q2)*(px*cos(q1)+py*sin(q1)))+sin(q3)*(pz*sin(q2)-cos(q2)*(px*cos(q1)+py*sin(q1)))+D3*sin(q3))-

cos(q5)*(cos(q4)*(cos(q3)*(pz*sin(q2)-cos(q2)*(px*cos(q1)+py*sin(q1)))+sin(q3)*(pz*cos(q2)+sin(q2)*(px*cos(q1)+py*sin(q1)))+D3*cos(q3))-

]

sin(q4)*(py*cos(q1)-px*sin(q1)))

[-cos(q5)*(cos(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2)) - sin(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1)))) - sin(q5)*(sin(q4)*(sy*cos(q1) -

sx*sin(q1))-cos(q4)*(cos(q3)*(sz*sin(q2)-cos(q2)*(sx*cos(q1)+sy*sin(q1)))+sin(q3)*(sin(q2)*(sx*cos(q1)+sy*sin(q1))+sz*cos(q2)))),

- sin(q5)*(sin(q4)*(ny*cos(q1) - nx*sin(q1)) - cos(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) +

ny*sin(q1)))))-cos(q5)*(cos(q3)*(nz*cos(q2)+sin(q2)*(nx*cos(q1)+ny*sin(q1)))-sin(q3)*(nz*sin(q2)-cos(q2)*(nx*cos(q1)+ny*sin(q1)))),

sin(q5)*(cos(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))) - sin(q4)*(ay*cos(q1) -

ax*sin(q1)))-cos(q5)*(cos(q3)*(az*cos(q2)+sin(q2)*(ax*cos(q1)+ay*sin(q1)))-sin(q3)*(az*sin(q2)-cos(q2)*(ax*cos(q1)+ay*sin(q1)))),

sin(q5)*(cos(q4)*(cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) + sin(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) + D3*cos(q3)) - sin(q4)*(py*cos(q1) - px*sin(q1))) + cos(q5)*(R4 - cos(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) + sin(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) +

py*sin(q1)))+D3*sin(q3))

]

[sin(q4)*(cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) + sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2))) + cos(q4)*(sy*cos(q1) - sx*sin(q1)), sin(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1)))) + cos(q4)*(ny*cos(q1) -

nx*sin(q1)),

sin(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))) + cos(q4)*(ay*cos(q1) -

ax*sin(q1)),

sin(q4)*(cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) + sin(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) + D3*cos(q3)) +

cos(q4)*(py*cos(q1)-px*sin(q1))

]

[0,

0,

0,

1]

T56 =

[cos(q6),

-sin(q6),

0,

0]

[0,

0,

-1,

0]

[sin(q6),

cos(q6),

0,

0]

[0,

0,

0,

1]

3.4.2.

Ecuaciones para cada q

%q1

ec1=py*cos(q1)-px*sin(q1);

q1s=atan2(py,px);

pretty(q1s);

%q2

ec2=simplify((cos(q2)*(px*cos(q1)+py*sin(q1))-pz*sin(q2)-D3)^2+(-pz*cos(q2)-

sin(q2)*(px*cos(q1)+py*sin(q1)))^2-R4^2);

q2s=solve(ec2,q2,'Real',true);

q2s=simplify(q2s);

pretty(q2s);

%q3

ec3=simplify(- cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) - sin(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) - D3*cos(q3));

q3s=solve(ec3,q3,'Real',true);

q3s = simplify(q3s)

pretty(q3s)

%

%

%q4

ec4=simplify(T36(3,3)/T36(1,3));

ec4=ec4-simplify(U3(3,3)/U3(1,3));

q4s=solve(ec4,q4,'Real',true);

q4s=simplify(q4s);

pretty(q4s);

%q5

ec5=simplify(T46(1,3)/T46(3,3));

ec5=ec5-simplify(U4(1,3)/U4(3,3));

q5s=solve(ec5,q5,'Real',true);

q5s=simplify(q5s);

pretty(q5s);

% q6

ec6=simplify(-T36(2,2)/T36(2,1));

ec6=ec6-simplify(-U3(2,2)/U3(2,1));

q6s=solve(ec6,q6,'Real',true);

q6s=simplify(q6s)

pretty(q6s)

Al realizar las operaciones en Matlab se obtuvieron los siguientes resultados para cada q:

q1s = atan2(py, px)

q2s =

-2*atan((2*D3*pz - (-(px^2*cos(q1)^2 + py^2*sin(q1)^2 - D3^2 - R4^2 + pz^2 - 2*D3*R4 + 2*px*py*cos(q1)*sin(q1))*(px^2*cos(q1)^2 + py^2*sin(q1)^2 - D3^2 - R4^2 + pz^2 +

2*D3*R4 + 2*px*py*cos(q1)*sin(q1)))^(1/2))/(px^2*cos(q1)^2 + py^2*sin(q1)^2 + D3^2 - R4^2 + pz^2 + 2*D3*px*cos(q1) + 2*D3*py*sin(q1) + 2*px*py*cos(q1)*sin(q1)))

-2*atan((2*D3*pz + (-(px^2*cos(q1)^2 + py^2*sin(q1)^2 - D3^2 - R4^2 + pz^2 - 2*D3*R4 + 2*px*py*cos(q1)*sin(q1))*(px^2*cos(q1)^2 + py^2*sin(q1)^2 - D3^2 - R4^2 + pz^2 + 2*D3*R4 + 2*px*py*cos(q1)*sin(q1)))^(1/2))/(px^2*cos(q1)^2 + py^2*sin(q1)^2 + D3^2 - R4^2 + pz^2 + 2*D3*px*cos(q1) + 2*D3*py*sin(q1) + 2*px*py*cos(q1)*sin(q1)))

\

| -atan| ------------ | 2 |

/ / 2 D3 pz - #1 \

|

\

#2

/

|

 

|

|

| / 2 D3 pz + #1 \

| -atan| ------------ | 2 |

|

 

\

\

#2

/

/

En donde:

 

2

2

2

2

2

2

#1 == sqrt(-(#5 + #4 - D3

- R4

+ pz

- 2 D3 R4 + #3) (#5 + #4 - D3

- R4

+ pz

+ 2 D3 R4 + #3))

 

2

2

2

#2 == #5 + #4 + D3 - R4 + pz + 2 D3 px cos(q1) + 2 D3 py sin(q1) + #3

#3 == 2 px py cos(q1) sin(q1)

2 2

#4 == py sin(q1)

2 2

#5 == px cos(q1)

q3s =

2*atan((pz*cos(q2) + (px^2*cos(q1)^2 - py^2*cos(q1)^2 + D3^2 + py^2 + pz^2 + px*py*sin(2*q1)

+

2*D3*pz*sin(q2)

-

2*D3*px*cos(q1)*cos(q2)

-

2*D3*py*cos(q2)*sin(q1))^(1/2)

+

px*cos(q1)*sin(q2)

+

py*sin(q1)*sin(q2))/(D3

+

pz*sin(q2)

-

px*cos(q1)*cos(q2)

-

py*cos(q2)*sin(q1)))

2*atan((pz*cos(q2) - (px^2*cos(q1)^2 - py^2*cos(q1)^2 + D3^2 + py^2 + pz^2 + px*py*sin(2*q1) + 2*D3*pz*sin(q2) - 2*D3*px*cos(q1)*cos(q2) - 2*D3*py*cos(q2)*sin(q1))^(1/2) + px*cos(q1)*sin(q2) + py*sin(q1)*sin(q2))/(D3 + pz*sin(q2) - px*cos(q1)*cos(q2) - py*cos(q2)*sin(q1)))

/ / pz cos(q2) + #1 + px cos(q1) sin(q2) + py sin(q1) sin(q2) \

\

|

atan| --------------------------------------------------------- | 2 |

 

|

\

#2

/

|

 

|

|

| / pz cos(q2) - #1 + px cos(q1) sin(q2) + py sin(q1) sin(q2) \

|

| atan| --------------------------------------------------------- | 2 |

En donde:

\

\

#2

2

2

2

2

2

2

#1 == sqrt(px cos(q1) - py cos(q1) + D3 + py px cos(q1) cos(q2) - 2 D3 py

cos(q2) sin(q1))

+ pz

2

/

/

+ px py sin(2 q1) + 2 D3 pz sin(q2) - 2 D3

#2 == D3 + pz sin(q2) - px cos(q1) cos(q2) - py cos(q2) sin(q1)

q4s =

-2*atan((ax*cos(q2 - q1 + q3) - ay*sin(q2 - q1 + q3) - 2*az*sin(q2 + q3) + ax*cos(q1 + q2 + q3) + ay*sin(q1 + q2 + q3) + 2^(1/2)*((ay^2*cos(2*q1))/2 - (ax^2*cos(2*q1))/2 + (ax^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (ax^2*cos(2*q1 + 2*q2 + 2*q3))/4 - (ay^2*cos(2*q2 - 2*q1 + 2*q3))/4 - (ay^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ax^2*cos(2*q2 + 2*q3))/2 + (ay^2*cos(2*q2 + 2*q3))/2 - az^2*cos(2*q2 + 2*q3) + (3*ax^2)/2 + (3*ay^2)/2 + az^2 - ax*az*sin(q1 + 2*q2 + 2*q3) - ay*az*cos(2*q2 - q1 + 2*q3) - ax*ay*sin(2*q1) - (ax*ay*sin(2*q2 - 2*q1 + 2*q3))/2 + (ax*ay*sin(2*q1 + 2*q2 + 2*q3))/2 - ax*az*sin(2*q2 - q1 + 2*q3) + ay*az*cos(q1 + 2*q2 + 2*q3))^(1/2))/(2*ay*cos(q1) - 2*ax*sin(q1)))

-2*atan((ax*cos(q2 - q1 + q3) - ay*sin(q2 - q1 + q3) - 2*az*sin(q2 + q3) + ax*cos(q1 + q2 + q3) + ay*sin(q1 + q2 + q3) - 2^(1/2)*((ay^2*cos(2*q1))/2 - (ax^2*cos(2*q1))/2 + (ax^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (ax^2*cos(2*q1 + 2*q2 + 2*q3))/4 - (ay^2*cos(2*q2 - 2*q1 + 2*q3))/4 - (ay^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ax^2*cos(2*q2 + 2*q3))/2 + (ay^2*cos(2*q2 + 2*q3))/2 - az^2*cos(2*q2 + 2*q3) + (3*ax^2)/2 + (3*ay^2)/2 + az^2 - ax*az*sin(q1 + 2*q2 + 2*q3) - ay*az*cos(2*q2 - q1 + 2*q3) - ax*ay*sin(2*q1) - (ax*ay*sin(2*q2 - 2*q1 + 2*q3))/2 + (ax*ay*sin(2*q1 + 2*q2 + 2*q3))/2 - ax*az*sin(2*q2 - q1 + 2*q3) + ay*az*cos(q1 + 2*q2 + 2*q3))^(1/2))/(2*ay*cos(q1) - 2*ax*sin(q1)))

/ / #4 - #2 - #6 + #5 + #3 + #7 \

\

| -atan| --------------------------- | 2 |

|

\

#1

/

|

 

|

|

| / #4 - #2 - #6 + #5 + #3 - #7 \ | -atan| --------------------------- | 2 |

|

\

\

#1

/

/

En donde:

#1 == 2 ay cos(q1) - 2 ax sin(q1)

#2 == ay sin(q2 - q1 + q3)

#3 == ay sin(q1 + q2 + q3)

#4 == ax cos(q2 - q1 + q3)

#5 == ax cos(q1 + q2 + q3)

#6 == 2 az sin(q2 + q3)

#8

ay #8

/ | ay cos(2 q1)

2

2

2

ax cos(2 q1)

2

ax cos(#9)

2

2

ax cos(#10)

2

2

ay cos(#9)

ay cos(#10)

ax

#7 == sqrt(2) sqrt| ------------- - ------------- + ----------- + ------------ - ----------- - ------------ + ------ + -

-----

 

\

2

2

4

4

4

4

2

2

2

2

2

3 ax

3 ay

2

ax ay sin(#9)

ax ay sin(#10)

- az #8 + ----- + ----- + az - ax az sin(#12) - ay az cos(#11) - ax ay sin(2 q1) - ------------- + --------

------

2

2

\

|

- ax az sin(#11) + ay az cos(#12) |

/

#8 == cos(2 q2 + 2 q3)

#9 == 2 q2 - 2 q1 + 2 q3

#10 == 2 q1 + 2 q2 + 2 q3

#11 == 2 q2 - q1 + 2 q3

#12 == q1 + 2 q2 + 2 q3

2

2

q5s =

2*atan(((az^2*cos(q2)^2*cos(q3)^2 + ay^2*cos(q1)^2*sin(q4)^2 + ax^2*sin(q1)^2*sin(q4)^2 + az^2*sin(q2)^2*sin(q3)^2 + ax^2*cos(q1)^2*cos(q2)^2*sin(q3)^2 + ax^2*cos(q1)^2*cos(q3)^2*sin(q2)^2 + az^2*cos(q2)^2*cos(q4)^2*sin(q3)^2 + az^2*cos(q3)^2*cos(q4)^2*sin(q2)^2 + ay^2*cos(q2)^2*sin(q1)^2*sin(q3)^2 + ay^2*cos(q3)^2*sin(q1)^2*sin(q2)^2 + ax^2*cos(q1)^2*cos(q4)^2*sin(q2)^2*sin(q3)^2 - 2*ax*ay*cos(q1)*sin(q1)*sin(q4)^2 + ay^2*cos(q4)^2*sin(q1)^2*sin(q2)^2*sin(q3)^2 - 2*az^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + ax^2*cos(q1)^2*cos(q2)^2*cos(q3)^2*cos(q4)^2 +

ay^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1)^2 - 2*ay*az*cos(q2)*sin(q1)*sin(q2)*sin(q3)^2 - 2*ay*az*cos(q3)*sin(q1)*sin(q2)^2*sin(q3) + 2*ax*ay*cos(q1)*cos(q2)^2*sin(q1)*sin(q3)^2 +

2*ax*ay*cos(q1)*cos(q3)^2*sin(q1)*sin(q2)^2

2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + 2*az^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) + 2*ay^2*cos(q2)*cos(q3)*sin(q1)^2*sin(q2)*sin(q3) + 2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*sin(q2) + 2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*sin(q3) - 2*ax*az*cos(q1)*cos(q2)*sin(q2)*sin(q3)^2 - 2*ax*az*cos(q1)*cos(q3)*sin(q2)^2*sin(q3) + 2*ay*az*cos(q2)*cos(q3)^2*sin(q1)*sin(q2) + 2*ay*az*cos(q2)^2*cos(q3)*sin(q1)*sin(q3) - 2*ay^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)^2*sin(q2)*sin(q3) - 2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q2) - 2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q3) + 2*ax*az*cos(q1)*cos(q2)*cos(q4)^2*sin(q2)*sin(q3)^2 + 2*ax*az*cos(q1)*cos(q3)*cos(q4)^2*sin(q2)^2*sin(q3) - 2*ay*az*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q1)*sin(q2) - 2*ay*az*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q1)*sin(q3) + 2*ay*az*cos(q2)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3)^2 + 2*ay*az*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3) - 2*ax^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4) + 2*ay^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4) + 2*ax^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4) - 2*ay^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4) - 2*ay*az*cos(q1)*cos(q2)*cos(q4)*sin(q3)*sin(q4) - 2*ay*az*cos(q1)*cos(q3)*cos(q4)*sin(q2)*sin(q4) + 2*ax*ay*cos(q1)*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1) + 2*ax*az*cos(q2)*cos(q4)*sin(q1)*sin(q3)*sin(q4) + 2*ax*az*cos(q3)*cos(q4)*sin(q1)*sin(q2)*sin(q4) + 2*ax*ay*cos(q1)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3)^2 + 2*ax*ay*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)*sin(q4) - 2*ax*ay*cos(q2)*cos(q3)*cos(q4)*sin(q1)^2*sin(q4) - 2*ax*ay*cos(q1)^2*cos(q4)*sin(q2)*sin(q3)*sin(q4) + 2*ax*ay*cos(q4)*sin(q1)^2*sin(q2)*sin(q3)*sin(q4) - 2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) + 4*ax*ay*cos(q1)*cos(q2)*cos(q3)*sin(q1)*sin(q2)*sin(q3) - 4*ax*ay*cos(q1)*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3))^(1/2) + az*cos(q2)*cos(q3) - az*sin(q2)*sin(q3) + ax*cos(q1)*cos(q2)*sin(q3) + ax*cos(q1)*cos(q3)*sin(q2) + ay*cos(q2)*sin(q1)*sin(q3) + ay*cos(q3)*sin(q1)*sin(q2))/(ax*sin(q1)*sin(q4) - ay*cos(q1)*sin(q4) + az*cos(q2)*cos(q4)*sin(q3) + az*cos(q3)*cos(q4)*sin(q2) - ax*cos(q1)*cos(q2)*cos(q3)*cos(q4) - ay*cos(q2)*cos(q3)*cos(q4)*sin(q1) + ax*cos(q1)*cos(q4)*sin(q2)*sin(q3) +

ay*cos(q4)*sin(q1)*sin(q2)*sin(q3)))

+

2*atan((az*cos(q2)*cos(q3) - (az^2*cos(q2)^2*cos(q3)^2 + ay^2*cos(q1)^2*sin(q4)^2 + ax^2*sin(q1)^2*sin(q4)^2 + az^2*sin(q2)^2*sin(q3)^2 + ax^2*cos(q1)^2*cos(q2)^2*sin(q3)^2 + ax^2*cos(q1)^2*cos(q3)^2*sin(q2)^2 + az^2*cos(q2)^2*cos(q4)^2*sin(q3)^2 + az^2*cos(q3)^2*cos(q4)^2*sin(q2)^2 + ay^2*cos(q2)^2*sin(q1)^2*sin(q3)^2 + ay^2*cos(q3)^2*sin(q1)^2*sin(q2)^2 + ax^2*cos(q1)^2*cos(q4)^2*sin(q2)^2*sin(q3)^2 - 2*ax*ay*cos(q1)*sin(q1)*sin(q4)^2 + ay^2*cos(q4)^2*sin(q1)^2*sin(q2)^2*sin(q3)^2 - 2*az^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + ax^2*cos(q1)^2*cos(q2)^2*cos(q3)^2*cos(q4)^2 + ay^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1)^2 - 2*ay*az*cos(q2)*sin(q1)*sin(q2)*sin(q3)^2 -

2*ay*az*cos(q3)*sin(q1)*sin(q2)^2*sin(q3) + 2*ax*ay*cos(q1)*cos(q2)^2*sin(q1)*sin(q3)^2 +

2*ax*ay*cos(q1)*cos(q3)^2*sin(q1)*sin(q2)^2

2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + 2*az^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) + 2*ay^2*cos(q2)*cos(q3)*sin(q1)^2*sin(q2)*sin(q3) + 2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*sin(q2) + 2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*sin(q3) - 2*ax*az*cos(q1)*cos(q2)*sin(q2)*sin(q3)^2 - 2*ax*az*cos(q1)*cos(q3)*sin(q2)^2*sin(q3) + 2*ay*az*cos(q2)*cos(q3)^2*sin(q1)*sin(q2) + 2*ay*az*cos(q2)^2*cos(q3)*sin(q1)*sin(q3) - 2*ay^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)^2*sin(q2)*sin(q3) - 2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q2) - 2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q3) + 2*ax*az*cos(q1)*cos(q2)*cos(q4)^2*sin(q2)*sin(q3)^2 + 2*ax*az*cos(q1)*cos(q3)*cos(q4)^2*sin(q2)^2*sin(q3) - 2*ay*az*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q1)*sin(q2) - 2*ay*az*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q1)*sin(q3) + 2*ay*az*cos(q2)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3)^2 + 2*ay*az*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3) - 2*ax^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4) + 2*ay^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4) + 2*ax^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4) - 2*ay^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4) - 2*ay*az*cos(q1)*cos(q2)*cos(q4)*sin(q3)*sin(q4) - 2*ay*az*cos(q1)*cos(q3)*cos(q4)*sin(q2)*sin(q4) + 2*ax*ay*cos(q1)*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1) + 2*ax*az*cos(q2)*cos(q4)*sin(q1)*sin(q3)*sin(q4) + 2*ax*az*cos(q3)*cos(q4)*sin(q1)*sin(q2)*sin(q4) + 2*ax*ay*cos(q1)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3)^2 + 2*ax*ay*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)*sin(q4) - 2*ax*ay*cos(q2)*cos(q3)*cos(q4)*sin(q1)^2*sin(q4) - 2*ax*ay*cos(q1)^2*cos(q4)*sin(q2)*sin(q3)*sin(q4) + 2*ax*ay*cos(q4)*sin(q1)^2*sin(q2)*sin(q3)*sin(q4) - 2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) + 4*ax*ay*cos(q1)*cos(q2)*cos(q3)*sin(q1)*sin(q2)*sin(q3) - 4*ax*ay*cos(q1)*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3))^(1/2) - az*sin(q2)*sin(q3) + ax*cos(q1)*cos(q2)*sin(q3) + ax*cos(q1)*cos(q3)*sin(q2) + ay*cos(q2)*sin(q1)*sin(q3) + ay*cos(q3)*sin(q1)*sin(q2))/(ax*sin(q1)*sin(q4) - ay*cos(q1)*sin(q4) + az*cos(q2)*cos(q4)*sin(q3) + az*cos(q3)*cos(q4)*sin(q2) - ax*cos(q1)*cos(q2)*cos(q3)*cos(q4) - ay*cos(q2)*cos(q3)*cos(q4)*sin(q1) + ax*cos(q1)*cos(q4)*sin(q2)*sin(q3) + ay*cos(q4)*sin(q1)*sin(q2)*sin(q3)))

+

/ / #1 + #8 - #7 + #6 + #5 + #4 + #3 \

\

| atan| -------------------------------- | 2 |

|

\

#2

/

|

 

|

|

| / #8 - #1 - #7 + #6 + #5 + #4 + #3 \

|

| atan| -------------------------------- | 2 |

En donde:

\

\

#2

/

/

2

2

2

2

2

2

2

2

2

2

2

2

#1 == sqrt(az cos(q2) cos(q3) + ay cos(q1) sin(q4) + ax sin(q1) sin(q4) + az sin(q2) sin(q3)

2 2

2

2

2

2

2

2

2

2

2

2

+ ax cos(q1) cos(q2) sin(q3) + ax cos(q1) cos(q3) sin(q2) + az cos(q2) cos(q4) sin(q3)

2 2

2

2

2

2

2

2

2

2

2

2

+ az cos(q3) cos(q4) sin(q2) + ay cos(q2) sin(q1) sin(q3) + ay cos(q3) sin(q1) sin(q2)

2 2

2

2

2

2

2

2

2

2

2

+ ax cos(q1) cos(q4) sin(q2) sin(q3) - 2 ax ay cos(q1) sin(q1) sin(q4) + ay cos(q4) sin(q1) sin(q2) sin(q3)

2

2

2

2

- 2 az cos(q2) cos(q3) sin(q2) sin(q3) + ax cos(q3) cos(q4) sin(q1)

2

2

2

2

2

2

2

2

cos(q1) cos(q2) cos(q3) cos(q4) + ay

2

2

cos(q2)

- 2 ay az cos(q2) sin(q1) sin(q2) sin(q3) cos(q1) cos(q2)

- 2 ay az cos(q3) sin(q1) sin(q2)

sin(q3) + 2 ax ay

2

2

2

2

2

2

sin(q1) sin(q3) sin(q2) sin(q3) + 2 az

+ 2 ax ay cos(q1) cos(q3)

sin(q1) sin(q2)

+ 2 ax

cos(q1)

cos(q2) cos(q3)

2

2

2

2

cos(q2) cos(q3) cos(q4) sin(q2) sin(q3) + 2 ay cos(q2) cos(q3) sin(q1) sin(q2) sin(q3) + 2 ax az cos(q1) cos(q2) cos(q3)

2

2

sin(q2) + 2 ax az cos(q1) cos(q2) cos(q3) sin(q3) - 2 ax az cos(q1) cos(q2) sin(q2) sin(q3)

2

2

2

- 2 ax az cos(q1) cos(q3) sin(q2) sin(q3) + 2 ay az cos(q2) cos(q3) sin(q1) sin(q2) + 2 ay az

cos(q2)

2

2

2

2

2

cos(q3) sin(q1) sin(q3) - 2 ay cos(q2) cos(q3) cos(q4) sin(q1) sin(q2) sin(q3) - 2 ax az cos(q1) cos(q2) cos(q3) cos(q4)

2

2

sin(q2) - 2 ax az cos(q1) cos(q2) sin(q2) sin(q3)

2

2

cos(q3) cos(q4)

2

2

2

sin(q3) + 2 ax az cos(q1) cos(q2) cos(q4)

2

2

+ 2 ax az cos(q1) cos(q3) cos(q4) sin(q2) sin(q3) - 2 ay az cos(q2) cos(q3) cos(q4) sin(q1) sin(q2) - 2 ay az cos(q2)

2

cos(q3) cos(q4) cos(q3) cos(q4)

2

2

sin(q1) sin(q3) + 2 ay az cos(q2) cos(q4)

2

sin(q1) sin(q2) sin(q3)

+ 2 ay az

2

2

2

sin(q1) sin(q2) sin(q3) - 2 ax cos(q1) cos(q2) cos(q3) cos(q4) sin(q1) sin(q4) + 2 ay

2

2

cos(q1) cos(q2) cos(q3) cos(q4) sin(q1) sin(q4) + 2 ax cos(q1) cos(q4) sin(q1) sin(q2) sin(q3) sin(q4) - 2 ay

cos(q1) cos(q4) sin(q1) sin(q2) sin(q3) sin(q4) - 2 ay az cos(q1) cos(q2) cos(q4) sin(q3) sin(q4) - 2 ay az

2

2

2

cos(q1) cos(q3) cos(q4) sin(q2) sin(q4) + 2 ax ay cos(q1) cos(q2) cos(q3) cos(q4) sin(q1) + 2 ax az

2

cos(q2) cos(q4) sin(q1) sin(q3) sin(q4) + 2 ax az cos(q3) cos(q4) sin(q1) sin(q2) sin(q4) + 2 ax ay cos(q1) cos(q4)

2

2

sin(q1) sin(q2)

sin(q3)

cos(q3) cos(q4) sin(q1)

2

2

+ 2 ax ay cos(q1)

2

cos(q2) cos(q3) cos(q4) sin(q4) - 2 ax ay cos(q2)

2

2

2

sin(q4) - 2 ax ay cos(q1) cos(q4) sin(q2) sin(q3) sin(q4) + 2 ax ay cos(q4) sin(q1) sin(q2) sin(q3) sin(q4) - 2 ax cos(q1)

2

cos(q2) cos(q3) cos(q4) sin(q2) sin(q3) + 4 ax ay cos(q1) cos(q2) cos(q3) sin(q1) sin(q2) sin(q3)

- 4 ax ay

2

cos(q1) cos(q2) cos(q3) cos(q4) sin(q1) sin(q2) sin(q3))

#2 == ax sin(q1) sin(q4) - ay cos(q1) sin(q4) + az cos(q2) cos(q4) sin(q3) + az cos(q3) cos(q4) sin(q2) - ax

cos(q1) cos(q2) cos(q3) cos(q4) - ay cos(q2) cos(q3) cos(q4) sin(q1) + ax cos(q1) cos(q4) sin(q2) sin(q3) + ay

cos(q4) sin(q1) sin(q2) sin(q3)

#3 == ay cos(q3) sin(q1) sin(q2)

#4 == ay cos(q2) sin(q1) sin(q3)

#5 == ax cos(q1) cos(q3) sin(q2)

#6 == ax cos(q1) cos(q2) sin(q3)

#7 == az sin(q2) sin(q3)

#8 == az cos(q2) cos(q3)

q6s =

2*atan((sy*cos(q2 - q1 + q3) + sx*sin(q2 - q1 + q3) + 2*sz*cos(q2 + q3) +

2^(1/2)*((nx^2*cos(2*q1))/2 - (ny^2*cos(2*q1))/2 - (nx^2*cos(2*q2 - 2*q1 + 2*q3))/4 - (nx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ny^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (ny^2*cos(2*q1 + 2*q2

+ 2*q3))/4 + (sx^2*cos(2*q1))/2 - (sy^2*cos(2*q1))/2 - (sx^2*cos(2*q2 - 2*q1 + 2*q3))/4 -

(sx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (sy^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (sy^2*cos(2*q1 + 2*q2

+ 2*q3))/4 - (nx^2*cos(2*q2 + 2*q3))/2 - (ny^2*cos(2*q2 + 2*q3))/2 + nz^2*cos(2*q2 + 2*q3) -

(sx^2*cos(2*q2 + 2*q3))/2 - (sy^2*cos(2*q2 + 2*q3))/2 + sz^2*cos(2*q2 + 2*q3) + nx^2/2 + ny^2/2 + nz^2 + sx^2/2 + sy^2/2 + sz^2 - ny*nz*cos(q1 + 2*q2 + 2*q3) - sy*sz*cos(q1 + 2*q2 + 2*q3) + nx*nz*sin(q1 + 2*q2 + 2*q3) + sx*sz*sin(q1 + 2*q2 + 2*q3) + ny*nz*cos(2*q2 - q1 + 2*q3) + sy*sz*cos(2*q2 - q1 + 2*q3) + nx*ny*sin(2*q1) + (nx*ny*sin(2*q2 - 2*q1 + 2*q3))/2 - (nx*ny*sin(2*q1 + 2*q2 + 2*q3))/2 + nx*nz*sin(2*q2 - q1 + 2*q3) + sx*sy*sin(2*q1) + (sx*sy*sin(2*q2 - 2*q1 + 2*q3))/2 - (sx*sy*sin(2*q1 + 2*q2 + 2*q3))/2 + sx*sz*sin(2*q2 - q1 + 2*q3))^(1/2) - sy*cos(q1 + q2 + q3) + sx*sin(q1 + q2 + q3))/(ny*cos(q2 - q1 + q3) + nx*sin(q2 - q1 + q3) + 2*nz*cos(q2 + q3) - ny*cos(q1 + q2 + q3) + nx*sin(q1 + q2 + q3)))

2*atan((sy*cos(q2 - q1 + q3) + sx*sin(q2 - q1 + q3) + 2*sz*cos(q2 + q3) - 2^(1/2)*((nx^2*cos(2*q1))/2 - (ny^2*cos(2*q1))/2 - (nx^2*cos(2*q2 - 2*q1 + 2*q3))/4 -

(nx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ny^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (ny^2*cos(2*q1 + 2*q2

+ 2*q3))/4 + (sx^2*cos(2*q1))/2 - (sy^2*cos(2*q1))/2 - (sx^2*cos(2*q2 - 2*q1 + 2*q3))/4 -

(sx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (sy^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (sy^2*cos(2*q1 + 2*q2

+ 2*q3))/4 - (nx^2*cos(2*q2 + 2*q3))/2 - (ny^2*cos(2*q2 + 2*q3))/2 + nz^2*cos(2*q2 + 2*q3) -

(sx^2*cos(2*q2 + 2*q3))/2 - (sy^2*cos(2*q2 + 2*q3))/2 + sz^2*cos(2*q2 + 2*q3) + nx^2/2 + ny^2/2 + nz^2 + sx^2/2 + sy^2/2 + sz^2 - ny*nz*cos(q1 + 2*q2 + 2*q3) - sy*sz*cos(q1 + 2*q2 + 2*q3) + nx*nz*sin(q1 + 2*q2 + 2*q3) + sx*sz*sin(q1 + 2*q2 + 2*q3) + ny*nz*cos(2*q2 - q1 + 2*q3) + sy*sz*cos(2*q2 - q1 + 2*q3) + nx*ny*sin(2*q1) + (nx*ny*sin(2*q2 - 2*q1 + 2*q3))/2 - (nx*ny*sin(2*q1 + 2*q2 + 2*q3))/2 + nx*nz*sin(2*q2 - q1 + 2*q3) + sx*sy*sin(2*q1) + (sx*sy*sin(2*q2 - 2*q1 + 2*q3))/2 - (sx*sy*sin(2*q1 + 2*q2 + 2*q3))/2 + sx*sz*sin(2*q2 - q1 + 2*q3))^(1/2) - sy*cos(q1 + q2 + q3) + sx*sin(q1 + q2 + q3))/(ny*cos(q2 - q1 + q3) + nx*sin(q2 -

q1 + q3) + 2*nz*cos(q2 + q3) - ny*cos(q1 + q2 + q3) + nx*sin(q1 + q2 + q3)))

/ / sy #11 + sx #9 + #1 + #2 - sy #12 + sx #10 \

\

| atan| ------------------------------------------ | 2 |

|

\

#3

/

|

 

|

|

| / sy #11 + sx #9 + #1 - #2 - sy #12 + sx #10 \

|

| atan| ------------------------------------------ | 2 |

En donde:

\

\

#1 == 2 sz cos(q2 + q3)

#3

/

/

/

2

2

2

2

2

2

2

| nx cos(2 q1)

ny cos(2 q1)

nx cos(#5)

nx cos(#6)

ny cos(#5)

ny cos(#6)

sx

cos(2 q1) #2 == sqrt(2) sqrt| ------------- - ------------- - ----------- - ----------- + ----------- + ----------- + ------------

-

 

\

2

2

4

4

4

4

2

2

2

2

2

2

2

2

2

2

sy cos(2 q1) sx cos(#5) sx cos(#6) sy cos(#5) sy cos(#6) nx #4 ny #4 2 sy #4

sx #4

- ------------- - ----------- - ----------- + ----------- + ----------- - ------ - ------ + nz #4 - ------ - ------

 

2

4

4

4

4

2

2

2

2

 

2

2

2

2

2

nx

ny

2

sx

sy

2

+ sz #4 + --- + --- + nz + --- + --- + sz - ny nz cos(#8) - sy sz cos(#8) + nx nz sin(#8) + sx sz sin(#8) + ny nz

2

2

2

2

nx ny sin(#5)

nx ny sin(#6)

sx sy sin(#5)

cos(#7) + sy sz cos(#7) + nx ny sin(2 q1) + ------------- - ------------- + nx nz sin(#7) + sx sy sin(2

q1) + -------------

-

\

sx sy sin(#6)

------------- + sx sz sin(#7) | /

|

2

2

2

2

#3 == ny #11 + nx #9 + 2 nz cos(q2 + q3) - ny #12 + nx #10

#4 == cos(2 q2 + 2 q3)

#5 == 2 q2 - 2 q1 + 2 q3

#6 == 2 q1 + 2 q2 + 2 q3

#7 == 2 q2 - q1 + 2 q3

#8 == q1 + 2 q2 + 2 q3

#9 == sin(q2 - q1 + q3)

#10 == sin(q1 + q2 + q3)

#11 == cos(q2 - q1 + q3)

#12 == cos(q1 + q2 + q3)

Se crearon dos funciones en Matlab una llamada MGD la cual nos solicita los valores de q1, q2, q3, q4, q5, q6 y nos entrega los valores de sx, sy, sz, nx, ny, nz, ax, ay, az, px, py, pz. Tal como se muestra a continuación:

Teniendo en cuenta que los limites que se probaron para q1, q2 y q3 según la estructura de la interfaz:

q1= -1.05 a 1.05

q2= 0 a -1.57

q3= 1.6 a 4

Los valores se establecen en radiales.

function salida=MGD (q1, q2, q3, q4, q5, q6) %Modelo Geométrico Directo R4=13.5; %cm D3=14; %cm
function salida=MGD (q1, q2, q3, q4, q5, q6)
%Modelo Geométrico Directo
R4=13.5; %cm
D3=14; %cm
U0=[ - sin(q6)*(cos(q4)*sin(q1) + cos(q2 + q3)*cos(q1)*sin(q4)) -
cos(q6)*(cos(q5)*(sin(q1)*sin(q4) - cos(q2 + q3)*cos(q1)*cos(q4)) +
sin(q2 + q3)*cos(q1)*sin(q5)), sin(q6)*(cos(q5)*(sin(q1)*sin(q4) -
cos(q2 + q3)*cos(q1)*cos(q4)) + sin(q2 + q3)*cos(q1)*sin(q5)) -
cos(q6)*(cos(q4)*sin(q1) + cos(q2 + q3)*cos(q1)*sin(q4)), sin(q2 +
q3)*cos(q1)*cos(q5) - sin(q5)*(sin(q1)*sin(q4) - cos(q2 +
q3)*cos(q1)*cos(q4)), cos(q1)*(R4*sin(q2 + q3) + D3*cos(q2));
sin(q6)*(cos(q1)*cos(q4) - cos(q2 + q3)*sin(q1)*sin(q4)) +
cos(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) -
sin(q2 + q3)*sin(q1)*sin(q5)), cos(q6)*(cos(q1)*cos(q4) - cos(q2 +
q3)*sin(q1)*sin(q4)) - sin(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q2 +
q3)*cos(q4)*sin(q1)) - sin(q2 + q3)*sin(q1)*sin(q5)),
sin(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) + sin(q2 +
q3)*cos(q5)*sin(q1), sin(q1)*(R4*sin(q2 + q3) + D3*cos(q2));
sin(q2 + q3)*sin(q4)*sin(q6) - cos(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 +
q3)*cos(q4)*cos(q5)),sin(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 +
q3)*cos(q4)*cos(q5)) + sin(q2 + q3)*cos(q6)*sin(q4),
cos(q2 + q3)*cos(q5) - sin(q2 + q3)*cos(q4)*sin(q5), R4*cos(q2
+ q3) - D3*sin(q2);
0, 0, 0, 1];
sx=U0(1,1);
sy=U0(2,1);
sz=U0(3,1);
nx=U0(1,2);
ny=U0(2,2);
nz=U0(3,2);
ax=U0(1,3);
ay=U0(2,3);
az=U0(3,3);

px=U0(1,4);

py=U0(2,4);

pz=U0(3,4);

salida= [sx sy sz nx ny nz ax ay az px py pz];

Teniendo los valores de sx, sy, sz, nx, ny, nz, ax, ay, az, px, py, pz se pues calcular el MGI para cual se creó la siguiente función con el fin que sean calculados los valore de q1, q2, q3, q4, q5, q6, Tal como se muestra a continuación:

function salida=MGI(sx,sy,sz,nx,ny,nz,ax,ay,az,px,py,pz)

R4=13.5;

D3=14; %cm

%Ecuación para q1

q1=atan2(py,px);

%Ecuación para q2

q2=-2*atan2((2*D3*pz+(-(px^2*cos(q1)^2+py^2*sin(q1)^2-D3^2-R4^2+pz^2-

2*D3*R4+2*px*py*cos(q1)*sin(q1))*(px^2*cos(q1)^2+py^2*sin(q1)^2-D3^2-

R4^2 + pz^2 + 2*D3*R4 + 2*px*py*cos(q1)*sin(q1)))^(1/2)),(px^2*cos(q1)^2

+ py^2*sin(q1)^2 + D3^2 - R4^2 + pz^2 + 2*D3*px*cos(q1) + 2*D3*py*sin(q1)

+ 2*px*py*cos(q1)*sin(q1)));

%Ecuación para q3

q3=2*atan2(-(pz*cos(q2)-(px^2*cos(q1)^2-py^2*cos(q1)^2+D3^2+py^2+pz^2

+px*py*sin(2*q1)+2*D3*pz*sin(q2)-2*D3*px*cos(q1)*cos(q2)-2*D3*py

*cos(q2)*sin(q1))^(1/2)+px*cos(q1)*sin(q2)+py*sin(q1)*sin(q2)),-

(D3+pz*sin(q2)- px*cos(q1)*cos(q2)-py*cos(q2)*sin(q1)));

%Ecuación para q4

q4=atan2(-(ay*cos(q1)-ax*sin(q1)), (cos(q3)*(az*sin(q2)-cos(q2)*(ax*cos

(q1)+ay*sin(q1)))+sin(q3)*(az*cos(q2)+sin(q2)*(ax*cos(q1)+ay*sin(q1))

)));

%Ecuación para q5

q5=2*atan2(-(az*cos(q2)*cos(q3)-(az^2*cos(q2)^2*cos(q3)^2+

ay^2*cos(q1)^2*sin(q4)^2+ax^2*sin(q1)^2*sin(q4)^2+

az^2*sin(q2)^2*sin(q3)^2+ax^2*cos(q1)^2*cos(q2)^2*sin(q3)^2+

ax^2*cos(q1)^2*cos(q3)^2*sin(q2)^2 + az^2*cos(q2)^2*cos(q4)^2*sin(q3)^2 +az^2*cos(q3)^2*cos(q4)^2*sin(q2)^2+ ay^2*cos(q2)^2*sin(q1)^2*sin(q3)^2

+ay^2*cos(q3)^2*sin(q1)^2*sin(q2)^2+

ax^2*cos(q1)^2*cos(q4)^2*sin(q2)^2*sin(q3)^2-

2*ax*ay*cos(q1)*sin(q1)*sin(q4)^2+

ay^2*cos(q4)^2*sin(q1)^2*sin(q2)^2*sin(q3)^2-

2*az^2*cos(q2)*cos(q3)*sin(q2)*sin(q3)+

ax^2*cos(q1)^2*cos(q2)^2*cos(q3)^2*cos(q4)^2+

ay^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1)^2-

2*ay*az*cos(q2)*sin(q1)*sin(q2)*sin(q3)^2-

2*ay*az*cos(q3)*sin(q1)*sin(q2)^2*sin(q3)+

2*ax*ay*cos(q1)*cos(q2)^2*sin(q1)*sin(q3)^2+

2*ax*ay*cos(q1)*cos(q3)^2*sin(q1)*sin(q2)^2+

2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*sin(q2)*sin(q3)+

2*az^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3)+

2*ay^2*cos(q2)*cos(q3)*sin(q1)^2*sin(q2)*sin(q3)+

2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*sin(q2)+

2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*sin(q3)-

2*ax*az*cos(q1)*cos(q2)*sin(q2)*sin(q3)^2-

2*ax*az*cos(q1)*cos(q3)*sin(q2)^2*sin(q3)+

2*ay*az*cos(q2)*cos(q3)^2*sin(q1)*sin(q2)+

2*ay*az*cos(q2)^2*cos(q3)*sin(q1)*sin(q3)-

2*ay^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)^2*sin(q2)*sin(q3)-

2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q2)-

2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q3)+

2*ax*az*cos(q1)*cos(q2)*cos(q4)^2*sin(q2)*sin(q3)^2+

2*ax*az*cos(q1)*cos(q3)*cos(q4)^2*sin(q2)^2*sin(q3)-

2*ay*az*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q1)*sin(q2)-

2*ay*az*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q1)*sin(q3)+

2*ay*az*cos(q2)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3)^2+

2*ay*az*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3)-

2*ax^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4)+

2*ay^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4)+

2*ax^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4)-

2*ay^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4)-

2*ay*az*cos(q1)*cos(q2)*cos(q4)*sin(q3)*sin(q4)-

2*ay*az*cos(q1)*cos(q3)*cos(q4)*sin(q2)*sin(q4)+

2*ax*ay*cos(q1)*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1)+

2*ax*az*cos(q2)*cos(q4)*sin(q1)*sin(q3)*sin(q4)+

2*ax*az*cos(q3)*cos(q4)*sin(q1)*sin(q2)*sin(q4)+

2*ax*ay*cos(q1)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3)^2+

2*ax*ay*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)*sin(q4)-

2*ax*ay*cos(q2)*cos(q3)*cos(q4)*sin(q1)^2*sin(q4)-

2*ax*ay*cos(q1)^2*cos(q4)*sin(q2)*sin(q3)*sin(q4)+

2*ax*ay*cos(q4)*sin(q1)^2*sin(q2)*sin(q3)*sin(q4)-

2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3)+

4*ax*ay*cos(q1)*cos(q2)*cos(q3)*sin(q1)*sin(q2)*sin(q3)-

4*ax*ay*cos(q1)*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3))^(1/

2)-az*sin(q2)*sin(q3)+ax*cos(q1)*cos(q2)*sin(q3)+

ax*cos(q1)*cos(q3)*sin(q2)+ay*cos(q2)*sin(q1)*sin(q3)+

ay*cos(q3)*sin(q1)*sin(q2)),-(ax*sin(q1)*sin(q4) - ay*cos(q1)*sin(q4)+

az*cos(q2)*cos(q4)*sin(q3)+az*cos(q3)*cos(q4)*sin(q2)-

ax*cos(q1)*cos(q2)*cos(q3)*cos(q4) - ay*cos(q2)*cos(q3)*cos(q4)*sin(q1)

+ax*cos(q1)*cos(q4)*sin(q2)*sin(q3)+ay*cos(q4)*sin(q1)*sin(q2)*sin(q3)

));

%Ecuación para q6

q6= 2*atan2((sy*cos(q2 - q1 + q3) + sx*sin(q2 - q1 + q3) + 2*sz*cos(q2 + q3) - 2^(1/2)*((nx^2*cos(2*q1))/2 - (ny^2*cos(2*q1))/2 - (nx^2*cos(2*q2 - 2*q1 + 2*q3))/4 - (nx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ny^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (ny^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (sx^2*cos(2*q1))/2 - (sy^2*cos(2*q1))/2 - (sx^2*cos(2*q2 - 2*q1 + 2*q3))/4 - (sx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (sy^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (sy^2*cos(2*q1 + 2*q2 + 2*q3))/4 - (nx^2*cos(2*q2 + 2*q3))/2 - (ny^2*cos(2*q2 + 2*q3))/2 + nz^2*cos(2*q2 + 2*q3) - (sx^2*cos(2*q2 + 2*q3))/2 - (sy^2*cos(2*q2 + 2*q3))/2 + sz^2*cos(2*q2 + 2*q3) + nx^2/2 + ny^2/2 + nz^2 + sx^2/2 + sy^2/2 + sz^2 - ny*nz*cos(q1 + 2*q2 + 2*q3) - sy*sz*cos(q1 + 2*q2 + 2*q3) + nx*nz*sin(q1 + 2*q2 + 2*q3) + sx*sz*sin(q1

+

- (nx*ny*sin(2*q1 + 2*q2 + 2*q3))/2 + nx*nz*sin(2*q2 - q1 + 2*q3) + sx*sy*sin(2*q1) + (sx*sy*sin(2*q2 - 2*q1 + 2*q3))/2 - (sx*sy*sin(2*q1 + 2*q2 + 2*q3))/2 + sx*sz*sin(2*q2 - q1 + 2*q3))^(1/2) - sy*cos(q1 + q2 + q3) + sx*sin(q1 + q2 + q3)),(ny*cos(q2 - q1 + q3) + nx*sin(q2 - q1 + q3)

+ 2*nz*cos(q2 + q3) - ny*cos(q1 + q2 + q3) + nx*sin(q1 + q2 + q3)));

2*q3) + nx*ny*sin(2*q1) + (nx*ny*sin(2*q2

2*q2 + 2*q3) + ny*nz*cos(2*q2 - q1 + 2*q3) + sy*sz*cos(2*q2 - q1 + -

2*q1

+ 2*q3))/2

salida= [q1 q2 q3 q4 q5 q6];

4. Prueba de los modelos geométricos en Simulink

4.1. Modelo de la interfaz en 3D

Mediante el uso de Simulink y más específicamente de la toolbox de 3D Animation se creó un modelo virtual mediante la implementación del bloque VR Sink (figura 4).

mediante la implementación del bloque VR Sink (figura 4). Figura 4. Bloque VR Sink El primer

Figura 4. Bloque VR Sink

El primer paso fue el de crear el modelo en 3D por medio del 3D World Editor en donde se tuvo como referencia los tres primeros ángulos del robot (q1, q2 y q3), obteniendo como resultado la siguiente estructura:

q2 y q3), obteniendo como resultado la siguiente estructura: Figura 5. Imagen del modelo 3D de

Figura 5. Imagen del modelo 3D de la interfaz en 3D World Editor

Figura 6. Imágenes del modelo 3D de la interfaz en 3D World Editor Posteriormente de
Figura 6. Imágenes del modelo 3D de la interfaz en 3D World Editor Posteriormente de

Figura 6. Imágenes del modelo 3D de la interfaz en 3D World Editor

Posteriormente de crear el modelo en el mundo virtual se creó en Simulink una simulación de los modelos geométricos MGD y MGI en donde con los parámetros de X, Y y Z dados por la función Circular creada por el Ing. Sergio Salinas se probarán los modelos geométricos. Al crear y conectar los diferentes bloques en Simulink se obtuvo el siguiente diagrama:

bloques en Simulink se obtuvo el siguiente diagrama: Figura 7. Diagrama de bloques en Simulink de

Figura 7. Diagrama de bloques en Simulink de las pruebas de los Modelos Geométricos.

Para realizar la prueba de los modelos geométricos se siguieron los siguientes pasos:

a) Se ejecutó el archivo circular.m con el fin de obtener los parámetros X, Y y Z.

b) Se ejecuta la simulación Inter1.slx

c) Se abre el VR sink con el fin de ver la simulación de la interfaz 3D.

Se adjuntan video generado por simulimk (video.avi) y los archivos necesarios para la correcta simulación.

y los archivos necesarios para la correcta simulación. Figura 8. Vista de 3D World Editor 5.

Figura 8. Vista de 3D World Editor

5. Tabla de parámetros inerciales

5.1. Cálculo de los parámetros inerciales de las 3 primeras articulaciones

El cálculo de los parámetros inerciales se realizó utilizando el el método de Lagrange explicado por el Ing. Sergio Salinas y descrito en el capítulo 4 del libro de Diseño y control de robots industriales[4]. El desarrollo de los cálculos se realizó de forma manual por lo cual se adjuntan imágenes (figuras 9 a 14) en donde se evidencia el proceso desarrollado.

figura 9. figura 10.

figura 9.

figura 9. figura 10.

figura 10.

figura 11. figura 12.

figura 11.

figura 11. figura 12.

figura 12.

figura 13. figura 14. Teniendo como resultado las siguientes ecuaciones para el cálculo de las

figura 13.

figura 13. figura 14. Teniendo como resultado las siguientes ecuaciones para el cálculo de las tres

figura 14.

Teniendo como resultado las siguientes ecuaciones para el cálculo de las tres primeras articulaciones.

ZZ1R=IA1 + d3**2*M3 + YY2 + YY3 + ZZ1

XX2R=-(d3**2*M3) + XX2 - YY2

XZ2R=-(d3*MZ3) + XZ2

ZZ2R=IA2 + d3**2*M3 + ZZ2

MX2R=d3*M3 + MX2

XX3R=XX3 - YY3

5.2. Tabla de parámetros inerciales completa

El proceso para calcular los parámetros inerciales se efectuó mediante el software SYMORO (Symbolic Modelingof Robots) en donde se ingresó cada uno de los valores de la tabla de parámetros geométricos, para lograr obtener un archivo con las ecuaciones y la tabla de parámetros inerciales los cuales se muestran a continuación:

(********************************************) (** SYMORO+ : SYmbolic MOdelling of RObots **)

(**========================================**)

(**

IRCCyN-ECN - 1, rue de la Noe

**)

(**

B.P.92101

**)

(**

44321 Nantes cedex 3, FRANCE

**)

(**

www.irccyn.ec-nantes.fr

**)

(********************************************)

Name of file : D:\Maestria\Sistemas Robo'ticos en Me'dicina\Archivo Ejemplo Symoro\RobotCompleto\RobotCompletobaseI.parbase.par

Geometric parameters

j sigma gamma b

ant

mu

alpha d

theta r

1 0

1

0

0

0

0

0

q1

0

 

-Pi

2 1

1

0

0

0

---

0

q2

0

 

2

3 2

1

0

0

0

0

d3

q3

0

 

Pi

4 3

0

0

0

0

--

0

q4

R4

 

2

-Pi

5 4

0

0

0

0

---

0

q5

0

 

2

Pi

6 5

0

0

0

0

--

0

q6

0

 

2

Inertial parameters

j

XX

XY

XZ

YY

YZ

ZZ

MX

MY

MZ

M

Ia

1

XX1

XY1

XZ1

YY1

YZ1

ZZ1

MX1

MY1

MZ1

M1

IA1

2

XX2

XY2

XZ2

YY2

YZ2

ZZ2

MX2

MY2

MZ2

M2

IA2

3

XX3

XY3

XZ3

YY3

YZ3

ZZ3

MX3

MY3

MZ3

M3

IA3

4

XX4

XY4

XZ4

YY4

YZ4

ZZ4

MX4

MY4

MZ4

M4

0

5

XX5

XY5

XZ5

YY5

YZ5

ZZ5

MX5

MY5

MZ5

M5

0

6

XX6

XY6

XZ6

YY6

YZ6

ZZ6

MX6

MY6

MZ6

M6

0

The regrouping relations are

ZZ1R=IA1 + d3**2*(M3 + M4 + M5 + M6) + YY2 + YY3 + ZZ1

XX2R=-(d3**2*(M3 + M4 + M5 + M6)) + XX2 - YY2

XZ2R=-(d3*MZ3) + XZ2

ZZ2R=IA2 + d3**2*(M3 + M4 + M5 + M6) + ZZ2

MX2R=d3*(M3 + M4 + M5 + M6) + MX2

XX3R=2*MZ4*R4 + (M4 + M5 + M6)*R4**2 + XX3 - YY3 + YY4

ZZ3R=2*MZ4*R4 + (M4 + M5 + M6)*R4**2 + YY4 + ZZ3

MY3R=MY3 - MZ4 - (M4 + M5 + M6)*R4

XX4R=XX4 - YY4 + YY5

ZZ4R=YY5 + ZZ4

MY4R=MY4 + MZ5

XX5R=XX5 - YY5 + YY6

ZZ5R=YY6 + ZZ5

MY5R=MY5 - MZ6

XX6R=XX6 - YY6

The base inertial parameters are

Inertial parameters

j

XX

XY

XZ

YY

YZ

ZZ

MX

MY

MZ

M

Ia

1

0

0

0

0

0

ZZ1R

0

0

0

0

0

2

XX2R

XY2

XZ2R

0

YZ2

ZZ2R

MX2R

MY2

0

0

0

3

XX3R

XY3

XZ3

0

YZ3

ZZ3R

MX3

MY3R

0

0

IA3

4

XX4R

XY4

XZ4

0

YZ4

ZZ4R

MX4

MY4R

0

0

0

5

XX5R

XY5

XZ5

0

YZ5

ZZ5R

MX5

MY5R

0

0

0

6

XX6R

XY6

XZ6

0

YZ6

ZZ6

MX6

MY6

0

0

0

6.

Piezas en SolidWorks del Robot

 

6.1. Diseño de piezas en Solidworks del robot

El diseño de las piezas se realizó teniendo como principal referente el diagrama de la patente US007411576B2, el resultado del trabajo realizado se evidencia en las figuras 15 a 22.

el resultado del trabajo realizado se evidencia en las figuras 15 a 22. Figura 15. Pieza

Figura 15. Pieza A

el resultado del trabajo realizado se evidencia en las figuras 15 a 22. Figura 15. Pieza

Figura 16. Pieza B

Figura 17. Pieza C Figura 18. Pieza D

Figura 17. Pieza C

Figura 17. Pieza C Figura 18. Pieza D

Figura 18. Pieza D

Figura 19. Pieza E Figura 20. Pieza F Se realizó el ensamble de las piezas

Figura 19. Pieza E

Figura 19. Pieza E Figura 20. Pieza F Se realizó el ensamble de las piezas diseñadas

Figura 20. Pieza F

Se realizó el ensamble de las piezas diseñadas en Solidworks, tal como se muestra a continuación en la figura 21.

Figura 21. Ensamble con las diferentes piezas diseñadas. 6.2. Tablas con los valores de las

Figura 21. Ensamble con las diferentes piezas diseñadas.

6.2. Tablas con los valores de las masas y parámetros de cada pieza del robot

Se utilizó la propiedad del software solidworks mediante el uso de cálculo de propiedades físicas. Obtenido los valores que se utilizaran en los modelos cinemáticos.

físicas. Obtenido los valores que se utilizaran en los modelos cinemáticos. Figura 22. Parámetros para la

Figura 22. Parámetros para la pieza A.

Figura 23. Parámetros para la pieza B. Figura 24. Parámetros para la pieza C.

Figura 23. Parámetros para la pieza B.

Figura 23. Parámetros para la pieza B. Figura 24. Parámetros para la pieza C.

Figura 24. Parámetros para la pieza C.

Figura 25. Parámetros para la pieza D. Figura 26. Parámetros para la pieza E.

Figura 25. Parámetros para la pieza D.

Figura 25. Parámetros para la pieza D. Figura 26. Parámetros para la pieza E.

Figura 26. Parámetros para la pieza E.

Figura 27. Parámetros para la pieza F. 7. Modelos Dinámicos El proceso para obtener los

Figura 27. Parámetros para la pieza F.

7. Modelos Dinámicos

El proceso para obtener los modelos dinámicos se efectuó mediante el software SYMORO (Symbolic Modelingof Robots) el cual se describe a continuación:

A. Cargar el archivo con los parámetros base o tabla de parámetros geométricos

B. Se ubica Dynamic en la parte superior del software SYMORO

C. Se ubica Inverse Dynamic model para generar un archivo. DYN

D. Ubicar Direct Dynamic model para generar un archivo.DDM

E. Se ubica la pestaña de optimizer en donde se ubicarán los archivos anteriores

F. Generar la optimización sobre los parámetros deseados como salidas de cada modelo dinamo.

G. Generar archivo .m para ser utilizados en Matlab

H. Modificar los archivos como funciones de Matlab

Al lograr realizar los anteriores pasos se obtiene los siguientes archivos en Matlab:

7.1. Modelo dinámico inverso

% (********************************************)

% (** SYMORO+ : SYmbolic MOdelling of RObots **)

% (**========================================**)

(**

% IRCCyN-ECN - 1, rue de la Noe

**)

(**

% B.P.92101

**)

(**

% 44321 Nantes cedex 3, FRANCE

**)

(**

% www.irccyn.ec-nantes.fr

**)

% (********************************************)

% Name of file : D:\Maestria\Sistemas Robo'ticos en Me'dicina\Archivo Ejemplo

Symoro\RobotCompleto\RobotCompletobaseI.parbase.dyn

%

Geometric parameters

 

%

j

ant

mu

sigma gamma b

alpha d

theta r

 

%

1

0

1

0

0

0

0

0

q1

0

%

-Pi

%

2

1

1

0

0

0

---

0

q2

0

%

2

%

3

2

1

0

0

0

0

d3

q3

0

%

Pi

%

4

3

0

0

0

0

--

0

q4

R4

%

2

%

-Pi

%

5

4

0

0

0

0

---

0

q5

0

%

2

%

Pi

%

6

5

0

0

0

0

--

0

q6

0

%

2

%

Inertial parameters

 

%

j

XX

XY

XZ

YY

YZ

ZZ

MX

MY

MZ

M

Ia

%

1

0

0

0

0

0

ZZ1R

0

0

0

0

0

%

2

XX2R

XY2

XZ2R

0

YZ2

ZZ2R

MX2R

MY2

0

 

0

0

%

3

XX3R

XY3

XZ3

0

YZ3

ZZ3R

MX3

MY3R

0

0

IA3

%

4

XX4R

XY4

XZ4

0

YZ4

ZZ4R

MX4

MY4R

0

0

0

%

5

XX5R

XY5

XZ5

0

YZ5

ZZ5R

MX5

MY5R

0

0

0

%

6

XX6R

XY6

XZ6

0

YZ6

ZZ6

MX6

MY6

0

0

0

%

External forces,friction parameters, joint velocities and accelerations

%

j

FX

FY

FZ

CX

CY

CZ

FS

FV

QP

QDP

 

%

1

FX1

FY1

FZ1

CX1

CY1

CZ1

FS1

FV1

QP1

QDP1

 

% 2

FX2

FY2

FZ2

CX2

CY2

CZ2

FS2

FV2

QP2

QDP2

% 3

FX3

FY3

FZ3

CX3

CY3

CZ3

FS3

FV3

QP3

QDP3

% 4

FX4

FY4

FZ4

CX4

CY4

CZ4

FS4

FV4

QP4

QDP4

% 5

FX5

FY5

FZ5

CX5

CY5

CZ5

FS5

FV5

QP5

QDP5

% 6