四元数和旋转

四元数和旋转(Quaternion & rotation) 四元数和旋转(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章。