空间描述与变换

阅读提示： 本文是基于《机器人学导论》第三版的学习笔记，除去书中内容，有一些自己的心得和理解。所以会对一些方面进行概括，长话短说。若需要专业解释强烈建议找到原书翻看定义，收获会很大！

2.1 位置、姿态和坐标系

要描述一个机械臂的姿态，我们应当明白，假设我们要描述手的姿态，容易想知对于指端来说，操作目标物体的轨迹应当在一个坐标系内被描述。然而，对于多元件的机械臂系统来说，仅仅靠这一个坐标系是不够的，甚至很混乱。所以我们应当设立一个参考系，然后建立操作手的坐标系，这样就能完全确定操作手的位置。

因此，点的位置可用矢量描述，物体的姿态可用坐标系描述。描述连体坐标系可用固定在物体上的坐标系来描述。

我们用参考系A的三个主轴单位矢量来描述连体坐标系B。设其为$X_A$,$Y_A$,$Z_A$，将这三个单位矢量排列成一个$3\times3$的矩阵，称其为旋转矩阵。它是一个相对表达，是坐标系B相对于坐标系A的表达，所以用符号$^AP$来表示。

由两个单位矢量的点积可得到二者之间夹角的余弦，所以，旋转矩阵的各分量也常被称为方向余弦。

进一步观察式子，

因此，可得：$^B_AR=^A_BR^T$。(重要结论)

进一步可得，旋转矩阵的逆矩阵等于它的转置：

$$^B_AR^A_BR^T=\left[\begin{matrix}^A\hat{X}^T_B\\^A\hat{Y}^T_B\\^A\hat{Z}^T_B\end{matrix}\right]\begin{pmatrix}^A\hat{X}^T_B&^A\hat{Y}^T_B&^A\hat{Z}^T_B\end{pmatrix}=I_3$$

实际上，一个正交阵的逆等于它的转置，这个结论也能很快证明上述结论。

$$^A_BR=^B_AR^{-1}=^B_AR^T$$

得到了位置和姿态，描述坐标系，需要一个原点。我们为方便起见，将物体上任选的一点作为连体坐标系的原点。于是我们将一个位置矢量和一个旋转矩阵组合在一起，描述一个坐标系：

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

其中$^AP_{BORG}$是确定坐标系原点的位置矢量。

特别地，如果参考系中的位置矢量是零矢量，那么它表示的就是姿态。

2.2 映射

映射描述从坐标系到坐标系的变换。

我们将讨论三种映射，并引入齐次变换矩阵这一重要工具。

2.2.1 平移坐标系的映射

这是最简单的一种映射，用矢量相加的方法表示：

$$^AP=^BP+^AP_{BORG}$$

2.2.2 旋转坐标系的映射

我们有之前所得的旋转矩阵的转置和逆的结论：

$$^A_BR=^B_AR^{-1}=^B_AR^T$$

一个旋转矩阵即为三个一组的列向量或者三个一组的行向量：

$$^A_BR=\begin{pmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{pmatrix}=\left[\begin{matrix}^B\hat{X}^T_A\\^B\hat{Y}^T_A\\^B\hat{Z}^T_A\end{matrix}\right]$$

因此，可知$^AP$的分量计算如下：

$$^Ap_x=^B\hat{X}_A\cdot^BP$$$$^Ap_y=^B\hat{Y}_A\cdot^BP$$$$^Ap_z=^B\hat{Z}_A\cdot^BP$$

写成简化形式：

$$^AP=^A_BR^BP$$

问题：如何表示坐标系B相对于坐标系A绕Z轴旋转30度？Z轴指向纸面向外。

先作图：

1.在A中写出B的单位矢量，排成旋转矩阵。

得：

稍微解释一下，我们知道第一列表示的是$^A\hat{X}_B$，取单位矢量$e_b$表示B的X轴在A中的位置，可由30°角得(0.866,0.5,0)，其中0表示相对于Z轴无移动。以此类推。

2.已知：

3.求出：

2.2.3 一般坐标系的映射

一般坐标系的映射可以概括为两步。

首先将$^BP$变换到一个中间坐标系，这个坐标系与A的姿态相同、原点与B的原点重合。这可以用左乘矩阵$^A_BR$得到。

然后用简单的矢量加法平移原点：

$$^AP=^A_BR^BP+^AP_{BORG}$$

更进一步地，我们可以将这组操作变换为使用一个矩阵算子表示从一个坐标系到另一个坐标系的映射。这个矩阵算子是$4\times4$的矩阵，将最后一行作为$[0, 0 ,0, 1]$，在4x1矩阵中添加最后一个分量为1。亦即：

将这个4x4的矩阵称为齐次变换矩阵。由于它可以表示坐标系间的变换，所以这是一个非常重要的概念！同时可以说，将来的运算将紧紧围绕齐次变换展开，请务必巩固相关知识。

2.3 算子：平移，旋转和变换

算子：用于坐标系间点的映射的通用数学表达式。

本章主要介绍平移算子，旋转算子和平移加旋转算子。

2.3.1 平移算子

实际上我们之前已经介绍了平移的矢量计算式。本节介绍将这种运算浓缩为一个算子，对算子的表示进行进一步探讨。

我们知道平移计算方法：

$$^AP_2=^AP_1+^AQ$$

用算子写出平移变换：

$$^AP_2=D_Q(q)^AP_1$$

算子$D_Q(q)$可被看做一个特殊的齐次变换阵：

由齐次变换矩阵分块可知，旋转矩阵是一个单位阵，原点阵的分量是平移矢量Q的分量。其中，$q=\sqrt{q_x^2+q^2_y+q^2_z}$。

现在我们初步认识了符号$D_Q(q)$，这个符号今后将用来表示坐标系和映射。

2.3.2 旋转算子

可以用旋转变换算子来定义旋转矩阵。

旋转矩阵不涉及两个具体的坐标系。所以，写的时候不需要写下标或上标。引入旋转矩阵$R$，这个已经比较熟悉的公式：

$$^AP_2=R^AP_1$$

用另一个符号定义旋转算子，以明确是绕哪个轴旋转：

$$^AP_2=R_K(\theta)^AP_1$$

符号$R_K(\theta)$是一个旋转算子，表示绕K轴旋转θ角度。这个算子可写成齐次变换矩阵。

代入到式子：

$$^A_BR=\begin{pmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{pmatrix}=\left[\begin{matrix}^B\hat{X}^T_A\\^B\hat{Y}^T_A\\^B\hat{Z}^T_A\end{matrix}\right]$$

可得：

$$R_K(\theta)=\left[\begin{matrix}cos\theta&-sin\theta&0&0\\sin\theta&cos\theta&0&0\\0&0&1&0\\0&0&0&1\end{matrix}\right]$$

再回到刚刚那个例子，也就是绕Z轴旋转30度。此时，旋转算子可写成：

2.3.3 变换算子

即平移加旋转的算子。一个变换通常被认为是由一个广义旋转矩阵和位置矢量分量组成的齐次变换的形式。

如下例：

$$^AP_2=T^AP_1$$

小结

这几节的核心就在于这个齐次变换矩阵。它有3个定义：

1. 它是坐标系的描述。

2. 它是变换映射。

3. 它是变换算子。

2.4 变换算法

本节介绍变换的乘法和变换的逆运算。

2.4.1 混合变换

我们有这样一个目标：即已知$^CP$，求$^AP$。

已知坐标系${C}$相对于坐标系$B$，并且已知坐标系$B$相对于坐标系$A$。所以，我们可将$^CP$变换成$^BP$：

$^BP=^B_CT^CP$

然后将$^BP$变换成$^AP$：

$$^AP=^A_BT^BP$$

联立上两式可以得到确定解。换言之，我们实际运算中，需要寻找ab和bc的这两个变换式，实现变换的传递。

由此定义：

$$^A_CT=^A_BT^B_CT$$

2.4.2 逆变换

由公式

$$^A_BT=^B_AT^{-1}$$

得若需要得到A相对于B的描述，需要求这个矩阵的逆。

利用变换的性质求逆：

为了求$^B_AT$，必须由$^A_BR$和$^AP_{BORG}$求出$^A_BT$和$^BP_{AORG}$。首先，回顾关于旋转矩阵的讨论：

$$^B_AR=^A_BR^T$$

其次利用

$$^AP=^A_BR^BP+^AP_{BORG}$$

将$^AP_{BORG}$转变成在B中的描述：

$$^B(^AP_{BORG})=^B_AR^AP_{BORG}+^BP_{AORG}$$

左边应为0，因为在A原点处离B原点的距离变换为在B原点处离B原点的距离就是0。所以变换后可得：

$$^BP_{AORG}=-^B_AR^AP_{BORG}=-^A_BR{^T}^AP_{BORG}$$

从而写出：

变换方程

回顾之前所学我们发现简单变换方程的构造依赖于三个坐标系所确定的两个变换式，那么多个坐标系也以此类推，使用的总是变换相乘的方法：

从而：

由这个变换方程我们可以解出：

从这个变换方程中我们可以得出C的两个可能的描述为：

和

例：假定已知上图中变换$^B_TT$描述了操作臂指端的坐标系T，它是相对于操作臂基座的坐标系B的，又已知工作台相对于操作臂基座的空间位置（因为已知与工作台相连的坐标系S是$^B_ST$），并且已知工作台上螺栓的坐标系相对于工作台坐标系的位置，即$^S_GT$。计算螺栓相对操作手的位姿，$^T_GT$。

由公式推导得到相对于操作手坐标系的螺栓坐标系为：

姿态的其他描述方法

旋转矩阵又被称为标准正交矩阵。由正交矩阵的凯莱公式可得，对于任何正交阵$R$，存在一个反对称矩阵$S$，满足：

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

式中是一个$3\times3$单位阵。一个3维反对称阵（即$S=-S^T$）可由三个参数$(s_x,s_y,s_z)$表示为

$$S=\left[\begin{matrix}0&-s_z&s_y\\s_z&0&-s_x\\-s_y&s_x&0\end{matrix}\right]$$

因此，任何3x3旋转矩阵都可以用三个参量确定。

显然，旋转矩阵的九个分量线性相关。实际上，对于一个旋转矩阵R很容易写出六个线性无关的分量。如上所述，假定R为三列：

由单位轴知识可知，三个矢量是参考坐标系中某坐标系的单位轴。每个矢量都是单位矢量，且相互垂直，所以9个矩阵元素有6个约束：

这些约束看起来非常臃肿，是否能够找到一种表达方法，用三个参量即可简便地表达？几个姿态表达法能够帮助我们做到这一点。

平移运动直观，但旋转变换往往并不直观，主要困难是旋转一般不互逆。即,$^A_BR^B_CR$与$^B_CR^A_BR$不同。频繁地输入九个元素的正交阵不是一件容易的工作，而我们引入这些姿态表达法则能简化运算。

X-Y-Z固定角

这种方法是，首先将坐标系B与一个已知参考坐标系A重合。先将B绕$\hat{X}^A$旋转$\gamma$角，再绕绕$\hat{Y}^A$旋转$\beta$角，最后绕$\hat{Z}^A$旋转$\alpha$角。

每个旋转都是绕着参考坐标系A的某个轴完成的。我们称这种方法为X-Y-Z固定角。有时，我们会称这些角为回转角、俯仰角和偏转角。但是使用中约定可能不同。

此时可直接推导等价旋转矩阵$^A_BR_{XYZ}(\gamma,\beta,\alpha)$：

其中$c\alpha$是$cos\alpha$的简写，$s\alpha$是$sin\alpha$的简写。

最重要的是搞清楚上式中的旋转顺序，将旋转看作算子，先X再Y后Z。

上式乘积后可得：

仅当旋转是按照上述顺序进行时，（2.64）才是正确的。这一点要万万记牢！

接着，不妨来看一组逆问题：

从一个旋转矩阵等价推出X-Y-Z固定角坐标系，这样的逆解取决于求解一组超越方程：如果方程相当于一个旋转矩阵，那么就有九个方程和三个未知量。在这九个方程中有六个方程是相关的，因此实际上只有三个方程和三个未知量。（超越方程：方程中有无法用自变数的多项式或开方表示的函数，常用近似解法去解，是目前计算数学中一个重要的问题）

令

由(2.64)式通过计算$r_{11}$和$r_{21}$的平方和的平方根，可求得$cos\beta$。然后用$-r_{31}$除以$cos\beta$再求其反正切可求得$\beta$。那么，只要$c\beta≠0$，就可以用$r_{21}/c\beta$除以$r_{11}/c\beta$，得到$\alpha$角，用$r_{32}/c\beta$除以$r_{33}/c\beta$，得到$\gamma$角，

即

式中$Atan2(y,x)$是一个双参变量的反正切函数。

虽然存在第二个解，但在上式中取$\beta$的正根以得到单解，满足$-90°≤\beta≤90°$。这样就可以在各种姿态表示法之间定义一一对应的映射函数。但是在某些情况下有必要求出所有的解，这在操作臂逆运动学那一章会讨论。如果$\beta=±90°$(即$c\beta=0$)，下式的解会退化。如此，只能求出$\alpha$和$\beta$的和或者差。这种情况下，一般取$\alpha=0.0$，结果如下：

如$\beta=90.0°$，解得：

如$\beta=-90.0°$，解得：

Z-Y-X欧拉角

有另外一种坐标系的表示法：首先将坐标系B与一个已知参考坐标系A重合。先将B绕$\hat{Z}^B$旋转$\alpha$角，再绕$\hat{Y}^B$旋转$\beta$角，最后绕$\hat{X}^B$旋转$\gamma$角。

同样都是重合后绕三个轴旋转，但是顺序有所不同。具体可以看图，所以运算上也出现比较大的不同，必须加以区分。这种方法叫做Z-Y-X欧拉角。

绕$\hat{Z}$轴旋转$\alpha$角使$\hat{X}$旋转到$\hat{X}^{'}$，$\hat{Y}$旋转到$\hat{Y}^{'}$，加撇号表明这是由欧拉角描述的旋转。

用中间坐标系$\{B^{'}\}$和$\{B^{''}\}$来表示$^A_BR_{Z^{'}Y^{'}X^{'}}(\alpha,\beta,\gamma)$。如果，把这些旋转看成是坐标系的描述，就可以立即写出：

$$^A_BR=^A_{B^{'}}R^{B^{'}}_{B^{''}}R^{B^{''}}_{B^{}}R$$

上式右边的每个旋转描述都是按照Z-Y-X欧拉角的定义给出的，即B相对于A的最终姿态为：

结果不太直观，但是与X-Y-Z固定角所得的结果相同。

Z-Y-Z欧拉角

先将B绕$\hat{Z}^B$旋转$\alpha$角，再绕$\hat{Y}^B$旋转$\beta$角，最后绕$\hat{Z}^B$旋转$\gamma$角。

按照上一节的推导，可得到等效矩阵

从旋转矩阵得出Z-Y-Z欧拉角介绍如下：

已知

$$^A_BR_{Z^{'}Y^{'}Z^{'}}(\alpha,\beta,\gamma)=\left[\begin{matrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{matrix}\right]$$

如$sin\beta≠0$，可得到

$$\beta=Atan2(\sqrt{r^2_{31}+r^2_{32}},r_{33})$$$$\alpha=Atan2(r_{23}/s\beta,r_{13}/s\beta)$$$$\gamma=Atan2(r_{32}/s\beta,-r_{31}/s\beta)$$

虽然存在第二个解（在式中取$\beta$的正平方根），但我们总是求满足$0.0≤\beta≤180.0°$的单解。如果$\beta=0.0$或$180.0°$，上式的结果就会退化。在这种情况下，仅能求出$\alpha$和$\gamma$的和或差。这种情况下一般取$\alpha=0.0$，结果如下：

其他角坐标系的表示法

总共有24种表示法，这三种是典型方法，都被称作角坐标系表示法。24种表示法中，12种为固定角表示法，另外12种为欧拉角坐标系法。二者有对偶性。

等效轴角坐标系表示法

这种方法是，首先将坐标系B和一个已知参考坐标系A重合，然后将B绕矢量$^A\hat{K}$按右手定则旋转$\theta$角。

矢量$\hat{K}$有时被称为有限旋转的等效轴。B相对于A的一般姿态可用$^A_BR(\hat{K},\theta)$或$R_K(\theta)$来表示，称作等效轴角坐标系表示法。确定矢量$^A\hat{K}$只需要两个参数，因为它的长度恒等于1。角度确定了第三个参数。经常用旋转量$\theta$乘以单位方向矢量$\hat{K}$形成一个简单的$3\times1$的矢量来描述姿态，用$K$表示（没有“帽号”），如上图所示。

当选择A的主轴中的一个轴作为旋转轴时，则等效旋转矩阵成为我们熟悉的平面旋转矩阵：

若旋转轴为一般轴，则等效旋转阵为：

对于任何旋转轴和任何角度，都能很容易地构造出等效旋转阵。

如此也产生了一个逆问题，也就是从一个给定的等效旋转阵求出$\hat{K}$和$\theta$。此处先给出一部分结果。

如果

然后

并且

由上式总可以计算出一个在0度到180度之间的$\theta$值。对于任意一对轴-角$(^A\hat{K},\theta)$，存在另一对轴-角，即$(-^A\hat{K},-\theta)$，它们在空间中的姿态相同，可用同样的旋转矩阵描述。因此，在将旋转矩阵转化为等效轴角坐标系表示法时，我们需要对解进行选择。一个更加重要的问题是，对于小角度的旋转，此时的轴将变得不确定。如果转动量$\theta$为0，那么旋转轴将完全无法确定，如果$\theta=0$或$180°时$，上式将无解。

例：坐标系B最初与坐标系A重合。我们使坐标系B绕矢量$\hat{K}=\left[\begin{matrix}0.707&0.70&70.0\end{matrix}\right]$（$^A\hat{K}$经过原点）旋转，转角$\theta=30°$，求坐标系B的描述。

代入式(2.80)可得坐标系描述的旋转矩阵分量。因为原点没有改变，所以位置矢量是$\left[\begin{matrix}0&0&0\end{matrix}\right]^T$。因此有

我们需要尽量讨论绕经过参考系原点的轴的旋转。若遇到的问题不属于这种情况时，我们可以定义另一个坐标系，该坐标系的原点在轴上，为此将这类问题简化为“经过原点的轴”的情况来解决，然后求解这个变换方程。

例：坐标系B最初与坐标系A重合。我们使坐标系B绕矢量$\hat{K}=\left[\begin{matrix}0.707&0.70&70.0\end{matrix}\right]$（此矢量经过点$^AP=\left[\begin{matrix}1.0&2.0&3.0\end{matrix}\right]$）旋转，转角$\theta=30°$，求坐标系B的描述。

在旋转之前，坐标系A和坐标系B是重合的。如上图所示，定义了两个新的坐标系A^'和B^'具有相同的方向，但它们的原点在旋转轴上，并且相对于坐标系A的原点有一定的偏移量。我们选择

同样，坐标系B相对于坐标系B^'的描述是

现在，保持其他关系不变，使坐标系B^'绕坐标系A^'旋转。因为旋转轴经过原点，所以用式(2.80)来计算坐标系B^'相对于A^'的旋转量。代入式（2.80）得到描述的旋转矩阵分量。又因为原点没有平移，所以位置矢量是$(0,0,0)^T$。因此，我们得到

我们可以用一个变换方程来计算要求的坐标系

$$^A_BT=^A_{A^{'}}T^{A^{'}}_{B^{'}}T^{B^{'}}_BT$$

从而得到

绕一个不经过原点的轴旋转会引起位置变化，另外加上姿态变化。该姿态变化跟旋转线轴通过原点引起的姿态变化一样。注意，我们可以使用任意定义的坐标系A^'和B^'，只要它们的原点选在旋转轴线上，旋转轴线方向的选择是任意的，而坐标系原点可以选择旋转轴线的任意位置。

欧拉参数

一种通过四个数值来表示的姿态表示法叫做欧拉参数。

定义：根据等效旋转轴$\hat{K}=\left[\begin{matrix}k_x& k_y&k_z\end{matrix}\right]^T$和等效旋转角$\theta$得到欧拉参数如下：

$$\epsilon_1=k_xsin\theta/2$$$$\epsilon_2=k_ysin\theta/2$$$$\epsilon_3=k_zsin\theta/2$$$$\epsilon_4=cos\theta/2$$

显然这四个参数不独立，并有关系式：

$$\epsilon_1^2+\epsilon_2^2+\epsilon_3^2+\epsilon_4^2=1$$

这个关系总是不变。

因此，一个姿态可描述为四维空间中单位超球面上的一点。

有时，可以将欧拉参数看做是一个$3\times1$的矢量加上一个标量。但是，虽然欧拉参数是一个$4\times1$的矢量，我们还是将欧拉参数看作是一个单位四元数。

用欧拉参数组表示的旋转矩阵$R_{\epsilon}$是

已知一个旋转矩阵得到对应的欧拉参数是

注意，从计算的意义上来讲，如果旋转矩阵表示的是绕某一轴$180°$的旋转，上式将失去意义，因为$\epsilon_4=0$。但可以看出，当取极限时，式（2.92）中的所有表达式都趋向于无穷大，对于上述情况也是如此。实际上从上式的定义中可以看出，对于所有的$\epsilon_i$，其值在[-1,1]间。

自由矢量的变换

这一章我们重点介绍了位置矢量，后面的章节主要对速度和力矢量进行讨论。

下面介绍两种很有用的矢量：

线矢量：指与作用线有关的矢量，其作用效果取决于矢量的大小和方向。通常情况下，力矢量的作用效果取决于力的作用线（或者是力的作用点），所以力矢量可看作线矢量。

自由矢量：指可能出现在空间任意位置的矢量，但它的性质没有改变。例如假定它的大小和方向保持不变。对于它的计算都是关于大小和方向的。

计算分析

在工业操作臂系统中常用的变换软件并不直接应用齐次变换方法，因为大量的时间会耗费在0和1的乘法运算上。

通常，我们左乘矩阵或进行矩阵计算来进行矢量变换，在不同的情况下需要选用不同的计算方法。