【个人简记】基于MDH，6R机器人运动学正解

【个人理解，有错请指出，谢谢】

MDH的建模方式先通过x轴变化再通过z轴变化

因此可以发现关节之间旋转的变化如下图

关节的变化矩阵如下图所示，以关节1变换到关节2为例

s1 = sinθ1； c1 = cosθ1； s_a1 = sinα1； c_a1 =  cosα1；

 

 matlab代码如下图，EndPosture为末端位姿，注意输入的θ的值要考虑机械臂 当前的角度 和 初始的旋转角度offset

%MDH变换
   T01 = [ c1      -s1        0      a1
           s1*c_a1  c1*c_a1  -s_a1  -d1*s_a1
           s1*s_a1  c1*s_a1   c_a1   d1*c_a1
           0 0 0 1];
   
   T12 = [ c2      -s2        0      a2
           s2*c_a2  c2*c_a2  -s_a2  -d2*s_a2
           s2*s_a2  c2*s_a2   c_a2   d2*c_a2
           0 0 0 1];
     
   T23 = [ c3      -s3        0      a3
           s3*c_a3  c3*c_a3  -s_a3  -d3*s_a3
           s3*s_a3  c3*s_a3   c_a3   d3*c_a3
           0 0 0 1];
       
   T34 = [ c4      -s4        0      a4
           s4*c_a4  c4*c_a4  -s_a4  -d4*s_a4
           s4*s_a4  c4*s_a4   c_a4   d4*c_a4
           0 0 0 1];
       
   T45 = [ c5      -s5        0      a5
           s5*c_a5  c5*c_a5  -s_a5  -d5*s_a5
           s5*s_a5  c5*s_a5   c_a5   d5*c_a5
           0 0 0 1];
       
   T56 = [ c6      -s6        0      a6
           s6*c_a6  c6*c_a6  -s_a6  -d6*s_a6
           s6*s_a6  c6*s_a6   c_a6   d6*c_a6
           0 0 0 1];
EndPosture = T01 * T12 * T23 * T34 * T45 * T56;
nx = EndPosture(1,1);  ox = EndPosture(1,2);  ax = EndPosture(1,3);  rx = EndPosture(1,4);
ny = EndPosture(2,1);  oy = EndPosture(2,2);  ay = EndPosture(2,3);  ry = EndPosture(2,4);
nz = EndPosture(3,1);  oz = EndPosture(3,2);  az = EndPosture(3,3);  rz = EndPosture(3,4);

确定了末端位姿的准确性之后，需要将除了θ的参数全部带入确定数值，特别是α的值带入后sinα和cosα值算出啦

%进行确切的alpha计算，不然会导致参数过大    
    s_a1 = 0; s_a2 = 1; s_a3 = 0; s_a4 = -1; s_a5 = 1; s_a6 = -1;
    c_a1 = 1; c_a2 = 0; c_a3 = 1; c_a4 = 0; c_a5 = 0; c_a6 = 0;

 最后为了方便快捷的计算，将末端位姿的4x4齐次矩阵中每个位置的计算公式写出来，完成正运动学公式编写

nx = - c6*(s5*(c1*c2*s3 + c1*c3*s2) + c5*(s1*s4 - c4*(c1*c2*c3 -c1*s2*s3))) - s6*(c4*s1 + s4*(c1*c2*c3 - c1*s2*s3));
ny = s6*(c1*c4 - s4*(c2*c3*s1 - s1*s2*s3)) - c6*(s5*(c2*s1*s3 + c3*s1*s2) - c5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3)));
nz = c6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - s4*s6*(c2*s3 + c3*s2);
ox = s6*(s5*(c1*c2*s3 + c1*c3*s2) + c5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3))) - c6*(c4*s1 + s4*(c1*c2*c3 - c1*s2*s3));
oy = s6*(s5*(c2*s1*s3 + c3*s1*s2) - c5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3))) + c6*(c1*c4 - s4*(c2*c3*s1 - s1*s2*s3));
oz =  - s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 + c3*s2);
ax = s5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3)) - c5*(c1*c2*s3 + c1*c3*s2);
ay = - c5*(c2*s1*s3 + c3*s1*s2) - s5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3));
az = c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2);

rx = a2*c1 - d4*(c1*c2*s3 + c1*c3*s2) + a3*c1*c2;
ry = a2*s1 - d4*(c2*s1*s3 + c3*s1*s2) + a3*c2*s1;
rz = d1 + a3*s2 + d4*(c2*c3 - s2*s3);

EndPosture = [nx, ox, ax, rx
              ny, oy, ay, ry
              nz, oz, az, rz
              0,  0,  0,  1];