机械臂D-H参数法分析
文章目录
- 前言
- 连杆D-H参数法介绍
- D-H参数法的坐标变换矩阵
- 特殊情况加实例分析
- 方法总结
前言
笔者在看D-H参数法的时候对此方法的适用性产生了好奇，通过个人构思推导得出传统D-H参数表有时不适用的原因，并相出一种简洁的判断方法，列在文章最末，原理十分简单，欢迎指点。
本人对手边的一只五自由度旋转舵机机械手进行了坐标系的建模。为了方便理解画了一个示意图，图省事所以画成了圆柱形，各个旋转关节不影响理解即可。
机械臂在底座上有四节连杆。第一节为相对于底座转动的连杆。第二节相对于第一节的连轴处旋转。第三节相对于第二节的连轴处旋转，第四节相对于第三节的连轴处旋转。第四节末端为机械手工作部件，抓取夹，绕第四节末端的轴转动，由于不关心夹子的开合角度，便简化为一个示意的形状。其中第四节的转轴距第四节形心轴向后偏了距离为h。
为了看得更清楚，在建模软件中调整一下角度，可看出机械臂大致的运动姿态。
下面进入正题，第一部分介绍推算D-H参数与坐标轴方向的方法；第二部分介绍一下根据D-H参数得到的坐标变换矩阵；第三部分为我在建模时遇到的问题和自己的分析；第四部分为我总结的步骤。
连杆D-H参数法介绍
建立连杆坐标系的基本原则如图所示。规定连杆 i i i的坐标系为 i i i，坐标系 i i i的 z i z_i zi轴与关节轴 i i i重合，坐标系 的原点位于连杆长度 a i a_i ai与关节轴线 i i i的交点处，坐标系 i i i的 x i x_i xi轴沿着 a i a_i ai由轴 i i i指向轴 i + 1 i+1 i+1，坐标系 i i i的 y i y_i yi由右手定则确定。
如图所示，表示了机器人的两个连杆 i − 1 i-1 i−1、 i i i，图中的四个参数 α i − 1 \alpha_{i-1} αi−1、 a i − 1 a_{i-1} ai−1、 d i d_i di、 θ i \theta_i θi为连杆 i i i的D-H参数。
根据蔡自兴老师的《机器人学》第三版第42页的Craig法则：可得出四个参数的计算方法。
a i − 1 a_{i-1} ai−1=沿 x i − 1 x_{i-1} xi−1轴，从 z i − 1 z_{i-1} zi−1移动到 z i z_i zi的距离。
α i − 1 \alpha_{i-1} αi−1=绕 x i − 1 x_{i-1} xi−1轴，从 z i − 1 z_{i-1} zi−1旋转到 z i z_i zi的转角。
d i d_i di=沿 z i z_i zi轴，从 x i − 1 x_{i-1} xi−1移动到 x i x_i xi的距离。
θ i \theta_i θi=沿 z i z_i zi轴，从 x i − 1 x_{i-1} xi−1旋转到 x i x_i xi的转角。
D-H参数法的坐标变换矩阵
建立相邻连杆的坐标系变换的一般形式，然后将这些变换联系起来，就可以求出最后一个连杆相对于基坐标系的位置和姿态，即运动学方程的矩阵表示，如图所示。连杆变换即坐标系 i − 1 i-1 i−1向坐标系 i i i的变换。变换过程可以看成是坐标系 i − 1 i-1 i−1经过4次子变换得到了坐标系 i i i，建立3个中间坐标系 R R R、 Q Q Q、 P P P。坐标系 i − 1 i-1 i−1先绕坐标轴 x i − 1 x_{i-1} xi−1旋转 α i − 1 \alpha_{i-1} αi−1得到坐标系 R R R；坐标系 R R R沿着轴线 i − 1 i-1 i−1和轴线 i i i的公垂线方向移动 a i − 1 a_{i-1} ai−1，得到坐标系 Q Q Q；坐标系 Q Q Q绕坐标轴 z Q z_Q zQ旋转 θ i \theta_i θi，得到坐标系 P P P；坐标系 P P P沿着坐标轴 z P z_P zP方向移动 d i d_i di，得到坐标系 i i i。
由坐标系变换法则可知，这四个变换可以用四个齐次变换矩阵表示，分别是 R i − 1 T _{R}^{i-1} T Ri−1T、 Q R T _{Q}^R T QRT、 P Q T _{P}^Q T PQT、 i P T _{i}^P T iPT，根据坐标变换的链式法则，坐标系 i − 1 i-1 i−1到坐标系 i i i的变换矩阵可以写成
i − 1 i T = R i − 1 T Q R T P Q T i P T = R o t ( x , α i − 1 ) T r a n s ( x , a i − 1 ) R o t ( z , θ i ) T r a n s ( z , d i ) = [ c o s θ i − s i n θ i 0 a i − 1 s i n θ i c o s α i − 1 c o s θ i c o s α i − 1 − s i n α i − 1 − d i s i n α i − 1 s i n θ i s i n α i − 1 c o s θ i s i n α i − 1 c o s α i − 1 d i c o s α i − 1 0 0 0 1 ] _{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} i−1iT=Ri−1TQRTPQTiPT=Rot(x,αi−1)Trans(x,ai−1)Rot(z,θi)Trans(z,di)=⎣⎢⎢⎡cosθisinθicosαi−1sinθisinαi−10−sinθicosθicosαi−1cosθisinαi−100−sinαi−1cosαi−10ai−1−disinαi−1dicosαi−11⎦⎥⎥⎤
其中， R o t ( x , α i − 1 ) Rot(x,\alpha_{i-1}) Rot(x,αi−1)表示坐标系绕 x x x轴旋转 α i − 1 \alpha_{i-1} αi−1， T r a n s ( x , a i − 1 ) Trans(x,a_{i-1}) Trans(x,ai−1)表示沿着 x x x轴平移 a i − 1 a_{i-1} ai−1。
在得到每个连杆变换后，通过链式法则就可以得到执行机构相对于基坐标系的位姿，即机械臂的运动学方程，可由齐次矩阵 0 5 T _0^5T 05T表示。
5 0 T = 1 0 T 2 1 T 3 2 T 4 3 T 5 4 T = [ 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 ] _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} 50T=10T21T32T43T54T=⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0PxPyPz1⎦⎥⎥⎤
特殊情况加实例分析
上述方法中，转轴仅绕参考坐标系即上一个关节的 x x x轴与 z z z轴旋转，移动。其中绕 z z z轴的转角为关节舵机的主动转角，若舵机转角为0， θ \theta θ为0，即仅绕 x x x轴产生转动，转动后的新 x x x轴应当与原 x x x轴平行。
从图中可以看出，这是一个五自由度的机械臂，所有关节均为转动关节。最底下为底座，将其圆心视为全局坐标系的 O O O点，竖直向上方向为全局坐标系的 z z z正方向，向前的方向为 x x x轴正方向。底座的高度为 l 0 l_0 l0。
在底座上有四节连杆。第一节为相对于底座转动的连杆，长度为 l 1 l_1 l1。第二节相对于第一节的连轴处旋转，长度为 l 2 l_2 l2。第三节相对于第二节的连轴处旋转，长度为 l 3 l_3 l3，第四节相对于第三节的连轴处旋转，长度为 l 4 l_4 l4。第四节末端为机械手工作部件，抓取夹，绕第四节末端的轴转动，其中此转轴距第四节形心轴向后偏了距离为 h h h，在此图中状态看为向全局坐标系x轴负方向偏了 h h h。
根据上述规则，将图中机械臂各关节坐标系一一标出。并得到D-H参数表。
| 连杆 | 关节角 θ i / ° \theta_i/° θi/° | 连杆偏移量 d i / m d_i/m di/m | 扭转角 α i − 1 / ° \alpha_{i-1}/° αi−1/° | 连杆长度 a i − 1 / m a_{i-1}/m ai−1/m |
|---|---|---|---|---|
| 1 | 0 | l 0 l_0 l0 | 0 | 0 |
| 2 | 0 | l 1 l_1 l1 | − 90 ° -90° −90° | 0 |
| 3 | -90° | 0 | 0 | l 2 l_2 l2 |
| 4 | 0 | 0 | 0 | l 3 l_3 l3 |
| 5 | -90° | 0 | − 90 ° -90° −90° | 0 |
其中前四个关节的D-H参数都依照规则较为简单地求出，但是第五关节比较奇怪。按照描述 a i − 1 a_{i-1} ai−1为沿 x i − 1 x_{i-1} xi−1轴，从 z i − 1 z_{i-1} zi−1移动到 z i z_i zi的距离。 d i d_i di为沿 z i z_i zi轴，从 x i − 1 x_{i-1} xi−1移动到 x i x_{i} xi的距离。
从 z 4 z_4 z4移动到 z 5 z_5 z5的距离为h，但是在 x 4 x_4 x4上的分量为0； x 4 x_4 x4与 x 5 x_5 x5相交，故 d d d也为 0 0 0。
但这很明显是不可能的，此关节的两个空间尺度 l 4 l_4 l4与 h h h都没有用上，那这个关节岂不是变成质点了吗？
于是我尝试了三种思路得到参数
(1)不管轴方向的分量，强行将此二值代入，认为:
| 连杆 | 关节角 θ i / ° \theta_i/° θi/° | 连杆偏移量 d i / m d_i/m di/m | 扭转角 α i − 1 / ° \alpha_{i-1}/° αi−1/° | 连杆长度 a i − 1 / m a_{i-1}/m ai−1/m |
|---|---|---|---|---|
| 5 | θ 5 \theta_5 θ5 | l 4 l_4 l4 | − 90 ° -90° −90° | h h h |
并得到变换矩阵 5 4 T _5^4T 54T
5 4 T = [ c o s θ 5 − s i n θ 5 0 h 0 0 1 l 4 − s i n θ 5 − c o s θ 5 0 0 0 0 0 1 ] _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} 54T=⎣⎢⎢⎡cosθ50−sinθ50−sinθ50−cosθ500100hl401⎦⎥⎥⎤
按照机械臂的初始状态， θ 5 = − 90 ° \theta_5=-90° θ5=−90°，代入得
5 4 T = [ 0 1 0 h 0 0 1 l 4 1 0 0 0 0 0 0 1 ] _5^4T=\begin{bmatrix}0&1&0&h\\0&0&1&l_4\\1&0&0&0\\0&0&0&1\end{bmatrix} 54T=⎣⎢⎢⎡001010000100hl401⎦⎥⎥⎤，明显不对
(2)考虑到坐标系 5 5 5的原点相对于坐标系 4 4 4在 x x x方向位移为 l 4 l_4 l4， y y y方向为 − h -h −h，猜测为
| 连杆 | 关节角 θ i / ° \theta_i/° θi/° | 连杆偏移量 d i / m d_i/m di/m | 扭转角 α i − 1 / ° \alpha_{i-1}/° αi−1/° | 连杆长度 a i − 1 / m a_{i-1}/m ai−1/m |
|---|---|---|---|---|
| 5 | θ 5 \theta_5 θ5 | -h | -90° | l 4 l_4 l4 |
可得坐标变换矩阵 5 4 T = [ 0 1 0 l 4 0 0 1 − h 1 0 0 0 0 0 0 1 ] _5^4T=\begin{bmatrix}0&1&0&l_4\\0&0&1&-h\\1&0&0&0\\0&0&0&1\end{bmatrix} 54T=⎣⎢⎢⎡001010000100l4−h01⎦⎥⎥⎤，坐标系 5 5 5的原点代入求该点在坐标系 4 4 4对应的坐标： [ 0 1 0 l 4 0 0 1 − h 1 0 0 0 0 0 0 1 ] [ 0 0 0 1 ] = [ l 4 − h 0 1 ] \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} ⎣⎢⎢⎡001010000100l4−h01⎦⎥⎥⎤⎣⎢⎢⎡0001⎦⎥⎥⎤=⎣⎢⎢⎡l4−h01⎦⎥⎥⎤，这次好像对了，再将点(1,2,3)代入试试 [ 0 1 0 l 4 0 0 1 − h 1 0 0 0 0 0 0 1 ] [ 1 2 3 1 ] = [ 2 + l 4 3 − h 1 1 ] \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} ⎣⎢⎢⎡001010000100l4−h01⎦⎥⎥⎤⎣⎢⎢⎡1231⎦⎥⎥⎤=⎣⎢⎢⎡2+l43−h11⎦⎥⎥⎤，可观察出在坐标系4中应当为 ( 3 + l 4 , − 1 − h , − 2 ) (3+l_4,-1-h,-2) (3+l4,−1−h,−2)，所以此矩阵错误。
(3) 这是为什么呢，我观察了下，现在它是 − 90 ° ， -90°， −90°，我把它转回去看看。即绕 z 5 z_5 z5轴正转 90 90 90度。
可以看出 x 5 x_5 x5与 x 4 x_4 x4并非平行，相反地， y 5 y_5 y5与 y 4 y_4 y4是平行的。说明坐标系 5 5 5为绕 y y y轴转动了 90 90 90度，然后在 x x x与 y y y方向产生了位移。这个变换矩阵应当自行计算：
5 4 T = T r a n s ( y , − h ) T r a n s ( x , l 4 ) R o t ( y , 90 ° ) R o t ( z , θ 5 ) = [ 1 0 0 l 4 0 1 0 − h 0 0 1 0 0 0 0 1 ] [ 0 0 1 0 0 1 0 0 − 1 0 0 0 0 0 0 1 ] [ c o s θ 5 − s i n θ 5 0 0 s i n θ 5 c o s θ 5 0 0 0 0 1 0 0 0 0 1 ] _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} 54T=Trans(y,−h)Trans(x,l4)Rot(y,90°)Rot(z,θ5)=⎣⎢⎢⎡100001000010l4−h01⎦⎥⎥⎤⎣⎢⎢⎡00−10010010000001⎦⎥⎥⎤⎣⎢⎢⎡cosθ5sinθ500−sinθ5cosθ50000100001⎦⎥⎥⎤
= [ 0 0 1 l 4 s i n θ 5 c o s θ 5 0 − h − c o s θ 5 s i n θ 5 0 0 0 0 0 1 ] =\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} =⎣⎢⎢⎡0sinθ5−cosθ500cosθ5sinθ501000l4−h01⎦⎥⎥⎤
将 θ = − 90 ° \theta=-90° θ=−90°代入得 5 4 T = [ 0 0 1 l 4 − 1 0 0 − h 0 − 1 0 0 0 0 0 1 ] _5^4T=\begin{bmatrix}0&0&1&l_4\\-1&0&0&-h\\0&-1&0&0\\0&0&0&1\end{bmatrix} 54T=⎣⎢⎢⎡0−10000−101000l4−h01⎦⎥⎥⎤，再将点 ( 1 , 2 , 3 ) (1,2,3) (1,2,3)代入试试 [ 0 0 1 l 4 − 1 0 0 − h 0 − 1 0 0 0 0 0 1 ] [ 1 2 3 1 ] = [ 3 + l 4 − 1 − h − 2 1 ] \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} ⎣⎢⎢⎡0−10000−101000l4−h01⎦⎥⎥⎤⎣⎢⎢⎡1231⎦⎥⎥⎤=⎣⎢⎢⎡3+l4−1−h−21⎦⎥⎥⎤，正确。
反复验证，此种方法得到的矩阵应该不存在问题。
方法总结
于是就出现了问题，难道传统 D − H D-H D−H参数法是错的？那肯定不是，之前的学者们肯定遇到了各种特殊情况，并且给出了解决方法，但我觉得书上描述的针对一般性问题的描述过于复杂，在此，我想出了一种比较快速的方法。
根据前面的描述，若舵机转角为 0 0 0， θ \theta θ为 0 0 0，即仅绕 x x x轴产生转动，转动后的新 x x x轴应当与原 x x x轴平行。
故，看到机械臂关节后的建立坐标转换矩阵的步骤为：
（1）根据 C r a i g Craig Craig规则，确定各原点以及 x i , y i , z i x_i,y_i,z_i xi,yi,zi 的方向。并根据规则算出此状态时的角度 θ i 0 \theta_{i0} θi0。
（2）将关节 i i i沿转轴 z i z_i zi旋转 − θ i 0 -\theta_{i0} −θi0，得到关节 i i i相对于上一个转角为 0 0 0时的坐标系，观察此时的 x x x轴是否与上一个关节的 x x x轴平行，若平行，则满足前述规则，直接求出 D − H D-H D−H参数，并代入矩阵
[ c o s θ i − s i n θ i 0 a i − 1 s i n θ i c o s α i − 1 c o s θ i c o s α i − 1 − s i n α i − 1 − d i s i n α i − 1 s i n θ i s i n α i − 1 c o s θ i s i n α i − 1 c o s α i − 1 d i c o s α i − 1 0 0 0 1 ] \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} ⎣⎢⎢⎡cosθisinθicosαi−1sinθisinαi−10−sinθicosθicosαi−1cosθisinαi−100−sinαi−1cosαi−10ai−1−disinαi−1dicosαi−11⎦⎥⎥⎤
(3)若此时的 x x x轴与上一个关节的 x x x轴不平行，则必定有绕 y y y轴的转动，此时需要观察，另行推算出坐标变换矩阵。一般优先考虑转角，后考虑位移。一般情况下机械臂不会有很奇怪的角度，通过观察可得到沿 x x x与 y y y轴的转角。若同时具有 x x x与 y y y轴的转角并且角度不是 90 90 90的倍数，也可根据坐标轴之间的夹角求出转角。
另：在上面那个机械臂的例子中 x 3 x_3 x3与 x 4 x_4 x4也不是平行的，但考虑到此状态的 θ = − 90 ° \theta=-90° θ=−90°，将 θ 3 \theta_3 θ3正转 90 ° 90° 90°后， x 3 x_3 x3与 x 4 x_4 x4平行。
2019-01-03 上海