机械臂基础（应用）知识学习2

前言

上篇文章中提到了 DH 法以及对应的 MATLAB 建模方法，但是这部分的内容只是确定了四个 DH 参数，剩下的相当于交给了工具库去做。但要真正理解这个转换过程，需要通过实际的计算求解出坐标值，这便是这篇的重点，也是正运动学的核心内容。

运动学正解理论

求运动学正解就是已知各关节的运动参数，求末端执行器的相对参考坐标系的位姿。求解步骤如下：

各连杆首尾相连；
确定各连杆间的齐次变换矩阵；
得到最后的总变换矩阵。
且该总变换矩阵内的未知数只有各轴的旋转角度，因此得到旋转角度即可得到六自由度机器人的末端笛卡尔空间坐标。

齐次变换矩阵：

已知机器人的四个参数为：关节角、连杆偏移、扭转角、连杆长度，它们的物理意义如下：

参数是轴和轴之间沿轴线测得的距离；
角度是在垂直于平面内测得的轴线和之间的夹角。角度的正向取值定义为从到，由右手规则来确定；
参数为从原点到轴线和交点之间的距离，该距离沿轴线进行测量得到；
是在垂直于的平面内测得的从到的角度。
理解了上述的符号约定及程序规则后，在此基础上，每个齐次变换矩阵都可以表示为是个基本矩阵的乘积。
对于标准型 D-H 参数：
其乘积顺序如下：

通用齐次变换矩阵如下：
对于改进型 D-H 参数：
其乘积顺序如下：

通用齐次变换矩阵如下：

总变换：
确定好 DH 参数建立方式并构建出各关节的 DH 参数后，代入各自的通用齐次变换矩阵后可以得到六个矩阵。在我参考的文章里作者使用的是 MDH 参数，但由于此前建模模型原因我在这里使用 SDH 参数进行计算。对该六个齐次变换矩阵按顺序相乘后得到六自由度机器人的总变换：

运动学正解代码实现

这个过程实际上是建模公式运转过程的具现化，通过下面代码进行计算，所得到的通用齐次变换矩阵可以和建模结果进行对比检查，最后一列前三行为坐标值，只要是采用的 SDH 方法，下面的代码是没什么问题的。

%连杆偏移
d1 = 152;
d2 = 0;
d3 = 0;
d4 = 130;
d5 = 102;
d6 = 100;
%连杆长度
a1 = 0;
a2 = -425;
a3 = -395;
a4 = 0;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = pi/2;
alpha2 = 0;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = 0;
%由于我们需要分析各轴θi所对应的机器人末端位置
%因此theta1 theta2 theta3 theta4 theta5 theta6仍设为未知量
%syms theta1 theta2 theta3 theta4 theta5 theta6
% 参数矩阵取名为SDH
theta1 = pi/3;
theta2 = pi/4;
theta3 = pi/5;
theta4 = pi/3;
theta5 = pi/4;
theta6 = pi/5;
SDH = [theta1          d1       a1       alpha1;
       theta2          d2       a2       alpha2;    
       theta3          d3       a3       alpha3;
       theta4          d4       a4       alpha4;
       theta5          d5       a5       alpha5;
       theta6          d6       a6       alpha6];
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;

结果对比如下：

小结

以上过程仅仅是偏向于应用的代码流程，真正的转换矩阵含义以及具体计算过程还需要深入研究，后续会再做学习。用这种方式快速上手再加以对理论的学习，我觉得要比从理论出发更具有效率。

本篇着重介绍正运动学部分，即如何从关节角度推出坐标和旋转角度。

前言

运动学正解理论

运动学正解代码实现

小结

FEATURED TAGS