引言

在讨论正向运动学之前，我们先了解两个概念，一个是运动学（Kinematics），一个是动力学（Dynamics）。看起来名字都差不多，那么它们有什么差别呢？

运动学（Kinematics）：讨论的是运动状态，未联系到产生运动的力。也就是位移，速度，加速度和时间之间的关系，比如$v=\frac{d}{dt}x$，$a=\frac{d}{dt}v$。
动力学（Dynamics）：讨论的是力和力矩是如何产生运动的。比如，$\sum F=ma$。

在了解完运动学的概念后，那么什么是正向运动学呢？假设我们有一个简单的机械臂，如下图所示：

当我们知道了$l_1,l_2,l_3,\theta_1,\theta_2,\theta_3$时，通过简单的三角函数计算我们就可以一步一步的计算出机械臂末端点P所在的位置，那么这个就叫做正向运动学。反之，如果我们想要机械臂末端点P到达某个位置，那么我们就需要知道这个位置所对应的$\theta_1,\theta_2,\theta_3$，这个过程就是反向运动学（Inverse Kinematics），这个我们在下一讲介绍。

机械手臂的几何描述

机械手臂是由多个杆件（link）和关节（joint）所组成的。

机械手臂的关节不但可以进行转动（revolute）还可以进行移动（prismatic），每个关节的运动都会沿着某特定axis进行。
机械手臂的杆件的作用就是将各个关节连接起来。

为了计算出机械臂末端点所在空间的位置，我们需要一步一步的找到各个joint在空间中的位置，因此我们可以在每一个joint上建立坐标系，然后利用上一讲中介绍的Transformation Matrix来计算各个坐标系在空间中的位置，这样我们就间接的知道了各个joint的位置。

第一步：建立frame

在joint上建立坐标系的方式如下：

首先要确定所有joint的$\hat Z$轴的方向，$\hat Z$轴就是这个joint转动或移动的方向。
然后再确定$\hat X_i$的方向，要沿着$a_i$的方向，$a_i$就是$\hat Z_i$和$\hat Z_{i+1}$之间的距离。但是，当$a_i=0$也就是说$\hat Z_i$和$\hat Z_{i+1}$相交时，$\hat X_i$要取和$\hat Z_i$和$\hat Z_{i+1}$两者都垂直的方向。
最后再确定$\hat Y_i$的方向，与$\hat X_i$和$\hat Z_i$两者垂直并遵循右手定则（拇指X，食指Y，中指Z）。

第二步：Denavit-Hartenberg表达法（Craig version）

Denavit-Hartenberg表达法简称为DH表达法，它可以用4个参数来表达两个joint之间的关系。因此，如果将所有joint的关系整合起来形成一个表格，那么这个表格就可以用来表达完整的机械手臂，我们把这个表格叫做DH表，那4个参数分别为：$\alpha,a,d,\theta$。

$\alpha_{i-1}$：是两个joint之间的夹角，计算方法是沿着$\hat X_{i-1}$的方向看过去，从$\hat Z_{i}$转到$\hat Z_{i-1}$的角度，顺时针为负，逆时针为正。
$a_{i-1}$：是两个joint之间的距离，计算方法是沿着$\hat X_{i-1}$的方向看过去，$\hat Z_{i-1}$到$\hat Z_i$的距离，可正可负。
$\theta_i$：对于转动的joint来说，这个值是变量。计算方法是沿着$\hat Z_{i}$的方向看过去，从$\hat X_{i}$转到$\hat X_{i-1}$的角度，顺时针为负，逆时针为正。
$d_i$：对于移动的joint来说，这个值是变量。计算方法是沿着$\hat Z_{i}$的方向看过去，从$\hat X_{i-1}$到$\hat X_{i}$的距离，可正可负。

第三步：计算每个joint之间的Transformation Matrix

接下来我们就要计算出每个joint之间的Transformation Matrix，计算方式如下：
$$
^{i-1}_iT=T_{\hat X_{i-1} }(\alpha_{i-1})T_{\hat X_R}(a_{i-1})T_{\hat Z_Q}(\theta_i)T_{\hat Z_P}(d_i) \\
= \begin{bmatrix}
c\theta_i & -s\theta_i & 0 & a_{i-1} \\
s\theta_ic\alpha_{i-1} & c\theta_ic\alpha_{i-1} & -s\alpha_{i-1} & -s\alpha_{i-1}d_i \\
s\theta_is\alpha_{i-1} & c\theta_is\alpha_{i-1} & c\alpha_{i-1} & c\alpha_{i-1}d_i \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

计算的过程都是遵循上一讲所说的Eular Angles的情况，所以计算过程都是右乘。首先呢，先将i-1 frame沿着X轴转动到R frame，再沿着X轴移动到Q frame，接着再沿着Z轴转动到P frame，最后呢再沿着Z轴移动到i frame，就得到了我们最终想要的$^{i-1}_iT$。
同样的，它也可以进行多个Transformation Matrix连乘：
$$
^{i-1}_iT=^{0}_1T^{1}_2T^{2}_3T…^{n-2}_{n-1}T^{n-1}_nT
$$

Note: 通过这种方式我们就可以得到任意joint相对于基坐标（0 frame）的Transformation Matrix，从而得到任意joint相对于基坐标的位置。

举例 PUMA 560 -1

建立frame

PUMA 560-1机械手臂坐标系的建立如下图所示，当然这并不是唯一的方法，X轴，Y轴，Z轴的方向都可以和下图所示的方法相反，只不过不同的方法会建立不同的DH表。

Note:

0 frame是基坐标，可以定在任意位置。

$\hat Z_1$与$\hat Z_2$相交，所以$\hat X_1$的方向要垂直与$\hat Z_1$和$\hat Z_2$

建立DH表

DH表如下图所示。因为我们的手臂有6个DOF，所以i的取值就从1-6。1代表1 frame，2代表2 frame，以此类推。首先看第一行，因为0 frame和1 frame完全重合，所以$\alpha_0, a_0, d_1$都是0，唯一可能会变的就是$\theta_1$这个变量。第二行中，沿着$\hat X_{1}$的方向看过去，从$\hat Z_{2}$转到$\hat Z_{1}$的角度为顺时针$90^\circ$，所以$\alpha_1=-90^\circ$。$\hat Z_{1}$到$\hat Z_{2}$的距离为0，所以$a_1=0$，$\hat X_{1}$到$\hat X_{2}$的距离为0，所以$d_1=0$，$\theta_2$同样是变量。以此类推就可以得到整个DH表。