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.

A3 Detalle A

A1

A2

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.

A4
A6

A5
Figura 4. Detalle A.
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 qi .

1 0 0 0 
0 1 0 0 
Ab  
0 0 1 700
 
0 0 0 1 

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

cos(q1) 0 sin(q1) 750*cos( q1) 

 sin(q1) 0  cos(q1) 750*sin( q1) 
0
A1  
 0 1 0 0 
 
 0 0 0 1 

cos(q 2)  sin( q 2) 0 1250 * cos(q 2)

 sin( q 2) cos(q 2) 0 1250 * sin( q 2) 
1
A2  
 0 0 1 0 
 
 0 0 0 1 

cos(q3) 0 sin(q3) 0
 sin(q3) 0  cos(q3) 0
2
A3  
 0 1 0 0
 
 0 0 0 1
 cos(q 4) 0 sin(q 4) 0 
 sin(q 4) 0  cos(q 4) 0 
3
A4  
 0 1 0 1500 
 
 0 0 0 1 

cos(q5) 0 sin(q5) 0
 sin(q5) 0  cos(q5) 0 
4
A5  
 0 1 0 0
 
 0 0 0 1

cos(q 6) sin(q 6) 0 0 
 sin(q6)  cos(q6) 0 0 
5
A6  
 0 0 1 230 
 
 0 0 0 1 

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

T 0A6  Ab*0 A1*1A2 *2 A3*3A4 *4 A5 *5A6

En donde 0 A6 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 6 AH , supondremos la misma orientación del
sistema de coordenadas 6 y un desplazamiento de 100 mm en sentido de Z 6:

6
AH  
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.

 c1* s 2* s3  c1* c2* c3 s1 c1* c2* s3  c1* s2* c3 1250* c1* c2  750* c1
  s1* s 2* s3  s1* c2* c3 c1 s1* c2* s3  s1* s 2* c3 1250* s1* c2  750* s1
T1  A3  
0 
 c 2* s3  s 2* c3 0 s 2* s3  c2* c3 1250* s 2 
 
 0 0 0 1 

Por último, la matriz T2 nos indica la localización del eje de coordenadas

asociado al elemento 6 con respecto a la base.

c 4* c5* c6  s 4* s6 c 4* c5* s6  s 4* c6 c4* s5 230* c4* s5 

 s 4* c5* c6  c 4* s6 s 4* c5* s6  c4* c6  s 4* s5 230* s 4* s5 
T2  A3  
6 
 s5* c6 s5* s6 c5 1500  230* c5
 
 0 0 0 1 
 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 q6 ) para que su extremo se posicione y oriente según la
T

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 = 0 Z6
0
Z6 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 · 0Z6

Con esta multiplicación obtemos el concepto anterior pero en coordenadas

x,y,z.
0
P6 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

𝑃𝑚 = 0𝑃6 − 𝑑 · 0𝑍6

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
a
ß
x
H

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

Es sencillo calcular β
Pz  d1
  a tan( )
PR  d 2
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)

𝑑32 = 𝑎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)

x
cos     x  d3 *cos 
d3

Del sistema despejamos una ecuación:

d 23  a 2  x 2   a 2  d 23  x 2

d 24  d 23  x2  H 2  2* H * x  x2

d 24  d 33  P2 z  P2 y  P2 x  2* H * d3 *cos 

Obteniendo:

d 23  H 2  d 2 4
cos  
2* d3 * 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 ?
ß
d3·Cosa
d4·Cos?
H
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 ?
ß
d3·Cosa
d4·Cos?
H

𝐻 < 𝑑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.
 Para las tres primeras articulaciones:

% 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 el cálculo de las tres últimas 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

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
R6  3R4 * 4 R5 * 5 R6

c 4* c5* c6  s 4* s6 c4* c5* s6  s4* c6 c4* s5 

3
R6   s 4* c5* c6  c 4* s6 s 4* c5* s6  c4* c6 s 4* s5 
 s5* c6 s5* s6 c5 

Por lo que algebraicamente sabemos

3
𝑅6 2,3
𝑞1 1 = atan⁡
(3 )
𝑅6 1,3

( 3𝑅6 3,3 )
𝑞1 2 = acos⁡
3
𝑅6 3,2
𝑞1 3 = atan⁡
(3 )
𝑅6 3,1

3
Y como conocemos los valores numéricos de la matriz 𝑅6 , gracias a la
ecuación:

𝑃𝑚 = 0𝑃6 − 𝑑 · 0𝑍6

La matriz de transformación del punto 3 respecto del 0 ( 0A3 ), 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);

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

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