CH3-5 四元数表示旋转

三维空间中任意点均可用一个纯虚四元数表示即$\mathbf{p}=[0,\mathbf{v}{]}^T$，经一个单位四元数$\mathbf{q}$的旋转后，得到${\mathbf{p}}^{\prime }$，则

$${\mathbf{p}}^{\prime }=\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}\text{\hspace{1em}(3-5-1)}$$

最终${\mathbf{p}}^{\prime }$的虚部即为旋转后点的坐标。

首先证明几个定理。注意这里向量和矩阵的转换。

（1）首先证明 $\mathrm{Gra}\beta \mathrm{mann}$积定理，也就是四元数乘法规则。

两个四元数 $q_1=a+b\vec{i}+c\vec{j}+d\vec{k}$ 、$q_2=e+f\vec{i}+g\vec{j}+h\vec{k}$，则

$$\begin{array}{rl}{q}_1{q}_2& =(a+b\vec{i}+c\vec{j}+d\vec{k})(e+f\vec{i}+g\vec{j}+h\vec{k})\\ & =ae+af\vec{i}+ag\vec{j}+ah\vec{k}+be\vec{i}+bf{\vec{i}}^2+bg\vec{i}\vec{j}+bh\vec{i}\vec{k}+\\ & +ce\vec{j}+cf\vec{j}\vec{i}+cg{\vec{j}}^2+ch\vec{j}\vec{k}+de\vec{k}+df\vec{k}\vec{i}+dg\vec{k}\vec{j}+dh{\vec{k}}^2\\ & =ae+af\vec{i}+ag\vec{j}+ah\vec{k}+be\vec{i}-bf+bg\vec{k}-bh\vec{j}+ce\vec{j}-cf\vec{k}-cg+ch\vec{i}+de\vec{k}+df\vec{j}-dg\vec{i}-dh\\ & =(ae-bf-cg-dh)+(af-dg+be+ch)\vec{i}+(-bh+ce+df+ag)\vec{j}+(ah+bg-cf+de)\vec{k}\end{array}\text{\hspace{1em}(3-5-2)}$$

写成矩阵形式，即

$$q_1{q}_2=\left[\begin{array}{cccc}a& -b& -c& -d\\ b& a& -d& c\\ c& d& a& -b\\ d& -c& b& a\end{array}\right]\left[\begin{array}{c}e\\ f\\ g\\ h\end{array}\right]\text{\hspace{1em}(3-5-3)}$$

令 $\vec{v}=(b,c,d)$、$\vec{u}=(f,g,h)$，则有

$$\vec{v}\cdot \vec{u}=bf+cg+dh\text{\hspace{1em}(3-5-4)}$$

$$\begin{array}{rl}\vec{v}\times \vec{u}& =\left|\begin{array}{ccc}\vec{i}& \vec{j}& \vec{k}\\ b& c& d\\ f& g& h\end{array}\right|\\ & =(ch-dg)\vec{i}+(df-bh)\vec{j}+(bg-cf)\vec{k}\end{array}\text{\hspace{1em}(3-5-5)}$$

综合式（3-5-2）、式（3-5-4）、式（3-5-5），得

$$q_1{q}_2=[ae-\vec{v}\cdot \vec{u},a\vec{u}+e\vec{v}+\vec{v}\times \vec{u}]\text{\hspace{1em}(3-5-6)}$$

综上：$\mathrm{Gra}\beta \mathrm{mann}$积定理

对任意四元数 $q_1=[s,\vec{v}]$，$q_2=[t,\vec{u}]$，有

$$q_1{q}_2=[st-\vec{v}\cdot \vec{u},s\vec{u}+t\vec{v}+\vec{v}\times \vec{u}]\text{\hspace{1em}(3-5-7)}$$

对于纯四元数 $q_1=[0,\vec{v}]$，$q_2=[0,\vec{u}]$，有

$$q_1{q}_2=[-\vec{v}\cdot \vec{u},\vec{v}\times \vec{u}]\text{\hspace{1em}(3-5-8)}$$

（2）证明：$q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2}$（$q^{\ast }$为$q$ 的共轭）.

$q$ 为单位四元数，则

$$q{q}^{-1}=1\text{\hspace{1em}(3-5-9)}$$

将上式左乘或右乘 $q\ast$，有

$$q^{\ast }q{q}^{-1}=q^{\ast },q{q}^{-1}{q}^{\ast }=q^{\ast }\text{\hspace{1em}(3-5-10)}$$

不妨设 $q=[s,\vec{v}]$，其中 $\vec{v}=[q_1,q_2,q_3]$，则根据四元数乘法

$$\begin{array}{rl}{q}^{\ast }q& =[s,-\vec{v}][s,\vec{v}]\\ & =[s^2-{\mathbf{v}}^{\mathrm{T}}\mathbf{v},0]\\ & =(s^2-{\mathbf{v}}^{\mathrm{T}}\mathbf{v})+0\mathbf{i}+0\mathbf{j}+0\mathbf{k}\\ & =s^2+q_1^2+q_2^2+q_3^2\\ & =\Vert q{\Vert }^2\end{array}\text{\hspace{1em}(3-5-11)}$$

则可得

$$q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2}\text{\hspace{1em}(3-5-12)}$$

证毕。

（3）下面讨论四元数和三维旋转矩阵之间的关系

        
                          

图中向量 $\vec{v}$ 绕 单位旋转向量 $\vec{u}$ 旋转 $\theta$ 度，我们分别来看 ${\vec{v}}_{\parallel }$ 和 ${\vec{v}}_{\perp }$ 的旋转。

1）首先看 $\vec{v}$ 的旋转。图一中，将向量分解

$$\vec{v}={\vec{v}}_{\parallel }+{\vec{v}}_{\perp }$$

$$\overrightarrow{v^{\prime }}={\overrightarrow{v^{\prime }}}_{\parallel }+{\overrightarrow{v^{\prime }}}_{\perp }\text{\hspace{1em}(3-5-13)}$$

图二中，

$${\overrightarrow{v^{\prime }}}_{\perp }={\vec{v}}_{v^{\prime }}+{\vec{v}}_{w^{\prime }}\text{\hspace{1em}(3-5-14)}$$

${\vec{v}}_w$ 是辅助向量，且 $\vec{v}w=\vec{u} \times \vec{v}{\perp} $。

由图二，可得

$$|{\vec{v}}_{v^{\prime }}|=|{\overrightarrow{v^{\prime }}}_{\perp }|\cos \theta =|{\vec{v}}_{\perp }|\cos \theta$$

$$|{\vec{v}}_{w^{\prime }}|=|{\overrightarrow{v^{\prime }}}_{\perp }|\sin \theta =|{\vec{v}}_w|\sin \theta \text{\hspace{1em}(3-5-15)}$$

推导，得

$${\vec{v}}_{v^{\prime }}={\vec{v}}_{\perp }\cos \theta$$

$${\vec{v}}_{w^{\prime }}={\vec{v}}_w\sin \theta \text{\hspace{1em}(3-5-16)}$$

又因为 ${\vec{v}}_w=\vec{u}\times {\vec{v}}_{\perp }$，有

$$\begin{array}{rl}{\overrightarrow{v^{\prime }}}_{\perp }& ={\vec{v}}_{v^{\prime }}+{\vec{v}}_{w^{\prime }}\\ & ={\vec{v}}_{\perp }\cos \theta +{\vec{v}}_w\sin \theta \\ & ={\vec{v}}_{\perp }\cos \theta +\sin \theta (\vec{u}\times {\vec{v}}_{\perp })\end{array}\text{\hspace{1em}(3-5-17)}$$

令 $v_{\perp }=[0,{\vec{v}}_{\perp }]$，$u=[0,\vec{u}]$，根据纯四元数乘法法则，有

$$u{v}_{\perp }=[-\vec{u}\cdot {\vec{v}}_{\perp },\vec{u}\times {\vec{v}}_{\perp }]\text{\hspace{1em}(3-5-18)}$$

因为 $\vec{u}$ 与 ${\vec{v}}_{\perp }$ 垂直，则

$$u{v}_{\perp }=[0,\vec{u}\times {\vec{v}}_{\perp }]=\vec{u}\times {\vec{v}}_{\perp }\text{\hspace{1em}(3-5-19)}$$

则式（3-5-17）可写为四元数形式

$$\begin{array}{rl}{v}_{\perp }^{\prime }& =v_{\perp }\cos \theta +\sin \theta (u{v}_{\perp })\\ & =(\cos \theta +\sin \theta u)v_{\perp }\end{array}\text{\hspace{1em}(3-5-20)}$$

令 $q=\cos \theta +\sin \theta u$，将其视作本次旋转四元数，得到

$$v_{\perp }^{\prime }=q{v}_{\perp }\text{\hspace{1em}(3-5-21)}$$

2） $\vec{v}$ 的平行分量并不会随旋转改变。下面直接推导 $\vec{v}$ 的旋转。

$$\overrightarrow{v^{\prime }}={\vec{v}}_{\parallel }+v_{\perp }^{\prime }={\vec{v}}_{\parallel }+q{v}_{\perp }\text{\hspace{1em}(3-5-22)}$$

在此之前，先证明几个引理：

① 假设旋转四元数 $q=[\cos \theta ,\sin \theta \vec{u}]$，其中 $\vec{u}$ 为单位旋转向量。推导可得

$$\begin{array}{rl}{q}^2=qq& =[\cos \theta ,\sin \theta \vec{u}][\cos \theta ,\sin \theta \vec{u}]\\ & =[{\cos }^2\theta -\sin \theta \vec{u}\cdot \sin \theta \vec{u},\cos \theta \sin \theta \vec{u}+\cos \theta \sin \theta \vec{u}+\sin \vec{u}\times \sin \theta \vec{u}]\\ & =[{\cos }^2\theta -{\sin }^2\theta {\vec{u}}^2,2\cos \theta \sin \theta \vec{u}+0]\\ & =[{\cos }^2\theta -{\sin }^2\theta ,2\cos \theta \sin \theta \vec{u}]\\ & =[\cos (2\theta ),\sin (2\theta )\vec{u}]\end{array}\text{\hspace{1em}(3-5-23)}$$

公式表明，绕 $\vec{u}$ 连续旋转 $\theta$角两次，相当于直接旋转 $2\theta$ 度。

② 接下来将式（3-5-22）变形。根据上式，令 $q=p^2$ ，则 $p=[\cos (\frac{\theta }{2}),\sin (\frac{\theta }{2})\vec{u}]$，则

$$\begin{array}{rl}\overrightarrow{v^{\prime }}& ={\vec{v}}_{\parallel }+v_{\perp }^{\prime }\\ & ={\vec{v}}_{\parallel }+q{v}_{\perp }\\ & =(p{p}^{-1}){\vec{v}}_{\parallel }+pp{\vec{v}}_{\perp }\end{array}\text{\hspace{1em}(3-5-24)}$$

因为 $p$是单位四元数，可得

$$q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2}=q^{\ast }\text{\hspace{1em}(3-5-25)}$$

代入上式

$$\begin{array}{rl}\overrightarrow{v^{\prime }}& =(p{p}^{-1}){\vec{v}}_{\parallel }+pp{\vec{v}}_{\perp }\\ & =(p{p}^{\ast }){\vec{v}}_{\parallel }+pp{\vec{v}}_{\perp }\end{array}\text{\hspace{1em}(3-5-26)}$$

③ 证明一个引理：若 $v_{\parallel }=[0,{\vec{v}}_{\parallel }]$，$q=[\alpha ,\beta \vec{u}]$，$\vec{u}$为单位旋转向量，且 ${\vec{v}}_{\parallel }\parallel \vec{u}$，则有

$$q{v}_{\parallel }=v_{\parallel }q\text{\hspace{1em}(3-5-27)}$$

下面进行证明：

$$\begin{array}{rl}左边=q{v}_{\parallel }& =[\alpha ,\beta \vec{u}]\cdot [0,{\vec{v}}_{\parallel }]\\ & =[-\beta \vec{u}\cdot {\vec{v}}_{\parallel },\alpha {\vec{v}}_{\parallel }+0+\beta \vec{u}\times {\vec{v}}_{\parallel }]\\ & =[-\beta \vec{u}\cdot {\vec{v}}_{\parallel },\alpha {\vec{v}}_{\parallel }]\end{array}$$

$$\begin{array}{rl}右边=v_{\parallel }q& =[0,{\vec{v}}_{\parallel }]\cdot [\alpha ,\beta \vec{u}]\\ & =[-\beta {\vec{v}}_{\parallel }\cdot \vec{u},\alpha {\vec{v}}_{\parallel }+\beta {\vec{v}}_{\parallel }\times \vec{u}]\\ & =[-\beta {\vec{v}}_{\parallel }\cdot \vec{u},\alpha {\vec{v}}_{\parallel }]\\ & =左边\end{array}$$

证毕。

④ 若 $v_{\perp }=[0,{\vec{v}}_{\perp }]$，$q=[\alpha ,\beta \vec{u}]$，$\vec{u}$ 为单位旋转向量，且 ${\vec{v}}_{\perp }\perp \vec{u}$，则有

$$q{v}_{\perp }=v_{\perp }{q}^{\ast }\text{\hspace{1em}(3-5-28)}$$

证明：

$$\begin{array}{rl}左边=q{v}_{\perp }& =[\alpha ,\beta \vec{u}]\cdot [0,{\vec{v}}_{\perp }]\\ & =[-\beta \vec{u}\cdot {\vec{v}}_{\perp },\alpha {\vec{v}}_{\perp }+0+\beta \vec{u}\times {\vec{v}}_{\perp }]\\ & =[-\beta \vec{u}\cdot {\vec{v}}_{\perp },\alpha {\vec{v}}_{\perp }+\beta \vec{u}\times {\vec{v}}_{\perp }]\end{array}$$
$$\begin{array}{rlr}右边=v_{\perp }{q}^{\ast }& =[0,{\vec{v}}_{\perp }]\cdot [\alpha ,-\beta \vec{u}]& \\ & =[-\beta {\vec{v}}_{\perp }\cdot \vec{u},\alpha {\vec{v}}_{\perp }-\beta {\vec{v}}_{\perp }\times \vec{u}]& \\ & =[-\beta {\vec{v}}_{\perp }\cdot \vec{u},\alpha {\vec{v}}_{\perp }+\beta \vec{u}\times {\vec{v}}_{\perp }]& =左边\end{array}$$

证毕。

3）结合上面两个引理和式（3-5-25），对式（3-5-26）推导（写成四元数形式）

$$\begin{array}{rl}{v}^{\prime }& =(p{p}^{\ast })v_{\parallel }+pp{v}_{\perp }\\ & =p(p^{\ast }{v}_{\parallel })+p(p{v}_{\perp })\\ & =p{v}_{\parallel }{p}^{\ast }+p{v}_{\perp }{p}^{\ast }\\ & =p(v_{\parallel }+v_{\perp })p^{\ast }\\ & =pv{p}^{\ast }\\ & =pv{p}^{-1}\end{array}\text{\hspace{1em}(3-5-26)}$$

CH3-6 四元数、旋转向量、旋转矩阵、欧拉角之间的转换

1、四元数和旋转向量的互相转换

由式（3-5-26），

$$v^{\prime }=p{p}^{\ast }{v}_{\parallel }+pp{v}_{\perp }\text{\hspace{1em}(3-6-1)}$$

可以看出，$v_{\parallel }$ 经过`$p$ 和 $p^{\ast }$ 两次旋转，抵消了；而 $v_{\perp }$ 经过两次旋转 $\frac{\theta }{2}$，变成了 $v_{\perp }^{\prime }$，这也说明了为什么 $p=[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\vec{u}]$。

假设已知单位旋转四元数`$p=[a,\vec{b}]$，根据上面的对应关系，可以提取出旋转角度和旋转向量：

$$\{\begin{array}{c}\frac{\theta }{2}=\arccos a\\ \vec{u}=\frac{\vec{b}}{\sin \frac{\theta }{2}}=\frac{\vec{b}}{\sin (\arccos a)}\end{array}\text{\hspace{1em}(3-6-2)}$$

反之，知道旋转向量也可求出四元数。

2、旋转矩阵和四元数相互转换

（1）前面我们已经推导过，左乘四元数 $q_1=a+b\vec{i}+c\vec{j}+d\vec{k}$，相当于左乘矩阵

$$\mathbf{L}(q)=\left[\begin{array}{cccc}a& -b& -c& -d\\ b& a& -d& c\\ c& d& a& -b\\ d& -c& b& a\end{array}\right]\text{\hspace{1em}(3-6-3)}$$

类似的，右乘矩阵为

$$\mathbf{R}(q)=\left[\begin{array}{cccc}a& -b& -c& -d\\ b& a& d& -c\\ c& -d& a& b\\ d& c& -b& a\end{array}\right]\text{\hspace{1em}(3-6-4)}$$

利用这两个式子，将 $v^{\prime }=qv{q}^{\ast }$ （式3-5-26）写成矩阵乘法形式。已知 $p=[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\vec{u}]$，令 $\vec{u}=[u_x,u_y,u_z]$，则

$$q=\cos \frac{\theta }{2}+\sin \frac{\theta }{2}{u}_x\vec{i}+\sin \frac{\theta }{2}{u}_y\vec{j}+\sin \frac{\theta }{2}{u}_z\vec{k}\text{\hspace{1em}(3-6-5)}$$

根据对应关系

$$\{\begin{array}{c}a=\cos \frac{\theta }{2}\\ b=\sin \frac{\theta }{2}{u}_x\\ c=\sin \frac{\theta }{2}{u}_y\\ d=\sin \frac{\theta }{2}{u}_z\end{array}\text{\hspace{1em}(3-6-6)}$$

将四元数乘法写为矩阵乘法形式

$$\begin{array}{rl}{v}^{\prime }=qv{q}^{\ast }=& \mathbf{L}(q)\mathbf{R}(q^{\ast })\mathbf{v}\\ =& \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1-2c^2-2d^2& 2bc-2ad& 2ac+2bd\\ 0& 2bc+2ad& 1-2b^2-2d^2& 2cd-2ab\\ 0& 2bd-2ac& 2ab+2cd& 1-2b^2-2c^2\end{array}\right]\left[\begin{array}{c}0\\ v_x\\ v_y\\ v_z\end{array}\right]\end{array}\text{\hspace{1em}(3-6-7)}$$

矩阵外围不会对变换产生影响，整理为

$$\overrightarrow{v^{\prime }}=\left[\begin{array}{ccc}1-2c^2-2d^2& 2bc-2ad& 2ac+2bd\\ 2bc+2ad& 1-2b^2-2d^2& 2cd-2ab\\ 2bd-2ac& 2ab+2cd& 1-2b^2-2c^2\end{array}\right]\vec{v}\text{\hspace{1em}(3-6-7)}$$
至此，我们由旋转向量得到了单位四元数，再得到了旋转矩阵。

（2）再由旋转矩阵恢复四元数，注意单位四元数（$a^2+b^2+c^2+d^2=1$）

$$\begin{array}{rl}{q}_0& =\frac{\sqrt{tr(\mathbf{R})+1}}{2}\\ & =\frac{\sqrt{(3-4b^2-4c^2-4d^2)+1}}{2}=\frac{\sqrt{4-4b^2-4c^2-4d^2}}{2}\\ & =\frac{\sqrt{4(1-b^2-c^2-d^2)}}{2}=\frac{\sqrt{4a^2}}{2}\\ & =a\end{array}\text{\hspace{1em}(3-6-8)}$$

其余三项分别为

$$q_1=\frac{m_{32}-m_{23}}{4q_0}=\frac{4ab}{4a}=b$$

$$q_2=\frac{m_{13}-m_{31}}{4q_0}=\frac{4ac}{4a}=c$$

$$q_1=\frac{m_{21}-m_{12}}{4q_0}=\frac{4ad}{4a}=d\text{\hspace{1em}(3-6-9)}$$

证毕。

3、旋转矩阵和相互转换

（1）欧拉角转旋转矩阵

假设绕 XYZ 三个轴的旋转角度分别为 $\alpha$ $\beta$ $\gamma$，则三次旋转的旋转矩阵分别为

$$\begin{array}{rl}& R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right]\\ & R_y(\beta )=\left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right]\\ & R_z(\gamma )=\left[\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & \\ 0& 0& 1\end{array}\right]\end{array}$$

可得旋转矩阵为

$$R=R_z(\gamma )\ast R_y(\beta )\ast R_x(\alpha )=\left[\begin{array}{ccc}\cos \beta \cos \gamma & \sin \alpha \sin \beta \cos \gamma -\cos \alpha \sin \gamma & \cos \alpha \sin \beta \cos \gamma +\sin \alpha \sin \gamma \\ \cos \beta \sin \gamma & \sin \alpha \sin \beta \sin \gamma +\cos \alpha \cos \gamma & \cos \alpha \sin \beta \sin \gamma -\sin \alpha \cos \gamma \\ -\sin \beta & \sin \alpha \cos \beta & \cos \alpha \cos \beta \end{array}\right]$$

（2）旋转矩阵转欧拉角

有旋转矩阵

$$R=\left[\begin{array}{lll}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

欧拉角为

$$\begin{array}{c}{\theta }_x=\mathrm{atan}2\left(r_{32},r_{33}\right)\\ {\theta }_y=\mathrm{atan}2\left(-r_{31},\sqrt{r_{32}^2+r_{33}^2}\right)\\ {\theta }_z=\mathrm{atan}2\left(r_{21},r_{11}\right)\end{array}$$

CH3-7 四元数复合

向量 $v$ 分别绕不同轴、 不同角度旋转两次

$$\begin{gathered} v^{\prime }=q_{1}v{q}_1^{\ast } \\ {v}^{\prime \prime }=q_2{v}^{\prime }{q}_2^{\ast }\text{\hspace{1em}(3-7-1)} \end{gathered}$$

合起来，写为

$$v^{\prime \prime }=q_2{q}_{1}v{q}_1^{\ast }{q}_2^{\ast }\text{\hspace{1em}(3-7-2)}$$

先证明一个引理：

对任意四元数 $q_1=[s,\vec{v}]$、$q_2=[t,\vec{u}]$，有

$$q_1^{\ast }{q}_2^{\ast }=(q_2{q}_1{)}^{\ast }\text{\hspace{1em}(3-7-3)}$$

证明：

$$\begin{array}{rl}左边& =q_1^{\ast }{q}_2^{\ast }\\ & =[s,-\vec{v}][t,-\vec{u}]\\ & =[st-\vec{u}\cdot \vec{v},-s\vec{u}-t\vec{v}+\vec{v}\times \vec{u}]\end{array}$$

$$\begin{array}{rl}右边& =(q_2{q}_1{)}^{\ast }\\ & =([t,\vec{u}][s,\vec{v}]{)}^{\ast }\\ & =([st-\vec{u}\cdot \vec{v},s\vec{u}+t\vec{v}+\vec{u}\times \vec{v}]{)}^{\ast }\\ & =[st-\vec{u}\cdot \vec{v},-s\vec{u}-t\vec{v}+\vec{v}\times \vec{u}]\\ & =左边\end{array}$$

证毕。

对于式（3-7-2）

$$\begin{array}{rl}{v}^{\prime \prime }& =q_2{q}_{1}v{q}_1^{\ast }{q}_2^{\ast }\\ & =(q_2{q}_1)v(q_2{q}_1{)}^{\ast }\\ & =qv{q}^{\ast }\end{array}$$

也就是说，$v$ 绕 $q_1$、$q_2$旋转两次等价于绕一个全新的轴旋转一次，且 $q=q_2{q}_1$