一、机械臂及运动学

1.1 机械臂构成

  机械臂多采用关节式机械结构，一般具有6个自由度，其中3个用来确定末端执行器的位置，另外3个则用来确定末端执行装置的方向(姿态)。
  如图所示，一个机械臂是由一组可做相对运动的关节连接的连杆结合体。第一个连杆固定，连接该机械臂的基座，而最后一个连杆连接的是它的末端执行器。

  通常可将关节划分为两种：第一种称为转动关节(或称为旋转关节)，转动关节可绕基准轴转动，相应的转动量称为关节角：第二种称为移动关节，移动关节是沿着基准轴移动，相应的位移称为关节偏距。还有一种特殊的关节称为球关节，球关节拥有三个自由度。可以用三个转动关节和一个零长度的连杆来描述一个球关节。
  位于机械臂固定基座的坐标系称为基坐标系；位于操作臂末端执行器的坐标系称为工具坐标系，通常用它来描述机械臂的位置。

1.2 机器人运动学介绍

  机器人运动学只研究机器人运动，不关注机器人运动过程中各零部件的质量及相关力，也不关注关节驱动力和力矩。
 （1）机器人正运动学：给定一组机器人关节变量（转角或位移），求解末端工具坐标系相对于基坐标系的位置和姿态。
 （2）机器人逆运动学：给定机器人末端工具箱坐标系的位置和姿态，求解机器人各关节变量。

二、D-H参数法

  D-H 参数全称为Denavit-Hartenberg参数，它使用连杆参数来描述机构运动关系。在DH参数法中，描述机械臂中的每一个连杆需要4个运动学参数：
● 连杆长度$a_{i-1}$：关节轴$i-1$与关节轴$i$之间公垂线的长度；
● 连杆转角${\alpha }_{i-1}$：第$i-1$个关节轴和第$i$个关节轴之间的夹角；
● 连杆偏距$d_i$：沿两个相邻连杆公共轴线方向的距离；
● 关节角${\theta }_i$：两相邻连杆绕公共轴线旋转的夹角。

2.1 标准D-H法（SDH）

建模规则：
（1）找出各关节轴，画出关节轴的延长线；
（2）确定$Z$轴：与关节轴线重合，其中$Z_{i-1}$轴与关节$i$的轴线重合；
（3）确定$X$轴：$Z_{i-1}$轴与$Z_i$轴的公垂线，方向由$Z_{i-1}$轴指向$Z_i$轴
  1）如果$Z_{i-1}$轴与$Z_i$轴平行，选取与前一关节的公垂线共线的一条公垂线作为$X$轴；
  2）如果$Z_{i-1}$轴与$Z_i$轴相交，$X$轴为$Z_{i-1}$轴与$Z_i$轴的叉积方向；
（4）确定$Y$轴：右手定则。

D-H参数含义：
（1）$a_i$：关节轴线$i-1$和关节轴线$i$的公垂线长度
（2）${\alpha }_i$：关节轴线$i-1$和关节轴线$i$的夹角，指向为从轴线$i-1$到轴线$i$；
（3）$d_i$：关节$i$上的两条公垂线$a_{i-1}$ 与$a_i$之间的距离，沿关节轴线$i$测；
（4）${\theta }_i$：连杆$i$相对于连杆$i-1$绕轴线$i$的旋转角度。

齐次变换矩阵：
  从坐标系$i-1$到坐标系$i$，先绕$Z_{i-1}$轴旋转角度${\theta }_i$l，再沿$Z_{i-1}$轴移动$d_i$，然后沿$X_i$轴平移$a_i$，最后绕$X_i$轴旋转${\alpha }_i$。齐次变换矩阵${}_{i-1}^{i}T$可以写为：

2.2 改进D-H法（MDH）

建模规则：
（1）找出各关节轴，画出关节轴的延长线；
（2）确定$Z$轴：与关节轴线重合，其中$Z_i$轴与关节$i$的轴线重合；
（3）确定$X$轴：$Z_i$轴与$Z_{i+1}$轴的公垂线，方向由$Z_i$轴指向$Z_{i+1}$轴；
（4）确定$Y$轴：右手定则。

D-H参数含义：
（1）$a_{i-1}$：沿$X_{i-1}$轴，从$Z_{i-1}$轴移动到$Z_i$轴的距离；
（2）${\alpha }_{i-1}$：绕$X_{i-1}$轴，从$Z_{i-1}$轴旋转到$Z_i$轴的角度；
（3）$d_i$：沿$Z_i$轴，从$X_{i-1}$轴移动到$X_i$轴的距离；
（4）${\theta }_i$：绕$Z_i$轴，从$X_{i-1}$轴旋转到$X_i$轴的角度；

齐次变换矩阵：
从坐标系$i-1$到坐标系$i$，先绕$X_{i-1}$轴旋转角度${\alpha }_{i-1}$，再沿$X_{i-1}$轴平移$a_{i-1}$，然后绕$Z_i$轴旋转角度${\theta }_i$，最后沿$Z_i$轴移动$d_i$。齐次变换矩阵${}_{i-1}^{i}T$可以写为：

2.3 SDH和MDH的区别及适用场景

2.3.1 SDH和MDH的区别

  （其实一直以来笔者都不太能区分这两种方法，笔者最开始学习机器人学是看的《机器人学导论》这本书，上面似乎是偏向于MDH方法，所以后续我都是习惯于用MDH去建模）
  （1）区别一：连杆坐标系建立的位置不同。SDH将连杆$i$的坐标系固定在连杆的远端；MDH将连杆$i$的坐标系固定在连杆的近端。

  （2）区别二：执行变换的顺序不同。

2.3.2 SDH和MDH的适用场景

  对于树形结构或者闭链机构的机器人来说，按照SDH方法建立的连杆坐标系会产生歧义，因为SDH的建系原则是把连杆$i$的坐标系建立在连杆的远端，如图(a)所示，导致连杆0上同时出现了两个坐标系。而MDH把连杆坐标系建立在每个连杆的近端，则不会坐标系重合的情况，如图(b)所示，这就克服了SDH方法建系的缺点。

总结：
（1）SDH适合应用于开链结构的机器人；
（2）当使用SDH表示树状或闭链结构的机器人时，会产生歧义；
（3）MDH法对开链、树状、闭链结构的机器人都适用。

三、机器人运动学

3.1 创建一个连杆对象

  在机器人工具箱中，还用变量${\sigma }_i$表示机器人的关节类型，${\sigma }_i=0$表示转动关节，${\sigma }_i=1$表示移动关节(若未指定该参数，默认为转动关节)。
  在工具箱中，用函数Link( )可以创建一个机械臂对象，其中输入的参数顺序分别是：关节角${\theta }_i$、连杆偏距$d_i$、连杆长度$a_{i-1}$、连杆转角${\alpha }_{i-1}$和关节类型。例如，创建一个关节角为30°，连杆偏距为0.2m，连杆长度为0.5m，连杆转角为60°，关节类型为旋转关节的连杆，代码如下所示：

L = Link([pi/6, 0.2, 0.5, pi/3, 0])

这里，Revolute表示转动关节，std表示标准D-H参数法，offset表示关节偏移量。可以通过以下命令获得连杆的各个参数：
 ● 获取连杆的关节类型：L.type( )（注意：老版本的工具箱是L.RP）;
 ● 获取连杆的关节角：L.theta;
 ● 获取连杆的连杆偏距：L.d;
 ● 获取连杆的连杆长度：L.a;
 ● 获取连杆的连杆转角：L.alpha。

 ● 获取改连杆的齐次变换矩阵：L.A($\theta$)，例如连杆转动了30°，其齐次变换矩阵T为：

3.2 创建一个平面3-DOF的机械臂

  这里创建了一个平面三R机构（三个转动关节），该平面三连杆机构的DH参数表如下所示：

% standard-表示标准DH法
%               theta(z)   d(z)     a(x)     alpha(x)  
RRR_L(1) = Link([  0       0        1        0    ],'standard');
RRR_L(2) = Link([  0       0      0.8        0    ],'standard');
RRR_L(3) = Link([  0       0      0.6        0    ],'standard');

运行结果：

通过构造函数SerialLink( )可以给创建的机械臂对象命名，并显示出对象的信息。输入命令:

three_link = SerialLink(RRR_L, 'name', 'three-link-RRR')

运行结果：

用以下的命令可以获取已创建机械臂的各个参数：

运行结果：

同时也可以对创建的机械臂对象进行复制，如复制一个名称为“three_link2”的机械臂输人命令:

three_link2 = SerialLink(RRR_L, 'name', 'three-link-RRR_2')

运行结果：

使用teach( )函数，可以对创建的机器人进行示教，如图所示：

3.3 机器人正运动学

  机器人正运动学即给定一组关节角，计算出机器人末端相对于基坐标系的位置和姿态，如下图：

  机器人工具箱中，fkine( )可以进行正运动学计算，即给定关节变量，得到末端坐标系关于基坐标系的齐次变换矩阵（位置和姿态）。这里以3.2节的3-DOF平面机械臂为例，当机器人三个关节角都为0°时：

% RRR机械臂
clear;
close all;
clc;

%               theta(z)   d(z)     a(x)     alpha(x)  
RRR_L(1) = Link([  0       0        1        0    ],'standard');
RRR_L(2) = Link([  0       0      0.8        0    ],'standard');
RRR_L(3) = Link([  0       0      0.6        0    ],'standard');

three_link = SerialLink(RRR_L, 'name', 'three-link-RRR');
q0 = [0 0 0]'*pi/180;
T0 = three_link.fkine(q0)

运行结果：

当机器人一关节10°，二关节-30°，三关节60°时，机器人末端的位置和姿态如图：

可以用plot( )函数绘制此时机器人的状态，如图：

例子：给定平面3-RRR机械臂各个关节的角度如图所示，求解机器人运动过程中末端点的三维坐标

% RRR机械臂
clear;
close all;
clc;

%               theta(z)   d(z)     a(x)     alpha(x)  
RRR_L(1) = Link([  0       0        1        0    ],'standard');
RRR_L(2) = Link([  0       0      0.8        0    ],'standard');
RRR_L(3) = Link([  0       0      0.6        0    ],'standard');

three_link = SerialLink(RRR_L, 'name', '3-DOF');

t = 0:0.05:4;
m = length(t);
theta1 = 120*sin(4*pi*t/4);
theta2 = 60*sin(2*pi*t/4);
theta3 = 30*sin(2*pi*t/4);

q = [theta1;theta2;theta3]*pi/180;

for i = 1:m
    T = three_link.fkine(q(:,i)');
    x(i) = T.t(1);
    y(i) = T.t(2);
    z(i) = T.t(3);
    
    subplot(3,3,[1 4 7])
    hold on
    plot3(x(i),y(i),z(i),'r*');
    hold on
    three_link.plot(q(:,i)');
    
    subplot(3,3,2)
    hold on
    plot(t(i),q(1,i)*180/pi,'b.')
    xlabel('time /s')
    ylabel('\theta_1 /deg')
    
    subplot(3,3,5)
    hold on
    plot(t(i),q(2,i)*180/pi,'r.')
    xlabel('time /s')
    ylabel('\theta_2 /deg')
    
    subplot(3,3,8)
    hold on
    plot(t(i),q(3,i)*180/pi,'m.')
    xlabel('time /s')
    ylabel('\theta_3 /deg')
    
    subplot(3,3,3)
    hold on
    plot(t(i),x(i),'b.')
    xlabel('time /s')
    ylabel('x /m')
    
    subplot(3,3,6)
    hold on
    plot(t(i),y(i),'r.')
    xlabel('time /s')
    ylabel('y /m')
    
    subplot(3,3,9)
    hold on
    plot(t(i),z(i),'m.')
    xlabel('time /s')
    ylabel('z /m')
end

运行结果：

3.4 机器人逆运动学（未完待续）

笔者最近有点忙。。。