工业六轴机器人常见的STD(标准)-DH模型建立方法

---

一、D-H模型

二、形式1

利用MATLAB机器人工具箱Matlab-Robotics-Toolbox建立标准Link函数，其中offset是以z轴为旋转轴的初始零位的补偿角度。

L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
a1=L1;d1=L2;a2=L3;a3=L4;d4=L5;d6=L6;
%        theta(z)   d(z)     a(x)     alpha(x)  
RL1=Link([  0       d1       a1       -pi/2     ],'standard');
RL2=Link([  0       0        a2         0       ],'standard');RL2.offset=-pi/2;
RL3=Link([  0       0        a3       -pi/2     ],'standard');
RL4=Link([  0       d4       0         pi/2     ],'standard');
RL5=Link([  0       0        0        -pi/2     ],'standard');
RL6=Link([  0       d6       0          0       ],'standard');
Robot_6=SerialLink([RL1,RL2,RL3,RL4,RL5,RL6],'name','Robot');
Robot_6.teach;

三、形式2

L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
RL1=Link('revolute','d',L2,'a',L1,'alpha',-pi/2,'standard');
RL2=Link('revolute','d',0,'a',L3,'alpha',0,'standard');RL2.offset=-pi/2;
RL3=Link('revolute','d',0,'a',L4,'alpha',-pi/2,'standard');
RL4=Link('revolute','d',L5,'a',0,'alpha',pi/2,'standard');
RL5=Link('revolute','d',0,'a',0,'alpha',-pi/2,'standard');
RL6=Link('revolute','d',L6,'a',0,'alpha',0,'standard');
Robot_6=SerialLink([RL1,RL2,RL3,RL4,RL5,RL6],'name','Robot');
Robot_6.teach;

四、形式3

逐步建立各关节轴的矩阵方程，主要依据DH模型。其中w=-pi/2就是上面offset设定的值。

L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
%初始角度设置
w1=0;w2=-pi/2;w3=0;w4=0;w5=0;w6=0;
%Rz*Tz(L2)*Tx(L1)*Rx(-pi/2)
T01=[cos(w1) -sin(w1) 0 0;sin(w1) cos(w1) 0 0;0 0 1 0;0 0 0 1]*...
    [1 0 0 0;0 1 0 0;0 0 1 L2;0 0 0 1]*...
    [1 0 0 L1;0 1 0 0;0 0 1 0;0 0 0 1]*...
    [1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1];
%Rz*Tx(L3)
T12=[cos(w2) -sin(w2) 0 0;sin(w2) cos(w2) 0 0;0 0 1 0;0 0 0 1]*...
    [1 0 0 L3;0 1 0 0;0 0 1 0;0 0 0 1];
%Rz*Tx(L4)*Rx(-pi/2)
T23=[cos(w3) -sin(w3) 0 0;sin(w3) cos(w3) 0 0;0 0 1 0;0 0 0 1]*...
    [1 0 0 L4;0 1 0 0;0 0 1 0;0 0 0 1]*...
    [1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1];
%Rz*Tz(L5)*Rx(pi/2)
T34=[cos(w4) -sin(w4) 0 0;sin(w4) cos(w4) 0 0;0 0 1 0;0 0 0 1]*...
    [1 0 0 0;0 1 0 0;0 0 1 L5;0 0 0 1]*...
    [1 0 0 0;0 cos(pi/2) -sin(pi/2) 0;0 sin(pi/2) cos(pi/2) 0;0 0 0 1];
%Rz*Rx(-pi/2)
T45=[cos(w5) -sin(w5) 0 0;sin(w5) cos(w5) 0 0;0 0 1 0;0 0 0 1]*...
    [1 0 0 0;0 cos(-pi/2) -sin(-pi/2) 0;0 sin(-pi/2) cos(-pi/2) 0;0 0 0 1];
%Rz*Tz(L6)
T56=[cos(w6) -sin(w6) 0 0;sin(w6) cos(w6) 0 0;0 0 1 0;0 0 0 1]*...
    [1 0 0 0;0 1 0 0;0 0 1 L6;0 0 0 1];

化简后得：

T=[cos(w1) 0 -sin(w1) L1*cos(w1);sin(w1) 0 cos(w1) L1*sin(w1);0 -1 0 L2;0 0 0 1]*...
  [cos(w2) -sin(w2) 0 L3*cos(w2);sin(w2) cos(w2) 0 L3*sin(w2);0 0 1 0;0 0 0 1]*...
  [cos(w3) 0 -sin(w3) L4*cos(w3);sin(w3) 0 cos(w3) L4*sin(w3);0 -1 0 0;0 0 0 1]*...
  [cos(w4) 0 sin(w4) 0;sin(w4) 0 -cos(w4) 0;0 1 0 L5;0 0 0 1]*...
  [cos(w5) 0 -sin(w5) 0;sin(w5) 0 cos(w5) 0;0 -1 0 0;0 0 0 1]*...
  [cos(w6) -sin(w6) 0 0;sin(w6) cos(w6) 0 0;0 0 1 L6;0 0 0 1]

运算结果：

T =

   1.0e+03 *

    0.0000         0    0.0010    1.0280
         0   -0.0010         0         0
    0.0010         0   -0.0000    1.2295
         0         0         0    0.0010

五、形式4

直接对矩阵进行化解：

L1=160;L2=449.5;L3=580;L4=200;L5=640;L6=228;
%初始角度设置
w1=0;w2=-pi/2;w3=0;w4=0;w5=0;w6=0;
deg=180/pi;
theta1=w1*deg;theta2=w2*deg;theta3=w3*deg;theta4=w4*deg;theta5=w5*deg;theta6=w6*deg;
s1=sind(theta1);s2=sind(theta2);s3=sind(theta3);s4=sind(theta4);s5=sind(theta5);s6=sind(theta6);
c1=cosd(theta1);c2=cosd(theta2);c3=cosd(theta3);c4=cosd(theta4);c5=cosd(theta5);c6=cosd(theta6);
s23=s2*c3+c2*s3;c23=c2*c3-s2*s3;

Nx=c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)+s1*(s4*c5*c6+c4*s6);
Ny=s1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-c1*(s4*c5*c6+c4*s6);
Nz=-s23*(c4*c5*c6-s4*s6)-c23*s5*c6;
Ox=c1*(c23*(-c4*c5*s6-s4*c6)+s23*s5*s6)-s1*(s4*c5*s6-c4*c6);
Oy=s1*(c23*(-c4*c5*s6-s4*c6)+s23*s5*s6)+c1*(s4*c5*s6-c4*c6);
Oz=s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
Ax=c1*(-c23*c4*s5-s23*c5)-s1*s4*s5;
Ay=s1*(-c23*c4*s5-s23*c5)+c1*s4*s5;
Az=s23*c4*s5-c23*c5;
Px=c1*(-c23*c4*s5*L6-s23*(c5*L6+L5)+c23*L4+c2*L3)-s1*s4*s5*L6+c1*L1;
Py=s1*(-c23*c4*s5*L6-s23*(c5*L6+L5)+c23*L4+c2*L3)+c1*s4*s5*L6+s1*L1;
Pz=s23*c4*s5*L6-c23*(c5*L6+L5)-s23*L4-s2*L3+L2;
TT=[Nx Ox Ax Px;
    Ny Oy Ay Py;
    Nz Oz Az Pz;
    0  0  0  1];

运算结果：

TT =

   1.0e+03 *

         0         0    0.0010    1.0280
         0   -0.0010         0         0
    0.0010         0         0    1.2295
         0         0         0    0.0010

---