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

1 前言

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

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

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

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

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]：

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

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

$z_j$ 轴沿关节 $j+1$ 的轴向(指向可任选，但通常都将各平行的 $z$ 轴均取为相同的指向)；
原点选取的原则：原点 $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$ 的公垂线上。
轴 $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 建系原则，进行以下步骤的建模：

确定各关节的旋转轴，用 $q_j$ 表示(可以参考下一节的图)。
确定 $z$ 轴，依据 DH 建系方法， $z_0 (z_{base})$ 应为 $q_1$，$z_1$ 应为 $q_2$，以此类推，最终，$z_6$ 应为 $q_6$，与 $z_5$ 相同。
确定坐标原点：
- $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$；
确定 $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$；

上述建模后存在的问题：

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

确定 DH 参数：

$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$.
$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$.
$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$.
$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$.
$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$.
$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 建系方法存在的不足：

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

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

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

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

确定各关节的旋转轴，用 $q_j$ 表示。
确定 $z$ 轴，依据 DH 建系方法， $z_0$ 应为 $q_1$，$z_1$ 应为 $q_2$，以此类推，最终，$z_6$ 应为 $q_6$，与 $z_5$ 相同。指定竖直方向的 $z$ 轴向上为正，水平方向的 $z$ 轴向左为正。
确定坐标原点：
- $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$，在末端；
确定 $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 参数：

$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$.
$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$.
$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$.
$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$.
$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$.
$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$.