功能描述：
ABB機器人IRB120的逆運動學(xué)求解程序，可以求出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
聯(lián)系：highspeedlogic
QQ ：1224848052
微信：HuangL1121
郵箱：1224848052@qq.com
網(wǎng)站：http://www.mat7lab.com/
網(wǎng)站：http://www.hslogic.com/
微信掃一掃：
|
