前言

笔者在看D-H参数法的时候对此方法的适用性产生了好奇，通过个人构思推导得出传统D-H参数表有时不适用的原因，并相出一种简洁的判断方法，列在文章最末，原理十分简单，欢迎指点。

本人对手边的一只五自由度旋转舵机机械手进行了坐标系的建模。为了方便理解画了一个示意图，图省事所以画成了圆柱形，各个旋转关节不影响理解即可。

机械臂在底座上有四节连杆。第一节为相对于底座转动的连杆。第二节相对于第一节的连轴处旋转。第三节相对于第二节的连轴处旋转，第四节相对于第三节的连轴处旋转。第四节末端为机械手工作部件，抓取夹，绕第四节末端的轴转动，由于不关心夹子的开合角度，便简化为一个示意的形状。其中第四节的转轴距第四节形心轴向后偏了距离为h。

为了看得更清楚，在建模软件中调整一下角度，可看出机械臂大致的运动姿态。

下面进入正题，第一部分介绍推算D-H参数与坐标轴方向的方法；第二部分介绍一下根据D-H参数得到的坐标变换矩阵；第三部分为我在建模时遇到的问题和自己的分析；第四部分为我总结的步骤。

连杆D-H参数法介绍

建立连杆坐标系的基本原则如图所示。规定连杆 $i$ 的坐标系为 $i$ ，坐标系 $i$ 的 $z_i$ 轴与关节轴 $i$ 重合，坐标系的原点位于连杆长度 $a_i$ 与关节轴线 $i$ 的交点处，坐标系 $i$ 的 $x_i$ 轴沿着 $a_i$ 由轴 $i$ 指向轴 $i + 1$ ，坐标系 $i$ 的 $y_i$ 由右手定则确定。

如图所示，表示了机器人的两个连杆 $i - 1$ 、 $i$ ，图中的四个参数 $\alpha_{i-1}$ 、 $a_{i-1}$ 、 $d_i$ 、 $\theta_i$ 为连杆 $i$ 的D-H参数。

根据蔡自兴老师的《机器人学》第三版第42页的Craig法则：可得出四个参数的计算方法。

$a_{i-1}$ =沿 $x_{i-1}$ 轴，从 $z_{i-1}$ 移动到 $z_i$ 的距离。
$\alpha_{i-1}$ =绕 $x_{i-1}$ 轴，从 $z_{i-1}$ 旋转到 $z_i$ 的转角。
$d_i$ =沿 $z_i$ 轴，从 $x_{i-1}$ 移动到 $x_i$ 的距离。
$\theta_i$ =沿 $z_i$ 轴，从 $x_{i-1}$ 旋转到 $x_i$ 的转角。

D-H参数法的坐标变换矩阵

建立相邻连杆的坐标系变换的一般形式，然后将这些变换联系起来，就可以求出最后一个连杆相对于基坐标系的位置和姿态，即运动学方程的矩阵表示，如图所示。连杆变换即坐标系 $i - 1$ 向坐标系 $i$ 的变换。变换过程可以看成是坐标系 $i - 1$ 经过4次子变换得到了坐标系 $i$ ，建立3个中间坐标系 $R$ 、 $Q$ 、 $P$ 。坐标系 $i - 1$ 先绕坐标轴 $x_{i-1}$ 旋转 $\alpha_{i-1}$ 得到坐标系 $R$ ；坐标系 $R$ 沿着轴线 $i - 1$ 和轴线 $i$ 的公垂线方向移动 $a_{i-1}$ ，得到坐标系 $Q$ ；坐标系 $Q$ 绕坐标轴 $z_Q$ 旋转 $\theta_i$ ，得到坐标系 $P$ ；坐标系 $P$ 沿着坐标轴 $z_P$ 方向移动 $d_i$ ，得到坐标系 $i$ 。

由坐标系变换法则可知，这四个变换可以用四个齐次变换矩阵表示，分别是 $_{R}^{i-1} T$ 、 $_{Q}^R T$ 、 $_{P}^Q T$ 、 $_{i}^P T$ ，根据坐标变换的链式法则，坐标系 $i - 1$ 到坐标系 $i$ 的变换矩阵可以写成
$_{i-1}^i T=_{R}^{i-1} T_{Q}^R T_{P}^Q T_{i}^P T =Rot(x,\alpha_{i-1})Trans(x,a_{i-1})Rot(z,\theta_{i})Trans(z,d_i) =\begin{bmatrix} cos\theta_i&-sin\theta_i&0&a_{i-1}\\ sin\theta_icos\alpha_{i-1}&cos\theta_icos\alpha_{i-1}&-sin\alpha_{i-1}&-d_isin\alpha_{i-1}\\ sin\theta_isin\alpha_{i-1}&cos\theta_isin\alpha_{i-1}&cos\alpha_{i-1}&d_icos\alpha_{i-1}\\ 0&0&0&1 \end{bmatrix}$

其中， $Rot(x,\alpha_{i-1})$ 表示坐标系绕 $x$ 轴旋转 $\alpha_{i-1}$ ， $Trans(x,a_{i-1})$ 表示沿着 $x$ 轴平移 $a_{i-1}$ 。

在得到每个连杆变换后，通过链式法则就可以得到执行机构相对于基坐标系的位姿，即机械臂的运动学方程，可由齐次矩阵 $_0^5T$ 表示。
$_5^0T=_1^0T _2^1T _3^2T _4^3T _5^4T=\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}$

特殊情况加实例分析

上述方法中，转轴仅绕参考坐标系即上一个关节的 $x$ 轴与 $z$ 轴旋转，移动。其中绕 $z$ 轴的转角为关节舵机的主动转角，若舵机转角为0， $\theta$ 为0，即仅绕 $x$ 轴产生转动，转动后的新 $x$ 轴应当与原 $x$ 轴平行。

从图中可以看出，这是一个五自由度的机械臂，所有关节均为转动关节。最底下为底座，将其圆心视为全局坐标系的 $O$ 点，竖直向上方向为全局坐标系的 $z$ 正方向，向前的方向为 $x$ 轴正方向。底座的高度为 $l_0$ 。

在底座上有四节连杆。第一节为相对于底座转动的连杆，长度为 $l_1$ 。第二节相对于第一节的连轴处旋转，长度为 $l_2$ 。第三节相对于第二节的连轴处旋转，长度为 $l_3$ ，第四节相对于第三节的连轴处旋转，长度为 $l_4$ 。第四节末端为机械手工作部件，抓取夹，绕第四节末端的轴转动，其中此转轴距第四节形心轴向后偏了距离为 $h$ ，在此图中状态看为向全局坐标系x轴负方向偏了 $h$ 。

根据上述规则，将图中机械臂各关节坐标系一一标出。并得到D-H参数表。

连杆	关节角 $\theta_i/°$	连杆偏移量 $d_i/m$	扭转角 $\alpha_{i-1}/°$	连杆长度 $a_{i-1}/m$
1	0	$l_0$	0	0
2	0	$l_1$	$- 90 °$	0
3	-90°	0	0	$l_2$
4	0	0	0	$l_3$
5	-90°	0	$- 90 °$	0

其中前四个关节的D-H参数都依照规则较为简单地求出，但是第五关节比较奇怪。按照描述 $a_{i-1}$ 为沿 $x_{i-1}$ 轴，从 $z_{i-1}$ 移动到 $z_i$ 的距离。 $d_i$ 为沿 $z_i$ 轴，从 $x_{i-1}$ 移动到 $x_{i}$ 的距离。

从 $z_4$ 移动到 $z_5$ 的距离为h，但是在 $x_4$ 上的分量为0； $x_4$ 与 $x_5$ 相交，故 $d$ 也为 $0$ 。
但这很明显是不可能的，此关节的两个空间尺度 $l_4$ 与 $h$ 都没有用上，那这个关节岂不是变成质点了吗？
于是我尝试了三种思路得到参数
(1)不管轴方向的分量，强行将此二值代入，认为:

连杆	关节角 $\theta_i/°$	连杆偏移量 $d_i/m$	扭转角 $\alpha_{i-1}/°$	连杆长度 $a_{i-1}/m$
5	$\theta_5$	$l_4$	$- 90 °$	$h$

并得到变换矩阵 $_5^4T$
$_5^4T=\begin{bmatrix} cos\theta_5&-sin\theta_5&0&h\\ 0&0&1&l_4\\ -sin\theta_5&-cos\theta_5&0&0\\ 0&0&0&1 \end{bmatrix}$

按照机械臂的初始状态， $\theta_5=-90°$ ，代入得
$_5^4T=\begin{bmatrix}0&1&0&h\\0&0&1&l_4\\1&0&0&0\\0&0&0&1\end{bmatrix}$ ，明显不对

(2)考虑到坐标系 $5$ 的原点相对于坐标系 $4$ 在 $x$ 方向位移为 $l_4$ ， $y$ 方向为 $- h$ ，猜测为

连杆	关节角 $\theta_i/°$	连杆偏移量 $d_i/m$	扭转角 $\alpha_{i-1}/°$	连杆长度 $a_{i-1}/m$
5	$\theta_5$	-h	-90°	$l_4$

可得坐标变换矩阵 $_5^4T=\begin{bmatrix}0&1&0&l_4\\0&0&1&-h\\1&0&0&0\\0&0&0&1\end{bmatrix}$ ，坐标系 $5$ 的原点代入求该点在坐标系 $4$ 对应的坐标： $\begin{bmatrix}0&1&0&l_4\\0&0&1&-h\\1&0&0&0\\0&0&0&1\end{bmatrix}\begin{bmatrix}0\\0\\0\\1\end{bmatrix}=\begin{bmatrix}l_4\\-h\\0\\1\end{bmatrix}$ ，这次好像对了，再将点(1,2,3)代入试试 $\begin{bmatrix}0&1&0&l_4\\0&0&1&-h\\1&0&0&0\\0&0&0&1\end{bmatrix}\begin{bmatrix}1\\2\\3\\1\end{bmatrix}=\begin{bmatrix}2+l_4\\3-h\\1\\1\end{bmatrix}$ ，可观察出在坐标系4中应当为 $3+l_4,-1-h,-2)$ ，所以此矩阵错误。

(3) 这是为什么呢，我观察了下，现在它是 $- 90 ° ，$ 我把它转回去看看。即绕 $z_5$ 轴正转 $90$ 度。

可以看出 $x_5$ 与 $x_4$ 并非平行，相反地， $y_5$ 与 $y_4$ 是平行的。说明坐标系 $5$ 为绕 $y$ 轴转动了 $90$ 度，然后在 $x$ 与 $y$ 方向产生了位移。这个变换矩阵应当自行计算：
$_5^4T=Trans(y,-h)Trans(x,l_4)Rot(y,90°)Rot(z,\theta_5) =\begin{bmatrix}1&0&0&l_4\\0&1&0&-h\\0&0&1&0\\0&0&0&1\end{bmatrix}\begin{bmatrix}0&0&1&0\\0&1&0&0\\-1&0&0&0\\0&0&0&1\end{bmatrix}\begin{bmatrix}cos\theta_5&-sin\theta_5&0&0\\sin\theta_5&cos\theta_5&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}$

$=\begin{bmatrix}0&0&1&l_4\\sin\theta_5&cos\theta_5&0&-h\\-cos\theta_5&sin\theta_5&0&0\\0&0&0&1\end{bmatrix}$

将 $\theta=-90°$ 代入得 $_5^4T=\begin{bmatrix}0&0&1&l_4\\-1&0&0&-h\\0&-1&0&0\\0&0&0&1\end{bmatrix}$ ，再将点 $(1, 2, 3)$ 代入试试 $\begin{bmatrix}0&0&1&l_4\\-1&0&0&-h\\0&-1&0&0\\0&0&0&1\end{bmatrix}\begin{bmatrix}1\\2\\3\\1\end{bmatrix}=\begin{bmatrix}3+l_4\\-1-h\\-2\\1\end{bmatrix}$ ，正确。

反复验证，此种方法得到的矩阵应该不存在问题。

方法总结

于是就出现了问题，难道传统 $D - H$ 参数法是错的？那肯定不是，之前的学者们肯定遇到了各种特殊情况，并且给出了解决方法，但我觉得书上描述的针对一般性问题的描述过于复杂，在此，我想出了一种比较快速的方法。

根据前面的描述，若舵机转角为 $0$ ， $\theta$ 为 $0$ ，即仅绕 $x$ 轴产生转动，转动后的新 $x$ 轴应当与原 $x$ 轴平行。

故，看到机械臂关节后的建立坐标转换矩阵的步骤为：

（1）根据 $C r a i g$ 规则，确定各原点以及 $x_i,y_i,z_i$ 的方向。并根据规则算出此状态时的角度 $\theta_{i0}$ 。
（2）将关节 $i$ 沿转轴 $z_i$ 旋转 $-\theta_{i0}$ ，得到关节 $i$ 相对于上一个转角为 $0$ 时的坐标系，观察此时的 $x$ 轴是否与上一个关节的 $x$ 轴平行，若平行，则满足前述规则，直接求出 $D - H$ 参数，并代入矩阵

$\begin{bmatrix}cos\theta_i&-sin\theta_i&0&a_{i-1}\\ sin\theta_icos\alpha_{i-1}&cos\theta_icos\alpha_{i-1}&-sin\alpha_{i-1}&-d_isin\alpha_{i-1}\\ sin\theta_isin\alpha_{i-1}&cos\theta_isin\alpha_{i-1}&cos\alpha_{i-1}&d_icos\alpha_{i-1}\\ 0&0&0&1\\ \end{bmatrix}$

(3)若此时的 $x$ 轴与上一个关节的 $x$ 轴不平行，则必定有绕 $y$ 轴的转动，此时需要观察，另行推算出坐标变换矩阵。一般优先考虑转角，后考虑位移。一般情况下机械臂不会有很奇怪的角度，通过观察可得到沿 $x$ 与 $y$ 轴的转角。若同时具有 $x$ 与 $y$ 轴的转角并且角度不是 $90$ 的倍数，也可根据坐标轴之间的夹角求出转角。