en otra ventana

Apéndice C
Programa MATLAB Cinemática Inversa
%Francisco Pérez Marcial
%Tesis: Diseño, Análisis y Modelado Cinemático de un Exoesqueleto Pasivo
de
%Extremidad Inferior con Propósito de Rehabilitación
%Este programa se encarga de calcular la cinemátca inversa de la caminata
%del exoesqueleto
%
Cinematica_Inversa(px,py,pz)
%
Función que recibe la posición desada del tobillo con respeto a la
base
%
del sistema.
%
Por lo que la matriz que regresa es la configuración final de los
%
posibles angulos que puede asignarse al exoesqueleto.
function [Exo_thes] = Cinematica_Inversa(px, py, pz)
%Paramétros del Exoesqueleto LEGO
a2=14;
a3=13;
%primer angulo
%dos posibles soluciones por la ecuación trascendental.
%py*c1+-px*s1=0 Ec. 52 Tesis
%Primera Solución
theta1(1)= atan2( py, px);
if (theta1(1)< pi/3) &&(theta1(1)>-pi/6) %condicion de abduccion/Aducción
else
theta1(1)=inf;
end
%disp('theta1_1');
%disp(theta1(1));
%Segunda Solución
theta1(2)= atan2(-py,-px);
if (theta1(2)< pi/3) &&(theta1(2)>-pi/6) %condicion de abduccion/Aducción
else
theta1(2)=inf;
end
%disp('theta1_2');
%disp(theta1(2));
%tercer angulo
%dos posibles soluciones
%c3=k the3= atan2(+-sqrt(1-k^2),k) Condición Algebraica Ec55 Tesis
b=(px^2+py^2+pz^2-a2^2-a3^2)/(2*a3*a2);
%Primera Solución
theta3(1) = atan2(sqrt(1-b^2),b);
if (theta3(1)<= pi/20) &&(theta3(1)>=-pi/4) %Restricción angular
else
theta3(1)=inf;
end
%disp('theta3_1');
%disp(theta3(1));
%Segunda Solución
theta3(2) = atan2(-sqrt(1-b^2),b);
if (theta3(2)<= pi/20) &&(theta3(2)>=-pi/4) %Restricción angular
else
108
theta3(2)=inf;
end
%disp('theta3_2');
%disp(theta3(2));
%segundo angulo
%cuatro posibles soluciones
%a*c2+b*s2=c
the2=atan2(b,a)+-atan2(sqrt(a^2+b^2-c^2),c) Ec. 60
%Se obtienen los terminos que ya fueron encontrados
phi_1= px*cos(theta1(1))+py*sin(theta1(1));
%disp('phi_1');
%disp(phi_1);
phi_2= px*cos(theta1(2))+py*sin(theta1(2));
%disp('phi_2');
%disp(phi_2);
c=a2+a3*b;
%Primera Solución
theta2(1)=atan2(-pz,phi_1)+atan2(sqrt(phi_1^2+pz^2-c^2),c);
if (theta2(1)< pi/2) &&(theta2(1)>-pi/4) %Restricción angular
else
theta2(1)=inf;
end
%Segunda Solución
theta2(3)=atan2(-pz,phi_1)-atan2(sqrt(phi_1^2+pz^2-c^2),c);
if (theta2(3)< pi/2) &&(theta2(3)>-pi/4) %Restriccion angular
else
theta2(3)=inf;
end
%Tercera Solución
theta2(2)=atan2(-pz,phi_2)+atan2(sqrt(phi_2^2+pz^2-c^2),c);
if (theta2(2)< pi/2) &&(theta2(2)>-pi/4) %Restricció angular
else
theta2(2)=inf;
end
%Cuarta Solución
theta2(4)=atan2(-pz,phi_2)-atan2(sqrt(phi_2^2+pz^2-c^2),c);
if (theta2(4)< pi/2) &&(theta2(4)>-pi/4) %Restricción angular
else
theta2(4)=inf;
end
%disp('theta2_1');
%disp(theta2(1));
%disp('theta2_2');
%disp(theta2(2));
%disp('theta2_3');
%disp(theta2(3));
%disp('theta2_4');
%disp(theta2(4));
%Se forma la matriz que tiene todos los ángulos
Exo_thes(1,:)= [theta1(1),theta1(2),0,0];
Exo_thes(2,:)= [theta2(1),theta2(2),theta2(2),theta2(2)];
Exo_thes(3,:)= [theta3(1),theta3(2),0,0];
end
109