Scribd
Cargar
34 vistas2 páginas

Cinemática Inversa en MATLAB

Este programa calcula la cinemática inversa de un exoesqueleto para la extremidad inferior mediante el cálculo de posibles ángulos de articulación que satisfacen una posición deseada del tobillo. Calcula dos posibles soluciones para el primer y tercer ángulo, y cuatro posibles soluciones para el segundo ángulo, sujetas a restricciones mecánicas y articulares. Devuelve los ángulos en una matriz de posibles configuraciones.

Cargado por

Ronald Butron Salvatierra
Derechos de autor
© All Rights Reserved
Nos tomamos en serio los derechos de los contenidos. Si sospechas que se trata de tu contenido, reclámalo aquí.
Formatos disponibles
Descarga como PDF, TXT o lee en línea desde Scribd
34 vistas2 páginas

Cinemática Inversa en MATLAB

Este programa calcula la cinemática inversa de un exoesqueleto para la extremidad inferior mediante el cálculo de posibles ángulos de articulación que satisfacen una posición deseada del tobillo. Calcula dos posibles soluciones para el primer y tercer ángulo, y cuatro posibles soluciones para el segundo ángulo, sujetas a restricciones mecánicas y articulares. Devuelve los ángulos en una matriz de posibles configuraciones.

Cargado por

Ronald Butron Salvatierra
Derechos de autor
© All Rights Reserved
Nos tomamos en serio los derechos de los contenidos. Si sospechas que se trata de tu contenido, reclámalo aquí.
Formatos disponibles
Descarga como PDF, TXT o lee en línea desde Scribd

108

Apndice C
Programa MATLAB Cinemtica Inversa
%Francisco Prez Marcial
%Tesis: Diseo, Anlisis y Modelado Cinemtico de un Exoesqueleto Pasivo
de
%Extremidad Inferior con Propsito de Rehabilitacin
%Este programa se encarga de calcular la cinemtca inversa de la caminata
%del exoesqueleto
% Cinematica_Inversa(px,py,pz)
% Funcin que recibe la posicin desada del tobillo con respeto a la
base
% del sistema.
% Por lo que la matriz que regresa es la configuracin final de los
% posibles angulos que puede asignarse al exoesqueleto.
function [Exo_thes] = Cinematica_Inversa(px, py, pz)

%Paramtros del Exoesqueleto LEGO
a2=14;
a3=13;
%primer angulo
%dos posibles soluciones por la ecuacin trascendental.
%py*c1+-px*s1=0 Ec. 52 Tesis
%Primera Solucin
theta1(1)= atan2( py, px);
if (theta1(1)< pi/3) &&(theta1(1)>-pi/6) %condicion de abduccion/Aduccin
else
theta1(1)=inf;
end
%disp('theta1_1');
%disp(theta1(1));
%Segunda Solucin
theta1(2)= atan2(-py,-px);
if (theta1(2)< pi/3) &&(theta1(2)>-pi/6) %condicion de abduccion/Aduccin
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) Condicin Algebraica Ec55 Tesis
b=(px^2+py^2+pz^2-a2^2-a3^2)/(2*a3*a2);
%Primera Solucin
theta3(1) = atan2(sqrt(1-b^2),b);
if (theta3(1)<= pi/20) &&(theta3(1)>=-pi/4) %Restriccin angular
else
theta3(1)=inf;
end
%disp('theta3_1');
%disp(theta3(1));
%Segunda Solucin
theta3(2) = atan2(-sqrt(1-b^2),b);
if (theta3(2)<= pi/20) &&(theta3(2)>=-pi/4) %Restriccin angular
else
109

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 Solucin
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) %Restriccin angular
else
theta2(1)=inf;
end
%Segunda Solucin
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 Solucin
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 Solucin
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) %Restriccin 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

También podría gustarte