四元数和旋转(Quaternion & rotation)

本篇文章主要讲述3D空间中的旋转和四元数之间的关系。其中会涉及到矩阵、向量运算，旋转矩阵，四元数，旋转的四元数表示，四元数表示的旋转如何转化为旋转矩阵。层层铺垫，可能文章有点长。基础好的同学，可以直接跳到四元数表示旋转部分，见下文公式(18)和公式(21)。

1 向量的点积和叉积

1.1 点积

给定两个n维向量$\mathbf{P}, \mathbf{Q}$，则它们的点积(dot product，又称为内积)为：

\[\mathbf{P}\cdot \mathbf{Q} = \left\|\mathbf{P}\right\| \left\|\mathbf{Q}\right\|\cos\alpha \qquad(1),
\]

其中$\alpha$是两向量之间的夹角。

图1 向量P在Q上的投影

如上面图1，向量$\mathbf{P}$在$\mathbf{Q}$上的投影为:

\[proj_Q \mathbf{P} = \frac{\mathbf{P}\cdot \mathbf{Q}}{\left\|\mathbf{Q}\right\|^2}\mathbf{Q}
\]

向量$\mathbf{P}$垂直于$\mathbf{Q}$的分量为：

\[\begin{aligned}
perp_Q \mathbf{P} &= \mathbf{P} - proj_Q \mathbf{P}\\
&=\mathbf{p} - \frac{\mathbf{P}\cdot \mathbf{Q}}{\left\|\mathbf{Q}\right\|^2}\mathbf{Q}
\end{aligned}
\]

其中，向量$\mathbf{P}$在$\mathbf{Q}$上的投影可以看作$\mathbf{P}$的线性变换，可以写成矩阵向量积：

\[proj_Q \mathbf{P} = \frac{1}{\left\|\mathbf{Q}\right\|^2}\left[
\begin{matrix}
\mathcal{Q}_x^2 & \mathcal{Q}_x\mathcal{Q}_y & \mathcal{Q}_x\mathcal{Q}_z\\
\mathcal{Q}_x\mathcal{Q}_y & \mathcal{Q}_y^2 & \mathcal{Q}_y\mathcal{Q}_z \\
\mathcal{Q}_x\mathcal{Q}_z & \mathcal{Q}_y\mathcal{Q}_z & \mathcal{Q}_z^2
\end{matrix}
\right]
\left[
\begin{matrix}
P_x\\
P_y\\
P_z
\end{matrix}
\right]

\qquad (2)
\]

1.2 叉积(cross product)

给定两个3D向量$\mathbf{P}, \mathbf{Q}$，则它们的叉积(又称为向量积，vector product)是一个向量:

\[\mathbf{P}\times \mathbf{Q} = \left \langle P_yQ_z - P_zQ_y, P_zQ_x - P_xQ_z, P_xQ_y - P_yQ_x \right \rangle.
\]

叉积的模:

\[\left \| \mathbf{P}\times \mathbf{Q}\right \| = \left \| \mathbf{P} \right \| \left \| Q \right \| \sin\alpha
\]

叉积也可以写成矩阵向量相乘的形式：

\[\mathbf{P}\times \mathbf{Q} =
\left[
\begin{matrix}
0 & -P_z & P_y \\
P_z & 0 & -P_x \\
-P_y & P_x & 0
\end{matrix}
\right]
\left[
\begin{matrix}
\mathcal{Q}_x \\
\mathcal{Q}_y \\
\mathcal{Q}_z
\end{matrix}
\right]
\qquad (3)
\]

2 旋转变换 (Rotation Transforms)

先看二维空间中的旋转。

图2 x-y平面中向量P旋转90度

如上面图2，在x-y平面上把向量$\mathbf{P} = \left \langle x, y \right \rangle$逆时针旋转90度，变成了向量$\mathbf{Q} = \left \langle -P_y, P_x \right \rangle = \left \langle -y, x \right \rangle$。向量$\mathbf{P}, \mathbf{Q}$形成了x-y平面的一个正交基，因此我们可以用这两个向量表示x-y平面的任意向量。

图3 x-y平面中向量P旋转theta度

如上面图3，向量$\mathbf{Q}$是向量$\mathbf{P}$逆时针旋转90度后得到的向量，向量$\mathbf{P'}$是向量$\mathbf{P}$旋转$\theta$度得到的向量，则：

\[\mathbf{P'} = \mathbf{P}\cos\theta + \mathbf{Q}\sin\theta .
\]

即

\[P'_x = P_x\cos\theta - P_y\sin\theta \\
P'_y = P_y\cos\theta + P_x\sin\theta
\]

写成矩阵形式，即：

\[\mathbf{P'}=
\left[
\begin{matrix}
\cos\theta & -\sin\theta\\
\sin\theta & \cos\theta
\end{matrix}
\right]
\mathbf{P}
\]

上面的旋转矩阵可以扩展为三维空间中绕z轴旋转$\theta$角度的矩阵(只要保证旋转后z坐标不变)：

\[\mathbf{R}_z(\theta) = \left[
\begin{matrix}
\cos\theta & -\sin\theta & 0\\
\sin\theta & \cos\theta & 0\\
0 & 0 & 1
\end{matrix}
\right]
\]

类似地，也可以推出三维空间中绕x轴、y轴的旋转矩阵：

\[\mathbf{R}_x(\theta) = \left[
\begin{matrix}
1 & 0 & 0\\
0 & \cos\theta & -\sin\theta\\
0 & \sin\theta & \cos\theta
\end{matrix}
\right]
\]

\[\mathbf{R}_y(\theta)=\left[
\begin{matrix}
\cos\theta & 0 & \sin\theta\\
0 & 1 & 0\\
-\sin\theta & 0 & \cos\theta
\end{matrix}
\right]
\]

绕任意轴的旋转

假设我们想要绕任意一个轴$\mathbf{A}$对向量$\mathbf{P}$旋转$\theta$度(其中$\mathbf{A}$是单位向量)。

因为旋转轴$\mathbf{A}$是单位向量，所以$\mathbf{P}$在$\mathbf{A}$上的投影可以简化为:

\[proj_{\mathbf{A}} \mathbf{P} = (\mathbf{A}\cdot \mathbf{P})\mathbf{A}
\]

则$\mathbf{P}$垂直于$\mathbf{A}$的分量为：

\[perp_{\mathbf{A}} \mathbf{P}=\mathbf{P}-(\mathbf{A}\cdot \mathbf{P})\mathbf{A}
\]

图4. 绕任意轴旋转示意图

与旋转轴平行的分量$proj_{\mathbf{A}} \mathbf{P}$不受旋转的影响，我们只需要考虑垂直旋转轴的分量$perp_{\mathbf{A}} \mathbf{P}$。要注意的是，最后计算出旋转后的$perp_{\mathbf{A}} \mathbf{P}$要加上与旋转轴平行的分量，才能得到最终的旋转后的向量。

垂直分量绕$\mathbf{A}$的旋转发生于垂直于旋转轴$\mathbf{A}$的平面上。像之前一样，我们可以把旋转后的向量表示成向量$perp_{\mathbf{A}} \mathbf{P}$和$perp_{\mathbf{A}} \mathbf{P}$绕$\mathbf{A}$逆时针旋转90度后的向量(即下文说的$\mathbf{A}\times \mathbf{P}$)的线性组合(linear combination)。

如图4所示，$perp_{\mathbf{A}} \mathbf{P}$的长度显然为$\left \|\mathbf{P}\right \|\sin\alpha$，又旋转轴$\mathbf{A}$是单位向量，则$\mathbf{A}\times \mathbf{P}$刚好长度和$相同，且垂直于$\mathbf{A}$和$\mathbf{P}$，正是我们想要的向量。

现在，按照上面的论述，$perp_{\mathbf{A}} \mathbf{P}$绕$\mathbf{A}$旋转$\theta$后的向量可以表示为:

\[\left[\mathbf{P} - (\mathbf{A}\cdot \mathbf{P})\mathbf{A} \right]\cos\theta + (\mathbf{A}\times\mathbf{P})\sin\theta
\]

再加上不受旋转影响的分量$proj_{\mathbf{A}} \mathbf{P}$，就得到了向量$\mathbf{P}$绕旋转轴$\mathbf{A}$旋转$\theta$角度后的向量$\mathbf{P'}$

\[\mathbf{P'} = \mathbf{P}\cos\theta + (\mathbf{A}\times \mathbf{P})\sin\theta + \mathbf{A}(\mathbf{A}\cdot\mathbf{P})(1-\cos\theta) \qquad (4)
\]

把$\mathbf{A}\times \mathbf{P}$和$\mathbf{A}(\mathbf{A}\cdot\mathbf{P})$换成对应的矩阵形式(参考上面的公式3和公式2)。我们可以得到：

\[\mathbf{P'} =
\left[
\begin{matrix}
1 & 0 & 0\\
0 & 1 & 0\\
0 & 0 & 1
\end{matrix}
\right]\mathbf{P}\cos\theta +
\left[
\begin{matrix}
0 & -A_z & A_y\\
A_z & 0 & -A_x\\
-A_y & A_x & 0
\end{matrix}
\right]\mathbf{P}\sin\theta \\+
\left[
\begin{matrix}
A_x^2 & A_xA_y & A_xA_z\\
A_xA_y & A_y^2 & A_yA_z\\
A_xA_z & A_yA_z & A_z^2
\end{matrix}
\right]\mathbf{P}(1-\cos\theta)
\qquad(5)
\]

把公式(5)中的各项结合起来(即提取出$\mathbf{P}$)，同时设$c=\cos\theta, s=\sin\theta$，则绕任意旋转轴$\mathbf{A}$旋转一个向量$\theta$角度的旋转矩阵为:

\[\mathbf{R}_{\mathbf{A}}(\theta)=\left[
\begin{matrix}
c+(1-c)A_x^2 & (1-c)A_xA_y-sA_z & (1-c)A_xA_z+sA_y\\
(1-c)A_xA_y+sA_z & c+(1-c)A_y^2 & (1-c)A_yA_z-sA_x\\
(1-c)A_xA_z-sA_y & (1-c)A_yA_z+sA_x & c+(1-c)A_z^2
\end{matrix}
\right]
\quad (6)
\]

3 四元数

3.1 四元数定义

四元数(quaternion)可以看作中学时学的复数的扩充，它有三个虚部。形式如下：

$\mathbf{q}=<w, x, y, z> = w + xi + yj + zk$，可以写成$\mathbf{q}=s+\mathbf{v}$

具有如下性质：

$i^2 = j^2 = k^2 = -1$

$ij = -ji = k$

$jk = -kj = i$

$ki = -ik = j$

设$\mathbf{q}_1 = s_1 + \mathbf{v}_1$，$\mathbf{q}_2 = s_2 + \mathbf{v}_2$，则

$\mathbf{q}_1\mathbf{q}_2 = s_1s_2 - \mathbf{v}_1\cdot \mathbf{v}_2 + s_1\mathbf{v}_2 + s_2\mathbf{v}_1 + \mathbf{v}_1\times \mathbf{v}_2$

3.2 共轭四元数

一个四元数$\mathbf{q} = s + \mathbf{v}$的共轭(用$\bar{\mathbf{q}}$表示)为$\bar{\mathbf{q}} = s - \mathbf{v}$

一个四元数和它的共轭的积等于该四元数与自身的点乘，也等于该四元数长度的平方。即，

$\mathbf{q}\bar{\mathbf{q}} = \bar{\mathbf{q}} \mathbf{q}=\mathbf{q}\cdot\mathbf{q} = ||\mathbf{q}||^2=q^2$

3.3 四元数的逆

一个非零四元数$\mathbf{q}$的逆为$\mathbf{q^{-1}=\frac{\bar{\mathbf{q}}}{q^2}}$。

显然$\mathbf{q}\mathbf{q^{-1}}=1$

3.4 四元数表示旋转

终于到正题了。

三维空间中的旋转可以被认为是一个函数$\phi$，从$\mathbb{R}^3$到自身的映射。函数$\phi$要想表示一个旋转，必须在旋转过程中保持向量长度(lengths)、向量夹角(angles)和handedness不变。

(handedness和左右手坐标系有关，例如左手坐标系中向量旋转后，仍要符合左手坐标系规则)

长度保持不变要满足：

\[\left \|\phi(\mathbf{P}) \right \| = \left \|\mathbf{P} \right\| \qquad (7)
\]

角度不变要满足：

\[\phi(\mathbf{P_1})\cdot \phi(\mathbf{P_2}) = \mathbf{P_1}\cdot \mathbf{P_2} \qquad (8)
\]

最后，handedness保持不变要满足：

\[\phi(\mathbf{P_1})\times \phi(\mathbf{P_2})=\phi(\mathbf{P_1}\times\mathbf{P_2}) \qquad(9)
\]

扩展函数$\phi$成为一个从$\mathbb{H}$到其自身的映射，要求$\phi(s+\mathbf{V})=s+\phi(\mathbf{V})$，这使得我们可以重写上面的方程(7)：

\[\phi(\mathbf{P_1})\cdot\phi(\mathbf{P_2})=\phi(\mathbf{P_1}\cdot\mathbf{P_2}) \qquad (10)
\]

注意，这里把$\mathbf{P_1}$和$\mathbf{P_2}$当作标量部分(scalar part)为0的四元数(其实就是三维空间中的点)，所以根据$\phi(s+\mathbf{V})=s+\phi(\mathbf{V})$可得到公式(10)。我们可以把方程(9)、(10)合写成一个公式(角度保持不变、handedness保持不变):

\[\phi(\mathbf{P_1})\phi(\mathbf{P_2})=\phi(\mathbf{P_1P_2}) \qquad (11)
\]

方程(11)两向量的乘其实既点乘、又是叉乘，二者合二为一，用一种形式写出来了。

一个满足方程(11)的函数$\phi$被称为同态(homomorphism，异质同形)。

重点来了：

由下面公式(12)给出的一类函数满足方程(7)和方程(11)，可以表示旋转。

\[\phi_{\mathbf{q}}(\mathbf{P})=\mathbf{q}\mathbf{P}\mathbf{q^{-1}} \qquad (12)
\]

这里$\mathbf{q}$是一个非零四元数，函数的参数$\mathbf{P}$可以看作三维空间中的点(即一个实部或标量部分为0的四元数)。

首先证明方程(12)中的函数满足方程(7):

\[\left \|\phi_{\mathbf{q}}(\mathbf{P})\right \| = \left \|\mathbf{qPq^{-1}}\right \|=\left \|q\right \| \left \|\mathbf{P}\right \| \left\| q^{-1} \right \| = \left \|\mathbf{P}\right \| \frac{\left \|\mathbf{q}\right \|\left \|\bar{\mathbf{q}}\right \|}{q^2}=\left \|\mathbf{P}\right \| \qquad (13)
\]

更进一步地，$\phi_{\mathbf{q}}$是一个homomorphism，即满足方程(11)，因为：

\[\phi_{\mathbf{q}}(\mathbf{P_1})\phi_{\mathbf{q}}(\mathbf{P_2})=\mathbf{qP_1q^{-1}qP_2q^{-1}}=\mathbf{qP_1P_2q^{-1}}=\phi_{\mathbf{q}}(\mathbf{P_1P_2}) \qquad (14)
\]

接下来我们要找到一个四元数$\mathbf{q}$，使它对应于一个绕旋转轴$\mathbf{A}$旋转$\theta$的旋转变换。可以很容易证明对于任意非零标量$a$，$\phi_{a\mathbf{q}}=\phi_{\mathbf{q}}$成立。为了使事情简单点，我们可以选择单位四元数$\mathbf{q}$。

设$\mathbf{q}=s + \mathbf{v}$为一个单位四元数，则它的逆为$\mathbf{q^{-1}} = s - \mathbf{v}$。再给一个三维空间中的点$\mathbf{P}$(相当于一个标量部分为0的四元数)，就有：

\[\begin{aligned}
\mathbf{qPq^{-1}} &= (s + \mathbf{v})\mathbf{P}(s-\mathbf{v})\\
&= (-\mathbf{v}\cdot \mathbf{P} + s\mathbf{P} + \mathbf{v}\times\mathbf{P})(s-\mathbf{v})\\
&= -s\mathbf{p}\times\mathbf{v}-\mathbf{v}\times\mathbf{p}\times\mathbf{v}+s^2\mathbf{P}+s\mathbf{v}\times\mathbf{P}+\mathbf{v}\mathbf{P}\mathbf{v}-\mathbf{v}\mathbf{P}s+s\mathbf{P}\mathbf{v}+\mathbf{v}\times\mathbf{P}\cdot\mathbf{V} \\
&=s^2\mathbf{P}+2s\mathbf{v}\times \mathbf{P}+(\mathbf{v}\cdot\mathbf{P})\mathbf{v}-\mathbf{v}\times\mathbf{P}\times\mathbf{v}
\end{aligned}
\qquad (15)
\]

定理：给定任意两个3D向量$\mathbf{P}, \mathbf{Q}$，则：

$\mathbf{P}\times(\mathbf{Q}\times\mathbf{P})=\mathbf{P}\times\mathbf{Q}\times\mathbf{P}=P^2\mathbf{Q}-(\mathbf{P}\cdot\mathbf{Q})\mathbf{P}$

所以，对方程(15)中的$\mathbf{v}\times\mathbf{P}\times\mathbf{v}$施加上述定理，方程(15)就变成了：

\[\mathbf{qPq^{-1}}=(s^2-\mathbf{v}^2)\mathbf{P}+2s\mathbf{v}\times\mathbf{P}+2(\mathbf{v}\cdot\mathbf{P})\mathbf{v} \qquad (16)
\]

设$\mathbf{v}=t\mathbf{A}$，此处$\mathbf{A}$是一个单位向量，即旋转轴，重写方程(16)为：

\[\mathbf{qPq^{-1}}=(s^2-t^2)\mathbf{P}+2st\mathbf{A}\times\mathbf{P}+2t^2(\mathbf{A}\cdot\mathbf{P})\mathbf{A} \qquad (17)
\]

把方程(17)和上面说过的绕任意轴旋转的公式(4)进行比较。

上面说过的公式(4)\mathbf{P'} = \mathbf{P}\cos\theta + (\mathbf{A}\times \mathbf{P})\sin\theta + \mathbf{A}(\mathbf{A}\cdot\mathbf{P})(1-\cos\theta) \qquad $

我们可以推出以下等式：

\[\begin{aligned}
s^2 - t^2 &= \cos\theta\\
2st &= \sin\theta\\
2t^2 &= 1 - \cos\theta
\end{aligned}
\]

由第三个等式可以解出:

\[t=\sqrt{\frac{1-\cos\theta}{2}}=\sin\frac{\theta}{2}
\]

由第一和第三个等式可推出$s^2+t^2=1$，所以$s=\cos\frac{\theta}{2}$。

所以，我们找到了一个单位四元数$\mathbf{q}$，其对应于绕旋转轴$\mathbf{A}$旋转$\theta$角度的旋转变换(重要结论)：

\[\begin{aligned}
\mathbf{q} &=s+\mathbf{v}\\
&=s+t\mathbf{A}\\
&=\cos\frac{\theta}{2} + \mathbf{A}\sin\frac{\theta}{2}
\end{aligned}
\qquad (18)
\]

这里顺便证明一下对于任意非零$a$(如，$a=-1$)，$a\mathbf{q}$和$\mathbf{q}$表示同一个旋转，因为

\[(a\mathbf{q})\mathbf{P}(a\mathbf{q})^{-1}=a\mathbf{qP}\frac{\mathbf{q^{-1}}}{a}=\mathbf{qPq^{-1}}
\]

两个四元数$\mathbf{q_1}$和$\mathbf{q_2}$的乘积也表示一个旋转。例如，$\mathbf{q_1q_2}$表示施加旋转$\mathbf{q_2}$，再施加旋转$\mathbf{q_1}$。因为：

\[\mathbf{q_1(q_2Pq_2^{-1})q_1^{-1}}=\mathbf{(q_1q_2)P(q_1q_2)^{-1}}
\]

我们可以连接任意多个表示旋转的四元数，形成一个单一的四元数。

两个四元数相乘需要16个乘加运算(multiply-add)，然而两个$3\times 4$矩阵相乘需要27个乘加运算。因此在某些情况下(比如要对同一个对象施加多个旋转变换时)，用四元数表示旋转，计算效率会高些。

总之，通过一个四元数$\mathbf{q}$对一个点$\mathbf{P}$施加旋转变换，只需要做如下计算:

\[\mathbf{P'}=\mathbf{qPq^{-1}}
\]

3.5 四元数转换为旋转矩阵

有时需要把四元数转换为$3\times 3$的旋转矩阵，例如，使用某些3D图形库时。

我们可以根据方程(2)和方程(3)，把方程(17)写程矩阵形式，来找到四元数$\mathbf{q}=s+t\mathbf{A}$对应的旋转矩阵:

\[\mathbf{qPq^{-1}}=\left[
\begin{matrix}
s^2-t^2 & 0 & 0\\
0 & s^2-t^2 & 0\\
0 & 0 & s^2-t^2
\end{matrix}
\right]\mathbf{P}+
\left[
\begin{matrix}
0 & -2stA_z & 2stA_y\\
2stA_z & 0 & -2stA_x\\
-2stA_y & 2stA_x &0
\end{matrix}
\right]\mathbf{P}\\
+\left[
\begin{matrix}
2t^2A_x^2 & 2t^2A_xA_y & 2t^2A_xA_z\\
2t^2A_xA_y & 2t^2A_y^2 & 2t^2A_yA_z\\
2t^2A_xA_y & 2t^2A_yA_z & 2t^2A_z^2
\end{matrix}
\right]\mathbf{P} \qquad (19)
\]

把四元数$\mathbf{q}$写成四维向量的形式$\mathbf{q}=\left \langle w, x, y, z \right\rangle$，其中$w=s, x=tA_x, y=tA_y, z=tA_z$。因为$\mathbf{A}$是单位向量，所以：

\[x^2 + y^2 + z^2 = t^2 \quad A^2=t^2
\]

用$w, x, y, z$的形式来重写方程(19)：

\[\mathbf{qPq^{-1}}=\left[
\begin{matrix}
w^2-x^2-y^2-z^2 & 0 & 0\\
0 & w^2-x^2-y^2-z^2 & 0\\
0 & 0 & w^2-x^2-y^2-z^2
\end{matrix}
\right]\mathbf{P}\\
+\left[
\begin{matrix}
0 & -2wz & 2wy\\
2wz & 0 & -2wx\\
-2wy & 2wx & 0
\end{matrix}
\right]\mathbf{P}+
\left[
\begin{matrix}
2x^2 & 2xy & 2xz\\
2xy & 2y^2 & 2yz\\
2xz & 2yz & 2z^2
\end{matrix}
\right]\mathbf{P} \qquad (20)
\]

因为$\mathbf{q}$是一个单位四元数，则$w^2+x^2+y^2+z^2=1$，所以

\[w^2 - x^2 - y^2 - z^2 = 1 -2x^2 - 2y^2 - 2z^2
\]

结合上面这个等式及方程(20)中的三个矩阵，我们可以得到对应于四元数$\mathbf{q}$的旋转矩阵：

\[\mathbf{R_q}=\left[
\begin{matrix}
1-2y^2-2z^2 & 2xy-2wz & 2xz+2wy\\
2xy+2wz & 1-2x^2-2z^2 & 2yz-2wx\\
2xz-2wy & 2yz+2wx & 1-2x^2-2y^2
\end{matrix}
\right] \qquad (21)
\]

原文，见知乎专栏文章:https://zhuanlan.zhihu.com/p/78987582

参考文献:

Mathematics for 3D Game Programming and Computer Graphics 英文版第三版，第2、3、4章。