半闲居士视觉SLAM十四讲笔记（3）三维空间刚体运动 - part 3 旋转向量、欧拉角、四元数

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/youngpan1101/article/details/71086851

本系列文章由 youngpan1101 出品，转载请注明出处。
文章链接： http://blog.csdn.net/youngpan1101/article/details/71086851
作者：宋洋鹏（youngpan1101）
邮箱： [email protected]

---

该讲详细资料下载链接 【Baidu Yun】【Video】【Code】

若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】
若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】
若您觉得本博文对您有帮助，请支持高博的新书《视觉SLAM十四讲》，【点击购买】

---

三维空间的刚体运动描述方式

旋转向量

• 旋转矩阵表示方法的$\color{Red}{缺点}$
• 旋转矩阵用 9 个量表达了 3 自由度的变换，该表达方式是${\color{Red}{冗余}}$的（编程会浪费存储空间）。
• 旋转矩阵自身为正交矩阵且行列式为 1 的这些${\color{Red}{约束}}$会给矩阵的估计或优化带来困难。
• 旋转向量（亦称轴角，Axis-Angle）表示三维旋转
  
  • 这种表示法只需一个三维向量即可，该表示方法没有约束。
  • 旋转轴为 ${\boldsymbol n}$ ，角度为 $\theta$ 的旋转对应的旋转向量为
    $${\boldsymbol w} = \theta \; {\boldsymbol n} \tag{3.11}$$
    旋转矩阵 和 旋转向量 的转换关系：
    
    • $\color{Red} {旋转向量 \Rightarrow 旋转矩阵}$（由罗德里格斯公式（Rodrigues’s Formula）表明）：
      $${\boldsymbol R} = cos\theta \cdot {\boldsymbol I} + (1 - cos\theta) {\boldsymbol n} {\boldsymbol n}^{T} + sin\theta \cdot {\boldsymbol n}^{\land} \tag{3.12}$$
      式 (3.12) 的符号 $^{\land}$ 是向量到反对称矩阵的转换符。
    • $\color{Red} {旋转矩阵 \Rightarrow 旋转向量}$：
      
      • 对于转角 $\theta$，有：
        $${\rm tr} ({\boldsymbol R}) = cos\theta \cdot {\rm tr}({\boldsymbol I}) + (1 - cos\theta) {\rm tr} ({\boldsymbol n} {\boldsymbol n}^{T}) + sin\theta \cdot {\rm tr} ({\boldsymbol n}^{\land}) = 1 + 2cos\theta \tag{3.13}$$
        所以：
        $$\theta = arccos\left( \frac{ {\rm tr}({\boldsymbol R}) - 1 } { 2 } \right) \tag{3.14}$$
      • 由于旋转轴上的向量在旋转后不会发生变化，有
        $${\boldsymbol R} {\boldsymbol n} = 1 \cdot {\boldsymbol n} \tag{3.15}$$
        所以转轴 ${\boldsymbol n}$ 是矩阵 ${\boldsymbol R}$ 特征值 1 对应的特征向量，求解此方程，再归一化，即可得到旋转轴。
• 旋转向量 和 旋转矩阵 只是表达方式不同，表达的东西是一样的。

---

——————————– 分割线<< $\color{Green}{家有小武，如有一母}$ >>分割线 ——————————–

---

欧拉角

1. 旋转矩阵、旋转向量很难让人直观地知道物体是如何进行旋转的。
2. 欧拉角提供了一种非常直观的方式来描述旋转，它使用三个分离的转角，即$\color{Red} {将一个旋转分解成三次饶不同轴的旋转}$。
3. ZYX 转角相当于把任意旋转分解成以下三个轴上的转角
   
   • 绕物体的 Z 轴 旋转，得到 偏航角 yaw
   • 绕旋转之后的 Y 轴 旋转，得到 俯仰角 pitch
   • 绕旋转之后的 X 轴 旋转，得到 滚转角 roll

yaw-pitch-roll	yaw	pitch	roll

$\quad \;$4. 欧拉角最大的缺点：万向锁（Gimbal Lock）

• ZYX 顺序中，若 Pitch 为 正负90度，则第三次旋转和第一次绕同一个轴，使得系统丢失了一个自由度 —— 存在 奇异性 问题。
  
• 不适于插值和迭代，一般只用于人机交互中。
• 用三个实数来表达三维旋转时，会不可避免地碰到奇异性问题。
• SLAM 程序中很少直接使用欧拉角表达姿态。

---

——————————– 分割线<< $\color{Green}{家有小武，如有一母}$ >>分割线 ——————————–

---

四元数

旋转矩阵	欧拉角、旋转向量	四元数
冗余性	紧凑，奇异性	紧凑，$\color{Red} {无奇异性}$

• 四元数是一种扩展的复数（复数可表达二维平面的旋转）。
• 四元数可以表达三维空间旋转。
• 四元数 ${\boldsymbol q}$ 由 一个实部 和 三个虚部 组成：
  $${\boldsymbol q} = q_{0} + q_{1}i + q_{2}j + q_{3}k \tag{3.16}$$
  式 (3.16) 中 $i,j,k$ 为四元数的三个虚部。虚部满足以下关系：
  $$\begin{cases} i^{2} = j^{2} = k^{2} = -1 \\ ij = k, ji = -k \\ jk = i, kj = -i \\ ki = j, ik = -j \\ \end{cases} \tag{3.17}$$
• 四元数的表示方法：
  ${\boldsymbol q} = [s, {\boldsymbol v}], s = q_{0} \in {\mathbb R} , {\boldsymbol v} = [q_{1}, q_{2}, q_{3}]^{T} \in {\mathbb R}^{3}$ （$s$ 为四元数的实部， ${\boldsymbol v}$ 为四元数的虚部）
• 四元数的运算

运算名称	公式
加法、减法	${\boldsymbol q}_{a} \pm {\boldsymbol q}_{b} = [s_{a} \pm s_{b}, {\boldsymbol v}_{a} \pm {\boldsymbol v}_{b}]$
乘法	1) ${\boldsymbol q}_{a} {\boldsymbol q}_{b} = s_{a}s_{b} - x_{a}x_{b} - y_{a}y_{b} - z_{a}z_{b} \\ \qquad \; \; \; + (s_{a}x_{b} + x_{a}s_{b}+y_{a}z_{b}-z_{a}y_{b})i \\ \qquad \; \; \; + (s_{a}y_{b}-x_{a}z_{b}+y_{a}s_{b}+z_{a}x_{b})j \\ \qquad \; \; \; + (s_{a}z_{b}+x_{a}y_{b}-x_{b}y_{a}+z_{a}s_{b})k \\ $ 2) 写成向量形式： ${\boldsymbol q}_{a} {\boldsymbol q}_{b} = [s_{a} s_{b} - {\boldsymbol v}_{a}^{T} {\boldsymbol v}_{b}, s_{a}{\boldsymbol v}_{b} +s_{b}{\boldsymbol v}_{a} + {\boldsymbol v}_{a} \times {\boldsymbol v}_{b}]$ 3) 乘法通常是不可交换的，除非 ${\boldsymbol v}_{a} 和 {\boldsymbol v}_{b}$ 在 ${\mathbb R}^{3}$ 中共线，那么外积项为零
共轭	1) ${\boldsymbol q}_{a}^{*} = s_{a} - x_{a}i - y_{a}j - z_{a}k = [s_{a}, -{\boldsymbol v}_{a}]$ 2) ${\boldsymbol q}^{*} {\boldsymbol q} = {\boldsymbol q} {\boldsymbol q}^{*} = [s_{a}^{2} + {\boldsymbol v}^{T} {\boldsymbol v}, {\boldsymbol 0}]$
模长	1) $||{\boldsymbol q}_{a}|| = \sqrt{s_{a}^{2}+x_{a}^{2}+y_{a}^{2}+z_{a}^{2}} \\ $ 2) $||{\boldsymbol q}_{a} {\boldsymbol q}_{b}|| = ||{\boldsymbol q}_{a}|| \ ||{\boldsymbol q}_{b}||$
逆	1) ${\boldsymbol q}^{-1} = \frac{{\boldsymbol q}^{*}}{||{\boldsymbol q}||^{2}} \\ $ 2) 四元数和自身的逆的乘积为实四元数的 ${\boldsymbol 1}$ ： ${\boldsymbol q} {\boldsymbol q}^{-1} = {\boldsymbol q}^{-1} {\boldsymbol q} = {\boldsymbol 1}$ 3) 如果 ${\boldsymbol q}$ 为单位四元数，逆和共轭就是同一个量 4) 乘积的逆： $({\boldsymbol q}_{a} {\boldsymbol q}_{b})^{-1} = {\boldsymbol q}_{b}^{-1} {\boldsymbol q}_{a}^{-1}$
数乘	$k {\boldsymbol q} = [ks, k {\boldsymbol v}]$
点乘	${\boldsymbol q}_{a} \cdot {\boldsymbol q}_{b} = s_{a}s_{b} + x_{a}x_{b}i + y_{a}y_{b}j + z_{a}z_{b}k$

• 四元数和角轴的关系
  假设某个旋转是绕单位向量 ${\boldsymbol n} = [n_{x}, n_{y}, n_{z}]^{T}$ 进行了角度为 $\theta$ 的旋转
  
  • $\color{Red} {角轴 \Rightarrow 四元数}$
    $${\boldsymbol q} = \left[ cos\frac{\theta}{2}, n_{x} sin\frac{\theta}{2}, n_{y} sin\frac{\theta}{2}, n_{z} sin\frac{\theta}{2} \right] ^{T} \tag{3.18}$$
    式 (3.18) 的 $\theta$ 加上 $2\pi$，得到一个相同的旋转，其对应的四元数变成了 $-{\boldsymbol q}$，即 任意的旋转都可以由两个互为相反数的四元数表示。
  • $\color{Red} {四元数 \Rightarrow 角轴}$
    $$\begin{cases} \theta = 2 \ arccos \ q_{0}\\ [n_{x}, n_{y}, n_{z}]^{T} = [q_{1}, q_{2}, q_{3}]^{T} / sin\frac{\theta}{2} \end{cases} \tag{3.19}$$
• 四元数和旋转矩阵的关系
  
  • $\color{Red} {四元数 \Rightarrow 旋转矩阵}$
    设四元数 ${\boldsymbol q} = q_{0}+q_{1}i+q_{2}j+q_{3}k$，对应的旋转矩阵为
    $${\boldsymbol R} = \begin{bmatrix} 1-2q_{2}^{2}-2q_{3}^{2} & 2q_{1}q_{2} + 2q_{0}q_{3} & 2q_{1}q_{3} - 2q_{0}q_{2} \\ 2q_{1}q_{2} - 2q_{0}q_{3} & 1-2q_{1}^{2}-2q_{3}^{2} & 2q_{2}q_{3}+2q_{0}q_{1} \\ 2q_{1}q_{3}+2q_{0}q_{2} & 2q_{2}q_{3} - 2q_{0} q_{1} & 1 - 2q_{1}^{2} - 2q_{2}^{2} \\ \end{bmatrix} \tag{3.20}$$
  • $\color{Red} {旋转矩阵 \Rightarrow 四元数}$
    设旋转矩阵 ${\boldsymbol R} = \left\{ m_{ij} \right\}, i,j \in [1,2,3]$，对应的四元数为
    $$q_{0} = \frac{\sqrt{{\rm tr}({\boldsymbol R})+1}}{2}, q_{1} = \frac{m_{23}-m_{32}}{4q_{0}}, q_{2} = \frac{m_{31} - m_{13}}{4q_{0}}, q_{3} = \frac{m_{12}-m_{21}}{4q_{0}} \tag{3.21}$$
• 用四元数旋转一个空间点
  将三维空间的点用虚四元数来描述：${\boldsymbol p} = [0, x, y, z] = [0, {\boldsymbol v}]$，
  点 ${\boldsymbol p}$ 经过一次以四元数 ${\boldsymbol q}$ 表示的旋转后，得到了 ${\boldsymbol p}'$ （${\boldsymbol p}'$ 为纯虚四元数）：
  $${\boldsymbol p}' = {\boldsymbol q} {\boldsymbol p} {\boldsymbol q}^{-1} \tag{3.22}$$

---

——————————– 分割线<< $\color{Green}{家有小武，如有一母}$ >>分割线 ——————————–

---

旋转矩阵、欧拉角、四元数比较

任务/性质	旋转矩阵	欧拉角	四元数
在坐标系间旋转点	能	不能（必须转换到矩阵）	不能（必须转换到矩阵）
连接或增量旋转	能，但经常比四元数慢，小心矩阵蠕变的情况	不能	能，比矩阵快
插值	基本上不能	能，但可能遭遇万向锁或其他问题	Slerp 提供了平滑插值
易用程度	难	易	难
存储内存	9个数	3个数	4个数
对给定方位的表达方式是否唯一	是	不是，有无数多种方法	不是，有两种方法，两者为相反数
可能导致非法	矩阵蠕变	任意三个数都能构成合法的欧拉角	可能会出现误差积累，从而产生非法的四元数

ps: 以上表格引自 旋转矩阵、欧拉角、四元数比较