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

摘要

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

主权项

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 \gamma \cos \psi +\sin \gamma \sin \theta \sin \psi & -\cos \gamma \sin \psi +\sin \gamma \sin \theta \cos \psi & -\cos \theta \sin \psi \\ \cos \theta \sin \psi & \cos \theta \cos \psi & \sin \theta \\ \sin \gamma \cos \psi +\cos \gamma \sin \theta \sin \psi & -\sin \gamma \sin \psi -\cos \gamma \sin \theta \cos \psi & \cos \gamma \cos \theta \end{array}\right],$所对应的四元数姿态阵为$Q\left(t\right)=\left[\begin{array}{c}\cos (\psi /2)\cos (\theta /2)\cos (\gamma /2)+\sin (\psi /2)\sin (\theta /2)\sin (\gamma /2)\\ \cos (\psi /2)\sin (\theta /2)\cos (\gamma /2)+\sin (\psi /2)\cos (\theta /2)\sin (\gamma /2)\\ \cos (\psi /2)\cos (\theta /2)\sin (\gamma /2)-\sin (\psi /2)\sin (\theta /2)\cos (\gamma /2)\\ \cos (\psi /2)\sin (\theta /2)\sin (\gamma /2)-\sin (\psi /2)\cos (\theta /2)\cos (\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₂₃)，$\gamma ={\mathrm{tg}}^{-1}\left(\frac{T_{13}}{T_{33}}\right),$$\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表示；${\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表示，${\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,\beta \ge 1,$然后利用更新的滤波观测噪声量信度因子C_t，计算更新的滤波观测噪声量矩阵，用R_t表示，R_t＝C_t(R⁰+α^kγ^kv^kQ⁰)；步骤六，微惯性捷联姿态组合：通过比力及地磁矢量计算出载体姿态角的观测量，与步骤二计算的捷联姿态作差，作为卡尔曼滤波器的观测量，送入卡尔曼滤波器，估计捷联姿态解算的姿态误差，再用此误差角修正捷联姿态解算结果，提高微惯性捷联姿态确定方法的精度和稳定性。