五自由度机械臂的建模 (2) —— DH 建模法
DH 建模法
DH 建模方法是由 Denavit 和 Hartenberg 提出的一种建模方法，主要用在机器人运动学上。此方法在机器人的每个连杆上建立一个坐标系，通过齐次坐标变换实现两个连杆上的坐标变换，建立多连杆串联系统中首末坐标系的变换关系。
DH 建模方法的几个要点如下：
- 建立连杆坐标系
- 确定四个参数 \(\alpha\)、\(a\)、\(d\)、\(\theta\)
- 列 DH 参数表
- 由参数表得到变换矩阵
DH 建模方法中，每个连杆使用 4 个参数 \(\alpha\)、\(a\)、\(d\) 和 \(\theta\) 来描述，2 个描述连杆本身，另外 2 个描述与相邻连杆的位姿（连接或几何关系）。对于转动关节，其中 \(\theta\) 为关节变量，其他三个参数固定不变，为连杆参数；对于移动关节，\(d\) 为关节变量，其他三个为关节参数。
根据连杆坐标系和关节对应关系的不同，DH 建模法可以分为传统 DH (Classic DH) 和改进 DH (Modified DH)，二者的主要区别如下表所示。
| 区别 | Classic DH | Modified DH |
|---|---|---|
| 连杆固定坐标系的位置 | 后一个关节坐标系 | 前一个关节坐标系 |
| \(X\) 轴的确定方式 | 当前坐标系 \(Z\) 轴和前一个坐标系 \(Z\) 轴的向量积 | 后一个坐标系 \(Z\) 轴与当前坐标系 \(Z\) 轴的向量积 |
| 坐标系间的参数变换顺序 | \(\theta\)、\(d\)、\(a\)、\(\alpha\) | \(\alpha\)、\(a\)、\(\theta\)、\(d\) |
Classic DH
Classic DH 的关节和坐标系关系如图所示，其中各个参数的含义如下：
- \(\theta_{i}\)：\(X_{i-1}\) 到 \(X_{i}\) 绕 \(Z_{i-1}\) 旋转的角度
- \(d_{i}\)：\(X_{i-1}\) 到 \(X_{i}\) 沿 \(Z_{i-1}\) 方向的距离
- \(a_{i}\)：\(Z_{i-1}\) 到 \(Z_{i}\) 沿 \(X_{i-1}\) 方向的距离
- \(\alpha_{i}\)：\(Z_{i-1}\) 到 \(Z_{i}\) 绕 \(X_{i-1}\) 旋转的角度
坐标系 \(O_{i-1}\) 与关节 \(i\) 对齐，其 DH 参数矩阵为
\[ _{i}^{i-1}T = \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} \]
Modified DH
Modified DH 的关节和坐标系关系如图所示，其中各个参数的含义如下：
- \(\alpha_{i-1}\)：\(Z_{i-1}\) 到 \(Z_{i}\) 绕 \(X_{i-1}\) 旋转的角度
- \(a_{i-1}\)：\(Z_{i-1}\) 到 \(Z_{i}\) 沿 \(X_{i-1}\) 方向的距离
- \(\theta_{i}\)：\(X_{i-1}\) 到 \(X_{i}\) 绕 \(Z_{i}\) 旋转的角度
- \(d_{i}\)：\(X_{i-1}\) 到 \(X_{i}\) 沿 \(Z_{i}\) 方向的距离
坐标系 \(O_{i-1}\) 与关节 \(i-1\) 对齐，其 DH 参数矩阵为
\[ _{i}^{i-1}T = \begin{bmatrix} \cos{\theta_{i}} & -\sin{\theta_{i}} & 0 & a_{i-1} \\ \sin{\theta_{i}} \cos{\alpha_{i-1}} & \cos{\theta_{i}} \cos{\alpha_{i-1}} & -\sin{\alpha_{i-1}} & -d_{i} \sin{\alpha_{i-1}} \\ \sin{\theta_{i}} \sin{\alpha_{i-1}} & \cos{\theta_{i}} \sin{\alpha_{i-1}} & \cos{\alpha_{i-1}} & d_{i} \cos{\alpha_{i-1}} \\ 0 & 0 & 0 & 1 \end{bmatrix} \]
Modified DH 克服了 Classic DH 在用于树型结构机器人时可能出现的问题，比较常用，故之后主要介绍这种方法，并使用该方法进行建模。
机械臂连杆坐标系的建立
建立机械臂连杆坐标系的步骤：
- 确定各个关节轴和连杆，坐标系的 \(Z\) 轴沿关节轴线方向
- 找出相邻两关节轴线的交点或公垂线，用于确定坐标系 \(\{i\}\) 的原点：以关节轴 \(i\) 和 \(i+1\) 的交点或公垂线与关节轴 \(i\) 的交点为原点
- 确定 \(X\) 轴：两轴线相交时，\(\vec{X_{i}} = \pm \vec{Z_{i+1}} \times \vec{Z_{i}}\)；两轴线不相交时，\(X_{i}\) 轴与公垂线重合，方向为 \(i\) 到 \(i+1\)
- 右手定则确定 \(Y_{i}\) 轴
- 确定基坐标系 \(\{0\}\)：为了简化问题，\(Z_0\) 通常与关节 1 的轴线方向重合，且当关节变量 1 为 0 时，坐标系 \(\{0\}\) 与 \(\{1\}\) 重合
- 确定末端坐标系 \(\{n\}\)：对于转动关节，\(\theta_n = 0\) 时，\(X_n\) 与 \(X_{n-1}\) 方向相同，选取原点使 \(d_n = 0\)；对于移动关节，取 \(X_n\) 方向使 \(\theta_n = 0\)，当 \(d_n = 0\) 时，取 \(X_{n-1}\) 与 \(X_n\) 的交点为原点
这里我使用的 5 自由度机械臂的实物如上图所示，根据如上所述的步骤建立坐标系，结果如下图所示。
DH 参数表
根据机械臂各个连杆间坐标系的关系，采用 Modified DH 形式，得到的参数表如下。
| \(i\) | \(\alpha_{i-1}\) | \(a_{i-1}\) | \(\theta_{i-1}\) | \(d_{i}\) | \(\theta\) 的范围 |
|---|---|---|---|---|---|
| 1 | \(0^{\circ}\) | \(0\) | \(\theta_{1}\) | \(0\) | \((-\frac{2 \pi}{3}, \frac{2 \pi}{3})\) |
| 2 | \(-90^{\circ}\) | \(a_{1}\) | \(\theta_{2}\) | \(0\) | \((-\frac{\pi}{2}, 0)\) |
| 3 | \(0^{\circ}\) | \(a_{2}\) | \(\theta_{3}\) | \(0\) | \((-\frac{2 \pi}{3}, \frac{2 \pi}{3})\) |
| 4 | \(0^{\circ}\) | \(a_{3}\) | \(\theta_{4}\) | \(0\) | \((-\frac{7 \pi}{6}, \frac{\pi}{6})\) |
| 5 | \(-90^{\circ}\) | \(0\) | \(\theta_{5}\) | \(0\) | \((-\frac{2 \pi}{3}, \frac{2 \pi}{3})\) |
其中，\(a_{1} = 3\ mm\)，\(a_{2} = 96\ mm\)，\(a_{3} = 96\ mm\)，\(\theta_{1}\)～\(\theta_{5}\) 为关节 1 ～ 5 的旋转角度。初始时刻，\(\theta_{1} = 0\)，\(\theta_{2} = 0\)，\(\theta_{3} = 0\)，\(\theta_{4} = -\frac{pi}{2}\)，\(\theta_{5} = 0\)。
齐次变换矩阵
将 DH 参数表代入 Modified DH 的 DH 参数矩阵，可以得到各个坐标系间的齐次变换矩阵 \(_{1}^{0}T\)、\(_{2}^{1}T\)、\(_{3}^{2}T\)、\(_{4}^{3}T\) 和 \(_{5}^{4}T\)，则可得基坐标系到末端坐标系的齐次变换矩阵
\[ _{5}^{0}T = {_{1}^{0}T} {_{2}^{1}T} {_{3}^{2}T} {_{4}^{3}T} {_{5}^{4}T} = \begin{bmatrix} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{bmatrix} \]
其中，\(\begin{bmatrix} p_{x} & p_{y} & p_{z} \end{bmatrix}^T\) 为机械臂末端在基坐标系中的位置，\(\begin{bmatrix} n_{x} & n_{y} & n_{z} \end{bmatrix}^T\) 为机械臂末端坐标系 \(X\) 轴在基坐标系中的方向矢量，\(\begin{bmatrix} o_{x} & o_{y} & o_{z} \end{bmatrix}^T\) 为机械臂末端坐标系 \(Y\) 轴在基坐标系中的方向矢量，\(\begin{bmatrix} a_{x} & a_{y} & a_{z} \end{bmatrix}^T\) 为机械臂末端坐标系 \(Z\) 轴在基坐标系中的方向矢量。
代入 DH 参数，可得
\[ \begin{aligned} n_{x} = & \ s_{1} s_{5} + c_{1} c_{2} c_{3} c_{4} c_{5} - c_{1} c_{2} c_{5} s_{3} s_{4} - c_{1} c_{3} c_{5} s_{2} s_{4} - c_{1} c_{4} c_{5} s_{2} s_{3} \\ n_{y} = & \ c_{2} c_{3} c_{4} c_{5} s_{1} - c_{1} s_{5} - c_{2} c_{5} s_{1} s_{3} s_{4} - c_{3} c_{5} s_{1} s_{2} s_{4} - c_{4} c_{5} s_{1} s_{2} s_{3} \\ n_{z} = & \ c_{5} s_{2} s_{3} s_{4} - c_{2} c_{4} c_{5} s_{3} - c_{3} c_{4} c_{5} s_{2} - c_{2} c_{3} c_{5} s_{4} \\ o_{x} = & \ c_{5} s_{1} - c_{1} c_{2} c_{3} c_{4} s_{5} + c_{1} c_{2} s_{3} s_{4} s_{5} + c_{1} c_{3} s_{2} s_{4} s_{5} + c_{1} c_{4} s_{2} s_{3} s_{5} \\ o_{y} = & \ c_{2} s_{1} s_{3} s_{4} s_{5} - c_{2} c_{3} c_{4} s_{1} s_{5} - c_{1} c_{5} + c_{3} s_{1} s_{2} s_{4} s_{5} + c_{4} s_{1} s_{2} s_{3} s_{5} \\ o_{z} = & \ c_{2} c_{3} s_{4} s_{5} + c_{2} c_{4} s_{3} s_{5} + c_{3} c_{4} s_{2} s_{5} - s_{2} s_{3} s_{4} s_{5} \\ a_{x} = & \ c_{1} s_{2} s_{3} s_{4} - c_{1} c_{2} c_{4} s_{3} - c_{1} c_{3} c_{4} s_{2} - c_{1} c_{2} c_{3} s_{4} \\ a_{y} = & \ s_{1} s_{2} s_{3} s_{4} - c_{2} c_{4} s_{1} s_{3} - c_{3} c_{4} s_{1} s_{2} - c_{2} c_{3} s_{1} s_{4} \\ a_{z} = & \ c_{2} s_{3} s_{4} + c_{3} s_{2} s_{4} + c_{4} s_{2} s_{3} - c_{2} c_{3} c_{4} \\ p_{x} = & \ a_{1} c_{1} + a_{2} c_{1} c_{2} + a_{3} c_{1} c_{2} c_{3} - a_{3} c_{1} s_{2} s_{3} \\ p_{y} = & \ a_{1} s_{1} + a_{2} c_{2} s_{1} + a_{3} c_{2} c_{3} s_{1} - a_{3} s_{1} s_{2} s_{3} \\ p_{z} = & \ -a_{2} s_{2} - a_{3} c_{2} s_{3} - a_{3} c_{3} s_{2} \end{aligned} \]
其中，\(s_{i} = \sin{\theta_{i}}\)，\(c_{i} = \cos{\theta_{i}}\)。