SLAM基础：旋转向量与四元数

3D矢量旋转

$$\begin{array}{rl}\mathbf{x}& ={\mathbf{x}}_{\parallel }+{\mathbf{x}}_{\perp }\\ {\mathbf{x}}_{\parallel }& =\mathbf{u}(\Vert \mathbf{x}\Vert \cos \alpha )={\mathbf{uu}}^T\mathbf{x}\\ {\mathbf{x}}_{\perp }& =\mathbf{x}-{\mathbf{x}}_{\parallel }=\mathbf{x}-{\mathbf{uu}}^T\mathbf{x}\end{array}$$
平行部分不旋转：
$${\mathbf{x}}_{\parallel }^{\prime }={\mathbf{x}}_{\parallel }$$
垂直部分在垂直于$\mathbf{u}$向量的平面内经历一个平面旋转。如果我们建立这个平面的一个正交基$\{{\mathbf{e}}_1,{\mathbf{e}}_2\}$：
$$\begin{gathered} {\mathbf{e}}_1={\mathbf{x}}_{\perp } \\ {\mathbf{e}}_2=\mathbf{u}\times {\mathbf{x}}_{\perp }=\mathbf{u}\times \mathbf{x} \end{gathered}$$
满足$\Vert {\mathbf{e}}_1\Vert =\Vert {\mathbf{e}}_2\Vert$，${\mathbf{x}}_{\perp }={\mathbf{e}}_1\cdot 1+{\mathbf{e}}_2\cdot 0$ 平面上的旋转角度$\varphi$
$$\begin{array}{rl}{\mathbf{x}}_{\perp }^{\prime }& ={\mathbf{e}}_1\cos \varphi +{\mathbf{e}}_2\sin \varphi \\ & ={\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi \end{array}$$
则
$$\begin{array}{rl}{\mathbf{x}}^{\prime }& ={\mathbf{x}}_{\parallel }^{\prime }+{\mathbf{x}}_{\perp }^{\prime }\\ & ={\mathbf{x}}_{\parallel }+{\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi \end{array}$$

旋转向量

任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量(或轴角/角轴，Axis-Angle)，只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。
考虑某个用旋转矩阵$\mathbf{R}$表示的旋转。假设旋转轴为一个单位长度的向量$\mathbf{n}$，角度为$\theta$，那么向量$\theta \mathbf{n}$也可以描述这个旋转。
从旋转向量到旋转矩阵的转换过程由罗德里格斯公式(Rodrigues’s Formula)表明，旋转向量其实就是特殊正交群的李代数，从李代数层面证明罗德里格斯公式。
$$\mathbf{R}=\cos \theta \mathbf{I}+\left(1-\cos \theta \right)\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }$$
$$\begin{array}{rl}\mathrm{tr}\left(\mathbf{R}\right)& =\cos \theta \mathrm{tr}(\mathbf{I})+(1-\cos \theta )\mathrm{tr}(\mathbf{n}{\mathbf{n}}^T)+\sin \theta \mathrm{tr}({\mathbf{n}}^{\wedge })\\ & =3\cos \theta +1-\cos \theta +0=2\cos \theta +1\end{array}$$
因此
$$\theta =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2}$$
旋转轴上的向量在旋转之后不发生改变
$$\mathbf{Rn}=\mathbf{n}$$
因此转轴$\mathbf{n}$是旋转矩阵$\mathbf{R}$特征值为1对应的特征向量。求解此方程，再归一化。

四元数

四元数定义

如果我们有两个复数$A=a+bi$和$C=c+di$，接着构建$Q=A+Cj$并定义$k\triangleq ij$则产生一个四元数空间$\mathbb{H}$中的数：
$$Q=a+bi+cj+dk\in \mathbb{H}$$
其中$\{a,b,c,d\}\in \mathbb{R}$，$\{i,j,k\}$是三个虚单位数：
$$i^2=j^2=k^2=ijk=-1$$
并且由上式可以导出
$$ij=-ji=k,jk=-kj=i,ki=-ik=j$$
一个四元数也可以表示为标量+向量的形式：
$$Q=q_w+q_{x}i+q_{y}j+q_{z}k⟺Q=q_w+{\mathbf{q}}_v$$
其中$q_w$代表实部，${\mathbf{q}}_v=q_{x}i+q_{y}j+q_{z}k=(q_x,q_y,q_z)$作为虚部或向量部分
我们更多会将一个四元数$Q$作为4维向量$\mathbf{q}$
q ≜ [ q w q v ] = [ q w q x q y q z ] \bold{q}\triangleq\begin{bmatrix}q_w\\\bold{q}_v\end{bmatrix}=\begin{bmatrix}q_w\\q_x\\q_y\\q_z\end{bmatrix} q≜[qw​qv​​]= ​qw​qx​qy​qz​​ ​
常规单位长度的复数$\mathbf{z}=e^{i\theta }$可以编码2D平面上的旋转：使用复数乘法${\mathbf{x}}^{\prime }=\mathbf{z}\cdot \mathbf{x}$
单位长度的扩展复数(四元数)$\mathbf{q}=e^{(q_{x}i+q_{y}j+q_{z}k)\theta /2}$可以编码3D空间中的旋转：使用双四元数乘法$\mathbf{x}=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }$

四元数的基本运算

相乘

$${\mathbf{q}}_{\mathbf{a}}\otimes {\mathbf{q}}_{\mathbf{b}}=(s_a+{\mathbf{v}}_{\mathbf{a}})\otimes (s_b+{\mathbf{v}}_{\mathbf{b}})=s_a{s}_b+s_a{\mathbf{v}}_{\mathbf{b}}+s_b{\mathbf{v}}_{\mathbf{a}}+{\mathbf{v}}_{\mathbf{a}}\otimes {\mathbf{v}}_{\mathbf{b}}$$
最后一项两个向量相乘根据虚部满足的关系：
$$\{\begin{array}{l}{\mathrm{i}}^2={\mathrm{j}}^2={\mathrm{k}}^2=-1\\ \mathrm{ij}=\mathrm{k},\mathrm{ji}=-\mathrm{k}\\ \mathrm{jk}=\mathrm{i},\mathrm{kj}=-\mathrm{i}\\ \mathrm{ki}=\mathrm{j},\mathrm{ik}=-\mathrm{j}\end{array}$$
第一个规则相同虚部相乘等价于向量点乘的相反数，后三条规则相异虚部相乘与与向量叉乘相同，则：
$${\mathbf{v}}_a\otimes {\mathbf{v}}_b=-{\mathbf{v}}_a^T{\mathbf{v}}_b+{\mathbf{v}}_a\times {\mathbf{v}}_b$$

$$\begin{array}{rl}{\mathbf{q}}_a\otimes {\mathbf{q}}_b& =(s_a{s}_b-{\mathbf{v}}_a^T{\mathbf{v}}_b)+s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\\ & =\left[\begin{array}{c}{s}_a{s}_b-{\mathbf{v}}_a^T{\mathbf{v}}_b\\ s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\end{array}\right]\\ & =\left[\begin{array}{cc}{s}_a& -{\mathbf{v}}_a^T\\ {\mathbf{v}}_a& s_a\mathbf{I}+{\mathbf{v}}_a^{\wedge }\end{array}\right]\left[\begin{array}{c}{s}_b\\ {\mathbf{v}}_b\end{array}\right]\\ & =\left[\begin{array}{cc}{s}_b& -{\mathbf{v}}_b^T\\ {\mathbf{v}}_b& s_b\mathbf{I}-{\mathbf{v}}_b^{\wedge }\end{array}\right]\left[\begin{array}{c}{s}_a\\ {\mathbf{v}}_a\end{array}\right]\end{array}$$
四元数相乘满足结合律但不满足交换律：
$$\begin{array}{rl}{\mathbf{q}}_a\otimes {\mathbf{q}}_b& \ne {\mathbf{q}}_b\otimes {\mathbf{q}}_a\\ ({\mathbf{q}}_a\otimes {\mathbf{q}}_b)\otimes {\mathbf{q}}_c& ={\mathbf{q}}_a\otimes ({\mathbf{q}}_b\otimes {\mathbf{q}}_c)\end{array}$$

设$\mathbf{q}=\left[\begin{array}{c}s\\ \mathbf{v}\end{array}\right]$定义如下的符号$[\cdot {]}_L$和$[\cdot {]}_R$为
$$\begin{array}{rl}{\left[\mathbf{q}\right]}_L& =\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]\\ {\left[\mathbf{q}\right]}_R& =\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s\mathbf{I}-{\mathbf{v}}^{\wedge }\end{array}\right]\end{array}$$
则
$$\begin{array}{r}{\mathbf{q}}_a\otimes {\mathbf{q}}_b={\left[{\mathbf{q}}_a\right]}_L{\mathbf{q}}_b={\left[{\mathbf{q}}_b\right]}_R{\mathbf{q}}_a\end{array}$$

共轭

四元数的共轭就是把虚部取成相反数：
$$\begin{array}{r}{\mathbf{q}}_a^{\ast }=s_a-{\mathbf{v}}_a=\left[\begin{array}{c}{s}_a\\ -{\mathbf{v}}_a\end{array}\right]\end{array}$$
四元数共轭与其本身相乘，会得到一个实四元数
$$\begin{array}{r}{\mathbf{q}}_a^{\ast }\otimes {\mathbf{q}}_a={\mathbf{q}}_a\otimes {\mathbf{q}}_a^{\ast }=\left[\begin{array}{c}{s}_a^2+{\mathbf{v}}_a^T{\mathbf{v}}_a\\ 0\end{array}\right]\end{array}$$

模长

$$\begin{array}{r}\Vert {\mathbf{q}}_a\Vert =\sqrt{{\mathbf{q}}_a\otimes {\mathbf{q}}_a^{\ast }}=\sqrt{s_a^2+{\mathbf{v}}_a^T{\mathbf{v}}_a}=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}\end{array}$$
可以验证，两个四元数乘积的模即模的乘积
$$\begin{array}{r}\Vert {\mathbf{q}}_a\otimes {\mathbf{q}}_b\Vert =\Vert {\mathbf{q}}_a\Vert \Vert {\mathbf{q}}_b\Vert \end{array}$$
证明：
$$\begin{array}{rl}\Vert {\mathbf{q}}_a\otimes {\mathbf{q}}_b{\Vert }^2=& {\left(s_a{s}_b-{\mathbf{v}}_a^T{\mathbf{v}}_b\right)}^2+{\left(s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\right)}^T\left(s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\right)\\ =& s_a^2{s}_b^2-2s_a{s}_b{\mathbf{v}}_a^T{\mathbf{v}}_b+{\left({\mathbf{v}}_a^T{\mathbf{v}}_b\right)}^2+s_a^2{\mathbf{v}}_b^T{\mathbf{v}}_b+s_b^2{\mathbf{v}}_a^T{\mathbf{v}}_a+{\left({\mathbf{v}}_a\times {\mathbf{v}}_b\right)}^T\left({\mathbf{v}}_a\times {\mathbf{v}}_b\right)+s_a{s}_b{\mathbf{v}}_b^T{\mathbf{v}}_a+s_a{s}_b{\mathbf{v}}_a^T{\mathbf{v}}_b\\ & +s_a\underset{0}{{\mathbf{v}}_b^T\left({\mathbf{v}}_a\times {\mathbf{v}}_b\right)}+s_b\underset{0}{{\mathbf{v}}_a^T\left({\mathbf{v}}_a\times {\mathbf{v}}_b\right)}+s_a\underset{0}{{\left({\mathbf{v}}_a\times {\mathbf{v}}_b\right)}^T{\mathbf{v}}_b}+s_b\underset{0}{{\left({\mathbf{v}}_a\times {\mathbf{v}}_b\right)}^T{\mathbf{v}}_a}\\ =& s_a^2{s}_b^2+s_a^2{\mathbf{v}}_b^T{\mathbf{v}}_b+s_b^2{\mathbf{v}}_a^T{\mathbf{v}}_a+{\left({\mathbf{v}}_a^T{\mathbf{v}}_b\right)}^2+{\left({\mathbf{v}}_a\times {\mathbf{v}}_b\right)}^T\\ =& s_a^2{s}_b^2+s_a^2{\mathbf{v}}_b^T{\mathbf{v}}_b+s_b^2{\mathbf{v}}_a^T{\mathbf{v}}_a+{\Vert {\mathbf{v}}_a\Vert }^2{\Vert {\mathbf{v}}_b\Vert }^2{\cos }^2\theta +{\Vert {\mathbf{v}}_a\Vert }^2{\Vert {\mathbf{v}}_b\Vert }^2{\sin }^2\theta \\ =& s_a^2{s}_b^2+s_a^2{\mathbf{v}}_b^T{\mathbf{v}}_b+s_b^2{\mathbf{v}}_a^T{\mathbf{v}}_a+{\Vert {\mathbf{v}}_a\Vert }^2{\Vert {\mathbf{v}}_b\Vert }^2\end{array}$$
且
$$\begin{array}{rl}{\Vert {\mathbf{q}}_a\Vert }^2{\Vert {\mathbf{q}}_b\Vert }^2& =\left(s_a^2+{\mathbf{v}}_a^T{\mathbf{v}}_a\right)\left(s_b^2+{\mathbf{v}}_b^T{\mathbf{v}}_b\right)\\ & =s_a^2{s}_b^2+s_a^2{\mathbf{v}}_b^T{\mathbf{v}}_b+s_b^2{\mathbf{v}}_a^T{\mathbf{v}}_a+{\mathbf{v}}_a^T{\mathbf{v}}_a{\mathbf{v}}_b^T{\mathbf{v}}_b\\ & =s_a^2{s}_b^2+s_a^2{\mathbf{v}}_b^T{\mathbf{v}}_b+s_b^2{\mathbf{v}}_a^T{\mathbf{v}}_a+{\Vert {\mathbf{v}}_a\Vert }^2{\Vert {\mathbf{v}}_b\Vert }^2\end{array}$$
故
$$\begin{array}{r}\Vert {\mathbf{q}}_a\otimes {\mathbf{q}}_b\Vert =\Vert {\mathbf{q}}_a\Vert \Vert {\mathbf{q}}_b\Vert \end{array}$$

逆

四元数的逆为
$$\begin{array}{r}{\mathbf{q}}^{-1}=\frac{{\mathbf{q}}^{\ast }}{\Vert \mathbf{q}{\Vert }^2}\end{array}$$
按此定义，四元数和自己的逆的乘积为实四元数1：
$$\mathbf{q}\otimes {\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\otimes \mathbf{q}=1$$
乘积的逆具有与矩阵相似的性质：
$$\begin{array}{r}({\mathbf{q}}_a\otimes {\mathbf{q}}_b{)}^{-1}={\mathbf{q}}_b^{-1}\otimes {\mathbf{q}}_a^{-1}\end{array}$$

单位四元数总是可以表示成以下形式：
$$\begin{array}{r}\mathbf{q}=\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]\end{array}$$
其中$\mathbf{u}$是单位向量。

四元数括号

$$\begin{array}{rl}[\mathbf{p},\mathbf{q}]& \triangleq \mathbf{p}\otimes \mathbf{q}-\mathbf{q}\otimes \mathbf{p}\\ & =2{\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}$$

四元数的指数映射

纯虚四元数相乘：

$$\begin{array}{rl}{\mathbf{v}}_a\otimes {\mathbf{v}}_b& =\left[\begin{array}{c}-{\mathbf{v}}_a^T{\mathbf{v}}_b\\ {\mathbf{v}}_a\times {\mathbf{v}}_b\end{array}\right]\\ {\mathbf{v}}_a\otimes {\mathbf{v}}_a& =-{\mathbf{v}}_a^T{\mathbf{v}}_a=-\Vert {\mathbf{v}}_a{\Vert }^2\end{array}$$
对于单位纯虚四元数
$$\begin{array}{r}\mathbf{u}\otimes \mathbf{u}=-1\end{array}$$
纯虚四元数的幂：

令$\mathbf{v}=\theta \mathbf{u}$，$\theta =\Vert \mathbf{v}\Vert \in \mathbb{R}$，$\mathbf{u}$是单位向量。
$$\begin{array}{r}{\mathbf{v}}^2=-{\theta }^2,{\mathbf{v}}^3=-\mathbf{u}{\theta }^3,{\mathbf{v}}^4={\theta }^4,\cdots \end{array}$$

四元数的指数映射
$$\begin{array}{r}\exp (\mathbf{q})=\sum _{k=0}^{\infty }\frac{1}{k!}{\mathbf{q}}^k\end{array}$$
纯虚四元数$\mathbf{v}=v_x\mathrm{i}+v_y\mathrm{j}+v_z\mathrm{k}$
$$\begin{array}{r}\exp (\mathbf{v})=\sum _{k=0}^{\infty }\frac{1}{k!}{\mathbf{v}}^k\end{array}$$
令$\mathbf{v}=\theta \mathbf{u}$，$\theta =\Vert \mathbf{v}\Vert \in \mathbb{R}$，$\mathbf{u}$是单位向量。
$$\begin{array}{rl}\exp (\theta \mathbf{u})=\left(1-\frac{{\theta }^2}{2!}+\frac{{\theta }^4}{4!}+\cdots \right)& +\left(\mathbf{u}\theta -\frac{\mathbf{u}{\theta }^3}{3!}+\frac{\mathbf{u}{\theta }^5}{5!}+\cdots \right)=\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]\\ \Vert \exp (\mathbf{u}\theta )\Vert & =\cos {\theta }^2+\sin {\theta }^2=1\end{array}$$
这是欧拉公式的拓展：$\exp (i\theta )=\cos \theta +i\sin \theta ,\Vert \exp (i\theta )\Vert =1$

四元数指数函数的共轭：
$$\exp (-\mathbf{v})={\left(\exp (\mathbf{v})\right)}^{\ast }$$
普通四元数的指数映射：
$$\exp (\mathbf{q})=\exp (s+\mathbf{v})=\exp (s)\exp (\mathbf{v})=\exp (s)\left[\begin{array}{c}\cos \Vert \mathbf{v}\Vert \\ \frac{\mathbf{v}}{\Vert \mathbf{v}\Vert }\sin \Vert \mathbf{v}\Vert \end{array}\right]$$
上式成立的原因是标量乘法可交换$s\mathbf{v}=\mathbf{v}s$

四元数的对数映射

单位四元数的对数映射

$$\begin{array}{r}\log \mathbf{q}=\log (\cos \theta +\mathbf{u}\sin \theta )=\log \left(\exp (\theta \mathbf{u})\right)=\theta \mathbf{u}=\left[\begin{array}{c}0\\ \theta \mathbf{u}\end{array}\right]\end{array}$$
其中
$$\begin{array}{rl}\mathbf{u}& =\frac{\mathbf{v}}{\Vert \mathbf{v}\Vert }\\ \theta & =\arctan (\Vert \mathbf{v}\Vert ,s)\end{array}$$

普通四元数的对数映射

$$\begin{array}{r}\log \mathbf{q}=\log \left(\Vert \mathbf{q}\Vert \frac{\mathbf{q}}{\Vert \mathbf{q}\Vert }\right)=\log \left(\Vert \mathbf{q}\Vert \right)+\log \left(\frac{\mathbf{q}}{\Vert \mathbf{q}\Vert }\right)=\log \left(\Vert \mathbf{q}\Vert \right)+\theta \mathbf{u}=\left[\begin{array}{c}\log \left(\Vert \mathbf{q}\Vert \right)\\ \theta \mathbf{u}\end{array}\right]\end{array}$$
Exponential forms of the type of ${\mathbf{q}}^t$:
$${\mathbf{q}}^t=\exp \left(\log \left({\mathbf{q}}^t\right)\right)=\exp \left(t\log \left(\mathbf{q}\right)\right)=\exp \left(t\cdot \theta \mathbf{u}\right)=\left[\begin{array}{c}\cos t\theta \\ \mathbf{u}\sin t\theta \end{array}\right]$$

用四元数表示旋转

假设有一个空间三维点$\mathbf{p}=[x,y,z]\in {\mathbb{R}}^3$，以及一个由单位四元数$\mathbf{q}$指定的旋转。
首先，把三维空间点用一个虚四元数来描述：
$$\mathbf{p}=[0,x,y,z{]}^T=[0,\mathbf{v}{]}^T$$
相当于把四元数的三个虚部与空间中的三个轴相对应。那么，旋转后的点$\mathbf{p}{}^{\prime }$可以表示为这样的乘积：
$$\begin{array}{r}r(\mathbf{p})={\mathbf{p}}^{\prime }=\mathbf{q}\otimes \mathbf{p}\otimes {\mathbf{q}}^{-1}\end{array}$$
可以验证结果的实部为0。最后把$\mathbf{p}{}^{\prime }$的虚部取出即得旋转之后点的坐标。

由四元数括号可知：
$$\begin{array}{rl}r\left(\mathbf{v}\right)\times r\left(\mathbf{w}\right)& =\left(\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }\right)\times \left(\mathbf{q}\otimes \mathbf{w}\otimes {\mathbf{q}}^{\ast }\right)\\ & =\frac{1}{2}\left(\left(\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }\right)\otimes \left(\mathbf{q}\otimes \mathbf{w}\otimes {\mathbf{q}}^{\ast }\right)-\left(\mathbf{q}\otimes \mathbf{w}\otimes {\mathbf{q}}^{\ast }\right)\otimes \left(\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }\right)\right)\\ & =\frac{1}{2}\left(\mathbf{q}\otimes \mathbf{v}\otimes \mathbf{w}\otimes {\mathbf{q}}^{\ast }-\mathbf{q}\otimes \mathbf{w}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }\right)\\ & =\frac{1}{2}\left(\mathbf{q}\otimes \left(\mathbf{v}\otimes \mathbf{w}-\mathbf{w}\otimes \mathbf{v}\right)\otimes {\mathbf{q}}^{\ast }\right)\\ & =\mathbf{q}\otimes \left(\mathbf{v}\times \mathbf{w}\right)\otimes {\mathbf{q}}^{\ast }\\ & =r\left(\mathbf{v}\times \mathbf{w}\right)\end{array}$$

四元数微分方程

一个单位四元数$\mathbf{q}\in S^3$，满足${\mathbf{q}}^{\ast }\otimes \mathbf{q}=1$，像对旋转矩阵的处理一样,${\mathbf{R}}^T\mathbf{R}=\mathbf{I}$
$$\frac{\mathrm{d}\left({\mathbf{q}}^{\ast }\otimes \mathbf{q}\right)}{\mathrm{d}t}={\dot{\mathbf{q}}}^{\ast }\otimes \mathbf{q}+{\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=0$$
可以推得
$${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=-\left({\dot{\mathbf{q}}}^{\ast }\otimes \mathbf{q}\right)=-{\left({\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}\right)}^{\ast }$$
这意味着${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}$是一个纯虚四元数。设一个纯虚四元数$\mathbf{\Omega }\in {\mathbb{H}}_p$满足
$${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=\mathbf{\Omega }=\left[\begin{array}{c}0\\ \mathbf{\Omega }\end{array}\right]$$
等式两侧左乘一个$\mathbf{q}$可得
$$\dot{\mathbf{q}}=\mathbf{q}\otimes \mathbf{\Omega }$$
在原点附近，我们有$\mathbf{q}=1$，上面的方程退化为$\dot{\mathbf{q}}=\mathbf{\Omega }\in {\mathbb{H}}_p$ 因此，纯虚四元数空间${\mathbb{H}}_p$构成了四元数单位球$S^3$的切向量或李代数。然而，在四元数的情况下，这个空间不是直接的速度空间，而是半-速度空间。
如果$\mathbf{\Omega }$为常数，微分方程可积分为
$$\mathbf{q}=\mathbf{q}(0)\otimes e^{\mathbf{\Omega }t}$$
其中$\mathbf{q}(0)$和$\mathbf{q}(t)$都是单位四元数，所以指数$e^{\mathbf{\Omega }t}$也是单位四元数，这在之前四元数的指数映射一节也已经推导过了。

定义$\mathbf{V}=\mathbf{\Omega }t$，我们有
$$\mathbf{q}=e^{\mathbf{V}}$$
这又是一个指数映射:从纯四元数空间到由单位四元数表示的旋转空间：
$$\exp :{\mathbb{H}}_p\to S^3;\mathbf{V}\mapsto \exp (\mathbf{V})=e^{\mathbf{V}}$$

四元数与旋转矩阵的转换

考虑使用单位四元数对空间点进行旋转的问题，有
$$\begin{array}{rl}{\mathbf{p}}^{\prime }& =\mathbf{q}\otimes \mathbf{p}\otimes {\mathbf{q}}^{\ast }={\left[\mathbf{q}\right]}_L\left(\mathbf{p}\otimes {\mathbf{q}}^{\ast }\right)={\left[\mathbf{q}\right]}_L{\left[{\mathbf{q}}^{\ast }\right]}_R\mathbf{p}\\ & =\left(\mathbf{q}\otimes \mathbf{p}\right)\otimes {\mathbf{q}}^{\ast }={\left[\mathbf{q}\right]}_L\mathbf{p}\otimes {\mathbf{q}}^{\ast }={\left[{\mathbf{q}}^{\ast }\right]}_R{\left[\mathbf{q}\right]}_L\mathbf{p}\end{array}$$
上式我们只利用了四元数乘法的结合律，可以得到对任意四元数都成立的如下关系式
$${\left[{\mathbf{q}}_1\right]}_R{\left[{\mathbf{q}}_2\right]}_L={\left[{\mathbf{q}}_2\right]}_L{\left[{\mathbf{q}}_1\right]}_R$$
代入两个符号对应的矩阵，得四元数乘法矩阵形式：
$$\begin{array}{rl}[\mathbf{q}{]}_L{\left[{\mathbf{q}}^{\ast }\right]}_R& =\left[\begin{array}{cc}s& -{\mathbf{v}}^T\\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]\left[\begin{array}{cc}s& {\mathbf{v}}^T\\ -\mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]\\ & =\left[\begin{array}{cc}{s}^2+{\mathbf{v}}^T\mathbf{v}& s{\mathbf{v}}^T-s{\mathbf{v}}^T-{\mathbf{v}}^T{\mathbf{v}}^{\wedge }\\ s\mathbf{v}-s\mathbf{v}-{\mathbf{v}}^{\wedge }\mathbf{v}& \mathbf{v}{\mathbf{v}}^T+{\left(s\mathbf{I}+{\mathbf{v}}^{\wedge }\right)}^2\end{array}\right]\\ & =\left[\begin{array}{cc}1& 0\\ 0& \mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+{\left({\mathbf{v}}^{\wedge }\right)}^2\end{array}\right]\end{array}$$

因为${\mathbf{p}}^{\prime }$和$\mathbf{p}$都是虚四元数，所以事实上该矩阵的右下角给出了从四元数到旋转矩阵的变换关系：
$$\begin{array}{r}\mathbf{R}=\mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+{\left({\mathbf{v}}^{\wedge }\right)}^2+2s{\mathbf{v}}^{\wedge }\end{array}$$
观察到前三项均为对称矩阵，最后一项为反对称矩阵，利用这个性质可以得到：
$$\begin{array}{r}\mathbf{R}-{\mathbf{R}}^T=4s{\mathbf{v}}^{\wedge }\end{array}$$
由下式求出$s$进而求出$\mathbf{v}$
$$\begin{array}{rl}\mathrm{tr}\left(\mathbf{R}\right)& =\mathrm{tr}\left(\mathbf{v}{\mathbf{v}}^T\right)+\mathrm{tr}\left(s^2\mathbf{I}\right)+\mathrm{tr}\left({\left({\mathbf{v}}^{\wedge }\right)}^2\right)+\mathrm{tr}\left(2s{\mathbf{v}}^{\wedge }\right)\\ & =\mathrm{tr}\left({\mathbf{v}}^T\mathbf{v}\right)+3s^2-2\left(v_1^2+v_2^2+v_3^2\right)+0\\ & =v_1^2+v_2^2+v_3^2+3s^2-2\left(v_1^2+v_2^2+v_3^2\right)\\ & =3s^2-\left(v_1^2+v_2^2+v_3^2\right)\\ & =3s^2-\left(1-s^2\right)\\ & =4s^2-1\end{array}$$
可以得出四元数转旋转矩阵有这样的性质：
$$\begin{array}{rl}\mathbf{R}\left\{{\left[1,0,0,0\right]}^T\right\}& =\mathbf{I}\\ \mathbf{R}\left\{-\mathbf{q}\right\}& =\mathbf{R}\left\{\mathbf{q}\right\}\\ \mathbf{R}\left\{{\mathbf{q}}^{\ast }\right\}& =\mathbf{R}{\left\{\mathbf{q}\right\}}^T\\ \mathbf{R}\left\{{\mathbf{q}}_1\otimes {\mathbf{q}}_2\right\}& =\mathbf{R}\left\{{\mathbf{q}}_1\right\}\otimes \mathbf{R}\left\{{\mathbf{q}}_2\right\}\end{array}$$

四元数与旋转向量的转换

由旋转向量的旋转角与旋转矩阵的转换关系可得：
$$\begin{array}{rl}& \theta =\arccos \left(\frac{\mathrm{tr}\left(\mathbf{R}\right)-1}{2}\right)=\arccos \left(2s^2-1\right)\\ & \cos \theta =2s^2-1=2{\cos }^2\frac{\theta }{2}-1\\ & ⟹\theta =2\arccos s\end{array}$$

$$\begin{array}{rl}\mathbf{R}=\mathbf{v}{\mathbf{v}}^T+s^{2}I+2s{\mathbf{v}}^{\wedge }+{\left({\mathbf{v}}^{\wedge }\right)}^2& ⟹\mathrm{tr}\left(\mathbf{R}\right)=\mathrm{tr}\left({\mathbf{v}}^T\mathbf{v}\right)+3s^2+0-2{\mathbf{v}}^T\mathbf{v}=4s^2-1\\ \mathbf{R}=\cos \theta I+\left(1-\cos \theta \right)n{n}^T+\sin \theta n^{\wedge }& ⟹\mathrm{tr}\left(\mathbf{R}\right)=3\cos \theta +1-\cos \theta +0=2\cos \theta +1\end{array}$$

$$\cos \theta =2s^2-1=2{\cos }^2\frac{\theta }{2}-1\Rightarrow s^2={\cos }^2\frac{\theta }{2}$$
至于旋转轴，将四元数乘法矩阵形式中的$\mathbf{p}$用$\mathbf{q}$的虚部代替，易知$\mathbf{q}$的虚部组成的向量在旋转时是不变的，即构成旋转轴。
$$\begin{array}{r}\mathbf{n}=\frac{\mathbf{v}}{k}(k^2=|\mathbf{v}{|}^2=1-s^2={\sin }^2\frac{\theta }{2})\end{array}$$
将上面两式代入旋转向量与旋转矩阵的转换式，得
$$\begin{array}{rl}\mathbf{R}=& \cos \theta \mathbf{I}+\left(1-\cos \theta \right)\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }\\ =& \left(2s^2-1\right)\mathbf{I}+2\left(1-s^2\right)\frac{\mathbf{v}{\mathbf{v}}^T}{1-s^2}+\sin \theta {\mathbf{n}}^{\wedge }\\ =& \left(2s^2-1\right)\mathbf{I}+2\mathbf{v}{\mathbf{v}}^T+\sin \theta {\mathbf{n}}^{\wedge }\\ =& \mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+\left[\mathbf{v}{\mathbf{v}}^T-\left(1-s^2\right)\mathbf{I}\right]+\sin \theta {\mathbf{n}}^{\wedge }\\ =& \mathbf{v}{\mathbf{v}}^T+s^2\mathbf{I}+{\left({\mathbf{v}}^{\wedge }\right)}^2+\sin \theta {\mathbf{n}}^{\wedge }\\ ⟹& 2s{\mathbf{v}}^{\wedge }=\sin \theta {\mathbf{n}}^{\wedge }=2\cos \frac{\theta }{2}\sin \frac{\theta }{2}{\mathbf{n}}^{\wedge }\\ ⟹& \frac{s}{\cos \frac{\theta }{2}}{\mathbf{v}}^{\wedge }=\frac{\sin \frac{\theta }{2}}{k}{\mathbf{v}}^{\wedge }\end{array}$$有两种对应关系：
$$\begin{array}{rl}s& =\cos \frac{\theta }{2}\\ k& =\sin \frac{\theta }{2}\end{array}\mathrm{or}\begin{array}{rl}s& =-\cos \frac{\theta }{2}\\ k& =-\sin \frac{\theta }{2}\end{array}$$
通常取
$$\begin{array}{rl}s& =\cos \frac{\theta }{2}\\ k& =\sin \frac{\theta }{2}\end{array}$$
则旋转向量转四元数：
$$\begin{array}{rl}s& =\cos \frac{\theta }{2}\\ \mathbf{v}& =\mathbf{n}\sin \frac{\theta }{2}\end{array}$$
四元数转旋转向量：
$$\begin{array}{rl}\theta & =2\arccos s\\ \mathbf{n}& =\frac{\mathbf{v}}{\sin \frac{\theta }{2}}\end{array}$$

大写指数映射

对于半-速度空间的一个通俗的解释是：旋转动作是由双积${\mathbf{x}}^{\prime }=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }$完成的，向量$\mathbf{x}$经历的旋转是由$\mathbf{q}$中编码的旋转的两倍，或者等效的，四元数$\mathbf{q}$编码了预期旋转的“一半”。
为了更好表达轴角和四元数的关系，$\mathbf{v}=\varphi \mathbf{u}\in {\mathbb{R}}^3$，我们定义一个大写版本的指数映射：
$$\mathrm{Exp}:{\mathbb{R}}^3\to S^3;\mathbf{v}\mapsto \mathrm{Exp}(\mathbf{v})=e^{\mathbf{v}/2}$$
它和指数映射的关系：
$$\mathrm{Exp}\triangleq \exp (\mathbf{v}/2)$$
引入角速度向量$\mathbf{\omega }=2\mathbf{\Omega }\in {\mathbb{R}}^3$
$$\begin{gathered} \dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega } \\ \mathbf{q}=e^{\mathbf{\omega }t/2} \end{gathered}$$
令$\mathbf{v}=\varphi \mathbf{u}$代表一个绕轴$\mathbf{u}$旋转角度$\varphi$的旋转向量
q ≜ E x p ( ϕ u ) = e ϕ u / 2 = cos ⁡ ϕ 2 + u sin ⁡ ϕ 2 = [ cos ⁡ ϕ 2 u sin ⁡ ϕ 2 ] \bold{q}\triangleq {\rm Exp}(\phi \bold{u})=e^{\phi \bold{u}/2}=\cos\dfrac{\phi}{2}+\bold{u}\sin\frac{\phi}{2}= \begin{bmatrix}\cos\dfrac{\phi}{2} \\ \bold{u}\sin\dfrac{\phi}{2}\end{bmatrix} q≜Exp(ϕu)=eϕu/2=cos2ϕ​+usin2ϕ​= ​cos2ϕ​usin2ϕ​​ ​
旋转向量到四元数的转换将被记作$\mathbf{q}=\mathbf{q}\{\mathbf{v}\}\triangleq \mathrm{Exp}(\mathbf{v})$

大写对数映射

我们定义对数映射是指数映射的逆
$$\mathrm{Log}:{\mathrm{S}}^3\to {\mathbb{R}}^3;\mathbf{q}\mapsto \mathrm{Log}(\mathbf{q})=\mathbf{u}\varphi$$
其中
$$\begin{array}{r}\varphi =2\arctan (\Vert {\mathbf{q}}_v\Vert ,q_w)\\ \mathbf{u}=\frac{{\mathbf{q}}_v}{\Vert {\mathbf{q}}_v\Vert }\end{array}$$
大写对数映射和对数映射的关系：
$$\mathrm{Log}(\mathbf{q})\triangleq 2\log (\mathbf{q})$$

Spherical linear interpolation（SLEARP）

四元数可以非常方便的计算适当的方向插值。
给定由四元数${\mathbf{q}}_1$和${\mathbf{q}}_1$表示的两个方向，我们想找到一个四元数函数$\mathbf{q}(t),t\in [0,1]$，它从$\mathbf{q}(0)={\mathbf{q}}_0$到$\mathbf{q}(1)={\mathbf{q}}_1$进行线性插值。这种插值是这样的，当$t$从0进化到1时，物体将沿着固定的轴以恒定的速度从方向${\mathbf{q}}_0$不断旋转到方向${\mathbf{q}}_1$。

方法1

第一种方法是使用四元数代数，首先计算从${\mathbf{q}}_0$到${\mathbf{q}}_1$的方向增量$\Delta \mathbf{q}$满足${\mathbf{q}}_1={\mathbf{q}}_0\otimes \Delta \mathbf{q}$
$$\Delta \mathbf{q}={\mathbf{q}}_0^{\ast }\otimes {\mathbf{q}}_1$$
接着使用对数映射获得对应的旋转向量$\Delta \mathbf{\varphi }=\mathbf{u}\Delta \varphi$
$$\mathbf{u}\Delta \varphi =\mathrm{Log}(\Delta \mathbf{q})$$
保持旋转轴$\mathbf{u}$不变，对旋转角度进行插值$\delta \varphi =t\Delta \varphi$。利用指数映射将其映射回四元数$\delta \mathbf{q}=\mathrm{Exp}(\mathbf{u}\delta \varphi )$，接着与原四元数组合得到最终的插值结果：
$$\begin{array}{rl}\mathbf{q}(t)& ={\mathbf{q}}_0\otimes \delta \mathbf{q}\\ & ={\mathbf{q}}_0\otimes \mathrm{Exp}(t\mathbf{u}\Delta \varphi )\\ & ={\mathbf{q}}_0\otimes \mathrm{Exp}(t\mathrm{Log}({\mathbf{q}}_0^{\ast }\otimes {\mathbf{q}}_1)\\ & ={\mathbf{q}}_0\otimes ({\mathbf{q}}_0^{\ast }\otimes {\mathbf{q}}_1{)}^t\end{array}$$
上式经常被实现为
$$\mathbf{q}(t)={\mathbf{q}}_0\otimes \left[\begin{array}{c}\cos (t\Delta \varphi /2)\\ \mathbf{u}\sin (t\Delta \varphi /2)\end{array}\right]$$

方法2

可以开发出与四元数代数内部无关的Slerp方法，甚至与嵌入圆弧的空间维数无关。
我们将四元数${\mathbf{q}}_0$和${\mathbf{q}}_1$作为单位球中的两个单位向量，在同一空间中进行插值，插值后的$\mathbf{q}(t)$是单位向量，它以恒定的角速度跟随连接${\mathbf{q}}_0$和${\mathbf{q}}_1$的最短球面路径。这条路径是有${\mathbf{q}}_0,{\mathbf{q}}_1$和原点定义的平面与单位球相交形成的平面弧。

第一种方法使用向量代数并遵循上面的思想。考虑${\mathbf{q}}_0$和${\mathbf{q}}_1$是两个单位向量;它们之间的夹角是由标量积得到的：
$$\cos (\Delta \theta )={\mathbf{q}}_0^T{\mathbf{q}}_1⟺\Delta \theta =\arccos ({\mathbf{q}}_0^T{\mathbf{q}}_1)$$
我们命名旋转平面为$\pi$，并构建该平面的一组单位正交基$\{{\mathbf{q}}_0,{\mathbf{q}}_{\perp }\}$，其中${\mathbf{q}}_{\perp }$来自于${\mathbf{q}}_1$对${\mathbf{q}}_0$的单位正交化
$${\mathbf{q}}_{\perp }=\frac{{\mathbf{q}}_1-({\mathbf{q}}_0^T{\mathbf{q}}_1){\mathbf{q}}_0}{\Vert {\mathbf{q}}_1-({\mathbf{q}}_0^T{\mathbf{q}}_1){\mathbf{q}}_0\Vert }$$
可以得到
$${\mathbf{q}}_1$$