标准6轴机器人正反解(2)-正解

连杆变换矩阵

概括起来，求正解即是求坐标系{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