CN103776450B - 适用于高速旋转飞行体的半捷联式惯性测量与导航算法 - Google Patents

Abstract

本发明涉及惯性测量与导航算法，具体是一种适用于高速旋转飞行体的半捷联式惯性测量与导航算法。本发明解决了半捷联式惯性测量系统测得的运动信息无法准确反映高速旋转飞行体的运动信息的问题。适用于高速旋转飞行体的半捷联式惯性测量与导航算法，该算法是采用如下步骤实现的：1）实时测出三维比力；实时测出三维角速率；2）实时更新计算出系到n系的姿态矩阵、系相对n系的三维加速度、系相对n系的三维速度、系相对n系的三维位置、系相对n系的三维姿态角；3）求解出三维比力；4）求解出三维角速率；5）求解出三维加速度；6）求解出三维速度；7）求解出三维位置；8）求解出三维姿态角。本发明适用于测量高速旋转飞行体的运动信息。

Description

适用于高速旋转飞行体的半捷联式惯性测量与导航算法

技术领域

本发明涉及惯性测量与导航算法，具体是一种适用于高速旋转飞行体的半捷联式惯性测量与导航算法。

背景技术

在传统的捷联惯导系统中，惯性测量组件（InertialMeasurementUnit，简称IMU）与载体捷联安装（载体运动过程中，惯性测量组件与载体之间没有任何相对运动），因此，三个轴向的加速度计和陀螺仪的敏感轴始终与载体系的对应轴方向一致，惯性测量组件实时测量载体系相对惯性系的运动角速率和加速度信息，然后利用牛顿运动定律推算载体运动的实时姿态、速度和位置等信息。而在适用于高速旋转飞行体的半捷联式惯性测量系统中，由于惯性测量组件是通过具有“隔转止旋”功能的特殊结构安装于高速旋转飞行体内部，因此，除横滚轴方向的加速度计和陀螺仪的敏感轴与高速旋转飞行体的纵轴方向始终一致外（横滚轴方向的加速度计和陀螺仪不随高速旋转飞行体高速旋转，只沿横滚轴方向作低角速率运动），其它两个轴向（俯仰轴方向和航向轴方向）的加速度计和陀螺仪的敏感轴与高速旋转飞行体的对应轴之间的角度会随载体的旋转而变化。因此，在半捷联式惯性测量系统中，惯性测量组件测得的运动信息并不代表载体的运动信息。为此有必要发明一种全新的半捷联式惯性测量与导航算法，以解决半捷联式惯性测量系统测得的运动信息无法准确反映高速旋转飞行体的运动信息的问题。

发明内容

本发明为了解决半捷联式惯性测量系统测得的运动信息无法准确反映高速旋转飞行体的运动信息的问题，提供了一种适用于高速旋转飞行体的半捷联式惯性测量与导航算法。

本发明是采用如下技术方案实现的：适用于高速旋转飞行体的半捷联式惯性测量与导航算法，该算法是采用如下步骤实现的：

1）假设高速旋转飞行体的发射坐标系为导航坐标系，简称为n系；假设高速旋转飞行体对应的坐标系为载体坐标系，简称为b系；假设半捷联式惯性测量系统对应的坐标系为测量坐标系，简称为系；

假设在高速旋转飞行体的发射时刻，系与b系的对应轴向完全一致；当高速旋转飞行体开始运动后，b系随高速旋转飞行体同步变化，系则由于半捷联平台的隔转止旋作用而不随高速旋转飞行体同步变化，但b系和系的横滚轴方向始终一致，且b系和系的横滚角之差为

通过半捷联式惯性测量系统中的三轴加速度计实时测出系相对n系的三维比力；通过半捷联式惯性测量系统中的三轴陀螺仪实时测出系相对n系的三维角速率、系与b系之间的横滚角之差；

2）根据系相对n系的三维比力、系相对n系的三维角速率，实时更新计算出系到n系的姿态矩阵、系相对n系的三维加速度、系相对n系的三维速度、系相对n系的三维位置、系相对n系的三维姿态角；

3）根据系相对n系的三维比力、系与b系之间的横滚角之差，求解出b系相对n系的三维比力；求解公式如下：

$\begin{array}{c}\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\\ 0& -\sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\end{array}\right]\left[\begin{array}{c}{f}_x^{\tilde{b}}\\ f_y^{\tilde{b}}\\ f_z^{\tilde{b}}\end{array}\right]\\ f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T\\ f^{\tilde{b}}={\left[\begin{array}{ccc}{f}_x^{\tilde{b}}& f_y^{\tilde{b}}& f_z^{\tilde{b}}\end{array}\right]}^T\end{array}---\left(1\right);$

式（1）中：f^b为b系相对n系的三维比力；为系相对n系的三维比力；为系与b系之间的横滚角之差；

4）根据系相对n系的三维角速率、系与b系之间的横滚角之差，求解出b系相对n系的三维角速率；求解公式如下：

$w_x^b=w_x^{\tilde{b}}+\stackrel{\·}{\mathrm{\Δ}}{\mathrm{\γ}}_{b\tilde{b}}$

$\begin{array}{c}\left[\begin{array}{c}{w}_y^b\\ w_z^b\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\\ -\sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\end{array}\right]\left[\begin{array}{c}{w}_y^{\tilde{b}}\\ w_z^{\tilde{b}}\end{array}\right]\\ w^b={\left[\begin{array}{ccc}{w}_x^b& w_y^b& w_z^b\end{array}\right]}^T\\ w^{\tilde{b}}={\left[\begin{array}{ccc}{w}_x^{\tilde{b}}& w_y^{\tilde{b}}& w_z^{\tilde{b}}\end{array}\right]}^T\end{array}---\left(2\right);$

式（2）中：w^b为b系相对n系的三维角速率；为系相对n系的三维角速率；为系与b系之间的横滚角之差；

5）根据系相对n系的三维加速度，求解出b系相对n系的三维加速度；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{a}_{\mathrm{bx}}^n=a_{\tilde{b}x}^n\\ a_{\mathrm{by}}^n=a_{\tilde{b}y}^n\\ a_{\mathrm{bz}}^n=a_{\tilde{b}z}^n\end{array}\right.\\ a_b^n={\left[\begin{array}{ccc}{a}_{\mathrm{bx}}^n& a_{\mathrm{by}}^n& a_{\mathrm{bz}}^n\end{array}\right]}^T\\ a_{\tilde{b}}^n={\left[\begin{array}{ccc}{a}_{\tilde{b}x}^n& a_{\tilde{b}y}^n& a_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(3\right);$

式（3）中：为b系相对n系的三维加速度；为系相对n系的三维加速度；

6）根据系相对n系的三维速度，求解出b系相对n系的三维速度；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{v}_{\mathrm{bx}}^n=v_{\tilde{b}x}^n\\ v_{\mathrm{by}}^n=v_{\tilde{b}y}^n\\ v_{\mathrm{bz}}^n=v_{\tilde{b}z}^n\end{array}\right.\\ v_b^n={\left[\begin{array}{ccc}{v}_{\mathrm{bx}}^n& v_{\mathrm{by}}^n& v_{\mathrm{bz}}^n\end{array}\right]}^T\\ v_{\tilde{b}}^n={\left[\begin{array}{ccc}{v}_{\tilde{b}x}^n& v_{\tilde{b}y}^n& v_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(4\right);$

式（4）中：为b系相对n系的三维速度；为系相对n系的三维速度；

7）根据系相对n系的三维位置，求解出b系相对n系的三维位置；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{S}_{\mathrm{bx}}^n=S_{\tilde{b}x}^n\\ S_{\mathrm{by}}^n=S_{\tilde{b}y}^n\\ S_{\mathrm{bz}}^n=S_{\tilde{b}z}^n\end{array}\right.\\ S_b^n={\left[\begin{array}{ccc}{S}_{\mathrm{bx}}^n& S_{\mathrm{by}}^n& S_{\mathrm{bz}}^n\end{array}\right]}^T\\ S_{\tilde{b}}^n={\left[\begin{array}{ccc}{S}_{\tilde{b}x}^n& S_{\tilde{b}y}^n& S_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(5\right);$

式（5）中：为b系相对n系的三维位置；为系相对n系的三维位置；

8）根据系相对n系的三维姿态角、系与b系之间的横滚角之差，求解出b系相对n系的三维姿态角；求解公式如下：

式（6）中：分别为b系相对n系的航向角、b系相对n系的俯仰角、b系相对n系的横滚角；分别为系相对n系的航向角、系相对n系的俯仰角、系相对n系的横滚角；为系与b系之间的横滚角之差。

所述步骤2）中，实时更新计算的步骤包括：

2.1）利用b系相对n系的三维姿态角，计算出b系到n系的姿态矩阵；计算公式如下：

式（7）中：为b系到n系的姿态矩阵；分别为b系相对n系的航向角、b系相对n系的俯仰角、b系相对n系的横滚角；

2.2）推导出用四元数表示的b系到n系的姿态矩阵；表示公式如下：

$C_b^n=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(8\right);$

式（8）中：为b系到n系的姿态矩阵；

2.3）利用初始对准的结果，确定出初始四元数；确定公式如下：

$\left\{\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}\\ \left|q_0\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\end{array}\right.---\left(9\right);$

$\left\{\begin{array}{c}{4q}_1{q}_0=T_{32}-T_{23}\\ {4q}_2{q}_0=T_{13}-T_{31}\\ {4q}_3{q}_0=T_{21}-T_{12}\end{array}\right.---\left(10\right);$

根据式（9）-（10），确定出q₀,q₁,q₂,q₃的符号；确定公式如下：

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{32}-T_{23})\right)\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{13}-T_{31})\right)\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{21}-T_{12})\right)\end{array}\right.---\left(11\right);$

2.4）根据初始姿态角，确定出初始姿态矩阵；确定公式如下：

式（12）中：C₀为初始姿态矩阵；分别为初始航向角、初始俯仰角、初始横滚角；

2.5）根据旋转矢量三子样算法，求解出旋转矢量；求解公式如下：

$\mathrm{\Φ}\left(h\right)=\mathrm{\Δ}{\mathrm{\θ}}_1+\mathrm{\Δ}{\mathrm{\θ}}_2+\mathrm{\Δ}{\mathrm{\θ}}_3+\frac{9}{20}\mathrm{\Δ}{\mathrm{\θ}}_1\×\mathrm{\Δ}{\mathrm{\θ}}_2+\frac{27}{40}\mathrm{\Δ}{\mathrm{\θ}}_2\×(\mathrm{\Δ}{\mathrm{\θ}}_3-\mathrm{\Δ}{\mathrm{\θ}}_1)---\left(13\right);$

式（13）中：Φ(h)为h时间内的旋转矢量；Δθ₁、Δθ₂、Δθ₃分别为 $\[t_k+\frac{h}{3},t_k+\frac{2h}{3}\],\[t_k+\frac{2h}{3},t_k\]$ 三个时间段内的角增量；

2.6）对姿态四元数进行即时修正；修正公式如下：

$Q\left(t_{k+1}\right)=Q\left(t_k\right)\⊗q\left(h\right)---\left(14\right);$

式（14）中：Q(t_k+1)为修正后的姿态四元数；Q(t_k)为修正前的姿态四元数；q(h)为姿态变化四元数；

2.7）应用旋转矢量求解出姿态变化四元数；求解公式如下：

$q\left(h\right)=\cos \frac{\left|\mathrm{\Φ}\left(h\right)\right|}{2}+\frac{\mathrm{\Φ}\left(h\right)}{\left|\mathrm{\Φ}\left(h\right)\right|}\sin \frac{\left|\mathrm{\Φ}\left(h\right)\right|}{2}---\left(15\right);$

式（15）中：q(h)为姿态变化四元数；Φ(h)为h时间内的旋转矢量；

2.8）将式（15）代入式（14），展开如下：

$\left[\begin{array}{c}{q}_0\left(t_{k+1}\right)\\ q_1\left(t_{k+1}\right)\\ q_2\left(t_{k+1}\right)\\ q_3\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{cccc}\cos \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_2}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ}\mathrm{\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}\end{array}\right]\left[\begin{array}{c}{q}_0\left(t_k\right)\\ q_1\left(t_k\right)\\ q_2\left(t_k\right)\\ q_3\left(t_k\right)\end{array}\right]---\left(16\right);$

2.9）根据式（16），对姿态四元数进行归一化；归一化公式如下：

$q_i=\frac{{\hat{q}}_i}{\sqrt{{\hat{q}}_0^2+{\hat{q}}_1^2+{\hat{q}}_2^2+{\hat{q}}_3^2}}---\left(17\right);$

式（17）中：i＝0,1,2,3；q_i为归一化后的姿态四元数；为四元数更新所得值；

2.10）将式（17）代入式（8），计算出系到n系的姿态矩阵；计算公式如下：

$C_{\tilde{b}}^n=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(18\right);$

式（18）中：为系到n系的姿态矩阵；

2.11）根据式（18），确定出速度方程；确定公式如下：

$a_{\tilde{b}}^n=\frac{{\mathrm{dV}}^n}{\mathrm{dt}}=C_{\tilde{b}}^n{f}^{\tilde{b}}-2(w_{\mathrm{ie}}^n+w_{\mathrm{en}}^n)\×V^n+g^n---\left(19\right);$

式（19）中：为系相对n系的三维加速度；为系到n系的姿态矩阵；为系相对n系的三维比力信息；为地球自转角速度在n系的值；为n系相对地球坐标系旋转的角速度在n系的投影值；gⁿ为重力加速度在n系的投影值；

由于弹体射程短，使得w_ie≈0，w_en≈0；因此，式（19）表示为：

$a_{\tilde{b}}^n=\frac{d{V}^n}{\mathrm{dt}}=C_{\tilde{b}}^n{f}^{\tilde{b}}+g^n;$

2.12）根据式（19），计算出系相对n系的三维速度；计算公式如下：

$V_{\tilde{b}}^n=V_0^n+\∫(C_{\tilde{b}}^n{f}_i^{\tilde{b}}+g^n)\mathrm{dt}---\left(20\right);$

式（20）中：为系相对n系的三维速度；为系相对n系的初始三维速度；为系到n系的姿态矩阵；为系相对n系的三维比力；gⁿ为重力加速度在n系的投影值；

2.13）根据式（20），计算出系相对n系的三维位置；计算公式如下：

$S_{\tilde{b}}^n=S_0^n+\∫V_{\tilde{b}}^n\mathrm{dt}---\left(21\right);$

式（21）中：为系相对n系的三维位置；为系相对n系的初始三维位置；为系相对n系的三维速度；

2.14）采用欧拉角表示出系到n系的姿态矩阵；表示公式如下：

根据式（22），提取出系相对n系的三维姿态角；提取公式如下：

式（22）-（23）中：分别为系相对n系的航向角、系相对n系的俯仰角、系相对n系的横滚角；为系到n系的姿态矩阵。

本发明所述的适用于高速旋转飞行体的半捷联式惯性测量与导航算法首先通过半捷联式惯性测量系统测出系相对n系的部分运动信息（系相对n系的三维比力、三维角速率），然后利用旋转矢量三子样算法，解算得出系相对n系的其它运动信息（系相对n系的三维加速度、三维速度、三维位置、三维姿态角），最后根据高速旋转飞行体与半捷联式惯性测量系统的相对运动关系，解算得出b系相对n系的全部运动信息（b系相对n系的三维加速度、三维速度、三维位置、三维姿态角），由此实现了根据半捷联式惯性测量系统测出的运动信息准确解算出高速旋转飞行体的运动信息，从而有效解决了半捷联式惯性测量系统测得的运动信息无法准确反映高速旋转飞行体的运动信息的问题。

本发明有效解决了半捷联式惯性测量系统测得的运动信息无法准确反映高速旋转飞行体的运动信息的问题，适用于测量高速旋转飞行体的运动信息。

附图说明

图1是本发明的步骤2.14）的示意图。

具体实施方式

适用于高速旋转飞行体的半捷联式惯性测量与导航算法，该算法是采用如下步骤实现的：

1）假设高速旋转飞行体的发射坐标系为导航坐标系，简称为n系；假设高速旋转飞行体对应的坐标系为载体坐标系，简称为b系；假设半捷联式惯性测量系统对应的坐标系为测量坐标系，简称为系；

假设在高速旋转飞行体的发射时刻，系与b系的对应轴向完全一致；当高速旋转飞行体开始运动后，b系随高速旋转飞行体同步变化，系则由于半捷联平台的隔转止旋作用而不随高速旋转飞行体同步变化，但b系和系的横滚轴方向始终一致，且b系和系的横滚角之差为

通过半捷联式惯性测量系统中的三轴加速度计实时测出系相对n系的三维比力；通过半捷联式惯性测量系统中的三轴陀螺仪实时测出系相对n系的三维角速率、系与b系之间的横滚角之差；

2）根据系相对n系的三维比力、系相对n系的三维角速率，实时更新计算出系到n系的姿态矩阵、系相对n系的三维加速度、系相对n系的三维速度、系相对n系的三维位置、系相对n系的三维姿态角；

3）根据系相对n系的三维比力、系与b系之间的横滚角之差，求解出b系相对n系的三维比力；求解公式如下：

$\begin{array}{c}\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\\ 0& -\sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\end{array}\right]\left[\begin{array}{c}{f}_x^{\tilde{b}}\\ f_y^{\tilde{b}}\\ f_z^{\tilde{b}}\end{array}\right]\\ f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T\\ f^{\tilde{b}}={\left[\begin{array}{ccc}{f}_x^{\tilde{b}}& f_y^{\tilde{b}}& f_z^{\tilde{b}}\end{array}\right]}^T\end{array}---\left(1\right);$

式（1）中：f^b为b系相对n系的三维比力；为系相对n系的三维比力；为系与b系之间的横滚角之差；

4）根据系相对n系的三维角速率、系与b系之间的横滚角之差，求解出b系相对n系的三维角速率；求解公式如下：

$w_x^b=w_x^{\tilde{b}}+\stackrel{\·}{\mathrm{\Δ}}{\mathrm{\γ}}_{b\tilde{b}}$

$\begin{array}{c}\left[\begin{array}{c}{w}_y^b\\ w_z^b\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\\ -\sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\end{array}\right]\left[\begin{array}{c}{w}_y^{\tilde{b}}\\ w_z^{\tilde{b}}\end{array}\right]\\ w^b={\left[\begin{array}{ccc}{w}_x^b& w_y^b& w_z^b\end{array}\right]}^T\\ w^{\tilde{b}}={\left[\begin{array}{ccc}{w}_x^{\tilde{b}}& w_y^{\tilde{b}}& w_z^{\tilde{b}}\end{array}\right]}^T\end{array}---\left(2\right);$

式（2）中：w^b为b系相对n系的三维角速率；为系相对n系的三维角速率；为系与b系之间的横滚角之差；

5）根据系相对n系的三维加速度，求解出b系相对n系的三维加速度；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{a}_{\mathrm{bx}}^n=a_{\tilde{b}x}^n\\ a_{\mathrm{by}}^n=a_{\tilde{b}y}^n\\ a_{\mathrm{bz}}^n=a_{\tilde{b}z}^n\end{array}\right.\\ a_b^n={\left[\begin{array}{ccc}{a}_{\mathrm{bx}}^n& a_{\mathrm{by}}^n& a_{\mathrm{bz}}^n\end{array}\right]}^T\\ a_{\tilde{b}}^n={\left[\begin{array}{ccc}{a}_{\tilde{b}x}^n& a_{\tilde{b}y}^n& a_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(3\right);$

式（3）中：为b系相对n系的三维加速度；为系相对n系的三维加速度；

6）根据系相对n系的三维速度，求解出b系相对n系的三维速度；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{v}_{\mathrm{bx}}^n=v_{\tilde{b}x}^n\\ v_{\mathrm{by}}^n=v_{\tilde{b}y}^n\\ v_{\mathrm{bz}}^n=v_{\tilde{b}z}^n\end{array}\right.\\ v_b^n={\left[\begin{array}{ccc}{v}_{\mathrm{bx}}^n& v_{\mathrm{by}}^n& v_{\mathrm{bz}}^n\end{array}\right]}^T\\ v_{\tilde{b}}^n={\left[\begin{array}{ccc}{v}_{\tilde{b}x}^n& v_{\tilde{b}y}^n& v_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(4\right);$

式（4）中：为b系相对n系的三维速度；为系相对n系的三维速度；

7）根据系相对n系的三维位置，求解出b系相对n系的三维位置；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{S}_{\mathrm{bx}}^n=S_{\tilde{b}x}^n\\ S_{\mathrm{by}}^n=S_{\tilde{b}y}^n\\ S_{\mathrm{bz}}^n=S_{\tilde{b}z}^n\end{array}\right.\\ S_b^n={\left[\begin{array}{ccc}{S}_{\mathrm{bx}}^n& S_{\mathrm{by}}^n& S_{\mathrm{bz}}^n\end{array}\right]}^T\\ S_{\tilde{b}}^n={\left[\begin{array}{ccc}{S}_{\tilde{b}x}^n& S_{\tilde{b}y}^n& S_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(5\right);$

式（5）中：为b系相对n系的三维位置；为系相对n系的三维位置；

8）根据系相对n系的三维姿态角、系与b系之间的横滚角之差，求解出b系相对n系的三维姿态角；求解公式如下：

式（6）中：分别为b系相对n系的航向角、b系相对n系的俯仰角、b系相对n系的横滚角；分别为系相对n系的航向角、系相对n系的俯仰角、系相对n系的横滚角；为系与b系之间的横滚角之差。

所述步骤2）中，实时更新计算的步骤包括：

2.1）利用b系相对n系的三维姿态角，计算出b系到n系的姿态矩阵；计算公式如下：

式（7）中：为b系到n系的姿态矩阵；分别为b系相对n系的航向角、b系相对n系的俯仰角、b系相对n系的横滚角；

2.2）推导出用四元数表示的b系到n系的姿态矩阵；表示公式如下：

$C_b^n=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(8\right);$

式（8）中：为b系到n系的姿态矩阵；

2.3）利用初始对准的结果，确定出初始四元数；确定公式如下：

$\left\{\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}\\ \left|q_0\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\end{array}\right.---\left(9\right);$

$\left\{\begin{array}{c}{4q}_1{q}_0=T_{32}-T_{23}\\ {4q}_2{q}_0=T_{13}-T_{31}\\ {4q}_3{q}_0=T_{21}-T_{12}\end{array}\right.---\left(10\right);$

根据式（9）-（10），确定出q₀,q₁,q₂,q₃的符号；确定公式如下：

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{32}-T_{23})\right)\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{13}-T_{31})\right)\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{21}-T_{12})\right)\end{array}\right.---\left(11\right);$

2.4）根据初始姿态角，确定出初始姿态矩阵；确定公式如下：

式（12）中：C₀为初始姿态矩阵；分别为初始航向角、初始俯仰角、初始横滚角；

2.5）根据旋转矢量三子样算法，求解出旋转矢量；求解公式如下：

$\mathrm{\Φ}\left(h\right)=\mathrm{\Δ}{\mathrm{\θ}}_1+\mathrm{\Δ}{\mathrm{\θ}}_2+\mathrm{\Δ}{\mathrm{\θ}}_3+\frac{9}{20}\mathrm{\Δ}{\mathrm{\θ}}_1\×\mathrm{\Δ}{\mathrm{\θ}}_2+\frac{27}{40}\mathrm{\Δ}{\mathrm{\θ}}_2\×(\mathrm{\Δ}{\mathrm{\θ}}_3-\mathrm{\Δ}{\mathrm{\θ}}_1)---\left(13\right);$

式（13）中：Φ(h)为h时间内的旋转矢量；Δθ₁、Δθ₂、Δθ₃分别为 $\[t_k+\frac{h}{3},t_k+\frac{2h}{3}\],\[t_k+\frac{2h}{3},t_k\]$ 三个时间段内的角增量；

2.6）对姿态四元数进行即时修正；修正公式如下：

$Q\left(t_{k+1}\right)=Q\left(t_k\right)\⊗q\left(h\right)---\left(14\right);$

式（14）中：Q(t_k+1)为修正后的姿态四元数；Q(t_k)为修正前的姿态四元数；q(h)为姿态变化四元数；

2.7）应用旋转矢量求解出姿态变化四元数；求解公式如下：

$q\left(h\right)=\cos \frac{\left|\mathrm{\Φ}\left(h\right)\right|}{2}+\frac{\mathrm{\Φ}\left(h\right)}{\left|\mathrm{\Φ}\left(h\right)\right|}\sin \frac{\left|\mathrm{\Φ}\left(h\right)\right|}{2}---\left(15\right);$

式（15）中：q(h)为姿态变化四元数；Φ(h)为h时间内的旋转矢量；

2.8）将式（15）代入式（14），展开如下：

$\left[\begin{array}{c}{q}_0\left(t_{k+1}\right)\\ q_1\left(t_{k+1}\right)\\ q_2\left(t_{k+1}\right)\\ q_3\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{cccc}\cos \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_2}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ}\mathrm{\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}\end{array}\right]\left[\begin{array}{c}{q}_0\left(t_k\right)\\ q_1\left(t_k\right)\\ q_2\left(t_k\right)\\ q_3\left(t_k\right)\end{array}\right]---\left(16\right);$

2.9）根据式（16），对姿态四元数进行归一化；归一化公式如下：

$q_i=\frac{{\hat{q}}_i}{\sqrt{{\hat{q}}_0^2+{\hat{q}}_1^2+{\hat{q}}_2^2+{\hat{q}}_3^2}}---\left(17\right);$

式（17）中：i＝0,1,2,3；q_i为归一化后的姿态四元数；为四元数更新所得值；

2.10）将式（17）代入式（8），计算出系到n系的姿态矩阵；计算公式如下：

$C_{\tilde{b}}^n=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(18\right);$

式（18）中：为系到n系的姿态矩阵；

2.11）根据式（18），确定出速度方程；确定公式如下：

$a_{\tilde{b}}^n=\frac{d{V}^n}{\mathrm{dt}}=C_{\tilde{b}}^n{f}^{\tilde{b}}-2(w_{\mathrm{ie}}^n+w_{\mathrm{en}}^n)\×V^n+g^n---\left(19\right);$

式（19）中：为系相对n系的三维加速度；为系到n系的姿态矩阵；为系相对n系的三维比力信息；为地球自转角速度在n系的值；为n系相对地球坐标系旋转的角速度在n系的投影值；gⁿ为重力加速度在n系的投影值；

由于弹体射程短，使得w_ie≈0，w_en≈0；因此，式（19）表示为：

$a_{\tilde{b}}^n=\frac{d{V}^n}{\mathrm{dt}}=C_{\tilde{b}}^n{f}^{\tilde{b}}+g^n;$

2.12）根据式（19），计算出系相对n系的三维速度；计算公式如下：

$V_{\tilde{b}}^n=V_0^n+\∫(C_{\tilde{b}}^n{f}_i^{\tilde{b}}+g^n)\mathrm{dt}---\left(20\right);$

式（20）中：为系相对n系的三维速度；为系相对n系的初始三维速度；为系到n系的姿态矩阵；为系相对n系的三维比力；gⁿ为重力加速度在n系的投影值；

2.13）根据式（20），计算出系相对n系的三维位置；计算公式如下：

$S_{\tilde{b}}^n=S_0^n{+\∫V}_{\tilde{b}}^n\mathrm{dt}---\left(21\right);$

式（21）中：为系相对n系的三维位置；为系相对n系的初始三维位置；为系相对n系的三维速度；

2.14）采用欧拉角表示出系到n系的姿态矩阵；表示公式如下：

根据式（22），提取出系相对n系的三维姿态角；提取公式如下：

式（22）-（23）中：分别为系相对n系的航向角、系相对n系的俯仰角、系相对n系的横滚角；为系到n系的姿态矩阵。

Claims (2)

1.一种适用于高速旋转飞行体的半捷联式惯性测量与导航算法，其特征在于：该算法是采用如下步骤实现的：

1）假设高速旋转飞行体的发射坐标系为导航坐标系，简称为n系；假设高速旋转飞行体对应的坐标系为载体坐标系，简称为b系；假设半捷联式惯性测量系统对应的坐标系为测量坐标系，简称为系；

假设在高速旋转飞行体的发射时刻，系与b系的对应轴向完全一致；当高速旋转飞行体开始运动后，b系随高速旋转飞行体同步变化，系则由于半捷联平台的隔转止旋作用而不随高速旋转飞行体同步变化，但b系和系的横滚轴方向始终一致，且b系和系的横滚角之差为

通过半捷联式惯性测量系统中的三轴加速度计实时测出系相对n系的三维比力；通过半捷联式惯性测量系统中的三轴陀螺仪实时测出系相对n系的三维角速率、系与b系之间的横滚角之差；

2）根据系相对n系的三维比力、系相对n系的三维角速率，实时更新计算出系到n系的姿态矩阵、系相对n系的三维加速度、系相对n系的三维速度、系相对n系的三维位置、系相对n系的三维姿态角；

3）根据系相对n系的三维比力、系与b系之间的横滚角之差，求解出b系相对n系的三维比力；求解公式如下：

$\begin{array}{c}\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\\ 0& -\sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\end{array}\right]\left[\begin{array}{c}{f}_x^{\tilde{b}}\\ f_y^{\tilde{b}}\\ f_z^{\tilde{b}}\end{array}\right]\\ f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T\\ f^{\tilde{b}}={\left[\begin{array}{ccc}{f}_x^{\tilde{b}}& f_y^{\tilde{b}}& f_z^{\tilde{b}}\end{array}\right]}^T\end{array}---\left(1\right);$

式（1）中：f^b为b系相对n系的三维比力；为系相对n系的三维比力；为系与b系之间的横滚角之差；

4）根据系相对n系的三维角速率、系与b系之间的横滚角之差，求解出b系相对n系的三维角速率；求解公式如下：

$w_x^b=w_x^{\tilde{b}}+\stackrel{\·}{\mathrm{\Δ}}{\mathrm{\γ}}_{b\tilde{b}}$

$\begin{array}{c}\left[\begin{array}{c}{w}_y^b\\ w_z^b\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\\ -\sin \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}& \cos \mathrm{\Δ}{\mathrm{\γ}}_{b\tilde{b}}\end{array}\right]\left[\begin{array}{c}{w}_y^{\tilde{b}}\\ w_z^{\tilde{b}}\end{array}\right]\\ w^b={\left[\begin{array}{ccc}{w}_x^b& w_y^b& w_z^b\end{array}\right]}^T\\ w^{\tilde{b}}={\left[\begin{array}{ccc}{w}_x^{\tilde{b}}& w_y^{\tilde{b}}& w_z^{\tilde{b}}\end{array}\right]}^T\end{array}---\left(2\right);$

式（2）中：w^b为b系相对n系的三维角速率；为系相对n系的三维角速率；为系与b系之间的横滚角之差；

5）根据系相对n系的三维加速度，求解出b系相对n系的三维加速度；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{a}_{\mathrm{bx}}^n=a_{\tilde{b}x}^n\\ a_{\mathrm{by}}^n=a_{\tilde{b}y}^n\\ a_{\mathrm{bz}}^n=a_{\tilde{b}z}^n\end{array}\right.\\ a_b^n={\left[\begin{array}{ccc}{a}_{\mathrm{bx}}^n& a_{\mathrm{by}}^n& a_{\mathrm{bz}}^n\end{array}\right]}^T\\ a_{\tilde{b}}^n={\left[\begin{array}{ccc}{a}_{\tilde{b}x}^n& a_{\tilde{b}y}^n& a_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(3\right);$

式（3）中：为b系相对n系的三维加速度；为系相对n系的三维加速度；

6）根据系相对n系的三维速度，求解出b系相对n系的三维速度；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{v}_{\mathrm{bx}}^n=v_{\tilde{b}x}^n\\ v_{\mathrm{by}}^n=v_{\tilde{b}y}^n\\ v_{\mathrm{bz}}^n=v_{\tilde{b}z}^n\end{array}\right.\\ v_b^n={\left[\begin{array}{ccc}{v}_{\mathrm{bx}}^n& v_{\mathrm{by}}^n& v_{\mathrm{bz}}^n\end{array}\right]}^T\\ v_{\tilde{b}}^n={\left[\begin{array}{ccc}{v}_{\tilde{b}x}^n& v_{\tilde{b}y}^n& v_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(4\right);$

式（4）中：为b系相对n系的三维速度；为系相对n系的三维速度；

7）根据系相对n系的三维位置，求解出b系相对n系的三维位置；求解公式如下：

$\begin{array}{c}\left\{\begin{array}{c}{S}_{\mathrm{bx}}^n=S_{\tilde{b}x}^n\\ S_{\mathrm{by}}^n=S_{\tilde{b}y}^n\\ S_{\mathrm{bz}}^n=S_{\tilde{b}z}^n\end{array}\right.\\ S_b^n={\left[\begin{array}{ccc}{S}_{\mathrm{bx}}^n& S_{\mathrm{by}}^n& S_{\mathrm{bz}}^n\end{array}\right]}^T\\ S_{\tilde{b}}^n={\left[\begin{array}{ccc}{S}_{\tilde{b}x}^n& S_{\tilde{b}y}^n& S_{\tilde{b}z}^n\end{array}\right]}^T\end{array}---\left(5\right);$

式（5）中：为b系相对n系的三维位置；为系相对n系的三维位置；

8）根据系相对n系的三维姿态角、系与b系之间的横滚角之差，求解出b系相对n系的三维姿态角；求解公式如下：

式（6）中：分别为b系相对n系的航向角、b系相对n系的俯仰角、b系相对n系的横滚角；分别为系相对n系的航向角、系相对n系的俯仰角、系相对n系的横滚角；为系与b系之间的横滚角之差。

2.根据权利要求1所述的适用于高速旋转飞行体的半捷联式惯性测量与导航算法，其特征在于：所述步骤2）中，实时更新计算的步骤包括：

2.1）利用b系相对n系的三维姿态角，计算出b系到n系的姿态矩阵；计算公式如下：

式（7）中：为b系到n系的姿态矩阵；分别为b系相对n系的航向角、b系相对n系的俯仰角、b系相对n系的横滚角；

2.2）推导出用四元数表示的b系到n系的姿态矩阵；表示公式如下：

$C_b^n=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(8\right);$

式（8）中：为b系到n系的姿态矩阵；

2.3）利用初始对准的结果，确定出初始四元数；确定公式如下：

$\left\{\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}\\ \left|q_0\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\end{array}\right.---\left(9\right);$

$\left\{\begin{array}{c}{4q}_1{q}_0=T_{32}-T_{23}\\ {4q}_2{q}_0=T_{13}-T_{31}\\ {4q}_3{q}_0=T_{21}-T_{12}\end{array}\right.---\left(10\right);$

根据式（9）-（10），确定出q₀,q₁,q₂,q₃的符号；确定公式如下：

$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{32}-T_{23})\right)\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{13}-T_{31})\right)\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left(\mathrm{sign}(T_{21}-T_{12})\right)\end{array}\right.---\left(11\right);$

2.4）根据初始姿态角，确定出初始姿态矩阵；确定公式如下：

式（12）中：C₀为初始姿态矩阵；分别为初始航向角、初始俯仰角、初始横滚角；

2.5）根据旋转矢量三子样算法，求解出旋转矢量；求解公式如下：

$\mathrm{\Φ}\left(h\right)=\mathrm{\Δ}{\mathrm{\θ}}_1+\mathrm{\Δ}{\mathrm{\θ}}_2+\mathrm{\Δ}{\mathrm{\θ}}_3+\frac{9}{20}\mathrm{\Δ}{\mathrm{\θ}}_1\×\mathrm{\Δ}{\mathrm{\θ}}_2+\frac{27}{40}\mathrm{\Δ}{\mathrm{\θ}}_2\×(\mathrm{\Δ}{\mathrm{\θ}}_3-\mathrm{\Δ}{\mathrm{\θ}}_1)---\left(13\right);$

式（13）中：Φ(h)为h时间内的旋转矢量；Δθ₁、Δθ₂、Δθ₃分别为 $\[t_k+\frac{h}{3},t_k+\frac{2h}{3}\],\[t_k+\frac{2h}{3},t_k\]$ 三个时间段内的角增量；

2.6）对姿态四元数进行即时修正；修正公式如下：

$Q\left(t_{k+1}\right)=Q\left(t_k\right)\⊗q\left(h\right)---\left(14\right);$

式（14）中：Q(t_k+1)为修正后的姿态四元数；Q(t_k)为修正前的姿态四元数；q(h)为姿态变化四元数；

2.7）应用旋转矢量求解出姿态变化四元数；求解公式如下：

$q\left(h\right)=\cos \frac{\left|\mathrm{\Φ}\left(h\right)\right|}{2}+\frac{\mathrm{\Φ}\left(h\right)}{\left|\mathrm{\Φ}\left(h\right)\right|}\sin \frac{\left|\mathrm{\Φ}\left(h\right)\right|}{2}---\left(15\right);$

式（15）中：q(h)为姿态变化四元数；Φ(h)为h时间内的旋转矢量；

2.8）将式（15）代入式（14），展开如下：

$\left[\begin{array}{c}{q}_0\left(t_{k+1}\right)\\ q_1\left(t_{k+1}\right)\\ q_2\left(t_{k+1}\right)\\ q_3\left(t_{k+1}\right)\end{array}\right]=\left[\begin{array}{cccc}\cos \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_z}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}\\ \frac{\mathrm{\Δ}{\mathrm{\θ}}_2}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& \frac{\mathrm{\Δ}{\mathrm{\θ}}_y}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ\θ}}{2}& -\frac{\mathrm{\Δ}{\mathrm{\θ}}_x}{\mathrm{\Δ\θ}}\sin \frac{\mathrm{\Δ}\mathrm{\θ}}{2}& \cos \frac{\mathrm{\Δ\θ}}{2}\end{array}\right]\left[\begin{array}{c}{q}_0\left(t_k\right)\\ q_1\left(t_k\right)\\ q_2\left(t_k\right)\\ q_3\left(t_k\right)\end{array}\right]---\left(16\right);$

2.9）根据式（16），对姿态四元数进行归一化；归一化公式如下：

$q_i=\frac{{\hat{q}}_i}{\sqrt{{\hat{q}}_0^2+{\hat{q}}_1^2+{\hat{q}}_2^2+{\hat{q}}_3^2}}---\left(17\right);$

式（17）中：i＝0,1,2,3；q_i为归一化后的姿态四元数；为四元数更新所得值；

2.10）将式（17）代入式（8），计算出系到n系的姿态矩阵；计算公式如下：

$C_{\tilde{b}}^n=\left[\begin{array}{ccc}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(18\right);$

式（18）中：为系到n系的姿态矩阵；

2.11)根据式（18），确定出速度方程；确定公式如下： $a_{\tilde{b}}^n=\frac{d{V}^n}{\mathrm{dt}}=C_{\tilde{b}}^n{f}^{\tilde{b}}-2(w_{\mathrm{ie}}^n+w_{\mathrm{en}}^n)\×V^n+g^n---\left(19\right);$

式（19）中：为系相对n系的三维加速度；为系到n系的姿态矩阵；为系相对n系的三维比力信息；为地球自转角速度在n系的值；为n系相对地球坐标系旋转的角速度在n系的投影值；gⁿ为重力加速度在n系的投影值；

由于弹体射程短，使得w_ie≈0，w_en≈0；因此，式（19）表示为：

$a_{\tilde{b}}^n=\frac{d{V}^n}{\mathrm{dt}}=C_{\tilde{b}}^n{f}^{\tilde{b}}+g^n;$

2.12）根据式（19），计算出系相对n系的三维速度；计算公式如下：

$V_{\tilde{b}}^n=V_0^n+\∫(C_{\tilde{b}}^n{f}_i^{\tilde{b}}+g^n)\mathrm{dt}---\left(20\right);$

式（20）中：为系相对n系的三维速度；为系相对n系的初始三维速度；为系到n系的姿态矩阵；为系相对n系的三维比力；gⁿ为重力加速度在n系的投影值；

2.13）根据式（20），计算出系相对n系的三维位置；计算公式如下：

$S_{\tilde{b}}^n=S_0^n{+\∫V}_{\tilde{b}}^n\mathrm{dt}---\left(21\right);$

式（21）中：为系相对n系的三维位置；为系相对n系的初始三维位置；为系相对n系的三维速度；

2.14）采用欧拉角表示出系到n系的姿态矩阵；表示公式如下：

根据式（22），提取出系相对n系的三维姿态角；提取公式如下：

式（22）-（23）中：分别为系相对n系的航向角、系相对n系的俯仰角、系相对n系的横滚角；为系到n系的姿态矩阵。