机器人学中的空间描述与坐标变换

发布于 2023-11-17
更新于 2025-01-24
机器人学
机器人学教程系列

这方面的一些表述并没有一个业界规范，不同的教材会介绍几乎完全不同的的表述方式，这里以斯坦福大学约翰 克雷格《机器人学导论》（introduction to robotics : mechanics and control, [美] 约翰 J.克雷格（John Craig） / 贠超 / 机械工业出版社,第四版）为主要参考资料，其余为辅助参考资料。

参阅

描述

位置描述

坐标系A下，点P的坐标记作： ${}^AP$ ，左上角是坐标系。三维坐标系{A}内有点P，坐标分别为$p_x$, $p_y$，$p_z$，表示为：

$$ {}^AP = \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} \tag{1} $$

姿态描述

现有坐标系 {B} ，相对于坐标系 {A} 的描述可以表示物体的姿态。（例如A为世界坐标系，B为机器人末端执行器的基坐标系，可以描述出机器人末端执行器在世界坐标系中的姿态）
坐标系{B}主轴方向的单位矢量记为：$\hat{X}_B$，$\hat{Y}_B$，$\hat{Z}_B$，那么根据上文的公式（1）式就有就有：${}^A\hat{X}_B$，${}^A\hat{Y}_B$，${}^A\hat{Z}_B$，含义很明显了，注意这是三个单位矢量，三个$3×1$的列向量，现在把它们组合起来，有$3×3$的矩阵，这就是旋转矩阵，记作${}^A_BR$，有：

$$ {}^A_BR = ({}^A\hat{X}_B \quad {}^A\hat{X}_B \quad {}^A\hat{X}_B) \tag{2} $$

教材上做了说明，(2)式中矩阵的行恰好是{A}的单位矢量在{B}中的表达，因此${}^B_AR$可以求得：

$${}^B_AR={}^A_BR^T \tag{3} $$

这与线性代数中“正交矩阵的逆等于它的转置”一致，这里是几何上的证明。
显然，旋转矩阵各列的模均为1，这个结论在后面会用到。

位姿描述

位置和姿态总共4个矢量，合成一组，有位姿{B}:

$$ \{B\} = \{^A_BR ,\quad {}^AP_{BORG}\} $$

ORG指的是原点。如果位姿的位置矢量是0矢量，那么它就是旋转矩阵，如果位姿的旋转矩阵是单位阵，那么它实际上表示一个位置。

映射

坐标平移

若A 、 B姿态相同，仅仅位移不同，那么有：

$$ {}^AP = {}^BP + {}^AP_{BORG} \tag{4} $$

坐标旋转

由（3）式，已知${}^A_BR$和${}^BP$时，有：

$${}^AP = {}^A_BR\enspace{}^BP \tag{5} $$

一般变换

由上面的（4）、（5）两式有：

$${}^AP = {}^A_BR\enspace{}^BP + {}^AP_{BORG} \tag{6}$$

将上式改写为：

$${}^AP = {}^A_BT\enspace{}^BP \tag{7}$$

定义：

$$ \begin{pmatrix} {}^AP \\ 1 \end{pmatrix} = \begin{pmatrix} \def\arraystretch{1.5} \begin{array}{c:c} {}^A_BR & {}^AP_{BORG} \\ \hdashline {0\qquad 0\qquad 0} & 1 \end{array} \end{pmatrix}\begin{pmatrix} {}^BP \\ 1 \end{pmatrix} \tag{8} $$

为齐次变换矩阵。齐次变换矩阵在计算机图形学中，可以用于计算透视、放大等操作。

变换算子

平移算子

平移算子：矢量${}^AP_1$经过矢量${}^AQ_1$平移后变为${}^AP_2$：

$$ {}^AP_2 = {}^AP_1 + {}^AQ_1 $$

改写为：

$$ {}^AP_2 = D_{Q(q)}\enspace{}{}^AP_1 \tag{9} $$

其中，$q$是沿着矢量$\hat{Q}$方向平移的数量，有符号。算子$D_Q$可以被看成是一个特殊形式的齐次变换：

$$ D_{Q(q)} = \begin{pmatrix} 1 & 0 & 0 & q_x \\ 0 & 1 & 0 & q_y \\ 0 & 0 & 1 & q_z \\ 0 & 0 & 0 & 1 \end{pmatrix} $$

其中 $q_x$、$q_y$、$q_z$是平移矢量$Q$的分量，且$q = \sqrt{q^2_x + q^2_y + q^2_z}$

旋转算子

将一个矢量 ${}^AP_1$ 用旋转 $R$ 变换成一个新的矢量 ${}^AP_2$：

$$ {}^AP_2 =R \enspace{}^AP_1 $$

定义：

$$ {}^AP_2 =R_{K(\theta)} \enspace{}^AP_1 $$

符号 $R_{K(\theta)}$ 是一个旋转算子，表示绕 $\hat{K}$ 轴旋转 $\theta$ 角度，可以将它改写成齐次变换矩阵的形式，其中位置矢量为0。例如当绕 $z$ 轴旋转时有：

$$ R_{z(\theta)} = \begin{pmatrix} \cos\theta & -\sin\theta & 0 & 0 \\ \sin\theta & \cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix} $$

变换算子

变换算子 $T$ ：

$${}^AP_2 = T\enspace{}^AP_1 \tag{10} $$

关于变换的复合计算和变换方程，它们仅仅是一些简单的数学推导而已，不难，不写了。

其他姿态描述

$3\times3$ 的旋转矩阵，共有9个数字，正交阵的凯莱公式说明：对任何正交阵R存在一个反对称矩阵S，满足：

$$R = (I_3 -S)^{-1}(I_3 +S) \tag{11} $$

$I_3$ 是 $3\times3$ 的单位阵，三维反对称阵可以由3个参数确定。因此任何 $3\times3$ 的旋转矩阵都可以用3给参数确定。旋转矩阵的9个分量不是相互独立的，存在6个约束方程，此处细节略去不表。

X-Y-Z 固定角

有坐标系{B}与坐标系{A}重合，先使{B}绕 $\hat{X}_A$ 轴旋转$\gamma$角，再绕$\hat{Y}_A$ 轴旋转$\beta$角，最后绕$\hat{Z}_A$ 轴旋转$\alpha$角。

$$ {}^A_BR_{XYZ}(\gamma, \beta ,\alpha) = R_{Z(\alpha)}R_{Y(\beta)}R_{X(\gamma)} $$

懒得再敲公式了，找张图片来：

xyzfixed.pngxyzfixed.png

可以看出实际上只有3个变量，求解公式我就不放上来了。

Z-Y-X 欧拉角

有坐标系{B}与坐标系{A}重合，先使{B}绕 $\hat{Z}_B$ 轴旋转$\alpha$角，再绕变化后的$\hat{Y}_B$ 轴旋转$\beta$角，最后绕$\hat{X}_B$ 轴旋转$\gamma$角。
注意：每次轴都在变化。

xyzola.pngxyzola.png

此外还有Z-Y-Z欧拉角、等效轴-角法等描述方式，不逐一列出，重点是下面的欧拉参数和四元数。
一般称：绕Z轴旋转角为偏航角yaw；
绕旋转后的Y轴，俯仰角，pitch；
再绕旋转后的X轴，滚转角，roll

万向锁、奇异性问题，使得欧拉角不适于插值和迭代，往往只用于人机交互中。

欧拉参数与四元数

四元数的来历就不多写了，四元数是Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性 。

欧拉参数与四元数欧拉参数与四元数

四元数的平方和为1