一、描述一台机械臂（D-H参数）

首先了解一下D-H参数描述机械臂的由来。一个机器人的关节结构可以用一串字符来描述，例如Puma 560为RRRRRR，斯坦福臂为RRPRRR，其中每个字符代表相应关节的类型，R是转动副，R为移动副，因此在1955年迪拉维特和哈腾贝尔提出了一种用于描述这种串联式链路上连杆和关节几何关系的系统方法，这就是我们熟知的D-H参数法。

如下图所示为参数法的说明，{j}\lbrace{j}\rbrace{j}坐标系固连于连杆$j$的远端或末梢，关节$j$的轴线即为改坐标系的$z$轴。这些连杆和关节的参数即我们所熟知的$D-H$参数，总结于下表中。

关节角	${\theta }_j$	$x_{j-1}$轴和$x_j$轴之间关于$z_{j-1}$ 轴的角度	转动关节变量
连杆偏移	$d_j$	沿着$z_{j-1}$轴，$j-1$坐标系原点到$x_j$轴的距离	转动关节变量
连杆长度	$a_j$	沿着$x_j$，$z_{j-1}$轴和$z_j$轴之间的距离；对于相交轴，它与${\hat{z}}_{j-1}\times {\hat{z}}_j$平行	常量
连杆扭转角	${\alpha }_j$	$z_{j-1}$轴和$z_j$轴之间关于$x_j$之间的角度	常量
关节类型	${\sigma }_j$	$\sigma =0$为转动副，$\sigma =1$为移动副	常量

上面的表格加上图片看起来很乱，那使用一种通俗的语言来描述前面的四个参数。首先两个连杆的转动关节是绕着一个轴来旋转的，因此以这两个轴来建立坐标系（即坐标系的$z$轴），而且两个中轴线肯定有一个公共的垂线，如图中所示，在{$j$}坐标系中，原点是公垂线与关节$j+1$的轴线的交点，$x_j$是沿着公垂线的反向延长线，将关节$j+1$的轴线沿着$x_j$的反向平移（即公垂线）会与关节$j$的中轴线相交，相交时两个中轴线的夹角即为连杆扭转角（${\alpha }_j$）。当关节$j+1$的坐标系平移到关节$j$的中轴线时，两个原点会都处在关节$j$的中轴线上，两个原点之间的距离即为连杆偏移（$d_j$），也可以这样理解，就是一个关节和前后两个关节都有公垂线，两个公垂线在这个关节中轴线上面的距离就是连杆偏移。所谓的连杆长度也就是公垂线的长度，即（$a_j$）。最后就是关节角，当{$j$}坐标系中沿$x_j$轴反向平移到关节$j$的中轴线的时，$x_{j-1}$和$x_j$之间的夹角就是关节角 (${\theta }_j$)。

• 这样就好理解一点，但是这里面所说的距离都是有方向的，该方向就是表格中关于xx轴的角度/距离，如果为反方向就为负。
• 当建立连杆的坐标系时，基坐标系一般取转动的轴为$z$轴，$x$轴和$y$轴一般和第二个坐标系中的$x$轴和$y$轴平行，后面的坐标系都是此关节中轴线和前一个关节公垂线的交点为原点建立坐标系。最后一个坐标系一般建立是考虑到运动的姿态。
• 按照约定，第一个关节，即关节1，连接连杆0和连杆1。连杆0为机器人的基座。因此，通常第一个连杆的连杆偏移和扭转角都为0，但是，我们可以设定$d_1>0$用来表示肩关节的高度高出基座。最后一个关节，通常工具通向指向$z$轴。
  从连杆坐标系$j-1$到坐标系$j$的变换被定义为基本旋转和平移，形式如下：
  $${}^{j-1}{A}_j\left({\theta }_j,d_j,a_j,{\alpha }_j\right)=T_{Rz}\left({\theta }_j\right)T_z\left(d_j\right)T_x\left(a_j\right)T_{Rx}\left({\alpha }_j\right)$$
  展开为：
  $${}^{j-i}{A}_j=\left(\begin{array}{cccc}\cos {\theta }_j& -\sin {\theta }_j\cos {\alpha }_j& \sin {\theta }_j\sin {\alpha }_j& {\alpha }_j\cos {\theta }_j\\ \sin {\theta }_j& \cos {\theta }_j\cos {\alpha }_j& -\cos {\theta }_j\sin {\alpha }_j& {\alpha }_j\sin {\theta }_j\\ 0& \sin {\alpha }_j& \cos {\alpha }_j& d_j\\ 0& 0& 0& 1\end{array}\right)$$
  参数$a_j$和${\alpha }_j$始终是常量。对于转动关节${\theta }_j$是关节变量且$d_j$是常量，然而对于移动关节$d_j$是关节变量且${\theta }_j$是常量，且${\alpha }_j=0$，在以后的公式中，我们将使用如下的广义关节坐标：
  $$q_j=\{\begin{array}{cc}{\theta }_j,& {\sigma }_j=0\\ d_j,& {\sigma }_j=1\end{array}$$
  用MATLAB来建立一个机器人连杆,其中$Link()$函数中的参数分别是${\theta }_j$,$d_j$,$a_j$,${\alpha }_j$,最后一个元素为0，则是转动，为1则为移动，如果没有定义默认为转动。运行之后的$Revolute(std)$表示的是转动$（R）$，以及标准的$D-H$参数$(standard)$。但是会发现即便给${\theta }_j$进行赋值了，但是还是没有在运行结果中显示出来，这个值在运动学参数系列中仅仅作为一个占位者，因此将被$q$所取代。

>>L = Link([0, 0.1, 0.2, pi/2, 0])
>L = 
Revolute(std): theta=q, d=0.1, a=0.2, alpha=1.5708, offset=0

当给$q$进行赋值的时候

>>L = Link([0, 0.1, 0.2, pi/2, 0]);
>>L.A(0.5)
>ans = 
    0.8776         0    0.4794    0.1755
    0.4794         0   -0.8776   0.09589
         0         1         0       0.1
         0         0         0         1

也可以进行参数值的读取,以及添加了偏移量之后给$\theta$赋值为0得到以下的结果。

>>L = Link([0, 0.1, 0.2, pi/2, 0])
>>L.a
>>L.d
>>L.offset=0.5;
>>L.A(0)
>ans =

    0.2000


ans =

    0.1000
>ans = 
    0.8776         0    0.4794    0.1755
    0.4794         0   -0.8776   0.09589
         0         1         0       0.1
         0         0         0         1

二、正运动学

机械臂正运动学的一般表述形式为：
$${\xi }_E=\mathrm{K}\left(q\right)$$
它表示末端执行器的位姿是基于关节坐标的一个函数。若使用齐次变换，将上面的式${}^{j-i}{A}_j$所给的单连杆变换矩阵的简单乘积。对于一个$N$轴机械臂，有
$${\xi }_E\sim {}^0{T}_E={}^0{A}_1{}^1{A}_2\cdots {}^{N-1}{A}_N$$
对于任何一个串联机械臂，无论其关节数量和类型如何，都可以计算出其正向运动学的解。一般${}^0{T}_E$中的$E$是关节的数量或者自由度。

1、传统方式对末端位姿的求取

设有一个5自由度的机械臂，5个关节均为转动关节，机械臂关节长度$a_2=1200mm$,$a_3=100mm$,偏置$d$均为0。运用MATLAB进行编程。

clc
clear
syms theta1 theta2 theta3 theta4 theta5
syms a1 a2 a3 t10 t21 t32 t43 t54
a2 = 1.2;
a3 = 1;
theta1 = 10 * pi/180;
theta2 = 40 * pi/180;
theta3 = 40 * pi/180;
theta4 = 10 * pi/180;
theta5 = 30 * pi/180;
t10 = [cos(theta1) -sin(theta1) 0 0;sin(theta1) cos(theta1) 0 0;0 0 1 0;0 0 0 1];
t21 = [cos(theta2) -sin(theta2) 0 0;0 0 1 0;-sin(theta2) -cos(theta2) 0 0;0 0 0 1];
t32 = [cos(theta3) -sin(theta3) 0 a2;sin(theta3) cos(theta3) 0 0;0 0 1 0;0 0 0 1];
t43 = [cos(theta4) -sin(theta4) 0 a3;sin(theta4) cos(theta4) 0 0;0 0 1 0;0 0 0 1];
t54 = [cos(theta5) -sin(theta5) 0 0;0 0 -1 0;sin(theta5) cos(theta5) 0 0;0 0 0 1];
t50 = t10*t21*t32*t43*t54

运行结果为：

t50 =

    0.6518   -0.5768    0.4924    2.0359
    0.6226    0.7777    0.0868    0.3590
   -0.4330    0.2500    0.8660    0.0684
         0         0         0    1.0000

2、利用机器人工具箱对末端位姿的求取

clc
clear
L(1) = Link([0 0 0 -pi/2]);
L(2) = Link([0 0 1.2 0]);
L(3) = Link([0 0 1 0]);
L(4) = Link([0 0 0 pi/2 ]);
L(5) = Link([0 0 0 0]);
five_link = SerialLink(L, 'name', 'fivelink');
five_link.fkine([10*pi/180 -20*pi/180 40*pi/180 10*pi/180 30*pi/180])

运行结果：

ans = 
    0.6518   -0.5768    0.4924     2.036
    0.6226    0.7777    0.0868     0.359
   -0.4330    0.2500    0.8660    0.0684
         0         0         0         1

可以看出，经过传统的方法和MATLAB工具箱计算出来的的结果完全相同。

二、逆运动学

给定期望的末端执行器位姿${\xi }_E$，求得所需的关节坐标，函数表达式如下：
$$q={\mathrm{K}}^{-1}\left(\xi \right)$$
一般这个函数的解不是唯一的，但对于某些类型的机械臂甚至没有封闭形式的解存在，只能求出一个数值解。

1、封闭形式解

对于一个六轴机器人，其逆运动学具有封闭形式解的一个必要条件是：三个腕关节的轴相交于一个点。这意味着腕关节的运动只改变末端执行器的姿态，而不是改变其位置。
以下将使用$Puma$机器人模型来了解逆运动学。

clc
clear
mdl_puma560
qn
T = p560.fkine(qn)    %标准状态的关节坐标
qi = p560.ikine6s(T)    %标准状态逆运动的封闭解
qiright = p560.ikine6s(T, 'ru')    %右手位形解

仿真结果

qn =

         0    0.7854    3.1416         0    0.7854         0

 

T = 
         0         0         1    0.5963
         0         1         0   -0.1501
        -1         0         0  -0.01435
         0         0         0         1

qi =

    2.6486   -3.9270    0.0940    2.5326    0.9743    0.3734


qiright =

   -0.0000    0.7854    3.1416   -0.0000    0.7854    0.0000

从仿真结果可以看出，当使用逆运动解出来的关节坐标$qi$与标准的关节坐标不太相同，也就是说两组不同的关节坐标值得到了相同的末端执行器位姿，而他们的位形却不相同。

2、数值解

对于不具有6个关节和球形腕关节的机器人，就需要一种迭代数值解。继续上面的机械臂进行分析。

clc
clear
mdl_puma560
T = p560.fkine(qn)
qi = p560.ikine(T)
qn
p560.fkine(qi)
p560.plot(qi)

仿真结果：

T = 
         0         0         1    0.5963
         0         1         0   -0.1501
        -1         0         0  -0.01435
         0         0         0         1

qi =

   -0.0000   -0.8335    0.0940    0.0000   -0.8312   -0.0000


qn =

         0    0.7854    3.1416         0    0.7854         0

 

ans = 
    0.0000   -0.0000         1    0.5963
   -0.0000         1    0.0000   -0.1501
        -1   -0.0000    0.0000  -0.01435
         0         0         0         1

仿真结果清晰的表明$ikine$方法找到了肘关节在下的位形。但是通用数值解法的局限性是：它不像解析法那样能提供对手臂运动位形的明显控制，只有通过设定关节坐标初始值（默认为零）的隐含控制。如果指定初始关节坐标：

>>qi = p560.ikine(T, [0 0 3 0 0 0])
>q =

   -0.0000   0.7854    3.1416    0.0000   -0.7854   -0.0000

这时其数值解收敛于肘关节在上的位形。
通用数值解法$ikine$要比解析解法$ikine6s$慢很多，但是它能够求解处于奇异位形以及非6关节型的机械臂。
注意：MATLAB机器人工具箱需要提前打开，打开代码：startup_rvc