参考：《Quaternion kinematics for error-state kalman filter》、《Indirect Kalman Filter for 3D Attitude Estimation》、《SLAM十四讲》

旋转向量（ the rotation vector or the angle-axis vector）和欧拉角可以使用3个量描述旋转，虽没有DoF的冗余，但是存在奇异性，就是我们常说的万向节死锁（Gimbal Lock），简单说就是，当两个旋转轴重合时，绕这两个轴的旋转效果是一样的，所以表征机体旋转的欧拉角就不再是唯一的了. 所以出于这种考虑，就出现了旋转矩阵和四元数，四元数其实就是扩展的复数，关于复数表示旋转的理解，阮老师的博客 虚数的意义 讲得很通俗易懂，所以四元数使用最少的量同时保证旋转的紧凑性和非奇异性.

因为3D空间的旋转具有3个自由度，如果使用旋转矩阵的9个量描述3个DoF，那么就需要满足正交矩阵并且行列式为1，同样，使用四元数表示旋转应当满足单位四元数.

一、四元数基本运算

关于四元数的基本概念、加减法、乘法点乘数乘的区别、共轭与求逆、求模运算就不在这提了，《SLAM十四讲》已经写得很清楚了.

1.1 四元数的乘法

(1) p ⊗ q = [ 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 ] = [ p w q w − p v ⊤ q v p w q v + q w p v + p v × q v ] \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] = \left[ \begin{array} { c } { p _ { w } q _ { w } – \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v } } \\ { p _ { w } \mathbf { q } _ { v } + q _ { w } \mathbf { p } _ { v } + \mathbf { p } _ { v } \times \mathbf { q } _ { v } } \end{array} \right] \tag{1} p⊗q=⎣⎢⎢⎡​pw​qw​−px​qx​−py​qy​−pz​qz​pw​qx​+px​qw​+py​qz​−pz​qy​pw​qy​−px​qz​+py​qw​+pz​qx​pw​qz​+px​qy​−py​qx​+pz​qw​​⎦⎥⎥⎤​=[pw​qw​−pv⊤​qv​pw​qv​+qw​pv​+pv​×qv​​](1)
可以看到乘积里面含有叉乘运算，叉乘不满足交换律，所以四元数的乘法不满足交换律.

进一步将四元数乘法转换成普通的矩阵与向量之间的乘法，可得：
q 1 ⊗ q 2 = [ q 1 ] L q 2 , q 1 ⊗ q 2 = [ q 2 ] R q 1 \mathbf { q } _ { 1 } \otimes \mathbf { q } _ { 2 } = \left[ \mathbf { q } _ { 1 } \right] _ { L } \mathbf { q } _ { 2 } \quad , \quad \mathbf { q } _ { 1 } \otimes \mathbf { q } _ { 2 } = \left[ \mathbf { q } _ { 2 } \right] _ { R } \mathbf { q } _ { 1 } q1​⊗q2​=[q1​]L​q2​,q1​⊗q2​=[q2​]R​q1​

[ q ] L = [ 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 ] , [ q ] R = [ 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 ] [ \mathbf { q } ] _ { L } = \left[ \begin{array} { c c c c } { 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] , \quad [ \mathbf { q } ] _ { R } = \left[ \begin{array} { c c c c } { 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] [q]L​=⎣⎢⎢⎡​qw​qx​qy​qz​​−qx​qw​qz​−qy​​−qy​−qz​qw​qx​​−qz​qy​−qx​qw​​⎦⎥⎥⎤​,[q]R​=⎣⎢⎢⎡​qw​qx​qy​qz​​−qx​qw​−qz​qy​​−qy​qz​qw​−qx​​−qz​−qy​qx​qw​​⎦⎥⎥⎤​

[ q ] L = q w I + [ 0 − q v ⊤ q v [ q v ] × ] , [ q ] R = q w I + [ 0 − q v ⊤ q v − [ q v ] × ] [ \mathbf { q } ] _ { L } = q _ { w } \mathbf { I } + \left[ \begin{array} { c c } { 0 } & { – \mathbf { q } _ { v } ^ { \top } } \\ { \mathbf { q } _ { v } } & { \left[ \mathbf { q } _ { v } \right] _ { \times } } \end{array} \right] , \quad [ \mathbf { q } ] _ { R } = q _ { w } \mathbf { I } + \left[ \begin{array} { c c } { 0 } & { – \mathbf { q } _ { v } ^ { \top } } \\ { \mathbf { q } _ { v } } & { – \left[ \mathbf { q } _ { v } \right] _ { \times } } \end{array} \right] [q]L​=qw​I+[0qv​​−qv⊤​[qv​]×​​],[q]R​=qw​I+[0qv​​−qv⊤​−[qv​]×​​]
由下式
q ⊗ x ⊗ p = ( q ⊗ x ) ⊗ p = [ p ] R [ q ] L x = q ⊗ ( x ⊗ p ) = [ q ] L [ p ] R x \begin{aligned} \mathbf { q } \otimes \mathbf { x } \otimes \mathbf { p } & = ( \mathbf { q } \otimes \mathbf { x } ) \otimes \mathbf { p } = [ \mathbf { p } ] _ { R } [ \mathbf { q } ] _ { L } \mathbf { x } \\ & = \mathbf { q } \otimes ( \mathbf { x } \otimes \mathbf { p } ) = [ \mathbf { q } ] _ { L } [ \mathbf { p } ] _ { R } \mathbf { x } \end{aligned} q⊗x⊗p​=(q⊗x)⊗p=[p]R​[q]L​x=q⊗(x⊗p)=[q]L​[p]R​x​
可得
[ p ] R [ q ] L = [ q ] L [ p ] R [ \mathbf { p } ] _ { R } [ \mathbf { q } ] _ { L } = [ \mathbf { q } ] _ { L } [ \mathbf { p } ] _ { R } [p]R​[q]L​=[q]L​[p]R​

1.2 纯虚四元数的乘法

两个纯虚四元数相乘的结果不再是纯虚四元数了
p v ⊗ q v = − p v ⊤ q v + p v × q v = [ − p v ⊤ q v p v × q v ] \mathbf { p } _ { v } \otimes \mathbf { q } _ { v } = – \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v } + \mathbf { p } _ { v } \times \mathbf { q } _ { v } = \left[ \begin{array} { c } { – \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v } } \\ { \mathbf { p } _ { v } \times \mathbf { q } _ { v } } \end{array} \right] pv​⊗qv​=−pv⊤​qv​+pv​×qv​=[−pv⊤​qv​pv​×qv​​]

1.3 纯虚四元数的指数

纯虚四元数为 v = v x i + v y j + v z k = u θ = [ 0 u θ ] \mathbf v = v_xi + v_yj + v_zk= \mathbf { u } \theta = \left[ \begin{array} { l } { 0 } \\ { \mathbf { u } \theta } \end{array} \right] v=vx​i+vy​j+vz​k=uθ=[0uθ​]，其中 θ = ∥ v ∥ ∈ R \theta = \| \mathbf { v } \| \in \mathbb { R } θ=∥v∥∈R， u \mathbf { u } u 为单位向量（后面会证明，这里的旋转其实是绕单位向量 u \mathbf { u } u 对目标物体进行角度 2 θ 2 \theta 2θ 的旋转，注意 θ \theta θ 前面的这个 2 2 2 系数）.

利用泰勒展开，纯虚四元数 v \boldsymbol v v 的指数 e v e^{\boldsymbol v} ev 构成一个新的四元数：

e v = e u θ = cos ⁡ θ + u sin ⁡ θ = [ cos ⁡ θ u sin ⁡ θ ] e ^ { \mathrm { v } } = e ^ { \mathbf { u } \theta } = \cos \theta + \mathbf { u } \sin \theta = \left[ \begin{array} { c } { \cos \theta } \\ { \mathbf { u } \sin \theta } \end{array} \right] ev=euθ=cosθ+usinθ=[cosθusinθ​]

上面形式不就是我们当初学复数时遇到的欧拉公式吗， e i θ = cos ⁡ θ + i sin ⁡ θ e ^ { i \theta } = \cos \theta + i \sin \theta eiθ=cosθ+isinθ
注意到， ∥ e v ∥ 2 = cos ⁡ 2 θ + sin ⁡ 2 θ = 1 \left\| e ^ { \mathrm { v } } \right\| ^ { 2 } = \cos ^ { 2 } \theta + \sin ^ { 2 } \theta = 1 ∥ev∥2=cos2θ+sin2θ=1，即纯虚四元数的指数是一个单位四元数. 这也说明了使用四元数表示旋转为什么需要是单位四元数了吧.
关于这个纯虚四元数的指数映射含义为：从纯虚四元数空间映射到以单位四元数表示的旋转空间（an application from the space of pure quaternions to the space of rotations represented by unit quaternions）.

1.4 单位四元数的对数

上面公式的逆运算也成立，即，如果四元数 q \mathbf { q } q 是单位四元数，那么它的对数将是一个纯虚四元数
log ⁡ q = log ⁡ ( cos ⁡ θ + u sin ⁡ θ ) = log ⁡ ( e u θ ) = u θ = [ 0 u θ ] \log \mathbf { q } = \log ( \cos \theta + \mathbf { u } \sin \theta ) = \log \left( e ^ { \mathbf { u } \theta } \right) = \mathbf { u } \theta = \left[ \begin{array} { c } { 0 } \\ { \mathbf { u } \theta } \end{array} \right] logq=log(cosθ+usinθ)=log(euθ)=uθ=[0uθ​]

二、旋转矩阵和四元数导数

2.1 旋转矩阵的导数

因为 R ⊤ R = I \mathbf { R } ^ { \top } \mathbf { R } = \mathbf I R⊤R=I，所以其导数必然是0，即，
d d t ( R ⊤ R ) = R ˙ ⊤ R + R ⊤ R ˙ = 0 \frac { d } { d t } \left( \mathbf { R } ^ { \top } \mathbf { R } \right) = \dot { \mathbf { R } } ^ { \top } \mathbf { R } + \mathbf { R } ^ { \top } \dot { \mathbf { R } } = 0 dtd​(R⊤R)=R˙⊤R+R⊤R˙=0

R ⊤ R ˙ = − ( R ⊤ R ˙ ) ⊤ \mathbf { R } ^ { \top } \dot { \mathbf { R } } = – \left( \mathbf { R } ^ { \top } \dot { \mathbf { R } } \right) ^ { \top } R⊤R˙=−(R⊤R˙)⊤
由此可知 R ⊤ R \mathbf { R } ^ { \top } \mathbf { R } R⊤R 是反对称矩阵（skew-symmetric）， 3 × 3 3 \times 3 3×3 维的反对称矩阵即是李代数 s o ( 3 ) \mathfrak { s o } ( 3 ) so(3) （只有3DoF），使用 ω ∈ R 3 ↔ [ ω ] × ∈ s o ( 3 ) \omega \in \mathbb { R } ^ { 3 } \leftrightarrow [ \omega ] _ {\times } \in \mathfrak { s o } ( 3 ) ω∈R3↔[ω]×​∈so(3) 表示，当然因为 ω \omega ω 与 [ ω ] × [ \omega ] _ { \times } [ω]×​ 只差了反对称符号，所以而这都可以表示为李代数，不做区分.
[ ω ] × ≜ [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] [ \omega ] _ { \times } \triangleq \left[ \begin{array} { c c c } { 0 } & { – \omega _ { z } } & { \omega _ { y } } \\ { \omega _ { z } } & { 0 } & { – \omega _ { x } } \\ { – \omega _ { y } } & { \omega _ { x } } & { 0 } \end{array} \right] [ω]×​≜⎣⎡​0ωz​−ωy​​−ωz​0ωx​​ωy​−ωx​0​⎦⎤​
而后就可以得到，旋转矩阵的导数形式：
R ˙ = R [ ω ] × \dot { \mathbf { R } } = \mathbf { R } [ \omega ] _ { \times } R˙=R[ω]×​
这里的 ω = ( ω x , ω y , ω z ) \omega =\left( \omega _ { x } , \omega _ { y } , \omega _ { z } \right) ω=(ωx​,ωy​,ωz​) 亦称为角速度，对应IMU测量得到的三个轴上的角速度（IMU座标系）. 既然旋转矩阵能写成上面这种形式，那么旋转矩阵一定是指数函数，所以假定 ω \omega ω 是常数，那么根据角度=速度*时间，得到下面的时域积分形式：
R ( t ) = R ( 0 ) e [ ω ] × t = R ( 0 ) e [ ω t ] × \mathbf { R } ( t ) = \mathbf { R } ( 0 ) e ^ { [ \omega ] _ { \times } t } = \mathbf { R } ( 0 ) e ^ { [ \omega t ] _ { \times } } R(t)=R(0)e[ω]×​t=R(0)e[ωt]×​
稍作变换，即得到旋转矩阵如下
R = R ( 0 ) ⊤ R ( t ) = e [ ω t ] × = e [ ϕ ] × \mathbf { R } = \mathbf { R } ( 0 ) ^ { \top } \mathbf { R } ( t ) =e ^ { [ \omega t ] _ { \times } } = e ^ { [ \phi ] _ { \times } } R=R(0)⊤R(t)=e[ωt]×​=e[ϕ]×​

这就是我们熟悉的李代数到李羣的转换：
exp ⁡ : s o ( 3 ) → S O ( 3 ) ; [ ϕ ] × ↦ exp ⁡ ( [ ϕ ] × ) = e [ ϕ ] × \exp : \mathfrak { s o } ( 3 ) \rightarrow S O ( 3 ) ; [ \phi ] _ { \times } \mapsto \exp \left( [ \phi ] _ { \times } \right) = e ^ { [ \phi ] _ {\times} } exp:so(3)→SO(3);[ϕ]×​↦exp([ϕ]×​)=e[ϕ]×​

2.2 四元数的导数

因为 q ∗ ⊗ q = 1 \mathbf { q } ^ { * } \otimes \mathbf { q }=1 q∗⊗q=1，所以其导数也等于0，旋转矩阵求导也是这么做的. 如下：
d ( q ∗ ⊗ q ) d t = q ˙ ∗ ⊗ q + q ∗ ⊗ q ˙ = 0 \frac { d \left( \mathbf { q } ^ { * } \otimes \mathbf { q } \right) } { d t } = \dot { \mathbf { q } } ^ { * } \otimes \mathbf { q } + \mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } = 0 dtd(q∗⊗q)​=q˙​∗⊗q+q∗⊗q˙​=0

q ∗ ⊗ q ˙ = − ( q ˙ ∗ ⊗ q ) = − ( q ∗ ⊗ q ˙ ) ∗ \mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } = – \left( \dot { \mathbf { q } } ^ { * } \otimes \mathbf { q } \right) = – \left( \mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } \right) ^ { * } q∗⊗q˙​=−(q˙​∗⊗q)=−(q∗⊗q˙​)∗

由上式可以看出， q ∗ ⊗ q ˙ \mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } q∗⊗q˙​ 是纯虚四元数，因为只有纯虚四元数的共轭的相反数等于其本身，所以，进一步将其写成：
q ∗ ⊗ q ˙ = Ω = [ 0 Ω ] ∈ H p \mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } = \mathbf { \Omega } = \left[ \begin{array} { l } { 0 } \\ { \mathbf { \Omega } } \end{array} \right] \in \mathbb { H } _ { p } q∗⊗q˙​=Ω=[0Ω​]∈Hp​
移项即得四元数的导数，但是这里的 Ω \Omega Ω 不再是角速度了，而是速度 ω \omega ω 的一半，即 Ω = ω / 2 \Omega = \omega / 2 Ω=ω/2.
q ˙ = q ⊗ Ω = 1 2 q ⊗ ω \dot { \mathbf { q } } = \mathbf { q } \otimes \Omega= \frac { 1 } { 2 } \mathbf { q } \otimes \mathbf { \omega } q˙​=q⊗Ω=21​q⊗ω
还有另外一种求四元数导数的方法，有兴趣参考《Indirect Kalman Filter for 3D Attitude Estimation》中的公式87.
假定 Ω \Omega Ω 是常量，那么就可以得到下式：
q ( t ) = q ( 0 ) ⊗ e Ω t \mathbf { q } ( t ) = \mathbf { q } ( 0 ) \otimes e ^ { \mathbf { \Omega } t } q(t)=q(0)⊗eΩt
移项即可得到
q = e V = e Ω t = e ω t / 2 \mathbf { q } = e ^ { \mathbf { V } }=e ^ { \mathbf { \Omega } t }=e ^ { \mathbf { \omega } t / 2} q=eV=eΩt=eωt/2
旋转向量的指数映射到四元数空间的操作如下：
Exp ⁡ : R 3 → S 3 ; ϕ ↦ Exp ⁡ ( ϕ ) = e ϕ / 2 \operatorname { Exp } : \mathbf { R } ^ { 3 } \rightarrow S ^ { 3 } ; \phi \mapsto \operatorname { Exp } ( \boldsymbol { \phi } ) = e ^ { \phi / 2 } Exp:R3→S3;ϕ↦Exp(ϕ)=eϕ/2

三、证明 θ = ϕ / 2 \theta = \phi /2 θ=ϕ/2

四元数 q \mathbf q q 在4D空间与实单位四元数 q 1 = [ 1 , 0 , 0 , 0 ] \mathbf { q } _ { 1 } = [ 1,0,0,0 ] q1​=[1,0,0,0] 之间的旋转角度 θ \theta θ 等于其在3D空间对目标物体所做旋转角度 ϕ \phi ϕ 的一半，即 θ = ϕ / 2 \theta = \phi /2 θ=ϕ/2.
下面的证明分为两部分进行：

1) 3D vector rotation formula

如上图，当绕轴 u \mathbf { u } u 对向量 x \mathbf { x } x 旋转 ϕ \mathbf { \phi } ϕ 角度后（ ξ = u ϕ \boldsymbol {\xi} = \mathbf { u }\mathbf {\phi} ξ=uϕ 即为旋转向量，也是李代数 s o ( 3 ) \mathfrak { s o } ( 3 ) so(3)），得到旋转后的向量 x ′ \mathbf {x}^{ \prime } x′，如下：
(3-1) x ′ = x ∥ + x ⊥ cos ⁡ ϕ + ( u × x ) sin ⁡ ϕ \mathbf { x } ^ { \prime } = \mathbf { x } _ { \| } + \mathbf { x } _ { \perp } \cos \phi + ( \mathbf { u } \times \mathbf { x } ) \sin \phi \tag{3-1} x′=x∥​+x⊥​cosϕ+(u×x)sinϕ(3–1)
这个公式就叫做三维向量旋转公式（3D vector rotation formula）

2) The rotation action

四元数 q \mathbf { q } q 对向量 x \mathbf { x } x 进行旋转得到 x ′ = q ⊗ x ⊗ q ∗ \mathbf { x } ^ { \prime } = \mathbf { q } \otimes \mathbf { x } \otimes \mathbf { q } ^ { * } x′=q⊗x⊗q∗，这种乘法叫做（the sandwich product），因为将向量 x \mathbf { x } x 写进了四元数的乘法当中，所以也需要将 x \mathbf { x } x 转换到四元数空间（向量空间转换到四元数空间），即，
(3-2) x = x i + y j + z k = [ 0 x ] ∈ H p \mathbf { x } = x i + y j + z k = \left[ \begin{array} { l } { 0 } \\ { \mathbf { x } } \end{array} \right] \in \mathbb { H } _ { p } \tag{3-2} x=xi+yj+zk=[0x​]∈Hp​(3–2)

假设所作的旋转轴为 u \mathbf u u，旋转角度为 ϕ \mathbf \phi ϕ，则四元数 q \mathbf { q } q 可以先写成 q = Exp ⁡ ( u ϕ ) = exp ⁡ ( u ϕ / 2 ) = cos ⁡ ϕ 2 + u sin ⁡ ϕ 2 \mathbf { q } = \operatorname{Exp}(\mathbf { u } \mathbf { \phi } ) = \operatorname{exp}(\mathbf { u } \mathbf { \phi } /2) = \cos \frac { \phi } { 2 } + \mathbf { u } \sin \frac { \phi } { 2 } q=Exp(uϕ)=exp(uϕ/2)=cos2ϕ​+usin2ϕ​，对旋转后的结果进行展开，如下：
(3-3) x ′ = q ⊗ x ⊗ q ∗ = ( cos ⁡ ϕ 2 + u sin ⁡ ϕ 2 ) ⊗ ( 0 + x ) ⊗ ( cos ⁡ ϕ 2 − u sin ⁡ ϕ 2 ) . . . . . . = x ⊥ cos ⁡ ϕ + ( u × x ) sin ⁡ ϕ + x ∥ \begin{aligned} \mathbf { x } ^ { \prime } & = \mathbf { q } \otimes \mathbf { x } \otimes \mathbf { q } ^ { * } \\ & = \left( \cos \frac { \phi } { 2 } + \mathbf { u } \sin \frac { \phi } { 2 } \right) \otimes ( 0 + \mathbf { x } ) \otimes \left( \cos \frac { \phi } { 2 } – \mathbf { u } \sin \frac { \phi } { 2 } \right) \\ & …… \\ & = \mathbf { x } _ { \perp } \cos \phi + ( \mathbf { u } \times \mathbf { x } ) \sin \phi + \mathbf { x } _ { \| } \end{aligned} \tag{3-3} x′​=q⊗x⊗q∗=(cos2ϕ​+usin2ϕ​)⊗(0+x)⊗(cos2ϕ​−usin2ϕ​)......=x⊥​cosϕ+(u×x)sinϕ+x∥​​(3–3)

对比公式(3-1)，二者一模一样，这就证明了四元数对目标物体旋转了 ϕ \phi ϕ 角度，这个四元数在其4D空间相对实单位四元数旋转了 ϕ / 2 \phi /2 ϕ/2 角度. 这时候祭出这张图再合适不过了.

<完>
@leatherwang