扰动_求导_积分

0.引言

经常查，总结一下。推导看原论文。

1.旋转的加减算子

$$\mathrm{S}=\mathrm{R}\oplus \mathbf{\theta }\triangleq \mathrm{R}\circ \mathrm{Exp}(\mathbf{\theta })\mathrm{R},\mathrm{S}\in SO(3),\mathbf{\theta }\in {\mathbb{R}}^3$$$$\begin{array}{rl}& {\mathrm{q}}_{\mathrm{S}}={\mathrm{q}}_{\mathrm{R}}\oplus \mathbf{\theta }={\mathrm{q}}_{\mathrm{R}}\otimes \mathrm{Exp}(\mathbf{\theta })\\ & {\mathbf{R}}_{\mathrm{S}}={\mathbf{R}}_{\mathrm{R}}\oplus \mathbf{\theta }={\mathbf{R}}_{\mathrm{R}}\cdot \mathrm{Exp}(\mathbf{\theta })\text{. }\end{array}$$

---

$$\mathbf{\theta }=\mathrm{S}\ominus \mathrm{R}\triangleq \log \left({\mathrm{R}}^{-1}\circ \mathrm{S}\right)\mathrm{R},\mathrm{S}\in SO(3),\mathbf{\theta }\in {\mathbb{R}}^3$$$$\begin{array}{rl}& \mathbf{\theta }={\mathbf{q}}_{\mathrm{S}}\ominus {\mathbf{q}}_{\mathrm{R}}=\log \left({\mathbf{q}}_{\mathrm{R}}^{\ast }\otimes {\mathbf{q}}_{\mathrm{S}}\right)\\ & \mathbf{\theta }={\mathbf{R}}_{\mathrm{S}}\ominus {\mathbf{R}}_{\mathrm{R}}=\log \left({\mathbf{R}}_{\mathrm{R}}^{\top }{\mathbf{R}}_{\mathrm{S}}\right)\end{array}$$

2.旋转的雅克比矩阵

2.1.对向量求导(被旋转的向量)

$$\frac{\partial (\mathbf{q}\otimes \mathbf{a}\otimes \mathbf{q}\ast )}{\partial \mathbf{a}}=\frac{\partial (\mathbf{R}\mathbf{a})}{\partial \mathbf{a}}=\mathbf{R}$$

2.2.对四元数求导

$$\frac{\partial (\mathbf{q}\otimes \mathbf{a}\otimes \mathbf{q}\ast )}{\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}}^{\top }-w[\mathbf{a}{]}_{\times }\right]\in {\mathbb{R}}^{3\times 4}$$

2.3.对旋转向量求导

• SO(3)的右雅克比（十四讲上面使用的是左雅克比）

$$\begin{array}{rl}{\mathbf{J}}_r(\mathbf{\theta })& =\mathbf{I}-\frac{1-\cos \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^2}[\mathbf{\theta }{]}_{\times }+\frac{\Vert \mathbf{\theta }\Vert -\sin \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^3}[\mathbf{\theta }{]}_{\times }^2\\ {\mathbf{J}}_r^{-1}(\mathbf{\theta })& =\mathbf{I}+\frac{1}{2}[\mathbf{\theta }{]}_{\times }+\left(\frac{1}{\Vert \mathbf{\theta }{\Vert }^2}-\frac{1+\cos \Vert \mathbf{\theta }\Vert }{2\Vert \mathbf{\theta }\Vert \sin \Vert \mathbf{\theta }\Vert }\right)[\mathbf{\theta }{]}_{\times }^2\end{array}$$

• 对旋转向量的雅克比

∂ ( 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​(θ)

3.旋转对时间的导数

3.1.扰动

扰动，使用 Hamilton convention, 右乘(十四讲使用的左乘,左扰动模型):

$$\tilde{\mathbf{q}}=\mathbf{q}\otimes \Delta {\mathbf{q}}_{\mathcal{L}},\tilde{\mathbf{R}}=\mathbf{R}\Delta {\mathbf{R}}_{\mathcal{L}}$$$${\tilde{\mathbf{q}}}_{\mathcal{L}}={\mathbf{q}}_{\mathcal{L}}\otimes \mathrm{Exp}\left(\Delta {\varphi }_{\mathcal{L}}\right),{\tilde{\mathbf{R}}}_{\mathcal{L}}={\mathbf{R}}_{\mathcal{L}}\cdot \mathrm{Exp}\left(\Delta {\varphi }_{\mathcal{L}}\right)$$

若扰动为小量，则（指数映射的近似）：

$$\Delta {\mathbf{q}}_{\mathcal{L}}\approx \left[\begin{array}{c}1\\ \frac{1}{2}\Delta {\varphi }_{\mathcal{L}}\end{array}\right],\Delta {\mathbf{R}}_{\mathcal{L}}\approx \mathbf{I}+{\left[\Delta {\varphi }_{\mathcal{L}}\right]}_{\times }$$

3.2.旋转对时间的导数

与角速度对应：
$$\begin{array}{rl}\dot{\mathbf{q}}& \triangleq \underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \Delta \mathbf{q}\mathcal{L}-\mathbf{q}}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \left(\left[\begin{array}{c}1\\ \Delta {\mathbf{\varphi }}_{\mathcal{L}}/2\end{array}\right]-\left[\begin{array}{l}1\\ 0\end{array}\right]\right)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \left[\begin{array}{c}0\\ \Delta {\mathbf{\varphi }}_{\mathcal{L}}/2\end{array}\right]}{\Delta t}\\ & =\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_{\mathcal{L}}\end{array}\right]\end{array}$$

令，
$$\mathbf{\Omega }(\mathbf{\omega })\triangleq [\mathbf{\omega }{]}_R=\left[\begin{array}{cc}0& -{\mathbf{\omega }}^{\top }\\ \mathbf{\omega }& -[\mathbf{\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]$$
可类似推导得：

$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{\Omega }\left({\mathbf{\omega }}_{\mathcal{L}}\right)\mathbf{q}=\frac{1}{2}\mathbf{q}\otimes {\mathbf{\omega }}_{\mathcal{L}},\dot{\mathbf{R}}=\mathbf{R}{\left[{\mathbf{\omega }}_{\mathcal{L}}\right]}_{\times }$$$${\mathbf{\omega }}_{\mathcal{L}}=2{\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}},{\left[{\mathbf{\omega }}_{\mathcal{L}}\right]}_{\times }={\mathbf{R}}^{\top }\dot{\mathbf{R}}$$

这个结果也可由 ${\mathbf{q}}^{\ast }\otimes \mathbf{q}=1$ 与 ${\mathbf{R}}^{\top }\mathbf{R}=\mathbf{I}$ 对时间求导推导出(十四讲就是从这个等式可是推导)。

---

• 性质：
  $${\left[\mathbf{R}\mathbf{\omega }\right]}_{\times }=\mathbf{R}{\left[\mathbf{\omega }\right]}_{\times }{\mathbf{R}}^T$$

4.旋转(rotation rates)对时间的积分

$${\mathbf{q}}_{n+1}=\mathbf{q}(t_n+\Delta t)={\mathbf{q}}_n+{\dot{\mathbf{q}}}_n\Delta t+\frac{1}{2!}{\ddot{\mathbf{q}}}_n\Delta t^2+\cdots$$
其中，
$$\begin{gathered} \begin{array}{rl}{\dot{\mathbf{q}}}_n& =\frac{1}{2}{\mathbf{q}}_n{\mathbf{\omega }}_n\\ {\ddot{\mathbf{q}}}_n& =\frac{1}{2^2}{\mathbf{q}}_n{\mathbf{\omega }}_n^2+\frac{1}{2}{\mathbf{q}}_n\dot{\mathbf{\omega }}\end{array} \\ ... \end{gathered}$$
注意省略了$\otimes$符号，${\mathbf{q}}_n=\frac{1}{2}{\mathbf{q}}_n\otimes {\mathbf{\omega }}_n$。

4.1.四元数零阶时间积分

$${\mathbf{q}}_{n+1}={\mathbf{q}}_n\otimes \left(1+\frac{1}{2}{\mathbf{\omega }}_n\Delta t+\frac{1}{2!}{\left(\frac{1}{2}{\mathbf{\omega }}_n\Delta t\right)}^2+\frac{1}{3!}{\left(\frac{1}{2}{\mathbf{\omega }}_n\Delta t\right)}^3+\cdots \right)$$

可以看出后半部分是 $e^{{\mathbf{\omega }}_n\Delta t/2}$ 的泰勒展开，同时我们有 $\Delta \theta ={\mathbf{\omega }}_n\Delta t$,则有：

e ω Δ t / 2 = Exp ⁡ ( ω Δ t ) = q { ω Δ t } = [ cos ⁡ ( ∥ ω ∥ Δ t / 2 ) ω ∥ ω ∥ sin ⁡ ( ∥ ω ∥ Δ t / 2 ) ] e^{\boldsymbol{\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}

4.2.四元数一阶时间积分

$$\begin{array}{rlr}\dot{\omega }& =\frac{{\mathbf{\omega }}_{n+1}-{\mathbf{\omega }}_n}{\Delta t},\ddot{\omega }=\cdots & =0.\end{array}$$

平均角速率：

$$\bar{\omega }={\omega }_n+\frac{1}{2}\dot{\omega }\Delta t$$ q n + 1 ≈ q n ⊗ ( q { ω ‾ Δ t } + Δ t 2 24 [ 0 ω n × ω n + 1 ] ) \mathbf{q}_{n+1} \approx \mathbf{q}_{n} \otimes\left(\mathbf{q}\{\overline{\boldsymbol{\omega}} \Delta t\}+\frac{\Delta t^{2}}{24}\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_{n} \times \boldsymbol{\omega}_{n+1} \end{array}\right]\right) qn+1​≈qn​⊗(q{ωΔt}+24Δt2​[0ωn​×ωn+1​​])