CN115200574A - 一种地球椭球模型下的极区横向组合导航方法 - Google Patents

Abstract

本发明属于导航技术领域，公开了一种地球椭球模型下的极区横向组合导航方法，适用于舰艇极区航行导航。本发明建立了椭球模型横坐标系下的坐标系变换关系、惯导系统机械编排以及组合导航滤波器，建立了系统误差状态与协方差矩阵在导航坐标系转换过程中的变换关系。一方面优化了极区组合导航系统的地球模型，减小了地球模型造成的几何误差；另一方面解决了导航坐标系转换过程中的滤波器超调与误差参数估计错误的问题。本发明在现有导航框架的基础上，解决了切换振荡的问题，适用于高精度组合导航系统跨极区航行。

Description

一种地球椭球模型下的极区横向组合导航方法

技术领域

本发明属于导航技术领域，涉及惯性/多普勒组合导航方法，特别涉及一种地球椭球模型下的极区横向组合导航方法，适用于水下潜航器等水下载体的跨极区导航航行。

背景技术

北极地区在资源、战略、科研、航道等方面有着重要的价值，因此对北极地区的探索具有重要意义。水下潜航器能够突破极地环境对于探测活动的时空限制，提高数据获取效率，实现智能化的极地环境探测，已经成为极区海洋探测主要设备之一。水下潜航器的工作环境位于水下，直接导致当前最常用的全球导航卫星系统难以应用，也决定了水下潜航器对导航自主性和可靠性的需求最为迫切。由于惯性导航系统(INS)不需要接收外界信号也不辐射信号就可以完成导航工作，因此是水下潜航器最主要的导航装备，也是水下潜航器实现极区导航的重要途经。

对极区的探索要求水下潜航器具备长航时自主导航、跨极区导航的能力，但是惯导系统的误差会随时间累积，并且以地理坐标系为导航坐标系的惯导系统在极点附近会存在计算溢出的现象。针对误差发散的问题，在惯性器件性能不变的条件下，综合运用多普勒测速仪(DVL)等多种辅助传感器，是提高水下潜航器导航精度最可行的方法；针对极区地理坐标系失效的问题，当水下潜航器进入极区时可将导航坐标系切换为横坐标系保证导航系统的精度。

近年来，国内外许多专家针对这一问题开展了大量的研究。然而，目前的研究中基于横坐标系的组合导航算法均使用地球圆球模型编排。虽然地球圆球模型下的计算较为简单，但是模型与地球实际形状差距较大，存在几何误差，在对精度要求低的任务场景可以满足需要，然而参考旋转椭球体更接近地球的真实形状，几何误差可以忽略不记，在执行精密测绘任务时由地球模型造成的误差是不能容忍的。除此之外，目前提出的极区导航算法只是对高纬度地区进行导航算法设计，然而中大型水下潜航器工作时间一般较长，并且往往在广海域活动，导航系统在进入、离开极区时导航坐标系的切换会导致组合导航滤波器结构改变，导致系统状态参数振荡，导航误差增大，需要长时间才能实现滤波稳定，极大地影响了高精度组合导航系统的导航精度。

本发明针对目前存在的问题，提出一种地球椭球模型下的极区横向组合导航方法，适用于水下潜航器惯性/多普勒组合导航系统,减小了地球模型的误差；以地理坐标系、横坐标系下的组合导航滤波器为基础，建立了系统误差状态及其协方差矩阵在两个导航坐标系之间的转换关系，设计了滤波状态稳定的组合导航滤波器，解决了坐标系转换过程中的滤波状态跳变问题。本发明设计的惯性/多普勒组合导航满足了水下潜航器全纬度航行作业任务的需求，具有十分重要的工程意义。

发明内容

本发明要解决的技术问题就在于：提出了一种全纬度的组合导航方案，解决潜航器进出极区过程中导航坐标系切换引起的系统状态误差状态估计不一致及滤波超调的问题；设计了基于椭球模型的横坐标系组合导航滤波器，建立了系统状态误差状态，减小由地球模型带来的误差，提高导航精度，为水下潜航器等水下载体的高精度全纬度导航提供解决方案。

为解决上述技术问题，本发明提出的解决方案为：

一种地球椭球模型下的极区横向组合导航方法，所述方法包括以下步骤：

(1)构建基于地球椭球模型的极区横坐标系，确定水下潜航器在高纬度地区航行的位置表示方式，确定横坐标系与地理坐标系的转换关系；

(1.1)确定基于椭球模型的横坐标系的定义以及极区导航位置表示方式：

首先将地球坐标系e绕其x_e轴顺时针旋转90°，进而绕第一次旋转之后中间坐标系的z′_e轴顺时针旋转90°得到横地球坐标系e′，将横地球坐标系e′与地球坐标系e之间的方向余弦矩阵

表示为：

确定横向极点，取(0°,90°E)为横向北极点，(0°,90°W)为横向南极点；取0°经线和180°经线组成的大椭圆为横向赤道；取90°E和90°W北半球部分组成的半个大椭圆为0°横经线；基于构建的横经纬网建立横地理坐标系，其中横北向指向横北极，天向与地理坐标系定义一致，通过右手法则确定横东向，构成横地理坐标系E^tN^tU^t；

将水下潜航器在地理坐标系下位置的表示为(L，λ，h)，L表示纬度，λ表示经度，h表示水下潜航器距离水平面的高度；根据构建的横经纬网络，将水下潜航器在横坐标系中位置表示为(L^t，λ^t，h)，其中，L^t表示横纬度，λ^t表示横经度；

(1.2)确定坐标系之间的转换关系，步骤如下：

确定地球坐标系e与地理坐标系n之间的方向余弦矩阵

为：

确定横地球坐标系与横地理坐标系之间的方向余弦矩阵

确定地理系与横地理系之间的方向余弦矩阵

其中，

式中，β为地理系与横地理系之间的旋转角度；

(2)利用惯性导航获得载体姿态、速度、位置相关信息，确定横坐标系下的姿态更新方程、速度更新方程、位置更新方程，具体步骤如下：

(2.1)确定横坐标系下的姿态更新方程：

式中，

表示从载体坐标系b到横坐标系t的方向余弦矩阵；

表示载体坐标系b相对于惯性坐标系i的旋转角速度；

表示横坐标系t相对于惯性坐标系i的旋转角速度；

(2.2)确定横坐标系下的速度v^t的更新方程：

其中，

式中，v^t表示横地理坐标系t下的载体速度；f^b表示载体坐标系b下表示的比力；

表示横地球坐标系e′相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影；ω_ie表示地球自转角速度的大小；

表示横地理坐标系t相对于横地球坐标系e′的旋转角速度在横坐标系t下的投影；g^t表示横地理坐标系下表示的重力矢量；

表示地理坐标系n相对于地球坐标系的旋转角速度在地理坐标系下的投影；

确定

其中：

式中，

表示横地理坐标系下载体的东向速度；

表示横地理坐标系下载体的北向速度；R_E为载体所在位置卯酉圈半径；R_N为载体所在位置子午圈半径；R_x为载体所在位置的横东向的曲率半径；R_y为载体所在位置的横北向的曲率半径；

为载体所在位置的扭曲率；R_e表示地球长轴半径；e表示地球偏心率；

确定β角的微分方程：

(2.3)确定横坐标系下的位置更新方程：

式中，

表示横地理坐标系下载体的天向速度；

(3)确定水下潜航器在横坐标系下的卡尔曼滤波模型，包括以下步骤：

(3.1)确定横坐标系下的系统状态方程：

(3.1.1)确定横坐标系下的系统误差状态：

其中，

分别为横坐标系下东向、北向、天向的姿态误差；

分别为横坐标系下东向、北向、天向的速度误差；δL^t,δλ^t,δh分别为横纬度误差、横经度误差、天向位置误差；

分别为x、y、z轴陀螺的常值零偏；

分别为x、y、z轴加速度计的常值零偏；δk为DVL标度因数误差；δθ、δψ分别为DVL的俯仰角安装误差、方位角安装误差；

(3.1.2)确定姿态误差方程表示：

式中，φ^t表示姿态误差，

表示横坐标系t相对于惯性坐标系i的旋转角速度误差；

表示载体坐标系b相对于惯性坐标系i的旋转角速度误差；

(3.1.3)确定速度误差方程表示：

其中，

式中，δv^t表示速度误差，f^t表示横坐标系t下表示的比力；

表示横地球坐标系e′相对于惯性坐标系i的旋转角速度误差；

表示横坐标系t相对于横地球坐标系e′的旋转角速度误差；δf^b表示b系下的比力误差；δ(·)表示“·”式分别关于L^t、λ^t、h的误差；

(3.1.4)位置误差方程表示为：

(3.1.5)确定陀螺、加表零偏，DVL误差量的误差方程：

(3.2)确定DVL的速度观测方程：

其中，

式中，

分别为INS的速度估计值与DVL的速度输出值；υ为等效噪声；

(4)在水下潜航器进入、离开极区时，根据导航参数、误差状态、协方差矩阵的转换关系，完成导航系统在地理坐标系与横坐标之间的切换，具体步骤如下：

(4.1)当水下潜航器进入极区时，确定导航参数中姿态、速度的转换关系：

式中，

为载体坐标系b与地理坐标系n之间的方向余弦矩阵；vⁿ为地理坐标系下的载体速度；

位置参数的转换关系为：

(4.2)当水下潜航器离开极区时，确定导航参数中姿态速度的转换关系：

确定位置参数的转换关系：

(4.3)确定误差状态转换关系：

(4.3.1)横坐标系下的姿态误差φ^t与地理坐标系下的姿态误差φⁿ的转换关系：

式中，

δL,δλ分别为纬度误差、经度误差；

(4.3.2)确定横坐标系下的速度误差δv^t与地理坐标系下的速度误差δvⁿ的转换关系：

(4.3.3)确定横坐标系下的位置误差与地理坐标系下的位置误差的转换关系：

(4.4)确定组合导航系统在横坐标系与地理坐标系下协方差矩阵的转换关系：

将误差状态的转换关系表示为：

x^t(t)＝Φxⁿ(t)，xⁿ(t)＝Φ^-1x^t(t)

式中，xⁿ为地理坐标系下的系统误差状态；Φ为系统误差状态从n系转换到t系的转换矩阵，具体表达式为：

式中，O_3×3为3×3的零矩阵；I_3×3为3×3的单位矩阵；当水下潜航器进入极区时，协方差矩阵转换关系表示为：

式中，

为横坐标系下的误差状态估计值，

为地理坐标系下的误差状态估计值；Pⁿ(t)为地理坐标系下的协方差矩阵；当水下潜航器离开极区时，协方差矩阵转换关系表示为：

Pⁿ(t)＝Φ^-1P^t(t)Φ^-T

式中，P^t(t)为横地理坐标系下的协方差矩阵；

进一步的，本发明所采用的组合导航滤波器采用闭环滤波，系统误差状态在每次闭环反馈校正后为0，系统误差状态无需进行转换。

进一步的，若载体接收到其他传感器的位置信息，包括但不限于GNSS位置信息、重力匹配位置信息、地磁匹配位置信息，基于接收到的位置信息对转换关系

或

进行修正更新。

进一步的，所述步骤(4)中，在导航坐标系切换的同时，DVL的速度观测量也进行切换，即在地理坐标系下观测量为DVL速度在地理坐标系下的投影，切换后观测量为DVL速度在横地理坐标系下的投影。

进一步的，所述步骤(4)中，导航坐标系的切换以纬度作为阈值，纬度阈值的选择根据实际情况选择，当载体所在位置的纬度大于设定的阈值，导航系切换至横地理系，当载体所在位置的纬度小于阈值，导航系切换至地理系。

综上所述，本发明的优点及积极效果为：本发明采用椭球模型下的横坐标系作为极区导航坐标系，解决了在极区经线快速收敛带来的导航误差增大的问题，避免了地球模型带来的几何误差；解决了水下潜航器的组合导航系统在驶入、离开极区时由于导航坐标系切换带来的滤波器振荡问题，有效提高了导航精度与稳定性，且具有完全自主性。

附图说明

图1是本发明实施例提供的方法流程图；

图2是本发明实施例提供的横坐标系定义图示；

图3是本发明实施例提供的船载实验中的相对姿态误差示意图；

图4是本发明实施例提供的船载实验中的相对位置误差示意图；

图5是本发明实施例提供的船载实验中的陀螺零偏相对估计误差示意图；

图6是本发明实施例提供的船载实验中的加速度计零偏相对估计误差示意图；

图7是本发明实施例提供的船载实验中的DVL误差参数相对估计误差示意图；

图8是本发明实施例提供的半实物仿真实验中的相对姿态误差示意图；

图9是本发明实施例提供的半实物仿真实验中的相对位置误差示意图；

图10是本发明实施例提供的半实物仿真实验中的陀螺零偏相对估计误差示意图；

图11是本发明实施例提供的半实物仿真实验中的加速度计零偏相对估计误差示意图；

图12是本发明实施例提供的半实物仿真实验中的DVL误差参数相对估计误差示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

现有基于横坐标系得极区组合导航算法均采用地球圆球模型，然而对于高精度水下导航系统来说，地球模型造成的几何误差是不可容忍的。除此之外，水下潜航器进入、离开极区时导航坐标系的切换会导致组合导航滤波器与阻尼滤波器结构的改变，引起滤波器超调与误差状态估计不连续的问题，造成系统状态参数振荡，导航误差增大。针对这些问题，本发明提出一种基于地球椭球模型的水下潜航器INS/DVL极区横向组合导航方法，所述INS/DVL组合导航方法如图1所示。具体实施方式如下：

(1)构建基于地球椭球模型的极区横坐标系，确定水下潜航器在高纬度地区航行的位置表示方式，确定横坐标系与地理坐标系的转换关系；

(1.1)确定基于椭球模型的横坐标系的定义以及极区导航位置表示方式：

首先将地球坐标系e绕其x_e轴顺时针旋转90°，进而绕第一次旋转之后中间坐标系的z′_e轴顺时针旋转90°得到横地球坐标系e′，将横地球坐标系e′与地球坐标系e之间的方向余弦矩阵

表示为：

确定横向极点，取(0°,90°E)为横向北极点，(0°,90°W)为横向南极点；取0°经线和180°经线组成的大椭圆为横向赤道；取90°E和90°W北半球部分组成的半个大椭圆为0°横经线；基于构建的横经纬网建立横地理坐标系，其中横北向指向横北极，天向与地理坐标系定义一致，通过右手法则确定横东向，构成横地理坐标系E^tN^tU^t；

将水下潜航器在地理坐标系下位置的表示为(L，λ，h)，L表示纬度，λ表示经度，h表示水下潜航器距离水平面的高度；根据构建的横经纬网络，将水下潜航器在横坐标系中位置表示为(L^t，λ^t，h)，其中，L^t表示横纬度，λ^t表示横经度；

(1.2)确定坐标系之间的转换关系，步骤如下：

确定地球坐标系e与地理坐标系n之间的方向余弦矩阵

为：

确定横地球坐标系与横地理坐标系之间的方向余弦矩阵

确定地理系与横地理系之间的方向余弦矩阵

其中，

式中，β为地理系与横地理系之间的旋转角度；

(2)利用惯性导航获得载体姿态、速度、位置相关信息，确定横坐标系下的姿态更新方程、速度更新方程、位置更新方程，具体步骤如下：

(2.1)确定横坐标系下的姿态更新方程：

式中，

表示从载体坐标系b到横坐标系t的方向余弦矩阵；

表示载体坐标系b相对于惯性坐标系i的旋转角速度；

表示横坐标系t相对于惯性坐标系i的旋转角速度；

(2.2)确定横坐标系下的速度v^t的更新方程：

其中，

式中，v^t表示横地理坐标系t下的载体速度；f^b表示载体坐标系b下表示的比力；

表示横地球坐标系e′相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影；ω_ie表示地球自转角速度的大小；

表示横地理坐标系t相对于横地球坐标系e′的旋转角速度在横坐标系t下的投影；g^t表示横地理坐标系下表示的重力矢量；

表示地理坐标系n相对于地球坐标系的旋转角速度在地理坐标系下的投影；

确定

其中：

式中，

表示横地理坐标系下载体的东向速度；

表示横地理坐标系下载体的北向速度；R_E为载体所在位置卯酉圈半径；R_N为载体所在位置子午圈半径；R_x为载体所在位置的横东向的曲率半径；R_y为载体所在位置的横东向的曲率半径；

为载体所在位置的扭曲率；R_e表示地球长轴半径；e表示地球偏心率；

确定β角的微分方程：

(2.3)确定横坐标系下的位置更新方程：

式中，

表示横地理坐标系下载体的天向速度；

(3)确定水下潜航器在横坐标系下的卡尔曼滤波模型，包括以下步骤：

(3.1)确定横坐标系下的系统状态方程：

(3.1.1)确定横坐标系下的系统误差状态：

其中，

分别为横坐标系下东向、北向、天向的姿态误差；

分别为横坐标系下东向、北向、天向的速度误差；δL^t,δλ^t,δh分别为横纬度误差、横经度误差、天向位置误差；

分别为x、y、z轴陀螺的常值零偏；

分别为x、y、z轴加速度计的常值零偏；δk为DVL标度因数误差；δθ、δψ分别为DVL的俯仰角安装误差、方位角安装误差；

(3.1.2)确定姿态误差方程表示：

式中，φ^t表示姿态误差，

表示横坐标系t相对于惯性坐标系i的旋转角速度误差；

表示载体坐标系b相对于惯性坐标系i的旋转角速度误差；

(3.1.3)确定速度误差方程表示：

其中，

式中，δv^t表示速度误差，f^t表示横坐标系t下表示的比力；

表示横地球坐标系e′相对于惯性坐标系i的旋转角速度误差；

表示横坐标系t相对于横地球坐标系e′的旋转角速度误差；δf^b表示b系下的比力误差；δ(·)表示“·”式分别关于L^t、λ^t、h的误差；

(3.1.4)位置误差方程表示为：

(3.1.5)确定陀螺、加表零偏，DVL误差量的误差方程：

(3.2)确定DVL的速度观测方程：

其中，

式中，

分别为INS的速度估计值与DVL的速度输出值；υ为等效噪声；

(4)在水下潜航器进入、离开极区时，根据导航参数、误差状态、协方差矩阵的转换关系，完成导航系统在地理坐标系与横坐标之间的切换，具体步骤如下：

(4.1)当水下潜航器进入极区时，确定导航参数中姿态、速度的转换关系：

式中，

为载体坐标系b与地理坐标系n之间的方向余弦矩阵；vⁿ为地理坐标系下的载体速度；

位置参数的转换关系为：

(4.2)当水下潜航器离开极区时，确定导航参数中姿态速度的转换关系：

确定位置参数的转换关系:

(4.3)确定误差状态转换关系：

(4.3.1)横坐标系下的姿态误差φ^t与地理坐标系下的姿态误差φⁿ的转换关系：

式中，

δL,δλ分别为纬度误差、经度误差；

(4.3.2)确定横坐标系下的速度误差δv^t与地理坐标系下的速度误差δvⁿ的转换关系：

(4.3.3)确定横坐标系下的位置误差与地理坐标系下的位置误差的转换关系：

(4.4)确定组合导航系统在横坐标系与地理坐标系下协方差矩阵的转换关系：

将误差状态的转换关系表示为：

x^t(t)＝Φxⁿ(t)，xⁿ(t)＝Φ^-1x^t(t)

式中，xⁿ为地理坐标系下的系统误差状态；Φ为系统误差状态从n系转换到t系的转换矩阵，具体表达式为：

式中，O_3×3为3×3的零矩阵；I_3×3为3×3的单位矩阵；当水下潜航器进入极区时，协方差矩阵转换关系表示为：

式中，

为横坐标系下的误差状态估计值，

为地理坐标系下的误差状态估计值；Pⁿ(t)为地理坐标系下的协方差矩阵；当水下潜航器离开极区时，协方差矩阵转换关系表示为：

Pⁿ(t)＝Φ^-1P^t(t)Φ^-T

式中，P^t(t)为横地理坐标系下的协方差矩阵；

本发明所采用的组合导航滤波器均采用闭环滤波，系统误差状态在每次闭环反馈校正后为0，故系统误差状态无需进行转换；

下面结合实验对本发明作进一步说明：

船载实验验证

因为横坐标系在除横北极、横南极以外的中低纬度地区同样适用，因此实验船只搭载INS/DVL组合导航系统在中低纬度进行航行实验；实验时前900s进行对准，行驶过程中前1h使用地理坐标系进行导航，设定该时刻导航坐标系切换至横坐标系进行导航，航行1h后再切换至地理坐标系，实验总时长为3h；因为中低纬度地区，地理坐标系下构建的INS/DVL组合导航滤波器不存在跟纬度相关的误差放大效应，将其组合导航结果作为对照，比较有协方差变换与无协方差变换的情况下，由于导航坐标系切换带来的系统误差状态估计值的变化；

图3为相对姿态误差曲线，实线为有无协方差变换的情况下的相对姿态误差，虚线为有协方差变换的相对姿态误差；无协方差变换情况下俯仰角的误差振荡峰值为0.3′，横滚角误差峰值0.23′；由于航向角的可观测性较差，受导航坐标系切换的影响较大，误差峰值为2.54′；协方差变化算法的相对姿态误差不明显，能够保证滤波状态估计值在导航坐标系切换前后的一致性；

图4为相对位置误差曲线，实线为有无协方差变换的情况下的相对位置误差，虚线为有协方差变换的相对位置误差；有协方差变换时的东向相对误差峰值为10.38m，北向相对误差峰值为27.72m；而无协方差变换时，东向相对误差峰值为30.11m，北向相对误差峰值为60.57m；虽然高度参数理论上不受导航坐标系切换的影响，但是由于其他误差的耦合效应也发生了振荡，振荡峰值为2.06m；

图5、图6为陀螺、加速度计零偏的相对估计误差，实线为有无协方差变换的情况下的相对估计误差，虚线为有协方差变换的相对估计误差；有协方差变换时几乎没有发生振荡，无协方差变换时，三轴陀螺零偏的估计振荡峰值分别为0.0045°/h、0.0058°/h、0.049°/h，三轴加速度计零偏的估计振荡峰值分别为45.21μg、97.13μg、3.11μg；陀螺、加速度计零偏在导航坐标系切换前后出现了持续振荡.由于方位陀螺(z轴)零偏的可观性弱于水平陀螺零偏，水平加速度计(x、y轴)零偏的可观性弱于天向加速度计零偏，相应的状态估计振荡更为剧烈；

图7为DVL比例因子及安装参数的相对估计误差曲线，实线为有无协方差变换的情况下的相对估计误差，虚线为有协方差变换的相对估计误差；无协方差变换时，AUV进出极区过程中导航坐标系切换会使得其误差参数估计值产生振荡，并且出现了估计值收敛错误；

从图3-图7可以看出，当导航坐标系切换时，协方差变换算法可以实现组合导航滤波器的平滑过渡，状态估计值振荡不明显；但如果忽略滤波器协方差矩阵的转换，状态估计值会出现不同程度的振荡，部分参数甚至收敛至错误值；验证了本发明提出的算法在中低纬度的有效性；

(5.2)半实物仿真实验验证

为检验算法在高纬度地区的适用性，采用半实物仿真方法，保持载体相对于地面的姿态和速度不变，将船载实验数据转换至高纬度地区，起始位置为北纬79.95°，水下潜航器在5500s时航行至北纬80°进入极区，此时从地理系切换至横地理坐标系进行导航；

图8给出了相对姿态误差的对比结果，其中无协方差变换时，导航坐标系切换带来的航向角误差峰值为26.05′，俯仰角与横滚角误差也达到了0.8′；从图9可以看出，导航坐标系切换后协方差变换算法可以保证位置误差估计的一致性，与参考结果一致，但无协方差变换情况下位置误差出现了振荡、发散，东向误差峰值为113m，北向误差峰值为84m；图10、图11显示，切换导航坐标系后，无协方差变换时方位陀螺(z轴)零偏、水平轴(x、y轴)加速度计零偏在高纬度地区的相对估计误差明显增大并且收敛至错误值；图12为DVL误差参数的相对估计误差，无协方差变换时导航坐标系切换引起的跳变幅值增大，振荡后发散；

图8-图12验证了协方差变换算法在极区能够保证滤波器的平滑过渡，状态估计精度能够得到保证；

所述步骤(1)中，若载体接收到其他传感器的位置信息，例如：GNSS、重力匹配、地磁匹配等，可对转换关系

或

进行修正更新。

所述步骤(4)中，在导航系统切换的同时，DVL的速度观测量也需要进行切换，即在地理坐标系下观测量为DVL速度在地理坐标系下的投影，切换后观测量为DVL速度在横地理坐标系下的投影。

所述步骤(4)中，导航坐标系的切换以纬度作为阈值，纬度阈值的选择可根据实际情况选择，当载体所在位置的纬度大于设定的阈值，导航系切换至横地理系，当载体所在位置的纬度小于阈值，导航系切换至地理系。

以上所述仅是本发明的优选实施方式，并不用以限制本发明，凡属于本发明思路下的技术方案均属于本发明的保护范围。在不脱离本发明原理前提下的若干改进和润饰等，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种地球椭球模型下的极区横向组合导航方法，其特征在于，包括以下步骤：

(1)构建基于地球椭球模型的极区横坐标系，确定水下潜航器在高纬度地区航行的位置表示方式，确定横坐标系与地理坐标系的转换关系；

(1.1)确定基于椭球模型的横坐标系的定义以及极区导航位置表示方式：

首先将地球坐标系e绕其x_e轴顺时针旋转90°，进而绕第一次旋转之后中间坐标系的z′_e轴顺时针旋转90°得到横地球坐标系e′，将横地球坐标系e′与地球坐标系e之间的方向余弦矩阵

表示为：

确定横向极点，取(0°,90°E)为横向北极点，(0°,90°W)为横向南极点；取0°经线和180°经线组成的大椭圆为横向赤道；取90°E和90°W北半球部分组成的半个大椭圆为0°横经线；基于构建的横经纬网建立横地理坐标系，其中横北向指向横北极，天向与地理坐标系定义一致，通过右手法则确定横东向，构成横地理坐标系E^tN^tU^t；

将水下潜航器在地理坐标系下位置的表示为(L，λ，h)，L表示纬度，λ表示经度，h表示水下潜航器距离水平面的高度；根据构建的横经纬网络，将水下潜航器在横坐标系中位置表示为(L^t，λ^t，h)，其中，L^t表示横纬度，λ^t表示横经度；

(1.2)确定坐标系之间的转换关系，步骤如下：

确定地球坐标系e与地理坐标系n之间的方向余弦矩阵

为：

确定横地球坐标系与横地理坐标系之间的方向余弦矩阵

确定地理系与横地理系之间的方向余弦矩阵

其中，

式中，β为地理系与横地理系之间的旋转角度；

(2)利用惯性导航获得载体姿态、速度、位置相关信息，确定横坐标系下的姿态更新方程、速度更新方程、位置更新方程，具体步骤如下：

(2.1)确定横坐标系下的姿态更新方程：

式中，

表示从载体坐标系b到横坐标系t的方向余弦矩阵；

表示载体坐标系b相对于惯性坐标系i的旋转角速度；

表示横坐标系t相对于惯性坐标系i的旋转角速度；

(2.2)确定横坐标系下的速度v^t的更新方程：

其中，

式中，v^t表示横地理坐标系t下的载体速度；f^b表示载体坐标系b下表示的比力；

表示横地球坐标系e′相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影；ω_ie表示地球自转角速度的大小；

表示横地理坐标系t相对于横地球坐标系e′的旋转角速度在横坐标系t下的投影；g^t表示横地理坐标系下表示的重力矢量；

表示地理坐标系n相对于地球坐标系的旋转角速度在地理坐标系下的投影；

确定

其中：

式中，

表示横地理坐标系下载体的东向速度；

表示横地理坐标系下载体的北向速度；R_E为载体所在位置卯酉圈半径；R_N为载体所在位置子午圈半径；R_x为载体所在位置的横东向的曲率半径；R_y为载体所在位置的横东向的曲率半径；

为载体所在位置的扭曲率；R_e表示地球长轴半径；e表示地球偏心率；

确定β角的微分方程：

(2.3)确定横坐标系下的位置更新方程：

式中，

表示横地理坐标系下载体的天向速度；

(3)确定水下潜航器在横坐标系下的卡尔曼滤波模型，包括以下步骤：

(3.1)确定横坐标系下的系统状态方程：

(3.1.1)确定横坐标系下的系统误差状态：

其中，

分别为横坐标系下东向、北向、天向的姿态误差；

分别为横坐标系下东向、北向、天向的速度误差；δL^t,δλ^t,δh分别为横纬度误差、横经度误差、天向位置误差；

分别为x、y、z轴陀螺的常值零偏；

分别为x、y、z轴加速度计的常值零偏；δk为DVL标度因数误差；δθ、δψ分别为DVL的俯仰角安装误差、方位角安装误差；

(3.1.2)确定姿态误差方程表示：

式中，φ^t表示姿态误差，

表示横坐标系t相对于惯性坐标系i的旋转角速度误差；

表示载体坐标系b相对于惯性坐标系i的旋转角速度误差；

(3.1.3)确定速度误差方程表示：

其中，

式中，δv^t表示速度误差，f^t表示横坐标系t下表示的比力；

表示横地球坐标系e′相对于惯性坐标系i的旋转角速度误差；

表示横坐标系t相对于横地球坐标系e′的旋转角速度误差；δf^b表示b系下的比力误差；δ(·)表示“·”式分别关于L^t、λ^t、h的误差；

(3.1.4)位置误差方程表示为：

(3.1.5)确定陀螺、加表零偏，DVL误差量的误差方程：

(3.2)确定DVL的速度观测方程：

其中，

式中，

分别为INS的速度估计值与DVL的速度输出值；υ为等效噪声；

(4)在水下潜航器进入、离开极区时，根据导航参数、误差状态、协方差矩阵的转换关系，完成导航系统在地理坐标系与横坐标之间的切换，具体步骤如下：

(4.1)当水下潜航器进入极区时，确定导航参数中姿态、速度的转换关系：

式中，

为载体坐标系b与地理坐标系n之间的方向余弦矩阵；vⁿ为地理坐标系下的载体速度；

位置参数的转换关系为：

(4.2)当水下潜航器离开极区时，确定导航参数中姿态速度的转换关系：

确定位置参数的转换关系:

(4.3)确定误差状态转换关系：

(4.3.1)横坐标系下的姿态误差φ^t与地理坐标系下的姿态误差φⁿ的转换关系：

式中，

δL,δλ分别为纬度误差、经度误差；

(4.3.2)确定横坐标系下的速度误差δv^t与地理坐标系下的速度误差δvⁿ的转换关系：

(4.3.3)确定横坐标系下的位置误差与地理坐标系下的位置误差的转换关系：

(4.4)确定组合导航系统在横坐标系与地理坐标系下协方差矩阵的转换关系：

将误差状态的转换关系表示为：

x^t(t)＝Φxⁿ(t)，xⁿ(t)＝Φ^-1x^t(t)

式中，xⁿ为地理坐标系下的系统误差状态；Φ为系统误差状态从n系转换到t系的转换矩阵，具体表达式为：

式中，O_3×3为3×3的零矩阵；I_3×3为3×3的单位矩阵；当水下潜航器进入极区时，协方差矩阵转换关系表示为：

式中，

为横坐标系下的误差状态估计值，

为地理坐标系下的误差状态估计值；xⁿ(t)为地理坐标系下的误差状态值；Pⁿ(t)为地理坐标系下的协方差矩阵；当水下潜航器离开极区时，协方差矩阵转换关系表示为：

Pⁿ(t)＝Φ^-1P^t(t)Φ^-T

式中，P^t(t)为横地理坐标系下的协方差矩阵。

2.如权利要求1所述的一种地球椭球模型下的极区横向组合导航方法，其特征在于，组合导航滤波器采用闭环滤波，系统误差状态在每次闭环反馈校正后为0，系统误差状态无需进行转换。

3.如权利要求1所述的一种地球椭球模型下的极区横向组合导航方法，其特征在于，若载体接收到其他传感器的位置信息，包括但不限于GNSS位置信息、重力匹配位置信息、地磁匹配位置信息，基于接收到的位置信息对转换关系

或

进行修正更新。

4.如权利要求1所述的一种地球椭球模型下的极区横向组合导航方法，其特征在于，所述步骤(4)中，在导航坐标系切换的同时，DVL的速度观测量也进行切换，即在地理坐标系下观测量为DVL速度在地理坐标系下的投影，切换后观测量为DVL速度在横地理坐标系下的投影。

5.如权利要求1所述的一种地球椭球模型下的极区横向组合导航方法，其特征在于，所述步骤(4)中，导航坐标系的切换以纬度作为阈值，纬度阈值的选择根据实际情况选择，当载体所在位置的纬度大于设定的阈值，导航系切换至横地理系，当载体所在位置的纬度小于阈值，导航系切换至地理系。

Images