一种声学测速辅助的极区格网惯性导航误差抑制方法与流程

本发明属于惯性导航技术领域，具体涉及一种声学测速辅助的极区格网惯性导航误差抑制方法。

背景技术：

为了满足极区导航需求并解决经线收敛引起的定位定向问题，通常采用基于格网导航坐标系的惯性导航系统作为极区运载器基础导航设备。格网惯性导航系统受到其工作原理的制约，存在三种振荡误差以及随时间积累的定位误差，无法满足高精度导航需求。引入声学测速信息作为辅助导航信息，采用基于卡尔曼滤波的误差抑制技术，可以有效估计格网惯导系统的姿态、速度和位置误差，在不破坏系统自主性的前提下，对格网惯导系统进行误差补偿，有效抑制导导航误差，提高系统导航精度。因此，基于声学测速的格网惯导系统误差抑制方法可以有效提高极区导航可靠性与精度，满足长航时高精度的工作条件下对系统性能的高要求，具有重要的工程应用价值。

在非极区，基于卡尔曼滤波的误差抑制方法可以有效估计指北惯导系统的导航误差，对其进行修正，以达到抑制导航误差提高精度的效果，但指北惯导系统不能克服经线收敛引起的定位定向问题，无法应用于极区。周琪等人在《极区飞行格网惯性导航算法原理》一文中提出了以格网坐标系为导航坐标系的格网惯性导航力学编排，可以实现极区惯性导航，现有的极区格网惯导系统性能提升方法，采用的是阻尼技术，仅能在一定程度上抑制振荡误差，调整时间长，且不能抑制积累误差，无法满足高精度导航需求。综上所述，现有的格网惯导误差抑制方法不能满足实际应用需求，限制了系统的实际工作效果与工作时长。

技术实现要素：

本发明的目的在于提供一种声学测速辅助的极区格网惯性导航误差抑制方法，实现快速有效的格网惯导系统导航性能提升。

本发明的目的是这样实现的：

一种声学测速辅助的极区格网惯性导航误差抑制方法，包括以下步骤：

(1)选取格网坐标系为导航坐标系，以惯性导航系统为主导航系统，引入多普勒计程仪作为速度测量器件提供载体速度；

(2)建立格网惯导系统误差特性方程，选取格网惯导系统的格网姿态误差、格网速度误差、地心地固坐标系下的位置误差以及惯性器件测量误差为状态量，选取多普勒速度作为观测量，建立卡尔曼滤波模型；

(3)滤波器获取格网惯导系统与多普勒计程仪数据后进行滤波估计，输出格网惯导系统的姿态误差、速度误差与位置误差的估计值；

(4)利用步骤3得到的导航误差估计值对格网惯导系统的输出进行校正，使格网惯导系统导航误差得到抑制，连续实时的输出校正后的高精度载体姿态、速度和位置误差，直至导航任务结束。

步骤2的格网惯导系统误差特性方程是这样建立的：

定义格网惯导系统主要导航参数误差，包括格网角误差δσ，姿态误差角φ＝[φxφyφz]^t，格网坐标系下的速度误差以及地心地固坐标系下的位置误差δr^e＝[δxδyδz]^t；

格网角误差δσ：

其中，位置误差表示为：

角速度误差为：

角速度误差为：

其中，

姿态更新四元数误差微分方程为：

其中，且

则格网系下的姿态误差方程表示为：

其中，cv_φ＝c4且cr_φ＝(c2c1+c3+c5+c6c1)cr2p；

建立格网惯导系统速度误差微分方程：

其中，cr_v＝(v^g×)(2c3cr2p+c5)+(v^g×)(2c2+c6)cr2σ,

假设计算格网系g^c与理想格网系g之间，存在位置误差角δθ^g，计算格网坐标系通过理想格网坐标系的三次旋转得到，因此位置误差角δθ^g表示为：

则位置误差表示为：

其中，且

因此，以格网惯导系统的格网姿态误差、格网速度误差、地心地固坐标系下的位置误差以及惯性器件测量误差为状态量的状态方程为：

以多普勒速度作为观测量的观测方程为：

步骤3所涉及的滤波方案为：

时间更新：

量测更新：

pk＝(i-kkhk)pk/k-1

式中，φk,k-1为一步转移阵，γk-1为系统噪声驱动阵，hk为量测阵，qk为系统噪声协方差阵，rk为量测噪声协方差阵，zk为当前时刻的量测值，和pk-1分别为上一时刻的状态量估计值和滤波协方差阵，和pk分别为当前时刻的状态量估计值和滤波协方差阵，该滤波方案的输出为包含了格网惯导系统的姿态误差、速度误差与位置误差的估计值。

本发明的有益效果在于：

本发明创新地将卡尔曼滤波技术与声学测速信息应用于极区格网惯导系统的误差抑制，可以在不破坏导航系统自主性的同时，消除格网惯导系统的振荡误差以及随时间累积的导航误差；本发明不同于传统基于阻尼网络的格网惯导误差抑制方法，可以更全面有效的抑制格网惯导系统导航误差，提高导航系统在极区工作的综合性能，保证长航时工作条件下的精度与可靠性，性能稳定且易于实现，因此本发明具有很高的工程应用价值。

附图说明

图1为本发明提出的格网惯导系统误差抑制方法的基本流程框图；

图2为导航系统姿态角误差曲线；

图3为导航系统速度误差曲线；

图4为导航系统位置误差曲线。

具体实施方式

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

本发明提出一种声学测速辅助的极区格网惯性导航误差抑制方法，其流程图如附图1所示，本发明方法的主要步骤如下：

(1)选取格网坐标系为导航坐标系，以惯性导航系统为主导航系统，引入多普勒计程仪作为速度测量器件提供载体速度；

(2)定义格网惯导系统主要导航参数误差，包括格网角误差δσ，姿态误差角φ＝[φxφyφz]^t，格网坐标系下的速度误差以及地心地固坐标系下的位置误差δr^e＝[δxδyδz]^t。

格网角误差δσ：

其中，位置误差可以表示为：

角速度误差为：

角速度误差为：

其中，

姿态更新四元数误差微分方程为：

其中，且

则格网系下的姿态误差方程可以表示为：

其中，cv_φ＝c4且cr_φ＝(c2c1+c3+c5+c6c1)cr2p。

建立格网惯导系统速度误差微分方程：

其中，cr_v＝(v^g×)(2c3cr2p+c5)+(v^g×)(2c2+c6)cr2σ,

设计算格网系g^c与理想格网系g之间，存在位置误差角δθ^g。计算格网坐标系可以通过理想格网坐标系的三次旋转得到，因此位置误差角δθ^g可以表示为：

则位置误差可以表示为：

其中，且

选取格网惯导系统的格网姿态误差、格网速度误差、地心地固坐标系下的位置误差以及惯性器件测量误差为状态量，选取多普勒速度作为观测量，建立卡尔曼滤波模型，其中，状态方程为：

观测方程为：

(3)滤波器获取格网惯导系统与多普勒计程仪数据后进行滤波估计，所涉及的滤波方案为：

时间更新：

量测更新：

pk＝(i-kkhk)pk/k-1

式中，φk,k-1为一步转移阵，γk-1为系统噪声驱动阵，hk为量测阵，qk为系统噪声协方差阵，rk为量测噪声协方差阵，zk为当前时刻的量测值，和pk-1分别为上一时刻的状态量估计值和滤波协方差阵，和pk分别为当前时刻的状态量估计值和滤波协方差阵，该滤波方案的输出为包含了格网惯导系统的姿态误差、速度误差与位置误差的估计值；

(4)利用步骤3得到的导航误差估计值对格网惯导系统的输出进行校正，使格网惯导系统导航误差得到抑制，连续实时的输出校正后的高精度载体姿态、速度和位置误差，直至导航任务结束。

为了验证本发明的合理性、可行性，基于visualstudio2010设计程序，进行仿真实验验证，仿真的方案、条件及结果如下所示：

1)仿真时间设置

仿真时长为4h，仿真步长为0.01s。

2)载体运动设置

初始纬度75°n，初始经度126°e。

模拟载体静基座条件下的工作状态，即载体无线运动和角运动。

3)误差参数设置

三个陀螺仪的常值漂移分别设置为0.0015°/h、0.0020°/h和0.0030°/h；加速度计的零偏设置为3×10^-5g；多普勒计程仪噪声为零均值白噪声。

4)仿真结果

依上述仿真条件，对所设计的捷联惯导系统精度性能进行仿真，图2、图3、图4分别为导航系统姿态、速度、位置误差曲线，每一个仿真图中包括了未进行误差抑制的格网惯导系统导航误差曲线与借助声学测速设备进行导航误差抑制后的导航误差曲线。由图2、图3与图4可知，通过采用基于声学测速信息的误差抑制技术，可以有效的抑制格网惯性导航系统姿态误差中的振荡误差与稳态误差。总体仿真结果表明，本发明中的基于声学测速信息的误差抑制技术，可以有效抑制格网惯导系统在极区工作时产生的振荡误差与积累误差，校正后的导航系统性能明显优于单独的格网惯导系统，导航误差均被限制在小范围内，有效抑制了误差发散，保证了长航时工作时的导航精度。

综合上述分析，得到如下分析结果：通过本发明提出的声学测速辅助的极区格网惯性导航误差抑制方法，可以有效抑制格网惯导系统的振荡误差与随时间积累误差，保证导航系统在极区长航时工作的导航性能。因此，本发明可以更为全面的提升导航性能，满足导航系统在极区长时间工作对可靠性与精度的应用需求。