正向运动学与D-H坐标

通常我们把机器人看做由关节(joint)连接的刚体连杆(link)所构成的运动链(kinematic chain)。关节可以是简单的只有一个自由度的转动关节(revolute joint)或者平动关节(prismatic joint)，也可以很复杂具有多个自由度。我们常常假设机器人身上的所有关节都是单自由度的，因为形式上把一个多自由度的关节看做是多个单自由度的关节的组合，对于分析系统的行为并没有影响。

在单自由度的假设下，每个关节的动作都可以通过一个实数来描述。转动关节允许连接的两个连杆绕一个轴相对转动，一般用字母$R$表示，其转过的角度记为$\theta$。平动关节允许一个连杆在一个方向上相对于另一个连杆平行移动，一般用字母$P$表示，其平移的距离记为$d$。所谓的正运动学是指根据机器人的各个关节的运动状态推导执行末端的位置和姿态；而逆运动学则是已知执行末端的位置和姿态，求各个关节的转角或平移距离。

本文关注机器人的正向运动学，先介绍运动链的正向运动学分析，然后介绍一种用于正运动学分析的工具D-H坐标(Denavit-Hartenberg)。最后以一款经典的PUMA机械臂为例介绍D-H坐标在正运动学中的应用。

1. 运动链(kinematic chain)的正向运动学

运动链是指由两个或两个以上的构件(link)通过运动幅(kinematic pair)链接起来的系统。如果构成的运动链首末相连则称之为闭链(closed chain)，否则称之为开链(open chain)。有时把由闭链系统构建的机器人称为并联机器人(parallel robot)。如图1(1)所示，机械臂通常都是开链的结构，而用于传送带上抓取系统的Delta机器人则是一种典型的闭链机构，如图1(2)所示。这里我们关注开链机构的机械臂正运动学求解问题。


图1(1) 机械臂通常都是开链的结构	图1(2) 一种典型的闭链机构——Delta机器人

对于一个开链机构，每个joint将连接两个link，而且一定有两个link只有一个关节joint。因此，一个有$n$个joint的开链机构将有$n+1$个link。我们从$0$到$n$对各个link编号，对joint则从$1$到$n$编号，关节$i$连接着构件$i-1$和构件$i$。当关节$i$运动时意味着构件$i$相对于构件$i-1$运动，所以一般情况下构件$0$是不动的，可以理解为机械臂上那个固定的基座，该坐标系为一个全局的惯性坐标系。我们关注执行末端，即构件$n$,在惯性坐标系$O_0$下的位置和姿态。用一个三维向量$\boldsymbol{d_n^0}$表示与执行末端固连的$O_n$系的坐标原点相对于$O_0$的位移，$R_n^0$为旋转矩阵，那么描述执行末端位置和姿态的齐次变换矩阵$H$可以表示如下： $$ \begin{equation}\label{f1} H = \begin{bmatrix} R_n^0 & \boldsymbol{d_n^0} \\ \boldsymbol{0} & 1 \end{bmatrix} \end{equation} $$

我们用$q_i$描述第i个关节的状态，若是转动关节则表示关节的转角$\theta_i$，对于平动关节则是平移距离$d_i$。为了进行运动学分析，为每个构件创建一个坐标系。当关节$i$发生运动时构件$i$将相对于构件$i-1$运动，与之固连的坐标系$O_i$随之运动。利用齐次变换矩阵$A_i$描述坐标系$O_i$ 相对于$O_{i-1}$的位置和姿态，在单自由度的假设下，$A_i$是一个关于$q_i$的单值函数: $$ \begin{equation}\label{f2} A_i = A_i(q_i) \end{equation} $$ 所以构件$i$相对于构件$j$的位置和姿态可以由齐次变换矩阵$A_i^j$得到： $$ \begin{equation}\label{f3} A_i^j = \begin{cases} A_{j+1}A_{j+2}...A_i, & \mbox{if } i > j \\ I, & \mbox{if } i = j \\ \left(A_j^i\right)^{-1} & \mbox{if } i < j \end{cases} \end{equation} $$ 那么公式($\ref{f1}$)所描述的执行末端的位置和姿态可以通过下式求得： $$ \begin{equation}\label{f4} H = A_n^0 = A_1A_2...A_n \end{equation} $$ 以上就是正向运动学的所有内容：根据机器人的结构为每个构件建立一个坐标系，并根据关节状态$q_i$确定变换矩阵$A_i(q_i)$；然后根据公式($\ref{f4}$)依次将各个矩阵相乘，得到执行末端的位置和姿态。但是，我们还可以引入一些约定简化这一分析过程，例如接下来将要介绍的D-H坐标。

2. D-H坐标(Denavit-Hartenberg)

在运动链的正向运动学分析中，我们希望直观地描述每个连杆相对于前一个连杆的位姿来确定执行末端的位置和姿态。而描述每个连杆的相对位姿需要6个参数，其中三个用于描述连杆的位置，三个用于描述连杆的旋转矩阵。虽然我们必须为每个连杆建立一个坐标系，但是坐标系的原点和坐标轴的方向是可以根据我们的喜好配置的。也就是说坐标原点$O_i$未必会在关节连接处，它可以在连杆上的任何一点，甚至是空间中的任何一点，只要保证坐标系与连杆固连在一起运动就可以。尽管不同人建立坐标系的方式不同，但得到的执行末端的齐次矩阵总是一样的。在分析复杂结构时，通过引入一些约束合理地建立坐标系有可能减少描述连杆位姿的参数，使问题得到简化。 Jacques Denavit和Richard Hartenberg于1955年提出的D-H坐标(D-H parameter)是目前比较常用的一种方法。

D-H坐标(D-H parameter)引入了如下的两个约束，允许我们只用四个参数来描述一个连杆的相对位置和姿态。

坐标轴$x_i$与坐标轴$z_{i-1}$垂直
坐标轴$x_i$与坐标轴$z_{i-1}$相交


图2(1) 满足D-H坐标约束的两个坐标系	图2(2) D-H坐标示例

图2(1)中展示了满足D-H坐标约束的两个坐标系，从图中我们可看出只需要$a, \alpha, d, \theta$四个参数就可以完整的描述$O_i$系相对于$O_{i-1}$系的位置和姿态。这四个参数通常被称为link length, link twist, link offset, joint angle。通常在做运动学分析时，我们会认为每个关节都只有一个自由度，因此这四个参数中有三个都是常量。对于转动关节只有$\theta$是其中的变量，而平动关节则是$d$。在D-H坐标的约束下，每个构件的齐次变换矩阵$A_i$都可以写成这四个参数构成的基本变换矩阵的积： $$ \begin{align} \label{f5} A_i & = Rot_{z,\theta_i}Trans_{z, d_i}Trans_{x, a_i}Rot_{x, \alpha_i} \\ & = \begin{bmatrix} \cos{\theta_i} & -\sin{\theta_i} & 0 & 0 \\ \sin{\theta_i} & cos{\theta_i} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & a_i \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos{\alpha_i} & -\sin{\alpha_i} & 0 \\ 0 & \sin{\alpha_i} & \cos{\alpha_i} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ & = \begin{bmatrix} \cos{\theta_i} & -\sin{\theta_i}\cos{\alpha_i} & \sin{\theta_i}\sin{\alpha_i} & a_i \cos{\theta_i} \\ \sin{\theta_i} & \cos{\theta_i}\cos{\alpha_i} & -\cos{\theta_i}\sin{\alpha_i} & a_i \sin{\theta_i} \\ 0 & \sin{\alpha_i} & \cos{\alpha_i} & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{align} $$

对于一个$n$关节的机械臂，我们总能找到一组$n+1$个坐标系满足上述的约束。需要说明的是，满足上述约束的坐标系组一般不止一个，工程师们可以根据自己的喜好进行设计，解决问题才是关键。下面介绍使用D-H坐标的方法和一些原则。

首先，从$z_i$轴的选择开始。$z_i$轴的方向可以是任意选择的，因为通过合理地调整参数$\alpha$和$\theta$，总能够满足$z_i$轴的选择。一般，我们根据关节$i+1$的运动形式设置$z_i$轴。比如，基座的坐标系$z_0$轴参考关节1，$z_1$轴参考关节2，以此类推。在设置$z_i$轴时有两种情况需要考虑：(i)如果关节$i+1$是转动关节，则选其转轴方向作为$z_i$轴方向；(ii)如果关节$i+1$是平动关节，则选择平动方向。参考关节$i+1$来设置$z_i$轴可能有点奇怪，回顾刚刚运动链的介绍会发现，这样做正好可以保证连杆$i$与坐标系$O_i$固连在一起运动。

然后，确定基座的坐标系$O_0$。其原点可以选定为$z_0$轴上任意一点，而$x_0$和$y_0$轴的方向也可以随意指定只要满足右手定则就可以了。

最后，就需要从关节1开始根据$O_{i-1}$系确定$O_i$系，直至确定完所有坐标系，这个过程可以参考图2(2)。根据$z_i$轴与$z_{i-1}$轴之间的关系，有三种情况需要考虑：

$z_i$轴与$z_{i-1}$轴不共面：在这种情况下，空间中只有一条直线垂直于两个轴。因此该直线就是确定了$x_i$轴，而该直线与$z_i$的交点就是原点$O_i$。至于$x_i$的朝向和$y_i$的确定只要满足右手定则即可。
$z_i$轴与$z_{i-1}$轴相交：该情形下，一般选择$x_i$轴垂直于$z_i$与$z_{i-1}$轴确定的平面，原点则为$z_i$轴与$z_{i-1}$轴的交点。此时，有D-H参数$a_i=0$
$z_i$轴与$z_{i-1}$轴平行：此时，空间中存在无数多条直线垂直于两个轴，我们就有了选择原点$O_i$的自由。通常会选择$x_i$轴经过$O_{i-1}$，原点$O_i$则是$x_i$与$z_i$的交点，那么我们就有D-H参数$d_i=0$。又由于两轴平行，有D-H参数$\alpha=0$。

经过上述的三个步骤，就可以为给定的一个运动链建立坐标系，而且可以使得正运动学分析过程得到简化。

3. 举个例子

这节我们以 PUMA(Programmable Universal Manipulation Arm)机械臂为例详细介绍D-H坐标在正运动学分析中的应用。图3(1)中显示了经典的PUMA-260机械臂，这是一款6自由度的机器人，如图3(2)所示每个关节都是旋转关节。


图3(1) PUMA-260机械臂	图3(2) PUMA-260结构

首先，我们依据每个关节的转轴确定各个连杆坐标系的$z$轴，然后从基座开始建立各个连杆坐标系，如图4所示。在图中并没有画出各个坐标系的$y$轴，它们可以根据右手定则确定。在图4中还列出了各个坐标系的4个参数，其中$\theta_i^*$为4个参数中的变量，描述了各个关节的转角。

图4 PUMA-260的D-H参数

由公式($\ref{f5}$)知各个关节的变换矩阵$A_i\left(i=1,2,...,6\right)$可以写成如下： \begin{matrix} A_1 = \begin{bmatrix} \cos{\theta_1} & 0 & -\sin{\theta_1} & 0 \\ \sin{\theta_1} & 0 & \cos{\theta_1} & 0 \\ 0 & -1 & 0 & d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix} & A_2 = \begin{bmatrix} \cos{\theta_2} & -\sin{\theta_2} & 0 & a_2\cos{\theta_2} \\ \sin{\theta_2} & \cos{\theta_2} & 0 & a_2\sin{\theta_2} \\ 0 & 0 & 1 & d_2 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ A_3 = \begin{bmatrix} \cos{\theta_3} & 0 & -\sin{\theta_3} & 0 \\ \sin{\theta_3} & 0 & \cos{\theta_3} & 0 \\ 0 & -1 & 0 & d_3 \\ 0 & 0 & 0 & 1 \end{bmatrix} & A_4 = \begin{bmatrix} \cos{\theta_4} & 0 & -\sin{\theta_4} & 0 \\ \sin{\theta_4} & 0 & \cos{\theta_4} & 0 \\ 0 & -1 & 0 & d_4 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ A_5 = \begin{bmatrix} \cos{\theta_5} & 0 & -\sin{\theta_5} & 0 \\ \sin{\theta_5} & 0 & \cos{\theta_5} & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} & A_6 = \begin{bmatrix} \cos{\theta_6} & -\sin{\theta_6} & 0 & 0 \\ \sin{\theta_6} & \cos{\theta_6} & 0 & 0 \\ 0 & 0 & 1 & d_6 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \end{matrix} 由公式($\ref{f4}$)，末端执行机构的位姿的齐次变换矩阵为： $$ H = A_n^0 = A_1A_2...A_n $$

4. 总结

对于机械臂这样的开链机构，我们从基座开始为每一个连杆(link)建立一个坐标系，该坐标系跟随连杆一起运动。然后依次计算相邻两个杆件之间的齐次变换矩阵，得到相对的位置和姿态描述，最后把各个齐次变换矩阵相乘就得到了执行末端的位置和姿态。 D-H坐标引入了两个约束，即某一杆件随体坐标系的$x$轴应当与前一杆件的$z$轴垂直且相交。在这两个约束下，我们可以只用4个参数来描述杆件之间的相对位置和姿态，简化了正运动学分析的过程。 D-H坐标和提供了进行运动学分析的基本流程，先根据关节的形式确定各个连杆的$z$轴，然后确定基座的坐标系，最后依次确定满足D-H坐标约束的$x$轴及坐标原点。这一流程为我们系统化地分析提供了参考。