CN110207694A - 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法 - Google Patents

Abstract

本发明提供一种基于相对位置信息的极区格网惯导/超短基线组合导航方法，以惯导系统作为基础导航设备，采用格网坐标系作为极区导航坐标系，引入超短基线定位系统测量运载体位置，基于卡尔曼滤波器，基于相对位置信息设计极区格网惯导/超短基线组合导航方法，实时、连续提供高精度导航信息。该方法不仅克服了极区经线收敛导致的误差放大，且在不破坏导航系统自主性的前提下，能够有效抑制惯导系统导航误差，提高导航精度，更好的保证了导航系统的可靠性，为极区运载器提供高精度导航信息。

Description

一种基于相对位置信息的极区格网惯导/超短基线组合导航 方法

技术领域

本发明涉及一种基于格网坐标系的极区组合导航方法，尤其涉及一种基于相对位置信息的极区格网惯导/超短基线组合导航方法。

背景技术

高精度导航技术是运载器极区工作与安全航行的重要前提。为了满足运载器极区导航信息全面性与连续性，通常采用格网惯导系统作为基础导航设备。格网惯导系统受到自身工作原理的制约，输出的导航信息中含有周期性振荡误差与随时间积累的稳态误差，为了获得更高精度的导航信息，格网惯导/多普勒组合导航技术并应用于极区导航。格网惯导/多普勒组合导航系统可以有效抑制周期性振荡误差，但位置误差仍然随时间积累，因此，为了有效抑制随时间积累的定位误差，格网惯导/超短基线组合导航方法可以通过采用相对位置信息进行滤波估计，提高导航精度，满足载体高精度定位需求，具有重要的工程应用价值。

惯导/超短基线组合导航技术已经在非极区得到广泛应用，但应用与中低纬度的惯导/超短基线组合导航系统以指北地理坐标系为导航坐标系，当工作于极区时，会因为经线收敛引发定位定向难题，无法应用于极区。此外，传统惯导/超短基线组合导航技术以经纬度作为观测信息，而经线收敛也会引起定位困难，在南北极点，经度已经无法描述载体位置坐标。综上所述，现有的惯导/超短基线组合导航方法受到极区特殊地理位置的限制，无法得到应用。

发明内容

本发明的目的是为了提供一种基于相对位置信息的极区格网惯导/超短基线组合导航方法，实现极区运载器的高精度导航。

本发明的目的是这样实现的：包括如下步骤：

步骤一：选取格网坐标系为导航坐标系，将格网惯导系统安装至载体并启动预热；

步骤二：惯性导航系统为主导航系统，引入超短基线定位系统，构建组合导航设备；

步骤三：超短基线定位设备包括安装在载体上的水听器声学测量基阵以及已知位置的声信标，水听器声学测量基阵构成基阵坐标系，超短基线定位系统可以输出测量基阵与声信标之间的相对位置信息，包括俯仰角α、方位角β以及斜距R；

步骤四：格网惯导系统通过陀螺仪与加速度计测量运载体的线运动与角运动信息，输出格网坐标系下的运载体姿态、速度信息以及地心地固坐标系下的载体位置坐标R^e＝[x y z]^T，以格网惯导输出信息解算测量基阵与声信标之间的相对位置信息[α_S β_S R_S]；

步骤五：设计格网惯性导航系统状态模型；

步骤六：设计超短基线定位系统状态模型，以超短基线定位系统相对位置信息误差[δα_U δβ_U]^T、测距因子误差δK_U以及超短基线安装误差ψ_U为超短基线状态量：x_USBL＝[δα_U δβ_U δK_U ψ_U]^T；超短基线定位系统状态模型为：

其中，F_USBL＝diag(-1/τ_α -1/τ_β 0 0 0 0)为超短基线系统转移矩阵，τ_α、τ_β为一阶马尔科夫过程相关时间，B_USBL＝[I_2×2 0_4×2]^T为噪声转移矩阵；

步骤七：基于相对位置信息，建立格网惯导/超短基线组合导航系统模型：

其中，系统状态量为：X＝[x_SINS x_DVL x_USBL]^T，系统转移矩阵为：系统噪声转移矩阵为：

步骤八：以[α_S β_S R_S]表示惯导系统输出信息解算得到的相对位置信息，[α_U β_UR_U]表示超短基线定位系统输出的相对位置信息；以相对位置信息为量测信息，建立量测模型；

步骤九：滤波器获取格网惯导系统与多普勒计程仪数据后进行滤波估计，

步骤十：利用步骤九得到的导航误差估计值对格网惯导系统进行校正，格网惯导/超短基线组合导航系统连续实时的输出校正后的高精度载体姿态、速度和位置误差，直至极区导航任务结束。

本发明还包括这样一些结构特征：

1.步骤四具体包括：以表示格网惯导输出信息解算得到的超短基线水听器基阵坐标系下声信标r的位置坐标：

其中，b表示载体坐标系，u表示超短基线水听器基阵坐标系，G表示格网坐标系，g表示地理坐标系，e表示地心地固坐标系，表示b系到u系的坐标转换矩阵，表示G系到b系的坐标转换矩阵，表示g系到G系的坐标转换矩阵，表示e系到g系的坐标转换矩阵，由惯性导航系统更新并提供；已知位置的声信标在地心地固坐标系下的位置坐标，由格网惯导系统解算得到的载体坐标系在地心地固坐标系下的位置坐标，与是载体坐标系与基阵坐标系之间的转换矩阵与杆臂信息，由设备使用前的标定实验提供；

由格网惯导输出信息解算得到的基阵与声信标之间的相对位置信息[α_S β_S R_S]为：

2.步骤五具体包括：定义格网惯导系统主要导航参数误差，包括格网角误差δσ，姿态误差角φ＝[φ_x φ_y φ_z]^T，格网坐标系下的速度误差以经纬高表示的位置误差以及地心地固坐标系下的位置误差δR^e＝[δx δy δz]^T，以姿态角误差、速度误差、位置误差、陀螺仪常值漂移ε^b以及加速度计零偏▽^b为格网惯性系统状态量：x_SINS＝[φ δV^G δR^e ε^b ▽^b]^T，设计格网惯导系统状态模型：

格网角误差δσ：

其中，为载体经度、纬度、高度位置信息，δP可以表示为：

其中，R_eh＝R_e+h，R_e为地球半径；

i表示惯性坐标系，格网系下地球自转角速度误差为：

角速度误差为：

其中，

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

其中，且 为陀螺量测误差；

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

其中，C_{V_φ}＝C₄且C_{R_φ}＝(C₂C₁+C₃+C₅+C₆C₁)C_R2P；

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

其中，C_{R_V}＝(V^G×)(2C₃C_R2P+C₅)+(V^G×)(2C₂+C₆)C_R2σ,δf^b为加速度计量测误差；

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

则位置误差可以表示为：

其中，且

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

格网惯导系统状态方程可以表示为：

其中，F_SINS为格网惯导系统转移矩阵，B_SINS为格网惯性系统噪声转移矩阵。

3.步骤八中的建立量测模型为：

其中：v_USBL是超短基线的量测噪声， 由设备使用前的实验室标定实验提供，与由格网惯导系统提供。

4.步骤九所涉及的滤波方案为：时间更新：

量测更新：

P_k＝(I-K_kH_k)P_k/k-1

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

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

附图说明

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

图2为超短基线声信标与声学基阵之间相对位置信息示意图；

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

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

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

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

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

(1)选取格网坐标系为导航坐标系，将格网惯导系统安装至载体并启动预热；

(2)惯性导航系统为主导航系统，引入超短基线定位系统，构建组合导航设备；

(3)超短基线定位设备包括安装在载体上的水听器声学测量基阵，以及已知位置的声信标。水听器声学测量基阵构成基阵坐标系。超短基线定位系统可以输出测量基阵与声信标之间的相对位置信息，包括俯仰角α、方位角β以及斜距R。

(4)格网惯导系统通过陀螺仪与加速度计测量运载体的线运动与角运动信息，输出格网坐标系下的运载体姿态、速度信息以及地心地固坐标系下的载体位置坐标R^e＝[x y z]^T。

以表示格网惯导输出信息解算得到的超短基线水听器基阵坐标系下声信标r的位置坐标：

其中，b表示载体坐标系，u表示超短基线水听器基阵坐标系，G表示格网坐标系，g表示地理坐标系，e表示地心地固坐标系，表示b系到u系的坐标转换矩阵，表示G系到b系的坐标转换矩阵，表示g系到G系的坐标转换矩阵，表示e系到g系的坐标转换矩阵，由惯性导航系统更新并提供；已知位置的声信标在地心地固坐标系下的位置坐标，由格网惯导系统解算得到的载体坐标系在地心地固坐标系下的位置坐标，与是载体坐标系与基阵坐标系之间的转换矩阵与杆臂信息，由设备使用前的标定实验提供。

由格网惯导输出信息解算得到的基阵与声信标之间的相对位置信息[α_Sβ_SR_S]：

(5)设计格网惯性导航系统状态模型。定义格网惯导系统主要导航参数误差，包括格网角误差δσ，姿态误差角φ＝[φ_x φ_y φ_z]^T，格网坐标系下的速度误差以经纬高表示的位置误差以及地心地固坐标系下的位置误差δR^e＝[δx δy δz]^T，以姿态角误差、速度误差、位置误差、陀螺仪常值漂移ε^b以及加速度计零偏▽^b为格网惯性系统状态量：x_SINS＝[φ δV^G δR^e ε^b ▽^b]^T，设计格网惯导系统状态模型：

格网角误差δσ：

其中，为载体经度、纬度、高度位置信息，δP可以表示为：

其中，R_eh＝R_e+h，R_e为地球半径。

i表示惯性坐标系，格网系下地球自转角速度误差为：

角速度误差为：

其中，

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

其中，且 为陀螺量测误差。

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

其中，C_{V_φ}＝C₄且C_{R_φ}＝(C₂C₁+C₃+C₅+C₆C₁)C_R2P；

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

其中，C_{R_V}＝(V^G×)(2C₃C_R2P+C₅)+(V^G×)(2C₂+C₆)C_R2σ,δf^b为加速度计量测误差。

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

则位置误差可以表示为：

其中，且

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

格网惯导系统状态方程可以表示为：

其中，F_SINS为格网惯导系统转移矩阵，B_SINS为格网惯性系统噪声转移矩阵。

(6)设计超短基线定位系统状态模型，以超短基线定位系统相对位置信息误差[δα_U δβ_U]^T、测距因子误差δK_U以及超短基线安装误差ψ_U为超短基线状态量：x_USBL＝[δα_U δβ_U δK_U ψ_U]^T。超短基线定位系统状态模型为：

其中，F_USBL＝diag(-1/τ_α -1/τ_β 0 0 0 0)为超短基线系统转移矩阵，τ_α、τ_β为一阶马尔科夫过程相关时间，B_USBL＝[I_2×2 0_4×2]^T为噪声转移矩阵。

(7)基于相对位置信息，建立格网惯导/超短基线组合导航系统模型：

其中，系统状态量为：X＝[x_SINS x_DVL x_USBL]^T，系统转移矩阵为：系统噪声转移矩阵为：

(8)以[α_S β_S R_S]表示惯导系统输出信息解算得到的相对位置信息，[α_U β_U R_U]表示超短基线定位系统输出的相对位置信息。以相对位置信息为量测信息，建立量测模型。建立量测模型：

其中v_USBL是超短基线的量测噪声。 由设备使用前的实验室标定实验提供，与由格网惯导系统提供。

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

量测更新：

P_k＝(I-K_kH_k)P_k/k-1

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

(10)利用步骤(9)得到的导航误差估计值对格网惯导系统进行校正，格网惯导/超短基线组合导航系统连续实时的输出校正后的高精度载体姿态、速度和位置误差，直至极区导航任务结束。

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

(1)仿真时间设置

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

(2)载体运动设置

初始纬度75°N，初始经度126°E。

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

(3)误差参数设置

三个陀螺仪的常值漂移分别设置为0.03°/h、0.03°/h和0.03°/h；加速度计的零偏设置为3×10^-5g；超短基线定位系统测距精度为10米，测向精度为1°，噪声为零均值白噪声。

(4)仿真结果

依上述仿真条件，对所设计的组合导航系统精度性能进行仿真，图3、图4、图5分别为格网惯导/超短基线组合导航系统姿态、速度、位置误差曲线，每一个仿真图中包括了格网惯导系统导航误差曲线与格网惯导/超短基线组合导航系统误差曲线。由图3、图4与图5可知，格网惯导系统导航误差曲线包含振荡误差与随时间积累的稳态误差；格网惯导/超短基线组合导航系统通过引入相对位置观测信息，构成组合导航滤波器，对格网惯导系统的导航误差进行了有效估计与抑制，可以实现高精度极区导航。

最终仿真结果表明，本发明中的格网惯导/超短基线组合导航技术，可以有效抑制格网惯导系统在极区工作时产生的振荡误差与积累误差，具有更高的导航精度，导航误差均被限制在小范围内，有效抑制了误差发散，保证了长航时工作时的导航精度。

综合上述分析，得到如下分析结果：通过本发明提出的基于相对位置信息的极区格网惯导/超短基线组合导航方法，可以有效抑制格网惯导系统的振荡误差与随时间积累的定位误差，保证导航系统在极区长航时工作的导航性能。因此，本发明可以更为全面的提升导航性能，满足导航系统在极区长时间工作对可靠性与精度的应用需求。

综上，本发明公开了一种基于相对位置信息的极区格网惯导/超短基线组合导航方法。该方法以惯导系统作为基础导航设备，采用格网坐标系作为极区导航坐标系，引入超短基线定位系统测量运载体位置，基于卡尔曼滤波器，基于相对位置信息设计极区格网惯导/超短基线组合导航方法，实时、连续提供高精度导航信息。该方法不仅克服了极区经线收敛导致的误差放大，且在不破坏导航系统自主性的前提下，能够有效抑制惯导系统导航误差，提高导航精度，更好的保证了导航系统的可靠性，为极区运载器提供高精度导航信息。

Claims (5)

1.一种基于相对位置信息的极区格网惯导/超短基线组合导航方法，其特征在于：包括如下步骤：

步骤一：选取格网坐标系为导航坐标系，将格网惯导系统安装至载体并启动预热；

步骤二：惯性导航系统为主导航系统，引入超短基线定位系统，构建组合导航设备；

步骤三：超短基线定位设备包括安装在载体上的水听器声学测量基阵以及已知位置的声信标，水听器声学测量基阵构成基阵坐标系，超短基线定位系统可以输出测量基阵与声信标之间的相对位置信息，包括俯仰角α、方位角β以及斜距R；

步骤四：格网惯导系统通过陀螺仪与加速度计测量运载体的线运动与角运动信息，输出格网坐标系下的运载体姿态、速度信息以及地心地固坐标系下的载体位置坐标R^e＝[x y z]^T，以格网惯导输出信息解算测量基阵与声信标之间的相对位置信息[α_S β_S R_S]；

步骤五：设计格网惯性导航系统状态模型；

步骤六：设计超短基线定位系统状态模型，以超短基线定位系统相对位置信息误差[δα_Uδβ_U]^T、测距因子误差δK_U以及超短基线安装误差ψ_U为超短基线状态量：x_USBL＝[δα_U δβ_U δK_Uψ_U]^T；超短基线定位系统状态模型为：

其中，F_USBL＝diag(-1/τ_α -1/τ_β 0 0 0 0)为超短基线系统转移矩阵，τ_α、τ_β为一阶马尔科夫过程相关时间，B_USBL＝[I_2×2 0_4×2]^T为噪声转移矩阵；

步骤七：基于相对位置信息，建立格网惯导/超短基线组合导航系统模型：

其中，系统状态量为：X＝[x_SINS x_DVL x_USBL]^T，系统转移矩阵为：系统噪声转移矩阵为：

步骤八：以[α_S β_S R_S]表示惯导系统输出信息解算得到的相对位置信息，[α_U β_U R_U]表示超短基线定位系统输出的相对位置信息；

步骤九：滤波器获取格网惯导系统与多普勒计程仪数据后进行滤波估计，

步骤十：利用步骤九得到的导航误差估计值对格网惯导系统进行校正，格网惯导/超短基线组合导航系统连续实时的输出校正后的高精度载体姿态、速度和位置误差，直至极区导航任务结束。

2.根据权利要求1所述的一种基于相对位置信息的极区格网惯导/超短基线组合导航方法，其特征在于：步骤四具体包括：以表示格网惯导输出信息解算得到的超短基线水听器基阵坐标系下声信标r的位置坐标：

其中，b表示载体坐标系，u表示超短基线水听器基阵坐标系，G表示格网坐标系，g表示地理坐标系，e表示地心地固坐标系，表示b系到u系的坐标转换矩阵，表示G系到b系的坐标转换矩阵，表示g系到G系的坐标转换矩阵，表示e系到g系的坐标转换矩阵，由惯性导航系统更新并提供；已知位置的声信标在地心地固坐标系下的位置坐标，由格网惯导系统解算得到的载体坐标系在地心地固坐标系下的位置坐标，与是载体坐标系与基阵坐标系之间的转换矩阵与杆臂信息，由设备使用前的标定实验提供；

由格网惯导输出信息解算得到的基阵与声信标之间的相对位置信息[α_S β_S R_S]为：

3.根据权利要求2所述的一种基于相对位置信息的极区格网惯导/超短基线组合导航方法，其特征在于：步骤五具体包括：定义格网惯导系统主要导航参数误差，包括格网角误差δσ，姿态误差角φ＝[φ_x φ_y φ_z]^T，格网坐标系下的速度误差以经纬高表示的位置误差以及地心地固坐标系下的位置误差δR^e＝[δx δyδz]^T，以姿态角误差、速度误差、位置误差、陀螺仪常值漂移ε^b以及加速度计零偏为格网惯性系统状态量：设计格网惯导系统状态模型：

格网角误差δσ：

其中，为载体经度、纬度、高度位置信息，δP可以表示为：

其中，R_eh＝R_e+h，R_e为地球半径；

i表示惯性坐标系，格网系下地球自转角速度误差为：

角速度误差为：

其中，

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

其中，且 为陀螺量测误差；

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

其中，C_{V_φ}＝C₄且C_{R_φ}＝(C₂C₁+C₃+C₅+C₆C₁)C_R2P；

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

其中，C_{R_V}＝(V^G×)(2C₃C_R2P+C₅)+(V^G×)(2C₂+C₆)C_R2σ,δf^b为加速度计量测误差；

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

则位置误差可以表示为：

其中，且

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

格网惯导系统状态方程可以表示为：

其中，F_SINS为格网惯导系统转移矩阵，B_SINS为格网惯性系统噪声转移矩阵。

4.根据权利要求3所述的一种基于相对位置信息的极区格网惯导/超短基线组合导航方法，其特征在于：步骤八中的建立量测模型为：

其中：v_USBL是超短基线的量测噪声，

由设备使用前的实验室标定实验提供，与由格网惯导系统提供。

5.根据权利要求4所述的一种基于相对位置信息的极区格网惯导/超短基线组合导航方法，其特征在于：步骤九所涉及的滤波方案为：时间更新：

量测更新：

P_k＝(I-K_kH_k)P_k/k-1

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