Quaternion kinematics for the error-state Kalman filter 文章理解

1. 四元数的定义和性质

1.1 四元数的定义

Cayley-Dickson construction 给出了一种非常吸引人的四元数定义：如果有两个复数$A=a+bi$ 和 $C=c+di$，构造一个新的数$Q=A+Cj$ 并且定义 $k\triangleq ij$，就产生了一个在四元数空间$\mathbb{H}$ 内的数字：
$$Q=a+bi+cj+dk\in \mathbb{H}\text{\hspace{1em}(1)}$$
其中 { a , b , c , d } ∈ R \{a,b,c,d\}\in R {a,b,c,d}∈R， { i , j , k } \{ i,j,k \} {i,j,k} 是三个单位虚数，有如下定义：
$$i^2=j^2=k^2=ijk=-1\text{\hspace{1em}(2a)}$$
从中可以推导出：
$$ij=-ji=k,jk=-kj=i,ki=-ik=j\text{\hspace{1em}(2b)}$$
从公式（1）可以看出，实数、虚数、复数的概念都嵌入在四元数的定义当中，这意味着，实数、虚数、复数都是一种四元数，如下表示
$$Q=a\in \mathbb{R}\subset \mathbb{H},Q=i\in \mathbb{I}\subset \mathbb{H},Q=a+bi\in \mathbb{Z}\subset \mathbb{H}\text{\hspace{1em}(3)}$$
类似的，在四元数空间中也可以定义$\mathbb{H}$的三维纯虚子空间，将其中的数字称作纯四元数，表示为${\mathbb{H}}_p=\mathrm{I}\mathrm{m}(\mathbb{H})$，
$$Q=bi+cj+dk\in {\mathbb{H}}_p\subset \mathbb{H}\text{\hspace{1em}(4)}$$
值得注意的是，单位长度的正规复数$\mathbf{z}=e^{i\theta }$ ，通过一个复数乘积${\mathbf{x}}^{\prime }=\mathbf{z}\cdot \mathbf{x}$可以表达二维空间内的旋转，而单位长度四元数，或者说是扩展复数$\mathbf{q}=e^{(u_{x}i+u_{y}j+u_{z}k)\theta /2}$，同样可以通过四元数成法${\mathbf{x}}^{\prime }=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }$来表示三维空间内的旋转，在后面章节展示

注意：并不是所有的四元数的表达都是一样的，有一些作者将乘积$bi$ 写作 $ib$，这会导致他们获得的性质是$k=ji=-ij$，以及$ijk=1$，这是一类左手四元数(left-handed quaternion)。有的作者习惯将实部放在虚部的后面，写作$Q=ai+bj+ck+d$，这个写法对理解上没有实质影响，但是会导致许多问题的答案写法完全不同，后面会在第三章讨论

注意：还有一些额外的规则会使得四元数相关的运算看起来不同，比如我们对于旋转运算的“解释”。我们可以解释成“旋转一个向量”或者“旋转一个矩阵”，这会导致两种不同的写法。同样会在第三章进行讨论

注意：本文使用Hamilton形式，也就是公式（2）中定义的样子

1.1.1 四元数的其他表达形式

四元数实部+虚部的表达形式 { 1 , i , j , k } \{1,i,j,k\} {1,i,j,k}有时候用起来并不是很方便。在满足（2）式运算的条件下，也可以用标量+向量的形式表示：
$$Q=q_w+q_{x}i+q_{y}j+q_{z}k\iff 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=⟨q_w,{\mathbf{q}}_v⟩\text{\hspace{1em}(6)}$$
最常用的一种表示是将四元数写成一个4维向量：
$$\mathbf{q}\triangleq \left[\begin{array}{c}{q}_w\\ {\mathbf{q}}_v\end{array}\right]=\left[\begin{array}{c}{q}_w\\ q_x\\ q_y\\ q_z\end{array}\right]\text{\hspace{1em}(7)}$$
利用上面这种表示方法可以在某些情况对四元数进行矩阵运算。在某些情况下，我们希望“=”有不同的含义，比如在表达实四元数和纯四元数的时候
$$\text{general}:\mathbf{q}=q_w+{\mathbf{q}}_v=\left[\begin{array}{c}{q}_w\\ {\mathbf{q}}_v\end{array}\right]\in \mathbb{H},\text{real}:q_w=\left[\begin{array}{c}{q}_w\\ 0_v\end{array}\right]\in \mathbb{R},\text{pure}:{\mathbf{q}}_v=\left[\begin{array}{c}0\\ {\mathbf{q}}_v\end{array}\right]\in {\mathbb{H}}_p\text{\hspace{1em}(8)}$$

1.2 四元数性质

1.2.1 Sum

四元数的和差计算如下：
$$\mathbf{p}\pm \mathbf{q}=\left[\begin{array}{c}{p}_w\\ {\mathbf{p}}_v\end{array}\right]\pm \left[\begin{array}{c}{q}_w\\ {\mathbf{q}}_v\end{array}\right]\left[\begin{array}{c}{p}_w\pm q_w\\ {\mathbf{p}}_v\pm {\mathbf{q}}_v\end{array}\right]\text{\hspace{1em}(9)}$$
四元数的和差计算有交换律和结合律
$$\mathbf{p}+\mathbf{q}=\mathbf{q}+\mathbf{p}\text{\hspace{1em}(10)}$$
$$\mathbf{p}+(\mathbf{q}+\mathbf{r})=(\mathbf{p}+\mathbf{q})+\mathbf{r}\text{\hspace{1em}(11)}$$

1.2.2 乘积

四元数乘积运算用$\otimes$表示，利用公式(1)(2)，将乘积结果写作向量形式有：
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-p_x{q}_x-p_y{q}_y-p_z{q}_z\\ p_w{q}_x+p_x{q}_w+p_y{q}_z-p_z{q}_y\\ p_w{q}_y-p_x{q}_z+p_y{q}_w+p_z{q}_x\\ p_w{q}_z+p_x{q}_y-p_y{q}_x+p_z{q}_w\end{array}\right]\text{\hspace{1em}(12)}$$
也可以将乘积结果写作标量向量形式：
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-{\mathbf{p}}_v^{⊺}{\mathbf{q}}_v\\ p_w{\mathbf{q}}_v+q_w{\mathbf{p}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right]\text{\hspace{1em}(13)}$$
上式中的叉积项的存在说明四元数乘法不满足一般意义下的交换律：
$$\mathbf{p}\times \mathbf{q}\ne \mathbf{q}\times \mathbf{p}\text{\hspace{1em}(14)}$$
只有当${\mathbf{p}}_v\times {\mathbf{q}}_v=0$时，才满足交换律，具体来说就是两种情况，一是四元数本身是个实数；二是四元数的向量部分是平行的。
四元数的乘积是具有结合律的
$$(\mathbf{p}\otimes \mathbf{q})\otimes \mathbf{r}=\mathbf{p}\otimes (\mathbf{q}\otimes \mathbf{r})\text{\hspace{1em}(15)}$$
四元数乘积可以对求和运算保持分配律：
$$\begin{gathered} \mathbf{p}\otimes (\mathbf{q}+\mathbf{r})=\mathbf{p}\otimes \mathbf{q}+\mathbf{p}\otimes \mathbf{r} \\ (\mathbf{p}+\mathbf{q})\otimes \mathbf{r}=\mathbf{p}\otimes \mathbf{r}+\mathbf{q}\otimes \mathbf{r}\text{\hspace{1em}(16)} \end{gathered}$$
两个四元数的乘积是“bi-linear”的，可以表达成矩阵乘积的形式：
$$\begin{gathered} {\mathbf{q}}_1\otimes {\mathbf{q}}_2=[{\mathbf{q}}_1{]}_L{\mathbf{q}}_2 \\ {\mathbf{q}}_1\otimes {\mathbf{q}}_2=[{\mathbf{q}}_2{]}_R{\mathbf{q}}_1\text{\hspace{1em}(17)} \end{gathered}$$
其中$[\mathbf{q}{]}_L$和$[\mathbf{q}{]}_R$分别是四元数乘积矩阵的左乘和右乘形式，可以通过公式(12)(17)进行推导
$$[\mathbf{q}{]}_L=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -qy& q_x& q_w\end{array}\right],[\mathbf{q}{]}_R=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& q_z& -q_y\\ q_y& -q_z& q_w& q_x\\ q_z& q_y& -q_x& q_w\end{array}\right]\text{\hspace{1em}(18)}$$
或者根据公式(13)(17)写成更紧凑的形式：
$$[\mathbf{q}{]}_L=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{⊺}\\ {\mathbf{q}}_v& [{\mathbf{q}}_v{]}_{\times }\end{array}\right],[\mathbf{q}{]}_R=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{⊺}\\ {\mathbf{q}}_v& -[{\mathbf{q}}_v{]}_{\times }\end{array}\right]\text{\hspace{1em}(19)}$$
这里的skew operator $[\bullet {]}_{\times }$，表示叉乘矩阵：
$$[\mathbf{a}{]}_{\times }\triangleq \left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]\text{\hspace{1em}(20)}$$
这个skew operator是反对称的，即$[\mathbf{a}{]}_{\times }^{⊺}=-[\mathbf{a}{]}_{\times }$，和叉乘运算的关系是：
$$[\mathbf{a}{]}_{\times }\mathbf{b}=\mathbf{a}\times \mathbf{b},\forall \mathbf{a},\mathbf{b}\in {\mathbb{R}}^3\text{\hspace{1em}(21)}$$
最后，由于
$$(\mathbf{q}\otimes \mathbf{x})\otimes \mathbf{p}=[\mathbf{p}{]}_R[\mathbf{q}{]}_L\mathbf{x}\text{ and }\mathbf{q}\otimes (\mathbf{x}\otimes \mathbf{p})=[\mathbf{q}{]}_L[\mathbf{p}{]}_R\mathbf{x}\text{\hspace{1em}(22)}$$
可以得到等式：
$$[\mathbf{p}{]}_R[\mathbf{q}{]}_L=[\mathbf{q}{]}_L[\mathbf{p}{]}_R\text{\hspace{1em}(23)}$$
这说明四元数乘积矩阵可以左右交换，这个矩阵的其他性质将在2.8节讲解。
四元数的乘积操作$\otimes$来自于一个非交换群。这个群的单位元素，${\mathbf{q}}_1=1$和逆元素${\mathbf{q}}^{-1}$，如下所示。

1.2.3 单位元

和乘积运算相关的单位元，满足性质${\mathbf{q}}_1\otimes \mathbf{q}=\mathbf{q}\otimes {\mathbf{q}}_1=\mathbf{q}$。四元数单位元就是将‘1’表达成四元数的形式
$${\mathbf{q}}_1=1=\left[\begin{array}{c}1\\ 0_v\end{array}\right]$$

1.2.4 共轭

四元数的共轭定义如下：
$${\mathbf{q}}^{\ast }\triangleq q_w-{\mathbf{q}}_v=\left[\begin{array}{c}{q}_w\\ -{\mathbf{q}}_v\end{array}\right]\text{\hspace{1em}(24)}$$
共轭满足如下性质：
$$\mathbf{q}\otimes {\mathbf{q}}^{\ast }={\mathbf{q}}^{\ast }\otimes \mathbf{q}=q_w^2+q_x^2+q_y^2+q_z^2=\left[\begin{array}{c}{q}_w^2+q_x^2+q_y^2+q_z^2\\ 0_v\end{array}\right]\text{\hspace{1em}(25)}$$
并且有：
$$(\mathbf{p}\otimes \mathbf{q}{)}^{\ast }={\mathbf{q}}^{\ast }\otimes {\mathbf{p}}^{\ast }\text{\hspace{1em}(26)}$$

1.2.5 模（Norm）

四元数的模定义：
$$||\mathbf{q}||\triangleq \sqrt{\mathbf{q}\otimes {\mathbf{q}}^{\ast }}=\sqrt{{\mathbf{q}}^{\ast }\otimes \mathbf{q}}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}\in \mathbb{R}\text{\hspace{1em}(27)}$$
模的性质：
$$||\mathbf{p}\otimes \mathbf{q}||=||\mathbf{q}\otimes \mathbf{p}||=||\mathbf{p}||||\mathbf{q}||\text{\hspace{1em}(28)}$$

1.2.6 逆（inverse）

一个四元数的逆${\mathbf{q}}^{-1}$和四元数本身相乘得到identity
$$\mathbf{q}\otimes {\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\otimes \mathbf{q}={\mathbf{q}}_1\text{\hspace{1em}(29)}$$
四元数的逆可以通过共轭来计算
$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}|{|}^2\text{\hspace{1em}(30)}$$

1.2.7 单位或者标准四元数(unit or normalized quaternion)

对于标准四元数，$||\mathbf{q}||=1$，因此有：
$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }\text{\hspace{1em}(31)}$$
当用标准四元数表示旋转相关含义，或者作为旋转运算符时，上述性质表明，反向旋转可以简单的用一个共轭四元数表示。一个标准四元数总可以写成如下形式：
$$\mathbf{q}=\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]\text{\hspace{1em}(32)}$$
其中，$\mathbf{u}=u_{x}i+u_{y}j+u_{z}k$ ，是一个单位向量$\theta$是一个标量。
从式(28)，标准四元数的$\otimes$运算构成一个非交换群，在这个群中，四元数的逆和共轭是同一个四元数

1.3 其他属性

1.3.1 四元数换向器(commutator)

四元数换向器定义为$[\mathbf{p},\mathbf{q}]\triangleq \mathbf{p}\otimes \mathbf{q}-\mathbf{q}\otimes \mathbf{p}$， 从式(13)可得：
$$\mathbf{p}\otimes \mathbf{q}-\mathbf{q}\otimes \mathbf{p}=2{\mathbf{p}}_v\times {\mathbf{q}}_v\text{\hspace{1em}(33)}$$
可以得到一个结论：
$${\mathbf{p}}_v\otimes {\mathbf{q}}_v-{\mathbf{q}}_v\otimes {\mathbf{p}}_v=2{\mathbf{p}}_v\times {\mathbf{q}}_v\text{\hspace{1em}(34)}$$
后面会用到这个结论。

1.3.2 纯四元数的乘积

纯四元数是指没有实部的四元数，$Q={\mathbf{q}}_v\text{ or }\mathbf{q}=[0,{\mathbf{q}}_v]$。利用式（13），可得：
$${\mathbf{p}}_v\otimes {\mathbf{q}}_v={\mathbf{p}}_v^{⊺}{\mathbf{q}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v=\left[\begin{array}{c}-{\mathbf{p}}_v^{⊺}{\mathbf{q}}_v\\ {\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right]\text{\hspace{1em}(35)}$$
这意味着：
$${\mathbf{q}}_v\times {\mathbf{q}}_v=-{\mathbf{q}}_v^{⊺}{\mathbf{q}}_v=-||{\mathbf{q}}_v|{|}^2\text{\hspace{1em}(36)}$$
对于标准纯四元数 $\mathbf{u}\in {\mathbb{H}}_p$，$||\mathbf{u}||=1$，
$$\mathbf{u}\otimes \mathbf{u}=-1\text{\hspace{1em}(37)}$$
这个和标准的虚部相乘有点像，$ii=-1$

1.3.3 纯四元数的自然幂natural powers of pure quaternion

定义${\mathbf{q}}^n,n\in \mathbb{N}$，是$\mathbf{q}$ 利用$\otimes$的n次乘积，如果给定一个纯四元数$\mathbf{v}=\mathbf{u}\theta$，其中$\theta =||\mathbf{v}||\in \mathbb{R}$，$\mathbf{u}$是一个单位向量，利用公式(36)，可以得到如下的循环模式：
$${\mathbf{v}}^2=-{\theta }^2,{\mathbf{v}}^3=-\mathbf{u}{\theta }^3,{\mathbf{v}}^4={\theta }^4,{\mathbf{v}}^5=\mathbf{u}{\theta }^5,{\mathbf{v}}^6=-{\theta }^6,\dots \text{\hspace{1em}(38)}$$

1.3.4 纯四元数的指数形式

和普通的指数函数差不多，四元数的指数形式，由绝对收敛的序列构成：
$$e^{\mathbf{q}}\triangleq \sum _{k=0}^{\infty }\frac{1}{k!}{\mathbf{q}}^k\in \mathbb{H}\text{\hspace{1em}(39)}$$
可以看出四元数的指数形式定义和常规的指数函数是一致的。
特别的，纯四元数$\mathbf{v}=v_{x}i+v_{y}j+v_{z}k$的指数形式:
$$e^{\mathbf{v}}\triangleq \sum _{k=0}^{\infty }\frac{1}{k!}{\mathbf{v}}^k\in \mathbb{H}\text{\hspace{1em}(40)}$$
令$\mathbf{v}=\mathbf{u}\theta$， 其中$\theta =||\mathbf{v}||\in \mathbb{R}$，$\mathbf{u}$是单位向量，利用公式(38)，展开：
$$e^{\mathbf{u}\theta }=(1-\frac{{\theta }^2}{2!}+\frac{{\theta }^4}{4!}+...)+(\mathbf{u}\theta -\frac{\mathbf{u}{\theta }^3}{3!}+\frac{\mathbf{u}{\theta }^5}{5!}+...)\text{\hspace{1em}(41)}$$
可以看出，右侧的两项分别是$\cos \theta \text{ and }\sin \theta$。表达成：
$$e^{\mathbf{v}}=e^{\mathbf{u}\theta }+\cos \theta +\mathbf{u}\sin \theta =\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]\text{\hspace{1em}(42)}$$
上式是对欧拉公式$e^{i\theta }=\cos \theta +i\sin \theta$的拓展。注意到由于$||e^v|{|}^2={\cos }^2\theta +{\sin }^2\theta =1$，因此，纯四元数的指数形式是一个标准四元数，并且有性质
$$e^{-\mathbf{v}}=(e^{\mathbf{v}}{)}^{\ast }\text{\hspace{1em}(43)}$$
对于小角度四元数，为了避免除零问题$\mathbf{u}=\mathbf{v}/||\mathbf{v}||$，可以通过泰勒展开进行截断求解，得到不同精确度的近似
$$e^{\mathbf{v}}\approx \left[\begin{array}{c}1-{\theta }^2/2\\ \mathbf{v}(1-{\theta }^2/6)\end{array}\right]\approx \left[\begin{array}{c}1\\ \mathbf{v}\end{array}\right]\underset{\theta \to 0}{\to }\left[\begin{array}{c}1\\ 0\end{array}\right]\text{\hspace{1em}(44)}$$

1.3.5 一般化的四元数指数形式

由于四元数乘积的非交换特性，我们无法直接写出$e^{\mathbf{p}+\mathbf{q}}=e^{\mathbf{p}}{e}^{\mathbf{q}}$，但是如果其中一项是标量就可以写出：
$$e^{\mathbf{q}}=e^{q_e+{\mathbf{q}}_v}=e^{q_w}{e}^{{\mathbf{q}}_v}\text{\hspace{1em}(45)}$$
利用公式(42)$\mathbf{u}\theta ={\mathbf{q}}_v$：
$$e^{\mathbf{q}}=e^{q_w}\left[\begin{array}{c}\cos ||{\mathbf{q}}_v||\\ \frac{{\mathbf{q}}_v}{||{\mathbf{q}}_v||}\sin ||{\mathbf{q}}_v||\end{array}\right]\text{\hspace{1em}(46)}$$

1.3.6 单位四元数的Log运算

如果$||\mathbf{q}||=1$
$$\log \mathbf{q}=\log (\cos \theta +\mathbf{u}\sin \theta )=\log (e^{\mathbf{u}\theta })=\mathbf{u}\theta =\left[\begin{array}{c}0\\ \mathbf{u}\theta \end{array}\right]\text{\hspace{1em}(47)}$$
标准四元数的log运算结果是纯四元数，轴角数值可以通过如下运算获得：
$$\mathbf{u}={\mathbf{q}}_v/||{\mathbf{q}}_v||\text{\hspace{1em}(48)}$$
$$\theta =\arctan (||{\mathbf{q}}_v||,q_w)\text{\hspace{1em}(49)}$$
对于小角度四元数，为了避免出现除零的情况，同样利用泰勒展开，并截断，得到一定精度的近似值：
$$\log (\mathbf{q})=\mathbf{u}\theta ={\mathbf{q}}_v\frac{\arctan (||{\mathbf{q}}_v||,q_w)}{||{\mathbf{q}}_v||}\approx \frac{{\mathbf{q}}_v}{q_w}(1-\frac{||{\mathbf{q}}_v|{|}^2}{3q_w^2})\approx {\mathbf{q}}_v\underset{\theta \to 0}{\to }0\text{\hspace{1em}(50)}$$

1.3.7 一般四元数的log运算

如果$\mathbf{q}$ 是一个一般形式的四元数，则有：
$$\log \mathbf{q}=\log (||\mathbf{q}||\frac{\mathbf{q}}{||\mathbf{q}||})=\log ||\mathbf{q}||+\log \frac{\mathbf{q}}{||\mathbf{q}||}=\log ||\mathbf{q}||+\mathbf{u}\theta =\left[\begin{array}{c}\log ||\mathbf{q}||\\ \mathbf{u}\theta \end{array}\right]\text{\hspace{1em}(51)}$$

1.3.8 ${\mathbf{q}}^t$的指数形式

如果有$\mathbf{q}\in \mathbb{H}\text{ and }t\in \mathbb{R}$，则
$${\mathbf{q}}^t=\exp (\log ({\mathbf{q}}^t))=\exp (t\log (\mathbf{q}))\text{\hspace{1em}(52)}$$
如果$||\mathbf{q}||=1$，可以写作$\mathbf{q}=[\cos \theta ,\mathbf{u}\sin \theta ]$， 因此有$\log (\mathbf{q})=\mathbf{u}\theta$，进而有
$${\mathbf{q}}^t=\exp (t\mathbf{u}\theta )=\left[\begin{array}{c}\cos t\theta \\ \mathbf{u}\sin t\theta \end{array}\right]\text{\hspace{1em}(53)}$$
指数t最终变成了一个线性的乘子，我们在后面会继续讨论。

旋转和交叉关系？cross-relations

2.1 3D向量的旋转公式

图1展示了一个右手规则的旋转，旋转一个向量$\mathbf{x}$，旋转轴是一个单位向量$\mathbf{u}$，旋转角度为$\varphi$，这个旋转过程通过将向量$\mathbf{x}$分解为平行于转轴$\mathbf{u}$的${\mathbf{x}}_{\Vert }$部分，和垂直于旋转轴$\mathbf{u}$的${\mathbf{x}}_{\perp }$部分：
$$\mathbf{x}={\mathbf{x}}_{\Vert }+{\mathbf{x}}_{\perp }$$
分解后的部分计算如下，
$$\begin{gathered} {\mathbf{x}}_{\Vert }=\mathbf{u}(||\mathbf{x}\cos \alpha )=\mathbf{u}{\mathbf{u}}^{⊺}\mathbf{x} \\ {\mathbf{x}}_{\perp }=\mathbf{x}-{\mathbf{x}}_{\Vert }=\mathbf{x}-\mathbf{u}{\mathbf{u}}^{⊺}\mathbf{x} \end{gathered}$$
在旋转的过程中，和旋转轴平行的部分不会发生变化，因此：
$${\mathbf{x}}_{\Vert }^{\prime }={\mathbf{x}}_{\Vert }$$
和转轴垂直的部分，经历了一个平面旋转，我们构造一个平面内的正交基 { e 1 , e 2 } \{ \bold{e}_1, \bold{e}_2\} {e1​,e2​}
$$\begin{gathered} {\mathbf{e}}_1={\mathbf{x}}_{\perp } \\ {\mathbf{e}}_2=\mathbf{u}\times {\mathbf{x}}_{\perp }=\mathbf{u}\times \mathbf{x} \end{gathered}$$
满足$||{\mathbf{e}}_1||=||{\mathbf{e}}_2||$，有${\mathbf{x}}_{\perp }={\mathbf{e}}_1\cdot 1+{\mathbf{e}}_2\cdot 0$。在平面内旋转$\varphi$弧度后，可以得到
$${\mathbf{x}}_{\perp }^{\prime }={\mathbf{e}}_1\cos \varphi +{\mathbf{e}}_2\sin \varphi$$
进而可得：
$${\mathbf{x}}_{\perp }^{\prime }={\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi$$
再添加上平行旋转轴的部分，就得到了向量旋转公式
$${\mathbf{x}}^{\prime }={\mathbf{x}}_{\Vert }+{\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi \text{\hspace{1em}(54)}$$

2.2 旋转群$SO(3)$

在${\mathbb{R}}^3$中能通过组合运算，围绕原点旋转组成的群就是旋转群$SO(3)$，旋转过程能保持向量的长度以及向量之间相对的角度。在机器人学方向旋转群非常重要，它被用来描述刚体在三维空间的运动。一个“刚体运动过程”要求在刚体内部，“距离，角度，以及相对方向”不发生变化。如果刚体内部的长度，角度，和相对方向发生了变化，那这就不再是一个“刚体”。

通过一个满足特定性质的函数$r$来定义旋转，$r:{\mathbb{R}}^3\to {\mathbb{R}}^3;\mathbf{v}\mapsto r(\mathbf{v})$，作用于一个向量$v\in {\mathbb{R}}^3$ ， 通过存在点积和叉积的欧式空间测度来定义。
(1) 旋转运动保持向量长度不变
$$||r(\mathbf{v})||=\sqrt{⟨r(\mathbf{v}),r(\mathbf{v})⟩}=\sqrt{⟨\mathbf{v},\mathbf{v}⟩}\triangleq ||\mathbf{v}||,\forall \mathbf{v}\in {\mathbb{R}}^3\text{\hspace{1em}(55a)}$$
(2) 旋转保持向量之间的角度不变
$$⟨r(\mathbf{v}),r(\mathbf{w})⟩=⟨\mathbf{v},\mathbf{w}⟩=||\mathbf{v}||||\mathbf{w}||\cos \alpha ,\forall \mathbf{v},\mathbf{w}\in {\mathbb{R}}^3\text{\hspace{1em}(55b)}$$
(3) 旋转保持向量之间的相对方向
$$\mathbf{u}\times \mathbf{v}=\mathbf{w}\iff r(\mathbf{u})\times r(\mathbf{v})=r(\mathbf{w})$$
可以很容易证明前两个条件等价。可以定义旋转群$SO(3)$ 如下：
S O ( 3 ) : { r : R 3 → R 3 / ∀ v , w ∈ R 3 , ∣ ∣ r ( v ) ∣ ∣ = ∣ ∣ v ∣ ∣ , r ( v ) × r ( w ) = r ( v × w ) } (57) SO(3):\{ r:\Bbb{R}^3 \rarr \Bbb{R}^3 / \forall \bold{v}, \bold{w} \in \Bbb{R}^3 ,||r(\bold{v})|| = ||\bold{v}|| , r(\bold{v}) \times r(\bold{w}) = r(\bold{v} \times \bold{w}) \} \tag{57} SO(3):{r:R3→R3/∀v,w∈R3,∣∣r(v)∣∣=∣∣v∣∣,r(v)×r(w)=r(v×w)}(57)
旋转群通常用旋转矩阵来表示。但是四元数也是一种很好的表示方法。这一章的一个目的是说明两种表示方法是等价的。他们在概念和几何上展现出了大量的相似性。最主要的区别在与四元数是$SO(3)$的一个double cover，因此事实上并不是$SO(3)$本身。但对于我们的应用来说这一点并不致命。

2.3 旋转群和旋转矩阵

函数$r()$是用标量和向量乘积进行定义的，因此是线性的。所以可利用一个矩阵$\mathbf{R}\in {\mathbb{R}}^{3\times 3}$来表示。对向量$\mathbf{v}$的旋转可以用一个矩阵乘积表示，如下：
$$r(\mathbf{v})=\mathbf{R}\mathbf{v}\text{\hspace{1em}(58)}$$
代入到公式(55a)，利用点乘表达$⟨\mathbf{a},\mathbf{b}⟩={\mathbf{a}}^{⊺}\mathbf{b}$，可得对于所有的$\mathbf{v}$
$$(\mathbf{R}\mathbf{v}{)}^{⊺}(\mathbf{R}\mathbf{v})={\mathbf{v}}^{⊺}{\mathbf{R}}^{⊺}\mathbf{R}\mathbf{v}={\mathbf{v}}^{⊺}\mathbf{v}\text{\hspace{1em}(59)}$$
可以得到$\mathbf{R}$需要满足的一个正交条件
$${\mathbf{R}}^{⊺}\mathbf{R}=\mathbf{I}=\mathbf{R}{\mathbf{R}}^{⊺}\text{\hspace{1em}(60)}$$

上述条件是描述正交特性的，如果令$\mathbf{R}=[{\mathbf{r}}_1,{\mathbf{r}}_2,{\mathbf{r}}_3]$，代入上式，可得，$\mathbf{R}$的列向量${\mathbf{r}}_{\mathbf{i}}$是单位向量，并且互相垂直：
$$\begin{gathered} ⟨{\mathbf{r}}_{\mathbf{i}},{\mathbf{r}}_{\mathbf{i}}⟩={\mathbf{r}}_{\mathbf{i}}^{⊺}{\mathbf{r}}_{\mathbf{i}}=1 \\ ⟨{\mathbf{r}}_{\mathbf{i}},{\mathbf{r}}_{\mathbf{j}}⟩={\mathbf{r}}_{\mathbf{i}}^{⊺}{\mathbf{r}}_{\mathbf{j}}=0,\text{if }i\ne j \end{gathered}$$
能保持向量长度和角度的匀速组成的群是正交群$O(3)$。正交群包含了旋转和反射，旋转是刚性变换，反射不是刚性变换。群的含义是两个正交的矩阵的乘积依然是一个正交的矩阵。每一个正交矩阵都有一个逆，公式(60)表明，正交矩阵的逆就是它自身的转置
$${\mathbf{R}}^{-1}={\mathbf{R}}^{⊺}\text{\hspace{1em}(61)}$$
在加上相对方向条件，公式(56)确保变换是刚性的，可得到另一个约束
$$\det (\mathbf{R})=1$$
正交矩阵中，有单位行列式值的矩阵称为“特殊的”。他们是特殊正交矩阵，是正交群的子群，叫做特殊正交群$SO(3)$。任意两个旋转矩阵的乘积仍然是一个旋转矩阵。

2.3.1 指数映射

指数映射是一个非常有用的数学工具，能让我们轻松并且严密的处理3维旋转。它是在旋转空间中进行微积分的起始点。它让我们可以定义微分、扰动和速度并处理它们。因此在处理旋转和方向的估计问题时，要先了解一下

旋转运动是刚性的。这意味着我们可以在$SO(3)$中定义一个光滑的轨迹$r(t)$，这个轨迹可以将一个刚体平滑的从$r(0)$变换至$r(t)$。由于是连续光滑的，因此可以进行对时间的求导。利用公式(60)和(62)进行。

首先，要注意到只要能一直满足公式(60)，我们就可以避免出现对公式（62）的违反。因为运动是连续的，只要是连续运动就不会出现旋转矩阵行列式值的跳变(从1到-1)。
将公式(60)对时间求导：
$$\frac{d}{dt}({\mathbf{R}}^{⊺}\mathbf{R})={\dot{\mathbf{R}}}^{⊺}\mathbf{R}+{\mathbf{R}}^{⊺}\dot{\mathbf{R}}=0\text{\hspace{1em}(63)}$$
移项：
$${\dot{\mathbf{R}}}^{⊺}\mathbf{R}=-{\mathbf{R}}^{⊺}\dot{\mathbf{R}}=-({\dot{\mathbf{R}}}^{⊺}\mathbf{R}{)}^{⊺}\text{\hspace{1em}(64)}$$
说明矩阵${\dot{\mathbf{R}}}^{⊺}\mathbf{R}$是反对称的，反对称的$3\times 3$矩阵记为$\mathfrak{s}\mathfrak{o}(3)$，这个是$SO(3)$的李代数。反对称的$3\times 3$矩阵有如下形式：
$$[\omega {]}_{\times }\triangleq \left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]\text{\hspace{1em}(65)}$$
反对称矩阵有3个自由度，和叉乘运算相关，如公式(20)，这个建立了从向量到李代数的一一映射$\omega \in {\mathbb{R}}^3\leftrightarrow [\omega {]}_{\times }\in \mathfrak{s}\mathfrak{o}(3)$。取一个向量$\omega =({\omega }_x,{\omega }_y,{\omega }_z)\in {\mathbf{R}}^3$，将上式写作：
$${\mathbf{R}}^{⊺}\dot{\mathbf{R}}=[\omega {]}_{\times }\text{\hspace{1em}(66)}$$
这是一个普通微分方程(ODE):
$$\dot{\mathbf{R}}=\mathbf{R}[\omega {]}_{\times }\text{\hspace{1em}(67)}$$
在原点附近有$\mathbf{R}=\mathbf{I}$，上式可以化简为$\dot{\mathbf{R}}=[\omega {]}_{\times }$，可见，可以将李代数$\mathfrak{s}\mathfrak{o}(3)$看做$r(t)$在原点附近的微分空间，它组成了$SO(3)$的正切空间或者叫速度空间，由此可以将$\omega$看做是瞬时角速度向量

如果$\omega$是常数，上述的微分方程求解可得：
$$\mathbf{R}(t)=\mathbf{R}(0)e^{[\omega {]}_{\times }t}=\mathbf{R}(0)e^{[\omega t{]}_{\times }}\text{\hspace{1em}(68)}$$
其中指数项$e^{[x{]}_{\times }}$是通过泰勒级数进行定义的，后面会讨论。由于$\mathbf{R}(0),\mathbf{R}(t)$是旋转矩阵，因此$e^{[\omega t{]}_{\times }}$是旋转矩阵。定义向量$\varphi \triangleq \omega \Delta t$ 为在时间段$\Delta t$内，整体的旋转，可得：
$$\mathbf{R}=e^{[\varphi {]}_{\times }}\text{\hspace{1em}(69)}$$
这个就是指数映射，从$\mathfrak{s}\mathfrak{o}(3)$到$SO(3)$：
$$\exp :\mathfrak{s}\mathfrak{o}(3)\to SO(3);[\varphi {]}_{\times }\mapsto \exp ([\varphi {]}_{\times })=e^{[\varphi {]}_{\times }}\text{\hspace{1em}(70)}$$

2.3.2 大写指数映射

上面的指数映射表达形式经常被滥用，和$\varphi \in {\mathbb{R}}^3$以及$[\varphi {]}_{\times }\in \mathfrak{s}\mathfrak{o}(3)$混淆，为了避免歧义，使用一个大写的Exp运算，来作为映射${\mathbb{R}}^3\to SO(3)$:
$$\text{Exp}:{\mathbb{R}}^3\to SO(3);\varphi \mapsto \text{Exp}(\varphi )=e^{[\varphi {]}_{\times }}\text{\hspace{1em}(71)}$$
大小写指数映射之间的关系：
$$\text{Exp}(\varphi )\triangleq \exp ([\varphi {]}_{\times })\text{\hspace{1em}(72)}$$
在后面的章节中，会看到，将向量$\varphi$称作旋转向量，或者轴角向量，$\varphi =\omega \Delta t=\varphi \mathbf{u}$，其中$\varphi$是角度，$\mathbf{u}$是旋转轴。