微分方程-姿态阵微分方程及求解-四元数微分方程及求解-等效旋转矢量微分方程及求解

一、姿态阵微分方程及求解

1、姿态阵微分方程

$${\dot{\mathbf{C}}}_b^i={\mathbf{C}}_b^i\left({\mathbf{\omega }}_{ib}^b\times \right)$$

描述的是姿态的变化和角速率 ${\mathbf{\omega }}_{ib}^b$ 的关系，角速率正好可以由陀螺仪测量得到，三个轴装三个陀螺，就有了微分方程，再结合初值，可得到姿态变化阵。

2、姿态阵微分方程的毕卡级数解

简记
$${\dot{\mathbf{C}}}_b^i={\mathbf{C}}_b^i\left({\mathbf{\omega }}_{ib}^b\times \right)\stackrel{\mathbf{\Omega }(t)=\left[{\mathbf{\omega }}_{ib}^b(t)\times \right]}{⟶}\dot{\mathbf{C}}(t)=\mathbf{C}(t)\mathbf{\Omega }(t)$$
形式上就是一个状态方程。在连续时间Kalman滤波中已经提到过，此方程无初等解，只有毕卡积分级数解：
$$\mathbf{C}(t)=\mathbf{C}(0)\left[\mathbf{I}+{\int }_0^t\mathbf{\Omega }(\tau )\mathrm{d}\tau +{\int }_0^t{\int }_0^{\tau }\mathbf{\Omega }\left({\tau }_1\right)\mathrm{d}{\tau }_1\mathbf{\Omega }(\tau )\mathrm{d}\tau +{\int }_0^t{\int }_0^{\tau }{\int }_0^{{\tau }_1}\mathbf{\Omega }\left({\tau }_2\right)\mathrm{d}{\tau }_2\mathbf{\Omega }\left({\tau }_1\right)\mathrm{d}{\tau }_1\mathbf{\Omega }(\tau )\mathrm{d}\tau +\cdots \right]$$
一般情况下，上式就是毕卡级数解，不可再继续化简。上式是收敛的，但还不方便得到闭合形式的初等解。只有在定轴转动的情况下，若转动角速率满足可交换性条件：
$$\mathbf{\Omega }(t)\mathbf{\Omega }(\tau )=\mathbf{\Omega }(\tau )\mathbf{\Omega }(t)$$
递推，可以得到幂指函数形式的闭合解：
$$\mathbf{C}(t)=\mathbf{C}(0)\left\{\mathbf{I}+{\int }_0^t\mathbf{\Omega }(\tau )\mathrm{d}\tau +\frac{1}{2!}{\left[{\int }_0^t\mathbf{\Omega }(\tau )\mathrm{d}\tau \right]}^2+\frac{1}{3!}{\left[{\int }_0^t\mathbf{\Omega }(\tau )\mathrm{d}\tau \right]}^3+\cdots \right\}=\mathbf{C}(0){\mathrm{e}}^{{\int }_0^{\prime }\Omega (\tau )\mathrm{d}\tau }$$

即：
$$\mathbf{C}(t)=\mathbf{C}(0){\mathrm{e}}^{{\int }_0^{\prime }\Omega (\tau )\mathrm{d}\tau }$$

3、定轴转动条件下的姿态更新算法

在 $\mathbf{C}(T)=\mathbf{C}(0){\mathrm{e}}^{{\int }_0^T\Omega (\tau )\mathrm{d}\tau }$ 中：
$${\mathrm{e}}^{{\int }_0^T\mathbf{\Omega }(\tau )\mathrm{d}\tau }={\mathrm{e}}^{{\int }_0^T[\mathbf{\omega }(\tau )\times ]\mathrm{d}\tau }={\mathrm{e}}^{[\mathbf{\theta }(T)\times ]}=\mathbf{I}+\frac{\sin \theta (T)}{\theta (T)}[\mathbf{\theta }(T)\times ]+\frac{1-\cos \theta (T)}{{\theta }^2(T)}[\mathbf{\theta }(T)\times {]}^2$$
将时间区间更改为 $\left[t_{m-1},t_m\right]$，时间的起点都认为是上一时刻的结束时间，则有姿态阵的更新方差：
$${\mathbf{C}}_{b(m)}^i={\mathbf{C}}_{b(m-1)}^i{\mathbf{C}}_{b(m)}^{b(m-1)}$$
当前时刻 $m$ 的姿态阵 ${\mathbf{C}}_{b(m)}^i$ 等于上一时刻的姿态阵 ${\mathbf{C}}_{b(m)}^{i-1}$ 乘以上一时刻到当前时刻的变化量 ${\mathbf{C}}_{b(m)}^{b(m-1)}$ ：
$${\mathbf{C}}_{b(m)}^{b(m-1)}=\mathbf{I}+\frac{\sin \Delta {\theta }_m}{\Delta {\theta }_m}\left(\Delta {\theta }_m\times \right)+\frac{1-\cos \Delta {\theta }_m}{\Delta {\theta }_m^2}{\left(\Delta {\theta }_m\times \right)}^2$$
就等于上一时刻到当前时刻角增量 $\Delta {\theta }_m$ 的幂指函数。所谓角增量，就是在时间段内角速率的积分 $\Delta {\theta }_m={\int }_{t_{m-1}}^{t_m^m}{\omega }_{ib}^b(t)\mathrm{d}t$

二、四元数微分方程及求解

$${\dot{Q}}_b^i=\frac{1}{2}{\mathbf{Q}}_b^i\circ {\mathbf{\omega }}_{\mathrm{b}}^b$$

写成矩阵形式为：
$$\dot{Q}(t)=\frac{1}{2}{\mathbf{M}}_{\mathbf{\omega }(t)}^{\prime }\mathbf{Q}(t)$$
类似于姿态阵微分方程的求解，只有在 $t,\tau \in [0,T]$ 时间段内满足定轴转动条件 $[\mathbf{\omega }(t)\times ][\mathbf{\omega }(\tau )\times ]=[\mathbf{\omega }(\tau )\times ][\mathbf{\omega }(t)\times ]$，使：${\mathbf{M}}_{\omega (t)}^{\prime }{\mathbf{M}}_{\omega (\tau )}^{\prime }={\mathbf{M}}_{\omega (\tau )}^{\prime }{\mathbf{M}}_{\omega (t)}^{\prime }$，才能求得闭合解：
$$\mathbf{Q}(T)={\mathrm{e}}^{\frac{1}{2}\mathbf{\theta }(T)}\mathbf{Q}(0)$$
经过一系列推导得：
$$\mathbf{Q}(T)=\mathbf{Q}(0)\circ \left[\begin{array}{c}\cos \frac{\theta (T)}{2}\\ \frac{\mathbf{\theta }(T)}{\theta (T)}\sin \frac{\theta (T)}{2}\end{array}\right]$$
若将研究时间区间从 $[0,T]$ 改为 $\left[t_{m-1},t_m\right]$，可得姿态四元数的递推公式（定轴转动条件下）：
$$\begin{array}{c}{Q}_{b(m)}^i=Q_{b(m-1)}^i\circ Q_{b(m)}^{b(m-1)}\\ Q_{b(m)}^{b(m-1)}=\left[\begin{array}{c}\cos \frac{\Delta {\theta }_m}{2}\\ \frac{\Delta {\mathbf{\theta }}_m}{\Delta {\theta }_m}\sin \frac{\Delta {\theta }_m}{2}\end{array}\right]\end{array}$$
其中，$Q_{b(m-1)}^i,Q_{b(m)}^i$ 分别表示 $t_{m-1}$ 和 $t_m$ 时刻的姿态变换四元数；$Q_{b(m)}^{b(m-1)}$ 是从 $t_{m-1}$ 时刻到 $t_m$ 时刻的姿态四元数变化，且有 $\Delta {\mathbf{\theta }}_m={\int }_{t_{m-1}}^{t_m}{\mathbf{\omega }}_{\vec{k}}^b\mathrm{d}t$ 和 $\Delta {\theta }_m=\left|\Delta {\mathbf{\theta }}_m\right|$ 。

姿态阵更新方程和四元数更新方程形式非常相似，但四元数算法的计算量和存储量约为姿态阵的一半。

三、等效旋转矢量微分方程及求解

1、Bortz 方程

由四元数推导可得等效旋转矢量微分方程：
$${\mathbf{\omega }}_{\vec{b}}^b={\mathbf{u}}_{\vec{z},}^b\dot{\varphi }+{\dot{\mathbf{u}}}_{\vec{j}}^b\sin \varphi -{\mathbf{u}}_{\vec{j}}^b\times {\dot{\mathbf{u}}}_{\vec{j},}^b(1-\cos \varphi )$$
由一系列的推导，可得更为常用的等效旋转矢量微分方程，也称 Bortz 方程：
$$\begin{array}{c}\dot{\varphi }=\mathbf{\omega }+\frac{1}{2}\mathbf{\varphi }\times \mathbf{\omega }+\frac{1}{{\varphi }^2}\left(1-\frac{\varphi \cdot 2\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}}{2\cdot 2{\sin }^2\frac{\varphi }{2}}\right)(\mathbf{\varphi }\times {)}^2\mathbf{\omega }=\\ \mathbf{\omega }+\frac{1}{2}\mathbf{\varphi }\times \mathbf{\omega }+\frac{1}{{\varphi }^2}\left(1-\frac{\varphi }{2}\cot \frac{\varphi }{2}\right)(\mathbf{\varphi }\times {)}^2\mathbf{\omega }\end{array}$$

2、Bortz 方程的简化

Bortz 方程虽然在理论上是严格成立的，但实际应用时略显繁杂。当等效转动角度 $\varphi =|\varphi |$ 为小量时，常常将方程右边的余切函数 $\cot (\varphi /2)$ 用泰勒级数展开，进行如下近似：
$$\dot{\mathbf{\varphi }}\approx \mathbf{\omega }+\frac{1}{2}\mathbf{\varphi }\times \mathbf{\omega }+\frac{1}{12}(\mathbf{\varphi }\times {)}^2\mathbf{\omega }$$
忽略二阶项，还可进一步近似为：
$$\dot{\varphi }\approx \omega +\frac{1}{2}\mathbf{\varphi }\times \omega$$
在 $\left[t_{m-1},t\right]$ 时间段，对上式两边积分，并迭代一次，再进行简化得：
$$\mathbf{\varphi }(t)\approx \mathbf{\varphi }\left(t_{m-1}\right)+\Delta \mathbf{\theta }\left(t,t_{m-1}\right)+\frac{1}{2}\mathbf{\varphi }\left(t_{m-1}\right)\times \Delta \mathbf{\theta }\left(t,t_{m-1}\right)+\frac{1}{2}{\int }_{t_{m-1}}^t\Delta \mathbf{\theta }\left(\tau ,t_{m-1}\right)\times \mathbf{\omega }(\tau )\mathrm{d}\tau$$
特别地，若假设 $t_{m-1}=0$ 且等效旋转矢量 $\mathbf{\varphi }(0)=0$，则 $\mathbf{\varphi }(t)$ 可表示从 0 时刻开始的等效旋转矢量“增量”，上式可简化为：
$$\mathbf{\varphi }(t)=\Delta \mathbf{\theta }(t)+\frac{1}{2}{\int }_0^t\Delta \mathbf{\theta }(\tau )\times \mathbf{\omega }(\tau )\mathrm{d}\tau =\Delta \mathbf{\theta }(t)+\mathbf{\sigma }(t)$$
其中，记：$\Delta \mathbf{\theta }(t)={\int }_0^t\mathbf{\omega }(\tau )\mathrm{d}\tau$ 和 $\mathbf{\sigma }(t)=\frac{1}{2}{\int }_0^t\Delta \mathbf{\theta }(\tau )\times \mathbf{\omega }(\tau )\mathrm{d}\tau$

$\mathbf{\sigma }(t)=\mathbf{\varphi }(t)-\Delta \mathbf{\theta }(t)$ 表示等效旋转矢量增量 $\mathbf{\varphi }(t)$ 与角增量 $\Delta \mathbf{\theta }(t)$ 之间的差异, 通常称为转动不可交换误差的修正量。对上式两边求得，可得到 Bortz 方程的另一种简化：
$$\dot{\mathbf{\varphi }}(t)=\mathbf{\omega }(t)+\frac{1}{2}\Delta \mathbf{\theta }(t)\times \mathbf{\omega }(t)$$
其优点是右端不再含变量 $\varphi (t)$，方便求解。需要特别指出的是，此式成立的前提条件是: $\varphi (0)=$ $\Delta \mathbf{\theta }(0)=0$ 且 $\mathbf{\varphi }(t)$ 始终为小量，$\mathbf{\varphi }(t)$ 越小近似精度越高。

实际应用时，一般总是以 $t_{m-1}$ 为新的时间起点，令 $\varphi \left(t_{m-1}\right)=0$, ，再根据式 (2.5.17) 计算等效旋 转矢量 $\varphi \left(t_m\right)$，相当于只进行一步计算，这样有利于保证等效旋转矢量始终为小量，降低公式推导过程中的近似误差。在获得 $\varphi \left(t_m\right)$ 之后，改等效旋转矢量递推计算为方向余弦阵或四元数递推，完成姿态递推更新，以四元数为例 (姿态阵类似)，等效旋转矢量与四元数相配合的姿态更新算法如下：
$$\begin{array}{c}{Q}_{b(m)}^i=Q_{b(m-1)}^i\circ Q_{b(m)}^{b(m-1)}\\ Q_{b(m)}^{b(m-1)}=\left[\begin{array}{c}\cos \frac{{\varphi }_m}{2}\\ \frac{{\mathbf{\varphi }}_{\mathbf{m}}}{{\varphi }_m}\sin \frac{{\varphi }_m}{2}\end{array}\right]\end{array}$$
其中, 将 $\mathbf{\varphi }\left({\mathbf{t}}_{\mathbf{m}}\right)$ 简记为 ${\mathbf{\varphi }}_{\mathbf{m}}$，且有 ${\varphi }_m=\left|{\mathbf{\varphi }}_{\mathbf{m}}\right|$，形式与四元数更新方程很相似，但两者有本质区别：

• 四元数更新方程只是简单的用进行变化四元数计算。
• 等效旋转矢量在计算中考虑了转动不可交换误差的补偿，精度更高。

3、不可交换误差的理解

不可交换误差是捷联惯导算法中绕不开的概念，何为不可交换误差？

我们知道矩阵乘法不满足交换律，$A\ast B$​ 不等于 $B\ast A$，但在惯导的姿态解算中，我们就当它们是可以交换的，以得到毕卡级数的收敛数值解。

以姿态阵微分方程为例，想知道可交换性条件的含义，我们可以把它展开成分量形式来看：
$$\begin{array}{l}\mathbf{\Omega }(t)\mathbf{\Omega }(\tau )=[\mathbf{\omega }(t)\times ][\mathbf{\omega }(\tau )\times ]=\left[\begin{array}{ccc}-{\omega }_{1y}{\omega }_{2y}-{\omega }_{1z}{\omega }_{2z}& {\omega }_{1y}{\omega }_{2x}& {\omega }_{1z}{\omega }_{2x}\\ {\omega }_{1x}{\omega }_{2y}& -{\omega }_{1x}{\omega }_{2x}-{\omega }_{1z}{\omega }_{2z}& {\omega }_{1z}{\omega }_{2y}\\ {\omega }_{1x}{\omega }_{2z}& {\omega }_{1y}{\omega }_{2z}& -{\omega }_{1x}{\omega }_{2x}-{\omega }_{1y}{\omega }_{2y}\end{array}\right]\\ \mathbf{\Omega }(\tau )\mathbf{\Omega }(t)=[\mathbf{\omega }(\tau )\times ][\mathbf{\omega }(t)\times ]=\left[\begin{array}{ccc}-{\omega }_{1y}{\omega }_{2y}-{\omega }_{1z}{\omega }_{2z}& {\omega }_{2y}{\omega }_{1x}& {\omega }_{2z}{\omega }_{1x}\\ {\omega }_{2x}{\omega }_{1y}& -{\omega }_{1x}{\omega }_{2x}-{\omega }_{1z}{\omega }_{2z}& {\omega }_{2z}{\omega }_{1y}\\ {\omega }_{2x}{\omega }_{1z}& {\omega }_{2y}{\omega }_{1z}& -{\omega }_{1x}{\omega }_{2x}-{\omega }_{1y}{\omega }_{2y}\end{array}\right]\end{array}$$