CN103743395A - 一种惯性重力匹配组合导航系统中时间延迟的补偿方法 - Google Patents

Abstract

本发明公开了一种惯性重力匹配组合导航系统中时间延迟的补偿方法，包括以下几个步骤：步骤一，采集惯性导航系统输出的纬度

经度λ、航向ψ和速度V及重力仪测得的重力信号；步骤二，计算重力信号的厄特弗斯校正值，并对厄特弗斯校正值进行滤波处理；步骤三，确定重力信号的延迟时间；步骤四，利用基于重力等值线的匹配算法，获取重力信号相应时刻的载体位置；步骤五，建立卡尔曼滤波器模型；步骤六，将载体位置的经度和纬度作为观测量，利用卡尔曼滤波实时估计重力信号对应时间点的惯性导航系统误差，对惯导系统进行校正；步骤七，进行卡尔曼滤波多步预测出当前时刻的状态向量，完成时间延迟补偿。本法明具有补偿重力信号时间延迟、高导航精度的优点。

Description

一种惯性重力匹配组合导航系统中时间延迟的补偿方法

技术领域

本发明属于组合导航领域，尤其涉及具有补偿重力信号延迟功能的一种惯性重力匹配组合导航系统中时间延迟的补偿方法。

背景技术

上世纪80-90年代美国和前苏联便相继开始了研制战略水下潜器的无源导航辅助系统。最初的辅助方法是基于图形匹配，包括与海底地形图、磁场图的匹配，但由于需要用声纳测量海底轮廓，导致海底地形匹配的隐蔽性较弱；同时由于磁场变化复杂目前还难以真正运用到水下潜器导航中，因此重力信号和重力梯度数据成为水下潜器导航的主要无源信息资源。重力辅助导航具有精度高、隐蔽性强、自主性强等优点，是潜艇等水下航行器理想的水下辅助导航定位手段。

重力辅助导航的前提是获取高精度的重力异常值，而重力异常一般是通过海洋重力仪获取。目前高精度的海洋重力仪为了抑制垂直加速度和高频噪声的影响，采用了强阻尼加滤波的方法。这就导致当前获取的重力异常值并不是运载体所处位置的重力异常，而是一段时间前所处位置的重力异常值。如果在这样的条件下进行重力匹配，就会产生误差，甚至可能影响惯导系统的精度。

传统计算延迟时间的方法是从理论上进行分析，由于重力仪的内部传感器结构比较复杂，无法把所有导致重力异常信号延迟的因素都考虑进来，这就会造成延迟时间计算不准确。在滤波系统中，常用的处理测量数据延迟的一个直观的方法是：等延迟的测量数据到达数据处理单元后再从发生延迟的时刻到当前时刻重新进行滤波，也就是重复滤波法。这样的方法可以保证滤波性能，也比较简单，但是当测量数据延迟较大时会给系统带来极大的计算压力和数据存储的空间压力，最终会导致测量数据更多地出现延迟的情况。

发明内容

本发明的目的是提供一种具有高精度的惯性重力匹配组合导航系统中时间延迟的补偿方法。

一种惯性重力匹配组合导航系统中时间延迟的补偿方法，包括以下几个步骤：

步骤一，采集惯性导航系统输出的纬度经度λ、航向ψ和速度V及重力仪测得的重力信号；

步骤二，利用惯性导航系统输出的纬度

航向ψ和速度V计算重力信号的厄特弗斯校正值，并且对厄特弗斯校正值进行滤波处理；

其中，Δg_E为厄特弗斯校正值，R_e为纬度处的地球半径，w_ie为地球自转角速度，其值为7.29211×10^-5rad/s，h为重力仪所在的载体的深度，g′为重力仪所感受的重力，g为地球表面上的实际重力，

其中，μ＝GM＝398600.5×10⁹m³/s²为引力常数；

步骤三，确定重力信号的延迟时间T_d；将厄特弗斯校正值与重力信号放在同一坐标系中，重力信号的波谷的时间减去相邻的厄特弗斯校正值的波峰的时间，取平均值得到重力信号的延迟时间；

步骤四，利用基于重力等值线的匹配算法，获取重力信号相应时刻的重力仪所在的载体位置；步骤五，建立惯性重力匹配组合导航系统的状态方程，确定状态变量x(t)、状态转移矩阵F，并确定系统的过程噪声w(t)；

惯性重力匹配组合导航系统的状态变量x(t)由惯性导航系统误差构成，

其中，δλ是经度误差，

是纬度误差，δV_E是东向速度误差，δV_N北向速度误差，α,β,γ是初始的姿态误差角，▽＝[▽_x ▽_y]^T是加速度计零偏，ε＝[ε_x ε_y ε_z]^T是三个轴的陀螺常值漂移；

惯性导航系统误差为

其中，V_E为东向速度，V_N为北向速度；

惯性重力匹配组合导航系统的状态方程为

$\stackrel{\·}{x}\left(t\right)=\mathrm{Fx}\left(t\right)+\mathrm{Bw}\left(t\right)$

其中，Q(t)是协方差矩阵，w(t)为系统的过程噪声；

系统的过程噪声w(t)为

w(t)＝[0 0 w_ax w_ay w_gx w_gy w_gz 0 0 0 0 0]^T

其中,w_ax和w_ay是x轴和y轴的加速度计误差，w_gy和w_gz是x轴和y轴的陀螺随机漂移，w_ax、w_ay、w_gx、w_gy和w_gz均为零均值的高斯白噪声；

系统的状态转移矩阵F为

$F=\left[\begin{array}{ccc}{F}_{2\×4}& 0_{2\×3}& 0_{2\×5}\\ F_{5\×4}& F_{5\×3}& I_{5\×5}\\ 0_{5\×4}& 0_{5\×3}& 0_{5\×5}\end{array}\right]$

其中，0_5×4表示全零矩阵，F_2×4，F_5×4，F_5×3分别表示如下：

系统噪声矩阵B为

B＝I_12×12；

步骤六，确定将重力仪所在的载体位置的经度和纬度作为观测量，确定量测矩阵H、量测噪声v；利用卡尔曼滤波实时估计重力信号对应时间点的状态转移矩阵和惯性导航系统误差，对惯性导航系统进行校正；

惯性重力匹配组合导航系统的量测方程

其中，

为量测噪声，R(t)为方差，λ_c，

为惯性导航系统输出的经度和纬度，λ_g为重力匹配得到的经度和纬度；

量测矩阵H为

H＝[I_2×2 0_2×10]

离散化的惯性重力匹配组合导航系统方程为

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_k+{\mathrm{\Γ}}_k{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\},k\≥1$

其中，Φ_k,k-1为离散化的状态转移矩阵，Γ_k为系统噪声驱动矩阵；

卡尔曼滤波时间更新和量测更新方程为

X_k(i)|k(i-1)＝Φ_k(i),k(i-1)X_k(i-1)

X_k(i)＝X_k(i)|k(i-1)+K_k(i)[Z_k(i)-H_k(i)X_k(i)|k(i-1)]

$P_{k\left(i\right)}={\mathrm{\Φ}}_{k\left(i\right),k(i-1)}{P}_{k(i-1)}{\mathrm{\Φ}}_{k\left(i\right),k(i-1)}^T+{\mathrm{\Γ}}_{k(i-1)}{Q}_{k(i-1)}{\mathrm{\Γ}}_{k(i-1)}^T$

$K_{k\left(i\right)}=P_{k\left(i\right)|k(i-1)}{H}_{k\left(i\right)}^T{(H_{k\left(i\right)}{P}_{k\left(i\right)|k(i-1)}{H}_{k\left(i\right)}^T+R_{k\left(i\right)})}^{-1}$

$P_{k\left(i\right)}=\left({I-K}_{k\left(i\right)}{H}_{k\left(i\right)}\right)P_{k\left(i\right)|k(i-1)}{(I-K_{k\left(i\right)}{H}_{k\left(i\right)})}^T+K_{k\left(i\right)}{R}_{k\left(i\right)}{K}_{k\left(i\right)}^T;$

步骤七，在重力信号的延迟时间T_d内，没有量测信息输出，只进行卡尔曼滤波器的时间更新，利用计重力信号对应时间点的状态转移矩阵进行卡尔曼滤波多步预测，从而预测出当前时刻的状态向量，

当前时刻为m，将延迟时间之前的状态向量通过重力信号对应时间点的状态转移矩阵Φ_k,k-m得到当前时刻的状态向量

在T_d这段时间内，只进行卡尔曼滤波的时间更新，

X_k(m)|k(m-1)＝Φ_k(m),k(m-1)X_k(m-1)

$P_{k\left(m\right)}={\mathrm{\Φ}}_{k\left(m\right),k(m-1)}{P}_{k(m-1)}{\mathrm{\Φ}}_{k\left(m\right),k(m-1)}^T+{\mathrm{\Γ}}_{k(m-1)}{Q}_{k(m-1)}{\mathrm{\Γ}}_{k(m-1)}^T$

得到当前时刻的状态向量

后，再对当前时刻的惯性导航系统输出进行误差补偿，完成对惯性重力匹配组合导航系统信号的时间延迟补偿。

本发明一种惯性重力匹配组合导航系统中时间延迟的补偿方法，还可以包括：

对厄特弗斯校正值进行滤波处理采用小波理论中的Mallat算法，选取db9小波基函数，分解后选取Penalize Medium阈值并以软阈值的方法对重力信号降噪，各层的阈值参数为：Lev1:1051.002；Lev2:586.308；Lev3:65.569；Lev4:66.213；Lev5:53.099；Lev6:148.424；Lev7:29.860；Lev8:29.860；Lev8:29.860，最后对各层分解信号重构，得到降噪后的厄特弗斯校正值。

本发明的有益效果：

（1）本方法从实际出发，利用理论上厄特弗斯修正值的相位和幅值与重力仪观测值相吻合的特点，把重力仪观测值发生变化的时间减去厄特弗斯效应修正值变化的时间，得到重力仪输出重力异常值的延迟时间。这种方法得到的延迟时间已经包括了所有可能造成时间延迟的因素，因此具有良好的准确性。

（2）为保导航系统的实时性，采用非等间隔滤波的方法来解决时间延迟问题。卡尔曼滤波可以分为两个信息更新过程：时间更新和量测更新。在量测信息输出的时刻，同时进行卡尔曼滤波器的时间更新和量测更新；而在没有量测信息输出时，只进行时间更新。由于重力仪提供的重力信号不是当前时刻位置的重力值，而是延时时间T_d时刻之前的重力值。因此，在当前时刻之前的T_d这段时间内，此时只进行时间更新。这样不但可以减少滤波器的负担，提高运算效率，而且保证了惯导系统的精度。解决了惯性/重力匹配组合导航系统中重力信号延迟的问题。

（3）针对惯性/重力匹配组合导航系统，由于重力信号的延迟和图像匹配定位输出需要耗匹配计算时间，从而造成量测信息滞后。采用常规的Kalman滤波算法难以获得高的滤波精度，将会导致惯导系统的误差发散，偏离真实轨迹。利用Kalman滤波的递推矩阵把延迟时间段内的状态向量地推到当前时刻，并对惯导系统进行校正，可以保证信号的融合在时间点相对应，从而实现了对时间延迟的补偿。因此,该发明提出的算法具有较强的现实应用意义，并可以应用到其他组合导航系统中，用于量测滞后的补偿。

附图说明

图1是本发明的方法流程图；

图2是本发明提供的惯性重力匹配组合导航系统中时间延迟的补偿方法图；

图3是本发明提供重力信号延迟时间确定方法图；

图4是本发明提供的时间补偿前和补偿后的位置对比图。

具体实施方式

下面将结合附图对本法明和实施例作进一步说明。

本发明一种惯性/重力匹配组合导航系统中时间延迟的补偿方法，流程图如图1所示，包括以下几个步骤：

步骤一、采集惯性导航系统输出的纬度

经度λ、航向ψ和速度信息V及重力仪测得的重力信号；

具体为，每一组数据包括惯性导航系统采集的纬度经度λ、航向ψ、速度信息V和重力仪测得的重力信号，每秒记录1组数据，保存在一个文档中。在记录的时候要保证每组各个信号的时间是同步的。

步骤二、利用惯导系统输出的纬度

航向ψ和速度V计算厄特弗斯校正值。厄特弗斯修正值Δg_E的计算式为

其中，R_e近似作为纬度为

处的地球半径，w_ie为地球自转角速度，其值为7.29211×10^-5rad/s。为了保证厄特弗斯校正值在时间上与重力信号值一一对应，本方法使用小波理论中的Mallat算法对厄特弗斯校正值进行滤波处理；

具体为，由于重力是地球质量引力与地球自转所产生的离心力的合力，故利用海洋重力仪系统在运动载体上进行海洋重力动态测量时，重力仪除受到地球自转影响外，还受到载体速度所产生的附加离心力的影响，这种影响就是所谓的厄特弗斯效应。

利用惯导系统输出的航向、位置和速度信息对所测的重力信号进行厄特弗斯校正。当载体在自传地球表面运动时，离心力和科氏力对安装在载体上的重力仪所产生的影响成为厄特弗斯效应。假设载体的航向角为ψ，航速为V，水下载体的航行深度为h。则东向和北向的分速度为V_E＝Vsinψ,V_N＝Vcosψ。用R_e近似作为纬度为处的地球半径。东向分速度在地球自转的基础上增加了一个角速度，大小为

北向地速对应的角速度

产生附加的离心力

直接作用于重力方向。于是，安装在以航速V和航向ψ的载体上的重力仪所感受的重力g′为

其中，μ＝GM＝398600.5×10⁹m³/s²为引力常数，w_ie为地球自转角速度，其值为7.29211×10^-5rad/s。地球表面上的实际重力g为

于是得到厄特弗斯修正计算式

顾及h的厄特弗斯改正值与忽略h计算的厄特弗斯改正值相差很小，目前高精度重力仪的测量精度一般为±1～±2mGal，因此，上式可简化为:

利用惯导系统输出的航向、纬度和速度信息都不同程度上被噪声污染，实际计算出来的厄特弗斯校正值含有较强的噪声，如果直接对重力信号值进行校正，会污染重力信号。其次，在处理重力信号的滤波过程中,输出值是一段时间内数据的滤波后效果,采集的测量值不是某一单独点上的重力值,而是一段时间内平均重力值的反映,但是惯导的输出时对应每一单独点的值。因此前面提到的厄特弗斯校正值也都是单一时刻的，如果用这些值去校正重力信号，时间上不能做到完全对应，误差也就随之增大。选取同样的参数的滤波器对厄特弗斯校正值也进行滤波处理，不但可以消除惯导系统导致的噪声，还可以保证厄特弗斯校正值在时间上与重力信号值一一对应。

使用小波理论中的Mallat算法对厄特弗斯校正值进行分解，选取db9小波基函数，根据实验，在分解至第九层的时候滤波效果最好。分解后选取Penalize Medium阈值并以软阈值的方法对重力信号降噪，各层的阈值参数为：Lev1:1051.002；Lev2:586.308；Lev3:65.569；Lev4:66.213；Lev5:53.099；Lev6:148.424；Lev7:29.860；Lev8:29.860；Lev8:29.860。最后对各层分解信号重构，得到降噪后的重力信号。

步骤三、确定重力信号的延迟时间。使载体作较大的机动运动（航向变化至少为30°），重力仪观测值会产生较大的厄特弗斯效应。如果输出的重力信号不存在延迟，厄特弗斯修正值的相位和幅值应与重力仪观测值的相吻合。实际上重力信号时有一定延迟的，利用这一特点，根据厄特弗斯修正值与重力仪观测值的波峰波谷在时间上的不对应，用重力仪观测值发生变化的时间减去厄特弗斯效应修正值变化的时间，就是重力仪的延迟时间。

具体为，进行时间补偿前，要确定延迟时间。由于海洋重力测量中垂直加速度干扰较大，重力仪一般工作在强阻尼模式，因此，重力仪输出的重力变化会有一个滞后的过程。利用惯导系统输出计算的厄特弗斯效应修正值却没有延迟。将计算出来的厄特弗斯修正值与重力仪观测值画在同一坐标系中，由于厄特弗斯对重力测量影响较大，选取厄特弗斯修正值的波谷与重力仪观测值的波峰作为测量点，每组选相邻的可互补的波峰和波谷。用重力仪观测值波峰的时间减去厄特弗斯效应修正值波谷的时间，取平均值，就是重力信号的延迟时间。

步骤四、选择实时性较好的基于重力等值线的匹配算法，获取重力信号相应时刻的载体位置；

步骤五、根据惯性/重力匹配组合导航系统的特点，建立Kalman滤波器模型，确定状态变量X、状态转移矩阵F和量测矩阵H；并根据匹配算法的误差、惯性器件和重力仪的测量误差确定系统的过程噪声W和量测噪声V；

具体为，状态变量x(t)由导航系统误差构成，它包含导航误差和惯性器件误差。在该模型中。取惯性/重力匹配组合导航系统的状态向量x(t)为

其中▽＝[▽_x ▽_y]^T是加速度计零偏，ε＝[ε_x ε_y ε_z]^T是陀螺常值漂移。系统的状态方程可表示如下

$\stackrel{\·}{x}\left(t\right)=\mathrm{Fx}\left(t\right)+\mathrm{Bw}\left(t\right)$

w(t)是协方差矩阵为Q(t)的零均值白噪声。系统噪声w(t)为

w(t)＝[0 0 w_ax w_ay w_gx w_gy w_gz 0 0 0 0 0]^T

其中,w_ax、w_ay、w_gx、w_gy和w_gz均为零均值的高斯白噪声,w_ax和w_ay是加速度计误差，w_gy和w_gz是陀螺随机漂移。

惯导系统误差模型由平台姿态误差角模型、位置误差模型、速度误差模型及惯性器件误差模型组成，采用东北天导航坐标系的惯导误差模型为

其中，V_E为东向速度，V_N为北向速度。系统的状态转移矩阵F和系统噪声矩阵B分别为

$F=\left[\begin{array}{ccc}{F}_{2\×4}& 0_{2\×3}& 0_{2\×5}\\ F_{5\×4}& F_{5\×3}& I_{5\×5}\\ 0_{5\×4}& 0_{5\×3}& 0_{5\×5}\end{array}\right]{B=I}_{12\×12}$

其中，0_5×4表示全零矩阵，F_2×4，F_5×4，F_5×3分别表示如下：

步骤六、将载体位置的经度和纬度作为观测量，利用Kalman滤波实时估计重力信号对应时间点的惯导误差，对惯导进行校正；

具体为，重力匹配算法为组合导航系统提供位置信息,但位置信息含有一定的误差。选取载体的经纬度作为量测信息，则组合导航系统的量测方程为

其中，量测噪声为

其方差为R(t)。

λ_c为惯导系统输出的位置，

λ_g为重力匹配得到的位置。系统观测矩阵H为

H＝[I_2×2 0_2×10]

将系统的状态方程和量测方程离散化后可得惯性/重力匹配组合导航系统方程为

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_k+{\mathrm{\Γ}}_k{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\},k\≥1$

其中，Φ_k,k-1为状态转移矩阵，Γ_k为系统噪声驱动矩阵。相应的卡尔曼滤波时间更新和量测更新方程为

X_k(i)|k(i-1)＝Φ_k(i),k(i-1)X_k(i-1)

X_k(i)＝X_k(i)|k(i-1)+K_k(i)[Z_k(i)-H_k(i)X_k(i)|k(i-1)]

$P_{k\left(i\right)}={\mathrm{\Φ}}_{k\left(i\right),k(i-1)}{P}_{k(i-1)}{\mathrm{\Φ}}_{k\left(i\right),k(i-1)}^T+{\mathrm{\Γ}}_{k(i-1)}{Q}_{k(i-1)}{\mathrm{\Γ}}_{k(i-1)}^T$

$K_{k\left(i\right)}=P_{k\left(i\right)|k(i-1)}{H}_{k\left(i\right)}^T{(H_{k\left(i\right)}{P}_{k\left(i\right)|k(i-1)}{H}_{k\left(i\right)}^T+R_{k\left(i\right)})}^{-1}$

$P_{k\left(i\right)}=\left({I-K}_{k\left(i\right)}{H}_{k\left(i\right)}\right)P_{k\left(i\right)|k(i-1)}{(I-K_{k\left(i\right)}{H}_{k\left(i\right)})}^T+K_{k\left(i\right)}{R}_{k\left(i\right)}{K}_{k\left(i\right)}^T$

步骤七、利用状态转移矩阵Φ_k,k-m进行Kalman滤波多步预测，从而预测出当前时刻的状态向量，完成对惯性/重力匹配组合导航系统信号的时间延迟补偿。

具体为，由于重力仪提供的重力信号不是当前时刻位置的重力值，而是延迟时间T_d时刻之前的重力值。因此，在当前时刻之前的T_d这段时间内，是没有重力值提供给匹配算法的，如果把T_d时刻之前的量测信息应用到当前的状态更新中，将会导致校正息在时间上与惯导信息不匹配，这就可能导致惯导系统的误差发散。

常规一个滤波周期内的卡尔曼滤波可以分为两个信息更新过程:时间更新和量测更新。在没有量测信息输出时,只进行时间更新;而在量测信息输出的时刻,同时进行卡尔曼滤波器的时间更新和量测更新,利用此点可以解决惯性/重力匹配组合导航系统中重力信号延迟的滤波问题。

设当前时刻为m，延迟时间长为d。为了得到当前时刻的惯导系统输出，需要将滤波后的结果

通过Kalman滤波的状态转移矩阵Φ_m,m-d递推到当前时刻，在T_d这段时间内，相应的卡尔曼滤波时间更新方程为

X_k(m)|k(m-1)＝Φ_k(m),k(m-1)X_k(m-1)

$P_{k\left(m\right)}={\mathrm{\Φ}}_{k\left(m\right),k(m-1)}{P}_{k(m-1)}{\mathrm{\Φ}}_{k\left(m\right),k(m-1)}^T+{\mathrm{\Γ}}_{k(m-1)}{Q}_{k(m-1)}{\mathrm{\Γ}}_{k(m-1)}^T$

得到

后，再对当前时刻的惯导系统输出进行误差补偿。图2为惯性重力匹配组合导航系统中时间延迟的补偿方法图。

实施过程：根据附图3可以确定重力信号的延迟为36秒，匹配算法延迟为5秒，则延迟时间T_d＝41s。假设运载体以10节的速度匀速直线运动,航向为北偏东45",初始位置为初始位置为北纬45.77°和东经126.67°。假设惯导每隔5秒记一次位置,即每5秒状态更新一次。选取陀螺漂移为0.01°/h,加速度计零漂为1×10^-5g,初始姿态角分别为0.008°,0.008°,0.016°，惯导的经纬度初始误差0.00358",0.00054"，重力加速度取g＝9.8069,地球自转角速率ω_ie＝7.27220417rad/s。卡尔曼滤波器初值如下：

X0＝[0 0 0 0 0 0 0 0 0 0 0 0]^T

P0＝diag{(0.00358")²(0.00054")²(0.1m/s)²(0.1m/s)²(0.008°)²(0.008°)²(0.016°)²(0.0001g)²(0.01°/h)²(0.01°/h)²(0.01°/h)²(0.01°/h)²}

R＝diag{(0.1m)²,(0.1m)²}

在此仿真条件下，根据惯性导航系统的误差方程，得到航行5小时未经过时间延迟补偿的航迹和经过时间延迟补偿的轨迹与真实航迹的比较，如附图4所示。

从以上实施例不难看出，针对惯性/重力匹配组合导航系统，由于重力信号的延迟和图像匹配定位输出需要耗匹配计算时间，从而造成量测信息滞后。采用常规的Kalman滤波算法难以获得高的滤波精度，将会导致惯导系统的误差发散，偏离真实轨迹。利用Kalman滤波的递推矩阵把延迟时间段内的状态向量地推到当前时刻，并对惯导系统进行校正，可以保证信号的融合在时间点相对应，从而实现了对时间延迟的补偿。因此,该发明提出的算法具有较强的现实应用意义，并可以应用到其他组合导航系统中，用于量测滞后的补偿。

Claims (2)

1.一种惯性重力匹配组合导航系统中时间延迟的补偿方法，其特征在于：包括以下几个步骤，

步骤一，采集惯性导航系统输出的纬度

经度λ、航向ψ和速度V及重力仪测得的重力信号；

步骤二，利用惯性导航系统输出的纬度

航向ψ和速度V计算重力信号的厄特弗斯校正值，并且对厄特弗斯校正值进行滤波处理；

其中，Δg_E为厄特弗斯校正值，R_e为纬度处的地球半径，w_ie为地球自转角速度，其值为7.29211×10^-5rad/s，h为重力仪所在的载体的深度，g′为重力仪所感受的重力，g为地球表面上的实际重力，

其中，μ＝GM＝398600.5×10⁹m³/s²为引力常数；

步骤三，确定重力信号的延迟时间T_d；将厄特弗斯校正值与重力信号放在同一坐标系中，重力信号的波谷的时间减去相邻的厄特弗斯校正值的波峰的时间，取平均值得到重力信号的延迟时间；

步骤四，利用基于重力等值线的匹配算法，获取重力信号相应时刻的重力仪所在的载体位置；

步骤五，建立惯性重力匹配组合导航系统的状态方程，确定状态变量x(t)、状态转移矩阵F，并确定系统的过程噪声w(t)；

惯性重力匹配组合导航系统的状态变量x(t)由惯性导航系统误差构成，

其中，δλ是经度误差，

是纬度误差，δV_E是东向速度误差，δV_N北向速度误差，α,β,γ是初始的姿态误差角，▽＝[▽_x ▽_y]^T是加速度计零偏，ε＝[ε_x ε_y ε_z]^T是三个轴的陀螺常值漂移；

惯性导航系统误差为

其中，V_E为东向速度，V_N为北向速度；

惯性重力匹配组合导航系统的状态方程为

$\stackrel{\·}{x}\left(t\right)=\mathrm{Fx}\left(t\right)+\mathrm{Bw}\left(t\right)$

其中，Q(t)是协方差矩阵，w(t)为系统的过程噪声；

系统的过程噪声w(t)为

w(t)＝[0 0 w_ax w_ay w_gx w_gy w_gz 0 0 0 0 0]^T

其中,w_ax和w_ay是x轴和y轴的加速度计误差，w_gy和w_gz是x轴和y轴的陀螺随机漂移，w_ax、w_ay、w_gx、w_gy和w_gz均为零均值的高斯白噪声；

系统的状态转移矩阵F为

$F=\left[\begin{array}{ccc}{F}_{2\×4}& 0_{2\×3}& 0_{2\×5}\\ F_{5\×4}& F_{5\×3}& I_{5\×5}\\ 0_{5\×4}& 0_{5\×3}& 0_{5\×5}\end{array}\right]$

其中，0_5×4表示全零矩阵，F_2×4，F_5×4，F_5×3分别表示如下：

系统噪声矩阵B为

B＝I_12×12；

步骤六，确定将重力仪所在的载体位置的经度和纬度作为观测量，确定量测矩阵H、量测噪声v；利用卡尔曼滤波实时估计重力信号对应时间点的状态转移矩阵和惯性导航系统误差，对惯性导航系统进行校正；

惯性重力匹配组合导航系统的量测方程

其中，

为量测噪声，R(t)为方差，λ_c，

为惯性导航系统输出的经度和纬度，

λ_g为重力匹配得到的经度和纬度；

量测矩阵H为

H＝[I_2×2 0_2×10]

离散化的惯性重力匹配组合导航系统方程为

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_k+{\mathrm{\Γ}}_k{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\},k\≥1$

其中，Φ_k,k-1为离散化的状态转移矩阵，Γ_k为系统噪声驱动矩阵；

卡尔曼滤波时间更新和量测更新方程为

X_k(i)|k(i-1)＝Φ_k(i),k(i-1)X_k(i-1)

X_k(i)＝X_k(i)|k(i-1)+K_k(i)[Z_k(i)-H_k(i)X_k(i)|k(i-1)]

$P_{k\left(i\right)}={\mathrm{\Φ}}_{k\left(i\right),k(i-1)}{P}_{k(i-1)}{\mathrm{\Φ}}_{k\left(i\right),k(i-1)}^T+{\mathrm{\Γ}}_{k(i-1)}{Q}_{k(i-1)}{\mathrm{\Γ}}_{k(i-1)}^T$

$K_{k\left(i\right)}=P_{k\left(i\right)|k(i-1)}{H}_{k\left(i\right)}^T{(H_{k\left(i\right)}{P}_{k\left(i\right)|k(i-1)}{H}_{k\left(i\right)}^T+R_{k\left(i\right)})}^{-1}$

$P_{k\left(i\right)}=\left({I-K}_{k\left(i\right)}{H}_{k\left(i\right)}\right)P_{k\left(i\right)|k(i-1)}{(I-K_{k\left(i\right)}{H}_{k\left(i\right)})}^T+K_{k\left(i\right)}{R}_{k\left(i\right)}{K}_{k\left(i\right)}^T;$

步骤七，在重力信号的延迟时间T_d内，没有量测信息输出，只进行卡尔曼滤波器的时间更新，利用计重力信号对应时间点的状态转移矩阵进行卡尔曼滤波多步预测，从而预测出当前时刻的状态向量，

当前时刻为m，将延迟时间之前的状态向量

通过重力信号对应时间点的状态转移矩阵Φ_k，k-m得到当前时刻的状态向量

在T_d这段时间内，只进行卡尔曼滤波的时间更新，

X_k(m)|k(m-1)＝Φ_k(m),k(m-1)X_k(m-1)

$P_{k\left(m\right)}={\mathrm{\Φ}}_{k\left(m\right),k(m-1)}{P}_{k(m-1)}{\mathrm{\Φ}}_{k\left(m\right),k(m-1)}^T+{\mathrm{\Γ}}_{k(m-1)}{Q}_{k(m-1)}{\mathrm{\Γ}}_{k(m-1)}^T$

得到当前时刻的状态向量

后，再对当前时刻的惯性导航系统输出进行误差补偿，完成对惯性重力匹配组合导航系统信号的时间延迟补偿。

2.根据权利要求1所述的一种惯性重力匹配组合导航系统中时间延迟的补偿方法，其特征在于：所述的对厄特弗斯校正值进行滤波处理采用小波理论中的Mallat算法，选取db9小波基函数，分解后选取Penalize Medium阈值并以软阈值的方法对重力信号降噪，各层的阈值参数为：Lev1:1051.002；Lev2:586.308；Lev3:65.569；Lev4:66.213；Lev5:53.099；Lev6:148.424；Lev7:29.860；Lev8:29.860；Lev8:29.860，最后对各层分解信号重构，得到降噪后的厄特弗斯校正值。

Images