SLAM十四讲（二）——变换矩阵、欧拉变换、四元数

一、旋转矩阵、变换矩阵

此知识点见我知乎文章：https://zhuanlan.zhihu.com/p/226132779

不再赘述

二、欧拉变换

1. 旋转向量

应用前面的知识，旋转矩阵可以描述旋转、变换矩阵可以描述一个6自由度的三位刚体运动，但是用这种矩阵表示方式至少有以下两个缺点：

1. SO(3)的旋转矩阵有9个量，但一次旋转只有3个自由度。显然，表达方式冗余。
2. 旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。使得估计或优化时，求解困难。

任意旋转都可以用一个旋转轴和一个旋转角来刻画，即只需一个三维向量即可描述旋转轴。

旋转向量：方向与旋转轴一致，长度等于旋转角。

旋转向量($\theta \mathbf{n}$)($\mathbf{n}$为单位向量，$\theta$ 为旋转角度)到旋转矩阵的转换过程由罗德里格斯公式推出：

$$\mathbf{R}=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{n}{\mathbf{n}}^{\mathbf{T}}+sin\theta \hat{\mathbf{n}}$$

从旋转矩阵到旋转向量的转换：

$$\theta =arccos\frac{tr(\mathbf{R})-1}{2}$$

$$\mathbf{R}\mathbf{n}=\mathbf{n}$$

因此，转轴$\mathbf{n}$是矩阵$\mathbf{R}$特征值1对应的特征向量

2. 欧拉角

使用3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。存在XYZ、ZYZ、ZYX等形式。

欧拉角当中比较常见的一种，使用"偏航-俯仰-滚转（yaw-pitch-raw）"3个角度来描述一个旋转。

• 绕物体的Z轴旋转，得到偏航角yaw
• 绕旋转之后的Y轴旋转，得到俯仰角pitch
• 绕旋转之后的X轴旋转，得到滚转角roll

欧拉角不适用于插值和迭代，往往只用于人机交互，很少在SLAM中直接使用欧拉角表达姿态。

（存在万向锁问题）

三、四元数

1. 定义

旋转矩阵用9个量描述3自由度的旋转，具有冗余性 ；欧拉角和旋转向量是紧凑的，但是具有奇异性。所以Hamilton找到一种扩展的复数，既是紧凑的，也没有奇异性——四元数。三维旋转可以由单位四元数来描述。

一个四元数$\mathbf{q}$拥有一个实部和三个虚部：

$$\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$$

其中，三个虚部满足以下关系式：

$$\{\begin{array}{c}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$

有时，人们也用一个标量和一个向量来表达四元数：

$$q=[s,v{]}^T,s=q_0\in \mathbb{R},v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$$

• 单元四元数

  $s=0,v$为单位向量的四元数: $\hat{q}=[0,\hat{v}]$

可以用单位四元数表示三维空间中的任意一个旋转(为了使旋转后该点的模不变，或者说到原点的距离不变)

2. 运算

For:

$q_a=s_a+x_{a}i+y_{a}j+z_{a}k$

$q_b=s_b+x_{b}i+y_{b}j+z_{b}k$

• 加法和减法

$$q_a\pm q_b=[s_a\pm s_b,v_a\pm v_b{]}^T$$

• 乘法

$$q_a{q}_b=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b{]}^T$$

• 模长

$$\Vert \begin{array}{c}{q}_a\end{array}\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$

$$\Vert \begin{array}{c}{q}_a{q}_b\end{array}\Vert =\Vert \begin{array}{c}{q}_a\end{array}\Vert \Vert \begin{array}{c}{q}_b\end{array}\Vert$$

• 共轭

$$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-v_a{]}^T$$

$$q^{\ast }q=q{q}^{\ast }=[s_a^2+v^{T}v,0{]}^T$$

• 逆

$$q^{-1}=q^{\ast }/{\Vert \begin{array}{c}q\end{array}\Vert }^2$$

$$q{q}^{-1}=q^{-1}q=1$$

$$(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1}$$

• 数乘

$$kq=[ks,kv{]}^T$$

3. 用四元数表示旋转

假设有一个空间三维点$p=[x,y,z]\in {\mathbb{R}}^3$，以及一个由单位四元数$q$指定的旋转。三维点$p$经过旋转之后变为$p^{\prime }$。如果用矩阵描述，那么有$p^{\prime }=Rp$。如果用四元数描述，则：

首先，$p$可用一个虚四元数来描述：$p=[0,x,y,z{]}^T=[0,v{]}^T$

则有：(此处左乘单位四元数，右乘其共轭，怎么推还不知道，汉密尔顿提出来的)

$$p^{\prime }=qp{q}^{-1}$$

这个运算形式是为了限制其运算结果所在的空间，左乘单位四元数，右乘其共轭的运算保证了结果是一个在三维超平面上中的纯四元数。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-8Anl1sQM-1636978339440)(https://s3-us-west-2.amazonaws.com/secure.notion-static.com/1d0aa0bb-1340-4858-a418-4dbc334c2f70/Untitled.png)]

最后把$p^{\prime }$的虚部取出，即得旋转之后点的坐标。

参考：https://blog.csdn.net/linyijiong/article/details/79777399

4. 四元数到其他旋转表示的转换

设$q=[s,v{]}^T$

• 从四元数到旋转矩阵的变换

  $$R=v{v}^T+s^{2}I+2s\hat{v}+(\hat{v}{)}^2$$

  其中，$\hat{v}$是$v$的反对称矩阵，即：

  $$\hat{v}=\left[\begin{array}{ccc}0& -v_3& v_2\\ v_3& 0& -v_1\\ -v_2& v_1& 0\end{array}\right]$$

• 从四元数到旋转向量的转换

  $$\{\begin{array}{c}\theta =2arccos{q}_0\\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}\end{array}$$

推导见《视觉SLAM十四讲》P59-60.