CN102607562A - 基于载体飞行模态判别的微惯性参数自适应姿态确定方法 - Google Patents

Abstract

本发明公开了一种基于载体飞行模态判别的微惯性参数自适应姿态确定方法，利用传感器输出数据，计算传感器的长周期和短周期特性参数，根据这些参数的变化及范围判断载体所处的运动模态，再根据载体不同运动模态下的微型姿态参考系统的误差变化特性，设计卡尔曼滤波参数的自适应调整策略；再通过卡尔曼滤波器中姿态观测残序列平方和的变化，在线差评估当前参数的滤波效果优劣，以参数置信度的形式反馈调节卡尔曼滤波器的滤波参数。本发明简化了传统的微惯性姿态确定方法中判断阀值选取和设置，避免了载体飞行高度和传感器漂移误差对判断条件的影响，全面考虑微型姿态参考系统的动静态运行特点，有效提高微型姿态参考系统的动态适应性和静态稳定性。

Description

基于载体飞行模态判别的微惯性参数自适应姿态确定方法

技术领域

本发明属于捷联惯性导航与姿态测量技术领域，特别是一种基于载体飞行模态判别的微惯性参数自适应姿态确定方法。

背景技术

微型姿态参考系统是采用MEMS(Micro Electro-Mechanical System)惯性传感器(微陀螺、微加速度计)作为惯性测量元件的惯性导航系统，采用MEMS技术制造的硅微陀螺仪和加速度计具有体积小、重量轻、成本低等特点，是目前微型姿态参考系统中应用的主要惯性测量元件。微型姿态参考系统，在民用方面主要应用于航空摄影、灾情监测、救灾搜寻、小型飞机导航等为应用方向的小型无人飞机和小型私人飞机，满足其控制和显示的姿态测量需求；同时，微型姿态参考系统还广发应用于现代战机的内嵌在数字仪表中，可在突发情况下为战斗机的安全飞行提供必要的姿态信息。

由于MEMS陀螺仪精度一般较低，在纯惯导解算模式，其输出姿态会因时间的推移发生漂移。因此，微型姿态参考系统需要通过内部加速度计及磁传感器数据计算姿态角作为观测量，通过卡尔曼滤波器实时估计捷联姿态解算误差，并实时修正误差，这就是传统微型姿态参考系统的姿态算法的主要思想。但由于载体加速、旋转、振动所引入的有害加速度对姿态观测量精度的影响，导致微型姿态参考系统输出的姿态角误差在载体机动状态不同时姿态输出精度波动较大。传统的微型姿态参考系统姿态组合算法通过加速度计输出和设定的门限值直接比较，作为判断是否进入卡尔曼姿态组合算法的判断条件，用来隔离或减小动态情况下有害加速度引入所带来的姿态误修正。但由于载体机动模态多变，机动动态范围宽、地球表面重力场的不均匀分布、加速度传感器温度漂移和加速度传感器零偏移等因素的影响，导致这样的门限值一般较难选取，如姿态组合进入门限值设置偏大，载体机动时也能进入姿态组合算法，导致姿态误差增大，使得微型姿态参考系统的动态误差增大；如姿态组合进入门限值设置偏小，会导致微型姿态参考系统在载体平稳飞行状态下长时间无法进入内部姿态组合，微型姿态参考系统长时间工作在纯捷联状态，姿态角误差随运行时间逐渐增大，最终使得微型姿态参考系统的输出姿态发散。

发明内容

本发明的目的在于提供一种基于载体飞行模态判别的微惯性参数自适应姿态确定方法，通过判别载体的机动状态，根据载体不同机动状态下的姿态角误差特性，自动调整卡尔曼滤波算法的修正力度，在线差评估当前参数的滤波效果优劣，以参数置信度的形式反馈调节卡尔曼滤波器的滤波参数，解决传统姿态确定方法在载体不同机动模态下的参数选取和姿态输出精度不统一的问题，提高载体在各种机动模态下的微型姿态参考系统输出的姿态角精度和稳定性。

实现本发明目的的技术解决方案为：一种基于载体飞行模态判别的微惯性参数自适应姿态确定方法，步骤如下：

步骤一，传感器数据采集：通过AD转换器或传感器数字接口直接采集MEMS传感器输出信号，获得载体k时刻绕X轴向的角速度载体绕Y轴向的角速度

载体绕Z轴向的角速度

载体在X轴向的比力值

载体在Y轴向的比力值

载体在Z轴向的比力值

地磁场在地理系下X轴向上的分量

Y轴向上的分量

Z轴向上的分量

步骤二，计算k时刻的捷联姿态：利用k时刻采集到的角速度信息，按四元数姿态解算流程，获得载体姿态角；系统时间t，t＝kΔt，Δt时间内载体转动过得角度Δθ的三个分量依次为横滚角增量Δθ_x，俯仰角增量Δθ_y，方位角增量Δθ_z，则转动的角度以矩阵的方式表示为：

$\mathrm{\Δ\θ}=\left[\begin{array}{ccc}0& -{\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z& 0& {-\mathrm{\Δ\θ}}_x\\ -{\mathrm{\Δ\θ}}_y& {\mathrm{\Δ\θ}}_x& 0\end{array}\right]=\mathrm{\Δt}\left[\begin{array}{ccc}0& -{\mathrm{Wibb}}_z& {\mathrm{Wibb}}_y\\ {\mathrm{Wibb}}_z& 0& {-\mathrm{Wibb}}_x\\ -{\mathrm{Wibb}}_y& {\mathrm{Wibb}}_x& 0\end{array}\right],$

t-Δt时刻的姿态矩阵为：

$C_n^b(t-\mathrm{\Δt})=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right],$

所对应的四元数姿态阵为

$Q\left(t\right)=\left[\begin{array}{c}\cos (\mathrm{\ψ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\γ}/2)+\sin (\mathrm{\ψ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\γ}/2)\\ \cos (\mathrm{\ψ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\γ}/2)+\sin (\mathrm{\ψ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\γ}/2)\\ \cos (\mathrm{\ψ}/2)\cos (\mathrm{\θ}/2)\sin (\mathrm{\γ}/2)-\sin (\mathrm{\ψ}/2)\sin (\mathrm{\θ}/2)\cos (\mathrm{\γ}/2)\\ \cos (\mathrm{\ψ}/2)\sin (\mathrm{\θ}/2)\sin (\mathrm{\γ}/2)-\sin (\mathrm{\ψ}/2)\cos (\mathrm{\θ}/2)\cos (\mathrm{\γ}/2)\end{array}\right],$

角增量模 $\left|\mathrm{\Δ\θ}\right|=\sqrt{{\mathrm{\Δ\θ}}_x^2+{\mathrm{\Δ\θ}}_y^2+{\mathrm{\Δ\θ}}_z^2},$ $\left[\mathrm{\Δ\θ}\right]=\left[\begin{array}{cccc}0& {-\mathrm{\Δ\θ}}_x& -{\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z\\ {\mathrm{\Δ\θ}}_x& 0& {\mathrm{\Δ\θ}}_z& -{\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_y& {-\mathrm{\Δ\θ}}_z& 0& {\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_x& 0\end{array}\right],$

利用一阶近似计算方法更新后的四元数矩阵 $Q\left(t\right)=(\cos \left(\frac{\left|\mathrm{\Δ\θ}\right|}{2}\right)I+\frac{1}{2}[\mathrm{\Δ\θ}\left]\right)Q={\left[\begin{array}{cccc}{Q}_0& Q_1& Q_2& Q_3\end{array}\right]}^T,$

得到更新后的姿态矩阵：

$C_n^b\left(t\right)=\left[\begin{array}{ccc}{Q_1}^2+{Q_0}^2-{Q_3}^2-{Q_2}^2& {2Q}_1{Q}_2+Q_0{Q}_3& {2Q}_1{Q}_3-Q_0{Q}_2\\ {2Q}_1{Q}_2-Q_0{Q}_3& {Q_2}^2-{Q_3}^2+{Q_0}^2-{Q_1}^2& {2Q}_2{Q}_3+Q_0{Q}_1\\ {2Q}_1{Q}_3+Q_0{Q}_2& {2Q}_2{Q}_3-Q_0{Q}_1& {Q_3}^2-{Q_2}^2-{Q_1}^2+{Q_0}^2\end{array}\right],$

将

简写为 $\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right],$ 则可计算出t时刻捷联惯导的俯仰角θ、横滚角γ和航向角ψ姿态角信息，θ＝sin^-1(T₂₃)， $\mathrm{\γ}={\mathrm{tg}}^{-1}\left(\frac{T_{13}}{T_{33}}\right),$ $\mathrm{\ψ}={\mathrm{tg}}^{-1}\left(\frac{T_{21}}{T_{32}}\right);$

步骤三，传感器参数预处理：

(1)微型姿态参考系统运行过程中，如k＞m₁，且微型姿态参考系统满足平稳条件，计算k时刻当地加速度的矢量模用Fb^k表示，

与前m₁个满足条件的平稳条件的加速度矢量模值求均值，用

表示， ${\mathrm{Fb}}_{m_1+1}^k=({\mathrm{Fb}}^{k-m_1}+{\mathrm{Fb}}^{k-m_1+1}+L+{\mathrm{Fb}}^k)/(m_1+1),$ m₁是微型姿态参考系统数秒内总采样点数；

(2)每隔m₂分钟计算一次飞机平稳状态下测得m₃个点的加速度矢量模的均值，并对所有加速度矢量模的稳态数据求均值，用

表示， ${\mathrm{Fb}}_{m_2\min }^k=({\mathrm{Fb}}_{m_3}^1+{\mathrm{Fb}}_{m_3}^2+L+{\mathrm{Fb}}_{m_3}^{m_3})/m_3,$ 作为当前载体状态判断的加速度矢量模的基准量；

(3)计算k时刻载体比力矢量模m₄个采样点的短时均值，用

表示， ${\mathrm{Fb}}_{m_4}^k=({\mathrm{Fb}}^{k-m_4+1}+{\mathrm{Fb}}^{k-m_4+2}+L+{\mathrm{Fb}}^k)/m_4,$ m₄个采样点的均方差

$\mathrm{std}\_{\mathrm{Fb}}_{m_4}^k=\sqrt{{({\mathrm{Fb}}^{k-m_4+1}-{\mathrm{Fb}}_{m_4}^k)}^2+{({\mathrm{Fb}}^{k-m_4+2}-{\mathrm{Fb}}_{m_4}^k)}^2+L+{({\mathrm{Fb}}^k-{\mathrm{Fb}}_{m_4}^k)}^2}/m_4;$

(4)计算k时刻载体的角速率矢量模，用Wibb^k表示， ${\mathrm{Wibb}}^k=\sqrt{{\mathrm{Wibb}}_x^{k^2}+{\mathrm{Wibb}}_y^{k^2}+{\mathrm{Wibb}}_z^{k^2}},$ m₄个采样点的载体角速率矢量模短时均值，用

表示， ${\mathrm{Wibb}}_{m_4}^k=({\mathrm{Wibb}}^{k-m_4+1}+{\mathrm{Wibb}}^{k-m_4+2}+L+{\mathrm{Wibb}}^k)/m_4,$ 和载体角速率矢量模m₄个采样点的均方差，用

表示， $\mathrm{std}\_{\mathrm{Wibb}}_{m_4}^k=\sqrt{{({\mathrm{Wibb}}^{k-m_4+1}-{\mathrm{Wibb}}_{m_4}^k)}^2+L+{({\mathrm{Wibb}}^k-{\mathrm{Wibb}}_{m_4}^k)}^2}/m_4;$

(5)计算k时刻地磁矢量的X轴分量m₄个采样点均值，用

表示， $E_{x/m_4}^k=(E_x^{k-m_4+1}+E_x^{k-m_4+2}+L+E_x^k)/m_4,$ X轴分量m₄个采样点均方差，用表示， $\mathrm{std}\_E_{x/m_4}^k=\sqrt{{(E_x^{k-m_4+1}-E_{x/m_4}^k)}^2+L+{(E_x^k-E_{x/m_4}^k)}^2}/m_4;$ 计算k时刻地磁矢量的Y轴分量m₄个采样点均值，用表示， $E_{y/m_4}^k=(E_y^{k-m_4+1}+E_y^{k-m_4+2}+L+E_y^k)/m_4,$ Y轴分量m₄个采样点均方差，用

表示， $\mathrm{std}\_E_{y/m_4}^k=\sqrt{{(E_y^{k-m_4+1}-E_{y/m_4}^k)}^2+L+{(E_y^k-E_{y/m_4}^k)}^2}/m_4;$ 计算k时刻地磁矢量的Z轴分量m₄个采样点均值，用

表示， $E_{z/m_4}^k=(E_z^{k-m_4+1}+E_z^{k-m_4+2}+L+E_z^k)/m_4,$ Z轴分量m₄个采样点均方差，用

表示， $\mathrm{std}\_E_{z/m_4}^k=\sqrt{{(E_z^{k-m_4+1}-E_{z/m_4}^k)}^2+L+{(E_z^k-E_{z/m_4}^k)}^2}/m_4,$ 根据

输出区间范围利用设计的飞行模态分类器，将载体机动级别Status分为平稳飞行Low、低机动Middle、高机动High三个级别；然后通过

将平稳飞行段分为平飞和振动状态；再引入磁传感器波动信息量：将低机动和高机动分别分为小加速、低滚转和大加速、高速滚转状态；

步骤四，飞行模态划分及观测噪声矩阵调节量计算：在步骤三获得传感器预处理信号和载体机动模态特征分类后，根据传感器的实时输出，选取观测噪声调整策略和调整力度，即对于加速类机动是根据载体加速机动幅度大小，计算加速情况下观测噪声影响因子，用α^k表示； ${\mathrm{\&alpha;}}^k=\left\{\begin{array}{c}1(\mathrm{Status}=\mathrm{Low})\\ a_0\frac{|{\mathrm{Fb}}_{m_4}^k-{\mathrm{Fb}}_{m_2\min }^k|}{{\mathrm{Fb}}_{5\min }^k}(\mathrm{Status}=\mathrm{Middle})(1<a_0<2)\\ a_1\frac{|{\mathrm{Fb}}_{m_4}^k-{\mathrm{Fb}}_{m_2\min }^k|}{{\mathrm{Fb}}_{m_2\min }^k}(\mathrm{Status}=\mathrm{High})(2<a_1<10)\end{array}\right\};$ 对于转动类机动根据载体转动角速率大小计算转动情况下的观测噪声影响因子，用γ^k表示， ${\mathrm{\&gamma;}}^k=\left\{\begin{array}{c}1(\mathrm{Status}=\mathrm{Low})\\ b_0{\mathrm{Wibb}}_{m_4}^k(\mathrm{Status}=\mathrm{Middle})(1<b_0<2)\\ b_1{\mathrm{Wibb}}_{m_4}^k(\mathrm{Status}=\mathrm{High})(2<b_1<10)\end{array}\right\};$ 减小观测噪声量影响因子计算平稳飞行观测噪声影响因子，用v^k表示， $v^k=c_0{\mathrm{Wibb}}_{m_4}^k/\mathrm{std}\_{\mathrm{Fb}}_{m_4}^k(1<c_0<2);$ 在获得观测噪声影响因子α^k、γ^k、v^k之后利用前一时刻的滤波观测噪声置信度因子C_t-Δt，计算基于载体模态判别的观测噪声矩阵R_t-Δt，R_t-Δt＝C_t-Δt(R⁰+α^kγ^kv^kQ⁰)；

步骤五，观测噪声矩阵的在线自适应调整：在步骤四获得基于载体模态判别的观测噪声矩阵R_t-Δt，利用此观测量，进行卡尔曼滤波计算，根据状态估计的一步预测值计算观测值的一步预测值，用

表示，计算公式为

再求出观测残差，用rt表示，

然后计算当前观测残差序列的平方和再通过得到的

计算当前时刻的滤波观测置信度因子，用C_t表示， $C_t=\frac{\mathrm{\&beta;Tr}\left[R_t{H}_t{P}_t^T\right]}{r_t^T{r}_t},0<C_t\&le;1,\mathrm{\&beta;}\&GreaterEqual;1,$ 然后利用更新的滤波观测噪声量信度因子C_t，计算更新的滤波观测噪声量矩阵，用R_t表示，R_t＝C_t(R⁰+α^kγ^kv^kQ⁰)；

步骤六，微惯性捷联姿态组合：通过比力及地磁矢量计算出载体姿态角的观测量，与步骤二计算的捷联姿态作差，作为卡尔曼滤波器的观测量，送入卡尔曼滤波器，估计捷联姿态解算的姿态误差，再用此误差角修正捷联姿态解算结果，提高微惯性捷联姿态确定方法的精度和稳定性。

本发明与现有技术相比，其显著优点：(1)本发明充分利用了传感器的长周期和短周期预处理数据，简化了传统的微惯性姿态确定方法中判断阀值选取和设置，避免了载体飞行高度和传感器漂移误差对判断条件的影响。(2)本发明对加速度计姿态估计误差进行了区分，分为加速误差、旋转滞后误差、振动误差，根据不同误差采用不同观测噪声调整策略，同时基于观测残差序列期望为零和滤波稳定条件，引入了滤波效果在线评估，通过调整置信度的方式对各模式观测噪声量进行了反馈修正，从而降低了调整系数初值设置精度要求，提高了多模态下的微惯性姿态确定方法的精度和稳定性。(3)本发明相对传统的微惯性姿态确定方法，由于其参数初值直接与传感器输出精度相关，实际运行时本发明会根据传感器输出，在线修改滤波进入门限值，自适应调整滤波参数，简化了参数获取过程，降低了姿态精度对姿态确定方法滤波参数初值的依赖，适应了微型姿态参考系统的工程化、产品化及批量生产中的标准化的需求。从方法上增强了系统对运行环境的适应性，增强了系统整体可靠性。(4)全面考虑了微型姿态参考系统的动静态运行特点，有效提高了微型姿态参考系统的动态适应性和静态稳定性。

下面结合附图对本发明作进一步详细描述。

附图说明

图1是本发明基于模态判别的微惯性参数自适应姿态确定方法的构成图。

图2是观测噪声矩阵自适应调整框图。

图3是实施例中传感器数据预处理获得参数及载体飞行模态判别整体框图。

图4是传统方法姿态输出与转台摆动对比曲线和姿态角误差曲线图。

图5是本发明姿态输出与转台摆动对比曲线和姿态角误差曲线图。

具体实施方式

微型姿态参考系统的传感器直接固定到飞行载体上，其传感器输出数据包含了载体机动状态信息，本发明充分利用了传感器原始数据的预处理提取的长周期和短周期参数，通过对这些参数的分类辨识，可以将载体机动状态划分到平稳飞行、低机动、和高机动三个等级。在微惯性航姿系统中，捷联姿态解算的精度直接跟MEMS陀螺仪精度和导航时间有关；而通过加速度计计算的姿态角误差是随载体机动大小和机动状态变化的，但不随导航时间增加。微惯性姿态确定方法就是利用加速度、磁矢量信息计算的姿态角作为观测量实时估计捷联姿态解算的姿态角误差，再对捷联姿态解算结果进行修正的算法。其中，误差估计的精度直接影响到微型姿态参考系统的姿态角输出精度和稳定性。由于该方法中卡尔曼滤波器的估计精度与观测噪声量和系统噪声量的大小设置有关；在载体平稳飞行的情况下，加速度计与地磁计算的姿态具有很高的精度且不随时间发散，所以载体平稳飞行时卡尔曼滤波器的观测噪声量较小，在载体做机动时，由于有害加速度的增加，加速度与地磁计算的姿态角误差增大。因此，本发明在辨别载体的机动状态，评估加速度计计算姿态和捷联姿态解算的误差特性的基础上，设计了基于模式飞行模态判别的观测噪声的优化调整策略。并利用测得残差平方和的大小变化实时在线评估微惯性姿态确定方法的计算精度，以置信度参数调整的形式反馈调整修正力度，实现了基于载体飞行模态判别的微惯性参数自适应姿态确定方法。其中，卡尔曼滤波器的参数自适应流程模块框图如图2所示。从而实现了对微惯性姿态确定方法中卡尔曼滤波系数的自适应调整，提高了姿态滤波精度和稳定性。

本发明基于载体飞行模态判别的微惯性参数自适应姿态确定方法的实现步骤如下：

步骤一、传感器数据采集：通过AD转换器或传感器数字接口直接采集MEMS传感器输出信号，获得载体k时刻绕X轴向的角速度

载体绕Y轴向的角速度

载体绕Z轴向的角速度

载体在X轴向的比力值

载体在Y轴向的比力值

载体在Z轴向的比力值

地磁场在地理系下X轴向上的分量Y轴向上的分量

Z轴向上的分量

步骤二、计算k时刻的捷联姿态：利用k时刻采集到的角速度信息，按四元数姿态解算流程，获得载体姿态角；系统时间t，t＝kΔt，Δt时间内载体转动过得角度Δθ的三个分量依次为横滚角增量Δθ_x，俯仰角增量Δθ_y，方位角增量Δθ_z，则转动的角度以矩阵的方式表示为： $\mathrm{\&Delta;\&theta;}=\left[\begin{array}{ccc}0& -{\mathrm{\&Delta;\&theta;}}_z& {\mathrm{\&Delta;\&theta;}}_y\\ {\mathrm{\&Delta;\&theta;}}_z& 0& {-\mathrm{\&Delta;\&theta;}}_x\\ -{\mathrm{\&Delta;\&theta;}}_y& {\mathrm{\&Delta;\&theta;}}_x& 0\end{array}\right]=\mathrm{\&Delta;t}\left[\begin{array}{ccc}0& -{\mathrm{Wibb}}_z& {\mathrm{Wibb}}_y\\ {\mathrm{Wibb}}_z& 0& {-\mathrm{Wibb}}_x\\ -{\mathrm{Wibb}}_y& {\mathrm{Wibb}}_x& 0\end{array}\right],$

t-Δt时刻的姿态矩阵为：

$C_n^b(t-\mathrm{\&Delta;t})=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ \cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right],$

所对应的四元数姿态阵为

$Q\left(t\right)=\left[\begin{array}{c}\cos (\mathrm{\&psi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&gamma;}/2)+\sin (\mathrm{\&psi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&gamma;}/2)\\ \cos (\mathrm{\&psi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&gamma;}/2)+\sin (\mathrm{\&psi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&gamma;}/2)\\ \cos (\mathrm{\&psi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&gamma;}/2)-\sin (\mathrm{\&psi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&gamma;}/2)\\ \cos (\mathrm{\&psi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&gamma;}/2)-\sin (\mathrm{\&psi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&gamma;}/2)\end{array}\right],$

角增量模 $\left|\mathrm{\&Delta;\&theta;}\right|=\sqrt{{\mathrm{\&Delta;\&theta;}}_x^2+{\mathrm{\&Delta;\&theta;}}_y^2+{\mathrm{\&Delta;\&theta;}}_z^2},$ $\left[\mathrm{\&Delta;\&theta;}\right]=\left[\begin{array}{cccc}0& {-\mathrm{\&Delta;\&theta;}}_x& -{\mathrm{\&Delta;\&theta;}}_y& -{\mathrm{\&Delta;\&theta;}}_z\\ {\mathrm{\&Delta;\&theta;}}_x& 0& {\mathrm{\&Delta;\&theta;}}_z& -{\mathrm{\&Delta;\&theta;}}_y\\ {\mathrm{\&Delta;\&theta;}}_y& {-\mathrm{\&Delta;\&theta;}}_z& 0& {\mathrm{\&Delta;\&theta;}}_x\\ {\mathrm{\&Delta;\&theta;}}_z& {\mathrm{\&Delta;\&theta;}}_y& -{\mathrm{\&Delta;\&theta;}}_x& 0\end{array}\right],$

利用一阶近似计算方法更新后的四元数矩阵 $Q\left(t\right)=(\cos \left(\frac{\left|\mathrm{\&Delta;\&theta;}\right|}{2}\right)I+\frac{1}{2}[\mathrm{\&Delta;\&theta;}\left]\right)Q={\left[\begin{array}{cccc}{Q}_0& Q_1& Q_2& Q_3\end{array}\right]}^T,$

得到更新后的姿态矩阵：

$C_n^b\left(t\right)=\left[\begin{array}{ccc}{Q_1}^2+{Q_0}^2-{Q_3}^2-{Q_2}^2& {2Q}_1{Q}_2+Q_0{Q}_3& {2Q}_1{Q}_3-Q_0{Q}_2\\ {2Q}_1{Q}_2-Q_0{Q}_3& {Q_2}^2-{Q_3}^2+{Q_0}^2-{Q_1}^2& {2Q}_2{Q}_3+Q_0{Q}_1\\ {2Q}_1{Q}_3+Q_0{Q}_2& {2Q}_2{Q}_3-Q_0{Q}_1& {Q_3}^2-{Q_2}^2-{Q_1}^2+{Q_0}^2\end{array}\right],$ 将

简写为 $\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right],$ 则可计算出t时刻捷联惯导的俯仰角θ、横滚角γ和航向角ψ等姿态角信息，θ＝sin^-1(T₂₃)， $\mathrm{\&gamma;}={\mathrm{tg}}^{-1}\left(\frac{T_{13}}{T_{33}}\right),$ $\mathrm{\&psi;}={\mathrm{tg}}^{-1}\left(\frac{T_{21}}{T_{32}}\right).$

步骤三、传感器参数预处理(下面五个方面的内容不分先后进行处理)：

(1)微型姿态参考系统运行过程中，如k＞m₁，且微型姿态参考系统满足平稳条件，计算k时刻当地加速度的矢量模用Fb^k表示，

与前m₁个满足条件的平稳条件的加速度矢量模值求均值，用

表示， ${\mathrm{Fb}}_{m_1+1}^k=({\mathrm{Fb}}^{k-m_1}+{\mathrm{Fb}}^{k-m_1+1}+L+{\mathrm{Fb}}^k)/(m_1+1),$ m₁是微型姿态参考系统数秒内总采样点数；

(2)每隔m₂分钟计算一次飞机平稳状态下测得m₃个点的加速度矢量模的均值，并对所有加速度矢量模的稳态数据求均值，用表示， ${\mathrm{Fb}}_{m_2\min }^k=({\mathrm{Fb}}_{m_3}^1+{\mathrm{Fb}}_{m_3}^2+L+{\mathrm{Fb}}_{m_3}^{m_3})/m_3,$ 作为当前载体状态判断的加速度矢量模的基准量(m₂取值可以是5-10分钟，m₃≤100)；

(3)计算k时刻载体比力矢量模m₄个采样点的短时均值，用

表示， ${\mathrm{Fb}}_{m_4}^k=({\mathrm{Fb}}^{k-m_4+1}+{\mathrm{Fb}}^{k-m_4+2}+L+{\mathrm{Fb}}^k)/m_4,$ m₄个采样点的均方差

$\mathrm{std}\_{\mathrm{Fb}}_{m_4}^k=\sqrt{{({\mathrm{Fb}}^{k-m_4+1}-{\mathrm{Fb}}_{m_4}^k)}^2+{({\mathrm{Fb}}^{k-m_4+2}-{\mathrm{Fb}}_{m_4}^k)}^2+L+{({\mathrm{Fb}}^k-{\mathrm{Fb}}_{m_4}^k)}^2}/m_4,$ m₄取值为0.1-0.5秒内的总采样数；

(4)计算k时刻载体的角速率矢量模，用Wibb^k表示， ${\mathrm{Wibb}}^k=\sqrt{{\mathrm{Wibb}}_x^{k^2}+{\mathrm{Wibb}}_y^{k^2}+{\mathrm{Wibb}}_z^{k^2}},$ m₄个采样点的载体角速率矢量模短时均值，用

表示， ${\mathrm{Wibb}}_{m_4}^k=({\mathrm{Wibb}}^{k-m_4+1}+{\mathrm{Wibb}}^{k-m_4+2}+L+{\mathrm{Wibb}}^k)/m_4,$ 和载体角速率矢量模m₄个采样点的均方差，用

表示， $\mathrm{std}\_{\mathrm{Wibb}}_{m_4}^k=\sqrt{{({\mathrm{Wibb}}^{k-m_4+1}-{\mathrm{Wibb}}_{m_4}^k)}^2+L+{({\mathrm{Wibb}}^k-{\mathrm{Wibb}}_{m_4}^k)}^2}/m_4;$

(5)计算k时刻地磁矢量的X轴分量m₄个采样点均值，用

表示， $E_{x/m_4}^k=(E_x^{k-m_4+1}+E_x^{k-m_4+2}+L+E_x^k)/m_4,$ X轴分量m₄个采样点均方差，用

表示， $\mathrm{std}\_E_{x/m_4}^k=\sqrt{{(E_x^{k-m_4+1}-E_{x/m_4}^k)}^2+L+{(E_x^k-E_{x/m_4}^k)}^2}/m_4;$ 计算k时刻地磁矢量的Y轴分量m₄个采样点均值，用

表示， $E_{y/m_4}^k=(E_y^{k-m_4+1}+E_y^{k-m_4+2}+L+E_y^k)/m_4,$ Y轴分量m₄个采样点均方差，用

表示， $\mathrm{std}\_E_{y/m_4}^k=\sqrt{{(E_y^{k-m_4+1}-E_{y/m_4}^k)}^2+L+{(E_y^k-E_{y/m_4}^k)}^2}/m_4;$ 计算k时刻地磁矢量的Z轴分量m₄个采样点均值，用

表示， $E_{z/m_4}^k=(E_z^{k-m_4+1}+E_z^{k-m_4+2}+L+E_z^k)/m_4,$ Z轴分量m₄个采样点均方差，用

表示， $\mathrm{std}\_E_{z/m_4}^k=\sqrt{{(E_z^{k-m_4+1}-E_{z/m_4}^k)}^2+L+{(E_z^k-E_{z/m_4}^k)}^2}/m_4,$ 根据

输出区间范围利用设计的飞行模态分类器，将载体机动级别Status分为平稳飞行Low、低机动Middle、高机动High三个级别；然后通过

将平稳飞行段分为平飞和振动状态；再引入磁传感器波动信息量：

将低机动和高机动分别分为小加速、低滚转和大加速、高速滚转状态(飞行模态分类器等内容属于现代模式识别中的分类器技术，可以参考模式识别相关文献)；

如图3所示，下面以m₁＝99，m₂＝5，m₃＝100，m₄＝5为例进行说明传感器参数的预处理过程。微型姿态参考系统运行过程中，如k＞99，且微型姿态参考系统满足平稳条件(静止或无加速平飞)，计算k时刻当地加速度的矢量模用Fb^k表示，

与前99个满足条件的平稳条件的加速度矢量模值求均值，用

表示， ${\mathrm{Fb}}_{100}^k=({\mathrm{Fb}}^{k-99}+{\mathrm{Fb}}^{k-98}+L+{\mathrm{Fb}}^k)/100.$

每隔五分钟计算一次飞机平稳状态下测得的加速度矢量模的100个点均值，并对所有加速度矢量模的稳态数据求均值，用

表示， ${\mathrm{Fb}}_{5\min }^k=({\mathrm{Fb}}_{100}^1+{\mathrm{Fb}}_{100}^2+L+{\mathrm{Fb}}_{100}^n)/n,$ 作为当前载体状态判断的加速度矢量模的基准量，其中n为参与求平均的数据总数。

计算k时刻载体比力矢量模5个采样点的短时均值，用表示， ${\mathrm{Fb}}_5^k=({\mathrm{Fb}}^{k-4}+{\mathrm{Fb}}^{k-3}+L+{\mathrm{Fb}}^k)/5,$ 5个采样点的均方差

$\mathrm{std}\_{\mathrm{Fb}}_5^k=\sqrt{{({\mathrm{Fb}}^{k-4}-{\mathrm{Fb}}_5^k)}^2+{({\mathrm{Fb}}^{k-3}-{\mathrm{Fb}}_5^k)}^2+L+{({\mathrm{Fb}}^k-{\mathrm{Fb}}_5^k)}^2}/5.$

计算k时刻载体的角速率矢量模，用Wibb^k表示， ${\mathrm{Wibb}}^k=\sqrt{{\mathrm{Wibb}}_x^{k^2}+{\mathrm{Wibb}}_y^{k^2}+{\mathrm{Wibb}}_z^{k^2}},$ 5个采样点的载体角速率矢量模短时均值，用

${\mathrm{Wibb}}_5^k=({\mathrm{Wibb}}^{k-4}+{\mathrm{Wibb}}^{k-3}+L+{\mathrm{Wibb}}^k)/5,$ 和载体角速率矢量模5个采样点的均方差，用

表示， $\mathrm{std}\_{\mathrm{Wibb}}_5^k=\sqrt{{({\mathrm{Wibb}}^{k-4}-\mathrm{Wib}{b}_5^k)}^2+L+{({\mathrm{Wibb}}^k-{\mathrm{Wibb}}_5^k)}^2}/5.$

计算k时刻地磁矢量的X轴分量5个采样点均值，用

表示， $E_{x5}^k=(E_x^{k-4}+E_x^{k-3}+L+E_x^k)/5,$ X轴分量5个采样点均方差，用

表示， $\mathrm{std}\_E_{x5}^k=\sqrt{{(E_x^{k-4}-E_{x5}^k)}^2+L+{(E_x^k-E_{x5}^k)}^2}/5;$ 计算k时刻地磁矢量的Y轴分量5个采样点均值，用

表示， $E_{y5}^k=(E_y^{k-4}+E_y^{k-3}+L+E_y^k)/5,$ Y轴分量5个采样点均方差，用

表示， $\mathrm{std}\_E_{y5}^k=\sqrt{{(E_y^{k-4}-E_{y5}^k)}^2+L+{(E_y^k-E_{y5}^k)}^2}/5;$ 计算k时刻地磁矢量的Z轴分量5个采样点均值，用

表示， $E_{z5}^k=(E_z^{k-4}+e_z^{k-3}+L+e_z^k)/5,$ Z轴分量5个采样点均方差，用

表示， $\mathrm{std}\_E_{z2}^k=\sqrt{{(e_z^{k-4}-E_{z5}^k)}^2+L+{(E_z^k-E_{z5}^k)}^2}/5.$ 根据

输出区间范围利用设计的飞行模态分类器，将载体机动级别Status分为平稳飞行(Low)、低机动(Middle)、高机动(High)三个级别；然后通过

将平稳飞行段分为平飞和振动状态；再引入磁传感器波动信息量：

将低机动和高机动分别分为小加速、低滚转和大加速、高速滚转状态。传感器数据预处理获得参数及载体飞行模态判别整体框图如图3所示。

步骤四，飞行模态划分及观测噪声矩阵调节量计算：

在步骤三获得传感器预处理信号和载体机动模态特征分类后，根据传感器的实时输出，选取观测噪声调整策略和调整力度，即对于加速类机动主要是根据载体加速机动幅度大小，计算加速情况下观测噪声影响因子，用α^k表示； ${\mathrm{\&alpha;}}^k=\left\{\begin{array}{c}1(\mathrm{Status}=\mathrm{Low})\\ a_0\frac{|{\mathrm{Fb}}_{m_4}^k-{\mathrm{Fb}}_{m_2\min }^k|}{{\mathrm{Fb}}_{5\min }^k}(\mathrm{Status}=\mathrm{Middle})(1<a_0<2)\\ a_1\frac{|{\mathrm{Fb}}_{m_4}^k-{\mathrm{Fb}}_{m_2\min }^k|}{{\mathrm{Fb}}_{m_2\min }^k}(\mathrm{Status}=\mathrm{High})(2<a_1<10)\end{array}\right\};$ 对于转动类机动根据载体转动角速率大小计算转动情况下的观测噪声影响因子，用γ^k表示， ${\mathrm{\&gamma;}}^k=\left\{\begin{array}{c}1(\mathrm{Status}=\mathrm{Low})\\ b_0{\mathrm{Wibb}}_{m_4}^k(\mathrm{Status}=\mathrm{Middle})(1<b_0<2)\\ b_1{\mathrm{Wibb}}_{m_4}^k(\mathrm{Status}=\mathrm{High})(2<b_1<10)\end{array}\right\};$ 对于载体平稳飞行模态下的载体随机振动，减小观测噪声量影响因子计算平稳飞行观测噪声影响因子，用v^k表示， $v^k=c_0{\mathrm{Wibb}}_{m_4}^k/\mathrm{std}\_{\mathrm{Fb}}_{m_4}^k(1<c_0<2).$ 在获得观测噪声影响因子α^k、γ^k、v^k，之后利用前一时刻的滤波观测置噪声量信度因子C_t-Δt，计算基于载体模态判别的观测噪声矩阵R_t-Δt，R_t-Δt＝C_t-Δt(R⁰+α^kγ^kv^kQ⁰)。

步骤五，观测噪声矩阵的在线自适应调整：在步骤四获得基于载体模态判别的观测噪声矩阵R_t-Δt，利用此观测量，进行卡尔曼滤波计算，根据状态估计的一步预测值计算观测值的一步预测值，用

表示，计算公式为

再求出观测残差，用rt表示，然后计算当前观测残差序列的平方和

再通过得到的

计算当前时刻的滤波观测置信度因子，用C_t表示， $C_t=\frac{\mathrm{\&beta;Tr}\left[R_t{H}_t{P}_t^T\right]}{r_t^T{r}_t},0<C_t\&le;1,\mathrm{\&beta;}\&GreaterEqual;1,$ 然后利用更新的滤波观测噪声量信度因子C_t，计算更新的滤波观测噪声量矩阵，用R_t表示，R_t＝C_t(R⁰+α^kγ^kv^kQ⁰)。

步骤六，微惯性捷联姿态组合：通过比力及地磁矢量计算出载体姿态角的观测量，与步骤二计算的捷联姿态作差，作为卡尔曼滤波器的观测量，送入卡尔曼滤波器，估计捷联姿态解算的姿态误差，再用此误差角修正捷联姿态解算结果，提高微惯性捷联姿态确定方法的精度和稳定性，具体过程如下：

首先，利用k时刻测得的载体在X轴向的比力值

载体在Y轴向的比力值

载体在Z轴向的比力值

地磁场在地理系下X轴向上的分量

Y轴向上的分量Z轴向上的分量

计算载体俯仰角，用θ_a表示，

计算载体横滚角，用γ_a表示，

计算载体航向角，用ψ_a表示，

与步骤二计算的捷联姿态作差，获得卡尔曼滤波器的姿态角误差观测量 $\left[\begin{array}{c}\mathrm{\&gamma;}-{\mathrm{\&gamma;}}_a\\ \mathrm{\&theta;}-{\mathrm{\&theta;}}_a\\ \mathrm{\&psi;}-{\mathrm{\&psi;}}_a\end{array}\right],$ 定义观测矩阵为 $H_t=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$ 在卡尔曼滤波器中，状态向量为三维，状态预测的方差阵P为3×3的矩阵，初始估值方差阵初值P₀由初始给定(一般为传感器噪声方差的平方)，系统噪声方差阵初值Q⁰，观测噪声方差矩阵初值R⁰矩阵初值由步骤二获得，状态转移矩阵φ_t，t-1是现有技术中已知矩阵(文献：刘建业等编著：《导航系统理论与应用》，2010年西北工业大学出版社)，系统噪声矩阵Q_t由系统噪声方差阵初值Q⁰确定，观测噪声矩阵R_t由步骤五计算得到。则各滤波时刻的状态估值的方差阵及状态预测的方差阵都可以递推得到。即在滤波时刻t-Δt的P_t-1基础上，按如下递推滤波方差，得到滤波时刻t的状态预测的方差阵P_t|t-Δt，滤波器增益矩阵K_t，状态估值

及状态估值的方差矩阵P_t可由下式计算得到。

Q_t＝Q⁰

$P_{t|t-\mathrm{\&Delta;t}}={\mathrm{\&phi;}}_{t,t}{P}_t{\mathrm{\&phi;}}_{t,t-\mathrm{\&Delta;t}}^T+Q_t$

$K_t=P_{t|t-\mathrm{\&Delta;t}}{H}_t^T{(H_t{P}_{t|t-\mathrm{\&Delta;t}}{H}_t^T+R_t)}^{-1}$

${\hat{X}}_t=K_t{Z}_t$

P_t＝(I-K_tH_t)P_t|t-Δt

上述计算的状态估计值

是步骤二捷联惯性四元数所得到的姿态角的误差角，在步骤二所得到的结果中减去估计值

中对应的量，即可提高微惯性组合导航系统的姿态精度。

通过实验室转台摆动实验，验证了该方法对于动静态情况下微型姿态参考系统精度及稳定性的提高，如图4所示，为横滚60°摆幅，0.2Hz频率下传统方法姿态输出与转台摆动对比曲线和姿态角误差曲线，如图5所示，为为横滚60°摆幅，0.2Hz频率下基于载体飞行模态判别的微惯性参数自适应姿态确定方法的姿态输出与转台摆动对比曲线和姿态角误差曲线，由图4和图5的姿态角误差对比可以看出，在使用传统固定阀值的微惯性姿态确定方法时，由于摆动过程中有害加速度的引入和滤波参数初值的设置不合理，导致输出横滚角误差达5°。而引入基于载体飞行模态判别的微惯性参数自适应姿态确定方法后，微型姿态参考系统能够自动适应动态过程，能够快速跟踪横滚角的变化，误差减小到1°以内。

Claims (2)

1.一种基于载体飞行模态判别的微惯性参数自适应姿态确定方法，其特征在于步骤如下：

步骤一，传感器数据采集：通过AD转换器或传感器数字接口直接采集MEMS传感器输出信号，获得载体k时刻绕X轴向的角速度

载体绕Y轴向的角速度

载体绕Z轴向的角速度载体在X轴向的比力值

载体在Y轴向的比力值

载体在Z轴向的比力值地磁场在地理系下X轴向上的分量Y轴向上的分量

Z轴向上的分量

步骤二，计算k时刻的捷联姿态：利用k时刻采集到的角速度信息，按四元数姿态解算流程，获得载体姿态角；系统时间t，t＝kΔt，Δt时间内载体转动过得角度Δθ的三个分量依次为横滚角增量Δθ_x，俯仰角增量Δθ_y，方位角增量Δθ_z，则转动的角度以矩阵的方式表示为：

$\mathrm{\&Delta;\&theta;}=\left[\begin{array}{ccc}0& -{\mathrm{\&Delta;\&theta;}}_z& {\mathrm{\&Delta;\&theta;}}_y\\ {\mathrm{\&Delta;\&theta;}}_z& 0& {-\mathrm{\&Delta;\&theta;}}_x\\ -{\mathrm{\&Delta;\&theta;}}_y& {\mathrm{\&Delta;\&theta;}}_x& 0\end{array}\right]=\mathrm{\&Delta;t}\left[\begin{array}{ccc}0& -{\mathrm{Wibb}}_z& {\mathrm{Wibb}}_y\\ {\mathrm{Wibb}}_z& 0& {-\mathrm{Wibb}}_x\\ -{\mathrm{Wibb}}_y& {\mathrm{Wibb}}_x& 0\end{array}\right],$

t-Δt时刻的姿态矩阵为：

$C_n^b(t-\mathrm{\&Delta;t})=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\cos \mathrm{\&theta;}\sin \mathrm{\&psi;}\\ \cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right],$

所对应的四元数姿态阵为

$Q\left(t\right)=\left[\begin{array}{c}\cos (\mathrm{\&psi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&gamma;}/2)+\sin (\mathrm{\&psi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&gamma;}/2)\\ \cos (\mathrm{\&psi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&gamma;}/2)+\sin (\mathrm{\&psi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&gamma;}/2)\\ \cos (\mathrm{\&psi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&gamma;}/2)-\sin (\mathrm{\&psi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&gamma;}/2)\\ \cos (\mathrm{\&psi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&gamma;}/2)-\sin (\mathrm{\&psi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&gamma;}/2)\end{array}\right],$

角增量模 $\left|\mathrm{\&Delta;\&theta;}\right|=\sqrt{{\mathrm{\&Delta;\&theta;}}_x^2+{\mathrm{\&Delta;\&theta;}}_y^2+{\mathrm{\&Delta;\&theta;}}_z^2},$ $\left[\mathrm{\&Delta;\&theta;}\right]=\left[\begin{array}{cccc}0& {-\mathrm{\&Delta;\&theta;}}_x& -{\mathrm{\&Delta;\&theta;}}_y& -{\mathrm{\&Delta;\&theta;}}_z\\ {\mathrm{\&Delta;\&theta;}}_x& 0& {\mathrm{\&Delta;\&theta;}}_z& -{\mathrm{\&Delta;\&theta;}}_y\\ {\mathrm{\&Delta;\&theta;}}_y& {-\mathrm{\&Delta;\&theta;}}_z& 0& {\mathrm{\&Delta;\&theta;}}_x\\ {\mathrm{\&Delta;\&theta;}}_z& {\mathrm{\&Delta;\&theta;}}_y& -{\mathrm{\&Delta;\&theta;}}_x& 0\end{array}\right],$

利用一阶近似计算方法更新后的四元数矩阵 $Q\left(t\right)=(\cos \left(\frac{\left|\mathrm{\&Delta;\&theta;}\right|}{2}\right)I+\frac{1}{2}[\mathrm{\&Delta;\&theta;}\left]\right)Q={\left[\begin{array}{cccc}{Q}_0& Q_1& Q_2& Q_3\end{array}\right]}^T,$

得到更新后的姿态矩阵：

$C_n^b\left(t\right)=\left[\begin{array}{ccc}{Q_1}^2+{Q_0}^2-{Q_3}^2-{Q_2}^2& {2Q}_1{Q}_2+Q_0{Q}_3& {2Q}_1{Q}_3-Q_0{Q}_2\\ {2Q}_1{Q}_2-Q_0{Q}_3& {Q_2}^2-{Q_3}^2+{Q_0}^2-{Q_1}^2& {2Q}_2{Q}_3+Q_0{Q}_1\\ {2Q}_1{Q}_3+Q_0{Q}_2& {2Q}_2{Q}_3-Q_0{Q}_1& {Q_3}^2-{Q_2}^2-{Q_1}^2+{Q_0}^2\end{array}\right],$

将

简写为 $\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right],$ 则可计算出t时刻捷联惯导的俯仰角θ、横滚角γ和航向角ψ姿态角信息，θ＝sin^-1(T₂₃)， $\mathrm{\&gamma;}={\mathrm{tg}}^{-1}\left(\frac{T_{13}}{T_{33}}\right),$ $\mathrm{\&psi;}={\mathrm{tg}}^{-1}\left(\frac{T_{21}}{T_{32}}\right);$

步骤三，传感器参数预处理：

(1)微型姿态参考系统运行过程中，如k＞m₁，且微型姿态参考系统满足平稳条件，计算k时刻当地加速度的矢量模用Fb^k表示，

与前m₁个满足条件的平稳条件的加速度矢量模值求均值，用表示， ${\mathrm{Fb}}_{m_1+1}^k=({\mathrm{Fb}}^{k-m_1}+{\mathrm{Fb}}^{k-m_1+1}+L+{\mathrm{Fb}}^k)/(m_1+1),$ m₁是微型姿态参考系统数秒内总采样点数；

(2)每隔m₂分钟计算一次飞机平稳状态下测得m₃个点的加速度矢量模的均值，并对所有加速度矢量模的稳态数据求均值，用表示， ${\mathrm{Fb}}_{m_2\min }^k=({\mathrm{Fb}}_{m_3}^1+{\mathrm{Fb}}_{m_3}^2+L+{\mathrm{Fb}}_{m_3}^{m_3})/m_3,$ 作为当前载体状态判断的加速度矢量模的基准量；

(3)计算k时刻载体比力矢量模m₄个采样点的短时均值，用

表示， ${\mathrm{Fb}}_{m_4}^k=({\mathrm{Fb}}^{k-m_4+1}+{\mathrm{Fb}}^{k-m_4+2}+L+{\mathrm{Fb}}^k)/m_4,$ m₄个采样点的均方差

$\mathrm{std}\_{\mathrm{Fb}}_{m_4}^k=\sqrt{{({\mathrm{Fb}}^{k-m_4+1}-{\mathrm{Fb}}_{m_4}^k)}^2+{({\mathrm{Fb}}^{k-m_4+2}-{\mathrm{Fb}}_{m_4}^k)}^2+L+{({\mathrm{Fb}}^k-{\mathrm{Fb}}_{m_4}^k)}^2}/m_4;$

(4)计算k时刻载体的角速率矢量模，用Wibb^k表示， ${\mathrm{Wibb}}^k=\sqrt{{\mathrm{Wibb}}_x^{k^2}+{\mathrm{Wibb}}_y^{k^2}+{\mathrm{Wibb}}_z^{k^2}},$ m₄个采样点的载体角速率矢量模短时均值，用

表示， ${\mathrm{Wibb}}_{m_4}^k=({\mathrm{Wibb}}^{k-m_4+1}+{\mathrm{Wibb}}^{k-m_4+2}+L+{\mathrm{Wibb}}^k)/m_4,$ 和载体角速率矢量模m₄个采样点的均方差，用

表示， $\mathrm{std}\_{\mathrm{Wibb}}_{m_4}^k=\sqrt{{({\mathrm{Wibb}}^{k-m_4+1}-{\mathrm{Wibb}}_{m_4}^k)}^2+L+{({\mathrm{Wibb}}^k-{\mathrm{Wibb}}_{m_4}^k)}^2}/m_4;$

(5)计算k时刻地磁矢量的X轴分量m₄个采样点均值，用

表示， $E_{x/m_4}^k=(E_x^{k-m_4+1}+E_x^{k-m_4+2}+L+E_x^k)/m_4,$ X轴分量m₄个采样点均方差，用

表示， $\mathrm{std}\_E_{x/m_4}^k=\sqrt{{(E_x^{k-m_4+1}-E_{x/m_4}^k)}^2+L+{(E_x^k-E_{x/m_4}^k)}^2}/m_4;$ 计算k时刻地磁矢量的Y轴分量m₄个采样点均值，用

表示， $E_{y/m_4}^k=(E_y^{k-m_4+1}+E_y^{k-m_4+2}+L+E_y^k)/m_4,$ Y轴分量m₄个采样点均方差，用

表示， $\mathrm{std}\_E_{y/m_4}^k=\sqrt{{(E_y^{k-m_4+1}-E_{y/m_4}^k)}^2+L+{(E_y^k-E_{y/m_4}^k)}^2}/m_4;$ 计算k时刻地磁矢量的Z轴分量m₄个采样点均值，用

表示， $E_{z/m_4}^k=(E_z^{k-m_4+1}+E_z^{k-m_4+2}+L+E_z^k)/m_4,$ Z轴分量m₄个采样点均方差，用

表示， $\mathrm{std}\_E_{z/m_4}^k=\sqrt{{(E_z^{k-m_4+1}-E_{z/m_4}^k)}^2+L+{(E_z^k-E_{z/m_4}^k)}^2}/m_4,$ 根据

输出区间范围利用设计的飞行模态分类器，将载体机动级别Status分为平稳飞行Low、低机动Middle、高机动High三个级别；然后通过

将平稳飞行段分为平飞和振动状态；再引入磁传感器波动信息量：

将低机动和高机动分别分为小加速、低滚转和大加速、高速滚转状态；

步骤四，飞行模态划分及观测噪声矩阵调节量计算：

在步骤三获得传感器预处理信号和载体机动模态特征分类后，根据传感器的实时输出，选取观测噪声调整策略和调整力度，即对于加速类机动是根据载体加速机动幅度大小，计算加速情况下观测噪声影响因子，用α^k表示； ${\mathrm{\&alpha;}}^k=\left\{\begin{array}{c}1(\mathrm{Status}=\mathrm{Low})\\ a_0\frac{|{\mathrm{Fb}}_{m_4}^k-{\mathrm{Fb}}_{m_2\min }^k|}{{\mathrm{Fb}}_{5\min }^k}(\mathrm{Status}=\mathrm{Middle})(1<a_0<2)\\ a_1\frac{|{\mathrm{Fb}}_{m_4}^k-{\mathrm{Fb}}_{m_2\min }^k|}{{\mathrm{Fb}}_{m_2\min }^k}(\mathrm{Status}=\mathrm{High})(2<a_1<10)\end{array}\right\};$ 对于转动类机动根据载体转动角速率大小计算转动情况下的观测噪声影响因子，用γ^k表示， ${\mathrm{\&gamma;}}^k=\left\{\begin{array}{c}1(\mathrm{Status}=\mathrm{Low})\\ b_0{\mathrm{Wibb}}_{m_4}^k(\mathrm{Status}=\mathrm{Middle})(1<b_0<2)\\ b_1{\mathrm{Wibb}}_{m_4}^k(\mathrm{Status}=\mathrm{High})(2<b_1<10)\end{array}\right\};$ 减小观测噪声量影响因子计算平稳飞行观测噪声影响因子，用v^k表示， $v^k=c_0{\mathrm{Wibb}}_{m_4}^k/\mathrm{std}\_{\mathrm{Fb}}_{m_4}^k(1<c_0<2);$ 在获得观测噪声影响因子α^k、γ^k、v^k之后利用前一时刻的滤波观测噪声置信度因子C_t-Δt，计算基于载体模态判别的观测噪声矩阵R_t-Δt，R_t-Δt＝C_t-Δt(R⁰+α^kγ^kv^kQ⁰)；

步骤五，观测噪声矩阵的在线自适应调整：在步骤四获得基于载体模态判别的观测噪声矩阵R_t-Δt，利用此观测量，进行卡尔曼滤波计算，根据状态估计的一步预测值计算观测值的一步预测值，用

表示，计算公式为

再求出观测残差，用rt表示，然后计算当前观测残差序列的平方和

再通过得到的

计算当前时刻的滤波观测置信度因子，用C_t表示， $C_t=\frac{\mathrm{\&beta;Tr}\left[R_t{H}_t{P}_t^T\right]}{r_t^T{r}_t},0<C_t\&le;1,\mathrm{\&beta;}\&GreaterEqual;1,$ 然后利用更新的滤波观测噪声量信度因子C_t，计算更新的滤波观测噪声量矩阵，用R_t表示，R_t＝C_t(R⁰+α^kγ^kv^kQ⁰)；

步骤六，微惯性捷联姿态组合：通过比力及地磁矢量计算出载体姿态角的观测量，与步骤二计算的捷联姿态作差，作为卡尔曼滤波器的观测量，送入卡尔曼滤波器，估计捷联姿态解算的姿态误差，再用此误差角修正捷联姿态解算结果，提高微惯性捷联姿态确定方法的精度和稳定性。

2.根据权利要求1所述的基于载体飞行模态判别的微惯性参数自适应姿态确定方法，其特征在于步骤六的微惯性捷联姿态组合过程如下：

首先，利用k时刻测得的载体在X轴向的比力值

载体在Y轴向的比力值

载体在Z轴向的比力值

地磁场在地理系下X轴向上的分量Y轴向上的分量

Z轴向上的分量计算载体俯仰角，用θ_a表示，计算载体横滚角，用γ_a表示，

计算载体航向角，用ψ_a表示，

与步骤二计算的捷联姿态作差，获得卡尔曼滤波器的姿态角误差观测量 $\left[\begin{array}{c}\mathrm{\&gamma;}-{\mathrm{\&gamma;}}_a\\ \mathrm{\&theta;}-{\mathrm{\&theta;}}_a\\ \mathrm{\&psi;}-{\mathrm{\&psi;}}_a\end{array}\right],$ 定义观测矩阵为 $H_t=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$ 在卡尔曼滤波器中，状态向量为三维，状态预测的方差阵P为3×3的矩阵，初始估值方差阵初值P₀由初始给定，系统噪声方差阵初值Q⁰，观测噪声方差矩阵初值R⁰矩阵初值由步骤二获得，状态转移矩阵φ_t，t-1是已知矩阵，系统噪声矩阵Q_t由系统噪声方差阵初值Q⁰确定，观测噪声矩阵R_t由步骤五计算得到，则各滤波时刻的状态估值的方差阵及状态预测的方差阵都递推得到，即在滤波时刻t-Δt的P_t-1基础上，按如下递推滤波方差，得到滤波时刻t的状态预测的方差阵P_t|t-Δt，滤波器增益矩阵K_t，状态估值及状态估值的方差矩阵P_t可由下式计算得到：

Q_t＝Q⁰

$P_{t|t-\mathrm{\&Delta;t}}={\mathrm{\&phi;}}_{t,t}{P}_t{\mathrm{\&phi;}}_{t,t-\mathrm{\&Delta;t}}^T+Q_t$

$K_t=P_{t|t-\mathrm{\&Delta;t}}{H}_t^T{(H_t{P}_{t|t-\mathrm{\&Delta;t}}{H}_t^T+R_t)}^{-1}$

${\hat{X}}_t=K_t{Z}_t$

P_t＝(I-K_tH_t)P_t|t-Δt

上述计算的状态估计值

是步骤二捷联惯性四元数所得到的姿态角的误差角，在步骤二所得到的结果中减去估计值

中对应的量，即可提高微惯性组合导航系统的姿态精度。

Images