Rodrguez Cardo Jorge Flix C.

Robots Industriales //Grupo 4 // Tarea 6// 090913

CINEMATICA INVERSA ROBOT ANTROPOMORFICO

1. Obtencin de los parmetros D.H

NOTA 1 Las distancias propuestas (a2, d4, d6) son las longitudes anatmicas aproximadas de mi antebrazo, brazo y mueca. NOTA 2 La longitud (d1) fue considerada tomando como base la longitud aproximada de mi torso ( 400 [mm] ) y luego restndole el valor necesario para que al formar un Angulo 1 = 90 (hombro), la punta del efector final llegase sin problemas a la base del manipulador. NOTA 3 No se colocaron o sealaron los orgenes de los sistemas evitando cargar la figura. Estos pueden ser fcilmente identificados.

2. Desacoplo cinemtico... 0OC = 0OD d60RD[0 0 1]T

3. Aplicacin de rotaciones aleatorias.

Renombrando al punto (Oc) como punto (Pc), cuyas coordenadas sern conocidas, y estarn determinadas por el vector de coordenadas(Pcx,Pcy,Pcz), entonces, de la figura (6.3) se puede observar que...

Nota Como atan2(y,x) no est definida para (y=0,x=0), se deber tener en cuenta esta limitante y notar que cuando esto suceda, el robot estar extendido totalmente sobre el eje Zo, por lo tanto, el ngulo theta_1 se fija en 90 grados. Luego si proyectamos sobre el plano (X1,Y1) los ejes coordenados de los sistemas de referencia involucrados, y al robot en la actual posicin...

Si observamos la figura anterior, al utilizar la ley de cosenos, obtenemos...

( )

( ) ( )( )

... pero tambin se observa que...

( )

( )

( )

( )

... por lo tanto, podemos establecer:

Para obtener el ngulo 2 podemos establecer las siguientes relaciones...

[( ( ) ( )

] ( )

... pero, utilizando la expresin [1.4], obtenemos en [1.6]...

) ( )

( )

Finalmente:

4. Obtencin de los ngulos (4,5,6) o ngulos de Euler.

Como la relacin existente entre los vectores oOC y
o

OD es...

... y a su vez... [ ... dnde: ( ( ( ( ) ) ) ) ( ( ( ( ) ) ) ) ( ( ( ( ) ) ) ) ( ( ) ( ) ( ) ) ( ( ( ) ) ) ( ( ) ) ]

CODIGO EN MATLAB
function Cinematik_Inversa = cinv(R,OD,length) %. % CINEMATIK_INVERSA = cinv(R,OD,length) % % 1- Regresa un Vctr con los ngulos de rotacin de cada articulacin del robot % 2R = Matriz de rotacin del efector final respecto al sistema base % 3OD = Posicin del efector final en el plano cartesiano base % 4- length = Longitudes de los eslabones del robot. d1 a2 d4 d6 = = = = length(1); length(2); length(3); length(4);

LT = a2 + d4 + d6; if abs(OD(1)) > LT || abs(OD(2)) > LT || abs(OD(3)) > LT+length(1) disp('COORDENADAS [ OD ] NO VALIDAS'); else OC = OD' - d6*R*[0 0 1]'; if OC(1) == OC(2) && OC(1) == 0 theta_1 = 90; else theta_1 = rad2deg(atan2(OC(2),OC(1))); %................. Ec [1.1] end M = (OC(2)^2+(d1-OC(3))^2-(a2^2)-(d4^2))/(-2*a2*d4); %............ Ec [1.2] theta_3 = rad2deg(atan2(-M,sqrt(1-(M^2)))); %..................... Ec [1.4] phi = rad2deg(atan2(d1-OC(3),OC(2))); %........................... Ec [1.5] F = (d4/(sqrt((d1-OC(3))^2+OC(2)^2)))*cosd(theta_3); %............ Ec [1.8] theta_2 = rad2deg(atan2(F,sqrt(1-F^2))) - phi; %.................. Ec [1.9] R03 = rotz(theta_1)*rotx(90)*rotz(theta_2+theta_3)*rotx(90); %.... Ec [2.2] R36 = R03\R; %.................................................... Ec [2.1] angles = euler(R36); Cinematik_Inversa = [theta_1; theta_2; theta_3; angles(1,2); angles(2,2); angles(3,2)]; for n=1:6 fprintf(1,'\nTheta %i = ', n); end end end