016四元数微分方程

果然，我还是没有看平台式惯导的初始对准和高度通道，而是直接来到了捷联惯导的部分。之前看过四元数部分的内容，所以现在就当是复习了。回顾一下我之前做的笔记，好像没有四元数微分方程，所以还是在这记录一下吧，也可以说搬运一下。

在捷联惯导中，表征从导航系n系到机体系b系的四元数为：

\vec{Q} = c o s \frac{θ}{2} + {\vec{u}}^{n} s i n \frac{θ}{2}

$\vec{Q}=cos\frac{\theta}{2}+\vec{u}^nsin\frac{\theta}{2}$

式中，刚体绕瞬时轴 $\vec{u}^n$ （单位向量）旋转。
两边求导可得：

\frac{d \vec{Q}}{d t} = - \frac{\dot{θ}}{2} s i n \frac{θ}{2} + {\vec{u}}^{n} \frac{\dot{θ}}{2} c o s \frac{θ}{2} + s i n \frac{θ}{2} \frac{d {\vec{u}}^{n}}{d t}

$\frac{d\vec{Q}}{dt}=-\frac{\dot{\theta} } {2}sin\frac{\theta}{2} + \vec{u}^n \frac{ \dot{\theta} }{2} cos\frac{\theta}{2} + sin\frac{\theta}{2} \frac{d\vec{u}^n}{dt}$

可以这样理解， $\vec{u}^n$ 为某一瞬时固定轴，是一个常量，所以其导数为0，则：

\frac{d \vec{Q}}{d t} = - \frac{\dot{θ}}{2} s i n \frac{θ}{2} + {\vec{u}}^{n} \frac{\dot{θ}}{2} c o s \frac{θ}{2}

$\frac{d\vec{Q}}{dt}=-\frac{\dot{\theta} } {2}sin\frac{\theta}{2} + \vec{u}^n \frac{ \dot{\theta} }{2} cos\frac{\theta}{2}$

对于 $\vec{u}^n \frac{ \dot{\theta} }{2}$ 和四元数 $\vec{Q}$ 的乘积有：

{\vec{u}}^{n} \frac{\dot{θ}}{2} ⨂ \vec{Q} = {\vec{u}}^{n} \frac{\dot{θ}}{2} ⨂ (c o s \frac{θ}{2} + {\vec{u}}^{n} \frac{\dot{θ}}{2}) = {\vec{u}}^{n} \frac{\dot{θ}}{2} c o s \frac{θ}{2} + {\vec{u}}^{n} ⨂ {\vec{u}}^{n} \frac{\dot{θ}}{2} s i n \frac{θ}{2}

$\vec{u}^n \frac{ \dot{\theta} }{2} \bigotimes \vec{Q} =\vec{u}^n \frac{ \dot{\theta} }{2} \bigotimes (cos\frac{\theta}{2} + \vec{u}^n \frac{ \dot{\theta} }{2}) = \vec{u}^n \frac{ \dot{\theta} }{2} cos\frac{\theta}{2} + \vec{u}^n \bigotimes \vec{u}^n \frac{\dot{\theta} } {2}sin\frac{\theta}{2}$

而 $\vec{u}^n \bigotimes \vec{u}^n = -1$ ，所以：

{\vec{u}}^{n} \frac{\dot{θ}}{2} ⨂ \vec{Q} = {\vec{u}}^{n} \frac{\dot{θ}}{2} c o s \frac{θ}{2} - \frac{\dot{θ}}{2} s i n \frac{θ}{2} = \frac{d \vec{Q}}{d t}

$\vec{u}^n \frac{ \dot{\theta} }{2} \bigotimes \vec{Q} = \vec{u}^n \frac{ \dot{\theta} }{2} cos\frac{\theta}{2} - \frac{\dot{\theta} } {2}sin\frac{\theta}{2} = \frac{d\vec{Q}}{dt}$

即：

\frac{d \vec{Q}}{d t} = \frac{\dot{θ}}{2} {\vec{u}}^{n} ⨂ \vec{Q}

$\frac{d\vec{Q}}{dt}=\frac{ \dot{\theta} }{2} \vec{u}^n \bigotimes \vec{Q}$

其中， $\dot{\theta}\vec{u}^n$ 表示角速度标量与角速度方向的乘积即为角速度矢量。这里旋转方向从n系到b系，在n系上的投影，如下：

\dot{θ} {\vec{u}}^{n} = {\vec{ω}}_{n b}^{n}

$\dot{\theta}\vec{u}^n = \vec{\omega}_{nb}^{n}$

所以：

\frac{d \vec{Q}}{d t} = \frac{1}{2} {\vec{ω}}_{n b}^{n} ⨂ \vec{Q}

$\frac{d\vec{Q}}{dt}=\frac{1}{2} \vec{\omega}_{nb}^n \bigotimes \vec{Q}$

$\vec{\omega}_{nb}^n$ 需要通过 $\vec{\omega}_{nb}^b$ 求得：

{\vec{ω}}_{n b}^{n} = \vec{Q} ⨂ {\vec{ω}}_{n b}^{b} ⨂ {\vec{Q}}^{*}

$\vec{\omega}_{nb}^n = \vec{Q} \bigotimes \vec{\omega}_{nb}^b \bigotimes \vec{Q}^*$

即：

\frac{d \vec{Q}}{d t} = \frac{1}{2} {\vec{ω}}_{n b}^{n} ⨂ \vec{Q} = \frac{1}{2} \vec{Q} ⨂ {\vec{ω}}_{n b}^{b} ⨂ {\vec{Q}}^{*} ⨂ \vec{Q} = \frac{1}{2} \vec{Q} ⨂ {\vec{ω}}_{n b}^{b}

$\frac{d\vec{Q}}{dt}=\frac{1}{2} \vec{\omega}_{nb}^n \bigotimes \vec{Q} =\frac{1}{2} \vec{Q} \bigotimes \vec{\omega}_{nb}^b \bigotimes \vec{Q}^* \bigotimes \vec{Q} =\frac{1}{2} \vec{Q} \bigotimes \vec{\omega}_{nb}^b$

\frac{d \vec{Q}}{d t} = \frac{1}{2} M^{'} ({\vec{ω}}_{n b}^{b}) \vec{Q}

$\frac{d\vec{Q}}{dt}=\frac{1}{2} M'(\vec{\omega}_{nb}^b) \vec{Q}$

这就是四元数微分方程

式中：

{\vec{ω}}_{n b}^{b} = {\vec{ω}}_{i b}^{b} - {\vec{ω}}_{i n}^{b} = {\vec{ω}}_{i b}^{b} - C_{n}^{b} {\vec{ω}}_{i n}^{n} = {\vec{ω}}_{i b}^{b} - C_{n}^{b} ({\vec{ω}}_{i e}^{n} + {\vec{ω}}_{e n}^{n})

$\vec{\omega}_{nb}^b = \vec{\omega}_{ib}^b - \vec{\omega}_{in}^b = \vec{\omega}_{ib}^b - C_n^b \vec{\omega}_{in}^n = \vec{\omega}_{ib}^b - C_n^b( \vec{\omega}_{ie}^n + \vec{\omega}_{en}^n )$

$\vec{\omega}_{ib}^b$ 为捷联陀螺的输出；
$C_n^b$ 由姿态更新的最新值确定；
$\vec{\omega}_{ie}^n$ 和 $\vec{\omega}_{en}^n$ 分别为导航系相对于地球的角速度和地球自转角速度在导航系上的投影，对于导航系取地理坐标系的情况：

{\vec{ω}}_{i e}^{n} + {\vec{ω}}_{e n}^{n} = [\begin{matrix} - \frac{V_{N}}{R_{M}} \\ ω_{i e} c o s L + \frac{V_{E}}{R_{N}} \\ ω_{i e} s i n L + \frac{V_{E}}{R_{N}} t a n L \end{matrix}]

$\vec{\omega}_{ie}^n + \vec{\omega}_{en}^n = \left[ \begin{matrix} -\frac{V_N}{R_M}\\ \\ \omega_{ie}cosL+\frac{V_E}{R_N}\\ \\ \omega_{ie}sinL+\frac{V_E}{R_N}tanL\\ \end{matrix} \right]$

这在指北方位惯导系统的平台指令角速度中已经证明过。

感觉最近推导的公式有点多，思绪有时候很混乱，所以打算捋一捋思绪，不知道又要有多长时间。

016四元数微分方程

猜你喜欢