六轴机器人matlab写运动学正解函数（DH模型）

1.分两个程序①主函数②function函数
2.main

clear;
clc;
%建立机器人模型
%       theta    d           a        alpha     offset
SL1=Link([0       0          0.180    -pi/2        0     ],'standard'); 
SL2=Link([0       0          0.600     0           0     ],'standard');
SL3=Link([0       0          0.130    -pi/2        0     ],'standard');
SL4=Link([0       0.630       0        pi/2        0     ],'standard');
SL5=Link([0       0           0       -pi/2        0     ],'standard');
SL6=Link([0       0.1075      0        0           0     ],'standard');
starobot=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','standard');
stat06=starobot.fkine([0,0,pi/2,0,0,pi/2]) %工具箱正解函数
stamyt06=mystafkine(0,0,pi/2,0,0,pi/2)  %手写正解函数

3.function

function [T06]=mystafkine(theta1,theta2,theta3,theta4,theta5,theta6)
SDH=[theta1   0       0.180   -pi/2;
     theta2   0       0.600    0;
     theta3   0       0.130   -pi/2;
     theta4   0.630   0        pi/2;
     theta5   0       0       -pi/2;
     theta6   0.1075  0        0];

 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=[cos(SDH(2,1))  -sin(SDH(2,1))*cos(SDH(2,4))   sin(SDH(2,1))*sin(SDH(2,4))    SDH(2,3)*cos(SDH(2,1));
      sin(SDH(2,1))   cos(SDH(2,1))*cos(SDH(2,4))  -cos(SDH(2,1))*sin(SDH(2,4))    SDH(2,3)*sin(SDH(2,1));
      0               sin(SDH(2,4))                 cos(SDH(2,4))                  SDH(2,2);
      0               0                             0                              1];
 T23=[cos(SDH(3,1))  -sin(SDH(3,1))*cos(SDH(3,4))   sin(SDH(3,1))*sin(SDH(3,4))    SDH(3,3)*cos(SDH(3,1));
      sin(SDH(3,1))   cos(SDH(3,1))*cos(SDH(3,4))  -cos(SDH(3,1))*sin(SDH(3,4))    SDH(3,3)*sin(SDH(3,1));
      0               sin(SDH(3,4))                 cos(SDH(3,4))                  SDH(3,2);
      0               0                             0                              1];
 T34=[cos(SDH(4,1))  -sin(SDH(4,1))*cos(SDH(4,4))   sin(SDH(4,1))*sin(SDH(4,4))    SDH(4,3)*cos(SDH(4,1));
      sin(SDH(4,1))   cos(SDH(4,1))*cos(SDH(4,4))  -cos(SDH(4,1))*sin(SDH(4,4))    SDH(4,3)*sin(SDH(4,1));
      0               sin(SDH(4,4))                 cos(SDH(4,4))                  SDH(4,2);
      0               0                             0                              1];
 T45=[cos(SDH(5,1))  -sin(SDH(5,1))*cos(SDH(5,4))   sin(SDH(5,1))*sin(SDH(5,4))    SDH(5,3)*cos(SDH(5,1));
      sin(SDH(5,1))   cos(SDH(5,1))*cos(SDH(5,4))  -cos(SDH(5,1))*sin(SDH(5,4))    SDH(5,3)*sin(SDH(5,1));
      0               sin(SDH(5,4))                 cos(SDH(5,4))                  SDH(5,2);
      0               0                             0                              1];
 T56=[cos(SDH(6,1))  -sin(SDH(6,1))*cos(SDH(6,4))   sin(SDH(6,1))*sin(SDH(6,4))    SDH(6,3)*cos(SDH(6,1));
      sin(SDH(6,1))   cos(SDH(6,1))*cos(SDH(6,4))  -cos(SDH(6,1))*sin(SDH(6,4))    SDH(6,3)*sin(SDH(6,1));
      0               sin(SDH(6,4))                 cos(SDH(6,4))                  SDH(6,2);
      0               0                             0                              1];

 T06=T01*T12*T23*T34*T45*T56;

4.结果
对比robotic toolbox里fkine的结果是一致的。

PS:这个手写的函数仅适用于DH模型。