题图来自monicore | Pixabay
三维空间的刚体运动
本讲主要主要关注一个问题:一个刚体在三维空间中的运动是如何描述的。
为什么要研究这个问题?因为当我们描述机器人的位姿时,就是在描述一个刚体在三维空间中的运动。而且我们还要对这个估计出来的位姿进行进一步的优化。如何机器人的位姿的表述就决定了机器人的运动方程和优化方程的表述。
三维空间中,刚体的运动可以用两个概念来表示:旋转和平移。平移比较简单一些,一般用一个表示位移的向量来表示。而旋转则有多种表示方法,例如旋转矩阵、旋转向量等等,不同的表示方法各有优劣。
向量
在描述旋转矩阵前我们先明确向量这个概念。向量是空间中的一个具体实物且不和任何实数相关联。为了描述向量,应该先确定一个具体的坐标系,明确该坐标系的线性基
因此,向量坐标的具体取值,和向量本身和选取的坐标系相关。
明确的向量的坐标后,就可以谈论向量间的运算,主要有两种:内积和外积(又叫叉积):
其中,符号
外积的计算结果仍然是一个向量,方向垂直于
欧式变换
假设存在两个坐标系:一个世界坐标系,定义为某个墙角和它的三条边,并是一个惯性系;一个机器人坐标系,是一个随机器人移动的坐标系。假设机器人观察到了某个向量
这个关系就是欧式变换。因为机器人的运动是一个刚体运动,所以同一个向量在不同坐标系下的模长和方向都不会发生变化。这样一个欧式变换就是由一个旋转和一个平移两部分组成。
旋转的描述
旋转矩阵
我们先考虑旋转。对于向量
等式右边同时左乘
我们把中间的矩阵拿出来,定义成一个矩阵
上述定义是一个充分必要条件,所以,我们可以将旋转矩阵的集合定义如下,称为特殊正交群:
旋转向量
旋转矩阵可以很好的描述相机的旋转了,但也有两个问题:
- 一次旋转只有3个自由度,但旋转矩阵却有9个量。显然这冗余了;
- 旋转矩阵自身带有两个约束。在进行估计和优化时,我们显然不想要这些附加的约束。
所以我们想要一个更紧凑的表达,最好就是3个量。而旋转向量就可以。
前面提到,任意旋转都可以用一个旋转轴和一个旋转角来描述。那么,我们可以用一个向量,其方向为旋转轴
旋转向量和旋转矩阵的转换可以用罗德里格斯公式表示:
反之则有
所以
而旋转轴上的向量在旋转后不发生改变,所以:
说明
欧拉角
除了旋转向量,我们也可以用欧拉角来紧凑地描述旋转。一个旋转可以分解成3次分别绕X,Y,Z 轴的旋转来表示,在航空摄影测量中,一般用“翻转 - 航偏 - 俯仰”(roll - yaw - pitch),也即XZY来表示。先绕X 轴旋转roll 角度,再绕Y 轴旋转yaw 角度,最后按照Z 轴旋转pitch 角度。这三个旋转矩阵相乘就得到了总的旋转矩阵。
此时,就可以用
四元数
既然我们已经有了旋转向量和欧拉角,为什么还有个四元数(Quaternion)?因为欧拉角和旋转向量具有奇异性(万向锁问题)。不存在不带有奇异性的三维向量描述方式。因此我们需要用到四元数,它既是紧凑的,也没有奇异性。
定义:一个四元数包含一个实部和三个虚部。
其中,后面的等式将四元数表达成一个标量和一个向量,
若一个四元数虚部全为0,则它是一个实四元数;如其实部为零,则称它为虚四元素。而且,一个虚四元数对应一个空间点。
我们能用单位四元数来表示三维空间中的任意一个旋转。我们先考虑下复数。在负数中,乘以i 表示在复平面内旋转90度。但在四元数中,情形却有微妙的不同:乘以i 表示旋转180度,这样才能保证
假设某个旋转的旋转向量为
反之则有
上式给人一种“转了一半”的感觉。将上式中的
而四元数和旋转矩阵的关系为:
设矩阵
NOTE: 由于
四元数的运算
这里只列出关于四元数的简单运算。
现有两个四元数
- 加减法
- 乘法
乘法将两个四元数的每一项两两相乘然后相加。注意其满足之前所述的四元数的性质。
该乘法定义下,两个实四元数的乘积仍然是实四元数。由于最后一项外积的存在,四元数的乘法不满足交换律,除非
- 共轭
共轭即将虚部取为相反数:
四元数共轭与其自身相乘可得一个实四元数,实部为模长的平方。
- 模长
可知,两个四元数乘积的模即为二者模的乘积。所以,单位四元数相乘后仍然得到单位四元数。
- 逆
可见,对于单位四元数,其逆和共轭相同。按照定义,四元数和自己逆的乘积为实四元数
- 数乘和点乘
四元数的数乘与点乘与向量相同:
用四元数表示旋转
要用四元数来计算旋转,首先,用一个虚四元数来表示三维空间点:
则旋转后的点
变换矩阵
解决了旋转问题,就可以考虑平移了。平移可以简单地用一个平移向量
但是,上面这个式子并不是线性的。所以我们引入了齐次坐标。在一个三维向量末尾加上一个1,就得到一个四维向量,成为齐次坐标。上式就可以写成
类似旋转矩阵
最后,连发了两片之后才发现公式没有附上来。只好又在修改了一番。修改完后又发现知乎的编辑器的公式只能偶尔居中,然后还没有编号。后面的文章常常会写到 式(10) 之类的。只能在想办法了。