四元数公式推导

@[toc]

在看PhysX源码的时候，看到四元数公式，想知道怎么推导过来的，因为网上一大片帖子都是直接写上这个公式。最主要是纠结公式中角度的一半是如何来的

公式

参考[2] 10.4.3

推导

参考[6]直接给出了证明，下面自己推导一遍

三维空间旋转公式

先抛开四元数，我们去求一个三位空间向量绕固定轴旋转之后的向量的公式是什么？也就是参考[2]书中提到的轴-角式旋转。而四元数是跟轴-角式的旋转相关的，先以这个为基础，再去推导四元数的公式会容易理解一些。 和参考[6]一样，用的右手坐标系，主要是用的绘图软件[9]，右手坐标系好看点。

设定：

• 任意向量$\pmb{v}$(向量用粗体小写字母表示)
• 绕经过原点的旋转轴$\pmb{u} = {(x, y, z)}^T$
• 右手坐标系，绕向量$pmb{u}$从箭头到原点方向逆时针旋转角度$\theta$
• $\pmb{u}$是单位向量：$\Vert{\pmb{u}}\Vert = \sqrt{x^2 + y ^2 + z^2} = 1$

为了简单起见，看下图所示

1.向量分解

将$\pmb{v}$分解为平行于$\pmb{u}$的$\pmb{v} {\Vert}$，和垂直于$\pmb{u}$的$\pmb{v} {\bot}$

$\pmb{v} = \pmb{v}_{\Vert} + \pmb{v}_{\bot}$ 分别旋转这两个分向量，再把最后旋转的两个分量相加，得到旋转后的向量$\pmb{v}^{‘}$ $\pmb{v}^{'} = {\pmb{v}_{\Vert}}^{'} + {\pmb{v}_{\bot}}^{'}$

如下图所示： 根据正交投影公式

$\begin{aligned} \pmb{v}_{\Vert} &= \frac {\pmb{u}.\pmb{v}}{\pmb{u}.\pmb{u}} \pmb{u} \\ &= (\pmb{u}.\pmb{v})\pmb{u} \qquad (\Vert {\pmb u} \Vert = 1) \end{aligned}$

因为$\pmb{v} = \pmb{v} {\Vert} + \pmb{v} {\bot}$，所以

$\begin{aligned} \pmb{v}_{\bot} &= \pmb v - \pmb{v}_{\Vert} \\ & = \pmb v - (\pmb{u}.\pmb{v})\pmb{u} \end{aligned}$

接下来分别计算~

2.水平方向$\pmb{v}_{\Vert}$的旋转

因为平行向量$\pmb{v}_{\Vert}$绕$\pmb u$旋转任意角度之后还是自身，所以有：

${\pmb{v}_{\Vert}}^{'} = \pmb{v}_{\Vert}$

3.垂直方向$\pmb{v}_{\bot}$的旋转

三维空间如图所示： 用2D的俯视图，会更容易看到清楚，对应于上图的下面一个虚线的圆 借助一个向量$\pmb w = \pmb u \times {\pmb v_{\bot}}$ 注意：右手坐标系统，叉乘顺序

$\begin{aligned} \Vert \pmb w \Vert & = \Vert \pmb u \times {\pmb v_{\bot}} \Vert \\ &= \Vert u \Vert . \Vert \pmb v_{\bot} \Vert . sin(\pi/2) \\ & = \Vert \pmb v_{\bot} \Vert \end{aligned}$

将$\pmb v ^ {‘} _ {\bot}$分解到$\pmb w$和$\pmb v_{\bot}$上，可以简单算出来下面的等式

$\begin{aligned} \pmb v ^ {'} _ {\bot} &= \pmb v^{'}_v + \pmb v^{'}_w \\ & = cos(\theta) \pmb v_\bot + sin(\theta) \pmb w \\ & = cos(\theta) \pmb v_\bot + sin(\theta)(\pmb u \times \pmb v_\bot) \end{aligned}$ 书上说提到用一点三角学的公式，其实主要是下面的这个三个向量的模都相等。按照投影或者分解，本应该是$\pmb v^{‘} v = cos(\theta) \pmb v^{‘} \bot$和$\pmb v^{‘} w = sin(\theta) \pmb v^{‘} \bot$。简单换算一下就是上面的等式了 $\begin{aligned} \pmb v^{'}_v &= cos(\theta) \pmb v^{'}_\bot = cos(\theta) \frac {\pmb v_\bot}{\Vert \pmb v_\bot \Vert} \Vert \pmb v^{'}_\bot \Vert = cos(\theta)\pmb v_\bot \\ \pmb v^{'}_w &= sin(\theta) \pmb v^{'}_\bot = sin(\theta) \frac {\pmb w}{\Vert \pmb w \Vert} \Vert \pmb v^{'}_\bot \Vert = sin(\theta) \pmb w = sin(\theta) (\pmb u \times \pmb v_\bot) \end{aligned}$

4.向量$\pmb v$的旋转公式

再由旋转后的向量组合起来得到

$\begin{aligned} \pmb v^{'} &= \pmb v^{'}_\Vert + \pmb v^{'}_\bot \\ & = \pmb v_\Vert + cos(\theta) \pmb v_\bot + sin(\theta)(\pmb u \times \pmb v_\bot) \end{aligned}$ 因为叉乘遵守分配律，有 $\begin{aligned} \pmb u \times \pmb v_\bot &= \pmb u \times (\pmb v - \pmb v_\Vert) \\ & = \pmb u \times \pmb v - \pmb u \times \pmb v_\Vert \\ & = \pmb u \times \pmb v \qquad (\pmb u 平行于 \pmb v_\Vert) \end{aligned}$ 最后，将$\pmb v_\Vert = (\pmb u {.} \pmb v){.} \pmb u$和$\pmb v_\bot = \pmb v - (\pmb u {.} \pmb v){.} \pmb u$代入，得到 $\begin{aligned} \pmb v^{'} = (\pmb u {.} \pmb v){.} \pmb u+ cos(\theta)(\pmb v - (\pmb u {.} \pmb v){.} \pmb u) + sin(\theta)(\pmb u \times \pmb v) \\ \quad = cos(\theta)\pmb v + (1 - cos(\theta))(\pmb u {.} \pmb v){.} \pmb u + sin(\theta)(\pmb u \times \pmb v) \end{aligned}$

注意这个公式，参考[6]中叫做 叫做「Rodrigues’ Rotation Formula」

四元数的3D旋转公式

接下来，我们要慢慢推导四元数的公式了，那么先要搞清楚四元数喝旋转之前的关系。下面的内容把四元数相关性质省略了，想看的话，看参考[6]或者其他书籍吧。

参考上一节，还是走相同的流程：三维空间旋转公式，也是分解向量$v$。但是，这次不是用的向量的三维表示，而是再上一个维度，用四元数去表示。高一维度能完全秒杀低一维度的生物，所以想想还是挺厉害的。

先放两个引理，后面还会遇到几个引理，都是为了方便推导公式：

• 引理一 纯四元数 简单来讲呢？现在要用一个四维的向量来表示空间的旋转，表示方法就有了一些对应的变化

注意：这里用非粗体字母表示四元数

• 引理二 Graßmann 积

直接列出来，如果看推理，参考[6]

按照引理一，列出所有的四元数表示

$\begin{aligned} &v = [0, \pmb v] &v^{'} = [0, \pmb v ^{'}] \\ &v_\bot = [0, \pmb v_\bot] &v^{'}_\bot = [0, \pmb v^{'}_\bot] \\ &v_\Vert = [0, \pmb v_\Vert] &v^{'}_\Vert = [0, \pmb v^{'}_\Vert] \\ &u = [0, \pmb u] \end{aligned}$ 并且有： $v = v_\bot + v_\Vert \qquad \qquad v^{'} = v^{'}_\bot + v^{'}_\Vert$

1.水平方向$v_\Vert$的旋转

同理，这个旋转没有发生任何变化

$v^{'}_\Vert = v_\Vert$

2.垂直方向$v_\bot$的旋转

之前推导有

$\pmb v^{'}_\bot = cos(\theta) \pmb v_\bot + sin(\theta)(\pmb u \times \pmb v_\bot)$ 把向量换成四元数，就可以有四元数表示的公式了，但是向量的叉乘怎么和四元数什么乘积对应起来呢？看下面，可以看到是 Graßmann 积

根据引理二的 Graßmann 积 ，假设有两个四元数$v = [0, \pmb v], u = [0, \pmb u]$，那么$vu = [-\pmb v {.} \pmb u, \pmb v \times \pmb u]$。由此，那么可以算出来推理一中的四元数

$\begin{aligned} uv_\bot &= [-\pmb u {.} \pmb v_\bot, \; \pmb u \times \pmb v_\bot] \\ &= [0, \; \pmb u \times \pmb v_\bot] \qquad (\pmb u {.} \pmb v_\bot = 0) \end{aligned}$ 结果还是一个四元数！所以，上面向量$\pmb v^{‘}_\bot$的表示，用四元数表示为： $v^{'}_\bot = cos(\theta)v_\bot + sin(\theta)(uv_\bot )$ 四元数遵循乘法分配律，得到 $\begin{aligned} v^{'}_\bot &= cos(\theta)v_\bot + sin(\theta)(uv_\bot ) \\ \quad &= (cos(\theta) + sin(\theta)u)v_\bot \end{aligned}$

接下来做一些数学上常用的技巧。

如果将$(cos(\theta) + sin(\theta)u)$看成一个旋转四元数，我们就能将旋转写成四元数的乘积了。到此为止，我们已经将旋转与四元数的积联系起来了

令：$q = cos(\theta) + sin(\theta)u$，得到

$v^{'}_\bot = q v_\bot$ 对q进行变形 $\begin{aligned} q &= cos(\theta) + sin(\theta)u \\ \quad &= [cos(\theta, \pmb 0)] + [0, sin(\theta)\pmb u] \\ \quad &= [cos(\theta), sin(\theta) \pmb u] \end{aligned}$

到这里，就和最开始的公式有点像了，但是还差个2倍的关系

注意：这里对一个向量$\pmb v_\bot$绕旋转轴$\pmb u$旋转左乘四元数$q$就可以了。但是，这里的一个前提条件是向量和旋转轴垂直的情况所以，接下来，我们再回到一般情况

3.四元数$v$的旋转公式

回到一般情况

$\begin{aligned} v^{'} &= v^{'}_\bot + v^{'}_\Vert \\ \quad &= v_\Vert + qv_\bot \end{aligned}$

这里，我把步骤写下来，完全参考[6]，对于中间的证明，可以翻阅参考[6]，非常的详细 接下来就是等式各种演变了

• 引理三

如果$q = [cos(\theta), sin(\theta) \pmb u]$，如果$\pmb u$为单位向量，$q^2 = qq = [cos(2\theta), sin(2\theta)\pmb u]$

我要找的2倍角出现了，有点苗头了.

$\begin{aligned} v^{'} &= v^{'}_\bot + v^{'}_\Vert \\ \quad &= v_\Vert + qv_\bot \\ \quad &= 1 {.} v_\Vert + qv_\bot \\ \quad &= pp^{-1}v_\Vert + ppv_\bot \qquad (令q = p^2, \;则p = [cos(1/2\theta), sin(1/2\theta) \pmb u]) \end{aligned}$ 因为$p$也是单位四元数，即$\Vert p \Vert = 1$，则有 $p^{-1} = p ^{\star}$ 继续做等式变换 $\begin{aligned} v^{'} &= pp^{-1}v_\Vert + ppv_\bot \\ \quad &= pp^{\star} v_\Vert + ppv_\bot \end{aligned}$

• 引理四

  假设$v_\Vert = [0, \pmb v_\Vert]$是一个纯四元数，而$q = [\alpha, \beta \pmb u]$，其中$\pmb u$是一个单位向量，$\alpha, \beta \in \mathbb R$。在这种情况下，如果$\pmb v_\Vert$平行于$\pmb u$，那么$qv_\Vert = v_\Vert q$

• 引理五

  假设 $v_\bot = [0, \pmb v_\bot]$ 是一个纯四元数，而$q = [\alpha, \beta \pmb u]$，其中 $\pmb u$ 是一个单位向量，$\alpha, \beta \in \mathbb R$．在这种条件下，如果 $\pmb v_\bot$ 正交于 $\pmb u$，那么 $qv_\bot = v_\bot q^{\star}$

则等式可以继续化简

$\begin{aligned} v^{'} &= pp^{\star} v_\Vert + ppv_\bot \\ \quad &= pv_\Vert p^{\star} + pv_\bot p^{\star} \\ \quad &= p(v_\Vert + v_\bot)p^{\star} \end{aligned}$ 可以看到$(v_\Vert + v_\bot) = v$，则 $v^{'} = p v p^{\star} = p v p^{-1}$

其中，$q = [cos(\theta/2), sin(\theta/2)\pmb u]$

小结

• 可以看到如果用四元数去旋转一个向量的话，并不是四元数左乘或者右乘一次就行了，而是后面还要乘以一个四元数的逆

到此，这个公式的证明就结束了，参考[6]说四元数的表示和3D旋转向量表示的「Rodrigues’ Rotation Formula」公式是等价的，虽然没有证明，但是其实证明就是各种三角函数的带入。而且文章中也给出了一个关键的叉乘公式，下面自己就手动证明一下，可以跳过。

两种公式等价证明

为了书写方便，先假设$c = cos(\theta/2), s = sin(\theta / 2)$。已知$v = [0, \pmb v]，q = [c, s \pmb u]$， Graßmann 积 算前面两个

$\begin{aligned} qv &= [0 - s \pmb u{.} \pmb v, c \pmb v + \pmb 0 + s \pmb u \times \pmb v] \\ \quad &= [-s \pmb u {.} \pmb v, c \pmb v + s \pmb u \times \pmb v] \end{aligned}$ 按照四元数性质，可以很容易知道$q^{-1} = q^{\star} = [c, -s \pmb u]$，继续使用 Graßmann 积 推导公式 $qvq^{-1} = [-sc \pmb u {.} \pmb v - (c \pmb v + s \pmb u \times \pmb v).(-s \pmb u), \; -s \pmb u {.} \pmb v {.}(-s\pmb u) + c(c\pmb v + s \pmb u \times \pmb v) + (c\pmb v + s \pmb u \times \pmb v) \times (-s \pmb u)] \\ $

分两部算逗号前面的，这样会清洗一点，不然公式写的老长

• 1.逗号前面四元数的实数部分

  $\begin{aligned} \quad &= -sc \pmb u {.} \pmb v - (c \pmb v + s \pmb u \times \pmb v).(-s \pmb u) \\ \quad &= -sc \pmb u {.} \pmb v + sc \pmb v {.} \pmb u - s \pmb u \times \pmb v {.}(-s \pmb u) \\ \quad &= s^2 \pmb u \times \pmb v {.} \pmb u \qquad &(向量点积满足交换律：\pmb a {.} \pmb b = \pmb b {.} \pmb a) \\ \quad &= 0 \qquad &(\pmb u \times \pmb v 和\pmb u垂直) \end{aligned}$

• 2.逗号后面四元数的虚数部分

  $\begin{aligned} & = -s \pmb u {.} \pmb v {.}(-s\pmb u) + c(c\pmb v + s \pmb u \times \pmb v) + (c\pmb v + s \pmb u \times \pmb v) \times (-s \pmb u) \hspace{1cm}\\ & = s^2 (\pmb u {.} \pmb v)\pmb u + c^2 \pmb v + cs \pmb u \times \pmb v - cs \pmb v \times \pmb u - s^2 \pmb u \times \pmb v \times \pmb u \\ & = s^2 (\pmb u {.} \pmb v)\pmb u + c^2 \pmb v + 2cs \pmb u \times \pmb v - s^2 \pmb u \times \pmb v \times \pmb u \quad (向量叉乘：\pmb a \times \pmb b = - \pmb b \times \pmb a) \\ &= s^2 (\pmb u {.} \pmb v)\pmb u + c^2 \pmb v + 2cs \pmb u \times \pmb v - s^2[(\pmb u {.} \pmb u)\pmb v - (\pmb u {.}\pmb v)\pmb u] \quad[(\pmb a \times \pmb b) \times \pmb c = (\pmb a {.}\pmb c)\pmb b - (\pmb a {.}\pmb b)\pmb c] \\ & = \color{aqua}{s^2 (\pmb u {.} \pmb v)\pmb u + s^2 (\pmb u {.}\pmb v)\pmb u} + \color{blue}{c^2 \pmb v - s^2\pmb v} + 2cs \pmb u \times \pmb v \\ & = cos(\theta)\pmb v + (1-cos(\theta))(\pmb u {.} \pmb v)\pmb u + sin(\theta)\pmb u \times \pmb v \qquad (三角函数两倍角公式) \end{aligned}$

• 3.再合在一起

  $qvq_{-1} =[0, cos(\theta)\pmb v + (1-cos(\theta))(\pmb u {.} \pmb v)\pmb u + sin(\theta)\pmb u \times \pmb v]$

  这样就完成证明了

参考

[1] Maths - AxisAngle to Quaternion [2] 3D数学基础图形与游戏开发 [4] 四元数与欧拉角（RPY角）的相互转换 [5] Converting to Euler & Tait-Bryan [6] 四元数旋转公式推导 [7] eater.net [8]四元数与三位旋转 [9] https://www.geogebra.org/classic [10] geogebra