Tarea 4
Simulación De Los Movimientos
Del Robot KR 16 L6-2 Y Trazado De
Ruta De La Herramienta
Índice
1. Introducción
1.1. Robot KR 16 L6-2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2. Desarrollo
1
2
3
2.1. Parámetros Geométricos del robot KR 16 L6-2 . . . . . . . . . . . . . . . . . . . .
3
2.2. Modelo cinemático directo de posición . . . . . . . . . . . . . . . . . . . . . . . .
3
2.3. Simulación de los Movimientos del Robot y Trazado de Ruta de la Herramienta .
5
3. Conclusión
6
4. Fuente de información
7
Modelado cinemático de posición
1.
1
Introducción
El modelado cinemático directo de posición de un robot manipulador es la relación que permite determinar el vector x de coordenadas operacionales del robot en función de su configuración
q:
x = f (q)
donde f es una función vectorial.
El modelado directo consiste en el establecimiento de las funciones que hacen posible determinar las coordenadas operacionales de un robot a partir de sus coordenadas articulares. Puesto
que las relaciones existentes entre las coordenadas operacionales y las variables articulares de
un robot dependen de la estructura geométrica de éste, se opta por emplear un procedimiento
normalizado para la descripción del esquema cinemático del robot, es decir, la represenación en
diagrama de alambre del robot. Para tal fin, en la sección (2) se presenta este procedimiento
basado en los parámetros de Denavit-Hartenberg modificados.
Conforme a lo establecido anteriormente, se tiene que al eslabón fijo de un robot se le asigna
un marco de referencia Σ0 , y que al órgano terminal (eslabón 6) se le asigna un marco Σ6 .
Por lo tanto, considerando las propiedades de las matrices de transformación homogéneas ,
la matriz 60 T define la situación del órgano terminal de robot respecto al marco 0. Se sabe que
esta matriz se puede obtener mediante:
6
0T
= 01 T 12 T 23 T 34 T 45 T 56 T
(1)
Obsérvese que, mientras que la matriz del lado izquierdo de esta última expresión contiene
implicitamente las coordenadas operacionales, el producto de las matrices del lado derecho determina implı́citamente las componentes de la función vectorial f. En consecuencia, el desarrollo
de este producto equivale a la resolución del modelo directo de posición.
Robótica
1.1.
2
Robot KR 16 L6-2
A continuación, se presentan vistas del caso de estudio, el robot KUKA KR 16 L6-2, las
cuales muestran la estructra geométrica del robot, también se muestra el esquema cinemático de
este manipulador. Además se muestran dos vistas de la herramienta propuesta.
(a) Vista lateral del robot
(b) Vista superior del robot
Figura 1: Vista lateral y superior del robot KR 16 L6-2.
Figura 2: Esquema cinemático del manipulador KUKA KR 16 L6-2
Modelado cinemático de posición
3
Figura 3: Herramienta propuesta para el manipulador KUKA KR 16 L6-2
2.
2.1.
Desarrollo
Parámetros Geométricos del robot KR 16 L6-2
En la figura 2 se indican los marcos de referencia que se asignan a cada eslabón de acuerdo a
la convención establecida. A partir de estos marcos se han identificado los parámetros de DenavitHartenberg modificados en la tercera tarea del curso, los cuales se retoman en la tabla 1. Donde
d2 = 260 mm, d3 = 680 mm, d4 = 35 mm, r4 = 970 mm, dh = 49.8 mm y dh = 519 mm.
j
1
2
3
4
5
6
σj
0
0
0
0
0
0
αj
0
90◦
0
−90◦
90◦
90◦
dj
0
d2
d3
d4
0
0
θj
θ1
θ2
θ3
θ4
θ5
θ6
rj
0
0
0
r4
0
0
Tabla 1: Parámetros geométricos del manipulador KUKA KR 16 L6-2.
2.2.
Modelo cinemático directo de posición
En la tercera tarea del curso se han obtenido las 6 matrices elementales correpondientes
a cada uno de los marcos unidos a los eslabones del robot KUKA KR 16 L6-2, de acuerdo a
los parámetros geométricos mostrados en la tabla 1, a continuación se presentan los resultados
obtenidos, empleado la notación sj ≡ sen(θj ), cj ≡ cos(θj ):
Robótica

c1 −s1

0
s1 c1
1T = 
0
0
0
0

c4 −s4

0
3
 0
4T = 
−s4 −c4
0
0
0
0
1
0

0
0

0
1

0 d4
1 r4 

0 0
0 1


c2 −s2 0 d2
0
0 −1 0 
1


T
=
2
s2 c2
0 0
0
0
0 1


c5 −s5 0 0
0
0 −1 0
4


5T = 
s 5 c5
0 0
0
0
0 1

c3 −s3

2
s3 c3
3T = 
0
0
0
0
4

0 d3
0 0

1 0
0 1

c6 −s6 0 0
0
0 −1 0
5


6T = 
s6 c6
0 0
0
0
0 1

La matriz 60 T define la situación del órgano terminal de robot respecto al marco 0. Sustituyendo las matrices elementales anteriores en la ecuación (1) y realizando los productos correspondientes, se han determinado los elementos de 60 T :


t11 t12 t13 t14
t21 t22 t23 t24 
0


6T = 
t31 t32 t33 t34 
0 0 0 1
Donde:
t11
t21
t31
t12
t22
t32
t13
t23
t33
t14
t24
t34
= c1 c2+3 c4 c5 c6 − c5 c6 s1 s4 − c1 c6 s2+3 s5 + c4 s1 s6 + c1 c2+3 s4 s6
= c2+3 c4 c5 c6 s1 + c1 c5 c6 s4 − c6 s1 s2+3 s5 − c1 c4 s6 + c2+3 s1 s4 s6
= c4 c5 c6 s2+3 + c2+3 c6 s5 + s2+3 s4 s6
= c4 c6 s1 + c1 c2+3 c6 s4 − c1 c2+3 c4 c5 s6 + c5 s1 s4 s6 + c1 s2+3 s5 s6
= −c1 c4 c6 + c2+3 c6 s1 s4 − c2+3 c4 c5 s1 s6 − c1 c5 s4 s6 + s1 s2+3 s5 s6
= c6 s2+3 s4 − c4 c5 s2+3 s6 − c2+3 s5 s6
= c1 c5 s2+3 + c1 c2+3 c4 s5 − s1 s4 s5
= c5 s1 s2+3 + c2+3 c4 s1 s5 + c1 s4 s5
= −c2+3 c5 + c4 s2+3 s5
= d2 c1 + d3 c1 c2 + d4 c1 c2+3 − r4 c1 s2+3
= d2 s1 + d3 c2 s1 + d4 c2+3 s1 − r4 s1 s2+3
= r4 c2+3 + d3 s2 + d4 s2+3
Modelado cinemático de posición
2.3.
5
Simulación de los Movimientos del Robot y Trazado de Ruta de
la Herramienta
A continuación, se aplicará el modelo directo de posición obtenido anteriormente para el
desarrollo de un programa de MATLAB para simular los movimientos del robot y trazar la
ruta que describe un punto Oh de la herramienta, cuando el robot sigue las siguientes consignas
articulares:
θi = θiIni + δθi fc (t)
(2)
Donde:
i
1
2
3
4
5
6
θiIni
0◦
90◦
0◦
0◦
0◦
0◦
δθi
−90◦
−45◦
45◦
−90◦
90◦
90◦
1
t
sen
fc (t) = −
T
2π
2πt
T
Considerando:
T = 5[s]
0≤t≤T
y la herramienta propuesta en la figura 3.
El código para la simulación desarrollado se incluye al final de este trabajo. Los resultados de
la simulación se muestran en las siguientes figuras, cuyos momentos de captura fueron distintos
a fin de observar la evolución de la simulación. Además se observa la ruta trazada que siguió el
origen del marco de la herramienta propuesta (punto Oh ) en el tiempo de simulación establecido
T.
Robótica
(a) Configuración de transición 1
(b) Configuración de transición 2
(c) Configuración de transición 3
(d) Configuración de transición 4
6
Figura 4: Configuraciones de transición en diversos momentos.
3.
Conclusión
En este tarea se logró desarrollar un código en MATLAB para simular los movimientos del
robot KUKA KR 16 L6-2 y trazar la ruta que describe el origen del marco de la herramienta
propuesta en clase, esto mediante la aplicación del modelado cinemático directo de posición
obtenido en la tarea anterior y siguiendo consignas articulares de movimiento cicloidal. Esta
tarea, a su vez, fue de gran utilidad para practicar el desarrollo de software que permita visualizar
dinámicamente la cinemática de un robot en el espacio y verificar que no ocurran colisiones entre
los eslabones del robot en una determinada configuración.
Modelado cinemático de posición
4.
Fuente de información
Pámanes J. A. Robótica: Introducción al modelado de manipuladores.
7
%%%%%%%%%%%%%%%%% Simulación de movimientos y trazado de Oh %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%% Robot KUKA KR 16 L6-2 %%%%%%%%%%%%%%%%%%%%%%%%%%
clc;clear;
syms th1 th2 th3 th4 th5 th6 %Definición de las variables articulares del robot.
% Posiciones iniciales de las variables articulares del robot.
th1_ini = deg2rad( 0);
th2_ini = deg2rad(90);
th3_ini = deg2rad( 0);
th4_ini = deg2rad( 0);
th5_ini = deg2rad( 0);
th6_ini = deg2rad( 0);
% Posiciones finales de las variables articulares del robot.
th1_fin = deg2rad(-90);
th2_fin = deg2rad(-45);
th3_fin = deg2rad( 45);
th4_fin = deg2rad(-90);
th5_fin = deg2rad( 90);
th6_fin = deg2rad( 90);
%%%%%%%%%%%%%%%%%%%%%%%%% Parámetros Geométricos %%%%%%%%%%%%%%%%%%%%%%%%%%
d2 = 260;
d3 = 680;
d4 = 35;
r4 = 970;
dh = 49.8;
rh = 519;
%%%%%%%%%%%%%%%%%%%% Matriz para dibujar la base %%%%%%%%%%%%%%%%%%%%%%%%%%
Tb0 = [1 0 0 0 ;
0 1 0 0 ;
0 0 1 261;
0 0 0 1];
%%%%%%%%%%%%%% Matriz del marco de la herramienta propuesta %%%%%%%%%%%%%%%
T6h = [sin(pi/4) 0 -sin(pi/4)
-dh ;
0
1
0
0 ;
cos(pi/4) 0
cos(pi/4)
rh ;
0
0
0
1 ];
np = 40;
T = 5;
for i = 0:np
t = T*(i/np); % Tiempo de la simulación actual.
clf; % Borrar ventana de la figura actual.
fc = t/T - (1/(2*pi))*sin((2*pi*t)/T); % Consigna articular cicloidal.
% Movimiento cicloidal de las articulaciones desde una posicion inicial
th1 = th1_ini + th1_fin*fc;
th2 = th2_ini + th2_fin*fc;
th3 = th3_ini + th3_fin*fc;
th4 = th4_ini + th4_fin*fc;
th5 = th5_ini + th5_fin*fc;
th6 = th6_ini + th6_fin*fc;
% Matrices elementales del
T01 = [cos(th1) -sin(th1)
sin(th1) cos(th1)
0
0
robot KUKA
0 0 ;
0 0 ;
1 0 ;
0
0
0 1];
T12 = [cos(th2) -sin(th2) 0
0
0
-1
sin(th2) cos(th2) 0
0
0
0
d2;
0 ;
0 ;
1];
T23 = [cos(th3) -sin(th3) 0
sin(th3) cos(th3) 0
0
0
1
0
0
0
d3;
0 ;
0 ;
1];
T34 = [cos(th4) -sin(th4) 0
0
0
1
-sin(th4) -cos(th4) 0
0
0
0
d4;
r4;
0 ;
1];
T45 = [cos(th5) -sin(th5) 0
0
0
-1
sin(th5) cos(th5) 0
0
0
0
0 ;
0 ;
0 ;
1];
T56 = [cos(th6) -sin(th6) 0
0
0
-1
sin(th6) cos(th6) 0
0
0
0
0 ;
0 ;
0 ;
1];
%Obtención del modelo directo de posición del robot
T02 = T01*T12;
T03 = T02*T23;
T04 = T03*T34;
T05 = T04*T45;
T06 = T05*T56;
T0h = T06*T6h;
%%%%%%%% Vectores de posicion vistos desde la base %%%%%%%%%%%%%%%%%%%%
%%%%%% para dibujar los eslabones y articulaciones %%%%%%%%%%%%%%%%%%%%
Rb1
= Tb0 * [T01(1,4) T01(2,4) T01(3,4) 1]';
Rb2
= Tb0 * [T02(1,4) T02(2,4) T02(3,4) 1]';
Rb3
= Tb0 * [T03(1,4) T03(2,4) T03(3,4) 1]';
Rb3_a = Tb0 * T03 * [d4 0 0 1]';
Rb4
= Tb0 * [T04(1,4) T04(2,4) T04(3,4) 1]';
Rb5
= Tb0 * [T05(1,4) T05(2,4) T05(3,4) 1]';
Rb6
= Tb0 * [T06(1,4) T06(2,4) T06(3,4) 1]';
Rb6_a = Tb0 * T06 * [0 0 115
1]';
Rb6_b = Tb0 * T06 * [0 0 469.2 1]';
Rbh
= Tb0 * [T0h(1,4) T0h(2,4) T0h(3,4) 1]';
%%%%%%%%%%%%%%%%%%%%%%%%%%% Posiciones para dibujar %%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%% eslabones y articulaciones %%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%% e = eslabón %%%%%%%%%%%% a = articulación %%%%%%%%%%%%%%%%
e1x=[Tb0(1,4) T01(1,4)]; e1y=[Tb0(2,4) T01(2,4)]; e1z=[Tb0(3,4) T01(3,4)];
a2x=[Tb0(1,4) Rb2(1) ]; a2y=[Tb0(2,4) Rb2(2) ]; a2z=[Tb0(3,4) Rb2(3) ];
e2x=[Rb2(1)
Rb3(1) ]; e2y=[Rb2(2) Rb3(2)
]; e2z=[Rb2(3)
Rb3(3) ];
%%%%%%%%%%%%%%%%%%%%%%%% Codo del eslabon 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e3ax=[Rb3(1) Rb3_a(1)];
e3ay=[Rb3(2) Rb3_a(2)]; e3az=[Rb3(3) Rb3_a(3)];
e3bx=[Rb3_a(1) Rb4(1) ];
e3by=[Rb3_a(2) Rb4(2) ]; e3bz=[Rb3_a(3) Rb4(3) ];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a5x =[Rb4(1) Rb5(1) ];
a5y=[Rb4(2) Rb5(2) ];
a5z=[Rb4(3) Rb5(3) ];
e5x =[Rb6(1)
Rb6_a(1) ];
e5y=[Rb6(2)
Rb6_a(2) ];
e5z=[Rb6(3)
Rb6_a(3)
];
%%%%%%%%%%%%%%%%%%%%%%%%%% Herramienta (antorcha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
e6ax=[Rb6_a(1) Rb6_b(1) ]; e6ay=[Rb6_a(2) Rb6_b(2) ]; e6az=[Rb6_a(3)
Rb6_b(3) ];
e6bx=[Rb6_b(1) Rbh(1) ]; e6by=[Rb6_b(2)
Rbh(2) ]; e6bz=[Rb6_b(3)
Rbh(3) ];
%%%%%%%%%%%%%%%%%%%%%%%% Dibujo del robot %%%%%%%%%%%%%%%%%%%%%%%%%%%
plot3(e1x,e1y,e1z,'k',a2x,a2y,a2z,'-ok',e2x,e2y,e2z,'-ok',...
e3ax,e3ay,e3az,'k',e3bx,e3by,e3bz,'k',a5x,a5y,a5z,'k',e5x,e5y,e5z,'-ok',....
e6ax,e6ay,e6az,'b',e6bx,e6by,e6bz,'b','linewidth',2);grid on
hold on
%%%%%%%%%%%%%%%%%%%%%%%%% Dibujo
dmb = 200; %Dimensión de la base
plot3([-dmb dmb],[-dmb -dmb],[0
hold on
plot3([-dmb dmb],[ dmb dmb],[0
hold on
plot3([-dmb -dmb],[ dmb -dmb],[0
hold on
plot3([ dmb dmb],[ dmb -dmb],[0
hold on
de la base %%%%%%%%%%%%%%%%%%%%%%%%%%%
0],'K','linewidth',3)
0],'K','linewidth',3)
0],'K','linewidth',3)
0],'K','linewidth',3)
pnx2 (i+1) = Rbh(1);
pny2 (i+1) = Rbh(2);
pnz2 (i+1) = Rbh(3);
if i>1
for j=2:i
plot3([pnx2(j-1),pnx2(j)],[pny2(j-1),pny2(j)],[pnz2(j1),pnz2(j)],'r','linewidth',2);
hold on
end
end
axis([-1000,1000,-1000,1000,-100,1500])
view([0.6 1 0.3])
xlabel('X');
ylabel('Y');
zlabel('Z');
pause(0.1)
end