Universidad Cooperativa de Colombia. Hector Luis Sanchez Rodríguez. Juan Pablo Castañeda. Andrés Felipe Cruz. Informe robótica.
1
INFORME ROBOTICA Héctor Luis, Sánchez Rodríguez.
[email protected] Juan Pablo Castañeda.
[email protected] Andrés Felipe Cruz.
[email protected] Universidad Cooperativa de Colombia
Abstract Este — Este
documento presenta el resultado de la cinemática inversa aplicada a un robot UR3 en tres diferentes puntos finales. finales.
Index Term Term — Cinematica, Cinematica, KUKA, UR3, robot.
I. MARCO TEÓRICO CINEMATICA INVERSA
startup_rvc L1=Link('d' L1=Link('d',1.52, ,1.52,'a' 'a',1.20, ,1.20,'alpha' 'alpha',pi/2, ,pi/2,'o 'o ffset',pi/2); ffset' ,pi/2); L2=Link('d' L2=Link('d',,0.93,'a' 0.93,'a',2.44, ,2.44,'alpha' 'alpha',0, ,0,'offset' 'offset',pi/2); ,pi/2); L3=Link('d' L3=Link('d',1.04, ,1.04,'a' 'a',2.13, ,2.13,'alpha' 'alpha',0); ,0); L4=Link('d' L4=Link('d',0, ,0,'a' 'a',0, ,0,'alpha' 'alpha',pi/2, ,pi/2,'offset' 'offset' ,pi/2); L5=Link('d' L5=Link('d',0.85, ,0.85,'a' 'a',0, ,0,'alpha' 'alpha',pi/2); ,pi/2); L6=Link('d' L6=Link('d',0.92, ,0.92,'a' 'a',0, ,0,'alpha' 'alpha',0); ,0); Robot=SerialLink([L1 L2 L3 L4 L5 L6]) Robot.teach
Lo usamos para comprobar que la cinemática directa este bien.
Es una técnica usada para determinar el movimiento dz ax offset Ꝋ α de un conjunto de articulaciones para llegar a un 0 0 0.152 pi/2 0.120 pi/2 punto en específico con el efector final, es decir el 1 1 -0.93 0 0.244 pi/2 objetivo final es encontrar las coordenadas de cada eje articulado en el plano espacial, dependiendo de 2 2 104 0 0.213 0 las características del robot las soluciones podrían ser 3 3 0 pi/2 0 pi/2 múltiples. El cálculo de la cinemática inversa es un 4 0.85 pi/2 0 0 4 procedimiento complejo, ya que consiste en la 5 0.92 0 0 0 5 solución de varias ecuaciones las cuales son dependientes a la configuración del robot. Tabla 1. Resultados cinemática directa. robot UR3 [1] II. DESARROLLO DE LA PRACTICA Por medio de la herramienta Matlab se implementar la cinemática inversa del robot ur3, con la ayuda del código propuesto. Primero se realiza la cinemática directa del robot, encontrando sus ejes y graficando el robot en Matlab por medio de la herramienta startup_rvc. Implementamos los ángulos al código: close all clear all clc
Seminario de robótica.
Luego de esto es hora de aplicarla cinemática inversa, definimos un punto de partida que en este caso en el código se llamara: thi =[0 0 0 0 90 0]
definimos los valores que usaremos en los ángulos inicialmente, en nuestro caso mantendremos el punto inicial igual y variaremos a 3 puntos finales distintos. El valor final se reconocerá en el código como: thf=[15 35 50 -45 90 0]
Universidad Cooperativa de Colombia. Hector Luis Sanchez Rodríguez. Juan Pablo Castañeda. Andrés Felipe Cruz. Informe robótica.
Los valores dentro del corchete se pueden cambiar a cualquier otro Angulo que se desee. Seguido de esto el robot empezara a moverse hasta llegar a dichos ángulos, definido por una cantidad de movimientos n, para encontrar dicha posición, en este caso esta se encuentra restringida hasta 1000.
2
III. CINEMATICA INVERSA Posición Indicial
Nmax=1000; n=1;”
“
Fig. 2. Comprobación cinemática directa [2]
Posición final 1
Fig. 3. Resultado cinemática inversa punto final 1 (Matlab) [3]
Posición final 2
Fig. 1. Robot UR3 [1]
Seminario de robótica.
Universidad Cooperativa de Colombia. Hector Luis Sanchez Rodríguez. Juan Pablo Castañeda. Andrés Felipe Cruz. Informe robótica.
3
T10=Rz(th(1)+pi/2)*dz(1.52)*Rx(pi/2)*dx(1 .20); T21=Rz(th(2)+pi/2)*dz(0.93)*Rx(0)*dx(2.44); T32=Rz(th(3))*dz(1.04)*Rx(0)*dx(2.13); T43=Rz(th(4)+pi/2)*dz(0)*Rx(pi/2)*dx(0); T54=Rz(th(5))*dz(0.85)*Rx(pi/2)*dx(0); T65=Rz(th(6))*dz(0.92)*Rx(0)*dx(0); %definicion matrices de transformacion
Fig. 4. Resultado cinemática inversa punto final 2 (Matlab) [4]
T20=T10*T21; T30=T20*T32; T40=T30*T43; T50=T40*T54; T60=T50*T65;
Posición final 3 % Movimiento inercial r0_ef=T60(1:3,4)-[0 0 0]'; Z0=[0 0 1]'; r1_ef=T60(1:3,4)-T10(1:3,4); Z1=T10(1:3,1:3)*[0 0 1]'; r2_ef=T60(1:3,4)-T20(1:3,4); Z2=T20(1:3,1:3)*[0 0 1]'; r3_ef=T60(1:3,4)-T30(1:3,4); Z3=T30(1:3,1:3)*[0 0 1]';
Fig. 5. Resultado cinemática inversa punto final (Matlab) [5] r4_ef=T60(1:3,4)-T40(1:3,4); Z4=T40(1:3,1:3)*[0 0 1]';
IV. CÓDIGO MATLAB %% close all clear all clc startup_rvc thf=[15 35 50 -45 90 0]'.*pi/180; %Posición y orientación final deseada (en angulos articulares) POF=POef(thf); thi=[0 0 0 0 90 0]'.*pi/180;%Posición y orientación inicial deseada (en angulos articulares) th=thi; %-----------------------------%parametros dh %------------------------------
Seminario de robótica.
r5_ef=T60(1:3,4)-T50(1:3,4); Z5=T50(1:3,1:3)*[0 0 1]'; %parametros dh L_1=Link('d',1.52,'a',1.20,'alpha',pi/2,' offset',pi/2); L_2=Link('d',0.93,'a',2.44,'alpha',0,'offset',pi/2); L_3=Link('d',1.04,'a',2.13,'alpha',0); L_4=Link('d',0,'a',0,'alpha',pi/2,'offset ',pi/2); L_5=Link('d',0.85,'a',0,'alpha',pi/2); L_6=Link('d',0.92,'a',0,'alpha',0); %impresion robot Robot=SerialLink([L_1, L_6]); Robot.plot(th'); Pa=T60(1:3,4);
L_2,L_3,
L_4,L_5,
Universidad Cooperativa de Colombia. Hector Luis Sanchez Rodríguez. Juan Pablo Castañeda. Andrés Felipe Cruz. Informe robótica. Da=tr2rpy(T60(1:3,1:3))'; Convierte una transformada angulos roll-pitch-yaw
4
% tr2rpy homogenea a r0_ef=T60(1:3,4)-[0 0 0]'; Z0=[0 0 1]';
e=POF - [Pa ; Da]; ec2p=sqrt(e(1:3)'*e(1:3));%elevar cuadrado (x y z) ec2o=sqrt(e(4:6)'*e(4:6));%elevar cuadrado (roll pitch yaw)
r1_ef=T60(1:3,4)-T10(1:3,4); Z1=T10(1:3,1:3)*[0 0 1]'; al r2_ef=T60(1:3,4)-T20(1:3,4); Z2=T20(1:3,1:3)*[0 0 1]';
al
r3_ef=T60(1:3,4)-T30(1:3,4); Z3=T30(1:3,1:3)*[0 0 1]';
Beta=0.05; movimiento. epmax=0.01; error posicion max. eomax=0.01; error orientacion max. Nmax=1000; n=1; %%
r4_ef=T60(1:3,4)-T40(1:3,4); Z4=T40(1:3,1:3)*[0 0 1]'; r5_ef=T60(1:3,4)-T50(1:3,4); Z5=T50(1:3,1:3)*[0 0 1]';
while (((ec2p>epmax) || (ec2o>eomax)) && (n
Pa=T60(1:3,4); Da=tr2rpy(T60)'; e=POF - [Pa ; Da];
Jdk=[ cross(Z0,r0_ef) cross(Z1,r1_ef) cross(Z2,r2_ef) cross(Z3,r3_ef) cross(Z4,r4_ef) cross(Z5,r5_ef); ... Z0 Z1 Z2 Z3 Z4 Z5 ];
ec2p=sqrt(e(1:3)'*e(1:3)); ec2o=sqrt(e(4:6)'*e(4:6)); Robot.plot(th'); n=n+1
deltaq=(inv(Jdk'*Jdk)*Jdk')*e; th=th+Beta*deltaq; th=AjustAng(th);
T10=Rz(th(1)+pi/2)*dz(1.52)*Rx(pi/2)*dx(1 .20); T21=Rz(th(2)+pi/2)*dz(0.93)*Rx(0)*dx(2.44);
%[det(Jdk) ec2 n] % [ POF(1:3) Pa POF(4:6) eomax]' [0 ec2p ec2o]'] %pause(0.2) end %%
Da
[0
epmax
n [POF(1:3) Pa POF(4:6) Da [0 epmax eomax]' [0 ec2p ec2o]'] x[thi thf th]
T32=Rz(th(3))*dz(1.04)*Rx(0)*dx(2.13);
V. CONCLUSIONES
T43=Rz(th(4)+pi/2)*dz(0)*Rx(pi/2)*dx(0); T54=Rz(th(5))*dz(0.85)*Rx(pi/2)*dx(0); T65=Rz(th(6))*dz(0.92)*Rx(0)*dx(0); T20=T10*T21; T30=T20*T32; T40=T30*T43; T50=T40*T54; T60=T50*T65;
Seminario de robótica.
Cuando el contador n llega a 1000 significara que no convergieron los puntos, por lo tanto, no llegara al punto final estimado, esto se da porque los puntos se vuelven dependientes. Existen ciertos puntos los cuales la cinemática inversa no puede alcanzar, debido
Universidad Cooperativa de Colombia. Hector Luis Sanchez Rodríguez. Juan Pablo Castañeda. Andrés Felipe Cruz. Informe robótica.
a que hay puntos fuera de rango en los que la cinemática no logra una simulación adecuada. VI. R EFERENCIAS
http://www.disclab.ua.es/robolab/EJS2/RRR _Intro_3.html
Seminario de robótica.
[1] Captura robot UR3. [2] Captura interfaz de Matlab. [3] Captura interfaz de Matlab. [4] Captura interfaz de Matlab. [5] Captura interfaz de Matlab.
5