关于四元数的一些总结

关于四元数的内容也是看过两次了，但总是容易忘记，这次打算好好总结一下以免再忘了= =

为什么要用四元数

在计算机图形学中，作旋转变换有两种方法，一种是欧拉角，另一种就是四元数。用欧拉角会有万向节死锁的问题，而四元数则弥补了这个缺陷，是最好的实现旋转的方案。

欧拉角与万向节死锁
在线性代数中肯定学到了旋转矩阵这个东西，它可以用来作旋转变换，而这里的旋转矩阵只能代表绕着某个坐标轴作旋转，在三维空间下会分别有绕x轴、y轴、z轴旋转的旋转矩阵，而如果要表示一次旋转变换，需要分别指定绕三个坐标轴旋转的角度，然后将这三个旋转矩阵相乘，就得到了最后的变换矩阵。欧拉角就是这样的一种方案，它指定三个角度，pitch、yaw、roll，分别对应绕x轴、y轴、z轴旋转的角度。

而这个方案的最大的缺陷在于，它的三个维度的旋转不是同时进行的，是有先后顺序的，假如用绕z轴旋转的矩阵×绕x轴旋转的矩阵×绕y轴旋转的矩阵，那么最后得到的这个旋转矩阵一定表示的是先绕z轴旋转、再绕x轴旋转、再绕y轴旋转，这里的三个坐标轴都是采用的线性空间下的基向量，不是模型本身的三个维度，这就会导致一个非常严重的问题——万向节死锁。考虑这样一种情形，假如先绕Z轴旋转一定的读数，然后再绕X轴旋转90°，这个时候原本模型的Z轴就会变到线性空间下的Y轴，那么这个时候再绕线性空间下的Y轴旋转，其实还是在绕模型本身的Z轴旋转，也就是说模型本身的Y轴在这个旋转中根本没有起作用，即上图中绿色的y轴没有起作用，这是不符合预期的想法的。
采用四元数就可以有效地解决这个问题，那么下面来具体看一下四元数的相关内容。

四元数的定义

高中数学中接触过复数的概念，一个复数可以用 $a+bi$ 来表示，其中a是实部，b是虚部。四元数的定义与复数类似，一个四元数由 $ai+bj+ck+d$来表示，其中 $i 、j、k$ 是对虚数中的 $i$进行的一个扩充，$i * i = j * j = k * k = -1 、i * j = k 、j * k = i 、k * i = j$ ，因此四元数也被称作是一种超复数。 $(a,b,c)$可以看做是三维空间下的一个向量，即四元数是以四维的形式来描述三维空间下的旋转变换。四元数也可被写作$(q_v,q_w)$，其中$q_v$是虚部，$q_w$ 是实部。

四元数的加法： $(q_v,q_w) + (r_v,r_w) = (q_v+r_v,q_w+r_w)$

四元数的共轭： $q^* = (q_v,q_w)^* = (-q_v,q_w)$

四元数的模： $n(q) = \sqrt{qq^*} = \sqrt{q^*q} = \sqrt{q_v*q_v+q_w^2} = \sqrt{q_x^2+q_y^2+q_z^2+q_w^2}$

四元数的乘法： 按分配律展开即可 $qr = ( iq_x+jq_y+kq_z+q_w)(ir_x+jr_y+kr_z+r_w) = (q_v × r_v+r_wq_v+r_vq_w,q_wr_w-q_v·r_v)$

四元数的逆：$q^{-1} = \frac{q^*}{n(q)^2}$ ，满足$qq^{-1} = 1$

单位四元数：满足$q_v^2+q_w^2 = 1$的四元数

用四元数来表示旋转

四元数可以用来将一个向量绕着任意方向的一个轴做旋转。

我所阅读的关于四元数的文章中大部分都是直接给出用四元数表示旋转的公式，而没有推敲其原理，少部分的文章有讲述一些推导的过程，但大多都是基于特殊情况来推测出最终的公式，看后仍旧会有疑问，为什么这样做可以用来表示旋转？它的数学原理是什么？那么这里不妨换个角度，由向量的旋转来探讨如何用四元数表示旋转。
现在我们假设有一个向量

$$\vec{a} = (a_x,a_y,a_z)$$
它的四元数形式为： $$r_a = (\vec{a},0)$$
现在将其绕某个轴旋转 $\theta$角，假设旋转后的向量为
$$\vec{b} = (b_x,b_y,b_z)$$
同理它的四元数形式为： $$r_b = (\vec{b} , 0)$$
能够将 $\vec{a}$ 转到 $\vec{b}$的转轴有无数条，现在考虑垂直于 $\vec{a}和\vec{b}$所在的平面的转轴，设此转轴的方向向量为 $e_n$，那么由向量的点乘和叉乘的性质可得：
$$\vec{a}·\vec{b} = l^2cos\theta$$ $$\vec{a}×\vec{b} = l^2sin\theta e_n$$
其中l为 $\vec{a}$的模。如果考虑这两个向量的四元数形式，则可得：
$$r_br_a= ( -l^2sin\theta e_n, -l^2cos\theta )$$
两边同时乘上 $r_a$的逆，则可得：
$$r_b = \frac{( -l^2sin\theta e_n, -l^2cos\theta ) r{_a}^*}{l^2} = (sin\theta e_n, cos\theta)r_a$$
这说明形如 $(sin\theta e_n,cos\theta)$的四元数 $r$可以用来作 将一个向量$\vec{a}$在$r$的虚部的垂面上以右手定则旋转$\theta$角的操作。显然这样的四元数一定是一个单位四元数。
这里将旋转操作局限在了一条固定的轴上，我们说最后要得到的应该是将一个向量绕任意轴旋转的方法，那么接下来就来探讨这个问题。
假设现在给定一个轴 $e_n$，我们需要将一个向量 $\vec{a}$绕着它以右手定则旋转 $\theta$角得到向量 $\vec{b}$，考虑整个旋转是如何进行的：将 $\vec{a}$分解为垂直于 $e_n$的 $\vec{a_{\bot}}$和平行于 $e_n$的 $\vec{a_{//}}$两部分。那么将它绕着 $e_n$作旋转实际上就是将垂直于它的 $\vec{a_{\bot}}$在其所在的垂面上旋转，然后再加上 $\vec{a_{//}}$，而对于 $\vec{a_{\bot}}$的旋转可以由上面的方法进行，因此可以得出如下关系： $$\vec{b} = \vec{a_{//}} + (sin\theta e_n,cos\theta)\vec{a_{\bot}}$$
$\vec{a_{//}}$与 $\vec{a_{\bot}}$是可以求出来的：
将上式全部用其对应的四元数形式来表示则可得：
$$r_b = r_{//} + (sin\theta e_n,cos\theta)r_{\bot}$$
现在来计算 $r_{//}$和 $r_{\bot}$：

由 $$\vec{a_{\bot}} = (e_n·\vec{a})e_n$$
得 $$r_{\bot} = -\frac{(r_ar_{en}+r_{en}r_a)}{2}r_{en}$$
又因为 $$r_{en}r_{en} = (en,0)(en,0) = (0,-1) = -1$$
得 $$r_{\bot} = \frac{(r_a +r_{en}r_ar_{en})}{2}$$

由

$$\vec{a_{//}} = \vec{a} - \vec{a_{\bot}}$$ 得
$$r_{//} = r_a - r_{\bot} = \frac{(r_a -r_{en}r_ar_{en})}{2}$$
代入 $r_b$的表达式可得： $$r_b = \frac{(r_a -r_{en}r_ar_{en}) + cos\theta(r_a +r_{en}r_ar_{en}) + sin\theta e_n(r_a +r_{en}r_ar_{en})}{2}= \frac{(1+cos\theta)r_a+sin\theta r_{en} r_a-sin\theta r_a r_{en} +(cos\theta - 1)r_{en}r_ar_{en}}{2}$$
观察这个式子：可以发现有 $r_{en}r_ar_{en}$一项存在，并且还有系数互为相反数的 $r_{en}r_a$、 $r_ar_{en}$两项，这表明最终这个旋转的表达式应该必须是左乘一个四元数并且还要右乘一个四元数的形式，并且这两个四元数要与 $r_{en}$有关，与因此不妨设最终的表达式为：
$$r_b = u r_a v$$
其中 $u = a + b r_{en} ；v = c + d r_{en}$
代入可得： $$r_b = (a+br_{en})r_a(c+dr_{en}) = acr_a + adr_ar_{en} + bcr_{en}r_a + bdr_{en}r_ar_{en}$$
将系数一一对应可得：
$$\begin{cases} ac = \frac{1+cos\theta}{2}\\ ad = \frac{-sin\theta}{2}\\ bc = \frac{sin\theta}{2}\\ bd = \frac{cos\theta - 1}{2}\\ \end{cases}$$
可得该方程组的一组解为：
$$\begin{cases} a = {cos\frac{\theta}{2}}\\ b = {sin\frac{\theta}{2}}\\ c = {cos\frac{\theta}{2}}\\ d = -{sin\frac{\theta}{2}}\\ \end{cases}$$
因此可得 $u = cos\frac{\theta}{2} + e_n sin\frac{\theta}{2} ；v = cos\frac{\theta}{2} - e_n sin\frac{\theta}{2} = u^* = u^{-1}$
因此可得用四元数表达旋转的表达式为：
$$r_b = ur_au^{-1}$$ $$u = cos\frac{\theta}{2} + e_n sin\frac{\theta}{2}$$
其中 $e_n$是旋转轴的方向向量， $\theta$是旋转的角度，这个式子也与很多文章中所说必须要用 $\frac{\theta}{2}$才能使结果为纯四元数这一点相符合。

四元数的矩阵形式

旋转四元数的形式为$u = ((n_x,n_y,n_z)sin\frac{\theta}{2},cos\frac{\theta}{2})$，原则上使用四元数的运算法则就可以完成旋转的操作，但也可以考虑将其转化为矩阵的形式，即将四元数的这个运算简化成一次矩阵运算$ur_au^{-1} = M r_a$，现在考虑如何构造这样的一个矩阵。
这个式子可以看做是先让r_a左乘u，这一步可以转化为一个矩阵乘向量；然后让结果再右乘u的逆四元数，这一步也可以转化为一次矩阵乘向量，那么最终要求的矩阵就应该是这两步得到的矩阵的乘积。
先考虑左乘四元数的情况：
设两个四元数分别为$(n_x,n_y,n_z,n_w)$ $(p_x,p_y,p_z,p_w)$
左乘的结果应该为$(n_v×p_v+n_wp_v+p_wn_v,n_wp_w-n_v·p_v)$
设$M(p_x,p_y,p_z,p_w)^T = (n_v×p_v+n_wp_v+p_wn_v,n_wp_w-n_v·p_v)^T$，现在我们需要构造这样一个4X4的矩阵，将右边的每一维都具体算出来：

$$\begin{cases} x = n_wp_x-n_zp_y+n_yp_z+n_xp_w\\ y = n_zp_x + n_wp_y-n_xp_z + n_yp_w \\ z = -n_yp_x+n_xp_y + n_wp_z + n_zp_w\\ w = -n_xp_x-n_yp_y-n_zp_z+n_wp_w\\ \end{cases}$$
即：
$$\begin{bmatrix} n_w & -n_z & n_y & n_x \\ n_z & n_w & -n_x & n_y \\ -n_y & n_x & n_w & n_z \\ -n_x & -n_y & -n_z & n_w \end{bmatrix} * \begin{bmatrix} p_x \\ p_y \\ p_z \\ p_w \end{bmatrix} = \begin{bmatrix} x \\ y \\ z \\ w \end{bmatrix}$$
因此左乘一个四元数 $(n_x,n_y,n_z,n_w)$等同于左乘一个矩阵 $$\begin{bmatrix} n_w & -n_z & n_y & n_x \\ n_z & n_w & -n_x & n_y \\ -n_y & n_x & n_w & n_z \\ -n_x & -n_y & -n_z & n_w \end{bmatrix}$$

右乘四元数的情形可以很容易由左乘的情形推得，右乘四元数时与左乘唯一的区别就在于虚部的叉乘项要取反，这对应x、y、z、w也只需要作微调，w与左乘完全相同，x、y、z中通过叉乘运算的那两项要取反，也就是：

$$\begin{cases} x = n_wp_x+n_zp_y-n_yp_z+n_xp_w\\ y = -n_zp_x + n_wp_y+n_xp_z + n_yp_w \\ z = n_yp_x-n_xp_y + n_wp_z + n_zp_w\\ w = -n_xp_x-n_yp_y-n_zp_z+n_wp_w\\ \end{cases}$$
但是同时也要注意到，右乘的四元数变成了左乘的四元数的逆，即虚部要取反，最终的 $n_x、n_y、n_z$都要取反，那么对应的做修改可得：
$$\begin{cases} x = n_wp_x-n_zp_y+n_yp_z-n_xp_w\\ y = n_zp_x + n_wp_y-n_xp_z - n_yp_w \\ z = -n_yp_x+n_xp_y + n_wp_z - n_zp_w\\ w = n_xp_x+n_yp_y+n_zp_z+n_wp_w\\ \end{cases}$$
因此右乘逆四元数所等价的矩阵应该是：
$$\begin{bmatrix} n_w & -n_z & n_y & -n_x \\ n_z & n_w & -n_x & -n_y \\ -n_y & n_x & n_w & -n_z \\ n_x & n_y & n_z & n_w \end{bmatrix}$$

现在可以计算最终的矩阵了（注意要利用$n{_x}^2+n{_y}^2+n{_z}^2+n{_w}^2 = 1$）：

$$\begin{bmatrix} n_w & -n_z & n_y & n_x \\ n_z & n_w & -n_x & n_y \\ -n_y & n_x & n_w & n_z \\ -n_x & -n_y & -n_z & n_w \end{bmatrix} * \begin{bmatrix} n_w & -n_z & n_y & -n_x \\ n_z & n_w & -n_x & -n_y \\ -n_y & n_x & n_w & -n_z \\ n_x & n_y & n_z & n_w \end{bmatrix} =$$ $$\begin{bmatrix} 1-2n_yn_y-2n_zn_z & 2n_xn_y-2n_wn_z & 2n_xn_z+2n_wn_y & 0 \\ 2n_xn_y+2n_wn_z & 1-2n_xn_x-2n_zn_z & 2n_yn_z-2n_wn_x &0 \\ 2n_xn_z-2n_wn_y & 2n_yn_z + 2n_wn_x & 1-2n_xn_x-2n_yn_y & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$
这就是要求的矩阵 $M$。

四元数的球面线性插值

用作旋转的四元数都是单位四元数，也可以说是四维空间下的一个单位向量，所有的这些单位向量构成了一个超球体。一般的线性插值都是：

$$r = p + (p - q)t$$ 向量的线性插值都会遇到一个问题，看下图：

最左侧和最右侧的向量为用作插值的向量，可以看到在中间的插值得到的结果，有一个严重的问题，就是插值得到的向量的模长发生了变化，也就是说在t均匀变化的过程中，插值得到的矢量的角速度的变化是不均匀的，也就是说这种插值方案是不够平滑的！究其原因，这样的插值方法其实是在对向量的终点做插值，将向量的终点在对应的弦上均匀的变化，但这并不是想要的结果，实际上需要的应该是对弧度做插值！

设 $$r = a(t)p + b(t)q$$
两边同时乘p得： $$cost\theta = a(t) + b(t)cos(1-t)\theta$$
两边同时乘q得： $$cos(1-t)\theta = a(t)cost\theta + b(t)$$
解这个方程组可得：
$$a(t) = \frac{sin(1-t)\theta}{sin\theta}$$
$$b(t) = \frac{sint\theta}{sin\theta}$$

因此可得对四元数的球面线性插值的公式：

$$Slerp(\vec{a},\vec{b},t) = \frac{sin(1-t)\theta\vec{a} + sint\theta\vec{b}}{sin\theta}$$

使用这个方法做插值的时候要注意，$\theta$的大小是未知的，要通过两个已知向量作点积求出$cos\theta$，然后再用反三角函数求出$\theta$，进行进一步的运算。
还要注意当两个向量的点积小于0的时候，这个时候做插值在4D球面上就会绕远路，对一个四维向量取反它所代表的四元数的旋转的含义是不变的，因此这个时候可以考虑将其中一个四元数取反，保证点积大于0，这样可以保证做插值的时候在4D球面上走的永远是最短路。
当$\theta$趋近于0的时候，$sin\theta$也会趋近于0，这个时候上述插值方法显得不太合适，这时可以改为使用一般的线性插值方法。

四元数与欧拉角的转换

欧拉角的旋转方式一共有12种，这里我们假设旋转方式为x-y-z，其余方式可以按照相同的模式推出结果。
先看由欧拉角转四元数：已知一次欧拉角旋转($\alpha$,$\beta$,$\gamma$)，求出其对应的四元数。这个相对比较容易，直接将每一维写成四元数然后相乘即可：

$$\begin{bmatrix} 0 \\ 0 \\ sin\frac{\gamma}{2} \\ cos\frac{\gamma}{2} \end{bmatrix} * \begin{bmatrix} 0 \\ sin\frac{\beta}{2} \\ 0 \\ cos\frac{\beta}{2} \end{bmatrix} *\begin{bmatrix} sin\frac{\alpha}{2} \\ 0 \\ 0 \\ cos\frac{\alpha}{2} \end{bmatrix} = \begin{bmatrix} sin\frac{\alpha}{2}cos\frac{\beta}{2}cos\frac{\gamma}{2}-cos\frac{\alpha}{2}sin\frac{\beta}{2}sin\frac{\gamma}{2} \\ cos\frac{\alpha}{2}sin\frac{\beta}{2}cos\frac{\gamma}{2}+sin\frac{\alpha}{2}cos\frac{\beta}{2}sin\frac{\gamma}{2} \\ cos\frac{\alpha}{2}cos\frac{\beta}{2}sin\frac{\gamma}{2}-sin\frac{\alpha}{2}sin\frac{\beta}{2}cos\frac{\gamma}{2} \\ cos\frac{\alpha}{2}cos\frac{\beta}{2}cos\frac{\gamma}{2}+sin\frac{\alpha}{2}sin\frac{\beta}{2}sin\frac{\gamma}{2} \end{bmatrix}$$

那么现在如果已知右边的这个四元数，如何反推出它的欧拉角？即已知四元数$(q_x,q_y,q_z,q_w)$，如何求出$(\alpha,\beta,\gamma)$？那么就是解如下的这个方程组：

$$\begin{cases} q_x = sin\frac{\alpha}{2}cos\frac{\beta}{2}cos\frac{\gamma}{2}-cos\frac{\alpha}{2}sin\frac{\beta}{2}sin\frac{\gamma}{2}\\ q_y = cos\frac{\alpha}{2}sin\frac{\beta}{2}cos\frac{\gamma}{2}+sin\frac{\alpha}{2}cos\frac{\beta}{2}sin\frac{\gamma}{2} \\ q_z =cos\frac{\alpha}{2}cos\frac{\beta}{2}sin\frac{\gamma}{2}-sin\frac{\alpha}{2}sin\frac{\beta}{2}cos\frac{\gamma}{2}\\ q_w = cos\frac{\alpha}{2}cos\frac{\beta}{2}cos\frac{\gamma}{2}+sin\frac{\alpha}{2}sin\frac{\beta}{2}sin\frac{\gamma}{2}\\ \end{cases}$$
解这个方程组可得：
$$\begin{cases} \alpha = atan2(2*(q_wq_x+q_yq_z),1-2({q_x}^2+{q_y}^2)) \\ \beta = arcsin(2*(q_wq_y-q_xq_z))\\ \gamma = atan2(2*(q_wq_z+q_xq_y),1-2({q_y}^2+{q_z}^2))\\ \end{cases}$$