CN117470234A - 基于Psi角误差模型的舰船跨极区滤波切换方法 - Google Patents

Abstract

本发明属于惯性导航技术领域，公开了一种基于Psi角误差模型的协方差变换跨极区导航方法，适用于舰船跨极区航行导航。本发明基于Psi角误差模型建立横坐标系下的坐标系变换关系、惯性导航系统机械编排，设计惯性/测速仪组合导航滤波器，构建横地理坐标系与地理坐标系下系统误差状态和协方差矩阵的转换关系。本发明减小了Phi角误差模型的近似误差，可以解决导航坐标系转换过程中组合导航滤波器超调与震荡误差的问题，用以实现舰船从非极区到极区的平稳过渡。

Description

基于Psi角误差模型的舰船跨极区滤波切换方法

技术领域

本发明属于惯性导航技术领域，涉及惯性/测速仪组合导航方法，特别涉及基于Psi角误差模型的舰船跨极区滤波切换方法，适用于舰船的跨极区导航航行。

背景技术

极地地区尤其是北极地区在资源、科研、航道等方面具有重要战略价值。由于惯性导航拥有极好的自主性，并且不受极地地区恶劣环境的影响，因此惯性导航已经成为极区重要的导航手段。

传统的惯性导航编排方案常用于中低纬度地区，然而随着纬度增加，该方案出现计算溢出，同时经线收敛速度增加，失去航向参考，因此传统惯性导航编排方案难以适用于极区。在原有基础上改进的游动方位、自由方位惯性导航编排方案在一定程度上扩大了舰船在极区的工作范围，但是无法解决极点附近固有奇异值的问题，无法满足全纬度导航的需求。针对这一问题，对比文件(Charles Broxmeyer.Inertial Navigation Systems.NewYork:McGraw-Hill Book Company,1964.)提出了横向坐标系统及其相应的导航方法，尽管该方法通过重新定义新的坐标系统避免极点的固有奇异值，但是仍然在横北极点、横南极点存在固有奇异点，因此仍然无法实现满足全纬度导航的需求。

目前常用的方法是结合横地理坐标系与地理坐标系的优势，避免各坐标系统的缺点，以实现全球导航，但该方法无法避免的是各坐标系统之间导航参数、导航系统结构的切换。对于组合导航系统，切换过程会影响导航滤波结构的连续性与一致性，导致滤波器结构突变、滤波超调、估计误差增大等问题，同时还极大地增加算法复杂度从而给系统带来极大的运算负担。因此要想实现坐标系的平滑切换需重点解决：1.横地理坐标系下惯性导航的机械编排。2.不同导航坐标系下各导航参数的转换关系。3.组合导航系统滤波器中所涉及的误差状态的转换关系和协方差矩阵的转换关系。4.减少系统的运算负担。

本发明针对目前存在的问题，面向长航时跨极区航海应用，提出基于Psi角误差模型的舰船跨极区滤波切换方法，以Psi角误差模型为基础，建立起导航参数、导航系统结构的切换关系以维持切换过程中组合导航系统参数及滤波结构的连续性与一致性；同时降低系统方案复杂度以减少系统运算负担。本发明可以提高组合导航系统切换过程中的稳定性，极大的减轻系统运算量，具有十分重要的工程意义。

发明内容

现有技术中大多采用“硬”切换，忽略了切换过程中对惯性导航系统尤其是组合导航系统滤波结构的连续性与一致性的影响，导致短时估计误差增大。此外，以Phi角误差模型为基础处理极区惯性导航问题，导致系统参数切换方案极其复杂，系统运算负担增大。本发明要解决的技术问题就在于：针对现有技术的不足，本发明提出基于Psi角误差模型的协方差变换跨极区导航方法，用于解决舰船进出极区过程中导航坐标系切换引起的组合导航系统滤波结构的不连续性与不一致的问题，以提高切换过程中滤波器的稳定性，同时优化协方差变换方法，解决切换过程中系统庞大计算量的问题，减少系统近似误差。

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

基于Psi角误差模型的舰船跨极区滤波切换方法，所述方法包括以下步骤：

(1)定义横地球坐标系，定义横向极点，定义横向经度和横向纬度，确定横向位置表示方式：所述横地球坐标系e′的原点位于地心，X轴沿着地球自转轴指向北极，Y轴指向本初子午线与赤道的交点，Z轴穿过东经90°子午线与赤道的交点；定义(0°,90°E)为横向北极点、(0°,90°W)为横向南极点；定义0°经线和180°经线组成的大椭圆为横向赤道；定义90°E和90°W北半球部分组成的半个大椭圆为0°横经线，且横向本初子午线为地理经度90°E所在的子午圈的北半球部分，横向子午线为过横向极点的平面与地球表面相交的轮廓线；定义地球表面上一点的地理法线与横向赤道面交角为该点的横向纬度；定义该点所在的横向子午面与横向本初子午面的交角为横向经度；根据构建的横经纬网络，将舰船在横地球坐标系中位置表示为(L^t，λ^t，h)，其中，其中L^t表示横纬度，λ^t表示横经度，h表示高度；

(2)定义横地理坐标系：横地理坐标系t的原点位于载体中心，Y轴沿横向经线的切线指向横向北极点，Z轴垂直于当地水平面指向天向，X轴与Y轴和Z轴构成右手坐标系，且为“横东-横北-天向”定义；

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

根据所述步骤(1)中横地球坐标系定义，确定地球坐标系e到横地球坐标系e′的方向余弦矩阵为：

确定地球坐标系e到地理坐标系g的方向余弦矩阵为：

其中L表示舰船所处的纬度，λ表示舰船所处的经度；

确定横地球坐标系e′到横地理坐标系t的方向余弦矩阵

根据链式法则，确定地理坐标系g到横地理坐标系t的方向余弦矩阵

式中表示为方向余弦矩阵/>的转置；

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

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

式中，表示从载体坐标系b到横地理坐标系t的方向余弦矩阵；/>表示载体坐标系b相对于惯性坐标系i的旋转角速度在载体坐标系b下的投影；/>表示横地理坐标系t相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影；

其中：表示地球坐标系e相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影，/>表示横地理坐标系t相对于地球坐标系e的旋转角速度在横地理坐标系t下的投影；

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

其中，

式中，v^t表示横地理坐标系t下的载体速度；f^b表示载体坐标系b下表示的比力；g^t表示横地理坐标系t下表示的重力矢量；表示横地理坐标系t下载体的东向速度；/>表示横地理坐标系t下载体的北向速度；R为地球半径；ω_ie表示地球自转角速度的大小；/>表示从地球坐标系e到横地理坐标系t的方向余弦矩阵；

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

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

(5)确定计算坐标系、平台坐标系与真实导航坐标系之间的转换关系：

确定计算坐标系c到平台坐标系p的方向余弦矩阵为：

确定真实导航坐标系t_g到平台坐标系p的方向余弦矩阵为：

确定真实导航坐标系t_g到计算地理坐标系c的方向余弦矩阵为：

式中I_3×3表示3×3的单位矩阵；ψ为漂移误差角，φ为姿态误差角，δθ为位置误差角；

确定漂移误差角、姿态误差角、位置误差角之间的关系为：

φ＝ψ+δθ

(6)确定舰船在计算横地理坐标系下的卡尔曼滤波模型，包括以下步骤：

(6.1)确定计算横地理坐标系下的系统状态方程：

(6.1.1)确定计算横地理坐标系下的系统误差状态：

其中，表示三维漂移误差角矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角；表示三维速度误差矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差；/>表示三维位置误差矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差；/>表示陀螺的零偏矢量，各分量分别为X、Y、Z轴陀螺的零偏；/>表示加速度计的零偏矢量，各分量分别为X、Y、Z轴加速度计的零偏；δk表示测速仪标度因数误差；δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差；

(6.1.2)确定计算横地理坐标系下惯性导航系统的姿态、速度和位置误差方程：

式中，表示计算横地理坐标系c'下地球自转角速度，/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度，/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵，f^c'表示计算横地理坐标系c'下表示的比力；

式中分别表示载体坐标系b下的陀螺误差和加速度计误差，表示为：

式中，和/>分别表示陀螺和加速度计的噪声；

(6.1.3)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差方程：

式中，τ_ε和τ_▽分别表示陀螺和加速度计的一阶马尔可夫相关时间，w_ε和w_▽分别表示陀螺和加速度计的高斯白噪声；

(6.2)确定测速仪的速度观测方程：

其中，

式中，表示惯性导航系统的速度估计值与测速仪的速度输出值；v^c'表示计算横地理坐标系c'下的速度矢量；

(6.3)确定系统误差状态修正方式：经滤波后的系统状态向量定义在计算横地理坐标系中，通过对系统状态进行修正使其定义在横地理坐标系t中：

φ^t＝ψ^c'+δθ^t

δv^t＝δv^c'-δθ^t×v^c'

式中，φ^t表示姿态误差角φ在横地理坐标系t中的投影；δv^t为速度误差δv在横地理坐标系t中的投影；表示计算横地理坐标系c'到横地理坐标系t的方向余弦矩阵；δθ^t表示位置误差角δθ在横地理坐标系t中的投影，具体表示为：

式中，分别为δr^t的北向分量、东向分量；δr^t为位置误差δr在横地理坐标系t中的投影，其中经纬度误差转换为位置误差δr^t表示为：

δr^t＝[(R+h)δλ^tcosL^t (R+h)δL^t δh]^T

位置误差δr^t转换为经纬度误差表示为：

其中为位置误差δr^t的天向分量；

(7)舰船进入、离开极区时，确定系统姿态、速度、位置的转换关系，确定组合惯性/测速仪组合导航系统误差状态、协方差矩阵的转换关系，具体步骤如下：

(7.1)进入极区导航坐标系切换至横地理坐标系时，确定系统姿态、速度的转换关系为：

式中，表示载体坐标系b到地理坐标系g的方向余弦矩阵；v^g表示地理坐标系下的载体速度；

位置的转换关系为：

(7.2)驶出极区导航坐标系切换至地理坐标系时，确定系统姿态、速度的转换关系为：

式中，表示横地理坐标系t到地理坐标系g的方向余弦矩阵；

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

(7.3)确定组合惯性/测速仪组合导航系统误差状态转换关系，步骤如下：

(7.3.1)确定计算横地理坐标系下漂移误差角ψ^c'和计算地理坐标系下漂移误差角ψ^c的转换关系：

式中表示计算地理坐标系c到计算横地理坐标系c'的方向余弦矩阵，具体表示为：

其中，为地球坐标系e到计算地理坐标系c的方向余弦矩阵，/>为横地球坐标系e′到计算横地理坐标系c'的方向余弦矩阵；

(7.3.2)确定计算横地理坐标系下的速度误差δv^c'与计算地理坐标系下的速度误差δv^c的转换关系：

(7.3.3)确定计算横地理坐标系下的位置误差δr^c'与计算地理坐标系下的位置误差δr^c的转换关系：

(7.4)确定组合惯性/测速仪组合导航系统协方差矩阵的转换关系，步骤如下：

根据步骤(7.3)所述，当舰船进入极区时，误差状态的转换关系表示为：

x^c'(t)＝Φx^c(t)

式中，x^c'表示计算横地理坐标系下的系统误差状态；Φ表示系统误差状态从计算地理坐标系c转换到计算横地理坐标系c'的转换矩阵，具体表达式为：

式中，diag{·}表示为块对角矩阵矩阵；I_3×3为3×3的单位矩阵；当舰船进入极区时并切换至横地理坐标系导航时，计算地理坐标系c下系统误差状态的协方差矩阵P^c(t)与计算横地理坐标系c'下系统误差状态的协方差矩阵P^c'(t)的转换关系表示为：

式中，表示计算横地理坐标系下的误差状态估计值，/>表示计算地理坐标系下的误差状态估计值；

当舰船离开极区并切换至地理坐标系导航时，误差状态和协方差矩阵转换关系表示为：

x^c(t)＝Φ^-1x^c'(t)，P^c(t)＝Φ^-1P^c'(t)Φ^-T。

进一步的，本发明所采用的组合导航滤波器对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈，测速仪标度因数误差和安装误差采用开环反馈，且每次闭环反馈的系统误差状态校正后置0。

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

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

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

进一步的，所述步骤(7)中，陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差三种系统状态在不同坐标系中保持一致，无需转换。

与现有技术相比，本发明具有以下优点：

本发明采用横地理坐标系与地理坐标系结合的方案，避免极区固有奇异点，可以满足全球导航的要求；本发明采用Psi角误差模型，更加贴合航海导航实际情况，基于该模型设计了横坐标系下惯性导航编排方案，构建了惯性/组合导航系统，简化了系统模型；本发明基于Psi角误差模型的舰船跨极区滤波切换方法，解决了坐标系切换过程中组合导航系统滤波器振荡的问题，使系统在坐标系切换过程中实现平滑滤波；本发明极大地减轻系统计算负担，减少近似误差。

附图说明

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

图2是本发明实施例提供的低纬度船载实验中滤波器估计的位置误差曲线示意图；

图3是本发明实施例提供的低纬度船载实验中的定位误差示意图；

图4是本发明实施例提供的高纬度半实物仿真中滤波器估计的位置误差曲线示意图；

图5是本发明实施例提供的高纬度半实物仿真中的定位误差示意图；

图6是本发明实施例提供的低纬度船载实验中不同切换时刻后100秒的标准差和均方根变化示意图。

具体实施方式

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

如图1所示，基于Psi角误差模型的舰船跨极区滤波切换方法，具体实施方式如下：

(1)定义横地球坐标系，定义横向极点，定义横向经度和横向纬度，确定横向位置表示方式：所述横地球坐标系e′的原点位于地心，X轴沿着地球自转轴指向北极，Y轴指向本初子午线与赤道的交点，Z轴穿过东经90°子午线与赤道的交点；定义(0°,90°E)为横向北极点、(0°,90°W)为横向南极点；定义0°经线和180°经线组成的大椭圆为横向赤道；定义90°E和90°W北半球部分组成的半个大椭圆为0°横经线，且横向本初子午线为地理经度90°E所在的子午圈的北半球部分，横向子午线为过横向极点的平面与地球表面相交的轮廓线；定义地球表面上一点的地理法线与横向赤道面交角为该点的横向纬度；定义该点所在的横向子午面与横向本初子午面的交角为横向经度；根据构建的横经纬网络，将舰船在横地球坐标系中位置表示为(L^t，λ^t，h)，其中，其中L^t表示横纬度，λ^t表示横经度，h表示高度；

(2)定义横地理坐标系：横地理坐标系t的原点位于载体中心，Y轴沿横向经线的切线指向横向北极点，Z轴垂直于当地水平面指向天向，X轴与Y轴和Z轴构成右手坐标系，且为“横东-横北-天向”定义；

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

根据所述步骤(1)中横地球坐标系定义，确定地球坐标系e到横地球坐标系e′的方向余弦矩阵为：

确定地球坐标系e到地理坐标系g的方向余弦矩阵为：

其中L表示舰船所处的纬度，λ表示舰船所处的经度；

确定横地球坐标系e′到横地理坐标系t的方向余弦矩阵

根据链式法则，确定地理坐标系g到横地理坐标系t的方向余弦矩阵

式中表示为方向余弦矩阵/>的转置；

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

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

式中，表示从载体坐标系b到横地理坐标系t的方向余弦矩阵；/>表示载体坐标系b相对于惯性坐标系i的旋转角速度在载体坐标系b下的投影；/>表示横地理坐标系t相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影；

其中：表示地球坐标系e相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影，/>表示横地理坐标系t相对于地球坐标系e的旋转角速度在横地理坐标系t下的投影；

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

其中，

式中，v^t表示横地理坐标系t下的载体速度；f^b表示载体坐标系b下表示的比力；g^t表示横地理坐标系t下表示的重力矢量；表示横地理坐标系t下载体的东向速度；/>表示横地理坐标系t下载体的北向速度；R为地球半径；ω_ie表示地球自转角速度的大小；/>表示从地球坐标系e到横地理坐标系t的方向余弦矩阵；

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

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

(5)确定计算坐标系、平台坐标系与真实导航坐标系之间的转换关系：

确定计算坐标系c到平台坐标系p的方向余弦矩阵为：

确定真实导航坐标系t_g到平台坐标系p的方向余弦矩阵为：

确定真实导航坐标系t_g到计算地理坐标系c的方向余弦矩阵为：

式中I_3×3表示3×3的单位矩阵；ψ为漂移误差角，φ为姿态误差角，δθ为位置误差角；确定漂移误差角、姿态误差角、位置误差角之间的关系为：

φ＝ψ+δθ

(6)确定舰船在计算横地理坐标系下的卡尔曼滤波模型，包括以下步骤：

(6.1)确定计算横地理坐标系下的系统状态方程：

(6.1.1)确定计算横地理坐标系下的系统误差状态：

x^c'(t)＝[ψ^c' δv^c' δr^c' ε^b ▽^b δk δη δγ]^T

其中，表示三维漂移误差角矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角；表示三维速度误差矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差；/>表示三维位置误差矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差；/>表示陀螺的零偏矢量，各分量分别为X、Y、Z轴陀螺的零偏；/>表示加速度计的零偏矢量，各分量分别为X、Y、Z轴加速度计的零偏；δk表示测速仪标度因数误差；δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差；

(6.1.2)确定计算横地理坐标系下惯性导航系统的姿态、速度和位置误差方程：

式中，表示计算横地理坐标系c'下地球自转角速度，/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度，/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵，f^c'表示计算横地理坐标系c'下表示的比力；

式中分别表示载体坐标系b下的陀螺误差和加速度计误差，表示为：

式中，和/>分别表示陀螺和加速度计的噪声；

(6.1.3)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差方程：

式中，τ_ε和τ_▽分别表示陀螺和加速度计的一阶马尔可夫相关时间，w_ε和w_▽分别表示陀螺和加速度计的高斯白噪声；

(6.2)确定测速仪的速度观测方程：

其中，

式中，表示惯性导航系统的速度估计值与测速仪的速度输出值；v^c'表示计算横地理坐标系c'下的速度矢量；

(6.3)确定系统误差状态修正方式：经滤波后的系统状态向量定义在计算横地理坐标系中，通过对系统状态进行修正使其定义在横地理坐标系t中：

φ^t＝ψ^c'+δθ^t

δv^t＝δv^c'-δθ^t×v^c'

/>

式中，φ^t表示姿态误差角φ在横地理坐标系t中的投影；δv^t为速度误差δv在横地理坐标系t中的投影；表示计算横地理坐标系c'到横地理坐标系t的方向余弦矩阵；δθ^t表示位置误差角δθ在横地理坐标系t中的投影，具体表示为：

式中，分别为δr^t的北向分量、东向分量；δr^t为位置误差δr在横地理坐标系t中的投影，其中经纬度误差转换为位置误差δr^t表示为：

δr^t＝[(R+h)δλ^t cosL^t(R+h)δL^tδh]^T

位置误差δr^t转换为经纬度误差表示为：

其中为位置误差δr^t的天向分量；

(7)舰船进入、离开极区时，确定系统姿态、速度、位置的转换关系，确定组合惯性/测速仪组合导航系统误差状态、协方差矩阵的转换关系，具体步骤如下：

(7.1)进入极区导航坐标系切换至横地理坐标系时，确定系统姿态、速度的转换关系为：

式中，表示载体坐标系b到地理坐标系g的方向余弦矩阵；v^g表示地理坐标系下的载体速度；

位置的转换关系为：

(7.2)驶出极区导航坐标系切换至地理坐标系时，确定系统姿态、速度的转换关系为：

式中，表示横地理坐标系t到地理坐标系g的方向余弦矩阵；

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

(7.3)确定组合惯性/测速仪组合导航系统误差状态转换关系，步骤如下：

(7.3.1)确定计算横地理坐标系下漂移误差角ψ^c'和计算地理坐标系下漂移误差角ψ^c的转换关系：

/>

式中表示计算地理坐标系c到计算横地理坐标系c'的方向余弦矩阵，具体表示为：

其中，为地球坐标系e到计算地理坐标系c的方向余弦矩阵，/>为横地球坐标系e′到计算横地理坐标系c'的方向余弦矩阵；

(7.3.2)确定计算横地理坐标系下的速度误差δv^c'与计算地理坐标系下的速度误差δv^c的转换关系：

(7.3.3)确定计算横地理坐标系下的位置误差δr^c'与计算地理坐标系下的位置误差δr^c的转换关系：

(7.4)确定组合惯性/测速仪组合导航系统协方差矩阵的转换关系，步骤如下：

根据步骤(7.3)所述，当舰船进入极区时，误差状态的转换关系表示为：

x^c'(t)＝Φx^c(t)

式中，x^c表示计算地理坐标系下的系统误差状态；Φ表示系统误差状态从计算地理坐标系c转换到计算横地理坐标系c'的转换矩阵，具体表达式为：

式中，diag{·}表示为块对角矩阵矩阵；I_3×3为3×3的单位矩阵；当舰船进入极区时并切换至横地理坐标系导航时，计算地理坐标系c下系统误差状态的协方差矩阵P^c(t)与计算横地理坐标系c'下系统误差状态的协方差矩阵P^c'(t)的转换关系表示为：

式中，表示计算横地理坐标系下的误差状态估计值，/>表示计算地理坐标系下的误差状态估计值；

当舰船离开极区并切换至地理坐标系导航时，误差状态和协方差矩阵转换关系表示为：

x^c(t)＝Φ^-1x^c'(t)，P^c(t)＝Φ^-1P^c'(t)Φ^-T。

进一步的，本发明所采用的组合导航滤波器对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈，测速仪标度因数误差和安装误差采用开环反馈，且每次闭环反馈的系统误差状态校正后置0。

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

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

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

进一步的，所述步骤(7)中，陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差三种系统状态在不同坐标系中保持一致，无需转换。

按照上述技术方案搭建基于Psi角的协方差变化方法，如图1所示为本方法的流程图。为验证本发明所提方法的有效性，以某惯导系统的低纬度船载实验和高纬度半实物仿真实验为例进行说明：

低纬度船载实验总时长为3小时，在航行1.5小时后由地理坐标系切换至横地理坐标系，高纬度半实物仿真实验是以低纬度船载实验数据为基础，按照文章(TransverseNavigation under the Ellipsoidal Earth Model and its Performance in bothPolar and Non-polar areas)中所涉及方案进行仿真，在纬度阈值达到84°时进行坐标系切换。导航过程中GNSS记录的位置信息为基准进行评估。

图2是低纬度船载实验中滤波器估计的位置误差曲线示意图，从图中可知，在切换过程中由于滤波器滤波状态和协方差矩阵的不连续导致了滤波误差短时剧烈振荡的情况，导致滤波器估计误差增大。图3是低纬度船载实验中的定位误差示意图，从图中可以看出，在切换坐标系后，经过本发明的方法处理后，定位误差明显下降，最终定位精度提高了41.3米。

图4是高纬度半实物仿真中滤波器估计的位置误差曲线示意图，从图中可知，在切换过程中滤波器估计误差振荡更加明显，因此在高纬度进行坐标系切换过程中更加不能忽略坐标系切换带来的影响，通过本发明所提方法进行处理后，滤波器估计误差下降近50％。图5是高纬度半实物仿真中的定位误差示意图，经过本发明的方法处理后，定位误差明显下降，最终定位精度提高了95.1米。

为进一步衡量本发明的有效性，多次修改切换时刻，图6展示了低纬度船载实验中不同切换时刻后100秒的标准差和均方根变化示意图，从图中可以看出，在经过本发明所提方法处理后，标准差和均方根均大幅度减小，降低幅度超过60％，意味着本发明在坐标系切换过程中可以有效减少误差振荡，实现平滑滤波。

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

Claims (6)

1.基于Psi角误差模型的舰船跨极区滤波切换方法，其特征在于，包括以下步骤：

(1)定义横地球坐标系，定义横向极点，定义横向经度和横向纬度，确定横向位置表示方式：所述横地球坐标系e′的原点位于地心，X轴沿着地球自转轴指向北极，Y轴指向本初子午线与赤道的交点，Z轴穿过东经90°子午线与赤道的交点；定义(0°,90°E)为横向北极点、(0°,90°W)为横向南极点；定义0°经线和180°经线组成的大椭圆为横向赤道；定义90°E和90°W北半球部分组成的半个大椭圆为0°横经线，且横向本初子午线为地理经度90°E所在的子午圈的北半球部分，横向子午线为过横向极点的平面与地球表面相交的轮廓线；定义地球表面上一点的地理法线与横向赤道面交角为该点的横向纬度；定义该点所在的横向子午面与横向本初子午面的交角为横向经度；根据构建的横经纬网络，将舰船在横地球坐标系中位置表示为(L^t，λ^t，h)，其中，其中L^t表示横纬度，λ^t表示横经度，h表示高度；

(2)定义横地理坐标系：横地理坐标系t的原点位于载体中心，Y轴沿横向经线的切线指向横向北极点，Z轴垂直于当地水平面指向天向，X轴与Y轴和Z轴构成右手坐标系，且为“横东-横北-天向”定义；

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

根据所述步骤(1)中横地球坐标系定义，确定地球坐标系e到横地球坐标系e′的方向余弦矩阵为：

确定地球坐标系e到地理坐标系g的方向余弦矩阵为：

其中L表示舰船所处的纬度，λ表示舰船所处的经度；

确定横地球坐标系e′到横地理坐标系t的方向余弦矩阵

根据链式法则，确定地理坐标系g到横地理坐标系t的方向余弦矩阵

式中表示为方向余弦矩阵/>的转置；

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

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

式中，表示从载体坐标系b到横地理坐标系t的方向余弦矩阵；/>表示载体坐标系b相对于惯性坐标系i的旋转角速度在载体坐标系b下的投影；/>表示横地理坐标系t相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影；

其中： 表示地球坐标系e相对于惯性坐标系i的旋转角速度在横地理坐标系t下的投影，/>表示横地理坐标系t相对于地球坐标系e的旋转角速度在横地理坐标系t下的投影；

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

其中，

式中，v^t表示横地理坐标系t下的载体速度；f^b表示载体坐标系b下表示的比力；g^t表示横地理坐标系t下表示的重力矢量；表示横地理坐标系t下载体的东向速度；/>表示横地理坐标系t下载体的北向速度；R为地球半径；ω_ie表示地球自转角速度的大小；/>表示从地球坐标系e到横地理坐标系t的方向余弦矩阵；

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

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

(5)确定计算坐标系、平台坐标系与真实导航坐标系之间的转换关系：

确定计算坐标系c到平台坐标系p的方向余弦矩阵为：

确定真实导航坐标系t_g到平台坐标系p的方向余弦矩阵为：

确定真实导航坐标系t_g到计算地理坐标系c的方向余弦矩阵为：

式中I_3×3表示3×3的单位矩阵；ψ为漂移误差角，φ为姿态误差角，δθ为位置误差角；

确定漂移误差角、姿态误差角、位置误差角之间的关系为：

φ＝ψ+δθ

(6)确定舰船在计算横地理坐标系下的卡尔曼滤波模型，包括以下步骤：

(6.1)确定计算横地理坐标系下的系统状态方程：

(6.1.1)确定计算横地理坐标系下的系统误差状态：

其中，表示三维漂移误差角矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角；表示三维速度误差矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差；/>表示三维位置误差矢量在计算横地理坐标系c'下的投影，各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差；/>表示陀螺的零偏矢量，各分量分别为X、Y、Z轴陀螺的零偏；/>表示加速度计的零偏矢量，各分量分别为X、Y、Z轴加速度计的零偏；δk表示测速仪标度因数误差；δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差；

(6.1.2)确定计算横地理坐标系下惯性导航系统的姿态、速度和位置误差方程：

式中，表示计算横地理坐标系c'下地球自转角速度，/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度，/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵，f^c'表示计算横地理坐标系c'下表示的比力；

式中分别表示载体坐标系b下的陀螺误差和加速度计误差，表示为：

式中，和/>分别表示陀螺和加速度计的噪声；

(6.1.3)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差方程：

式中，τ_ε和分别表示陀螺和加速度计的一阶马尔可夫相关时间，w_ε和/>分别表示陀螺和加速度计的高斯白噪声；

(6.2)确定测速仪的速度观测方程：

其中，

式中，表示惯性导航系统的速度估计值与测速仪的速度输出值；v^c'表示计算横地理坐标系c'下的速度矢量；

(6.3)确定系统误差状态修正方式：经滤波后的系统状态向量定义在计算横地理坐标系中，通过对系统状态进行修正使其定义在横地理坐标系t中：

φ^t＝ψ^c'+δθ^t

δv^t＝δv^c'-δθ^t×v^c'

式中，φ^t表示姿态误差角φ在横地理坐标系t中的投影；δv^t为速度误差δv在横地理坐标系t中的投影；表示计算横地理坐标系c'到横地理坐标系t的方向余弦矩阵；δθ^t表示位置误差角δθ在横地理坐标系t中的投影，具体表示为：

式中，分别为δr^t的北向分量、东向分量；δr^t为位置误差δr在横地理坐标系t中的投影，其中经纬度误差转换为位置误差δr^t表示为：

δr^t＝[(R+h)δλ^tcosL^t (R+h)δL^t δh]^T

位置误差δr^t转换为经纬度误差表示为：

其中为位置误差δr^t的天向分量；

(7)舰船进入、离开极区时，确定系统姿态、速度、位置的转换关系，确定组合惯性/测速仪组合导航系统误差状态、协方差矩阵的转换关系，具体步骤如下：

(7.1)进入极区导航坐标系切换至横地理坐标系时，确定系统姿态、速度的转换关系为：

式中，表示载体坐标系b到地理坐标系g的方向余弦矩阵；v^g表示地理坐标系下的载体速度；

位置的转换关系为：

(7.2)驶出极区导航坐标系切换至地理坐标系时，确定系统姿态、速度的转换关系为：

式中，表示横地理坐标系t到地理坐标系g的方向余弦矩阵；

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

(7.3)确定组合惯性/测速仪组合导航系统误差状态转换关系，步骤如下：

(7.3.1)确定计算横地理坐标系下漂移误差角ψ^c'和计算地理坐标系下漂移误差角ψ^c的转换关系：

式中表示计算地理坐标系c到计算横地理坐标系c'的方向余弦矩阵，具体表示为：

其中，为地球坐标系e到计算地理坐标系c的方向余弦矩阵，/>为横地球坐标系e′到计算横地理坐标系c'的方向余弦矩阵；

(7.3.2)确定计算横地理坐标系下的速度误差δv^c'与计算地理坐标系下的速度误差δv^c的转换关系：

(7.3.3)确定计算横地理坐标系下的位置误差δr^c'与计算地理坐标系下的位置误差δr^c的转换关系：

(7.4)确定组合惯性/测速仪组合导航系统协方差矩阵的转换关系，步骤如下：

根据步骤(7.3)所述，当舰船进入极区时，误差状态的转换关系表示为：

x^c'(t)＝Φx^c(t)

式中，x^c表示计算地理坐标系下的系统误差状态；Φ表示系统误差状态从计算地理坐标系c转换到计算横地理坐标系c'的转换矩阵，具体表达式为：

式中，diag{·}表示为块对角矩阵矩阵；I_3×3为3×3的单位矩阵；当舰船进入极区时并切换至横地理坐标系导航时，计算地理坐标系c下系统误差状态的协方差矩阵P^c(t)与计算横地理坐标系c'下系统误差状态的协方差矩阵P^c'(t)的转换关系表示为：

式中，表示计算横地理坐标系下的误差状态估计值，/>表示计算地理坐标系下的误差状态估计值；

当舰船离开极区并切换至地理坐标系导航时，误差状态和协方差矩阵转换关系表示为：

x^c(t)＝Φ^-1x^c'(t)，P^c(t)＝Φ^-1P^c'(t)Φ^-T。

2.如权利要求1所述的基于Psi角误差模型的舰船跨极区滤波切换方法，其特征在于，组合导航滤波器对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈，测速仪标度因数误差和安装误差采用开环反馈，且每次闭环反馈的系统误差状态校正后置0。

3.如权利要求1所述的基于Psi角误差模型的舰船跨极区滤波切换方法，其特征在于，若载体接收到其他传感器的位置信息，基于接收到的位置信息对转换关系或/>进行修正更新。

4.如权利要求1所述的基于Psi角误差模型的舰船跨极区滤波切换方法，其特征在于，所述步骤(7)中，在导航坐标系切换的同时，测速仪的速度观测量也进行切换，即切换前观测量为测速仪的速度在计算地理坐标系下的投影，切换后观测量为测速仪的速度在计算横地理坐标系下的投影。

5.如权利要求1所述的基于Psi角误差模型的舰船跨极区滤波切换方法，其特征在于，所述步骤(7)中，导航坐标系的切换以纬度作为阈值，纬度阈值的选择根据实际情况选择，当载体所在位置的纬度大于设定的阈值，导航系切换至横地理坐标系，当载体所在位置的纬度小于阈值，导航系切换至地理坐标系。

6.如权利要求1所述的基于Psi角误差模型的舰船跨极区滤波切换方法，其特征在于，所述步骤(7)中，陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差三种系统状态在不同坐标系中保持一致，无需转换。