连杆变换矩阵
概括起来，求正解即是求坐标系{6}相对于坐标系{0}的变换，这个问题可以分解成6个子问题，即确定6个坐标系{i}相对于坐标系{i-1}的表示，每个变换过程又包含4个子过程，即绕X(i-1)轴旋转alpha(i-1)，沿X(i-1)轴平移a(i-1)，绕Z(i)轴旋转theta(i)，沿Z(i)轴平移d(i)；
我们可以得到这个变换为：
将旋转算子和平移算子带入上式，即可得到相邻连杆变换矩阵的一般形式：
求解目标连杆坐标系相对于某坐标系的表示，即从某连杆开始的转换矩阵一直右乘相邻变换矩阵，直到目标矩阵为止；比如求坐标系{N}相对于极坐标系{0}的表示：
Matlab实现
以上一篇文章中提到的MDH参数表为例，matlab实现如下：
function [ T06 ] = fkine(theta1, theta2, theta3, theta4, theta5, theta6)
L1=160;
L2=575;
L3=130;
L4=645;
% theta d a alpha
DH=[ theta1 0 0 0;
theta2 0 L1 pi/2;
theta3 0 L2 0;
theta4 L4 L3 pi/2;
theta5 0 0 -pi/2;
theta6 0 0 pi/2];
T01=[cos(DH(1,1)) -sin(DH(1,1)) 0 DH(1,3);
cos(DH(1,4))*sin(DH(1,1)) cos(DH(1,4))*cos(DH(1,1)) -sin(DH(1,4)) -DH(1,2)*sin(DH(1,4));
sin(DH(1,4))*sin(DH(1,1)) sin(DH(1,4))*cos(DH(1,1)) cos(DH(1,4)) DH(1,2)*cos(DH(1,4));
0 0 0 1];
T12=[cos(DH(2,1)) -sin(DH(2,1)) 0 DH(2,3);
cos(DH(2,4))*sin(DH(2,1)) cos(DH(2,4))*cos(DH(2,1)) -sin(DH(2,4)) -DH(2,2)*sin(DH(2,4));
sin(DH(2,4))*sin(DH(2,1)) sin(DH(2,4))*cos(DH(2,1)) cos(DH(2,4)) DH(2,2)*cos(DH(2,4));
0 0 0 1];
T23=[cos(DH(3,1)) -sin(DH(3,1)) 0 DH(3,3);
cos(DH(3,4))*sin(DH(3,1)) cos(DH(3,4))*cos(DH(3,1)) -sin(DH(3,4)) -DH(3,2)*sin(DH(3,4));
sin(DH(3,4))*sin(DH(3,1)) sin(DH(3,4))*cos(DH(3,1)) cos(DH(3,4)) DH(3,2)*cos(DH(3,4));
0 0 0 1];
T34=[cos(DH(4,1)) -sin(DH(4,1)) 0 DH(4,3);
cos(DH(4,4))*sin(DH(4,1)) cos(DH(4,4))*cos(DH(4,1)) -sin(DH(4,4)) -DH(4,2)*sin(DH(4,4));
sin(DH(4,4))*sin(DH(4,1)) sin(DH(4,4))*cos(DH(4,1)) cos(DH(4,4)) DH(4,2)*cos(DH(4,4));
0 0 0 1];
T45=[cos(DH(5,1)) -sin(DH(5,1)) 0 DH(5,3);
cos(DH(5,4))*sin(DH(5,1)) cos(DH(5,4))*cos(DH(5,1)) -sin(DH(5,4)) -DH(5,2)*sin(DH(5,4));
sin(DH(5,4))*sin(DH(5,1)) sin(DH(5,4))*cos(DH(5,1)) cos(DH(5,4)) DH(5,2)*cos(DH(5,4));
0 0 0 1];
T56=[cos(DH(6,1)) -sin(DH(6,1)) 0 DH(6,3);
cos(DH(6,4))*sin(DH(6,1)) cos(DH(6,4))*cos(DH(6,1)) -sin(DH(6,4)) -DH(6,2)*sin(DH(6,4));
sin(DH(6,4))*sin(DH(6,1)) sin(DH(6,4))*cos(DH(6,1)) cos(DH(6,4)) DH(6,2)*cos(DH(6,4));
0 0 0 1];
T06=T01*T12*T23*T34*T45*T56;
end