ABB机器人IRB120的逆运动学求解程序，可以求出6个theta，并可自动判定方向值

%������������˶�ѧ��������ģ��Ϊ���￡¡��ɣң￡����� 
clear; 
clc; 
%ABB120���� 
alpha=(pi/2).*[-1 0 -1 1 -1 0];%�˴��õĻ��Ƚ� 
a=[0 270 70 0 0 0]; 
d=[290 0 0 302 0 072];  
 
%������ʼ���� 
Pxyz=[404.4	-321.1	136.8 
      400.2	-240.2	137 
      371.2	-269.9	137.7 
      321	-230	138 
      297.7	-256	138.4 
      347	-297.5	138.4 
      319.7	-329.8	138.5]; 
 
for i=5:5 
     
px=Pxyz(i,1);		 
py=Pxyz(i,2); 
pz=Pxyz(i,3); 
Phi=-180*(pi/180);		 
The=0*(pi/180); 
Psi=180*(pi/180); 
 
Rz=[cos(Phi) -sin(Phi) 0; 
    sin(Phi) cos(Phi)  0; 
    0        0         1]; 
Ru=[cos(The)  0  sin(The); 
    0         1        0; 
   -sin(The)  0   cos(The)]; 
Rw=[1       0           0; 
   0  cos(Psi)  -sin(Psi); 
   0  sin(Psi)  cos(Psi)]; 
R=Rz*Ru*Rw; 
nx=R(1,1); 
ny=R(2,1); 
nz=R(3,1); 
sx=R(1,2); 
sy=R(2,2); 
sz=R(3,2); 
ax=R(1,3); 
ay=R(2,3); 
az=R(3,3); 
% ��ֵ������� 
%��һ�׶�����theta1\theta2\theta3 
%theta1 
theta1=atan2(py-ay*d(6),px-ax*d(6)); 
deg_theta1=theta1/pi*180 
%theta2 
M1=(py-ay*d(6))/sin(theta1); 
M2=pz-az*d(6)-d(1); 
M3=(M1^2+M2^2-(d(4)^2+a(3)^2+a(2)^2))/(2*a(2)); 
r=sqrt(d(4)^2+a(3)^2); 
theta31=atan2(a(3),d(4))-atan2(M3,sqrt(r^2-M3^2)); 
theta32=atan2(a(3),d(4))-atan2(M3,-sqrt(r^2-M3^2)); 
if theta31>(-110/180*pi)&&theta31<(110/180*pi) 
    theta3=theta31; 
elseif theta32>(-110/180*pi)&&theta32<(110/180*pi) 
    theta3=theta32; 
else error('theta3 is out of calculation,programme will stop!'); 
end 
deg_theta3=theta3/pi*180 
%theta3 
M4=-d(4)*sin(theta3)+a(3)*cos(theta3)+a(2); 
M5=d(4)*cos(theta3)+a(3)*sin(theta3); 
theta2=atan2((M1*M4-M2*M5),(M1*M5+M2*M4)); 
deg_theta2=theta2/pi*180 
%theta5 
sin5=sqrt((ax*sin(theta1)-ay*cos(theta1))^2+(az*cos(theta2+theta3)+ax*sin(theta2+theta3)*cos(theta1)+ay*sin(theta2+theta3)*sin(theta1))^2); 
 
theta5=atan2(sin5,(ax*cos(theta2+theta3)*cos(theta1)-az*sin(theta2+theta3)+ay*cos(theta2+theta3)*sin(theta1))); 
deg_theta5=theta5/pi*180 
%theta4 
theta4=atan2(-(ax*sin(theta1)-ay*cos(theta1))/sin(theta5),-(az*cos(theta2+theta3)+ax*sin(theta2+theta3)*cos(theta1)+ay*sin(theta2+theta3)*sin(theta1))/sin(theta5)); 
deg_theta4=theta4/pi*180 
 
%theta6 
theta6=atan2((sx*cos(theta2+theta3)*cos(theta1)-sz*sin(theta2+theta3)+sy*cos(theta2+theta3)*sin(theta1))/sin(theta5),(nz*sin(theta2+theta3)-nx*cos(theta2+theta31)*cos(theta1)-ny*cos(theta2+theta3)*sin(theta1))/sin(theta5)); 
deg_theta6=theta6/pi*180 
 
end 

联系：highspeedlogic

QQ ：1224848052

微信：HuangL1121

邮箱：1224848052@qq.com

网站：http://www.mat7lab.com/

网站：http://www.hslogic.com/

微信扫一扫：