四轴码垛机器人DH模型运动学_四轴码垛机械臂运动学模型

一、DH模型参数介绍

二、正运动学解算

%输入关节变量[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*(-d5*s4+a4*c4)-s23*(d5*c4+a4*s4)+a3*c23+a2*s2)+a1*c1;
Py=s1*(c23*(-d5*s4+a4*c4)-s23*(d5*c4+a4*s4)+a3*c23+a2*s2)+a1*s1;
Pz=-(s23*(-d5*s4+a4*c4)+c23*(d5*c4+a4*s4)+a3*s23-a2*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-a1-a4;
k2=pz-d1+d5;
f1=2*k2*a3;
f2=-2*k1*a3;
f3=a2^2-a3^2-k1^2-k2^2;
Theta23=(atan2(f3,sqrt(f1^2+f2^2-f3^2))-atan2(f2,f1))*180/pi;

g1=2*k1*a2;
g2=2*k2*a2;
g3=k1^2+k2^2+a2^2-a3^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