机械臂的正向运动学（标准D-H参数的变换矩阵）

研究机械臂的正向运动学需要从机械臂的变换矩阵开始。
变换矩阵可以说有两种形式，一种产生了标准D-H参数，另一种产生了改进D-H参数表。

标准D-H参数——变换矩阵

引用一些博客的图片，以及我多次的试验，在这里记下来。

在此之前，需要重点强调的是，无论是标准D-H参数法还是改进D-H参数法，我们都是使用矩阵右乘的法则：
在下面提到的变换的四个过程中，比如有两个坐标系，一个是初始坐标i，一个是目的坐标系j。我们会设定一个作为中间转换过程的坐标系A，一开始这个坐标系A是与初始坐标i重合。我们仔细留意一下，**就知道下面的每一次转化都是在运动的坐标A或者是最后的坐标系j来进行的。**这样就呼应了右乘的方法。
标准D-H参数法的变换矩阵的顺序是，
按照Z-X的顺序：
① 先绕坐标系i的Z轴旋转θ角，该值便为θ1。使得Xi轴与Xj轴平行。—— 先绕坐标系A的轴旋转θ角，使得XA与Xj平行。 一样的功能，破折号前是简便的说法。
②在沿着坐标系i的Z轴，将Xi轴移动D1的距离，与Xi轴与Xj重合。——沿着坐标系A的Z轴，将XA移动D1的距离，这里就可以清晰理解，坐标系i其实是没动过的，动的只是坐标系A，而坐标j也算是运动的坐标系
③接着绕着坐标系j的Xj轴(同时也是Xi轴，应为此前已经重合了），旋转α角，该值为α1。—— 现在到绕这坐标系A的X轴旋转了
④最后便是沿着坐标系j的Xj轴，将坐标系i的Zi轴平移a1的距离。
这样一来便可以产生一个连杆，这个连杆可以命名为LINK(I);
最最最重要的一步是：
上面有四个步骤，每个步骤都可以写出一个齐次坐标矩阵(44),
由于使用的是矩阵右乘，来求出末端相对于参考系的位姿，（我的理解是在正向运动学中，末端是未知，是运动的。只能设置每个的关节的角度，按照矩阵乘法的概念），可以写成：
T1=ROT(θ)Trans(D1)ROT(α)Trans(a1),
然后就是有末端执行器的[xj,yj,zj,1]T=T1[xi,yi,zi,1]。
最后上面的四个变换便可以写成一个公式，(在MATLAB上找到证据)

如果是标准D-H参数的话，就选第一个作为计算正向运动的式子，如果是改进D-H参数，就选择第二个。
假设我有一个六自由度的机械臂，那么就有六个这样的矩阵，
①号关节-②号
②号——③号
③号——④号
④号——⑤号
⑤号——⑥号
⑥号——工具(最末端的坐标系)
这样子，利用这些矩阵便可以求出末端执行器相对于基坐标的位姿。
公式是这样的：

   i2=T(1-2)*i1..............
   最后变换便可以得到
   iE=T(1-2)T(2-3)T(3-4)T(4-5)T(5-6)T(6-E)i0,(当然这个公式是基于基坐标系的，参考坐标系不同，公式就会不同。
   为此我也在Matlab上验证了一下。

L1 = Link([0 348.5 0 pi/2 ],‘standard’);%1 2
L2 = Link([0 0 284.5 0 ],‘standard’);%2 3
L3 = Link([0 0 30 pi/2 ],‘standard’);%3 4
L4 = Link([0 286.5 0 -pi/2 ],‘standard’);%4 5
L5 = Link([0 0 0 pi/2 ],‘standard’);%5 6
L6 = Link([0 81.5 0 0 ],‘standard’);%6 和末端执行器
robot = SerialLink([L1 L2 L3 L4 L5 L6 ],‘name’, ‘my robot’);
%robot.plot([0 0 0 0 0 0 ]);
teach(robot);
display(robot);

theta1=0;
theta2=0;%
theta3=0;
theta4=0;
theta5=0;
theta6=0;
q=[theta1 theta2 theta3 theta4 theta5 theta6];
T=robot.fkine(q);
robot.plot(q);

    a2= L2.a;
    a3= L3.a;
    d1= L1.d;
    d4= L4.d;
    d6= L6.d;

s1=sin(0);
c1=cos(0);
s2=sin(0);
c2=cos(0);
s3=sin(0);
c3=cos(0);
s4=sin(0);
c4=cos(0);
s5=sin(0);
c5=cos(0);
s6=sin(0);
c6=cos(0);



T1=[c1   0    s1  0;     s1    0     -c1  0;       0       1        0    d1;      0   0   0  1;]%1-2
T2=[c2  -s2   0    a2*c2;    s2    c2     0  a2*s2;        0       0       1    0;      0   0   0  1;] %2-3   
T3=[c3   0    s3   a3*c3;    s3    0     -c3  a3*s3;       0       1       0    0;      0   0   0  1;]%3-4
T4=[c4   0   -s4   0;     s4    0     c4   0;       0       -1      0   d4;    0   0   0  1;]%4-5
T5=[c5   0    s5   0;     s5    0     -c5  0;        0      1       0    0;      0   0   0  1;]%5-6
T6=[c6  -s6    0    0;     s6    c6    0  0;         0      0       1   d6;     0   0   0  1;]%6-7
Tz=T1*T2*T3*T4*T5*T6

我用的自己设计一款高仿机械臂，朋友们可以参考我的一篇博客，里面有介绍一些的详细的数据，经过验证，是正确的。
关于书籍的内容，
蔡自兴教授有编写过一版书
在介绍变换矩阵时，介绍了标准D-H参数法对应的变换方法和顺序，但是在后面介绍PUMA560时，用的是改进D-H参数对应的变换方法和顺序，以及他的公式。(我发现）
后来又看了一版书，教授使用的都是改进D-H参数，我觉得是这样的，不知到大家有没有发现。