正运动学¶

参考书目¶

《机器人建模与控制》[美] 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) $$} = \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$角度。

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

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\ &= \begin{bmatrix}a_ic_{\theta_i}\a_is_{\theta_i}\d_i\end{bmatrix} \end{aligned}}\ s_{\theta_i}\ 0\end{bmatrix} + d_i \cdot \begin{bmatrix}0\0\1\end{bmatrix

$$

这里的$(^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)