正运动学¶

参考书目¶

《机器人建模与控制》[美] Mark W. Spong, Seth Hutchinson, M. Vidyasager

1 齐次变换矩阵¶

\[ _n^0T = \begin{bmatrix} _n^0R & _n^0o \\ 0 & 1 \\ \end{bmatrix} \]

用来表达坐标系 \(o_nx_ny_nz_n\) 相对于参考系 \(o_0x_0y_0z_0\) 的位置和姿态关系。

其中：

\(_n^0R\) 表示坐标系 \(o_nx_ny_nz_n\) 相对于参考系 \(o_0x_0y_0z_0\) 的旋转。
\(_n^0o\) 表示坐标系 \(o_nx_ny_nz_n\) 相对于参考系 \(o_0x_0y_0z_0\) 的平移，也就是 \(o_n\) 在参考系 \(o_0x_0y_0z_0\) 中的坐标值。

理解：以一般的三维情况为例。

\[ \begin{aligned} (_3^2H)\cdot (_4^3H) &= \begin{bmatrix} (_3^2R)_{3\times3} & (_3^2o)_{3\times1} \\ 0_{1\times3} & (1)_{1\times1} \\ \end{bmatrix} \cdot \begin{bmatrix} (_4^3R) & (_4^3o) \\ 0 & 1\\ \end{bmatrix}\\ & = \begin{bmatrix} (_3^2R)(_4^3R) & (_3^2R)(_4^3o) + (_3^2o) \\ 0 & 1 \end{bmatrix} \end{aligned} \]

其中：

\(_4^2R = (_3^2R)(_4^3R)\)：描述了从系 4 到系 2 的旋转变换；
\((_3^2R)(_4^3o) + _3^2o\)：先将系3旋转成和系 2 平行（不妨记为系 3'），将节点 4 的坐标在系 3' 中表示。由于系 3' 与系 2 平行，系 3' 与系 3 原点相同，所以此时节点 4 的坐标就可以用：
\(_3^2o\)（系 3 原点在系 2 中的坐标）\(+ (_3^2R)(_4^3o)\)（节点 4 在系 3' 中的坐标）的方式表示。

再理解：仍旧以三维情况为例。

\[ H \cdot \begin{bmatrix}x_{i1}\\y_{i1}\\z_{i1}\\1\end{bmatrix} = \begin{bmatrix}x_1 \cdot x_0 & y_1 \cdot x_0 & z_1 \cdot x_0 & x_0-x_1\\ x_1 \cdot y_0 & y_1 \cdot y_0 & z_1 \cdot y_0 & y_0 - y_1\\ x_1 \cdot z_0 & y_1 \cdot z_0 & z_1 \cdot z_0 & z_0 - z_1\\ 0 & 0 & 0 & 1\end{bmatrix} \cdot \begin{bmatrix}x_{i1}\\y_{i1}\\z_{i1}\\1\end{bmatrix} \]

对于一个在坐标系1中表示的点/向量，我们给它增加一个维度，并且定义该维度的值始终为1。（不妨称这种描述方式为“多一维”）

上式的计算结果是一个 \(4\times1\) 的向量。第一个分量是 \(x_{i1}, y_{i1}, z_{i1}\) 三个分量在 \(x_0\) 轴上的投影，加上 \(o_1\) 相对于 \(o_0\) 在 \(x_0\) 轴方向上的偏移。从而得到了原向量在坐标系 0 中第一个维度的值。第二、第三个分量同理。由于我们有意设置齐次变换矩阵的最后一行为 \(\begin{bmatrix}0 & 0 & 0 &1\end{bmatrix}\)，所以第四个分量计算结果为 1。这满足我们对“多一维”值之规定。

综上，我们可以发现，齐次变换矩阵和“多一维”向量共同组成了一个描述不同坐标系间位姿关系，以及坐标系中向量/点位置的自洽符号系统。

补充

\[ _0^nT = (_n^0T)^{-1} = \begin{bmatrix}R^T & -R^To\\ 0 & 1\end{bmatrix} \]

2 DH 约定（DH 方法）¶

在 DH 约定中，任意一个齐次变换矩阵 \(A_i\) 可以如下拆解：（Rotation 旋转，Translation 平移）

\[ \begin{aligned} A_i &= Rot_{z, \theta_i}Trans_{z, d_i}Trans_{x,a_i}Rot_{x,\alpha_i}\\ &= \begin{bmatrix} c_{\theta_i} & -s_{\theta_i} & 0 & 0\\ s_{\theta_i} & c_{\theta_i} & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & d_i\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & a_i\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & c_{\alpha_i} & -s_{\alpha_i} & 0\\ 0 & s_{\alpha_i} & c_{\alpha_i} & 0\\ 0 & 0 & 0 & 1\end{bmatrix}\\ &= \begin{bmatrix}c_{\theta_i} & -s_{\theta_i} c_{\alpha_i} & s_{\theta_i}s_{\alpha_i} & a_ic_{\theta_i}\\ s_{\theta_i} & c_{\theta_i}c_{\alpha_i} & -c_{\theta_i}s_{\alpha_i} & a_is_{\theta_i}\\ 0 & s_{\alpha_i} & c_{\alpha_i} & d_i\\ 0 & 0 & 0 & 1\end{bmatrix} \end{aligned} \]

\(c_{\theta_i}\) 是 \(\cos(\theta_i)\) 的简写，\(s_{\theta_i}\) 是 \(\sin(\theta_i)\) 的简写

上述四个矩阵分别代表：

\(Rot_{z, \theta_i}\)：绕 \(z\) 轴旋转 \(\theta_i\) 角度；
\(Trans_{z,d_i}\)：沿着 \(z\) 轴平移 \(d_i\) 距离；
\(Trans_{x,a_i}\)：沿着 \(x\) 轴平移 \(a_i\) 距离；
\(Rot_{x,\alpha_i}\)：绕 \(x\) 轴旋转 \(\alpha_i\) 角度。

这四个矩阵的含义可以结合“齐次变换矩阵”的内容和 Chapter 1 中“基本旋转矩阵”的内容来理解。

2.1 参数¶

DH约定中只需要四个参数就能表示一个齐次变换矩阵，而不是六个（三个欧拉角加三个方向的平移距离），这是因为DH约定对坐标系的配置有约束。

四个参数的符号和名称如下：

\(\theta_i\) —————— 关节角度（joint angle）（平动关节恒值，转动关节变量）
\(\alpha_i\) —————— 连杆扭曲（link twist）（常数）
\(d_i\) —————— 连杆偏置（link offset）（平动关节变量，转动关节恒值）
\(a_i\) —————— 连杆长度（link length）（常数）

在满足2.2中（DH1、DH2）性质的情况下，四个参数的物理含义为：

\(\theta_i\) —————— 垂直于 \(z_{i-1}\) 的平面内测得的 \(x_{i-1}\) 到 \(x_i\) 的角度
\(\alpha_i\) —————— 垂直于 \(x_i\) 的平面内测得的 \(z_{i-1}\) 到 \(z_i\) 的角度
\(d_i\) —————— 从原点 \(o_{i-1}\) 到轴线 \(x_i\) 与 \(z_{i-1}\) 交点之间的距离（轴 \(x_{i-1}\) 和轴 \(x_i\) 之间沿轴线 \(z_{i-1}\) 测得的距离）
\(a_i\) —————— 从原点 \(o_{i}\) 到轴线 \(x_i\) 与 \(z_{i-1}\) 交点之间的距离（轴 \(z_{i-1}\) 和轴 \(z_i\) 之间沿轴线 \(x_i\) 测得的距离）

2.2 存在和唯一性问题¶

在本小节中，我们试图证明以下断言——

若坐标系 0 和坐标系 1 满足如下两条性质：

（DH1）坐标轴 \(x_1\) 垂直于坐标轴 \(z_0\)。

（DH2）坐标轴 \(x_1\) 与坐标轴 \(z_0\) 相交。

则存在唯一的 \(\theta_i, d_i, a_i, \alpha_i\) 使得

\[A_i = Rot_{z, \theta_i}Trans_{z, d_i}Trans_{x,a_i}Rot_{x,\alpha_i}.\]

法1¶

因为 \(x_1\) 与 \(z_0\) 垂直相交（DH1,DH2），又因为 \(x_1\) 与 \(z_1\) 垂直相交，所以必然可以绕着 \(x_1\) 轴旋转坐标系1，使得 \(z_1 \parallel z_0\)（或重合）。
接下来，沿着 \(x_1\) 轴平移坐标系 1，即可使 \(z_1\) 轴与 \(z_0\) 轴重合。
随后，沿着 \(z_1\) 轴平移坐标系 1，即可使原点 \(o_1\) 与 \(o_0\) 重合。现在，\(x_{0}o_{0}y_{0}\) 平面与 \(x_{1}o_{1}y_{1}\) 平面自然是重合的。
因此，最后只需要绕着 \(z_1\) 轴转动坐标系1，即可使 \(x_1\) 与 \(x_0\) 重合，\(y_1\) 与 \(y_0\) 重合。

综上所述，\(A_i\) 描述了坐标系 1 相对于坐标系 0 的位置与姿态关系。任意一个隶属于坐标系1的向量，都可通过左乘 \(A_i\) 得到它在坐标系 0 中的坐标值。

法2¶

即证：

\[ \begin{bmatrix}x_0 \cdot x_1 & x_0 \cdot y_1 & x_0 \cdot z_1 \\ y_0 \cdot x_1 & y_0 \cdot y_1 & y_0 \cdot z_1 \\z_0 \cdot x_1 & z_o \cdot y_1 & z_0 \cdot z_1 \end{bmatrix} = \begin{bmatrix}c_{\theta_i} & -s_{\theta_i} c_{\alpha_i} & s_{\theta_i}s_{\alpha_i}\\ s_{\theta_i} & c_{\theta_i}c_{\alpha_i} & -c_{\theta_i}s_{\alpha_i}\\ 0 & s_{\alpha_i} & c_{\alpha_i}\end{bmatrix}, \begin{bmatrix}x_0-x_1\\ y_0-y_1\\ z_0-z_1\end{bmatrix} = \begin{bmatrix}a_ic_{\theta_i}\\ a_is_{\theta_i}\\ d_i \end{bmatrix} \]

先证第一个式子。因为 \(z_0 \perp x_1\)，所以 \(z_0 \cdot x_1 = 0\)。根据旋转矩阵的性质，每行每列都是一个单位向量，所以存在唯一的角度 \(\theta_i\) 和 \(\alpha_i\)，使得

\[(r_{11},r_{21}) = (c_{\theta_i}, s_{\theta_i}), (r_{32}, r_{33}) = (s_{\alpha_i}, c_{\alpha_i}) \]

这里的唯一是指，当坐标系 0 和坐标系 1 确定时，参数的取值唯一。

然后可以用旋转矩阵性质——“每行每列都是一个单位向量”和“旋转矩阵的转置为自身的逆矩阵”，计算得到 \(r_{12}, r_{13}, r_{22}, r_{23}\)。~~（这里省略了，不太好算）~~

再证第二个式子。由于 \(x_1\) 与 \(z_0\) 相交，所以可以通过沿着 \(z_0\) 轴和 \(x_1\) 轴方向移动 \(o_1\)，使其与 \(o_0\) 重合。

\[ \begin{aligned} _1^0o &= a_i \cdot (^0x_1) + d_i \cdot z_0\\ &= a_i \cdot \begin{bmatrix}c_{\theta_i}\\ s_{\theta_i}\\ 0\end{bmatrix} + d_i \cdot \begin{bmatrix}0\\0\\1\end{bmatrix}\\ &= \begin{bmatrix}a_ic_{\theta_i}\\a_is_{\theta_i}\\d_i\end{bmatrix} \end{aligned} \]

这里的 \((^0x_1)\) 是旋转矩阵的第一列，表示了坐标系 1 的 \(x\) 方向上的单位向量在坐标系 0 中的坐标值。这里的 \(z_0\) 是坐标系 0 的 \(z\) 方向上的单位向量。

2.3 坐标系的配置¶

对 (0, 1, 2, …, n) 连杆，分别建立固连坐标系 (0, 1, 2, …, n)。

设置 \(z_i\) 轴为第 \(i+1\) 个关节的驱动轴（转动关节->转动轴，平动关节->移动轴）。（只要调整 \(\theta_i\) 和 \(\alpha_i\) 的值，就能实现 \(z_i\) 轴角度的任意配置。）
将基础坐标系（坐标系 0）的原点放在 \(z_0\) 的任意位置，再任意设置 \(x_0\) 和 \(y_0\)，形成右手系。
轴 \(z_{i-1}\) 和轴 \(z_{i}\) 不共面。此时，两轴之间存在唯一的最短线段。（记得 DH1、DH2 吗？这条最短线段不就是我们想要的 \(x_{i}\) 吗？）\(x_i\) 与 \(z_i\) 的交点就是坐标系 \(i\) 的原点 \(o_i\)。之后，选取 \(y_i\) 构成右手系即可。（叉乘的魅力！）
轴 \(z_{i-1}\) 和轴 \(z_i\) 平行。根据（DH1）和（DH2），此时 \(x_i\) 的方向是唯一确定的（在 \(z_{i-1}\) 和 \(z_i\) 构成的平面内且垂直于 \(z_{i-1}\)），但是原点 \(o_i\) 的位置不确定，因此可以任意选取。一种常见的方法是，选取穿过 \(o_{i-1}\) 的法线与 \(z_{i}\) 的交点为 \(o_i\)。这条法线就是 \(x_i\)，再选 \(y_i\) 构成右手系即可。
轴 \(z_{i-1}\) 和轴 \(z_i\) 相交。选择 \(x_i\) 垂直于 \(z_{i-1}\) 与 \(z_{i}\) 组成的平面。原点 \(o_i\) 可以自然地选成 \(z_{i-1}\) 与 \(z_{i}\) 的交点，也可以选成 \(z_{i}\) 上的任意一点。再选 \(y_i\) 构成右手系即可。
递推重复步骤3的操作，直至完成 (0, 1, 2, …, n-1) 连杆的坐标系配置。

以上步骤无法完成 n 号连杆/末端执行器的坐标系配置，因为 \(z_i|_{i_{max}} = z_{n-1}\)。

\(z_i\) 轴为第 \(i+1\) 个关节的驱动轴。

n 号连杆/末端执行器的坐标系称为末端执行器坐标系或工具坐标系。最常见的是，将原点\(o_n\)以对称方式布置在执行器（以夹持器为例）的中心，沿 \(x_n, y_n, z_n\) 三个方向的单位向量分别记为\(n, s, a\)。 (\(n\)：法线方向，normal；\(s\)：滑动方向，sliding；\(a\)：接近方向，approach)