一种极区格网惯导/超短基线紧组合导航方法与流程

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

背景技术：

高精度导航技术是运载器在极区工作与安全航行的重要前提，惯性导航系统以其自主性、隐蔽性与信息完备性成为海洋运载器必备导航设备之一，然而，极区运载器的惯性导航系统面对两大难题，一是经线收敛导致的定位定向难题，二是惯导系统自身含有周期性振荡误差以及随时间积累的导航误差。为了解决经线收敛导致的定位定向问题，以格网坐标系为导航坐标系的格网惯导系统被应用于极区运载器。然而格网惯导系统受到工作原理的制约，输出仍然包含振荡误差与随时间积累的稳态误差。在中低纬度地区，惯导/超短基线组合导航技术得到了广泛应用，通过设计卡尔曼滤波器，实现了惯导系统误差实时估计与校正，与惯导系统相比导航性能得到了有效提升。然而，现有的惯导/超短基线组合导航技术以指北坐标系为导航坐标系，且以经纬度为观测信息构建滤波器，受到极区特殊地理位置的限制，南北极近极点处经线急剧收敛，指北导航系统存在航向与经度误差放大现象，无法得到应用。综上所述，现有的导航方法无法适应极区特殊应用环境，不能满足极区运载器对导航性能的要求。

技术实现要素：

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

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

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

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

步骤三：超短基线定位设备包括安装在载体上的水听器声学测量基阵，以及已知位置的声信标；水听器声学测量基阵构成基阵坐标系；超短基线定位系统通过测量声学基阵与声信标之间的声信号传播时间t、以及声信号经过声学基阵相邻两水听器时的相位差获得载体位置信息；

步骤四：格网惯导系统通过陀螺仪与加速度计测量运载体的线运动与角运动信息，输出格网坐标系下的运载体姿态、速度信息以及地心地固坐标系下的载体位置坐标r^e＝[xyz]^t，以格网惯导输出信息解算声信号经过声学基阵相邻两水听器时的相位差、声学基阵与声信标之间的声信号传播时间：

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

其中，fsins为格网惯导系统转移矩阵，bsins为格网惯性系统噪声转移矩阵；

步骤六：设计超短基线定位系统状态模型：

以超短基线定位系统相位差误差传播时间测量误差δt为超短基线状态量：则超短基线定位系统状态模型为：

其中，为超短基线系统转移矩阵，τ为一阶马尔科夫过程相关时间，busbl＝i3×3为噪声转移矩阵；

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

其中，系统状态量为：x＝[xsinsxusbl]^t，系统转移矩阵为：系统噪声转移矩阵为：

步骤八：以表示惯导系统输出的相对位置信息，表示超短基线定位系统输出的相对位置信息，以相对位置信息为量测信息，建立量测模型为：

其中：vusbl是超短基线的量测噪声；

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

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

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

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

1.步骤三中的信号传播时间t、以及相位差与相对位置信息之间的关系可以表示为：

其中，[x^uy^uz^u]^t为超短基线基阵坐标系下声信标位置坐标，r为基阵与声信标之间的斜距，d为相邻两水听器之间的安装距离，λ为水下声波波长，c为水下声速，标度因数

2.步骤四具体为：

以格网惯导输出信息解算基阵与声信标之间的相对位置信息，超短基线基阵坐标系下声信标位置坐标：

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

由格网惯导解算得到的声信号相位差与传播时间可以表示为

3.步骤五具体为：

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

格网角误差δσ：

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

其中，rnh＝rn+h，rn为地球半径；

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

角速度误差为：

其中，

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

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

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

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

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

其中，cr_v＝(v^g×)(2c3cr2p+c5)+(v^g×)(2c2+c6)cr2σ，δf^b为加速度计量测误差；

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

则位置误差可以表示为：

其中，且

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

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

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

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

量测更新：

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

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

与现有技术相比，本发明的有益效果是：本发明创新性的将超短基线定位系统更加原始的量测信息作为观测量，提出了可应用于极区的格网惯导/超短基线深组合技术，并有效提升了格网惯性导航系统的精度；本发明不同于传统的极区导航算法或惯导/超短基线组合导航算法，有效克服了极区经线收敛引起的定位定向困难，采用超短基线更为原始的测量信息为观测信息，提升了导航系统在极区工作的综合性能，保证长航时工作条件下的精度与可靠性，性能稳定且易于实现，因此本发明具有很高的工程应用价值。

附图说明

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

图2为超短基线定位系统水听器基阵与已知位置声信标安装示意图；

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

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

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

具体实施方式

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

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

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

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

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

(3)超短基线定位设备包括安装在载体上的水听器声学测量基阵，以及已知位置的声信标。水听器声学测量基阵构成基阵坐标系。超短基线定位系统通过测量声学基阵与声信标之间的声信号传播时间t、以及声信号经过声学基阵相邻两水听器时的相位差获得载体位置信息。

信号传播时间t、以及相位差与相对位置信息之间的关系可以表示为：

其中，[x^uy^uz^u]^t为超短基线基阵坐标系下声信标位置坐标，r为基阵与声信标之间的斜距，d为相邻两水听器之间的安装距离，λ为水下声波波长，c为水下声速，标度因数

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

以格网惯导输出信息解算基阵与声信标之间的相对位置信息，超短基线基阵坐标系下声信标位置坐标：

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

由格网惯导解算得到的声信号相位差与传播时间可以表示为

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

格网角误差δσ：

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

其中，rnh＝rn+h，rn为地球半径。

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

角速度误差为：

其中，

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

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

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

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

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

其中，cr_v＝(v^g×)(2c3cr2p+c5)+(v^g×)(2c2+c6)cr2σ，δf^b为加速度计量测误差。

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

则位置误差可以表示为：

其中，且

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

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

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

(6)设计超短基线定位系统状态模型，以超短基线定位系统相位差误差传播时间测量误差δt为超短基线状态量：超短基线定位系统状态模型为：

其中，为超短基线系统转移矩阵，为一阶马尔科夫过程相关时间，busbl＝i3×3为噪声转移矩阵。

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

其中，系统状态量为：x＝[xsinsxusbl]^t，系统转移矩阵为：系统噪声转移矩阵为：

(8)以表示惯导系统输出信息解算得到的声信号相位差及传播时间，表示超短基线定位系统输出的声信号相位差及传播时间。以声信号相位差及传播时间为量测信息，建立量测模型。建立量测模型：

其中vusbl是超短基线的量测噪声。

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

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

量测更新：

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

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

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

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

(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-5可知，格网惯导/超短基线紧组合算法不受极区经线收敛的影响，可以连续有效提供极区运载器所需的姿态、速度和位置信息，且于格网惯导系统相比，格网惯导/超短基线紧组合算法不包含周期性振荡误差，位置误差不随时间发散，具有更高的导航精度。

最终仿真结果表明，本发明中的格网惯导/超短基线紧组合导航技术不受极区特殊地理位置的影响，具有更高的导航精度，导航误差均被限制在小范围内，有效抑制了误差发散，保证了长航时工作时的导航精度。

综合上述分析，得到如下分析结果：通过本发明提出的极区格网惯导/超短基线紧组合导航方法，可以保证导航系统在极区长航时工作的导航性能。因此，本发明可以更为全面的提升导航性能，满足导航系统在极区长时间工作对可靠性与精度的应用需求。