标准DH模型正运动学求解程序编写

依据Peter Croke所建立的机器人工具箱模型。
标准DH模型，其第j个关节，第j个连杆与第j个坐标系建立关系如下所示。即j坐标系是建立在关节j+1处。

改进DH模型，其第j个关节，第j个连杆与第j个坐标系建立关系如下所示。即j坐标系是建立在关节j处。

如下机器人运动学正解程序是建立在标准DH模型下；
根据Peter Croke所著的《Robotic, Vision and Control—Fundamental Algorithms in MATLAB》我们可以得到从连杆坐标系{j-1}到坐标系{j}的变换公式如下；

由矩阵平移和旋转变换可展开得：

由此，对于六自由度机器人的第六关节坐标系相对于基坐标系得变换矩阵可以写为：
T6-0=T1-0* T2-1* T3-2* T4-3* T5-4* T6-5
T6-1代表第六关节坐标系相对基坐标系得转换矩阵。
首先，写出机器人标准DH参数：

SDH=[theta1   0.000   -0.05      pi/2;
     theta2   0.000   0.425    0.000;
     theta3   0.050   0.000    -pi/2;
     theta4   0.425   0.000    pi/2;
     theta5   0.000   0.000    -pi/2;
     theta6   0.000   0.000    0.000];

进而写出变换矩阵：

T01=[cos(SDH(1,1))  -sin(SDH(1,1))*cos(SDH(1,4))   sin(SDH(1,1))*sin(SDH(1,4))    SDH(1,3)*cos(SDH(1,1));
      sin(SDH(1,1))   cos(SDH(1,1))*cos(SDH(1,4))  -cos(SDH(1,1))*sin(SDH(1,4))    SDH(1,3)*sin(SDH(1,1));
      0               sin(SDH(1,4))                 cos(SDH(1,4))                  SDH(1,2);
      0               0                             0                              1];

依照上面，依次写出T12、T23、T32、T45、T56;
最后：
T06=T01T12T23T34T45*T56；完成机器人标准DH模型运动学正解得求解。