CN117516518A - 一种地球椭球模型下的跨极区阻尼切换方法 - Google Patents

Abstract

本发明属于惯性导航技术领域，公开了一种地球椭球模型下的跨极区阻尼切换方法，适用于舰船长航时跨极区航行导航。本发明基于椭球模型，构建了惯性导航系统误差状态方程，立足于现代控制理论，利用卡尔曼最优估计实现动态阻尼网络和系统极点自动配置，实现纯惯性导航振荡误差的阻尼。同时针对跨极区阻尼切换的超调误差问题，设计了阻尼切换方案使系统参数与阻尼系统同步切换，避免阻尼超调误差。本发明实现了导航过程中的动态阻尼，避免了切换时刻传统阻尼方案中的阻尼超调振荡问题。所构建的跨极区阻尼切换方案可以保证长航时跨极区航行对定位精度的要求，可以进一步提高舰船全纬度长航时导航能力。

Description

一种地球椭球模型下的跨极区阻尼切换方法

技术领域

本发明属于惯性导航技术领域，涉及惯性导航系统阻尼方法，特别涉及一种地球椭球模型下的跨极区阻尼切换方法，适用于舰船的长航时跨极区航行。

背景技术

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

舰船极区航行导航通常要求导航设备具备长航时导航能力，然而惯性导航系统的误差随时间积累，长航时导航必然导致定位精度下降。此外，纯惯性导航系统的水平通道是无阻尼的，系统产生了三种周期性振荡误差，即舒勒振荡、傅科振荡、地球自转周期振荡，在长航时导航中振荡误差严重影响导航定位精度。根据现代控制理论分析，纯惯性导航的系统误差方程极点存在于复平面的虚轴上，为无阻尼振荡系统，需要进行极点配置实现系统阻尼，提高导航精度。

传统惯性导航编排方案工作在极区面临着计算溢出、失去航向参考等问题，横向导航方案常用于解决传统方案在极区的导航难题，然而为了简便，大多横向导航方案基于地球圆球模型设计，无法满足长航时导航对定位精度的需要。此外，利用横向导航方案意味着跨极区导航航行将采用两种导航坐标系，以适应极区和非极区不同的地理特点，这必然涉及到阻尼切换问题，由于传统阻尼系统为定阻尼网络设计，在导航坐标系切换时刻无法自动调节阻尼系统，造成系统的阻尼超调和不稳定。

本发明针对目前存在的问题，面向长航时跨极区航海导航，必须重点解决：1.设计基于地球椭球模型的横向编排方案，保证模型精度；2.变阻尼网络的设计，实现系统自动配置阻尼系数，使惯性导航系统工作稳定，振荡误差得到抑制；3.阻尼切换时刻阻尼系数的同步切换，避免阻尼超调误差。因此本发明提出了一种地球椭球模型下的跨极区阻尼切换方法：通过引入测速仪为外速度参考，构建了地球椭球模型的横向导航方案，建立横向系统方程并配置合适极点；通过卡尔曼最优估计设计变阻尼网络；构建切换方案使阻尼系数和系统参数同时切换。本发明可以实现纯惯性导航振荡误差的阻尼，抑制误差积累，避免切换时刻阻尼超调误差，可以保证长航时导航精度，具有十分重要的工程意义。

发明内容

在极区长航时航海导航领域，更高的自主导航精度需要更精准的地球模型和惯性阻尼系统。传统阻尼系统常常基于定阻尼网络设计，在面临跨极区坐标系切换时会产生超调振荡。本发明要解决的技术问题就在于：针对现有技术的不足，本发明提出一种地球椭球模型下的跨极区阻尼切换方法，基于地球椭球模型构建横向系统导航方案，立足于现代控制理论对系统状态空间进行分析，采用卡尔曼最优估计方法实现变阻尼网络和极点自动配置，使系统工作在稳定的状态，提高系统稳定性；构建变阻尼系统跨极区切换方案，实现阻尼系数和参数同时切换，解决阻尼切换超调问题。

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

一种地球椭球模型下的跨极区阻尼切换方法，所述方法包括以下步骤：

(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的方向余弦矩阵

式中表示为方向余弦矩阵的转置；σ表示横地理坐标系t与地理坐标系g之间的夹角，具体表示为：

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

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

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

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

其中，表示地球自转角速度在地球坐标系e下的投影，ω_ie表示地球自转角速度的大小；表示从地球坐标系e到横地理坐标系t的方向余弦矩阵；表示地理坐标系g相对于地球坐标系e的旋转角速度在地理坐标系g下的投影；表示横地理坐标系t相对于地理坐标系g的旋转角速度在横地理坐标系t下的投影；

和具体表示为：

其中，和分别表示地理坐标系下载体的北向速度和东向速度；R_M表示载体处的子午圈半径，R_N表示载体处的卯酉圈半径，具体表示为：

其中，R_e表示地球长半轴半径，ρ表示地球的偏心率；

确定

式中，和分别为横地理坐标系t下载体东向和北向的速度；表示横地理坐标系下载体处的扭曲率，和分别为横地理东向和横地理北向的曲率，具体表示为：

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

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

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

步骤(4.1)所述的参数其变化由横向经度、横向纬度的变化所引起，具体表示为：

对比步骤(4.1)所确定的参数确定横向经度、横向纬度微分方程：

高度变化由天向速度所引起，确定高度微分方程：

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

(5)确定横地理坐标系下阻尼系统的误差模型：

(5.1)确定横地理坐标系下阻尼系统的状态方程：

其中x^t为横地理坐标系下的系统状态向量；F为状态转移矩阵；G为系统噪声分配矩阵；w为系统噪声向量；K为阻尼系数矩阵；u为阻尼系统反馈矩阵；

将横地理坐标系下的系统状态向量x^t表示为：

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

其中，表示三维姿态误差角矢量在横地理坐标系t下的投影，各分量分别为横地理坐标系t下东向、北向、天向的姿态误差角；表示三维速度误差矢量在横地理坐标系t下的投影，各分量分别为横地理坐标系t下东向、北向、天向的速度误差；δr^t＝[δL^t δλ^t δh]^T表示在横地理坐标系t下的纬度误差、经度误差和高度误差；表示陀螺的零偏矢量，各分量分别为X、Y、Z轴陀螺的零偏；表示加速度计的零偏矢量，各分量分别为X、Y、Z轴加速度计的零偏；δk表示测速仪标度因数误差；δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差；

(5.2)确定横地理坐标系下阻尼系统的误差方程：

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

其中，和分别为旋转角速度矢量和的误差，它们之间的关系表示为：

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

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

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

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

(6)确定阻尼系统的阻尼系数矩阵K：

步骤(5)所述阻尼系数矩阵K与卡尔曼最优估计的增益系数一致，即建立卡尔曼最优估计的状态方程和观测方程即获得阻尼系数矩阵K；由于卡尔曼最优估计是依据惯性导航系统不同时刻不同状态的特征进行估计，因此其增益系数为时变的，进而实现系统的变阻尼网络；

(6.1)确定卡尔曼最优估计系统状态方程：

其中系统状态向量x^t、状态转移矩阵F、系统噪声分配矩阵G、系统噪声向量w与步骤(5)所述相同；

(6.2)确定卡尔曼最优估计系统观测方程：

其中，表示惯性导航系统的速度估计值与测速仪的速度输出值；υ为等效噪声；为测速仪安装误差矩阵；v^t表示横地理坐标系t下的速度矢量；υ为测量噪声向量；H为状态观测矩阵，具体表示为：

式中，H₁和H₂分别为矩阵的第一列和第三列向量；

(6.3)确定阻尼系数矩阵K：

根据卡尔曼最优估计方法，其中预测过程为：

更新过程为：

式中，为系统第n-1时刻的误差状态，为系统第n时刻预测的误差状态，为系统第n时刻的误差状态；F_n/n-1为从n-1时刻至n时刻的状态转移矩阵；为系统第n-1时刻误差状态的协方差矩阵，为系统第n时刻预测的误差状态的协方差矩阵，为系统第n时刻误差状态的协方差矩阵；Q_n-1为系统第n-1时刻噪声向量的协方差矩阵；B_n-1为系统第n-1时刻系统噪声分配矩阵；H_n为系统第n时刻系统的观测矩阵；R_n为系统第n时刻测量噪声向量的协方差矩阵；z_n为系统第n时刻系统测量误差向量；K_n为系统第n时刻的卡尔曼最优估计增益矩阵，即系统阻尼系数矩阵；I_18×18表示18×18的单位矩阵；

(7)确定跨极区导航阻尼切换方案：

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

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

确定位置的转换关系为：

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

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

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

(7.3)确定卡尔曼最优估计系统误差状态的转换关系，步骤如下：

(7.3.1)确定横地理坐标系下的姿态误差φ^t与地理坐标系下的姿态误差φ^g的转换关系：

式中，参数τ^g具体表示为：

其中，δL、δλ和δh分别为地理坐标系下的纬度误差、经度误差和高度误差；参数τ^t具体表示为：

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

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

(7.4)确定卡尔曼最优估计系统协方差矩阵的转换关系，步骤如下：

根据步骤(7.3)所述，舰船进入极区并切换至横地理坐标系时，将误差状态的转换关系表示为：

x^t(t)＝Φx^g(t)

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

当舰船进入极区时并切换至横地理坐标系导航时，地理坐标系g下系统误差状态的协方差矩阵P^g(t)与横地理坐标系t下系统误差状态的协方差矩阵P^t(t)的转换关系表示为：

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

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

x^g(t)＝Φ-¹x^t(t)，P^g(t)＝Φ^-1P^t(t)Φ^-T。

进一步的，本发明的卡尔曼最优估计对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈，测速仪标度因数误差和安装误差采用开环反馈，且每次闭环反馈的系统误差状态校正后置0。

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

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

本发明基于地球椭球模型构建了横向导航方案，采用现代控制理论设计了变阻尼网络，利用卡尔曼最优估计实现阻尼系数的时变估计和极点自动配置，实现纯惯性导航的振荡误差抑制，提高系统的稳定性；针对跨极区长航时导航场景，设计了阻尼系统切换方案，使系统实现坐标系切换过程中的平稳过渡，避免传统阻尼切换的超调误差。本发明使用的地球椭球模型更加精准，降低模型近似误差，变阻尼网络设计和切换方案可以提高舰船跨极区航行的稳定性，满足系统长航时工作需要，有利于提高长航时导航精度。

附图说明

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

具体实施方式

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

如图1所示，一种地球椭球模型下的跨极区阻尼切换方法，具体实施方式如下：

(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的方向余弦矩阵

式中表示为方向余弦矩阵的转置；σ表示横地理坐标系t与地理坐标系g之间的夹角，具体表示为：

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

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

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

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

其中，表示地球自转角速度在地球坐标系e下的投影，ω_ie表示地球自转角速度的大小；表示从地球坐标系e到横地理坐标系t的方向余弦矩阵；表示地理坐标系g相对于地球坐标系e的旋转角速度在地理坐标系g下的投影；表示横地理坐标系t相对于地理坐标系g的旋转角速度在横地理坐标系t下的投影；

和具体表示为：

其中，和分别表示地理坐标系下载体的北向速度和东向速度；R_M表示载体处的子午圈半径，R_N表示载体处的卯酉圈半径，具体表示为：

其中，R_e表示地球长半轴半径，ρ表示地球的偏心率；

确定

式中，和分别为横地理坐标系t下载体东向和北向的速度；表示横地理坐标系下载体处的扭曲率，和分别为横地理东向和横地理北向的曲率，具体表示为：

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

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

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

步骤(4.1)所述的参数其变化由横向经度、横向纬度的变化所引起，具体表示为：

对比步骤(4.1)所确定的参数确定横向经度、横向纬度微分方程：

高度变化由天向速度所引起，确定高度微分方程：

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

(5)确定横地理坐标系下阻尼系统的误差模型：

(5.1)确定横地理坐标系下阻尼系统的状态方程：

其中x^t为横地理坐标系下的系统状态向量；F为状态转移矩阵；G为系统噪声分配矩阵；w为系统噪声向量；K为阻尼系数矩阵；u为阻尼系统反馈矩阵；

将横地理坐标系下的系统状态向量x^t表示为：

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

其中，表示三维姿态误差角矢量在横地理坐标系t下的投影，各分量分别为横地理坐标系t下东向、北向、天向的姿态误差角；表示三维速度误差矢量在横地理坐标系t下的投影，各分量分别为横地理坐标系t下东向、北向、天向的速度误差；δr^t＝[δL^t δλ^t δh]^T表示在横地理坐标系t下的纬度误差、经度误差和高度误差；表示陀螺的零偏矢量，各分量分别为X、Y、Z轴陀螺的零偏；表示加速度计的零偏矢量，各分量分别为X、Y、Z轴加速度计的零偏；δk表示测速仪标度因数误差；δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差；

(5.2)确定横地理坐标系下阻尼系统的误差方程：

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

其中，和分别为旋转角速度矢量和的误差，它们之间的关系表示为：

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

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

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

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

(6)确定阻尼系统的阻尼系数矩阵K：

步骤(5)所述阻尼系数矩阵K与卡尔曼最优估计的增益系数一致，即建立卡尔曼最优估计的状态方程和观测方程即获得阻尼系数矩阵K；由于卡尔曼最优估计是依据惯性导航系统不同时刻不同状态的特征进行估计，因此其增益系数为时变的，进而实现系统的变阻尼网络；

(6.1)确定卡尔曼最优估计系统状态方程：

其中系统状态向量x^t、状态转移矩阵F、系统噪声分配矩阵G、系统噪声向量w与步骤(5)所述相同；

(6.2)确定卡尔曼最优估计系统观测方程：

其中，表示惯性导航系统的速度估计值与测速仪的速度输出值；υ为等效噪声；为测速仪安装误差矩阵；v^t表示横地理坐标系t下的速度矢量；υ为测量噪声向量；H为状态观测矩阵，具体表示为：

H＝[-[v^t×] I_3×3 0_3×3 0_3×3 0_3×3 -v^t H₁ H₂]

式中，H₁和H₂分别为矩阵的第一列和第三列向量；

(6.3)确定阻尼系数矩阵K：

根据卡尔曼最优估计方法，其中预测过程为：

更新过程为：

式中，为系统第n-1时刻的误差状态，为系统第n时刻预测的误差状态，为系统第n时刻的误差状态；F_n/n-1为从n-1时刻至n时刻的状态转移矩阵；为系统第n-1时刻误差状态的协方差矩阵，为系统第n时刻预测的误差状态的协方差矩阵，为系统第n时刻误差状态的协方差矩阵；Q_n-1为系统第n-1时刻噪声向量的协方差矩阵；B_n-1为系统第n-1时刻系统噪声分配矩阵；H_n为系统第n时刻系统的观测矩阵；R_n为系统第n时刻测量噪声向量的协方差矩阵；z_n为系统第n时刻系统测量误差向量；K_n为系统第n时刻的卡尔曼最优估计增益矩阵，即系统阻尼系数矩阵；I_18×18表示18×18的单位矩阵；

(7)确定跨极区导航阻尼切换方案：

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

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

确定位置的转换关系为：

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

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

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

(7.3)确定卡尔曼最优估计系统误差状态的转换关系，步骤如下：

(7.3)确定卡尔曼最优估计系统误差状态的转换关系，步骤如下：

(7.3.1)确定横地理坐标系下的姿态误差φ^t与地理坐标系下的姿态误差φ^g的转换关系：

式中，参数τ^g具体表示为：

其中，δL、δλ和δh分别为地理坐标系下的纬度误差、经度误差和高度误差；参数τ^t具体表示为：

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

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

(7.4)确定卡尔曼最优估计系统协方差矩阵的转换关系，步骤如下：

根据步骤(7.3)所述，舰船进入极区并切换至横地理坐标系时，将误差状态的转换关系表示为：

x^t(t)＝Φx^g(t)

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

当舰船进入极区时并切换至横地理坐标系导航时，地理坐标系g下系统误差状态的协方差矩阵P^g(t)与横地理坐标系t下系统误差状态的协方差矩阵P^t(t)的转换关系表示为：

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

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

x^g(t)＝Φ-¹x^t(t)，P^g(t)＝Φ^-1P^t(t)Φ^-T。

进一步的，本发明的卡尔曼最优估计对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈，测速仪标度因数误差和安装误差采用开环反馈，且每次闭环反馈的系统误差状态校正后置0。

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

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

Claims (3)

1.一种地球椭球模型下的跨极区阻尼切换方法，其特征在于，包括以下步骤：

(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的方向余弦矩阵

式中表示为方向余弦矩阵的转置；σ表示横地理坐标系t与地理坐标系g之间的夹角，具体表示为：

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

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

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

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

其中，表示地球自转角速度在地球坐标系e下的投影，ω_ie表示地球自转角速度的大小；表示从地球坐标系e到横地理坐标系t的方向余弦矩阵；表示地理坐标系g相对于地球坐标系e的旋转角速度在地理坐标系g下的投影；表示横地理坐标系t相对于地理坐标系g的旋转角速度在横地理坐标系t下的投影；

和具体表示为：

其中，和分别表示地理坐标系下载体的北向速度和东向速度；R_M表示载体处的子午圈半径，R_N表示载体处的卯酉圈半径，具体表示为：

其中，R_e表示地球长半轴半径，ρ表示地球的偏心率；

确定

式中，和分别为横地理坐标系t下载体东向和北向的速度；表示横地理坐标系下载体处的扭曲率，和分别为横地理东向和横地理北向的曲率，具体表示为：

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

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

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

步骤(4.1)所述的参数其变化由横向经度、横向纬度的变化所引起，具体表示为：

对比步骤(4.1)所确定的参数确定横向经度、横向纬度微分方程：

高度变化由天向速度所引起，确定高度微分方程：

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

(5)确定横地理坐标系下阻尼系统的误差模型：

(5.1)确定横地理坐标系下阻尼系统的状态方程：

其中x^t为横地理坐标系下的系统状态向量；F为状态转移矩阵；G为系统噪声分配矩阵；w为系统噪声向量；K为阻尼系数矩阵；u为阻尼系统反馈矩阵；

将横地理坐标系下的系统状态向量x^t表示为：

其中，表示三维姿态误差角矢量在横地理坐标系t下的投影，各分量分别为横地理坐标系t下东向、北向、天向的姿态误差角；表示三维速度误差矢量在横地理坐标系t下的投影，各分量分别为横地理坐标系t下东向、北向、天向的速度误差；δr^t＝[δL^t δλ^t δh]^T表示在横地理坐标系t下的纬度误差、经度误差和高度误差；表示陀螺的零偏矢量，各分量分别为X、Y、Z轴陀螺的零偏；表示加速度计的零偏矢量，各分量分别为X、Y、Z轴加速度计的零偏；δk表示测速仪标度因数误差；δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差；

(5.2)确定横地理坐标系下阻尼系统的误差方程：

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

其中，和分别为旋转角速度矢量和的误差，它们之间的关系表示为：

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

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

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

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

(6)确定阻尼系统的阻尼系数矩阵K：

步骤(5)所述阻尼系数矩阵K与卡尔曼最优估计的增益系数一致，即建立卡尔曼最优估计的状态方程和观测方程即获得阻尼系数矩阵K；由于卡尔曼最优估计是依据惯性导航系统不同时刻不同状态的特征进行估计，因此其增益系数为时变的，进而实现系统的变阻尼网络；

(6.1)确定卡尔曼最优估计系统状态方程：

其中系统状态向量x^t、状态转移矩阵F、系统噪声分配矩阵G、系统噪声向量w与步骤(5)所述相同；

(6.2)确定卡尔曼最优估计系统观测方程：

其中，表示惯性导航系统的速度估计值与测速仪的速度输出值；υ为等效噪声；为测速仪安装误差矩阵；υ为测量噪声向量；H为状态观测矩阵，具体表示为：

H＝[-[v^t×] I_3×3 0_3×3 0_3×3 0_3×3 -v^t H₁ H₂]

式中，H₁和H₂分别为矩阵的第一列和第三列向量；

(6.3)确定阻尼系数矩阵K：

根据卡尔曼最优估计方法，其中预测过程为：

更新过程为：

式中，为系统第n-1时刻的误差状态，为系统第n时刻预测的误差状态，为系统第n时刻的误差状态；F_n/n-1为从n-1时刻至n时刻的状态转移矩阵；为系统第n-1时刻误差状态的协方差矩阵，为系统第n时刻预测的误差状态的协方差矩阵，为系统第n时刻误差状态的协方差矩阵；Q_n-1为系统第n-1时刻噪声向量的协方差矩阵；B_n-1为系统第n-1时刻系统噪声分配矩阵；H_n为系统第n时刻系统的观测矩阵；R_n为系统第n时刻测量噪声向量的协方差矩阵；z_n为系统第n时刻系统测量误差向量；K_n为系统第n时刻的卡尔曼最优估计增益矩阵，即系统阻尼系数矩阵；I_18×18表示18×18的单位矩阵；

(7)确定跨极区导航阻尼切换方案：

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

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

确定位置的转换关系为：

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

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

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

(7.3)确定卡尔曼最优估计系统误差状态的转换关系，步骤如下：

(7.3.1)确定横地理坐标系下的姿态误差φ^t与地理坐标系下的姿态误差φ^g的转换关系：

式中，参数τ^g具体表示为：

其中，δL、δλ和δh分别为地理坐标系下的纬度误差、经度误差和高度误差；参数τ^t具体表示为：

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

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

(7.4)确定卡尔曼最优估计系统协方差矩阵的转换关系，步骤如下：

根据步骤(7.3)所述，舰船进入极区并切换至横地理坐标系时，将误差状态的转换关系表示为：

x^t(t)＝Φx^g(t)

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

当舰船进入极区时并切换至横地理坐标系导航时，地理坐标系g下系统误差状态的协方差矩阵P^g(t)与横地理坐标系t下系统误差状态的协方差矩阵P^t(t)的转换关系表示为：

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

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

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

2.如权利要求1所述的一种地球椭球模型下的跨极区阻尼切换方法，其特征在于，卡尔曼最优估计对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈，测速仪标度因数误差和安装误差采用开环反馈，且每次闭环反馈的系统误差状态校正后置0。

3.如权利要求1所述的一种地球椭球模型下的跨极区阻尼切换方法，其特征在于，若载体接收到其他传感器的位置信息，基于接收到的位置信息对转换关系或进行修正更新。