一文带你完全掌握机器人DH参数建模(详细步骤+实例+代码)

一、前言

1、描述两个坐标系的变换关系需要6个参数(3个表示位置变换，3个表示姿态变换),为什么DH参数只需4个？

  空间任意两个坐标系的变换关系确实需要6个参数来表示，然而，在建立各个连杆的坐标系时，我们可以拟定一些规则，使得坐标系满足某些约束，从而只需4个参数则可以表示两个坐标系的变换关系。
  若坐标轴$x_i$与坐标轴$z_{i-1}$垂直相交(示例如下图)，则只需4个参数(杆件长度$a_i$，杆件扭角${\alpha }_i$，关节距离$d_i$，关节转角${\theta }_i$)就可以表示两个坐标系的变换关系。下面的证明来于书籍：Robot Modeling and Control(First Edition),by Mark W. Spong, Seth Hutchinson, and M. Vidyasagar.

  若坐标系$\{x_1{y}_1{z}_1\}$与坐标系$\{x_0{y}_0{z}_0\}$垂直相交，则坐标系$\{x_1{y}_1{z}_1\}$到坐标系$\{x_0{y}_0{z}_0\}$的齐次变换矩阵为：
$$A=Ro{t}_{z,\theta }Tran{s}_{z,d}Tran{s}_{x,a}Ro{t}_{x,\alpha }\text{\hspace{1em}(1)}$$
  (1)式可以写成：
$$A=\left[\begin{array}{cc}{R}_1^0& O_1^0\\ 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$
  因为$x_1$垂直于$z_0$，故有：
$$0=x_1^0\cdot z_0^0=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=r_{31}\text{\hspace{1em}(3)}$$
  由于$r_{31}=0$，只要证明存在唯一的杆件扭角$\alpha$，关节转角$\theta$，使得：
$$R_1^0=Ro{t}_{z,\theta }Ro{t}_{x,\alpha }=\left[\begin{array}{ccc}{c}_{\theta }& -s_{\theta }{c}_{\alpha }& s_{\theta }{s}_{\alpha }\\ s_{\theta }& c_{\theta }{c}_{\alpha }& -c_{\theta }{s}_{\alpha }\\ 0& s_{\alpha }& c_{\alpha }\end{array}\right]\text{\hspace{1em}(4)}$$

  因此，存在唯一的$\theta ,\alpha$满足：
$$\{\begin{array}{c}(r_{11},r_{21})=(c_{\theta },s_{\theta })\\ (r_{32},r_{33})=(c_{\alpha },s_{\alpha })\end{array}\text{\hspace{1em}(5)}$$
  易得：
$$\{\begin{array}{c}\theta =atan2(r_{21},r_{11})\\ \alpha =atan2(r_{32},r_{33})\end{array}\text{\hspace{1em}(6)}$$
  根据旋转矩阵的性质，可以推导得到：
$$\{\begin{array}{c}-s_{\theta }{c}_{\alpha }=r_{12}\\ s_{\theta }{s}_{\alpha }=r_{13}\\ c_{\theta }{c}_{\alpha }=r_{22}\\ -c_{\theta }{s}_{\alpha }=r_{23}\end{array}\text{\hspace{1em}(7)}$$
  可见，坐标系$\{x_1{y}_1{z}_1\}$到坐标系$\{x_0{y}_0{z}_0\}$的旋转变换关系用$\theta ,\alpha$来表达就足够了。
  若$x_1$与$z_0$相交，坐标系$\{x_1{y}_1{z}_1\}$的原点在坐标系$\{x_0{y}_0{z}_0\}$下的坐标(或平移向量)为：

$$O_1^0=O_0^0+d{z}_0^0+a{x}_1^0=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]+d\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]+a\left[\begin{array}{c}{c}_{\theta }\\ s_{\theta }\\ 0\end{array}\right]=\left[\begin{array}{c}a{c}_{\theta }\\ a{s}_{\theta }\\ d\end{array}\right]\text{\hspace{1em}(8)}$$
  综上，坐标系$\{x_1{y}_1{z}_1\}$到坐标系$\{x_0{y}_0{z}_0\}$的齐次变换关系用杆件长度$a$，杆件扭角$\alpha$，关节距离$d$，关节转角$\theta$ 这4个参数就够了。

2、利用DH参数建模时，各个连杆坐标系的建立是唯一的吗？

  利用DH参数建模时，各个连杆坐标系的建立不是唯一的。只要在建立坐标系时，坐标轴$x_i$与坐标轴$z_{i-1}$垂直相交即可。

3、DH参数表是唯一的吗？

  DH参数表不是唯一的。因为各个连杆坐标系的建立不是唯一的，导致DH参数不同，但是最终计算得到末端工具坐标系到机器人基坐标系下的齐次变换矩阵是唯一的。(前提是机器人基坐标系与末端工具坐标系的建立保持不变)

4、标准DH参数与修正DH参数有何异同？

  祥见参考资料2,3。
  (1)标准DH参数坐标系建立在传动轴上，而修正DH参数坐标系建立在驱动轴上。
  (2)由于坐标系建立位置发生了变化，连杆之间的坐标系变换关系自然也发生变化。
  标准DH相邻连杆之间坐标系的变换关系为：
$${}^{i-1}{A}_i=Ro{t}_{z,{\theta }_i}Tran{s}_{z,d_i}Tran{s}_{x,a_i}Ro{t}_{x,{\alpha }_i}\text{\hspace{1em}(9)}$$
  修正DH相邻连杆之间坐标系的变换关系为：
$${}^{i-1}{A}_i=Ro{t}_{x,{\alpha }_{i-1}}Tran{s}_{x,a_{i-1}}Ro{t}_{z,{\theta }_i}Tran{s}_{z,d_i}\text{\hspace{1em}(10)}$$
  (3)修正DH参数中各个参数的物理意义与标准DH参数是一样的。
  (4)对于传统的串联机器人而言，两者的表现能力是一样的，没有优劣之分，我们可以选择其中一种方法进行建模。然而，由于修正DH参数坐标系建立在驱动轴上，对于树状结构的机器人，其表现能力更强，可以简化问题。
  (5)对于标准DH参数，根据DH参数表，并对式(9) 连乘得到的是末端工具坐标系到机器人基坐标系的齐次变换矩阵；对于修正DH参数，根据DH参数表，并对式(10) 连乘得到的是最后一个驱动关节上的坐标系到机器人基坐标系的齐次变换矩阵，变换到末端工具坐标系还需增加一个变换(通常为平移变换)。

二、标准DH参数

1、DH四个参数的定义

  (1)$d_i$ ：坐标轴$x_{i-1}$与坐标轴$x_i$沿着坐标轴$z_{i-1}$的有向距离。
  (2)$a_i$：坐标轴$z_{i-1}$与坐标轴$z_i$沿着坐标轴$x_i$的有向距离。
  (3)${\alpha }_i$：坐标轴$z_{i-1}$与坐标轴$z_i$的夹角，方向定义如下：

  (4) ${\theta }_i$：坐标轴$x_{i-1}$与坐标轴$x_i$的夹角，方向定义如下：

2、DH连杆坐标系的约定

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

3、根据DH连杆坐标系约定，建立各个连杆坐标系

4、根据DH四个参数的定义，创建DH参数表

三、六轴机器人实例(standard DH)

  这里以六轴机器人为例，通过建立3种不同的连杆坐标系，创建对应的DH表，比较最终运动学正解最终结果是否一致。其中，$d_1=0.3991，a_2=0.448，a_3=0.042，d_4=0.451，d_6=0.082$，以米为单位。两种不同的连杆坐标系的建立方法得到不同的DH参数表，但最终的机器人运动学正解完全一样！

1、第一种连杆坐标系建立方法

  连杆坐标系建立如下：

  DH参数表如下：

Link	$d_i$	$a_i$	${\alpha }_i$	${\theta }_i$
1	$d_1$	0	$-\pi /2$	${\theta }_1$
2	0	$a_2$	0	${\theta }_2-\pi /2$
3	0	$a_3$	$-\pi /2$	${\theta }_3$
4	$d_4$	0	$\pi /2$	${\theta }_4$
5	0	0	$-\pi /2$	${\theta }_5$
6	$d_6$	0	0	${\theta }_6+\pi$

2、第二种连杆坐标系建立方法

  连杆坐标系建立如下：

  DH参数表如下：

Link	$d_i$	$a_i$	${\alpha }_i$	${\theta }_i$
1	$d_1$	0	$-\pi /2$	${\theta }_1$
2	0	$-a_2$	0	${\theta }_2+\pi /2$
3	0	$-a_3$	$\pi /2$	${\theta }_3$
4	$d_4$	0	$-\pi /2$	${\theta }_4$
5	0	0	$\pi /2$	${\theta }_5$
6	$d_6$	0	0	${\theta }_6$

四、MATLAB代码

clc;
clear;

syms d1 d2 d3 d4 d5 d6 a1 a2 a3 a4 a5 a6 real
syms alpha1 alpha2 alpha3 alpha4 alpha5 alpha6 real
syms theta1 theta2 theta3 theta4 theta5 theta6 real;

%% (1)
a1 = sym(0);
alpha1 = sym(-pi/2);
A1 = simplify([cos(theta1), -sin(theta1)*cos(alpha1), sin(theta1)*sin(alpha1), a1*cos(theta1)
    sin(theta1), cos(theta1)*cos(alpha1), -cos(theta1)*sin(alpha1), a1*sin(theta1)
    0, sin(alpha1), cos(alpha1), d1
    0, 0, 0, 1]);

d2 = sym(0);
alpha2 = sym(0);
A2 = simplify([cos(theta2 - pi/2), -sin(theta2 - pi/2)*cos(alpha2), sin(theta2 - pi/2)*sin(alpha2), a2*cos(theta2 - pi/2)
    sin(theta2 - pi/2), cos(theta2 - pi/2)*cos(alpha2), -cos(theta2 - pi/2)*sin(alpha2), a2*sin(theta2 - pi/2)
    0, sin(alpha2), cos(alpha2), d2
    0, 0, 0, 1]);

d3 = sym(0);
alpha3 = sym(-pi/2);
A3 = simplify([cos(theta3), -sin(theta3)*cos(alpha3), sin(theta3)*sin(alpha3), a3*cos(theta3)
    sin(theta3), cos(theta3)*cos(alpha3), -cos(theta3)*sin(alpha3), a3*sin(theta3)
    0, sin(alpha3), cos(alpha3), d3
    0, 0, 0, 1]);

a4 = sym(0);
alpha4 = sym(pi/2);
A4 = simplify([cos(theta4), -sin(theta4)*cos(alpha4), sin(theta4)*sin(alpha4), a4*cos(theta4)
    sin(theta4), cos(theta4)*cos(alpha4), -cos(theta4)*sin(alpha4), a4*sin(theta4)
    0, sin(alpha4), cos(alpha4), d4
    0, 0, 0, 1]);

a5 = sym(0);
d5 = sym(0);
alpha5 = sym(-pi/2);
A5 = simplify([cos(theta5), -sin(theta5)*cos(alpha5), sin(theta5)*sin(alpha5), a5*cos(theta5)
    sin(theta5), cos(theta5)*cos(alpha5), -cos(theta5)*sin(alpha5), a5*sin(theta5)
    0, sin(alpha5), cos(alpha5), d5
    0, 0, 0, 1]);

a6 = sym(0);
alpha6 = sym(0);
A6 = simplify([cos(theta6+pi), -sin(theta6+pi)*cos(alpha6), sin(theta6+pi)*sin(alpha6), a6*cos(theta6+pi)
    sin(theta6+pi), cos(theta6+pi)*cos(alpha6), -cos(theta6+pi)*sin(alpha6), a6*sin(theta6+pi)
    0, sin(alpha6), cos(alpha6), d6
    0, 0, 0, 1]);

T1 = simplify(A1*A2*A3*A4*A5*A6);

%% (2)
a1 = sym(0);
alpha1 = sym(-pi/2);
A1 = simplify([cos(theta1), -sin(theta1)*cos(alpha1), sin(theta1)*sin(alpha1), a1*cos(theta1)
    sin(theta1), cos(theta1)*cos(alpha1), -cos(theta1)*sin(alpha1), a1*sin(theta1)
    0, sin(alpha1), cos(alpha1), d1
    0, 0, 0, 1]);

d2 = sym(0);
alpha2 = sym(0);
A2 = simplify([cos(theta2 + pi/2), -sin(theta2 + pi/2)*cos(alpha2), sin(theta2 + pi/2)*sin(alpha2), (-a2)*cos(theta2 + pi/2)
    sin(theta2 + pi/2), cos(theta2 + pi/2)*cos(alpha2), -cos(theta2 + pi/2)*sin(alpha2), (-a2)*sin(theta2 + pi/2)
    0, sin(alpha2), cos(alpha2), d2
    0, 0, 0, 1]);

d3 = sym(0);
alpha3 = sym(pi/2);
A3 = simplify([cos(theta3), -sin(theta3)*cos(alpha3), sin(theta3)*sin(alpha3), (-a3)*cos(theta3)
    sin(theta3), cos(theta3)*cos(alpha3), -cos(theta3)*sin(alpha3), (-a3)*sin(theta3)
    0, sin(alpha3), cos(alpha3), d3
    0, 0, 0, 1]);

a4 = sym(0);
alpha4 = sym(-pi/2);
A4 = simplify([cos(theta4), -sin(theta4)*cos(alpha4), sin(theta4)*sin(alpha4), a4*cos(theta4)
    sin(theta4), cos(theta4)*cos(alpha4), -cos(theta4)*sin(alpha4), a4*sin(theta4)
    0, sin(alpha4), cos(alpha4), d4
    0, 0, 0, 1]);

a5 = sym(0);
d5 = sym(0);
alpha5 = sym(pi/2);
A5 = simplify([cos(theta5), -sin(theta5)*cos(alpha5), sin(theta5)*sin(alpha5), a5*cos(theta5)
    sin(theta5), cos(theta5)*cos(alpha5), -cos(theta5)*sin(alpha5), a5*sin(theta5)
    0, sin(alpha5), cos(alpha5), d5
    0, 0, 0, 1]);

a6 = sym(0);
alpha6 = sym(0);
A6 = simplify([cos(theta6), -sin(theta6)*cos(alpha6), sin(theta6)*sin(alpha6), a6*cos(theta6)
    sin(theta6), cos(theta6)*cos(alpha6), -cos(theta6)*sin(alpha6), a6*sin(theta6)
    0, sin(alpha6), cos(alpha6), d6
    0, 0, 0, 1]);
    
T2 = simplify(A1*A2*A3*A4*A5*A6);
err = simplify(T1 - T2)

五、参考资料

1.Robot Modeling and Control(First Edition),by Mark W. Spong, Seth Hutchinson, and M. Vidyasagar
2.https://blog.csdn.net/hitgavin/article/details/105018983
3.https://blog.csdn.net/jldemanman/article/details/80508683