题图来自monicore | Pixabay

三维空间的刚体运动

本讲主要主要关注一个问题：一个刚体在三维空间中的运动是如何描述的。

为什么要研究这个问题？因为当我们描述机器人的位姿时，就是在描述一个刚体在三维空间中的运动。而且我们还要对这个估计出来的位姿进行进一步的优化。如何机器人的位姿的表述就决定了机器人的运动方程和优化方程的表述。

三维空间中，刚体的运动可以用两个概念来表示：旋转和平移。平移比较简单一些，一般用一个表示位移的向量来表示。而旋转则有多种表示方法，例如旋转矩阵、旋转向量等等，不同的表示方法各有优劣。

向量

在描述旋转矩阵前我们先明确向量这个概念。向量是空间中的一个具体实物且不和任何实数相关联。为了描述向量，应该先确定一个具体的坐标系，明确该坐标系的线性基  后才能够确定一个向量  在该坐标系下的坐标：

因此，向量坐标的具体取值，和向量本身和选取的坐标系相关。

明确的向量的坐标后，就可以谈论向量间的运算，主要有两种：内积和外积（又叫叉积）：

其中，符号  表示取该向量的反对称矩阵。

外积的计算结果仍然是一个向量，方向垂直于  和  , 大小为  . 值得注意的是，外积还可以表示向量的旋转。因为任何旋转都可以描述为：绕某个旋转轴顺时针/ 逆时针旋转多少角度。后面会在详细描述。

欧式变换

假设存在两个坐标系：一个世界坐标系，定义为某个墙角和它的三条边，并是一个惯性系；一个机器人坐标系，是一个随机器人移动的坐标系。假设机器人观察到了某个向量  , 它在这两个坐标系中分别有一套坐标。前面说了，向量是一个客观存在的实体，那么必然有一个关系能够将这两套坐标联系起来。

这个关系就是欧式变换。因为机器人的运动是一个刚体运动，所以同一个向量在不同坐标系下的模长和方向都不会发生变化。这样一个欧式变换就是由一个旋转和一个平移两部分组成。

旋转的描述

旋转矩阵

我们先考虑旋转。对于向量  ，它在两个坐标系下的表示应该是相等的：

等式右边同时左乘  可得

我们把中间的矩阵拿出来，定义成一个矩阵  ，它描述了旋转本身，又称为旋转矩阵，是由两组基之间的内积组成的。而且它是一个行列式为1的正交矩阵。它的逆（亦即转置）描述了一个相反的的旋转。

上述定义是一个充分必要条件，所以，我们可以将旋转矩阵的集合定义如下，称为特殊正交群：

旋转向量

旋转矩阵可以很好的描述相机的旋转了，但也有两个问题：

• 一次旋转只有3个自由度，但旋转矩阵却有9个量。显然这冗余了；
• 旋转矩阵自身带有两个约束。在进行估计和优化时，我们显然不想要这些附加的约束。

所以我们想要一个更紧凑的表达，最好就是3个量。而旋转向量就可以。

前面提到，任意旋转都可以用一个旋转轴和一个旋转角来描述。那么，我们可以用一个向量，其方向为旋转轴  的方向，大小则为旋转角  ，这种向量就是旋转向量，表示为  . 前面说到，外积可以用来表示旋转就是因为外积可以用来表示旋转向量：考虑两个不平行的向量  和  ，在右手法则下，用右手的4个指头从  转向  ，大拇指的朝向就是旋转向量的方向，即  的方向，而它的大小则由  和  的夹角决定。

旋转向量和旋转矩阵的转换可以用罗德里格斯公式表示：

反之则有

所以

而旋转轴上的向量在旋转后不发生改变，所以：

说明  是矩阵  的特征值 1 所对应的特征向量。求解后并归一化则可得到  .

欧拉角

除了旋转向量，我们也可以用欧拉角来紧凑地描述旋转。一个旋转可以分解成3次分别绕X，Y，Z 轴的旋转来表示，在航空摄影测量中，一般用“翻转 - 航偏 - 俯仰”（roll - yaw - pitch)，也即XZY来表示。先绕X 轴旋转roll 角度，再绕Y 轴旋转yaw 角度，最后按照Z 轴旋转pitch 角度。这三个旋转矩阵相乘就得到了总的旋转矩阵。

此时，就可以用  这三个量来描述任意旋转，这种表示方式会比其他方式更直观、更易理解。

四元数

既然我们已经有了旋转向量和欧拉角，为什么还有个四元数（Quaternion）？因为欧拉角和旋转向量具有奇异性（万向锁问题）。不存在不带有奇异性的三维向量描述方式。因此我们需要用到四元数，它既是紧凑的，也没有奇异性。

定义：一个四元数包含一个实部和三个虚部。

其中，后面的等式将四元数表达成一个标量和一个向量，  表示四元数的三个虚部，满足：

若一个四元数虚部全为0，则它是一个实四元数；如其实部为零，则称它为虚四元素。而且，一个虚四元数对应一个空间点。

我们能用单位四元数来表示三维空间中的任意一个旋转。我们先考虑下复数。在负数中，乘以i 表示在复平面内旋转90度。但在四元数中，情形却有微妙的不同：乘以i 表示旋转180度，这样才能保证  的性质。而  ，说明绕i 轴旋转360度后得到一个相反的东西，而要旋转720度（两周）才能得到它原先的样子。（？）

假设某个旋转的旋转向量为  , 则

反之则有

上式给人一种“转了一半”的感觉。将上式中的  加上  后得到一个相同的旋转，但是对应的四元数却变成了  . 所以，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。

而四元数和旋转矩阵的关系为：

设矩阵  , 则由上式可以推得：

NOTE: 由于  和  表示同一个旋转，所以一个旋转矩阵对应的四元数表示并不惟一且存在其他转换公式。在实际中，如果  接近于0，会造成其他3个数的解不稳定，应采用其他公式。

四元数的运算

这里只列出关于四元数的简单运算。

现有两个四元数  , 向量表示分别为  。

• 加减法

• 乘法

乘法将两个四元数的每一项两两相乘然后相加。注意其满足之前所述的四元数的性质。

该乘法定义下，两个实四元数的乘积仍然是实四元数。由于最后一项外积的存在，四元数的乘法不满足交换律，除非  和  在  中共线（此时外积为零）。

• 共轭

共轭即将虚部取为相反数：

四元数共轭与其自身相乘可得一个实四元数，实部为模长的平方。

• 模长

可知，两个四元数乘积的模即为二者模的乘积。所以，单位四元数相乘后仍然得到单位四元数。

• 逆

可见，对于单位四元数，其逆和共轭相同。按照定义，四元数和自己逆的乘积为实四元数  . 最后，四元数乘积的逆有和矩阵相乘相似的性质：

• 数乘和点乘

四元数的数乘与点乘与向量相同：

用四元数表示旋转

要用四元数来计算旋转，首先，用一个虚四元数来表示三维空间点：

则旋转后的点  则为

变换矩阵

解决了旋转问题，就可以考虑平移了。平移可以简单地用一个平移向量  来表示。则变换后的坐标为

但是，上面这个式子并不是线性的。所以我们引入了齐次坐标。在一个三维向量末尾加上一个1，就得到一个四维向量，成为齐次坐标。上式就可以写成

类似旋转矩阵  ，变换矩阵  可以组成一个集合，叫特殊欧式群

最后，连发了两片之后才发现公式没有附上来。只好又在修改了一番。修改完后又发现知乎的编辑器的公式只能偶尔居中，然后还没有编号。后面的文章常常会写到 式(10) 之类的。只能在想办法了。