产品:51
联系我们
二、正运动学解算
%输入关节变量[theta1 theta2 theta3 theta5]
%其中theta4=-(theta2+theta3)
s1=sind(theta1);s2=sind(theta2);s3=sind(theta3);s4=sind(theta4);s5=sind(theta5);
c1=cosd(theta1);c2=cosd(theta2);c3=cosd(theta3);c4=cosd(theta4);c5=cosd(theta5);
s23=s2*c3+c2*s3;c23=c2*c3-s2*s3;
Nx=c1*(c23*c4*c5-s23*s4*c5)-s1*s5;
Ny=s1*(c23*c4*c5-s23*s4*c5)+c1*s5;
Nz=-s23*c4*c5-c23*s4*c5;
Ox=c1*(-c23*c4*s5+s23*s4*s5)-s1*c5;
Oy=s1*(-c23*c4*s5+s23*s4*s5)+c1*c5;
Oz=s23*c4*s5+c23*s4*s5;
Ax=c1*(c23*s4+s23*c4);
Ay=s1*(c23*s4+s23*c4);
Az=-s23*s4+c23*c4;
Px=c1*(c23*(a5*c4-d5*s4+a4)-s23*(a5*s4+d5*c4)+a3*s2+a2);
Py=s1*(c23*(a5*c4-d5*s4+a4)-s23*(a5*s4+d5*c4)+a3*s2+a2);
Pz=-s23*(a5*c4-d5*s4+a4)-c23*(a5*s4+d5*c4)+a3*c2+d1;
T=[Nx Ox Ax Px;...
Ny Oy Ay Py;...
Nz Oz Az Pz;...
0 0 0 1];
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
三、逆运动学解算
%theta1
Theta1=atan2(py,px)*180/pi;
%theta5
s1=sind(Theta1);c1=cosd(Theta1);
Theta5=atan2(-s1*nx+c1*ny,-s1*ox+c1*oy)*180/pi;
%theta23
k1=c1*px+s1*py-a2-a5;
k2=pz-d1+d5;
f1=2*k2*a4;
f2=-2*k1*a4;
f3=a3^2-k1^2-k2^2-a4^2;
Theta23=(atan2(f3,sqrt(f1^2+f2^2-f3^2))-atan2(f2,f1))*180/pi;
g1=2*k1*a3;
g2=2*k2*a3;
g3=k1^2+k2^2+a3^2-a4^2;
Theta2=(atan2(g3,sqrt(g1^2+g2^2-g3^2))-atan2(g2,g1))*180/pi;
Theta3=Theta23-Theta2;
THETA=[Theta1 Theta2 Theta3 Theta5];
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
————————————————来源 网络
更多详情：埃斯顿机器人