其实老早就想对四元数相关知识进行一个总结了,这次学习MSCKF时,阅读了参考文献《Quaternion Kinematics for the Error-state Kalman Filter》,这篇文献对四元数相关知识进行了一个非常详细的说明,这篇博客是对这篇参考文献中四元数相关知识的笔记,如果想了解得比较深入,建议还是直接看文献。
这一部分是描述了四元数的一些基本性质,是后面推到旋转微分积分的基础。
加法就是实部虚部分别相加:
p ± q = [ p w p v ] ± [ q w q v ] = [ p w ± q w p v ± q v ] \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] p±q=[pwpv]±[qwqv]=[pw±qwpv±qv]满足交换律和结合律: p + q = q + p p + ( q + r ) = ( p + q ) + r \begin{aligned} \mathbf{p}+\mathbf{q} &=\mathbf{q}+\mathbf{p} \\ \mathbf{p}+(\mathbf{q}+\mathbf{r}) &=(\mathbf{p}+\mathbf{q})+\mathbf{r} \end{aligned} p+qp+(q+r)=q+p=(p+q)+r
乘法一共有三种计算法方式:
第一种:
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 ] \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] p⊗q=⎣⎢⎢⎡pwqw−pxqx−pyqy−pzqzpwqx+pxqw+pyqz−pzqypwqy−pxqz+pyqw+pzqxpwqz+pxqy−pyqx+pzqw⎦⎥⎥⎤
第二种: p ⊗ q = [ 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}-\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] p⊗q=[pwqw−pv⊤qvpwqv+qwpv+pv×qv]
第三种: q 1 ⊗ q 2 = [ q 1 ] L q 2 and 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 \text { and } \quad \mathbf{q}_{1} \otimes \mathbf{q}_{2}=\left[\mathbf{q}_{2}\right]_{R} \mathbf{q}_{1} q1⊗q2=[q1]Lq2 and q1⊗q2=[q2]Rq1其中: [ 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}{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], \quad[\mathbf{q}]_{R}=\left[\begin{array}{ccccc} 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=⎣⎢⎢⎡qwqxqyqz−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤,[q]R=⎣⎢⎢⎡qwqxqyqz−qxqw−qzqy−qyqzqw−qx−qz−qyqxqw⎦⎥⎥⎤或者表示为: [ 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}{cc} 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}{cc} 0 & -\mathbf{q}_{v}^{\top} \\ \mathbf{q}_{v} & -\left[\mathbf{q}_{v}\right]_{\times} \end{array}\right] [q]L=qwI+[0qv−qv⊤[qv]×],[q]R=qwI+[0qv−qv⊤−[qv]×]满足分配率和结合率 ( p ⊗ q ) ⊗ r = p ⊗ ( q ⊗ r ) (\mathbf{p} \otimes \mathbf{q}) \otimes \mathbf{r}=\mathbf{p} \otimes(\mathbf{q} \otimes \mathbf{r}) (p⊗q)⊗r=p⊗(q⊗r) p ⊗ ( q + r ) = p ⊗ q + p ⊗ r \mathbf{p} \otimes(\mathbf{q}+\mathbf{r})=\mathbf{p} \otimes \mathbf{q}+\mathbf{p} \otimes \mathbf{r} \quad p⊗(q+r)=p⊗q+p⊗r
四元数的共轭定义为 q ∗ ≜ q w − q v = [ q w − q v ] \mathbf{q}^{*} \triangleq q_{w}-\mathbf{q}_{v}=\left[\begin{array}{c} q_{w} \\ -\mathbf{q}_{v} \end{array}\right] q∗≜qw−qv=[qw−qv]共轭的性质为 q ⊗ q ∗ = q ∗ ⊗ q = q w 2 + q x 2 + q y 2 + q z 2 = [ q w 2 + q x 2 + q y 2 + q z 2 0 v ] \mathbf{q} \otimes \mathbf{q}^{*}=\mathbf{q}^{*} \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} \\ \mathbf{0}_{v} \end{array}\right] q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2=[qw2+qx2+qy2+qz20v] ( p ⊗ q ) ∗ = q ∗ ⊗ p ∗ (\mathbf{p} \otimes \mathbf{q})^{*}=\mathbf{q}^{*} \otimes \mathbf{p}^{*} (p⊗q)∗=q∗⊗p∗
四元数的模的定义为: ∥ q ∥ ≜ q ⊗ q ∗ = q ∗ ⊗ q = q w 2 + q x 2 + q y 2 + q z 2 ∈ R \|\mathbf{q}\| \triangleq \sqrt{\mathbf{q} \otimes \mathbf{q}^{*}}=\sqrt{\mathbf{q}^{*} \otimes \mathbf{q}}=\sqrt{q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}} \in \mathbb{R} ∥q∥≜q⊗q∗=q∗⊗q=qw2+qx2+qy2+qz2∈R模的性质为: ∥ p ⊗ q ∥ = ∥ q ⊗ p ∥ = ∥ p ∥ ∥ q ∥ \|\mathbf{p} \otimes \mathbf{q}\|=\|\mathbf{q} \otimes \mathbf{p}\|=\|\mathbf{p}\|\|\mathbf{q}\| ∥p⊗q∥=∥q⊗p∥=∥p∥∥q∥
四元数的逆的定义为: q ⊗ q − 1 = q − 1 ⊗ q = q 1 \mathbf{q} \otimes \mathbf{q}^{-1}=\mathbf{q}^{-1} \otimes \mathbf{q}=\mathbf{q}_{1} q⊗q−1=q−1⊗q=q1同样,可以被计算为 q − 1 = q ∗ / ∥ q ∥ 2 \mathbf{q}^{-1}=\mathbf{q}^{*} /\|\mathbf{q}\|^{2} q−1=q∗/∥q∥2
单位四元数通常指 ∥ q ∥ = 1 \|\mathbf{q}\|=1 ∥q∥=1,可以被写为: q = [ cos θ u sin θ ] \mathbf{q}=\left[\begin{array}{c} \cos \theta \\ \mathbf{u} \sin \theta \end{array}\right] q=[cosθusinθ]其中 u = u x i + u y j + u z k \mathbf{u}=u_{x} i+u_{y} j+u_{z} k u=uxi+uyj+uzk是一个单位向量,其满足性质: q − 1 = q ∗ \mathbf{q}^{-1}=\mathbf{q}^{*} q−1=q∗
这里的四元数的指数通常指纯四元数(实部为0的四元数),即 q = [ 0 , q v ] \mathbf{q}=\left[0, \mathbf{q}_{v}\right] q=[0,qv],根据乘法的定义可以有 q v ⊗ q v = − q v ⊤ q v = − ∥ q v ∥ 2 \mathbf{q}_{v} \otimes \mathbf{q}_{v}=-\mathbf{q}_{v}^{\top} \mathbf{q}_{v}=-\left\|\mathbf{q}_{v}\right\|^{2} qv⊗qv=−qv⊤qv=−∥qv∥2那么对于纯单位四元数 u \mathbf{u} u有 ∥ u ∥ = 1 \|\mathbf{u}\|=1 ∥u∥=1 u ⊗ u = − 1 \mathbf{u} \otimes \mathbf{u}=-1 u⊗u=−1那么对于一个纯四元数我们可以定义为 v = u θ \mathbf{v}=\mathbf{u} \theta v=uθ,那么我们可以得到纯四元数的幂运算为: v 2 = − θ 2 , v 3 = − u θ 3 , v 4 = θ 4 , v 5 = u θ 5 , v 6 = − θ 6 , ⋯ \mathbf{v}^{2}=-\theta^{2} \quad, \quad \mathbf{v}^{3}=-\mathbf{u} \theta^{3} \quad, \quad \mathbf{v}^{4}=\theta^{4} \quad, \quad \mathbf{v}^{5}=\mathbf{u} \theta^{5} \quad, \quad \mathbf{v}^{6}=-\theta^{6} \quad, \quad \cdots v2=−θ2,v3=−uθ3,v4=θ4,v5=uθ5,v6=−θ6,⋯那么四元数的指数可以通过泰勒展开获得如下形式 e v = ∑ k = 0 ∞ 1 k ! v k e^{\mathbf{v}}=\sum_{k=0}^{\infty} \frac{1}{k !} \mathbf{v}^{k} ev=k=0∑∞k!1vk将 v = u θ \mathbf{v}=\mathbf{u} \theta v=uθ代入得 e u θ = ( 1 − θ 2 2 ! + θ 4 4 ! + ⋯ ) + ( u θ − u θ 3 3 ! + u θ 5 5 ! + ⋯ ) e^{\mathbf{u} \theta}=\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) euθ=(1−2!θ2+4!θ4+⋯)+(uθ−3!uθ3+5!uθ5+⋯)进而我们可以获得经典的欧拉公式 e v = e u θ = cos θ + u sin θ = [ cos θ u sin θ ] 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] ev=euθ=cosθ+usinθ=[cosθusinθ]对于一般的四元数,我们可以进行指数形式的拓展有 e q = e q w + q v = e q w e q v e^{\mathbf{q}}=e^{q_{w}+\mathbf{q}_{v}}=e^{q_{w}} e^{\mathbf{q}_ v} eq=eqw+qv=eqweqv令 u θ = q v \mathbf{u} \theta=\mathbf{q}_{v} uθ=qv有 e q = e q w [ cos ∥ q v ∥ q v ∥ q v ∥ sin ∥ q v ∥ ] e^{\mathbf{q}}=e^{q_{w}}\left[\begin{array}{c} \cos \left\|\mathbf{q}_{v}\right\| \\ \frac{\mathbf{q}_{v}}{\left\|\mathbf{q}_{v}\right\|} \sin \left\|\mathbf{q}_{v}\right\| \end{array}\right] eq=eqw[cos∥qv∥∥qv∥qvsin∥qv∥]
有了前面的推到,这里可以给出单位四元数 q = [ cos θ , u sin θ ] \mathbf{q}=[\cos \theta, \mathbf{u} \sin \theta] q=[cosθ,usinθ]的对数形式 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θ]这里可以看到一个单位四元数的对数是一个纯四元数,对于一般的四元数,我们可以拓展为 log q = log ( ∥ q ∥ q ∥ q ∥ ) = log ∥ q ∥ + log q ∥ q ∥ = log ∥ q ∥ + u θ = [ log ∥ q ∥ u θ ] \log \mathbf{q}=\log \left(\|\mathbf{q}\| \frac{\mathbf{q}}{\|\mathbf{q}\|}\right)=\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] logq=log(∥q∥∥q∥q)=log∥q∥+log∥q∥q=log∥q∥+uθ=[log∥q∥uθ]前面我们推到了纯四元数的幂运算,这里通过结合四元数的指数和对数的性质可以进一步推导单位四元数的幂运算,有 q t = exp ( log ( q t ) ) = exp ( t log ( q ) ) \mathbf{q}^{t}=\exp \left(\log \left(\mathbf{q}^{t}\right)\right)=\exp (t \log (\mathbf{q})) qt=exp(log(qt))=exp(tlog(q))因为是单位四元数,所以有 q = [ cos θ , u sin θ ] \mathbf{q}=[\cos \theta, \mathbf{u} \sin \theta] q=[cosθ,usinθ],那么有 q t = exp ( t u θ ) = [ cos t θ u sin t θ ] \mathbf{q}^{t}=\exp (t \mathbf{u} \theta)=\left[\begin{array}{c} \cos t \theta \\ \mathbf{u} \sin t \theta \end{array}\right] qt=exp(tuθ)=[costθusintθ]
研究四元数的目的就是用来表示旋转,接下来就是总结下四元数如何表示旋转以及四元数与其他旋转表示形式之间的关系,包括轴角和旋转矩阵。
对一个向量进行旋转的操作如下图所示:
对旋转操作表示最为直接的是轴角表示形式,如上图所示 u \mathbf{u} u即为旋转轴, ϕ \phi ϕ即为旋转角,对于向量 x \mathbf{x} x可以分解为与旋转轴平行的部分 x ∥ \mathbf{x}_{\|} x∥以及与旋转轴垂直的部分 x ⊥ \mathbf{x}_{\perp} x⊥,如下: x ∥ = u ( ∥ x ∥ cos α ) = u u ⊤ x x ⊥ = x − x ∥ = x − u u ⊤ x \begin{array}{l} \mathbf{x}_{\|}=\mathbf{u}(\|\mathbf{x}\| \cos \alpha)=\mathbf{u} \mathbf{u}^{\top} \mathbf{x} \\ \mathbf{x}_{\perp}=\mathbf{x}-\mathbf{x}_{\|}=\mathbf{x}-\mathbf{u} \mathbf{u}^{\top} \mathbf{x} \end{array} x∥=u(∥x∥cosα)=uu⊤xx⊥=x−x∥=x−uu⊤x向量平行部分不会受到旋转操作的影响,而垂直部分经过旋转操作变为 x ⊥ ′ = x ⊥ cos ϕ + ( u × x ) sin ϕ \mathbf{x}_{\perp}^{\prime}=\mathbf{x}_{\perp} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi x⊥′=x⊥cosϕ+(u×x)sinϕ因此旋转操作可以表示为 x ′ = x ∥ + x ⊥ cos ϕ + ( u × x ) sin ϕ \mathbf{x}^{\prime}=\mathbf{x}_{\|}+\mathbf{x}_{\perp} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi x′=x∥+x⊥cosϕ+(u×x)sinϕ旋转操作同时具备一下三条性质:
第一条:旋转保留向量的长度 ∥ r ( v ) ∥ = ⟨ r ( v ) , r ( v ) ⟩ = ⟨ v , v ⟩ ≜ ∥ v ∥ , ∀ v ∈ R 3 \|r(\mathbf{v})\|=\sqrt{\langle r(\mathbf{v}), r(\mathbf{v})\rangle}=\sqrt{\langle\mathbf{v}, \mathbf{v}\rangle} \triangleq\|\mathbf{v}\|, \quad \forall \mathbf{v} \in \mathbb{R}^{3} ∥r(v)∥=⟨r(v),r(v)⟩=⟨v,v⟩≜∥v∥,∀v∈R3第二条:旋转保留向量间的角度: ⟨ r ( v ) , r ( w ) ⟩ = ⟨ v , w ⟩ = ∥ v ∥ ∥ w ∥ cos α , ∀ v , w ∈ R 3 \langle r(\mathbf{v}), r(\mathbf{w})\rangle=\langle\mathbf{v}, \mathbf{w}\rangle=\|\mathbf{v}\|\|\mathbf{w}\| \cos \alpha, \quad \forall \mathbf{v}, \mathbf{w} \in \mathbb{R}^{3} ⟨r(v),r(w)⟩=⟨v,w⟩=∥v∥∥w∥cosα,∀v,w∈R3第三条:保留了向量间的相对方向 u × v = w ⟺ r ( u ) × r ( v ) = r ( w ) \mathbf{u} \times \mathbf{v}=\mathbf{w} \Longleftrightarrow r(\mathbf{u}) \times r(\mathbf{v})=r(\mathbf{w}) u×v=w⟺r(u)×r(v)=r(w)因此,旋转可以定义为 S O ( 3 ) S O(3) SO(3)群,即: S O ( 3 ) : { r : R 3 → R 3 / ∀ v , w ∈ R 3 , ∥ r ( v ) ∥ = ∥ v ∥ , r ( v ) × r ( w ) = r ( v × w ) } S O(3):\left\{r: \mathbb{R}^{3} \rightarrow \mathbb{R}^{3} / \forall \mathbf{v}, \mathbf{w} \in \mathbb{R}^{3},\|r(\mathbf{v})\|=\|\mathbf{v}\|, r(\mathbf{v}) \times r(\mathbf{w})=r(\mathbf{v} \times \mathbf{w})\right\} SO(3):{r:R3→R3/∀v,w∈R3,∥r(v)∥=∥v∥,r(v)×r(w)=r(v×w)}
旋转矩阵和轴角之间的关系学习过《视觉SLAM十四讲》的同学应该都比较清楚了,这里先回顾一下,旋转矩阵对向量进行旋转操作的公式为: x ′ = R x \mathbf{x}^{\prime}=\mathbf{R} \mathbf{x} x′=Rx旋转矩阵的性质有 R ⊤ R = I = R R ⊤ \mathbf{R}^{\top} \mathbf{R}=\mathbf{I}=\mathbf{R} \mathbf{R}^{\top} R⊤R=I=RR⊤根据这个性质我们对时间进行微分有 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} \mathbf{R}\right)^{\top} R⊤R˙=−(R⊤R)⊤这就意味着 R ⊤ R ˙ \mathbf{R}^{\top} \dot{\mathbf{R}} R⊤R˙是一个反对称矩阵,那么我可以定义这样一个反对称矩阵为 [ ω ] × ≜ [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] [\boldsymbol{\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] [ω]×≜⎣⎡0ωz−ωy−ωz0ωxωy−ωx0⎦⎤这样我们就可以通过 ω = ( ω x , ω y , ω z ) ∈ R 3 \boldsymbol{\omega}=\left(\omega_{x}, \omega_{y}, \omega_{z}\right) \in \mathbf{R}^{3} ω=(ωx,ωy,ωz)∈R3来描述旋转矩阵及其微分间的关系,而由 ω \boldsymbol{\omega} ω构成的反对称矩阵 [ ω ] × [\boldsymbol{\omega}]_{\times} [ω]×即构成了我们熟悉的李代数空间 s o ( 3 ) \mathfrak{s o}(3) so(3),因为我们的目的是用来描述相机的旋转变化,因此我们可以将旋转矩阵与时间 t t t进行关联,然后就可以获得这样微分方程 R ˙ ( t ) = R ( t ) [ ω ] × \dot{\mathbf{R}}(t)=\mathbf{R}(t)[\boldsymbol{\omega}]_{\times} R˙(t)=R(t)[ω]×我们可以发现,当 R ( 0 ) = I \mathbf{R}(0)=\mathbf{I} R(0)=I时, R ˙ ( 0 ) = [ ω ] × \dot{\mathbf{R}}(0)=[\boldsymbol{\omega}]_{\times} R˙(0)=[ω]×,那么我们可以将 s o ( 3 ) \mathfrak{s o}(3) so(3)理解为 S O ( 3 ) S O(3) SO(3)在原点处( t = 0 t=0 t=0)的正切空间或者速度空间,当 ω \boldsymbol{\omega} ω为常数时,微分方程对时间积分得 R ( t ) = R ( 0 ) e [ ω ] × t = R ( 0 ) e [ ω t ] × \mathbf{R}(t)=\mathbf{R}(0) e^{[\boldsymbol{\omega}]_{\times} t}=\mathbf{R}(0) e^{[\boldsymbol{\omega} t]_{\times}} R(t)=R(0)e[ω]×t=R(0)e[ωt]×我们将 ϕ ≜ ω Δ t \boldsymbol{\phi} \triangleq \boldsymbol{\omega} \Delta t ϕ≜ωΔt,那么有 R = e [ ϕ ] × \mathbf{R}=e^{[\boldsymbol{\phi}]_{\times}} R=e[ϕ]×我们定义 ϕ = v \boldsymbol{\phi}=\mathbf{v} ϕ=v那么 ϕ \boldsymbol{\phi} ϕ、 [ ϕ ] × [\boldsymbol{\phi}]_{\times} [ϕ]×和 R \mathbf{R} R之间的关系可以通过下图描述:这里定义了旋转矩阵的指数映射 Exp : R 3 → S O ( 3 ) ; ϕ ↦ Exp ( ϕ ) = e [ ϕ ] \operatorname{Exp}: \mathbb{R}^{3} \rightarrow S O(3) ; \boldsymbol{\phi} \mapsto \operatorname{Exp}(\boldsymbol{\phi})=e^{[\boldsymbol{\phi}]} Exp:R3→SO(3);ϕ↦Exp(ϕ)=e[ϕ],我们进一步对指数映射进行展开,定义 ϕ = ϕ u \boldsymbol{\phi}=\phi \mathbf{u} ϕ=ϕu, u \mathbf{u} u是单位向量,那么 [ ϕ ] × = ϕ [ u ] × [\phi]_{\times}=\phi[\mathbf{u}]_{\times} [ϕ]×=ϕ[u]×,指数映射就可以泰勒展开为 R = e ϕ [ u ] × = I + ϕ [ u ] × + 1 2 ϕ 2 [ u ] × 2 + 1 3 ! ϕ 3 [ u ] × 3 + 1 4 ! ϕ 4 [ u ] × 4 + … \mathbf{R}=e^{\phi[\mathbf{u}]_{\times}}=\mathbf{I}+\phi[\mathbf{u}]_{\times}+\frac{1}{2} \phi^{2}[\mathbf{u}]_{\times}^{2}+\frac{1}{3 !} \phi^{3}[\mathbf{u}]_{\times}^{3}+\frac{1}{4 !} \phi^{4}[\mathbf{u}]_{\times}^{4}+\ldots R=eϕ[u]×=I+ϕ[u]×+21ϕ2[u]×2+3!1ϕ3[u]×3+4!1ϕ4[u]×4+…因为 u \mathbf{u} u是单位向量,那么有 [ u ] × 2 = u u ⊤ − I [ u ] × 3 = − [ u ] × \begin{array}{l} {[\mathbf{u}]_{\times}^{2}=\mathbf{u u}^{\top}-\mathbf{I}} \\ {[\mathbf{u}]_{\times}^{3}=-[\mathbf{u}]_{\times}} \end{array} [u]×2=uu⊤−I[u]×3=−[u]×以及 [ u ] x 4 = − [ u ] x 2 [ u ] x 5 = [ u ] × [ u ] × 6 = [ u ] × 2 [ u ] × 7 = − [ u ] × ⋯ [\mathbf{u}]_{x}^{4}=-[\mathbf{u}]_{x}^{2} \quad[\mathbf{u}]_{x}^{5}=[\mathbf{u}]_{\times} \quad[\mathbf{u}]_{\times}^{6}=[\mathbf{u}]_{\times}^{2} \quad[\mathbf{u}]_{\times}^{7}=-[\mathbf{u}]_{\times} \cdots [u]x4=−[u]x2[u]x5=[u]×[u]×6=[u]×2[u]×7=−[u]×⋯因此可以将指数映射的泰勒展开式划分为以 [ u ] × [\mathbf{u}]_{\times} [u]×和以 [ u ] × 2 [\mathbf{u}]_{\times}^{2} [u]×2为基的两组,前面的系数分别是 sin ϕ \sin \phi sinϕ和 cos ϕ \cos \phi cosϕ的展开式,因此我们就获得了著名的Rodirigues公式: R = I + sin ϕ [ u ] × + ( 1 − cos ϕ ) [ u ] × 2 \mathbf{R}=\mathbf{I}+\sin \phi[\mathbf{u}]_{\times}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2} R=I+sinϕ[u]×+(1−cosϕ)[u]×2有了指数映射我们逆推出对数映射 Log : S O ( 3 ) → R 3 ; R ↦ Log ( R ) = u ϕ \operatorname{Log}: S O(3) \rightarrow \mathbb{R}^{3} ; \mathbf{R} \mapsto \operatorname{Log} (\mathbf{R})=\mathbf{u} \phi Log:SO(3)→R3;R↦Log(R)=uϕ,即 ϕ = arccos ( trace ( R ) − 1 2 ) u = ( R − R ⊤ ) ∨ 2 sin ϕ \begin{array}{l} \phi=\arccos \left(\frac{\operatorname{trace}(\mathbf{R})-1}{2}\right) \\ \mathbf{u}=\frac{\left(\mathbf{R}-\mathbf{R}^{\top}\right)^{\vee}}{2 \sin \phi} \end{array} ϕ=arccos(2trace(R)−1)u=2sinϕ(R−R⊤)∨这里我们再回过头来看下旋转矩阵对向量的操作 x ′ = R x \mathbf{x}^{\prime}=\mathbf{R} \mathbf{x} x′=Rx,我们用Rodirigues公式进行替换,有 x ′ = R x = ( I + sin ϕ [ u ] × + ( 1 − cos ϕ ) [ u ] × 2 ) x = x + sin ϕ [ u ] × x + ( 1 − cos ϕ ) [ u ] × 2 x = x + sin ϕ ( u × x ) + ( 1 − cos ϕ ) ( u u ⊤ − I ) x = x ∥ + x ⊥ + sin ϕ ( u × x ) − ( 1 − cos ϕ ) x ⊥ = x ∥ + ( u × x ) sin ϕ + x ⊥ cos ϕ \begin{aligned} \mathbf{x}^{\prime}=& \mathbf{R} \mathbf{x} \\ =&\left(\mathbf{I}+\sin \phi[\mathbf{u}]_{\times}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2}\right) \mathbf{x} \\ =& \mathbf{x}+\sin \phi[\mathbf{u}]_{\times} \mathbf{x}+(1-\cos \phi)[\mathbf{u}]_{\times}^{2} \mathbf{x} \\ =& \mathbf{x}+\sin \phi(\mathbf{u} \times \mathbf{x})+(1-\cos \phi)\left(\mathbf{u} \mathbf{u}^{\top}-\mathbf{I}\right) \mathbf{x} \\ =& \mathbf{x}_{\|}+\mathbf{x}_{\perp}+\sin \phi(\mathbf{u} \times \mathbf{x})-(1-\cos \phi) \mathbf{x}_{\perp} \\ =& \mathbf{x}_{\|}+(\mathbf{u} \times \mathbf{x}) \sin \phi+\mathbf{x}_{\perp} \cos \phi \end{aligned} x′======Rx(I+sinϕ[u]×+(1−cosϕ)[u]×2)xx+sinϕ[u]×x+(1−cosϕ)[u]×2xx+sinϕ(u×x)+(1−cosϕ)(uu⊤−I)xx∥+x⊥+sinϕ(u×x)−(1−cosϕ)x⊥x∥+(u×x)sinϕ+x⊥cosϕ然后我们发现了什么!这前面定义的轴角对向量的旋转操作公式一模一样,那么说明 ϕ ≜ ω Δ t \boldsymbol{\phi} \triangleq \boldsymbol{\omega} \Delta t ϕ≜ωΔt即旋转操作的轴角 v \mathbf{v} v, ω \boldsymbol{\omega} ω即旋转角速度
前面介绍旋转矩阵与轴角的关系,接下来按照同样的思路介绍四元数与轴角的关系,四元数对旋转的操作为: r ( v ) = q ⊗ v ⊗ q ∗ r(\mathbf{v})=\mathbf{q} \otimes \mathbf{v} \otimes \mathbf{q}^{*} r(v)=q⊗v⊗q∗根据旋转操作的性质,旋转操作不改变向量的长度 ∥ q ⊗ v ⊗ q ∗ ∥ = ∥ q ∥ 2 ∥ v ∥ = ∥ v ∥ \left\|\mathbf{q} \otimes \mathbf{v} \otimes \mathbf{q}^{*}\right\|=\|\mathbf{q}\|^{2}\|\mathbf{v}\|=\|\mathbf{v}\| ∥q⊗v⊗q∗∥=∥q∥2∥v∥=∥v∥,因此有 q ∗ ⊗ q = 1 = q ⊗ q ∗ \mathbf{q}^{*} \otimes \mathbf{q}=1=\mathbf{q} \otimes \mathbf{q}^{*} q∗⊗q=1=q⊗q∗可以看出来,表示旋转的四元数都单位四元数,我们将这样的四元数构成空间定义为 q ∈ S 3 q \in S^{3} q∈S3,同样我们进行微分操作 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˙是一个纯四元数,同理我们定义个纯四元数 Ω ∈ H p \Omega \in \mathbb{H}_{p} Ω∈Hp有 q ∗ ⊗ q ˙ = Ω = [ 0 Ω ] ∈ H p \mathbf{q}^{*} \otimes \dot{\mathbf{q}}=\Omega=\left[\begin{array}{l} 0 \\ \Omega \end{array}\right] \in \mathbb{H}_{p} q∗⊗q˙=Ω=[0Ω]∈Hp同样我们和时间 t t t关联起来,然后就可以获得微分方程 q ( t ) ˙ = q ( t ) ⊗ Ω \dot{\mathbf{q}(t)}=\mathbf{q}(t) \otimes \Omega q(t)˙=q(t)⊗Ω,当 q ( 0 ) = 1 \mathbf{q}(0)=1 q(0)=1时, q ˙ ( 0 ) = Ω ∈ H p \dot{\mathbf{q}}(0)=\Omega \in \mathbb{H}_{p} q˙(0)=Ω∈Hp,因此 H p \mathbb{H}_{p} Hp就是 S 3 S^{3} S3的正切空间或者李代数,我们假设 Ω \Omega Ω是常数,我们就可以进一步积分有 q ( t ) = q ( 0 ) ⊗ e Ω Δ t \mathbf{q}(t)=\mathbf{q}(0) \otimes e^{\Omega \Delta t} q(t)=q(0)⊗eΩΔt因为 q ( t ) \mathbf{q}(t) q(t)和 q ( 0 ) \mathbf{q}(0) q(0)都是单位四元数,因此 e Ω Δ t e^{\Omega \Delta t} eΩΔt也是单位四元数,我们定义 V ≜ Ω Δ t \mathbf{V} \triangleq \Omega \Delta t V≜ΩΔt,因此有 q = e V \mathbf{q}=e^{\mathbf{V}} q=eV根据1.8节数四元数对数定义的介绍, V \mathbf{V} V是一个纯四元数,因此我们可以定义 V = θ u \mathbf{V}=\theta \mathbf{u} V=θu,然后我们定义 v = ϕ u \mathbf{v}=\phi \mathbf{u} v=ϕu,其中 θ = ϕ / 2 \theta=\phi / 2 θ=ϕ/2(和旋转矩阵一样,接下来会通过推到证明 v \mathbf{v} v其实就是轴角),这里给出四元数指数映射关系这里定义的指数映射为 Exp ( ϕ ) ≜ exp ( ϕ / 2 ) \operatorname{Exp}(\phi) \triangleq \exp (\phi / 2) Exp(ϕ)≜exp(ϕ/2),我们对指数映射进行展开有 q ≜ Exp ( ϕ u ) = e ϕ u / 2 = cos ϕ 2 + u sin ϕ 2 = [ cos ( ϕ / 2 ) u sin ( ϕ / 2 ) ] \mathbf{q} \triangleq \operatorname{Exp}(\phi \mathbf{u})=e^{\phi \mathbf{u} / 2}=\cos \frac{\phi}{2}+\mathbf{u} \sin \frac{\phi}{2}=\left[\begin{array}{c} \cos (\phi / 2) \\ \mathbf{u} \sin (\phi / 2) \end{array}\right] q≜Exp(ϕu)=eϕu/2=cos2ϕ+usin2ϕ=[cos(ϕ/2)usin(ϕ/2)]同理定义对数映射 Log : S 3 → R 3 ; q ↦ Log ( q ) = u ϕ \operatorname{Log} : S^{3} \rightarrow \mathbb{R}^{3} ; \mathbf{q} \mapsto \operatorname{Log} (\mathbf{q})=\mathbf{u} \phi Log:S3→R3;q↦Log(q)=uϕ,其中 ϕ = 2 arctan ( ∥ q v ∥ , q w ) u = q v / ∥ q v ∥ \begin{array}{l} \phi=2 \arctan \left(\left\|\mathbf{q}_{v}\right\|, q_{w}\right) \\ \mathbf{u}=\mathbf{q}_{v} /\left\|\mathbf{q}_{v}\right\| \end{array} ϕ=2arctan(∥qv∥,qw)u=qv/∥qv∥最后我们回过头来看四元数对向量的旋转操作来证明四元数与轴角的关系 x ′ = q ⊗ x ⊗ q ∗ = ( cos ϕ 2 + u sin ϕ 2 ) ⊗ ( 0 + x ) ⊗ ( cos ϕ 2 − u sin ϕ 2 ) = x cos 2 ϕ 2 + ( u ⊗ x − x ⊗ u ) sin ϕ 2 cos ϕ 2 − u ⊗ x ⊗ u sin 2 ϕ 2 = x cos 2 ϕ 2 + 2 ( u × x ) sin ϕ 2 cos ϕ 2 − ( x ( u ⊤ u ) − 2 u ( u ⊤ x ) ) sin 2 ϕ 2 = x ( cos 2 ϕ 2 − sin 2 ϕ 2 ) + ( u × x ) ( 2 sin ϕ 2 cos ϕ 2 ) + u ( u ⊤ x ) ( 2 sin 2 ϕ 2 ) = x cos ϕ + ( u × x ) sin ϕ + u ( u ⊤ x ) ( 1 − cos ϕ ) = ( x − u u ⊤ x ) cos ϕ + ( u × x ) sin ϕ + u u ⊤ x = 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} \cos ^{2} \frac{\phi}{2}+(\mathbf{u} \otimes \mathbf{x}-\mathbf{x} \otimes \mathbf{u}) \sin \frac{\phi}{2} \cos \frac{\phi}{2}-\mathbf{u} \otimes \mathbf{x} \otimes \mathbf{u} \sin ^{2} \frac{\phi}{2} \\ =& \mathbf{x} \cos ^{2} \frac{\phi}{2}+2(\mathbf{u} \times \mathbf{x}) \sin \frac{\phi}{2} \cos \frac{\phi}{2}-\left(\mathbf{x}\left(\mathbf{u}^{\top} \mathbf{u}\right)-2 \mathbf{u}\left(\mathbf{u}^{\top} \mathbf{x}\right)\right) \sin ^{2} \frac{\phi}{2} \\ =& \mathbf{x}\left(\cos ^{2} \frac{\phi}{2}-\sin ^{2} \frac{\phi}{2}\right)+(\mathbf{u} \times \mathbf{x})\left(2 \sin \frac{\phi}{2} \cos \frac{\phi}{2}\right)+\mathbf{u}\left(\mathbf{u}^{\top} \mathbf{x}\right)\left(2 \sin ^{2} \frac{\phi}{2}\right) \\ =& \mathbf{x} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi+\mathbf{u}\left(\mathbf{u}^{\top} \mathbf{x}\right)(1-\cos \phi) \\ =&\left(\mathbf{x}-\mathbf{u} \mathbf{u}^{\top} \mathbf{x}\right) \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi+\mathbf{u} \mathbf{u}^{\top} \mathbf{x} \\ =& \mathbf{x}_{\perp} \cos \phi+(\mathbf{u} \times \mathbf{x}) \sin \phi+\mathbf{x}_{\|} \end{aligned} x′========q⊗x⊗q∗(cos2ϕ+usin2ϕ)⊗(0+x)⊗(cos2ϕ−usin2ϕ)xcos22ϕ+(u⊗x−x⊗u)sin2ϕcos2ϕ−u⊗x⊗usin22ϕxcos22ϕ+2(u×x)sin2ϕcos2ϕ−(x(u⊤u)−2u(u⊤x))sin22ϕx(cos22ϕ−sin22ϕ)+(u×x)(2sin2ϕcos2ϕ)+u(u⊤x)(2sin22ϕ)xcosϕ+(u×x)sinϕ+u(u⊤x)(1−cosϕ)(x−uu⊤x)cosϕ+(u×x)sinϕ+uu⊤xx⊥cosϕ+(u×x)sinϕ+x∥可以看出来,前面我们定义的 v = ϕ u \mathbf{v}=\phi \mathbf{u} v=ϕu其实就是轴角,而四元数代表的角度 θ \theta θ其实为实际旋转角度的一半,这也就是论文中提到的四元素的double cover性质。
我们已知 q ⊗ x ⊗ q ∗ = R x \mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*}=\mathbf{R} \mathbf{x} q⊗x⊗q∗=Rx令 x \mathbf{x} x为单位向量,那么就可以获得 R = [ q w 2 + q x 2 − q y 2 − q z 2 2 ( q x q y − q w q z ) 2 ( q x q z + q w q y ) 2 ( q x q y + q w q z ) q w 2 − q x 2 + q y 2 − q z 2 2 ( q y q z − q w q x ) 2 ( q x q z − q w q y ) 2 ( q y q z + q w q x ) q w 2 − q x 2 − q y 2 + q z 2 ] \mathbf{R}=\left[\begin{array}{ccc} q_{w}^{2}+q_{x}^{2}-q_{y}^{2}-q_{z}^{2} & 2\left(q_{x} q_{y}-q_{w} q_{z}\right) & 2\left(q_{x} q_{z}+q_{w} q_{y}\right) \\ 2\left(q_{x} q_{y}+q_{w} q_{z}\right) & q_{w}^{2}-q_{x}^{2}+q_{y}^{2}-q_{z}^{2} & 2\left(q_{y} q_{z}-q_{w} q_{x}\right) \\ 2\left(q_{x} q_{z}-q_{w} q_{y}\right) & 2\left(q_{y} q_{z}+q_{w} q_{x}\right) & q_{w}^{2}-q_{x}^{2}-q_{y}^{2}+q_{z}^{2} \end{array}\right] R=⎣⎡qw2+qx2−qy2−qz22(qxqy+qwqz)2(qxqz−qwqy)2(qxqy−qwqz)qw2−qx2+qy2−qz22(qyqz+qwqx)2(qxqz+qwqy)2(qyqz−qwqx)qw2−qx2−qy2+qz2⎦⎤根据四元数的性质我们还可以获得另外一种表达形式 q ⊗ x ⊗ q ∗ = [ q ∗ ] R [ q ] L [ 0 x ] = [ 0 R x ] \mathbf{q} \otimes \mathbf{x} \otimes \mathbf{q}^{*}=\left[\begin{array}{l} \mathbf{q}^{*} \end{array}\right]_{R}[\mathbf{q}]_{L}\left[\begin{array}{l} 0 \\ \mathbf{x} \end{array}\right]=\left[\begin{array}{c} 0 \\ \mathbf{R} \mathbf{x} \end{array}\right] q⊗x⊗q∗=[q∗]R[q]L[0x]=[0Rx]那么我们可以获得 R = ( q w 2 − q v ⊤ q v ) I + 2 q v q v ⊤ + 2 q w [ q v ] × \mathbf{R}=\left(q_{w}^{2}-\mathbf{q}_{v}^{\top} \mathbf{q}_{v}\right) \mathbf{I}+2 \mathbf{q}_{v} \mathbf{q}_{v}^{\top}+2 q_{w}\left[\mathbf{q}_{v}\right]_{\times} R=(qw2−qv⊤qv)I+2qvqv⊤+2qw[qv]×以上两种都是从四元数到旋转矩阵的关系,从旋转矩阵获得四元数可以根据上面第一种表达式直接逆推获得 q w = ( tr ( R ) + 1 2 q x = R 32 − R 23 4 q w q y = R 13 − R 31 4 q w q z = R 21 − R 12 4 q w \begin{aligned} q_w &=\frac{\sqrt{(\operatorname{tr}(\mathbf{R})+1}}{2} \\ q_x &=\frac{\mathbf{R}_{32}-\mathbf{R}_{23}}{4 q_w} \\ q_y &=\frac{\mathbf{R}_{13}-\mathbf{R}_{31}}{4 q_w} \\ q_z &=\frac{\mathbf{R}_{21}-\mathbf{R}_{12}}{4 q_w} \end{aligned} qwqxqyqz=2(tr(R)+1=4qwR32−R23=4qwR13−R31=4qwR21−R12
参考文献中这一部分还介绍了很多内容,包括旋转的雅克比等,由于篇幅原因,这里只总结其中关于四元数积分和微分的一部分
参考文献中给出了三种类型的雅克比
第一种是向量对向量的雅克比: ∂ ( q ⊗ a ⊗ q ∗ ) ∂ a = ∂ ( R a ) ∂ a = R \frac{\partial(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q} *)}{\partial \mathbf{a}}=\frac{\partial(\mathbf{R} \mathbf{a})}{\partial \mathbf{a}}=\mathbf{R} ∂a∂(q⊗a⊗q∗)=∂a∂(Ra)=R第二种是向量对四元数的雅克比(推导过程参考文献): ∂ ( q ⊗ a ⊗ q ∗ ) ∂ q = 2 [ w a + v × a ∣ v ⊤ a I 3 + v a ⊤ − a v ⊤ − w [ a ] × ] ∈ R 3 × 4 \frac{\partial\left(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q}^{*}\right)}{\partial \mathbf{q}}=2\left[w \mathbf{a}+\mathbf{v} \times \mathbf{a} \mid \mathbf{v}^{\top} \mathbf{a} \mathbf{I}_{3}+\mathbf{v} \mathbf{a}^{\top}-\mathbf{a} \mathbf{v}^{\top}-w[\mathbf{a}]_{\times}\right] \in \mathbb{R}^{3 \times 4} ∂q∂(q⊗a⊗q∗)=2[wa+v×a∣v⊤aI3+va⊤−av⊤−w[a]×]∈R3×4 ∂ ( q ⊗ a ⊗ q ∗ ) ∂ q = 2 [ w a + v × a ∣ v ⊤ a I 3 + v a ⊤ − a v ⊤ − w [ a ] × ] ∈ R 3 × 4 \frac{\partial\left(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q}^{*}\right)}{\partial \mathbf{q}}=2\left[w \mathbf{a}+\mathbf{v} \times \mathbf{a} \mid \mathbf{v}^{\top} \mathbf{a} \mathbf{I}_{3}+\mathbf{v} \mathbf{a}^{\top}-\mathbf{a} \mathbf{v}^{\top}-w[\mathbf{a}]_{\times}\right] \in \mathbb{R}^{3 \times 4} ∂q∂(q⊗a⊗q∗)=2[wa+v×a∣v⊤aI3+va⊤−av⊤−w[a]×]∈R3×4第三种是向量对轴角的雅克比(推导过程参考文献): ∂ ( q ⊗ a ⊗ q ∗ ) ∂ δ θ = ∂ ( R a ) ∂ δ θ = − R { θ } [ a ] × J r ( θ ) \frac{\partial\left(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q}^{*}\right)}{\partial \delta \boldsymbol{\theta}}=\frac{\partial(\mathbf{R} \mathbf{a})}{\partial \delta \boldsymbol{\theta}}=-\mathbf{R}\{\boldsymbol{\theta}\}[\mathbf{a}]_{\times} \mathbf{J}_{r}(\boldsymbol{\theta}) ∂δθ∂(q⊗a⊗q∗)=∂δθ∂(Ra)=−R{θ}[a]×Jr(θ)第三种求导方式其实就是李代数求导方式,结合《视觉SLAM十四讲》中的知识,这种求导方式其实是不友好的,通常采用扰动模型进行,这篇文献中没有涉及,这里也不进一步进行展开了。
四元数的微分在推导IMU的运动方程时将会用到,四元数的微分定义如下: q ˙ ≜ lim Δ t → 0 q ( t + Δ t ) − q ( t ) Δ t = lim Δ t → 0 q ⊗ Δ q L − q Δ t = lim Δ t → 0 q ⊗ ( [ 1 Δ ϕ L / 2 ] − [ 1 0 ] ) Δ t = lim Δ t → 0 q ⊗ [ 0 Δ ϕ L / 2 ] Δ t = 1 2 q ⊗ [ 0 ω L ] \begin{aligned} \dot{\mathbf{q}} & \triangleq \lim _{\Delta t \rightarrow 0} \frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t} \\ &=\lim _{\Delta t \rightarrow 0} \frac{\mathbf{q} \otimes \Delta \mathbf{q}_{\mathcal{L}}-\mathbf{q}}{\Delta t} \\& =\lim _{\Delta t \rightarrow 0} \frac{\mathbf{q} \otimes\left(\left[\begin{array}{c} 1 \\ \Delta \boldsymbol{\phi}_{\mathcal{L}} / 2 \end{array}\right]-\left[\begin{array}{l} 1 \\ \mathbf{0} \end{array}\right]\right)}{\Delta t} \\& =\lim _{\Delta t \rightarrow 0} \frac{\mathbf{q} \otimes\left[\begin{array}{c} 0 \\ \Delta \boldsymbol{\phi}_{\mathcal{L}} / 2 \end{array}\right]}{\Delta t} \\& =\frac{1}{2} \mathbf{q} \otimes\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_{\mathcal{L}} \end{array}\right] \end{aligned} q˙≜Δt→0limΔtq(t+Δt)−q(t)=Δt→0limΔtq⊗ΔqL−q=Δt→0limΔtq⊗([1ΔϕL/2]−[10])=Δt→0limΔtq⊗[0ΔϕL/2]=21q⊗[0ωL]其中, Δ q L \Delta \mathbf{q}_\mathcal{L} ΔqL为一个局部扰动,根据四元数的指数映射有 Δ q L ≜ Exp ( Δ ϕ L u ) = e Δ ϕ L u / 2 = cos Δ ϕ L 2 + u sin Δ ϕ L 2 = [ cos ( Δ ϕ L / 2 ) u sin ( Δ ϕ L / 2 ) ] \Delta \mathbf{q}_\mathcal{L} \triangleq \operatorname{Exp}(\Delta \phi_\mathcal{L} \mathbf{u})=e^{\Delta \phi_\mathcal{L} \mathbf{u} / 2}=\cos \frac{\Delta \phi_\mathcal{L}}{2}+\mathbf{u} \sin \frac{\Delta \phi_\mathcal{L}}{2}=\left[\begin{array}{c} \cos (\Delta \phi_\mathcal{L} / 2) \\ \mathbf{u} \sin (\Delta \phi_\mathcal{L}/ 2) \end{array}\right] ΔqL≜Exp(ΔϕLu)=eΔϕLu/2=cos2ΔϕL+usin2ΔϕL=[cos(ΔϕL/2)usin(ΔϕL/2)]当 Δ ϕ L \Delta \phi_\mathcal{L} ΔϕL较小时,有如下近似: [ cos ( Δ ϕ L / 2 ) u sin ( Δ ϕ L / 2 ) ] = [ 1 Δ ϕ L / 2 ] \left[\begin{array}{c} \cos \left(\Delta \phi_{\mathcal{L}} / 2\right) \\ \mathbf{u} \sin \left(\Delta \phi_{\mathcal{L}} / 2\right) \end{array}\right]=\left[\begin{array}{c} 1 \\ \Delta \phi_{\mathcal{L}} / 2 \end{array}\right] [cos(ΔϕL/2)usin(ΔϕL/2)]=[1ΔϕL/2]根据四元数乘法规则,我们可以定义 Ω ( ω ) ≜ [ ω ] R = [ 0 − ω ⊤ ω − [ ω ] × ] = [ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y − ω x 0 ] \boldsymbol{\Omega}(\boldsymbol{\omega}) \triangleq[\boldsymbol{\omega}]_{R}=\left[\begin{array}{cc} 0 & -\boldsymbol{\omega}^{\top} \\ \boldsymbol{\omega} & -[\boldsymbol{\omega}]_{\times} \end{array}\right]=\left[\begin{array}{cccc} 0 & -\omega_{x} & -\omega_{y} & -\omega_{z} \\ \omega_{x} & 0 & \omega_{z} & -\omega_{y} \\ \omega_{y} & -\omega_{z} & 0 & \omega_{x} \\ \omega_{z} & \omega_{y} & -\omega_{x} & 0 \end{array}\right] Ω(ω)≜[ω]R=[0ω−ω⊤−[ω]×]=⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎦⎥⎥⎤那么四元数的微分即 q ˙ = 1 2 Ω ( ω L ) q = 1 2 q ⊗ ω L \dot{\mathbf{q}}=\frac{1}{2} \Omega\left(\boldsymbol{\omega}_{\mathcal{L}}\right) \mathbf{q}=\frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega}_{\mathcal{L}} q˙=21Ω(ωL)q=21q⊗ωL如果转化成旋转矩阵是 R ˙ = R [ ω L ] \dot{\mathbf{R}}=\mathbf{R}\left[\boldsymbol{\omega}_{\mathcal{L}}\right]_{ } R˙=R[ωL]
四元数的积分在运动状态的积分时会用到,这里只介绍下最为简单的零阶积分,在我们利用IMU进行测量是实际上获得是如下微分方程 q ˙ ( t ) = 1 2 q ( t ) ⊗ ω ( t ) \dot{\mathbf{q}}(t)=\frac{1}{2} \mathbf{q}(t) \otimes \boldsymbol{\omega}(t) q˙(t)=21q(t)⊗ω(t)根据泰勒展开 q n + 1 = q n + q ˙ n Δ t + 1 2 ! q ¨ n Δ t 2 + 1 3 ! q ¨ n Δ t 3 + 1 4 ! q ¨ n Δ t 4 + ⋯ \mathbf{q}_{n+1}=\mathbf{q}_{n}+\dot{\mathbf{q}}_{n} \Delta t+\frac{1}{2 !} \ddot{\mathbf{q}}_{n} \Delta t^{2}+\frac{1}{3 !} \ddot{\mathbf{q}}_{n} \Delta t^{3}+\frac{1}{4 !} \ddot{\mathbf{q}}_{n} \Delta t^{4}+\cdots qn+1=qn+q˙nΔt+2!1q¨nΔt2+3!1q¨nΔt3+4!1q¨nΔt4+⋯由于 ω ¨ = 0 \ddot{\omega}=0 ω¨=0,因此有 q ˙ n = 1 2 q n ω n q ¨ n = 1 2 2 q n ω n 2 + 1 2 q n ω ˙ q ¨ n = 1 2 3 q n ω n 3 + 1 4 q n ω ˙ ω n + 1 2 q ω n ω ˙ q n ( i ≥ 4 ) = 1 2 i q n ω n i + ⋯ \begin{aligned} \dot{\mathbf{q}}_{n} &=\frac{1}{2} \mathbf{q}_{n} \boldsymbol{\omega}_{n} \\ \ddot{\mathbf{q}}_{n} &=\frac{1}{2^{2}} \mathbf{q}_{n} \boldsymbol{\omega}_{n}^{2}+\frac{1}{2} \mathbf{q}_{n} \dot{\boldsymbol{\omega}} \\ \ddot{\mathbf{q}}_{n} &=\frac{1}{2^{3}} \mathbf{q}_{n} \boldsymbol{\omega}_{n}^{3}+\frac{1}{4} \mathbf{q}_{n} \dot{\boldsymbol{\omega}} \boldsymbol{\omega}_{n}+\frac{1}{2} \mathbf{q} \boldsymbol{\omega}_{n} \dot{\boldsymbol{\omega}} \\ \mathbf{q}_{n}^{(i \geq 4)} &=\frac{1}{2^{i}} \mathbf{q}_{n} \boldsymbol{\omega}_{n}^{i}+\cdots \end{aligned} q˙nq¨nq¨nqn(i≥4)=21qnωn=221qnωn2+21qnω˙=231qnωn3+41qnω˙ωn+21qωnω˙=2i1qnωni+⋯当角速度 ω ( t ) \boldsymbol{\omega}(t) ω(t)为常数时,上面的泰勒展开式进一步退化为 q n + 1 = q n ⊗ ( 1 + 1 2 ω n Δ t + 1 2 ! ( 1 2 ω n Δ t ) 2 + 1 3 ! ( 1 2 ω n Δ t ) 3 + ⋯ ) \mathbf{q}_{n+1}=\mathbf{q}_{n} \otimes\left(1+\frac{1}{2} \boldsymbol{\omega}_{n} \Delta t+\frac{1}{2 !}\left(\frac{1}{2} \boldsymbol{\omega}_{n} \Delta t\right)^{2}+\frac{1}{3 !}\left(\frac{1}{2} \boldsymbol{\omega}_{n} \Delta t\right)^{3}+\cdots\right) qn+1=qn⊗(1+21ωnΔt+2!1(21ωnΔt)2+3!1(21ωnΔt)3+⋯)右测的泰勒展开式其实就是 e ω n Δ t / 2 e^{\omega_{n} \Delta t / 2} eωnΔt/2,并且根据四元数的指数映射有 e ω Δ t / 2 = Exp ( ω Δ t ) = q { ω Δ t } = [ cos ( ∥ ω ∥ Δ t / 2 ) ω ∥ ω ∥ sin ( ∥ ω ∥ Δ t / 2 ) ] e^{\omega \Delta t / 2}=\operatorname{Exp}(\boldsymbol{\omega} \Delta t)=\mathbf{q}\{\boldsymbol{\omega} \Delta t\}=\left[\begin{array}{c} \cos (\|\boldsymbol{\omega}\| \Delta t / 2) \\ \frac{\omega}{\|\boldsymbol{\omega}\|} \sin (\|\boldsymbol{\omega}\| \Delta t / 2) \end{array}\right] eωΔt/2=Exp(ωΔt)=q{ωΔt}=[cos(∥ω∥Δt/2)∥ω∥ωsin(∥ω∥Δt/2)]对于前向积分有 q n + 1 = q n ⊗ q { ω n Δ t } \mathbf{q}_{n+1}=\mathbf{q}_{n} \otimes \mathbf{q}\left\{\boldsymbol{\omega}_{n} \Delta t\right\} qn+1=qn⊗q{ωnΔt}对于后向积分有 q n = q n − 1 ⊗ q { ω n Δ t } \mathbf{q}_{n}=\mathbf{q}_{n-1} \otimes \mathbf{q}\left\{\boldsymbol{\omega}_{n} \Delta t\right\} qn=qn−1⊗q{ωnΔt}对于中值积分有 q n + 1 = q n ⊗ q { ω ‾ Δ t } \mathbf{q}_{n+1}=\mathbf{q}_{n} \otimes \mathbf{q}\{\overline{\boldsymbol{\omega}} \Delta t\} qn+1=qn⊗q{ωΔt}
到这里就按照文献的思路总结了部分四元数的基础知识,中间有一部分知识还需要再研究研究,比如四元数的雅克比,这一部分等研究MSCKF的后端时再仔细推导推导,有问题欢迎指出交流~