机械臂正运动学标准DH参数建立技巧

1. 切记，{i}坐标系建立在i+1关节轴上，如{0}坐标系建立在关节1轴上，依次类推。{6}坐标系与{5}坐标系姿态一致，固连在法兰盘接口末端
2. 坐标系原点建立：若1 2轴垂直或异面垂直，则坐标系{1}原点在1轴与2轴的交点，{0}坐标系原点建在1轴与2轴的交点上，若1 2轴平行，则建立在公垂线与轴的交点上。
3. z轴与关节轴方向一致，Xi轴与Zi-1轴垂直且相交，其中X0轴方向指向1 2轴公法线方向，y轴由右手定则确定。
4. 参数确定： a为两Z轴距离，d为两x轴距离；alpha为两Z轴夹角，theta为两x轴夹角，方向判断如下图：
5. 实例：
6. 正运动学标准DH参数matlab代码：

clear;
clc;
syms theta1 theta2 theta3 theta4 theta5 theta6;
syms alpha1 alpha2 alpha3 alpha4 alpha5 alpha6;
syms a1 a2 a3 a4 a5 a6;
syms d1 d2 d3 d4 d5 d6;


T1 = [cos(theta1), -sin(theta1)*cos(alpha1), sin(theta1)*sin(alpha1), a1*cos(theta1);...
      sin(theta1), cos(theta1)*cos(alpha1),  -cos(theta1)*sin(alpha1), a1*sin(theta1);...
          0      ,       sin(alpha1),                cos(alpha1),      d1           ;...
          0      ,             0         ,              0        ,       1           ];
T2 = [cos(theta2), -sin(theta2)*cos(alpha2), sin(theta2)*sin(alpha2), a2*cos(theta2);...
      sin(theta2), cos(theta2)*cos(alpha2),  -cos(theta2)*sin(alpha2), a2*sin(theta2);...
          0      ,       sin(alpha2),                cos(alpha2),      d2           ;...
          0      ,             0         ,              0        ,       1           ];
T3 = [cos(theta3), -sin(theta3)*cos(alpha3), sin(theta3)*sin(alpha3), a3*cos(theta3);...
      sin(theta3), cos(theta3)*cos(alpha3),  -cos(theta3)*sin(alpha3), a3*sin(theta3);...
          0      ,       sin(alpha3),                cos(alpha3),      d3           ;...
          0      ,             0         ,              0        ,       1           ];
T4 = [cos(theta4), -sin(theta4)*cos(alpha4), sin(theta4)*sin(alpha4), a4*cos(theta4);...
      sin(theta4), cos(theta4)*cos(alpha4),  -cos(theta4)*sin(alpha4), a2*sin(theta4);...
          0      ,       sin(alpha4),                cos(alpha4),      d4           ;...
          0      ,             0         ,              0        ,       1           ];
T5 = [cos(theta5), -sin(theta5)*cos(alpha5), sin(theta5)*sin(alpha5), a5*cos(theta5);...
      sin(theta5), cos(theta5)*cos(alpha5),  -cos(theta5)*sin(alpha5), a5*sin(theta5);...
          0      ,       sin(alpha5),                cos(alpha5),      d5           ;...
          0      ,             0         ,              0        ,       1           ];
 T6 = [cos(theta6), -sin(theta6)*cos(alpha6), sin(theta6)*sin(alpha6), a6*cos(theta6);...
      sin(theta6), cos(theta6)*cos(alpha6),  -cos(theta6)*sin(alpha6), a6*sin(theta6);...
          0      ,       sin(alpha6),                cos(alpha6),      d6           ;...
          0      ,             0         ,              0        ,       1           ];

  T = simplify(eval(T1*T2*T3*T4*T5*T6));
  nx = T(1,1)
  ny = T(2,1)
  nz = T(3,1)
  ox = T(1,2)
  oy = T(2,2)
  oz = T(2,3)
  ax = T(1,3)
  ay = T(2,3)
  az = T(3,3)
  px = T(1,4) 
  py = T(2,4)
  pz = T(3,4)