CN117470235B - 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 - Google Patents
一种优化的地球椭球模型下长航时跨极区阻尼切换方法 Download PDFInfo
- Publication number
- CN117470235B CN117470235B CN202311492703.6A CN202311492703A CN117470235B CN 117470235 B CN117470235 B CN 117470235B CN 202311492703 A CN202311492703 A CN 202311492703A CN 117470235 B CN117470235 B CN 117470235B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- error
- abscissa
- matrix
- determining
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 110
- 238000013016 damping Methods 0.000 title claims abstract description 96
- 230000008569 process Effects 0.000 claims abstract description 39
- 230000008859 change Effects 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 144
- 238000006243 chemical reaction Methods 0.000 claims description 63
- 239000013598 vector Substances 0.000 claims description 57
- 238000009434 installation Methods 0.000 claims description 15
- 230000007704 transition Effects 0.000 claims description 10
- 238000012937 correction Methods 0.000 claims description 9
- 238000005259 measurement Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 5
- 230000014509 gene expression Effects 0.000 claims description 4
- 230000010355 oscillation Effects 0.000 abstract description 11
- 238000013461 design Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000009897 systematic effect Effects 0.000 description 3
- 230000000737 periodic effect Effects 0.000 description 2
- 238000005727 Friedel-Crafts reaction Methods 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/13—Differential equations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Automation & Control Theory (AREA)
- Mathematical Analysis (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Geometry (AREA)
- Operations Research (AREA)
- Evolutionary Computation (AREA)
- Computer Hardware Design (AREA)
- Computing Systems (AREA)
- Navigation (AREA)
Abstract
本发明属于惯性导航技术领域,公开了一种优化的地球椭球模型下长航时跨极区阻尼切换方法,适用于舰船长航时跨极区航行导航。本发明基于地球椭球模型,利用Psi角误差模型定义在计算坐标系的特点,构建了更简单的横向导航方案。立足于现代控制理论,利用卡尔曼最优估计实现阻尼系数的动态变化和极点自动配置。同时设计了阻尼切换方案使系统参数与阻尼系统同步切换,避免跨极区阻尼切换产生的超调误差。本发明实现了导航过程中的动态阻尼,阻尼切换方法避免了切换时刻定阻尼网络带来的超调振荡,所构建的优化的地球椭球模型导航方法在减少系统模型误差的同时,保证了系统导航精度,具有重要的工程意义,适用于舰船长航时跨极区航行导航。
Description
技术领域
本发明属于惯性导航技术领域,涉及惯性导航系统阻尼方法,特别涉及一种优化的地球椭球模型下长航时跨极区阻尼切换方法,适用于舰船的长航时跨极区航行。
背景技术
极地地区尤其是北极地区在资源、科研、航道等方面具有重要战略价值,以美国、俄罗斯为代表的各个大国加快极地地区的战略布局及考察研究。由于惯性导航拥有极好的自主性,并且不受极地地区恶劣环境的影响,因此惯性导航已经成为极区重要的导航手段。
舰船极区航行导航通常要求导航设备具备长航时导航能力,然而惯性导航系统的误差随时间积累,长航时导航必然导致定位精度下降。此外,纯惯性导航系统的水平通道是无阻尼的,系统产生了三种周期性振荡误差,即舒勒振荡、傅科振荡、地球自转周期振荡,在长航时导航中振荡误差严重影响导航定位精度。根据现代控制理论分析,纯惯性导航的系统误差方程极点存在于复平面的虚轴上,为无阻尼振荡系统,需要进行极点配置实现系统阻尼,提高导航精度。
传统惯性导航编排方案工作在极区面临着计算溢出、失去航向参考等问题,横向坐标系常被用以解决这些难题,然而为了简化系统方程,有关横向导航系统的研究大多是基于地球圆球模型而建立,不适用于极区长航时航海导航,然而采用地球椭球模型的横向坐标系机械编排十分复杂,一些与地球模型相关的系统误差参数解析表达式冗长繁琐,给系统计算带来计算负担。此外,利用横向坐标系的编排方案意味着跨极区导航航行将采用两种导航坐标系,以适应极区和非极区不同的地理特点,这必然涉及到阻尼切换问题,但是传统阻尼网络为定阻尼系统,其无法适应切换时刻给系统带来的影响进而产生阻尼超调误差,造成系统短时的不稳定性。
本发明针对目前存在的问题,面向长航时跨极区航海导航,必须重点解决:1.优化地球椭球模型下横坐标系的编排方案,保证导航精度的同时降低系统计算负担。2.变阻尼网络的设计,实现系统自动配置合适极点,使惯性导航系统工作稳定,振荡误差得到抑制,提高不同环境下的系统稳定性;3.阻尼切换时刻阻尼系数的同步切换,避免阻尼超调误差。因此本发明提出了一种优化的地球椭球模型下长航时跨极区阻尼切换方法:通过引入测速仪为外速度参考,构建了基于Psi角误差模型的横坐标系导航方案,降低椭球模型给系统带来的计算负担,同时构建系统方程并配置合适极点;通过卡尔曼最优估计设计变阻尼网络;构建切换方案使阻尼系数和系统参数同时切换。本发明可以实现振荡误差的阻尼,抑制误差积累,避免切换时刻阻尼超调误差,可以提高长航时导航精度,具有十分重要的工程意义。
发明内容
在极区长航时航海导航领域,更高的自主导航精度需要更精准的地球模型和惯性阻尼系统。基于地球圆球模型的横向导航方案常能够解决解决传统惯性惯性导航方案在极地面临的计算溢出、失去航向参考等问题,但是其带来的地球模型近似误差无法满足长航时航海导航对定位精度的需求。为进一步提高导航精度,地球椭球模型的横向导航方案设计是必要的,然而在地球椭球模型下,横向机械编排方案涉及到与地球模型相关的误差参数以及误差方程求解,解析求解过程十分复杂,给系统带来了巨大的计算负担。此外传统阻尼系统常常基于定阻尼网络设计,在面临跨极区坐标系切换时,阻尼网络无法切换进而产生超调振荡。本发明要解决的技术问题就在于:针对现有技术的不足,本发明提出一种优化的地球椭球模型下长航时跨极区阻尼切换方法,利用Psi角误差模型定义在计算坐标系的特点,优化地球椭球模型下横向导航方案;采用现代控制理论对系统状态空间进行分析,配置合适的极点使系统工作在稳定的状态,采用卡尔曼最优估计方法实现变阻尼系数,增加阻尼系统稳定性;构建变阻尼系统跨极区切换方案,实现阻尼系数和参数同时切换,解决阻尼切换超调问题。
为解决上述技术问题,本发明提出的解决方案为:
一种优化的地球椭球模型下长航时跨极区阻尼切换方法,所述方法包括以下步骤:
(1)定义横地球坐标系e′,定义横向极点,定义横向经度和横向纬度,确定横向位置表示方式:所述横地球坐标系e′的原点位于地心,X轴沿着地球自转轴指向北极,Y轴指向本初子午线与赤道的交点,Z轴穿过东经90°子午线与赤道的交点;定义(0°,90°E)为横向北极点、(0°,90°W)为横向南极点;定义0°经线和180°经线组成的大椭圆为横向赤道;定义90°E和90°W北半球部分组成的半个大椭圆为0°横经线,且横向本初子午线为地理经度90°E所在的子午圈的北半球部分,横向子午线为过横向极点的平面与地球表面相交的轮廓线;定义地球表面上一点的地理法线与横向赤道面交角为该点的横向纬度;定义该点所在的横向子午面与横向本初子午面的交角为横向经度;根据构建的横经纬网络,将舰船在横地球坐标系中位置表示为(Lt,λt,h),其中,Lt表示横纬度,λ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下的投影;
和/>具体表示为:
其中,和/>分别表示地理坐标系下载体的北向速度和东向速度;RM表示载体处的子午圈半径,RN表示载体处的卯酉圈半径,具体表示为:
其中,Re表示地球长半轴半径,ρ表示地球的偏心率;
确定
式中,和/>分别为横地理坐标系t下载体东向和北向的速度;/>表示横地理坐标系下载体处的扭曲率,/>和/>分别为横地理东向和横地理北向的曲率,具体表示为:
(4.2)确定横地理坐标系下的速度vt的更新方程:
式中,vt表示横地理坐标系t下的载体速度;fb表示载体坐标系b下表示的比力;gt表示横地理坐标系t下表示的重力矢量;
(4.3)确定横地理坐标系下的位置更新方程:
步骤(4.1)所述的参数其变化由横向经度、横向纬度的变化所引起,具体表示为:
对比步骤(4.1)所确定的参数确定横向经度、横向纬度微分方程:
高度变化由天向速度所引起,确定高度微分方程:
式中,表示横地理坐标系t下载体的天向速度;
(5)确定计算地理坐标系、平台坐标系与真实导航坐标系之间的转换关系:
确定计算地理坐标系c到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到计算地理坐标系c的方向余弦矩阵为:
式中I3×3表示3×3的单位矩阵;ψ为漂移误差角,φ为姿态误差角,δθ为位置误差角;确定漂移误差角、姿态误差角、位置误差角之间的关系为:
φ=ψ+δθ
(6)确定计算横地理坐标系下阻尼系统的误差模型:
(6.1)确定计算横地理坐标系下阻尼系统的状态方程:
其中xc'为系统状态向量;F为状态转移矩阵;G为系统噪声分配矩阵;w为系统噪声向量;K为阻尼系数矩阵;u为阻尼系统反馈矩阵;
将系统状态向量xc'表示为:
其中,表示三维漂移误差角矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角;表示三维速度误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差;/>表示三维位置误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差;/>表示陀螺的零偏矢量,各分量分别为X、Y、Z轴陀螺的零偏;/>表示加速度计的零偏矢量,各分量分别为X、Y、Z轴加速度计的零偏;δk表示测速仪标度因数误差;δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差;
(6.2)确定计算横地理坐标系下阻尼系统的误差方程:
(6.2.1)确定计算横坐标系下惯性导航系统的姿态、速度和位置误差方程:
式中,表示计算横地理坐标系c'下地球自转角速度,/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度,/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵,fc'表示计算横地理坐标系c'下表示的比力;
式中分别表示载体坐标系b下的陀螺误差和加速度计误差,表示为:
式中,和/>分别表示陀螺和加速度计的噪声;
其中,姿态和速度误差微分方程投影在计算横地理坐标系下,均为已知确定量,姿态误差微分方程中不存在与/>相关的扰动误差项,速度误差微分方程中不存在与/>相关的扰动误差项,即不存在与地球椭球模型下地理位置相关的误差项;
(6.2.2)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差方程:
式中,τε和分别表示陀螺和加速度计的一阶马尔可夫相关时间,wε和/>分别表示陀螺和加速度计的高斯白噪声;
将计算横地理坐标系下阻尼系统表示为:
其中,表示惯性导航系统的速度估计值与测速仪的速度输出值;03×3表示3×3的零矩阵;
(7)确定阻尼系统的阻尼系数矩阵K:
步骤(6)所述阻尼系数矩阵K与卡尔曼最优估计的增益系数一致,即建立卡尔曼最优估计的状态方程和观测方程即获得阻尼系数矩阵K;由于卡尔曼最优估计是依据惯性导航系统不同时刻不同状态的特征进行估计,因此其增益系数为时变的,进而实现系统的变阻尼网络;
(7.1)确定卡尔曼最优估计系统状态方程:
其中系统状态向量xc'、状态转移矩阵F、系统噪声分配矩阵G、系统噪声向量w与步骤(6)所述相同;
(7.2)确定卡尔曼最优估计系统观测方程:
其中,为测速仪安装误差矩阵;vc'表示计算横地理坐标系c'下的速度矢量;υ为测量噪声向量;H为状态观测矩阵,将其表示为:
H=[-[vc'×] I3×3 03×3 03×3 03×3 -vc' H1 H2]
式中,H1和H2分别为矩阵的第一列和第三列向量;
(7.3)确定阻尼系数矩阵K:
根据卡尔曼最优估计方法,其中预测过程为:
更新过程为:
式中,为系统第n-1时刻的误差状态,/>为系统第n时刻预测的误差状态,/>为系统第n时刻的误差状态;Fn/n-1为从n-1时刻至n时刻的状态转移矩阵;/>为系统第n-1时刻误差状态的协方差矩阵,/>为系统第n时刻预测的误差状态的协方差矩阵,/>为系统第n时刻误差状态的协方差矩阵;Qn-1为系统第n-1时刻噪声向量的协方差矩阵;Bn-1为系统第n-1时刻系统噪声分配矩阵;Hn为系统第n时刻系统的观测矩阵;Rn为系统第n时刻测量噪声向量的协方差矩阵;zn为系统第n时刻系统测量误差向量;Kn为系统第n时刻的卡尔曼最优估计增益矩阵,即系统阻尼系数矩阵;I18×18表示18×18的单位矩阵;
(7.4)确定系统误差状态修正方式:卡尔曼最优估计的系统状态向量定义在计算横地理坐标系中,通过对系统状态进行修正使其定义在横地理坐标系t中:
φt=ψc'+δθt
δvt=δvc'-δθt×vc'
式中,φt表示姿态误差角φ在横地理坐标系t中的投影;δvt为速度误差δv在横地理坐标系t中的投影;表示计算横地理坐标系c'到横地理坐标系t的方向余弦矩阵;δθt表示位置误差角δθ在横地理坐标系t中的投影,具体表示为:
式中,分别为δrt的北向分量、东向分量;δrt为位置误差δr在横地理坐标系t中的投影,其中经纬度误差转换为位置误差δrt表示为:
δrt=[(RN+h)δλtcosLt (RM+h)δLt δh]T
位置误差δrt转换为经纬度误差表示为:
其中为位置误差δrt的天向分量。
(8)确定跨极区导航阻尼切换方案:
(8.1)进入极区导航坐标系切换至横地理坐标系时,确定阻尼系统姿态、速度的转换关系为:
式中,表示载体坐标系b到地理坐标系g的方向余弦矩阵;vg表示地理坐标系下的载体速度;
确定位置的转换关系为:
(8.2)驶出极区导航坐标系切换至地理坐标系时,确定阻尼系统姿态、速度的转换关系为:
式中,表示横地理坐标系t到地理坐标系g的方向余弦矩阵;
确定位置参数的转换关系:
(8.3)确定卡尔曼最优估计系统误差状态的转换关系,步骤如下:
(8.3.1)确定计算横地理坐标系下漂移误差角ψc'和计算地理坐标系下漂移误差角ψc的转换关系:
式中表示计算地理坐标系c到计算横地理坐标系c'的方向余弦矩阵,具体表示为:
其中为地球坐标系e到计算地理坐标系c的方向余弦矩阵,/>为横地球坐标系e′到计算横地理坐标系c'的方向余弦矩阵;
(8.3.2)确定计算横地理坐标系下的速度误差δvc'与计算地理坐标系下的速度误差δvc的转换关系:
(8.3.3)确定计算横地理坐标系下的位置误差δrc'与计算地理坐标系下的位置误差δrc的转换关系:
(8.4)确定卡尔曼最优估计系统协方差矩阵的转换关系,步骤如下:
根据步骤(8.3)所述,舰船进入极区并切换至横地理坐标系时,将误差状态的转换关系表示为:
xc'(t)=Φxc(t)
式中,xc(t)表示计算地理坐标系下的系统误差状态;Φ表示系统误差状态从计算地理坐标系c转换到计算横地理坐标系c'的转换矩阵,具体表达式为:
式中,diag{·}表示块对角矩阵;当舰船进入极区时并切换至横地理坐标系导航时,计算地理坐标系c下系统误差状态的协方差矩阵Pc(t)与计算横地理坐标系c'下系统误差状态的协方差矩阵Pc'(t)的转换关系表示为:
式中,表示计算横地理坐标系下的误差状态估计值,/>表示计算地理坐标系下的误差状态估计值;
当舰船离开极区并切换至地理坐标系导航时,误差状态和协方差矩阵转换关系表示为:
xc(t)=Φ-1xc'(t),Pc(t)=Φ-1Pc'(t)Φ-T。
进一步的,本发明的卡尔曼最优估计对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈,测速仪标度因数误差和安装误差采用开环反馈,且每次闭环反馈的系统误差状态校正后置0。
进一步的,若载体接收到其他传感器的位置信息,包括但不限于GNSS位置信息、重力匹配位置信息、地磁匹配位置信息,基于接收到的位置信息对转换关系或/>进行修正更新。
与现有技术相比,本发明具有以下优点:
本发明基于Psi角误差模型优化了复杂的椭球模型下横向导航方案,在保证长航时导航精度的同时降低了系统的计算负担;基于现代控制理论设计了变阻尼网络,实现了惯性导航系统的极点配置,进而抑制了纯惯性振荡误差;利用卡尔曼最优估计实现阻尼系数的时变估计,提高系统的稳定性;在面向跨极区长航时导航领域,设计了阻尼系统切换方案,使系统实现坐标系切换过程中的平稳过渡,避免了传统阻尼切换的超调误差。本发明使用的地球椭球模型更加精准,变阻尼网络设计和切换方案使舰船跨极区航行更加稳定性,满足系统长航时工作需要,有利于提高长航时导航精度。
附图说明
图1是本发明实施例提供的方法流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
如图1所示,一种优化的地球椭球模型下长航时跨极区阻尼切换方法,具体实施方式如下:
(1)定义横地球坐标系,定义横向极点,定义横向经度和横向纬度,确定横向位置表示方式:所述横地球坐标系e′的原点位于地心,X轴沿着地球自转轴指向北极,Y轴指向本初子午线与赤道的交点,Z轴穿过东经90°子午线与赤道的交点;定义(0°,90°E)为横向北极点、(0°,90°W)为横向南极点;定义0°经线和180°经线组成的大椭圆为横向赤道;定义90°E和90°W北半球部分组成的半个大椭圆为0°横经线,且横向本初子午线为地理经度90°E所在的子午圈的北半球部分,横向子午线为过横向极点的平面与地球表面相交的轮廓线;定义地球表面上一点的地理法线与横向赤道面交角为该点的横向纬度;定义该点所在的横向子午面与横向本初子午面的交角为横向经度;根据构建的横经纬网络,将舰船在横地球坐标系中位置表示为(Lt,λt,h),其中,Lt表示横纬度,λ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下的投影;
和/>具体表示为:
其中,和/>分别表示地理坐标系下载体的北向速度和东向速度;RM表示载体处的子午圈半径,RN表示载体处的卯酉圈半径,具体表示为:
其中,Re表示地球长半轴半径,ρ表示地球的偏心率;
确定
式中,和/>分别为横地理坐标系t下载体东向和北向的速度;/>表示横地理坐标系下载体处的扭曲率,/>和/>分别为横地理东向和横地理北向的曲率,具体表示为:
(4.2)确定横地理坐标系下的速度vt的更新方程:
式中,vt表示横地理坐标系t下的载体速度;fb表示载体坐标系b下表示的比力;gt表示横地理坐标系t下表示的重力矢量;
(4.3)确定横地理坐标系下的位置更新方程:
步骤(4.1)所述的参数其变化由横向经度、横向纬度的变化所引起,具体表示为:
对比步骤(4.1)所确定的参数确定横向经度、横向纬度微分方程:
高度变化由天向速度所引起,确定高度微分方程:
式中,表示横地理坐标系t下载体的天向速度;
(5)确定计算地理坐标系、平台坐标系与真实导航坐标系之间的转换关系:
确定计算地理坐标系c到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到计算地理坐标系c的方向余弦矩阵为:
/>
式中I3×3表示3×3的单位矩阵;ψ为漂移误差角,φ为姿态误差角,δθ为位置误差角;确定漂移误差角、姿态误差角、位置误差角之间的关系为:
φ=ψ+δθ
(6)确定计算横地理坐标系下阻尼系统的误差模型:
(6.1)确定计算横地理坐标系下阻尼系统的状态方程:
其中xc'为系统状态向量;F为状态转移矩阵;G为系统噪声分配矩阵;w为系统噪声向量;K为阻尼系数矩阵;u为阻尼系统反馈矩阵;
将系统状态向量xc'表示为:
其中,表示三维漂移误差角矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角;表示三维速度误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差;/>表示三维位置误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差;/>表示陀螺的零偏矢量,各分量分别为X、Y、Z轴陀螺的零偏;/>表示加速度计的零偏矢量,各分量分别为X、Y、Z轴加速度计的零偏;δk表示测速仪标度因数误差;δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差;
(6.2)确定计算横地理坐标系下阻尼系统的误差方程:
(6.2.1)确定计算横坐标系下惯性导航系统的姿态、速度和位置误差方程:
式中,表示计算横地理坐标系c'下地球自转角速度,/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度,/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵,fc'表示计算横地理坐标系c'下表示的比力;
式中分别表示载体坐标系b下的陀螺误差和加速度计误差,表示为:
式中,和/>分别表示陀螺和加速度计的噪声;
其中,姿态和速度误差微分方程投影在计算横地理坐标系下,均为已知确定量,姿态误差微分方程中不存在与/>相关的扰动误差项,速度误差微分方程中不存在与/>相关的扰动误差项,即不存在与地球椭球模型下地理位置相关的误差项;
(6.2.2)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差方程:
式中,τε和分别表示陀螺和加速度计的一阶马尔可夫相关时间,wε和/>分别表示陀螺和加速度计的高斯白噪声;
将计算横地理坐标系下阻尼系统表示为:
其中,表示惯性导航系统的速度估计值与测速仪的速度输出值;03×3表示3×3的零矩阵;
(7)确定阻尼系统的阻尼系数矩阵K:
步骤(6)所述阻尼系数矩阵K与卡尔曼最优估计的增益系数一致,即建立卡尔曼最优估计的状态方程和观测方程即获得阻尼系数矩阵K;由于卡尔曼最优估计是依据惯性导航系统不同时刻不同状态的特征进行估计,因此其增益系数为时变的,进而实现系统的变阻尼网络;
(7.1)确定卡尔曼最优估计系统状态方程:
其中系统状态向量xc'、状态转移矩阵F、系统噪声分配矩阵G、系统噪声向量w与步骤(6)所述相同;
(7.2)确定卡尔曼最优估计系统观测方程:
/>
其中,为测速仪安装误差矩阵;vc'表示计算横地理坐标系c'下的速度矢量;υ为测量噪声向量;H为状态观测矩阵,将其表示为:
H=[-[vc'×] I3×3 03×3 03×3 03×3 -vc' H1 H2]
式中,H1和H2分别为矩阵的第一列和第三列向量;
(7.3)确定阻尼系数矩阵K:
根据卡尔曼最优估计方法,其中预测过程为:
更新过程为:
式中,为系统第n-1时刻的误差状态,/>为系统第n时刻预测的误差状态,/>为系统第n时刻的误差状态;Fn/n-1为从n-1时刻至n时刻的状态转移矩阵;/>为系统第n-1时刻误差状态的协方差矩阵,/>为系统第n时刻预测的误差状态的协方差矩阵,/>为系统第n时刻误差状态的协方差矩阵;Qn-1为系统第n-1时刻噪声向量的协方差矩阵;Bn-1为系统第n-1时刻系统噪声分配矩阵;Hn为系统第n时刻系统的观测矩阵;Rn为系统第n时刻测量噪声向量的协方差矩阵;zn为系统第n时刻系统测量误差向量;Kn为系统第n时刻的卡尔曼最优估计增益矩阵,即系统阻尼系数矩阵;I18×18表示18×18的单位矩阵;
(7.4)确定系统误差状态修正方式:卡尔曼最优估计的系统状态向量定义在计算横地理坐标系中,通过对系统状态进行修正使其定义在横地理坐标系t中:
φt=ψc'+δθt
δvt=δvc'-δθt×vc'
式中,φt表示姿态误差角φ在横地理坐标系t中的投影;δvt为速度误差δv在横地理坐标系t中的投影;表示计算横地理坐标系c'到横地理坐标系t的方向余弦矩阵;δθt表示位置误差角δθ在横地理坐标系t中的投影,具体表示为:
式中,分别为δrt的北向分量、东向分量;δrt为位置误差δr在横地理坐标系t中的投影,其中经纬度误差转换为位置误差δrt表示为:
δrt=[(RN+h)δλt cosL t(RM+h)δLt δh]T
位置误差δrt转换为经纬度误差表示为:
/>
其中为位置误差δrt的天向分量。
(8)确定跨极区导航阻尼切换方案:
(8.1)进入极区导航坐标系切换至横地理坐标系时,确定阻尼系统姿态、速度的转换关系为:
式中,表示载体坐标系b到地理坐标系g的方向余弦矩阵;vg表示地理坐标系下的载体速度;
确定位置的转换关系为:
(8.2)驶出极区导航坐标系切换至地理坐标系时,确定阻尼系统姿态、速度的转换关系为:
式中,表示横地理坐标系t到地理坐标系g的方向余弦矩阵;
确定位置参数的转换关系:
(8.3)确定卡尔曼最优估计系统误差状态的转换关系,步骤如下:
(8.3.1)确定计算横地理坐标系下漂移误差角ψc'和计算地理坐标系下漂移误差角ψc的转换关系:
式中表示计算地理坐标系c到计算横地理坐标系c'的方向余弦矩阵,具体表示为:
其中为地球坐标系e到计算地理坐标系c的方向余弦矩阵,/>为横地球坐标系e′到计算横地理坐标系c'的方向余弦矩阵;
(8.3.2)确定计算横地理坐标系下的速度误差δvc'与计算地理坐标系下的速度误差δvc的转换关系:
(8.3.3)确定计算横地理坐标系下的位置误差δrc'与计算地理坐标系下的位置误差δrc的转换关系:
/>
(8.4)确定卡尔曼最优估计系统协方差矩阵的转换关系,步骤如下:
根据步骤(8.3)所述,舰船进入极区并切换至横地理坐标系时,将误差状态的转换关系表示为:
xc'(t)=Φxc(t)
式中,xc'(t)表示计算横地理坐标系下的系统误差状态;xc表示计算地理坐标系下的系统误差状态;Φ表示系统误差状态从计算地理坐标系c转换到计算横地理坐标系c'的转换矩阵,具体表达式为:
式中,diag{·}表示块对角矩阵;当舰船进入极区时并切换至横地理坐标系导航时,计算地理坐标系c下系统误差状态的协方差矩阵Pc(t)与计算横地理坐标系c'下系统误差状态的协方差矩阵Pc'(t)的转换关系表示为:
式中,表示计算横地理坐标系下的误差状态估计值,/>表示计算地理坐标系下的误差状态估计值;
当舰船离开极区并切换至地理坐标系导航时,误差状态和协方差矩阵转换关系表示为:
xc(t)=Φ-1xc'(t),Pc(t)=Φ-1Pc'(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所在的子午圈的北半球部分,横向子午线为过横向极点的平面与地球表面相交的轮廓线;定义地球表面上一点的地理法线与横向赤道面交角为该点的横向纬度;定义该点所在的横向子午面与横向本初子午面的交角为横向经度;根据构建的横经纬网络,将舰船在横地球坐标系中位置表示为(Lt,λt,h),其中,Lt表示横纬度,λ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下的投影;
和/>具体表示为:
其中,和/>分别表示地理坐标系下载体的北向速度和东向速度;RM表示载体处的子午圈半径,RN表示载体处的卯酉圈半径,具体表示为:
其中,Re表示地球长半轴半径,ρ表示地球的偏心率;
确定
式中,和/>分别为横地理坐标系t下载体东向和北向的速度;/>表示横地理坐标系下载体处的扭曲率,/>和/>分别为横地理东向和横地理北向的曲率,具体表示为:
(4.2)确定横地理坐标系下的速度vt的更新方程:
式中,vt表示横地理坐标系t下的载体速度;fb表示载体坐标系b下表示的比力;gt表示横地理坐标系t下表示的重力矢量;
(4.3)确定横地理坐标系下的位置更新方程:
步骤(4.1)所述的参数其变化由横向经度、横向纬度的变化所引起,具体表示为:
对比步骤(4.1)所确定的参数确定横向经度、横向纬度微分方程:
高度变化由天向速度所引起,确定高度微分方程:
式中,表示横地理坐标系t下载体的天向速度;
(5)确定计算地理坐标系、平台坐标系与真实导航坐标系之间的转换关系:
确定计算地理坐标系c到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到计算地理坐标系c的方向余弦矩阵为:
式中I3×3表示3×3的单位矩阵;ψ为漂移误差角,φ为姿态误差角,δθ为位置误差角;
确定漂移误差角、姿态误差角、位置误差角之间的关系为:
φ=ψ+δθ
(6)确定计算横地理坐标系下阻尼系统的误差模型:
(6.1)确定计算横地理坐标系下阻尼系统的状态方程:
其中xc'为系统状态向量;F为状态转移矩阵;G为系统噪声分配矩阵;w为系统噪声向量;K为阻尼系数矩阵;u为阻尼系统反馈矩阵;
将系统状态向量xc'表示为:
其中,表示三维漂移误差角矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角;表示三维速度误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差;/>表示三维位置误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差;/>表示陀螺的零偏矢量,各分量分别为X、Y、Z轴陀螺的零偏;/>表示加速度计的零偏矢量,各分量分别为X、Y、Z轴加速度计的零偏;δk表示测速仪标度因数误差;δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差;
(6.2)确定计算横地理坐标系下阻尼系统的误差方程:
(6.2.1)确定计算横坐标系下惯性导航系统的姿态、速度和位置误差方程:
式中,表示计算横地理坐标系c'下地球自转角速度,/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度,/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵,fc'表示计算横地理坐标系c'下表示的比力;
式中分别表示载体坐标系b下的陀螺误差和加速度计误差,表示为:
式中,和/>分别表示陀螺和加速度计的噪声;
(6.2.2)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差方程:
式中,τε和分别表示陀螺和加速度计的一阶马尔可夫相关时间,wε和/>分别表示陀螺和加速度计的高斯白噪声;
将计算横地理坐标系下阻尼系统表示为:
其中,表示惯性导航系统的速度估计值与测速仪的速度输出值;03×3表示3×3的零矩阵;
(7)确定阻尼系统的阻尼系数矩阵K:
步骤(6)所述阻尼系数矩阵K与卡尔曼最优估计的增益系数一致,即建立卡尔曼最优估计的状态方程和观测方程即获得阻尼系数矩阵K;由于卡尔曼最优估计是依据惯性导航系统不同时刻不同状态的特征进行估计,因此其增益系数为时变的,进而实现系统的变阻尼网络;
(7.1)确定卡尔曼最优估计系统状态方程:
其中系统状态向量xc'、状态转移矩阵F、系统噪声分配矩阵G、系统噪声向量w与步骤(6)所述相同;
(7.2)确定卡尔曼最优估计系统观测方程:
其中,为测速仪安装误差矩阵;vc'表示计算横地理坐标系c'下的速度矢量;υ为测量噪声向量;H为状态观测矩阵,将其表示为:
H=[-[vc'×] I3×3 03×3 03×3 03×3 -vc' H1 H2]
式中,H1和H2分别为矩阵的第一列和第三列向量;
(7.3)确定阻尼系数矩阵K:
根据卡尔曼最优估计方法,其中预测过程为:
更新过程为:
式中,为系统第n-1时刻的误差状态,/>为系统第n时刻预测的误差状态,/>为系统第n时刻的误差状态;Fn/n-1为从n-1时刻至n时刻的状态转移矩阵;/>为系统第n-1时刻误差状态的协方差矩阵,/>为系统第n时刻预测的误差状态的协方差矩阵,/>为系统第n时刻误差状态的协方差矩阵;Qn-1为系统第n-1时刻噪声向量的协方差矩阵;Bn-1为系统第n-1时刻系统噪声分配矩阵;Hn为系统第n时刻系统的观测矩阵;Rn为系统第n时刻测量噪声向量的协方差矩阵;zn为系统第n时刻系统测量误差向量;Kn为系统第n时刻的卡尔曼最优估计增益矩阵,即系统阻尼系数矩阵;I18×18表示18×18的单位矩阵;
(7.4)确定系统误差状态修正方式:卡尔曼最优估计的系统状态向量定义在计算横地理坐标系中,通过对系统状态进行修正使其定义在横地理坐标系t中:
φt=ψc'+δθt
δvt=δvc'-δθt×vc'
式中,φt表示姿态误差角φ在横地理坐标系t中的投影;δvt为速度误差δv在横地理坐标系t中的投影;表示计算横地理坐标系c'到横地理坐标系t的方向余弦矩阵;δθt表示位置误差角δθ在横地理坐标系t中的投影,具体表示为:
式中,分别为δrt的北向分量、东向分量;δrt为位置误差δr在横地理坐标系t中的投影,其中经纬度误差转换为位置误差δrt表示为:
δrt=[(RN+h)δλtcosLt (RM+h)δLt δh]T
位置误差δrt转换为经纬度误差表示为:
其中为位置误差δrt的天向分量;
(8)确定跨极区导航阻尼切换方案:
(8.1)进入极区导航坐标系切换至横地理坐标系时,确定阻尼系统姿态、速度的转换关系为:
式中,表示载体坐标系b到地理坐标系g的方向余弦矩阵;vg表示地理坐标系下的载体速度;
确定位置的转换关系为:
(8.2)驶出极区导航坐标系切换至地理坐标系时,确定阻尼系统姿态、速度的转换关系为:
式中,表示横地理坐标系t到地理坐标系g的方向余弦矩阵;
确定位置参数的转换关系:
(8.3)确定卡尔曼最优估计系统误差状态的转换关系,步骤如下:
(8.3.1)确定计算横地理坐标系下漂移误差角ψc'和计算地理坐标系下漂移误差角ψc的转换关系:
式中表示计算地理坐标系c到计算横地理坐标系c'的方向余弦矩阵,具体表示为:
其中为地球坐标系e到计算地理坐标系c的方向余弦矩阵,/>为横地球坐标系e′到计算横地理坐标系c'的方向余弦矩阵;
(8.3.2)确定计算横地理坐标系下的速度误差δvc'与计算地理坐标系下的速度误差δvc的转换关系:
(8.3.3)确定计算横地理坐标系下的位置误差δrc'与计算地理坐标系下的位置误差δrc的转换关系:
(8.4)确定卡尔曼最优估计系统协方差矩阵的转换关系,步骤如下:
根据步骤(8.3)所述,舰船进入极区并切换至横地理坐标系时,将误差状态的转换关系表示为:
xc'(t)=Φxc(t)
式中,xc(t)表示计算地理坐标系下的系统误差状态;Φ表示系统误差状态从计算地理坐标系c转换到计算横地理坐标系c'的转换矩阵,具体表达式为:
式中,diag{·}表示块对角矩阵;当舰船进入极区时并切换至横地理坐标系导航时,计算地理坐标系c下系统误差状态的协方差矩阵Pc(t)与计算横地理坐标系c'下系统误差状态的协方差矩阵Pc'(t)的转换关系表示为:
式中,表示计算横地理坐标系下的误差状态估计值,/>表示计算地理坐标系下的误差状态估计值;
当舰船离开极区并切换至地理坐标系导航时,误差状态和协方差矩阵转换关系表示为:
xc(t)=Φ-1xc'(t),Pc(t)=Φ-1Pc'(t)Φ-T。
2.如权利要求1所述的一种优化的地球椭球模型下长航时跨极区阻尼切换方法,其特征在于,卡尔曼最优估计对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈,测速仪标度因数误差和安装误差采用开环反馈,且每次闭环反馈的系统误差状态校正后置0。
3.如权利要求1所述的一种优化的地球椭球模型下长航时跨极区阻尼切换方法,其特征在于,若载体接收到其他传感器的位置信息,基于接收到的位置信息对转换关系或/>进行修正更新。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311492703.6A CN117470235B (zh) | 2023-11-10 | 2023-11-10 | 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311492703.6A CN117470235B (zh) | 2023-11-10 | 2023-11-10 | 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117470235A CN117470235A (zh) | 2024-01-30 |
CN117470235B true CN117470235B (zh) | 2024-04-26 |
Family
ID=89634490
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311492703.6A Active CN117470235B (zh) | 2023-11-10 | 2023-11-10 | 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117470235B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6459990B1 (en) * | 1999-09-23 | 2002-10-01 | American Gnc Corporation | Self-contained positioning method and system thereof for water and land vehicles |
CN108225325A (zh) * | 2017-12-29 | 2018-06-29 | 中国人民解放军海军工程大学 | 基于虚拟球模型的极地横向导航方法 |
CN108426575A (zh) * | 2018-02-12 | 2018-08-21 | 中国人民解放军海军工程大学 | 用地球椭球模型改进的捷联惯导极地横向导航方法 |
CN109029454A (zh) * | 2018-07-13 | 2018-12-18 | 哈尔滨工程大学 | 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法 |
CN114001731A (zh) * | 2021-10-12 | 2022-02-01 | 苏州大学 | 虚拟圆球模型下极区惯性导航相位调制阻尼方法及系统 |
CN115200574A (zh) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | 一种地球椭球模型下的极区横向组合导航方法 |
CN116481564A (zh) * | 2023-03-11 | 2023-07-25 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的极地双惯导协同标定方法 |
-
2023
- 2023-11-10 CN CN202311492703.6A patent/CN117470235B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6459990B1 (en) * | 1999-09-23 | 2002-10-01 | American Gnc Corporation | Self-contained positioning method and system thereof for water and land vehicles |
CN108225325A (zh) * | 2017-12-29 | 2018-06-29 | 中国人民解放军海军工程大学 | 基于虚拟球模型的极地横向导航方法 |
CN108426575A (zh) * | 2018-02-12 | 2018-08-21 | 中国人民解放军海军工程大学 | 用地球椭球模型改进的捷联惯导极地横向导航方法 |
CN109029454A (zh) * | 2018-07-13 | 2018-12-18 | 哈尔滨工程大学 | 一种基于卡尔曼滤波的横坐标系捷联惯导系统阻尼算法 |
CN114001731A (zh) * | 2021-10-12 | 2022-02-01 | 苏州大学 | 虚拟圆球模型下极区惯性导航相位调制阻尼方法及系统 |
CN115200574A (zh) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | 一种地球椭球模型下的极区横向组合导航方法 |
CN116481564A (zh) * | 2023-03-11 | 2023-07-25 | 中国人民解放军国防科技大学 | 基于Psi角误差修正模型的极地双惯导协同标定方法 |
Non-Patent Citations (4)
Title |
---|
Hybrid Transverse Polar Navigation for High-Precision and Long-Term INSs;Wu Ruonan et al.;《SENSORS》;20180820;第18卷(第5期);1-18 * |
基于协方差变换的 INS/DVL 全球组合导航算法;梁钟泓等;《中国惯性技术学报》;20220430;第30卷(第2期);159-167 * |
基于横坐标系的捷联惯性导航系统/多普勒速度仪极区组合导航算法;张福斌等;《兵工学报》;20160731;第37卷(第7期);1229-1235 * |
横坐标系捷联惯导系统极区导航及阻尼设计;李倩;孙枫;奔粤阳;于飞;;系统工程与电子技术;20141231(第12期);162-169 * |
Also Published As
Publication number | Publication date |
---|---|
CN117470235A (zh) | 2024-01-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106767900B (zh) | 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法 | |
CN100541135C (zh) | 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法 | |
CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN115200574B (zh) | 一种地球椭球模型下的极区横向组合导航方法 | |
CN111024064A (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN104697520B (zh) | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 | |
CN106895853B (zh) | 一种电磁计程仪辅助船用陀螺罗经行进间对准方法 | |
Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
CN116734887B (zh) | 基于速度误差修正模型的极地双惯导协同标定方法 | |
CN117537811B (zh) | 一种优化的地球椭球模型下跨极区导航切换方法 | |
CN116481564B (zh) | 基于Psi角误差修正模型的极地双惯导协同标定方法 | |
CN117470235B (zh) | 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 | |
CN110319833A (zh) | 一种无误差的光纤陀螺捷联惯导系统速度更新方法 | |
CN114111771A (zh) | 一种双轴稳定平台的动态姿态测量方法 | |
CN117516518B (zh) | 一种地球椭球模型下的跨极区阻尼切换方法 | |
CN111189446A (zh) | 一种基于无线电的组合导航方法 | |
CN117516520B (zh) | 一种优化的地球椭球模型下极区最优阻尼方法 | |
CN117516519B (zh) | 一种跨极区最优阻尼切换方法 | |
CN117705096A (zh) | 一种长航时跨极区阻尼切换方法 | |
CN117470233B (zh) | 一种优化的地球椭球模型下横坐标系组合导航方法 | |
CN117470234B (zh) | 基于Psi角误差模型的舰船跨极区滤波切换方法 | |
CN111380520B (zh) | 一种引入径向速度的sins/usbl松组合导航定位方法 | |
CN110057383B (zh) | 一种auv推位导航系统杆臂误差标校方法 | |
CN111473786A (zh) | 一种基于局部反馈的两层分布式多传感器组合导航滤波方法 | |
Ben et al. | DVL aided fine alignment for marine SINS |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |