CN109708670A - 基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法 - Google Patents

Abstract

本发明属于惯性导航领域，具体涉及一种基于改进的Sage‑Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法。本发明通过向系统过程噪声中注入白噪声以补偿挠曲变形产生的干扰误差，结合以格林威治子午线为航向参考线的格网坐标系下的导航误差方程，设计了一种极区环境下传递对准挠曲变形的噪声补偿法，采用“速度+姿态”的匹配方式，利用改进的Sage‑Husa自适应卡尔曼滤波解算，估计出子惯导系统的姿态失准角和速度误差值，完成了传递对准。该方法解决了极区环境下因挠曲变形建模补偿而导致的计算量大和鲁棒性差等问题，提高了极区传递对准精度和适用性。

Description

基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠 曲变形的噪声补偿方法

技术领域

本发明属于惯性导航领域，具体涉及一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法。

背景技术

初始对准作为捷联惯性导航关键技术之一，是捷联惯导系统进行导航的前提，其精度直接影响到系统的导航精度。为了缩短对准时间和提高对准精度，对于大型船舶的附属航行器与工作船，其初始对准一般不采用自主式对准方法，而是采用传递对准的方式实现其初始对准，通过估计出主、子惯导之间的失准角，达到借助高精度主惯导系统信息完成低精度子惯导系统初始对准的目的。当前主流的导航方式是基于地理坐标系的指北方位导航方法，但是经线随着纬度升高极剧收敛，其在极地地区存在计算溢出和导航误差放大等问题，因此在极区对导航系统力学编排采用格网系下的力学编排。同时，在恶劣的极区海况下，受强风、巨浪和海洋暗流等干扰，以及载体本身发动机振动等因素影响，船舶会发生一定程度的挠曲变形，对传递对准的速度和精度产生不利影响。当前大多数研究采用的是模型补偿法，其模型为白噪声驱动的马尔可夫过程，但是该补偿法会增加滤波模型的维度，加重系统计算负担，系统鲁棒性降低，因此采用通过调整过程噪声强度的方法来替代挠曲变形建模，此称为噪声补偿法。但是，极区恶劣的天气使得载体的噪声统计特性将会变的不确定，而作为传递对准状态估计方法的卡尔曼滤波，则会导致系统精度降低设置于系统发散。因此，在噪声统计特性不确定的情况下，需要引入Sage-Husa自适应卡尔曼滤波对极区传递对准进行估计。综上，在格网坐标系下采用基于Sage-Husa自适应卡尔曼滤波的挠曲变形的噪声补偿法，以解决极区恶劣环境下的载体挠曲形变对传递对准的不利影响。

孙昌跃等人在《舰体挠曲运动在线建模研究》(发表于期刊《系统工程与电子技术》，第29卷，2期)一文中，提出了船舶挠曲变形的二阶马尔可夫模型，并利用自相关函数法求出挠曲变形模型参数的方法。模型补偿法虽然可以准确地对挠曲变形进行建模，但其对挠曲变形角及其角速度的建模会使得滤波模型维数增加，进而导致计算量增大，系统鲁棒性降低。因此，Kain J在《Rapid TransferAlignment for Tactical Weapon》(AIAAGuidance,Navigation and Control Conference,1989)一文中，删去了所建立的传递对准误差模型中的挠曲变形的补偿项，使滤波状态量的维数从42维降低至24维，降低了计算量并提高了系统的鲁棒性；同时，Kain J通过对过程噪声进行调整，完成了对挠曲变形的补偿。这种通过调整过程噪声强度来替代挠曲变形建模的方法，被称为噪声补偿法。但是，此状态方程和观测方程的力学编排均投影在地理坐标系下，在极区存在计算溢出和导航系统误差放大的问题。并且，Kain J采用的传统卡尔曼滤波进行状态估计，无法解决在极区恶劣环境下噪声的统计特性不确定的难题。综上所述，现有的技术不能有效地实现极区环境下对传递对准挠曲变形的噪声补偿。

发明内容

本发明的目的在于提供一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，其能有效地实现极区环境下对传递对准挠曲变形的噪声补偿。

一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，具体包括如下步骤：

步骤1、子惯导系统启动、预热；

步骤2、子惯导系统利用主惯导系统发送的导航参数完成一次装订；

步骤3、子惯导系统进行惯导解算，同步采集主、子惯导系统在格网系下输出的速度和姿态信息，依据主惯导解算信息补偿动态挠曲杆臂速度，并以经过杆臂速度补偿的速度误差和姿态误差构成观测量；

步骤4、向系统的过程噪声中注入白噪声，借助二阶马尔可夫过程的谱密度确定白噪声的谱密度，并对姿态误差方程、速度误差方程和动态挠曲杆臂模型重新改写；

步骤5、依据格网系下的导航力学编排，结合格网导航误差方程，采用“速度+姿态”的匹配方式，建立格网系下的系统状态方程和量测方程；

步骤6、利用改进的Sage-Husa滤波对设计的格网坐标系下的状态方程、量测方程和构造的量测量进行解算，估算出子惯导系统的姿态失准角、速度的状态估算值，完成传递对准。

所述一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，步骤2中导航参数包括速度、姿态矩阵和位置信息。

所述一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，步骤3中所述经过杆臂速度补偿的速度误差δv^G，其表达式为：

式中，和分别为格网系下子惯导解算速度、主惯导速度和动态挠曲杆臂速度；

所述的观测量Z，其表达式为：

z＝[δv^G φ_m]

式中，φ_m是格网系下的姿态失准角。

所述一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，步骤5中所述的状态变量为：

所述的系统状态方程为：

式中，是加速度常值漂移，是陀螺常值漂移，φ_a ^G是安装失调角，r^m是动态挠曲杆臂，是加速度计量测量，是格网系下的地球自转速率，是格网系下的格网系相对地理坐标系的旋转角速率，是在载体系下载体系相对格网系的测量旋转角速率，ω_φ、ω_v和ω_r为白噪声过程，且满足可简化表示为：

式中，A为状态矩阵，B为过程噪声矩阵，w为过程噪声可以表示如下：

式中，和为加速度计和陀螺仪的随机漂移；

所述的观测方程为：

Z＝HX+v

式中，H是观测矩阵，v为观测噪声，如下所示：

v＝[(v_v)^T (v_φ)^T]^T

式中，v_v为速度误差观测噪声，v_φ为姿态误差观测噪声。

本发明的有益效果在于：

本发明不同于对系统模型进行状态增广的建模补偿法，噪声补偿法将挠曲变形角θ及其角速度从系统状态量中剔除，进而降低了系统维数与计算量；通过对系统过程噪声进行增强来补偿挠曲变形，进一步增加了系统的鲁棒性；改进的Sage-Husa自适应卡尔曼滤波解决了传统自适应卡尔曼滤波算法中容易出现的Q_k失去半正定性或R_k失去正定性的现象；改进的Sage-Husa自适应卡尔曼滤波在过程噪声中加入了替代忽略项的白噪声，从而解决了传统自适应卡尔曼滤波算法中过程噪声协方差阵Q_k和测量噪声协方差阵R_k无法同时估计的问题，所以可只考虑过程噪声的不确定问题。

附图说明

图1为本发明提出的一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法的基本流程图；

图2为本发明方法和格网系基于常规卡尔曼滤波的未补偿的传递对准方法在匀加速直线运动状态时φ_ax的估计值曲线对比图；

图3为本发明方法和格网系基于常规卡尔曼滤波的未补偿的传递对准方法在匀加速直线运动状态时φ_ay的估计值曲线对比图；

图4为本发明方法和格网系基于常规卡尔曼滤波的未补偿的传递对准方法在匀加速直线运动状态时φ_az的估计值曲线对比图。

具体实施方式

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

本发明提出了一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，如附图1所示，该方法的主要步骤如下：

1.完成子惯导系统的启动、预热准备，子惯导系统利用主惯导系统发送的导航参数完成一次装订，所述的导航参数包括速度、姿态矩阵和位置信息；

2.子惯导系统进行惯导解算，同步采集主、子惯导系统在格网系下输出的速度和姿态信息，依据主惯导解算信息补偿动态挠曲杆臂速度，并以经过杆臂速度补偿的速度误差和姿态误差构成观测量；

所涉及的经过杆臂速度补偿的速度误差δv^G，其表达式为：

式中，和分别为格网系下子惯导解算速度，主惯导速度和动态挠曲杆臂速度，其中动态挠曲杆臂速度为：

所涉及的观测量Z，其表达式为：

z＝[δv^G φ_m]

式中，φ_m是格网系下的姿态失准角；

3.基于噪声补偿法，向系统的过程噪声中注入白噪声，借助二阶马尔可夫过程的谱密度确定白噪声的谱密度，并对未补偿的姿态误差方程、速度误差方程和动态挠曲杆臂模型重新改写；

所涉及的二阶马尔可夫过程的谱密度为：

再根据的值，确定ω_φ、ω_v和ω_r的谱密度，其体现在过程噪声矩阵Q的对角线元素上，在设置时需要大于已确定的ω_φ、ω_v和ω_r谱密度；

所涉及的未补偿的姿态误差方程、速度误差方程和动态挠曲杆臂模型的表达式为：

式中，利用挠曲变形的噪声补偿法，和可被忽略，为了替代被忽略项，可以在系统过程噪声中加入白噪声ω_φ、ω_v和ω_r，且满足因此格网系误差方程可以重写为：

4.依据格网系下的导航力学编排，结合格网导航误差方程，采用“速度+姿态”的匹配方式，建立格网系下的系统状态方程和量测方程；

所涉及的状态变量为：

所涉及的系统状态方程为：

式中，是加速度常值漂移，是陀螺常值漂移，φ_a ^G是实际物理失准角，是加速度计量测量，是格网系下的地球自转速率，是格网系下的格网系相对地理坐标系的旋转角速率，是在载体系下载体系相对格网系的测量旋转角速率，可简化表示为：

式中，A为状态矩阵，B为过程噪声矩阵，w为过程噪声可以表示如下：

式中，和为加速度计和陀螺仪的随机漂移；

所涉及的观测方程为：

式中，H是观测矩阵，v为观测噪声，如下所示：

v＝[(v_v)^T (v_φ)^T]^T

式中，v_v为速度误差观测噪声，v_φ为姿态误差观测噪声；

5.利用改进的Sage-Husa自适应卡尔曼滤波对设计的格网坐标系下的状态方程、量测方程和构造的量测量进行解算，估算出子惯导系统的姿态失准角、速度的状态估算值，完成传递对准；

其涉及的改进的Sage-Husa自适应卡尔曼滤波，其过程表示为：

P_k＝[I-K_kH_k]P_k/k-1

式中，P_k为k时刻估计状态量的协方差矩阵，K_k和V_k分别为k时刻的滤波增益与新息，q_k和Q_k分别为k时刻系统噪声序列的均值与协方差矩阵，r_k和R_k分别为k时刻观测噪声序列的均值与协方差矩阵，由于挠曲变形的噪声补偿法只在过程噪声中加入了替代忽略项的白噪声，故可只考虑Q_k的噪声不确定问题，即在R_k已知的情况下对Q_k进行估计，其可以表示如下：

d_k＝(1-b)/(1-b^k+1)

式中，b为遗忘因子，通常取值范围为0.95＜b＜0.995，为了保证滤波精度，对于式中负项和的对角线元素做取绝对值处理。

为了验证本发明的合理性、可行性，利用Matlab程序对设计的基于Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿法进行仿真，为实现对比分析，格网系基于常规卡尔曼滤波的未补偿的传递对准方法称为传统方法，本发明提出的基于改进的Sage-Husa自适应卡尔曼滤波的噪声补偿后的传递对准方法称为改进方法；

Matlab仿真所设定的初始条件为：

(1)子惯导惯性器件测量误差的相关参数分别为：三轴陀螺常值漂移分别为6.3865×10^-9rad/s、-4.0947×10^-9rad/s和-1.9605×10^-8rad/s；三轴陀螺随机漂移分别为3.917×10^-6rad/s、3.263×10^-6rad/s和1.534×10^-6rad/s；三轴加速度常值漂移分别为5.0024×10^-6rad/s、2.7452×10^-6rad/s和-7.2110×10^-6rad/s；三轴加速度随机漂移分别为0.00151m/s²、0.001698m/s²和0.0003723m/s²。

(2)对准时间为25s，步长为0.1s，初始状态估计协方差阵P₀、系统过程噪声协方差阵Q和观测噪声协方差阵R分别为：

R＝diag{(0.01m/s)²,(0.01m/s)²,(0.1°)²,(0.1°)²,(0.1°)²}

(3)以上述仿真条件，仿真得到的结果如下表所示：

由图2～图4可以看出，仿真情况下，改进方法可以在20秒内完成对实际物理失准角的估计，而传统方法中实际物理失准角的估计误差一直处于发散状态，即无法完成对实际物理失准角的估计。同传统方法的估计误差相比，改进方法的估计误差更低，同时在不同运动状态下也更稳定。相比纵向与横向失准角误差，现有方法的方位失准角误差更大。这是因为传统方法的本质仍是经典传递对准。改进方法中三轴实际物理失准角的估计误差分别在0.05角分、0.02角分和0.25角分内。同时，δφ_ax、δφ_ay和δφ_az平均值的绝对值与均方根误差都十分接近，这说明在δφ_a收敛后，其收敛结果非常平稳。

综合上述分析结果，得到如下的分析结论：在极区传递对准应用中，格网系传递对准改进方法的对准精度与速度要明显优于现有格网系传递对准方法，且改进方法能够在极区环境下实现快速、准确的传递对准，并可以有效地补偿挠曲变形干扰误差。

应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。

Claims (4)

1.一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，其特征在于，具体包括以下步骤：

步骤1、子惯导系统启动、预热；

步骤2、子惯导系统利用主惯导系统发送的导航参数完成一次装订；

步骤3、子惯导系统进行惯导解算，同步采集主、子惯导系统在格网系下输出的速度和姿态信息，依据主惯导解算信息补偿动态挠曲杆臂速度，并以经过杆臂速度补偿的速度误差和姿态误差构成观测量；

步骤4、向系统的过程噪声中注入白噪声，借助二阶马尔可夫过程的谱密度确定白噪声的谱密度，并对姿态误差方程、速度误差方程和动态挠曲杆臂模型重新改写；

步骤5、依据格网系下的导航力学编排，结合格网导航误差方程，采用“速度+姿态”的匹配方式，建立格网系下的系统状态方程和量测方程；

步骤6、利用改进的Sage-Husa滤波对设计的格网坐标系下的状态方程、量测方程和构造的量测量进行解算，估算出子惯导系统的姿态失准角、速度的状态估算值，完成传递对准。

2.根据权利要求1所述一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，其特征在于：步骤2中所述导航参数包括速度、姿态矩阵和位置信息。

3.根据权利要求1所述一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，其特征在于：步骤3中所述经过杆臂速度补偿的速度误差δv^G，其表达式为：

式中，和分别为格网系下子惯导解算速度、主惯导速度和动态挠曲杆臂速度；

所述的观测量Z，其表达式为：

z＝[δv^G φ_m]

式中，φ_m是格网系下的姿态失准角。

4.根据权利要求1所述一种基于改进的Sage-Husa自适应卡尔曼滤波的极区传递对准挠曲变形的噪声补偿方法，其特征在于：步骤5中所述的状态变量为：

所述的系统状态方程为：

式中，是加速度常值漂移，是陀螺常值漂移，φ_a ^G是安装失调角，r^m是动态挠曲杆臂，是加速度计量测量，是格网系下的地球自转速率，是格网系下的格网系相对地理坐标系的旋转角速率，是在载体系下载体系相对格网系的测量旋转角速率，ω_φ、ω_v和ω_r为白噪声过程，且满足简化表示为：

式中，A为状态矩阵，B为过程噪声矩阵，w为过程噪声，表示如下：

式中，和为加速度计和陀螺仪的随机漂移；

所述的观测方程为：

Z＝HX+v

式中，H是观测矩阵，v为观测噪声，如下所示：

v＝[(v_v)^T (v_φ)^T]^T

式中，v_v为速度误差观测噪声，v_φ为姿态误差观测噪声。