SDH,一学就会

介绍标准 DH(SDH) 机器人建系方法。

1 前言

在做机器人控制的时候,为了能够方便地对机器人进行力矩控制,自己搭建了一套控制器。在搭建完基本的硬件平台后,需要对机器人进行建模。首先是机器人坐标系的建立。以UR10为例。

有两种可以在 MATLAB 中使用的工具箱:

  • PerterCoke的Robotics ToolBox
  • ARTE: A ROBOTICS TOOLBOX FOR EDUCATION

第一种工具箱对应的有一本书 《Robotics, Vision and Control》[1],这个用的比较多,不再赘述。

第二种工具箱下载好后将路径添加到 MATLAB 中,然后在它的文件夹下运行一下命令即可。

1
2
3
4
5
init_lib
robot = load_robot('UR','UR10');
% 关节坐标轴是否可见
% robot.graphical.draw_axes = 1
drawrobot3d(robot,[0 0 0 0 0 0])

2 SDH建立坐标系的过程

SDH

上图所示的转动关节 DH 参数的定义[1]

  1. 关节距离 $d_j$:从 $x_{j-1}$ 轴沿 $z_{j-1}$ 轴到 $x_j$ 轴的距离;
  2. 关节转角 $\theta _j$:从 $x_{j-1}$ 轴沿 $z_{j-1}$ 轴转到 $x_j$ 轴的角度,且 $\theta_j \in (-\pi,\pi]$;
  3. 杆间距离 $a_j$:从 $z_{j-1}$ 轴沿 $x_j$ 轴到 $z_j$ 轴的距离;
  4. 杆件扭角 $\alpha_j$:从 $z_{j-1}$ 轴沿 $x_j$ 轴转到 $z_j$ 轴的角度,且 $\alpha_j \in (-\pi,\pi]$.

DH 坐标系中轴与原点确定的基本原则是[2]

  1. $z_j$ 轴沿关节 $j+1$ 的轴向(指向可任选,但通常都将各平行的 $z$ 轴均取为相同的指向);
  2. 原点选取的原则:原点 $O_j$ 为 $z_{j-1}$ 轴和 $z_j$ 轴的公法线与 $z_{j-1}$ 轴的交点.
    • 当 $z_{j-1}$ 轴和 $z_j$ 轴平行时,两轴之间有无数条公垂线,若两轴重合,取 $O_j = O_{j-1}$;若两轴平行但不重合,则过 $O_{j-1}$ 点作 $z_{j-1}$ 轴和 $z_j$ 轴的公垂线,此公垂线与 $z_j$ 轴的交点即 $O_j$.
    • 由于没有 $z_{-1}$,所以上述方法不适用于 $O_0$,这时确定 $O_0$ 的方法是:若 $z_0$ 与 $z_1$ 相交,取 $O_0 = O_1$;若$z_0$ 与 $z_1$ 不相交,取 $O_0$ 在 $z_0$ 与 $z_1$ 的公垂线上。
  3. 轴 $x_j$ 与轴 $z_{j-1}$ 相交且垂直,即 $x_j$ 为沿过 $z_{j-1}$ 轴和 $z_j$ 轴的公垂线,方向从 $z_{j-1}$ 指向 $z_j$.
    • 当 $z_{j-1}$ 轴和 $z_j$ 轴重合时,这时 $O_j = O_{j-1}$,那么在零位时要求 $x_j = x_{j-1}$.
    • 当 $z_{j-1}$ 轴和 $z_j$ 轴相交且不重合时,选择 $x_j = \pm(z_{j-1}\times z_j)$,通常使所有平行的 $x$ 轴均具有相同的指向.
    • 当 $j = 0$时,$O_0 = O_1$, 或 $O_0$ 在 $z_0$ 轴和 $z_1$ 轴的公垂线上,选取 $x_0$ 轴与 $x_1$ 轴重合.

      Tip: 上述方法没有考虑平动关节的建系,可参见[2]。

3 官网给出的 UR10 的 SDH 模型

UR官网给出在零位时的UR10模型如下图所示。

UR10

按照第 2 节给出的 DH 建系原则,进行以下步骤的建模:

  1. 确定各关节的旋转轴,用 $q_j$ 表示(可以参考下一节的图)。
  2. 确定 $z$ 轴,依据 DH 建系方法, $z_0 (z_{base})$ 应为 $q_1$,$z_1$ 应为 $q_2$,以此类推,最终,$z_6$ 应为 $q_6$,与 $z_5$ 相同。
  3. 确定坐标原点:
    • $j = 0$ 时,$z_0$ 与 $z_1$ 相交,取 $O_0 = O_1$;
    • $j = 1$ 时,$z_0$ 与 $z_1$ 相交,取其交点为 $O_1$;
    • $j = 2$ 时,$z_1$ 与 $z_2$ 平行,过 $O_1$ 作 $z_1$ 与 $z_2$ 的公垂线并与 $z_2$ 相交的交点为 $O_2$;
    • $j = 3$ 时,$z_2$ 与 $z_3$ 平行,过 $O_2$ 作 $z_2$ 与 $z_3$ 的公垂线并与 $z_3$ 相交的交点为 $O_3$;
    • $j = 4$ 时,$z_3$ 与 $z_4$ 相交,取其交点为 $O_4$;
    • $j = 5$ 时,$z_4$ 与 $z_5$ 相交,取其交点为 $O_5$;
    • $j = 6$ 时,$z_5$ 与 $z_6$ 重合,取原点为 $O_6 = O_5$;
  4. 确定 $x$ 轴:
    • $j = 0$ 时,$O_0 = O_1$,应有 $x_0 = x_1$;
    • $j = 1$ 时,$z_0$ 与 $z_1$ 相交,选择 $x_1 = z_0 \times z_1$ (以上图为例,可以认为是图内指向向右);
    • $j = 2$ 时,$z_1$ 与 $z_2$ 平行,$x_2$ 即 $z_1$ 与 $z_2$ 的公垂线,且从 $z_1$ 指向 $z_2$,为了保持平行方向的 $x$ 轴朝向一致,指定 $x_2$ 从 $z_2$ 指向 $z_1$;
    • $j = 3$ 时,$z_2$ 与 $z_3$ 平行,$x_3$ 即 $z_2$ 与 $z_3$ 的公垂线,且从 $z_2$ 指向 $z_3$,为了保持平行方向的 $x$ 轴朝向一致,指定 $x_3$ 从 $z_3$ 指向 $z_2$;
    • $j = 4$ 时,$z_3$ 与 $z_4$ 相交,选择 $x_4 = z_3 \times z_4$;
    • $j = 5$ 时,$z_4$ 与 $z_5$ 相交,选择 $x_5 = z_4 \times z_5$,为了保持平行方向的 $x$ 轴朝向一致,改为$x_5 = -z_4 \times z_5$;
    • $j = 6$ 时,$z_5$ 与 $z_6$ 重合,取 $x_6 = x_5$;

上述建模后存在的问题:

  1. $0$ 系和 $1$ 系重合,而 $O_1$ 为 $z_0$ 与 $z_1$ 的交点, 这样没有考虑到 $O_1$ 与地面的高度,因此,将 $O_0$ 移动至底座与地面的接触面上,坐标系方向不变;
  2. 同理,$5$ 系和 $6$ 系重合,没有考虑到连杆 $6$ 的长度,因此将 $O_6$ 移动至机器人末端,坐标系朝向不变。

确定 DH 参数:

  1. $j = 1$ 时:
    • 从 $x_0$ 轴沿 $z_0$ 轴到 $x_1$ 轴的距离为 $d_1$,即 $O_1$ 到地面的高度;
    • 因 $x_0 // x_1$,从 $x_0$ 轴沿 $z_0$ 轴转到 $x_1$ 轴的角度为 $\theta_1 = 0$;
    • 因 $z_0 \perp z_1$ 且共面,从 $z_0$ 轴沿 $x_1$ 轴到 $z_1$ 轴的距离为 $a_1 = 0$;
    • 从 $z_0$ 轴沿 $x_1$ 轴转到 $z_1$ 轴的角度为 $\alpha_1 = \pi/2$.
  2. $j = 2$ 时:
    • 从 $x_1$ 轴沿 $z_1$ 轴到 $x_2$ 轴的距离为 $d_2 = 0$;
    • 从 $x_1$ 轴沿 $z_1$ 轴转到 $x_2$ 轴的角度为 $\theta_2 = 0$;
    • 从 $z_1$ 轴沿 $x_2$ 轴到 $z_2$ 轴的距离为 $-a_2$,即 连杆 $2$ 长度的相反数;
    • 因 $z_1 // z_2$,从 $z_1$ 轴沿 $x_2$ 轴转到 $z_2$ 轴的角度为 $\alpha_2 = 0$.
  3. $j = 3$ 时:
    • 从 $x_2$ 轴沿 $z_2$ 轴到 $x_3$ 轴的距离为 $d_3 = 0$;
    • 因 $x_2 // x_3$,从 $x_2$ 轴沿 $z_2$ 轴转到 $x_3$ 轴的角度为 $\theta_3 = 0$;
    • 从 $z_2$ 轴沿 $x_3$ 轴到 $z_3$ 轴的距离为 $-a_3$,即 连杆 $3$ 竖直段的长度;
    • 因 $z_2 // z_3$,从 $z_2$ 轴沿 $x_3$ 轴转到 $z_3$ 轴的角度为 $\alpha_3 = 0$.
  4. $j = 4$ 时,
    • 从 $x_3$ 轴沿 $z_3$ 轴到 $x_4$ 轴的距离为 $d_4$,即连杆 $4$ 长度;
    • 因 $x_3 // x_4$ ,从 $x_3$ 轴沿 $z_3$ 轴转到 $x_4$ 轴的角度为 $\theta_4 = 0$;
    • 因 $z_3 \perp z_4$ 且共面,从 $z_3$ 轴沿 $x_4$ 轴到 $z_4$ 轴的距离为 $a_4 = 0$;
    • 从 $z_3$ 轴沿 $x_4$ 轴转到 $z_4$ 轴的角度为 $\alpha_4 = \pi/2$.
  5. $j = 5$ 时,
    • 从 $x_4$ 轴沿 $z_4$ 轴到 $x_5$ 轴的距离为 $d_5$,即连杆 $5$ 长度;
    • 因 $x_4 // x_5$,从 $x_4$ 轴沿 $z_4$ 轴转到 $x_5$ 轴的角度为 $\theta_5 = 0$;
    • 因 $z_4 \perp z_5$ 且共面,从 $z_4$ 轴沿 $x_5$ 轴到 $z_5$ 轴的距离为 $a_5 = 0$;
    • 从 $z_4$ 轴沿 $x_5$ 轴转到 $z_5$ 轴的角度为 $\alpha_5 = -\pi/2$.
  6. $j = 6$ 时,
    • 从 $x_5$ 轴沿 $z_5$ 轴到 $x_6$ 轴的距离为 $d_6$,即连杆 $6$ 长度;
    • 因 $x_5 // x_6$,从 $x_5$ 轴沿 $z_5$ 轴转到 $x_6$ 轴的角度为 $\theta_6 = 0$;
    • 因 $z_5$ 与 $z_6$ 重合,从 $z_5$ 轴沿 $x_6$ 轴到 $z_6$ 轴的距离为 $a_6 = 0$;
    • 从 $z_5$ 轴沿 $x_6$ 轴转到 $z_6$ 轴的角度为 $\alpha_6 = 0$.

综上所述,DH 参数可以汇总如下表:

$j$ $\theta$ $d$ $a$ $\alpha$
1 $0$ $d_1$ $0$ $\pi/2$
2 $0$ $0$ $-a_2$ $0$
3 $0$ $0$ $-a_3$ $0$
4 $0$ $d_4$ $0$ $\pi/2$
5 $0$ $d_5$ $0$ $-\pi/2$
6 $0$ $d_6$ $0$ $0$

对照官网给出的 DH 参数:

puma560

上述给出的 DH 建系方法存在的不足

  1. $j = 2$ 时,$z_1$ 与 $z_2$ 平行,按照基本原则应过 $O_1$ 作 $z_1$ 与 $z_2$ 的公垂线,其与 $z_2$ 的交点为 $O_2$。这样子建系存在的问题是未将连杆 $1$ 的长度考虑在内
  2. 同理,未考虑连杆 $3$ 在 $z_2(q_3)$ 轴方向的长度。
  3. 官网的零位在实际中是躺在地上的一种位姿,笔者在实际应用中要求零位是竖直状态。
  4. 仿真中进行显示的时候会感觉效果不好。

4 包含全部参数的SDH建系方法

首先,将零位指定为竖直状态,如下图所示。

puma560
puma560
puma560

再进行一次坐标系的确定:

  1. 确定各关节的旋转轴,用 $q_j$ 表示。
  2. 确定 $z$ 轴,依据 DH 建系方法, $z_0$ 应为 $q_1$,$z_1$ 应为 $q_2$,以此类推,最终,$z_6$ 应为 $q_6$,与 $z_5$ 相同。指定竖直方向的 $z$ 轴向上为正,水平方向的 $z$ 轴向左为正。
  3. 确定坐标原点:
    • $j = 0$ 时,为了包含 $1$ 关节到地面的距离,应取原点 $O_0$ 为基座与地面的接触底面中心;
    • $j = 1$ 时,$z_0$ 与 $z_1$ 相交,取其交点为 $O_1$,与第3节一致;
    • $j = 2$ 时,$z_1$ 与 $z_2$ 平行,第3节中对应的位置为 $O_2’$,为包含连杆 $1$ 的长度,将原来的 $O_2$ 移动至连杆 $2$ 的中心线与 $z_2$ 的交点;
    • $j = 3$ 时,$z_2$ 与 $z_3$ 平行,按照基本原则应确定为 $O_3’$,注意到连杆 $3$ 实际上由电机所在水平段与包含 $4$ 关节在内的竖直段组成,为包含连杆 $3$ 在水平段的长度,应该移至连杆 $3$ 与 $z_3$ 的交点,即 $O_3$,反而与第3节的 $O_3$ 一致;
    • $j = 4$ 时,$z_3$ 与 $z_4$ 相交,取其交点为 $O_4$,与第3节一致;
    • $j = 5$ 时,$z_4$ 与 $z_5$ 相交,取其交点为 $O_5$,与第3节一致;
    • $j = 6$ 时,$z_5$ 与 $z_6$ 重合,为包含连杆 $6$ 的长度,取原点为 $O_6$,在末端;
  4. 确定 $x$ 轴:
    • $j = 0$ 时,与第3节一致;
    • $j = 1$ 时,$z_0$ 与 $z_1$ 相交,选择 $x_1 = z_0 \times z_1$ (以上图为例,可以认为是指向纸面外);
    • $j = 2$ 时,$z_1$ 与 $z_2$ 平行,$x_2$ 即 $z_1$ 与 $z_2$ 的公垂线,且从 $z_1$ 指向 $z_2$;
    • $j = 3$ 时,$z_2$ 与 $z_3$ 平行,$x_3$ 即 $z_2$ 与 $z_3$ 的公垂线,且从 $z_2$ 指向 $z_3$;
    • $j = 4$ 时,$z_3$ 与 $z_4$ 相交,选择 $x_4 = -z_3 \times z_4$,与 $x_1$ 保持方向一致;
    • $j = 5$ 时,$z_4$ 与 $z_5$ 相交,选择 $x_5 = z_4 \times z_5$;
    • $j = 6$ 时,$z_5$ 与 $z_6$ 重合,取 $x_6 // x_5$ 且正方向相同;

确定 DH 参数:

  1. $j = 1$ 时:
    • 从 $x_0$ 轴沿 $z_0$ 轴到 $x_1$ 轴的距离为 $d_1$,即 $O_1$ 到地面的高度;
    • 因 $x_0 // x_1$,从 $x_0$ 轴沿 $z_0$ 轴转到 $x_1$ 轴的角度为 $\theta_1 = 0$;
    • 因 $z_0 \perp z_1$ 且共面,从 $z_0$ 轴沿 $x_1$ 轴到 $z_1$ 轴的距离为 $a_1 = 0$;
    • 从 $z_0$ 轴沿 $x_1$ 轴转到 $z_1$ 轴的角度为 $\alpha_1 = \pi/2$.
  2. $j = 2$ 时:
    • 从 $x_1$ 轴沿 $z_1$ 轴到 $x_2$ 轴的距离为 $d_2$,即 $z_0$ 与连杆 $2$ 中轴线的距离,即连杆 $1$ 的长度;
    • 从 $x_1$ 轴沿 $z_1$ 轴转到 $x_2$ 轴的角度为 $\theta_2 = \pi/2$;
    • 从 $z_1$ 轴沿 $x_2$ 轴到 $z_2$ 轴的距离为 $a_2$,即 连杆 $2$ 的长度;
    • 因 $z_1 // z_2$,从 $z_1$ 轴沿 $x_2$ 轴转到 $z_2$ 轴的角度为 $\alpha_2 = 0$.
  3. $j = 3$ 时,注意连杆 $3$ 实际上由电机所在水平段与包含 $4$ 关节在内的竖直段组成:
    • 从 $x_2$ 轴沿 $z_2$ 轴到 $x_3$ 轴的距离为 $-d_3$,即连杆 $3$ 水平段长度的相反数;
    • 因 $x_2 // x_3$,从 $x_2$ 轴沿 $z_2$ 轴转到 $x_3$ 轴的角度为 $\theta_3 = 0$;
    • 从 $z_2$ 轴沿 $x_3$ 轴到 $z_3$ 轴的距离为 $a_3$,即 连杆 $3$ 竖直段的长度;
    • 因 $z_2 // z_3$,从 $z_2$ 轴沿 $x_3$ 轴转到 $z_3$ 轴的角度为 $\alpha_3 = 0$.
  4. $j = 4$ 时,
    • 从 $x_3$ 轴沿 $z_3$ 轴到 $x_4$ 轴的距离为 $d_4$,即连杆 $4$ 长度;
    • 因 $x_3 \perp x_4$ 且不共面,从 $x_3$ 轴沿 $z_3$ 轴转到 $x_4$ 轴的角度为 $\theta_4 = -\pi/2$;
    • 因 $z_3 \perp z_4$ 且共面,从 $z_3$ 轴沿 $x_4$ 轴到 $z_4$ 轴的距离为 $a_4 = 0$;
    • 从 $z_3$ 轴沿 $x_4$ 轴转到 $z_4$ 轴的角度为 $\alpha_4 = -\pi/2$.
  5. $j = 5$ 时,
    • 从 $x_4$ 轴沿 $z_4$ 轴到 $x_5$ 轴的距离为 $d_5$,即连杆 $5$ 长度;
    • 因 $x_4 // x_5$,从 $x_4$ 轴沿 $z_4$ 轴转到 $x_5$ 轴的角度为 $\theta_5 = 0$;
    • 因 $z_4 \perp z_5$ 且共面,从 $z_4$ 轴沿 $x_5$ 轴到 $z_5$ 轴的距离为 $a_5 = 0$;
    • 从 $z_4$ 轴沿 $x_5$ 轴转到 $z_5$ 轴的角度为 $\alpha_5 = \pi/2$.
  6. $j = 6$ 时,
    • 从 $x_5$ 轴沿 $z_5$ 轴到 $x_6$ 轴的距离为 $d_6$,即连杆 $6$ 长度;
    • 因 $x_5 // x_6$,从 $x_5$ 轴沿 $z_5$ 轴转到 $x_6$ 轴的角度为 $\theta_6 = 0$;
    • 因 $z_5$ 与 $z_6$ 重合,从 $z_5$ 轴沿 $x_6$ 轴到 $z_6$ 轴的距离为 $a_6 = 0$;
    • 从 $z_5$ 轴沿 $x_6$ 轴转到 $z_6$ 轴的角度为 $\alpha_6 = 0$.

综上所述,DH 参数可以汇总如下表:

$j$ $\theta$ $d$ $a$ $\alpha$
1 $0$ $d_1$ $0$ $\pi/2$
2 $\pi/2$ $d_2$ $a_2$ $0$
3 $0$ $-d_3$ $a_3$ $0$
4 $-\pi/2$ $d_4$ $0$ $-\pi/2$
5 $0$ $d_5$ $0$ $\pi/2$
6 $0$ $d_6$ $0$ $0$

5 结果对比

给出 PeterCoke工具箱中UR10的 DH 参数和上述建立的 DH 参数仿真结果:

上述结果的左侧为工具箱的仿真结果,右侧为本文建立的 DH 参数仿真结果。”offset”即表示关节角的偏转,对应于 DH 中的 $\theta$。

PeterCoke的工具箱是不包含 UR10 的 STL 模型的,ARTE 工具箱中包含了该模型,需要将它导入,可以参考 [5]。STL 模型必须包含所有连杆长度信息才能正常显示。

6 思考

至此,还有两个小小的疑问供大家思考:

  1. 对于零位在水平方向的情况,包含全部参数的 DH 参数是多少?
  2. 零位在水平方向时,连杆 $1$ 的长度和连杆 $3$ 水平方向的长度是否被考虑在 DH 参数内会影响最后的计算结果吗?

7 参考文献与连接

[1] Corke P. Robotics, vision and control: fundamental algorithms in MATLAB® second, completely revised[M].
Springer, 2017.

[2] 霍伟. 机器人动力学与控制[M]. 高等教育出版社, 2005.

[3] ARTE: A ROBOTICS TOOLBOX FOR EDUCATION

[4] UR官网

[5] 机器人工具箱——3D可视化显示

------ 本文结束感谢您的阅读------
Donate a cup of cola?