文章翻译—基于误差状态卡尔曼滤波器的四元数运动学—第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），

$$\begin{array}{rl}{\mathbf{a}}^{\prime }=& \mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }\\ =& (w+\mathbf{v})\otimes \mathbf{a}\otimes (w-\mathbf{v})\\ =& w^2\mathbf{a}+w(\mathbf{v}\otimes \mathbf{a}-\mathbf{a}\otimes \mathbf{v})-\mathbf{v}\otimes \mathbf{a}\otimes \mathbf{v}\\ =& w^2\mathbf{a}+2w(\mathbf{v}\times \mathbf{a})-[(-{\mathbf{v}}^{\top }\mathbf{a}+\mathbf{v}\times \mathbf{a})\otimes \mathbf{v}]\\ =& w^2\mathbf{a}\\ =& \\ =& \\ =& \end{array}$$