六轴机器人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模型。