5、matlab串联机器人DH改进型-手写正解函数(推荐)

1、手写矩阵函数myfkine.m

%改进型D-H建模
function [T06]=myfkine(theta1,theta2,theta3,theta4,theta5,theta6)

%    theta      d         a           alpha
%    关节转角   关节距离   连杆长度     连杆扭角

MDH=[theta1     0.4       0.025       pi/2;
     theta2     0         0.56        0;
     theta3     0         0.035       pi/2;
     theta4     0.515     0           pi/2;
     theta5     0         0           pi/2;
     theta6     0.08       0         0];
 
T01=[cos(MDH(1,1))                 -sin(MDH(1,1))                 0               MDH(1,3);
     cos(MDH(1,4))*sin(MDH(1,1))    cos(MDH(1,4))*cos(MDH(1,1))  -sin(MDH(1,4))  -MDH(1,2)*sin(MDH(1,4));
     sin(MDH(1,4))*sin(MDH(1,1))    sin(MDH(1,4))*cos(MDH(1,1))   cos(MDH(1,4))   MDH(1,2)*cos(MDH(1,4));
     0                              0                             0               1];
T12=[cos(MDH(2,1))                 -sin(MDH(2,1))                 0               MDH(2,3);
     cos(MDH(2,4))*sin(MDH(2,1))    cos(MDH(2,4))*