四轴码垛机器人MDH模型运动学

---

一、MDH模型参数介绍

二、正运动学解算

%输入关节变量[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];

三、逆运动学解算

%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];