MODELADO Y CONTROL CINEMÁTICO DE ROBOTS

CONTROL Y PROGRAMACIÓN DE ROBOTS ANTONIO JOSÉ SÁNCHEZ SALMERÓN

AUTORES MORGAN G. VÁSQUEZ

JOAN CARLES PUCHALT ELIO LÜTHI

UNIVERSIDAD POLITÉCNICA DE VALENCIA 2009

CONTENIDO

INTRODUCCIÓN 1. OBJETIVOS 1.1. OBJETIVO GENERAL 1.2. OBJETIVOS ESPECIFICOS

2. DESCRIPCIÓN DEL SISTEMA ROBOTIZADO 2.1. DESCRIPCION DEL MODELO CINEMÁTICO DEL ROBOT 2.2. ENTORNO DE TRABAJO 2.3. CÁLCULO DE LAS MATRICES DE TRANSFORMACIÓN HOMOGÉNEA 3. CINEMÁTICA INVERSA DEL ROBOT 4. SIMULACIÓN DEL ROBOT EN MATLAB (PICK & PLACE) 5. INTERFAZ GRÁFICA

6. CONCLUSIONES

BIBLIOGRAFÍA

ANEXO

INTRODUCCIÓN

Los robots industriales, surgidos a principios de la década de los sesenta, lograron superar importantes dificultades tanto tecnológicas como socioeconómicas y alcanzaron su mayoría de edad en la década de los noventa, estabilizándose entonces su demanda y consiguiendo una aceptación y reconocimiento pleno en la industria. Alcanzada la madurez del robot industrial, los ingenieros e investigadores dedicados a la robótica se plantearon dar un paso más hacia el mito, sacando a los robots de las líneas de producción y dotándoles de la capacidad de actuar en otros entornos, poco o nada estructurados, y de colaborar con personas no especializadas en su uso. A pesar de su enorme dificultad científico tecnológica de este objetivo, los esfuerzos realizados y el aprovechamiento de los avances conseguidos en las disciplinas que sirven de soporte a la robótica – como son la electrónica y la informática – ha dado lugar a que en la primera década del siglo XXI se viva un nuevo resurgimiento de la robótica, evolucionando desde el entorno industrial a dos nuevos sectores: la robótica de servicios profesionales y la robótica profesional. La robótica es una disciplina en auge, y la formación del profesional de la ingeniería, tanto en sus ramas de automatización, mecánica, informática o incluso generalista, no es indiferente a esta situación, incluyendo desde finales de los años ochenta a la robótica como parte de sus enseñanzas. La robótica posee un reconocido carácter interdisciplinar, partiendo en ella diferentes ciencias básicas y tecnologías tales como la teoría de control, la mecánica, la electrónica el álgebra y la informática, entre otras. [1]

OBJETIVOS

1.1. OBJETIVO GENERAL:

Modelar y simular el comportamiento cinemático de un sistema robotizado industrial concreto mediante Matlab, para entender su funcionamiento.

1.2. OBJETIVOS ESPECÍFICOS

Comprender la utilidad y las formas de operar con transformaciones homogéneas.

Dominar el modelo matemático de representación cinemática propuesto por Denavit – Hartenberg.

Familiarizarse con la herramienta CAE Matlab de gran utilidad en robótica y en otras áreas de Ingeniería de Sistemas y Automática.

Comprender algunos conceptos básicos de generación de trayectorias.

2. DESCRIPCIÓN DEL SISTEMA ROBOTIZADO

El robot asignado para el desarrollo de la presente práctica es el modelo KR120 de KUKA, el cuál posee 6 articulaciones de tipo rotacional.

Figura 1. Robot KR120 de KUKA.

Los datos sobre las dimensiones del robot y sus articulaciones se pueden encontrar en la web de KUKA [2].

Figura 2. Dimensiones del robot KR120 de KUKA (mm).

Basándonos en los datos del robot que ofrece el fabricante, hemos asignado una posición de reposo. A partir de ésta, realizaremos un modelo del robot representado por líneas mediante Matlab.

Esto se obtiene con la implementación de funciones que permiten dibujar, modificar (tanto la longitud como la posición de los elementos que componen el robot) y asignar las respectivas articulaciones según los parámetros de Denavit-Hartenberg (D-H) establecidos que podremos ver en el apartado 2.1.

Figura 3. Modelo del robot en Matlab.

El color de cada línea de la figura anterior, representa un elemento o eslabón del modelo del robot. Cada elemento está separado por una de las articulaciones, que como hemos mencionado anteriormente, son del tipo rotacional.

Figura 4. Detalle A.

A1

A2

A3 Detalle A

A4

A5

A6

2.1. DESCRIPCIÓN DEL MODELO CINEMÁTICO DEL ROBOT

Para asignar un sistema de coordenadas para cada una de las articulaciones de un robot, la forma habitual que se suele utilizar en robótica es la representación de Denavit - Hartenberg (D-H). El cual es un método matricial que establece la localización que debe tomar cada sistema de coordenadas ligado a cada eslabón de una cadena articulada, para poder sistematizar la obtención de las ecuaciones cinemáticas de la cadena completa. [2]

Figura 5. Sistema de coordenadas del robot.

En la figura anterior podremos observar los ejes coordenados que se han asignado según las normas de D-H para cada una de las articulaciones del robot. A excepción de la articulación A2, que hemos decidido reubicar en el mismo punto que el de la articulación A3 para la reducción de los cálculos posteriores. Una vez asignados correctamente los ejes coordenados del robot, obtendremos los parámetros de Denavit – Hartenberg. Tabla 1. Parámetros de Denavit – Hartenberg para el robot KR120 de KUKA.

Articulación Θ (Rot. z) d (Transl. z) a (Transl. x) α (Rot. x)

0 0 700 0 0

1 0* 0 750 π/2

2 π/2* 0 1250 0

3 0* 0 0 π/2

4 0* 1500 0 π/2

5 0* 0 0 π/2

6 0* -230 0 π

En la tabla 1, la primera fila nos indica el sistema de referencia de Matlab. La matriz de transformación obtenida con estos parámetros nos relaciona el sistema de coordenadas cero del robot (X0,Y0,Z0) con el de Matlab. Además esta matriz de transformación es una matriz fija, por lo cual no incluye variable de articulación. En la siguiente figura podremos ver el modelo del robot con cada uno de sus respectivos ejes coordenados.

Figura 6. Sistema de coordenadas del robot.

2.2. ENTORNO DE TRABAJO DEL ROBOT

El entorno de trabajo del robot que hemos elegido, consiste en el ensamble de una pieza, específicamente en el capó de un coche. Este entorno está conformado por el robot, donde le hemos asignado un único color para mejorar su apariencia y dos bandas transportadoras con las cuales se desplazarán los objetos en el sentido correcto para llevar a cabo el proceso (Figura 7). Estos objetos se han creado por medio de las funciones que hemos utilizado anteriormente para el modelo del robot, con la diferencia de que no se basan en una matriz de D-H, sino en la creación de líneas según la longitud y posición, para obtener la forma del objeto que deseamos construir.

Figura 7. Entorno de trabajo del robot.

El proceso se inicia llevando el coche por medio de una cinta transportadora a una posición cercana al robot, donde éste tomará el capó que viene en sentido contrario al del coche y lo pondrá sobre el mismo en la posición correcta. Una vez hecho esto, se desplazarán los dos objetos (coche y capó) a una posición al final de la cinta transportadora donde finaliza el proceso.

2.3. CÁLCULO DE LAS MATRICES DE TRANSFORMACIÓN HOMOGÉNEA

Para el cálculo de las matrices de transformación homogénea hemos utilizado Matlab. Estas matrices estarán sólo en función de las variables de articulación del robot. Basándonos en los parámetros de D-H de la tabla 1, obtendremos las matrices para cada cambio de base que permite pasar del sistema asociado al eslabón i-1. Esta matriz dependerá, además de constantes geométricas propias del

eslabón, del grado de libertad iq .

1000

700100

0010

0001

bA

Puesto que hemos elegido el modelo cinemático mínimo al colocar el sistema de coordenadas asociado a la base, este se encuentra desplazado en sentido del eje Z0 a 700 m.m. Este desplazamiento lo tenemos en cuenta por medio de

la matriz bA .

0

1

cos( 1) 0 sin( 1) 750*cos( 1)

sin( 1) 0 cos( 1) 750*sin( 1)

0 1 0 0

0 0 0 1

q q q

q q qA

1000

0100

)2sin(*12500)2cos()2sin(

)2cos(*12500)2sin()2cos(

2

1qqq

qqq

A

1000

0010

0)3cos(0)3sin(

0)3sin(0)3cos(

3

2qq

qq

A

3

4

cos( 4) 0 sin( 4) 0

sin( 4) 0 cos( 4) 0

0 1 0 1500

0 0 0 1

q q

q qA

4

5

cos( 5) 0 sin( 5) 0

sin( 5) 0 cos( 5) 0

0 1 0 0

0 0 0 1

q q

q qA

5

6

cos( 6) sin( 6) 0 0

sin( 6) cos( 6) 0 0

0 0 1 230

0 0 0 1

q q

q qA

Considerando todos los grados de libertad del robot, obtendremos la matriz T:

6

5

5

4

4

3

3

2

2

1

1

0

6

0 ****** AAAAAAAbAT

En donde 6

0 A representa la localización del punto de herramienta con respecto

a la base del robot.

Para calcular la localización del extremo de la herramienta respecto a la última

articulación del robot, es decir HA6 , supondremos la misma orientación del

sistema de coordenadas 6 y un desplazamiento de 100 mm en sentido de Z6:

6

HA

c6*(c5*(s1*s4 + c4*(c1*c2*c3 - c1*s2*s3)) + s5*(c1*c2*s3 + c1*c3*s2)) - s6*(c4*s1 - s4*(c1*c2*c3 - c1*s2*s3)), c6*(c4*s1 - s4*(c1*c2*c3 - c1*s2*s3)) + s6*(c5*(s1*s4 + c4*(c1*c2*c3 - c1*s2*s3)) + s5*(c1*c2*s3 + c1*c3*s2)), c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 + c4*(c1*c2*c3 - c1*s2*s3)), 750*c1 + 1250*c1*c2 - 230*s5*(s1*s4 + c4*(c1*c2*c3 - c1*s2*s3)) + 230*c5*(c1*c2*s3 + c1*c3*s2) + 1500*c1*c2*s3 + 1500*c1*c3*s2;

s6*(c1*c4 - s4*(s1*s2*s3 - c2*c3*s1)) - c6*(c5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) - s5*(c2*s1*s3 + c3*s1*s2)), - c6*(c1*c4 - s4*(s1*s2*s3 - c2*c3*s1)) - s6*(c5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) - s5*(c2*s1*s3 + c3*s1*s2)), s5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) + c5*(c2*s1*s3 + c3*s1*s2), 750*s1 +

1250*c2*s1 + 230*s5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) + 230*c5*(c2*s1*s3 + c3*s1*s2) + 1500*c2*s1*s3 + 1500*c3*s1*s2;

s4*s6*(c2*s3 + c3*s2) - c6*(s5*(c2*c3 - s2*s3) - c4*c5*(c2*s3 + c3*s2)), - s6*(s5*(c2*c3 - s2*s3) - c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 + c3*s2), - c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2), 1250*s2 - 1500*c(2 + 3) + 115*s(4 - 5)*s(2 + 3) - 115*s(2 + 3)*s(4 + 5) - 230*c(2 + 3*c5 + 800; 0, 0, 0, 0, 1] La siguiente matriz de transformación nos indica la localización del eje de coordenadas asociado al elemento 3 con respecto a la base.

0

1 3

1* 2* 3 1* 2* 3 1 1* 2* 3 1* 2* 3 1250* 1* 2 750* 1

1* 2* 3 1* 2* 3 1 1* 2* 3 1* 2* 3 1250* 1* 2 750* 1

2* 3 2* 3 0 2* 3 2* 3 1250* 2

0 0 0 1

c s s c c c s c c s c s c c c c

s s s s c c c s c s s s c s c sT A

c s s c s s c c s

Por último, la matriz T2 nos indica la localización del eje de coordenadas asociado al elemento 6 con respecto a la base.

6

2 3

4* 5* 6 4* 6 4* 5* 6 4* 6 4* 5 230* 4* 5

4* 5* 6 4* 6 4* 5* 6 4* 6 4* 5 230* 4* 5

5* 6 5* 6 5 1500 230* 5

0 0 0 1

c c c s s c c s s c c s c s

s c c c s s c s c c s s s sT A

s c s s c c

3. CINEMÁTICA INVERSA DEL ROBOT

El objetivo del problema cinemático inverso consiste en encontrar los valores que deben adoptar las coordenadas articulares del robot (para nuestro caso

q q1 q2 q3 q4 q5 q6T

) para que su extremo se posicione y oriente según la

determinada localización espacial. Se basa en obtener las articulaciones a partir de la posición y orientación final del Sistema de Coordenadas del Punto Central de la Herramienta (Tool Centre Point, TCP). Así cómo es posible abordar el problema cinemático directo de una manera sistemática a partir de la utilización de matrices de transformación homogéneas, e independientemente de la configuración del robot, no ocurre lo mismo con el problema cinemático inverso, siendo el procedimiento de obtención de las ecuaciones fuertemente dependientes de la configuración del robot. Es así pues que utilizaremos el método de desacoplo, el cual se fundamenta en la división en dos del problema inicial:

Posicionar el punto de muñeca (q1, q2, q3)

La orientación del eje z del último SC, mediante (q4, q5, q6) Previamente a estos dos cálculos hemos de saber en qué posición y orientación se hayará el punto de muñeca (supuestamente SC6). Nuestros datos son la posición y orientación a la que queremos llegar:

𝑛𝑥 𝑠𝑥𝑛𝑦 𝑠𝑦

𝑎𝑥 𝑝𝑥

𝑎𝑦 𝑝𝑦

𝑛𝑧 𝑠𝑥0 0

𝑎𝑧 𝑝𝑧

0 1

El vector 𝑎 representa la orientación de la 𝑧 del Sistema de Coordenadas último. Por lo que cualquier escalar multiplicado por este dará una distancia expresada sobre este vector.

a = Z60

Z60 Es el vector Z del SC 6 respecto a 0 (en este caso el SC fijo)

Si realizamos esta operación, siendo d6 la distancia entre el punto de muñeca y el SC del TCP.

ΔP = d6 · Z60

Con esta multiplicación obtemos el concepto anterior pero en coordenadas x,y,z.

P60 Es el punto al que queremos llegar por lo que Pm es el punto de muñeca

que queremos obtener.

𝑃0 = 𝑃𝑓 − ΔP Visto de manera gráfica.

Con lo que deducimos la fórmula

𝑃𝑚 = 𝑃60 − 𝑑 · 𝑍6

0

Posicionar el punto de muñeca Una vez sabido cual es el punto de muñeca, buscaremos su posicionamiento por medio de las 3 primeras articulaciones, utilizando relaciones trigonométricas. La primera articulación q1:

𝑞1 = tan−1𝑃𝑥𝑃𝑦

Para la 𝑞2 se ha de tener en cuenta que los puntos x e y serán proyecciones

según el ángulo 𝑞1, o que se utilizará un eje auxiliar de verdadera magnitud.

Por lo Px y Py desaparecerán para dar paso al eje R y a su punto Pr7.

𝑃𝑟 = 𝑃𝑥2 + 𝑃𝑦

2

a

ß

H

a

x

𝐻 = 𝑃𝑧 − d1 2 + 𝑃𝑟 − d2 2

Es sencillo calcular β

1

2

tan( )z

R

P da

P d

Tenemos dos triángulos rectángulos, relacionados por un lado y con lo que conseguimos un sistema de 2 ecuaciones y dos incógnitas (resoluble)

𝑑3

2 = 𝑎2 + 𝑥2

𝑑42 = 𝑎2 + (𝐻 − 𝑥)2

Y además buscando la relación entre el ángulo α y una de las incógnitas (en este caso x)

3

3

cos *cosx

x dd

Del sistema despejamos una ecuación:

2 2 2 2 2 2

3 3d a x a d x

2 2 2 2 2

4 3 2* *d d x H H x x

2 3 2 2 2

4 3 32* * *cosz y xd d P P P H d

Obteniendo:

2 2 2

3 4

3

cos2* *

d H d

d H

Ahora fijándonos en los 4 cuadrantes vemos que:

Codo arriba => alpha+beta (q2)

Codo abajo => -alpha-beta (q2) Lo siguiente es buscar la tercera articulación

a

ß

H

?

d3·Cosa

d4·Cos?

Esta última articulación será el ángulo que forma ella misma con la anterior. Ahora buscaremos el ángulo gamma.

𝛾 = 𝑎𝑟𝑐𝑜𝑠 𝐻 − 𝑑3 · 𝐶𝑜𝑠 𝛼

𝑑4

Las dos articulaciones para codo arriba y codo abajo, dependerán evidentemente de q2. Y al igual que antes observando los cuatro cuadrantes tenemos.

Codo arriba => -alpha-gamma+pi/2 (q3)

Codo abajo => -q_abj(2)+beta+gamma+pi/2 (q3)

Con éstas ecuaciones obtendremos los grados de inclinación para los

quales llegaremos al punto de muñeca, aunque también tendremos en cuenta que en el robot real tendremos ángulos máximos:

Árbol Grad Máx Grad Mín Rad Máx Rad Mín

1 +185º -185º 1.0278·π -1.0278·π

2 +120º -70º 0.6667·π -0.3889·π

3 +119º -155º 0.6611·π -0.8611·π

4 +350º -350º 1.9444·π -1.9444·π

5 +125º -125º 0.6945·π -0.6945·π

6 +350º -350º 1.9444·π -1.9444·π

La lógica de programación será que en el caso de que exista alguna q que supere estos máximos entonces, la opción que se calcule se eliminará.

La otra restricción es cuando haya un punto al cual el robot no pueda llegar físicamente. Ese punto cumplirá que la H de los esquemas anteriores:

a

ß

H

?

d3·Cosa

d4·Cos?

𝐻 < 𝑑3 + 𝑑4

Utilizando las restricciones anteriores con las ecuaciones, las introducimos en Matlab completando 3 funciones. Una para las tres primeras articulaciones, otra para las tres últimas y una más para englobar a las dos.

% Resuelve la cinemática inversa para un robot KR120

% Parámetros de entrada:

% Modelo cinemático del robot (tabla DH)

% Localización del TCP0 deseada

% Parámetros de salida:

% Configuracines izquierda y derecha

function [q_arb, q_abj]=ciKR120_0_3(robot,P)

% Cuidado esta función depende de como se defina el modelo del robot (Tabla DH)

% Posibles soluciones para las articulaciones principales

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

d1 = robot(1,3);

d2 = robot(2,2);

d3 = robot(3,2);

d4 = robot(5,3);

%% Articulacio 1

% posicio;

q_1 = atan2(P(2),P(1));

q_arb(1) = q_1;

q_abj(1) = q_1;

%% Articulacio 2

PR=sqrt(P(2)^2+P(1)^2);

H=sqrt((PR-d2)^2+(P(3)-d1)^2);

if H>(d3+d4)

warndlg('Punto Fuera de Alcance. No se puede llegar a esa

distancia','Peligro')

end

alpha=acos((d3^2+H^2-d4^2)/(2*d3*H));

beta=atan2(P(3)-d1,PR-d2);

q_arb(2)=alpha+beta;

q_abj(2)=-alpha+beta;

%% Articulacio 3

gamma=acos((H-d3*cos(alpha))/d4);

q_arb(3)=-alpha-gamma+pi/2;

q_abj(3)=-q_abj(2)+beta+gamma+pi/2;

%% Valores máximos de las articulaciones

% No existirá solución si alguno de los ángulos excede de su máximo

if ((q_arb(1)>1.0278*pi) || (q_arb(2)>0.6667*pi) || (q_arb(3)>3.6477*pi))

q_arb=[0 pi/2 0];

('Máximo alcanzado en una articulación para q_arb (codo arriba). No existe

solución para esta configuración')

end

if ((q_abj(1)<-1.0278*pi) || (q_abj(2)<-0.3889*pi) || (q_abj(3)<-1.1345*pi))

q_abj=[0 pi/2 0];

('Máximo alcanzado en una articulación para q_abj (codo abajo). No existe

solución para esta configuración')

end

Para las tres primeras articulaciones:

% Devuelve las dos configuraciones de la muñeca para alcanzar la

orientación deseada. % Datos de entrada: la matriz que define el robot (rob) la matriz

homogenea deseada (MH), % y las coordenadas de las tres primeras articulaciones.

function [q1, q2]=ciKR1203_6(robot,MH,q3)

% Se encuentra la matriz de transformación del origen al sis. coord.

3 y la matriz % de rotación correspondiente.

A03=dh(robot,0,0)*dh(robot,1,q3(1))*dh(robot,2,q3(2))*dh(robot,3,q3(3

)); R03=rotacion(A03);

% Se encuentra la matriz de rotación del sist. coord. 3 al 6. R36=inv(R03)*rotacion(MH);

% Se extraen los datos de las tres últimas coordenadas generalizadas

para los dos casos.

q1(1)=atan2(R36(2,3),R36(1,3)); q1(2)=acos(R36(3,3)); q1(3)=atan2(R36(3,2),R36(3,1));

q2(1)=q1(1)-pi; q2(2)=q1(2); q2(3)=q1(3)-pi;

%% Valores máximos de las articulaciones % No existirá solución si alguno de los ángulos excede de su máximo % if ((q1(1)>1.944*pi) || (q1(2)>2.1817) || (q1(3)>6.1087)) % q1=[0 0 0]; % ('Máximo alcanzado en una articulación para q1 (codo arriba).

No existe solución para esta configuración') % end % % if ((q2(1)<-1.944*pi) || (q2(2)<-2.1817) || (q2(3)<-6.1087)) % q2=[0 0 0]; % ('Máximo alcanzado en una articulación para q2 (codo abajo). No

existe solución para esta configuración') % end

Para el cálculo de las tres últimas articulaciones:

El código anterior también tiene restricciones, pero se las hemos comentado para que no funcionen. Si implementamos estas restricciones se nos reduce el campo de alcance, ya que se reducen las posibilidades para crear trayectoria con la spline cúbica natural, por lo que nos da error. Por ello no hemos tenido en cuenta estas restricciones de los ángulos máximos para estas últimas articulaciones. Para poder solucionar este problema deberíamos llevar el robot punto a punto utilizando, por ejemplo, una función de línea recta en el espacio

cartesiano (pero no tenemos más tiempo y es lo que nos falta para que esté perfecto). La obtención de la última q se ha conseguido gracias a las matrices de transformación obtenidas algebraicamente (gracias al Matlab en modo simbólico):

3 3 4 5

6 4 5 6* *R R R R

3

6

4* 5* 6 4* 6 4* 5* 6 4* 6 4* 5

4* 5* 6 4* 6 4* 5* 6 4* 6 4* 5

5* 6 5* 6 5

c c c s s c c s s c c s

R s c c c s s c s c c s s

s c s s c

Por lo que algebraicamente sabemos

𝑞1 1 = atan(𝑅6

3 2,3

𝑅63 1,3

)

𝑞1 2 = acos( 𝑅63 3,3 )

𝑞1 3 = atan(𝑅6

3 3,2

𝑅63 3,1

)

Y como conocemos los valores numéricos de la matriz 𝑅63 , gracias a la

ecuación:

𝑃𝑚 = 𝑃60 − 𝑑 · 𝑍6

0

La matriz de transformación del punto 3 respecto del 0 ( A30 ), sacamos la

matriz de rotación:

A03=dh(robot,0,0)*dh(robot,1,q3(1))*dh(robot,2,q3(2))*dh(robot,3,q3(3));

R03=rotacion(A03);

Y por último calculamos la variación de la matriz de rotación inicial de la final:

R36=inv(R03)*rotacion(MH);

𝑅63 = 𝑅36

% Devuelve las cuatro soluciones posibles de la cinemática inversa de

un robot PRR. % Los datos de entrada son la matriz que define el robot y la matriz

homogenea deseada % del sistems de coordenadas del origen al del efector final.

function [q]=invKR120(rob, matriz)

% Se extraen la matriz de rotación, el vector de posición y el eje z

deseado.

R06=rotacion(matriz); p6=pos(matriz); z6=zeta(matriz);

% Se encuentra la posición deseada de los sist. coord. 4 y 5.

d6=-rob(7,3); p45=p6-d6*z6;

% Se encuentran las dos configuraciones posibles que ponen los sist.

coord. 4 y 5 % en la posición deseada.

[q_izd, q_der]=ciKR120_0_3(rob,p45);

% Se encuentra las coordenadas de las tres últimas articulaciones

para los dos casos. [q_i1, q_i2]=ciKR1203_6(rob,matriz,q_izd); [q_d1, q_d2]=ciKR1203_6(rob,matriz,q_der);

q1(1,:)=[q_izd q_i1]; q1(2,:)=[q_izd q_i2]; q1(3,:)=[q_der q_d1]; q1(4,:)=[q_der q_d2];

%% Tan sólo devuelve las soluciones existentes n=size(q1); n=n(1,1); k=1; for i=1:3 if p6(i)>0 p6max(i)=p6(i)*1.001; else p6max(i)=p6(i)*0.999; end if p6(i)>0 p6min(i)=p6(i)*0.999; else p6min(i)=p6(i)*1.001; end end

Por último está la suma de las dos, que ésta además se asegura de que sólo aparezcan los resultados con solución

q(1,:)=[10 0 0 0 0 0]; for i=1:n xyz=pos(cindir(rob,q1(i,:)'))';

if ((p6min<xyz)&(xyz<p6max)) q(k,:)=q1(i,:); k=k+1; end

end

if q(1,1)==10 warndlg('No existe ninguna solución') end return

5. INTERFAZ GRÁFICA

La interfaz gráfica de Matlab que se ha implementado, nos permite realizar un cambio en la posición de cada una de las articulaciones del robot y a su vez resuelve la cinemática directa, mostrándonos los resultados actualizados.

Por medio de esta interfaz, podremos observar la posición y orientación del robot según variemos los ángulos de giro (en grados) del mismo. Los ángulos están expresados en RPY (roll, picht yaw), donde alpha es la rotación con respecto al eje Z, beta la rotación con respecto a Y, y gamma con respecto a X.

A continuación podremos ver unas imágenes de la interfaz gráfica desarrollada.

6. CONCLUSIONES Aunque costoso, el trabajo ha sido muy interesante, hemos podido desarrollar y desenvolvernos mejor con las matrices y el espacio tridimensional. También nos hemos sorprendido por el gran potencial que puede ofrecer el programa Matlab, el cual no nos ha dado limitaciones con estos principios de robótica. Hemos adquirido conceptos básicos para toda la estructura de elaboración de un sistema robotizado completo, desde visión hasta desplazamientos mecánicos, con sus tareas básicas de órdenes como mover, pick&place,... Para el desarrollo de este trabajo hemos invertido bastante tiempo, debido a la problemática de la cinemática inversa para un robot de 6 grados de libertad. En resumen, el tiempo aproximado será de unas 100 horas cada uno. Ha sido muy didáctico e interesante.

BIBLIOGRAFÍA

[1] FUNDAMENTO DE ROBÓTICA, Antonio Barrientos, Luis Felipe Peñín, Carlos Balaguer, Rafael Aracil. Mc Graw Hill – 2ª Edición. [2] http://www.kuka-robotics.com/spain/es/products/industrial_robots/high/kr120_2_p/

[3] Aníbal Ollero Baturone, ROBÓTICA Manipuladores y robots móviles. Marcombo.