文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第4章

基于误差状态卡尔曼滤波器的四元数运动学

4. 扰动，导数和积分

4.1 SO(3)SO(3)中的加法和减法运算符

在向量空间${\mathbb{R}}^n$中，加和减操作是通过’+‘和’-'号实现的。在$SO(3)$中，这是不行的，但是可以定义等效运算符以建立适当的微积分语料库。

    我们因此在元素$\mathsf{R}\in SO(3)$和$\mathsf{R}$的正切空间的元素$\mathbf{\theta }\in {\mathbb{R}}^3$间定义加和减操作符，$\oplus ,\ominus$。

加操作符 “加”操作符$\oplus :SO(3)\times {\mathbb{R}}^3\to SO(3)$得到一个$SO(3)$中的一个元素$\mathsf{S}$,它以一个（通常很小的）旋转构成$SO(3)$的参考元素，即，
$$\mathsf{S}=\mathsf{R}\oplus \mathbf{\theta }\triangleq \mathsf{R}\circ \mathrm{E}\mathrm{x}\mathrm{p}(\mathbf{\theta })\mathsf{R},\mathsf{S}\in SO(3),\mathbf{\theta }\in {\mathbb{R}}^3.\text{\hspace{1em}(157)}$$

请注意，可以为$SO(3)$的任何表示形式定义此运算符。特别是对于四元数和旋转矩阵，
$${\mathbf{q}}_{\mathsf{S}}={\mathbf{q}}_{\mathsf{R}}\oplus \mathbf{\theta }={\mathbf{q}}_{\mathsf{R}}\otimes \mathrm{E}\mathrm{x}\mathrm{p}(\mathbf{\theta })\text{\hspace{1em}(158)}$$

$${\mathbf{R}}_{\mathsf{S}}={\mathbf{R}}_{\mathsf{R}}\oplus \mathbf{\theta }={\mathbf{R}}_{\mathsf{R}}\cdot \mathrm{E}\mathrm{x}\mathrm{p}(\mathbf{\theta }).\text{\hspace{1em}(159)}$$

减操作符 “减”操作符$\ominus :SO(3)\times SO(3)\to {\mathbb{R}}^3$是上面的逆。它返回$SO(3)$的两个元素间的向量角度差$\mathbf{\theta }\in {\mathbb{R}}^3$。该差在参考元素$\mathsf{R}$的正切向量空间中被表达，
$$\mathbf{\theta }=\mathsf{S}\ominus \mathsf{R}\triangleq \mathrm{L}\mathrm{o}\mathrm{g}({\mathsf{R}}^{-1}\circ S)\mathsf{R},\mathsf{S}\in SO(3),\mathbf{\theta }\in {\mathbb{R}}^3,\text{\hspace{1em}(160)}$$

对于四元数和旋转矩阵，
$$\mathbf{\theta }={\mathbf{q}}_{\mathsf{S}}\ominus {\mathbf{q}}_{\mathsf{R}}=\mathrm{L}\mathrm{o}\mathrm{g}({\mathbf{q}}_{\mathsf{R}}^{\ast }\otimes {\mathbf{q}}_{\mathsf{S}})\text{\hspace{1em}(161)}$$

$$\mathbf{\theta }={\mathbf{R}}_{\mathsf{S}}\ominus {\mathbf{R}}_{\mathsf{R}}=\mathrm{L}\mathrm{o}\mathrm{g}({\mathbf{R}}_{\mathsf{R}}^{\top }{\mathbf{R}}_{\mathsf{S}}).\text{\hspace{1em}(162)}$$

    在这两种情况下，注意，即时的向量差通常被假设很小，上述的定义对任何值都成立（直到$SO(3)$流形的第一次覆盖，即对角度$\theta <\pi$）。

4.2 四个可能的导数定义

4.2.1 从向量空间到向量空间的函数

标量和向量的情况遵循导数的经典定义：给定一个函数$f:{\mathbb{R}}^m\to {\mathbb{R}}^n$，我们使用{ $+$,$-$}来定义导数
$$\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\triangleq \underset{\delta \mathbf{x}\to 0}{\lim }\frac{f(\mathbf{x}+\delta \mathbf{x})-f(\mathbf{x})}{\delta \mathbf{x}}\in {\mathbb{R}}^{n\times m}\text{\hspace{1em}(163)}$$

欧拉积分得到该形式的线性表达式
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\Delta \mathbf{x}\in {\mathbb{R}}^n$$

4.2.2 从SO(3)到SO(3)的空间

给定一个函数$f:SO(3)\to SO(3)$，其中$\mathsf{R}\in SO(3)$以及一个局部的小角度变量$\mathbf{\theta }\in {\mathbb{R}}^3$，我们使用{ $\oplus ,\ominus$}来定义如下导数
$$\begin{array}{r}\frac{\partial f(\mathsf{R})}{\partial \mathbf{\theta }}\triangleq \underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{f(\mathsf{R}\oplus \delta \mathbf{\theta })\ominus f(\mathsf{R})}{\delta \mathbf{\theta }}\in {\mathbb{R}}^{3\times 3}\end{array}\text{\hspace{1em}(164)}$$

$$=\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{\mathrm{L}\mathrm{o}\mathrm{g}({\mathrm{f}}^{-1}(\mathsf{R})\mathrm{f}(\mathsf{R}\mathrm{E}\mathrm{x}\mathrm{p}(\delta \mathbf{\theta })))}{\delta \mathbf{\theta }}\text{\hspace{1em}(165)}$$

欧拉积分得到该形式的表达式，
$$f(\mathsf{R}\oplus \Delta \mathbf{\theta })\approx f(\mathsf{R})\oplus \frac{\partial f(\mathsf{R})}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\triangleq f(\mathsf{R})\mathrm{E}\mathrm{x}\mathrm{p}(\frac{\partial f(\mathsf{R})}{\partial \mathbf{\theta }}\Delta \mathbf{\theta })\in SO(3)$$

4.2.3 从向量空间到SO(3)的函数

对于函数$f:{\mathbb{R}}^m\to SO(3)$，我们用${}^{\prime }{+}^{\prime }$表示对向量施加扰动，而${}^{\prime }{\ominus }^{\prime }$表示$SO(3)$的差，
$$\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\triangleq \underset{\delta \mathbf{x}\to 0}{\lim }\frac{f(\mathbf{x}+\delta \mathbf{x})\ominus f(\mathbf{x})}{\delta \mathbf{x}}\in {\mathbb{R}}^{3\times m}\text{\hspace{1em}(166)}$$

$$=\underset{\delta \mathbf{x}\to 0}{\lim }\frac{\mathrm{L}\mathrm{o}\mathrm{g}(f^{-1}(\mathbf{x})f(\mathbf{x}+\delta \mathbf{x})))}{\delta \mathbf{x}}\text{\hspace{1em}(167)}$$

欧拉积分得到该形式的表达式，
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})\oplus \frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\Delta \mathbf{x}\triangleq f(\mathbf{x})\mathrm{E}\mathrm{x}\mathrm{p}(\frac{\partial \mathrm{f}(\mathbf{x})}{\partial \mathbf{x}}\mathrm{\Delta }\mathbf{x})\in SO(3)$$

4.2.4 从SO(3)到向量空间的函数

对于函数$f:SO(3)\to {\mathbb{R}}^n$，我们用${}^{\prime }{\oplus }^{\prime }$表示$SO(3)$扰动，${}^{\prime }{-}^{\prime }$表示向量差，
$$\frac{\partial f(\mathsf{R})}{\partial \mathbf{\theta }}\triangleq \underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{f(\mathsf{R}\oplus \delta \mathbf{\theta })-f(\mathsf{R})}{\delta \mathbf{\theta }}\in {\mathbb{R}}^{n\times 3}\text{\hspace{1em}(168)}$$

$$=\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{f(\mathsf{R}\mathrm{E}\mathrm{x}\mathrm{p}(\delta \mathbf{\theta }))-f(\mathsf{R})}{\delta \mathbf{\theta }}\text{\hspace{1em}(169)}$$

欧拉积分得到该形式的表达式，
$$f(\mathsf{R}\oplus \delta \mathbf{\theta })\approx f(\mathsf{R})\oplus \frac{\partial f(\mathsf{R})}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\triangleq f(\mathsf{R})+\mathrm{E}\mathrm{x}\mathrm{p}(\frac{\partial f(\mathsf{R})}{\partial \mathbf{\theta }}\Delta \mathbf{\theta })\in SO(3)$$

4.3 有用的，且非常有用的，旋转的雅可比

让我们考虑对一个向量$\mathbf{a}$，绕轴$\mathbf{u}$旋转$\theta$弧度。让我们以三种等效形式表达旋转规范，即 θ = θ u ,   q = q { θ } \mathbf{\boldsymbol\theta} = \theta \mathbf{u},~\mathbf{q} = \mathbf{q}\{\boldsymbol\theta\} θ=θu, q=q{ θ}且 R = R { θ } \mathbf{R} = \mathbf{R}\{\boldsymbol\theta\} R=R{ θ}。我们对旋转结果关于不同幅度的雅可比矩阵感兴趣。

4.3.1 关于向量的雅可比

向量$\mathbf{a}$的旋转相对于此向量本身的导数很一般，
$$\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}.\text{\hspace{1em}(170)}$$

4.3.2 关于四元数的雅可比

相反，关于四元数$\mathbf{q}$的旋转的导数很棘手。为了方便，我们对四元数使用较浅的符号，$\mathbf{q}=[w,\mathbf{v}]=w+\mathbf{v}$。我们利用式（35），（34），以及恒等式$\mathbf{a}\times (\mathbf{b}\times \mathbf{c})=(\mathbf{c}\times \mathbf{b})\times \mathbf{a}=({\mathbf{a}}^{\top }\mathbf{c})\mathbf{b}-({\mathbf{a}}^{\top }\mathbf{b})\mathbf{c}$，从而开发基于四元数的旋转（107），

a ′ = q ⊗ a ⊗ q ∗ = ( w + v ) ⊗ a ⊗ ( w − v ) = w 2 a + w ( v ⊗ a − a ⊗ v ) − v ⊗ a ⊗ v = w 2 a + 2 w ( v × a ) − [ ( − v ⊤ a + v × a ) ⊗ v ] = w 2 a + 2 w ( v × a ) − [ ( − v ⊤ a ) v + ( v × a ) ⊗ v ] = w 2 a + 2 w ( v × a ) − [ ( − v ⊤ a ) v − ( v × a ) ⊤ v + ( v × a ) × v ] = w 2 a + 2 w ( v × a ) − [ ( − v ⊤ a ) v + ( v ⊤ v ) a − ( v ⊤ a ) v ] = w 2 a + 2 w ( v × a ) + 2 ( v ⊤ a ) v − ( v ⊤ v ) a   . (171) \begin{aligned}\mathbf{a}' =& \mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q}^* \\ =& (w + \mathbf{v}) \otimes \mathbf{a} \otimes (w - \mathbf{v}) \\ =& w^2\mathbf{a} + w(\mathbf{v \otimes a - a \otimes v}) - \mathbf{v \otimes a \otimes v} \\ =& w^2\mathbf{a} + 2w(\mathbf{v \times a}) - [(-\mathbf{v}^\top \mathbf{a} + \mathbf{v \times a}) \otimes \mathbf{v}] \\ =& w^2\mathbf{a} + 2w(\mathbf{v \times a}) - [(-\mathbf{v}^\top \mathbf{a}) \mathbf{v} + \mathbf{(v \times a) \otimes v}] \\ =& w^2\mathbf{a} + 2w(\mathbf{v \times a}) - [(-\mathbf{v}^\top \mathbf{a})\mathbf{v} -\cancel{(\mathbf{v \times a})^\top \mathbf{v}} + \mathbf{(v \times a) \times v}]\\ =& w^2\mathbf{a} + 2w(\mathbf{v \times a}) - [(-\mathbf{v}^\top \mathbf{a})\mathbf{v} +\mathbf{(v^\top v)a} - \mathbf{(v^\top a)v}]\\ =& w^2\mathbf{a} + 2w(\mathbf{v \times a}) + 2(\mathbf{v}^\top \mathbf{a})\mathbf{v} - (\mathbf{v}^\top \mathbf{v})\mathbf{a}~. \\ \end{aligned} \tag{171} a′========​q⊗a⊗q∗(w+v)⊗a⊗(w−v)w2a+w(v⊗a−a⊗v)−v⊗a⊗vw2a+2w(v×a)−[(−v⊤a+v×a)⊗v]w2a+<