CN117470233B - 一种优化的地球椭球模型下横坐标系组合导航方法 - Google Patents
一种优化的地球椭球模型下横坐标系组合导航方法 Download PDFInfo
- Publication number
- CN117470233B CN117470233B CN202311492697.4A CN202311492697A CN117470233B CN 117470233 B CN117470233 B CN 117470233B CN 202311492697 A CN202311492697 A CN 202311492697A CN 117470233 B CN117470233 B CN 117470233B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- abscissa
- error
- transverse
- under
- 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 87
- 238000004364 calculation method Methods 0.000 claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 39
- 230000008569 process Effects 0.000 claims description 24
- 239000013598 vector Substances 0.000 claims description 24
- 238000009434 installation Methods 0.000 claims description 15
- 238000006243 chemical reaction Methods 0.000 claims description 14
- 230000008859 change Effects 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims 1
- 238000005259 measurement Methods 0.000 abstract 1
- 238000004088 simulation Methods 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 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
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Data Mining & Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Computing Systems (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Probability & Statistics with Applications (AREA)
- Navigation (AREA)
Abstract
本发明属于惯性导航技术领域,公开了一种优化的地球椭球模型下横坐标系组合导航方法,适用于舰船极区航行导航。本发明基于Psi角误差模型,建立了椭球模型下横坐标系的导航编排方案,包括横向速度、位置、姿态解算方案,同时构建了椭球模型下横坐标系惯性/测速仪组合导航系统,包括组合导航系统方程、量测方程。本发明采用Psi角误差模型对椭球模型下横坐标系组合导航方案法进行优化,构建了更为简洁的组合导航系统,避免了Phi角误差模型下复杂的参数计算,极大地减轻了系统的计算负担,不存在简化误差,提高长航时导航精度,适用于长航时导航领域。
Description
技术领域
本发明属于惯性导航技术领域,涉及惯性/测速仪组合导航方法,特别涉及一种优化的地球椭球模型下横坐标系组合导航方法,适用于舰船的极区导航航行。
背景技术
极地地区尤其是北极地区在资源、科研、航道等方面具有重要战略价值。由于惯性导航拥有极好的自主性,并且不受极地地区恶劣环境的影响,因此惯性导航已经成为极区重要的导航手段。
传统的惯性导航编排方案常用于中低纬度地区,然而随着纬度增加,该方案出现计算溢出,同时经线收敛速度增加,失去航向参考,因此传统惯性导航编排方案难以适用于极区,无法提供准确的位置和方位。针对这一问题,对比文件(CharlesBroxmeyer.Inertial Navigation Systems.New York:McGraw-Hill Book Company,1964.)提出了横坐标系统及其相应的导航方法,该方法通过重新定义新的坐标系统避免传统惯性导航编排方案在极区存在的问题。
目前基于Phi角误差模型的横坐标系极区导航方法所涉及的参数耦合了许多变量,由于Phi角误差模型定义在真实坐标系下,在真实坐标系下各类导航参数相对于系统是未知的,因此当涉及到微分求解时,各类未知量相互耦合形成极其复杂的微分方程,特别在极地组合导航应用中所涉及的系统参数微扰极其复杂不利于系统误差分析,增大了导航计算量。为了保持系统简洁,多采用地球圆球模型构建简洁的惯性导航编排方案。然而地球是一个椭球体,相应的简化方案必然给惯性导航系统带来近似误差,尤其是系统长时间工作在极区时,这种误差更加不能忽略,将会显著降低导航精度。
本发明针对目前存在的问题,面向长航时极区航海应用,提出一种优化的地球椭球模型下横坐标系组合导航方法,以Psi角误差模型为基础,建立了椭球模型下横坐标系解算方案;针对组合导航系统,构建了更为简洁惯性/测速仪组合导航模型。本发明采用Psi角误差模型对椭球模型下横坐标系解算方法进行优化,避免系统各类参数之间的耦合,极大的减轻系统运算量,减少简化导致的近似误差,提高长航时导航精度,对组合导航系统误差分析及滤波器构建具有十分重要的意义。
发明内容
现有基于Phi角误差模型的极区导航方案为了保持惯性导航编排方案的简洁性,多采用了地球圆球模型,因此牺牲了导航精度,存在近似误差,无法满足长航时航海导航系统对精度的要求。如果基于地球椭球模型编排方案,并且采用横向坐标系导航在极区工作,极区惯性导航编排方案冗长且复杂,特别是在极区组合导航方法中涉及到导航误差状态方程时,各种参数相互耦合导致极其复杂、繁琐的参数计算,给系统误差分析带来极大困难。本发明要解决的技术问题就在于:针对现有技术的不足,本发明提出一种优化的地球椭球模型下横坐标系组合导航方法,采用Psi角误差模型解决极区组合导航过程中系统模型精度与简洁性之间的冲突,在采用椭球模型提高长航时精度的同时可以极大地优化系统模型,使系统复杂度和运算负担降低,方便系统误差分析。
为解决上述技术问题,本发明提出的解决方案为:
一种优化的地球椭球模型下横坐标系组合导航方法,所述方法包括以下步骤:
(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)确定横向经纬度与地理经纬度的转换关系:
由地理经纬度转换至横向经纬度的关系为:
由横向经纬度转换至地理经纬度的关系为:
(5)利用惯性导航获得载体姿态、速度、位置相关信息,确定横地理坐标系下的姿态微分方程、速度微分方程、位置微分方程,具体步骤如下:
(5.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下载体东向和北向的速度;/>表示横地理坐标系下载体处的扭曲率,/>和/>分别为横地理东向和横地理北向的曲率,具体表示为:
(5.2)确定横地理坐标系下的速度vt的微分方程:
式中,vt表示横地理坐标系t下的载体速度;fb表示载体坐标系b下表示的比力;gt表示横地理坐标系t下表示的重力矢量;
(5.3)确定横地理坐标系下的位置微分方程:
步骤(5.1)所述的参数其变化由横向经度、横向纬度的变化所引起,具体表示为:
对比步骤(5.1)所确定的参数确定横向经度、横向纬度微分方程:
高度变化由天向速度所引起,确定高度微分方程:
式中,表示横地理坐标系t下载体的天向速度;
(6)确定计算坐标系、平台坐标系与真实导航坐标系之间的转换关系:
确定计算坐标系c到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到计算地理坐标系c的方向余弦矩阵为:
式中I3×3表示3×3的单位矩阵;ψ为漂移误差角,φ为姿态误差角,δθ为位置误差角;确定漂移误差角、姿态误差角、位置误差角之间的关系为:
φ=ψ+δθ
(7)确定舰船在计算横地理坐标系下的卡尔曼滤波模型,包括以下步骤:
(7.1)确定计算横地理坐标系下的系统状态方程:
(7.1.1)确定计算横地理坐标系下的系统误差状态:
xc'(t)=[ψc' δvc' δrc' εb ▽b δk δη δγ]T
其中,表示三维漂移误差角矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角;表示三维速度误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差;/>表示三维位置误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差;/>表示陀螺的零偏矢量,各分量分别为X、Y、Z轴陀螺的零偏;/>表示加速度计的零偏矢量,各分量分别为X、Y、Z轴加速度计的零偏;δk表示测速仪标度因数误差;δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差;
(7.1.2)确定计算横地理坐标系下惯性导航系统的姿态、速度和位置误差微分方程:
式中,表示计算横地理坐标系c'下地球自转角速度,/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度,/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵,fc'表示计算横地理坐标系c'下表示的比力;
式中分别表示载体坐标系b下的陀螺误差和加速度计误差,表示为:
式中,和/>分别表示陀螺和加速度计的噪声;
其中,姿态和速度误差微分方程投影在计算横地理坐标系下,均为已知确定量,姿态误差微分方程中/>的扰动误差项/>为0,速度误差微分方程中/>和的扰动误差项/>和/>均为0,即不存在与地球椭球模型下地理位置相关的误差项;
(7.1.3)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差微分方程:
式中,τε和分别表示陀螺和加速度计的一阶马尔可夫相关时间,wε和/>分别表示陀螺和加速度计的高斯白噪声;
(7.2)确定测速仪的速度观测方程:
其中,
式中,表示惯性导航系统的速度估计值与测速仪的速度输出值;/>表示计算横地理坐标系c'下的速度矢量;
(7.3)确定系统误差状态修正方式:经滤波后的系统状态向量定义在计算横地理坐标系中,通过对系统状态进行修正使其定义在横地理坐标系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的天向分量。
进一步的,本发明所采用的组合导航滤波器对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈,测速仪标度因数误差和安装误差采用开环反馈,且每次闭环反馈的系统误差状态校正后置0。
进一步的,若载体接收到其他传感器的位置信息,包括但不限于GNSS位置信息、重力匹配位置信息、地磁匹配位置信息,基于接收到的位置信息对转换关系或/>进行修正更新。
与现有技术相比,本发明具有以下优点:
本发明采用Psi角误差模型,更加贴合航海导航实际情况,基于该模型优化了椭球模型下横坐标系惯性导航编排方案,构建了更为简洁的惯性/测速仪组合导航系统。由于Psi角误差模型定义在计算坐标系中,各类变量在计算坐标系中均为已知量,因此相关变量微分为0,避免了Phi角误差模型中误差状态所涉及的复杂变量的计算,该误差量与地球椭球模型相关,展开后涉及的误差项极其复杂,通常的简化方法将带来原理误差,将降低长航时导航的精度。本发明在提高系统精度的同时极大地简化了系统模型,减少系统运算负担。
附图说明
图1是本发明实施例提供的方法流程图;
图2是本发明实施例提供的低纬度下不同误差模型的实测定位误差对比图;
图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)确定横向经纬度与地理经纬度的转换关系:
由地理经纬度转换至横向经纬度的关系为:
由横向经纬度转换至地理经纬度的关系为:
(5)利用惯性导航获得载体姿态、速度、位置相关信息,确定横地理坐标系下的姿态微分方程、速度微分方程、位置微分方程,具体步骤如下:
(5.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下载体东向和北向的速度;/>表示横地理坐标系下载体处的扭曲率,/>和/>分别为横地理东向和横地理北向的曲率,具体表示为:
(5.2)确定横地理坐标系下的速度vt的微分方程:
式中,vt表示横地理坐标系t下的载体速度;fb表示载体坐标系b下表示的比力;gt表示横地理坐标系t下表示的重力矢量;
(5.3)确定横地理坐标系下的位置微分方程:
步骤(5.1)所述的参数其变化由横向经度、横向纬度的变化所引起,具体表示为:
对比步骤(5.1)所确定的参数确定横向经度、横向纬度微分方程:
高度变化由天向速度所引起,确定高度微分方程:
式中,表示横地理坐标系t下载体的天向速度;
(6)确定计算坐标系、平台坐标系与真实导航坐标系之间的转换关系:
确定计算坐标系c到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到计算地理坐标系c的方向余弦矩阵为:
式中I3×3表示3×3的单位矩阵;ψ为漂移误差角,φ为姿态误差角,δθ为位置误差角;确定漂移误差角、姿态误差角、位置误差角之间的关系为:
φ=ψ+δθ
(7)确定舰船在计算横地理坐标系下的卡尔曼滤波模型,包括以下步骤:
(7.1)确定计算横地理坐标系下的系统状态方程:
(7.1.1)确定计算横地理坐标系下的系统误差状态:
其中,表示三维漂移误差角矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角;表示三维速度误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差;/>表示三维位置误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差;/>表示陀螺的零偏矢量,各分量分别为X、Y、Z轴陀螺的零偏;/>表示加速度计的零偏矢量,各分量分别为X、Y、Z轴加速度计的零偏;δk表示测速仪标度因数误差;δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差;
(7.1.2)确定计算横地理坐标系下惯性导航系统的姿态、速度和位置误差微分方程:
式中,表示计算横地理坐标系c'下地球自转角速度,/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度,/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵,fc'表示计算横地理坐标系c'下表示的比力;
式中分别表示载体坐标系b下的陀螺误差和加速度计误差,表示为:
/>
式中,和/>分别表示陀螺和加速度计的噪声;
其中,姿态和速度误差微分方程投影在计算横地理坐标系下,均为已知确定量,姿态误差微分方程中不存在与/>相关的扰动误差项,速度误差微分方程中不存在与/>相关的扰动误差项,即不存在与地球椭球模型下地理位置相关的误差项;
(7.1.3)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差微分方程:
式中,τε和τ▽分别表示陀螺和加速度计的一阶马尔可夫相关时间,wε和w▽分别表示陀螺和加速度计的高斯白噪声;
(7.2)确定测速仪的速度观测方程:
其中,
式中,表示惯性导航系统的速度估计值与测速仪的速度输出值;vc'表示计算横地理坐标系c'下的速度矢量;
(7.3)确定系统误差状态修正方式:经滤波后的系统状态向量定义在计算横地理坐标系中,通过对系统状态进行修正使其定义在横地理坐标系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的天向分量。
进一步的,本发明所采用的组合导航滤波器对系统的姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈,测速仪标度因数误差和安装误差采用开环反馈,且每次闭环反馈的系统误差状态校正后置0。
进一步的,若载体接收到其他传感器的位置信息,包括但不限于GNSS位置信息、重力匹配位置信息、地磁匹配位置信息,基于接收到的位置信息对转换关系或/>进行修正更新。
按照上述技术方案搭建一种优化的地球椭球模型下横坐标系组合导航方法,如图1所示为本方法的流程图。为验证本发明所提方法的有效性,以某惯导系统的低纬度船载实验和高纬度半实物仿真实验为例进行说明:
低纬度船载实验总时长为2.2小时,实验在横地理坐标系下采用椭球模型按照不同误差模型分别构建组合导航系统并进行数据处理。此外按照对比文件(TransverseNavigation under the Ellipsoidal Earth Model and its Performance in bothPolar and Non-polar areas)中所涉及方案进行半实物仿真实验,在高纬度下基于横向坐标按照不同误差模型构建组合导航系统并进行数据处理,导航过程中GNSS记录的位置信息为基准进行评估。
图2是低纬度下不同误差模型的实测定位误差对比图,结果表明不同误差模型下,低纬度所处理的定位误差曲线高度重合,证实误差模型之间的等价性。随着时间的增加,Psi角误差模型会显现出优势。
图3是高纬度下不同误差模型的半实物仿真定位误差对比图,结果表明在高纬度地区以Psi角误差模型构建的组合导航系统定位误差更小,显现出更为明显优势。从定位精度上看,以Psi角误差模型建立极区组合导航系统可以与目前常用的Phi误差模型构建的组合导航系统保持相同精度,甚至略优于Phi误差模型。从系统模型上看以Psi角误差模型构建极区组合导航系统过程中更加简洁、方便,可大幅度降低系统计算负担。
以上所述仅是本发明的优选实施方式,并不用以限制本发明,凡属于本发明思路下的技术方案均属于本发明的保护范围。在不脱离本发明原理前提下的若干改进和润饰等,这些改进和润饰也应视为本发明的保护范围。
Claims (3)
1.一种优化的地球椭球模型下横坐标系组合导航方法,其特征在于,包括以下步骤:
(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)确定横向经纬度与地理经纬度的转换关系:
由地理经纬度转换至横向经纬度的关系为:
由横向经纬度转换至地理经纬度的关系为:
(5)利用惯性导航获得载体姿态、速度、位置相关信息,确定横地理坐标系下的姿态微分方程、速度微分方程、位置微分方程,具体步骤如下:
(5.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下载体东向和北向的速度;/>表示横地理坐标系下载体处的扭曲率,/>和/>分别为横地理东向和横地理北向的曲率,具体表示为:
(5.2)确定横地理坐标系下的速度vt的微分方程:
式中,vt表示横地理坐标系t下的载体速度;fb表示载体坐标系b下表示的比力;gt表示横地理坐标系t下表示的重力矢量;
(5.3)确定横地理坐标系下的位置微分方程:
步骤(5.1)所述的参数其变化由横向经度、横向纬度的变化所引起,具体表示为:
对比步骤(5.1)所确定的参数确定横向经度、横向纬度微分方程:
高度变化由天向速度所引起,确定高度微分方程:
式中,表示横地理坐标系t下载体的天向速度;
(6)确定计算坐标系、平台坐标系与真实导航坐标系之间的转换关系:
确定计算坐标系c到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到平台坐标系p的方向余弦矩阵为:
确定真实导航坐标系tg到计算地理坐标系c的方向余弦矩阵为:
式中I3×3表示3×3的单位矩阵;ψ为漂移误差角,φ为姿态误差角,δθ为位置误差角;
确定漂移误差角、姿态误差角、位置误差角之间的关系为:
φ=ψ+δθ
(7)确定舰船在计算横地理坐标系下的卡尔曼滤波模型,包括以下步骤:
(7.1)确定计算横地理坐标系下的系统状态方程:
(7.1.1)确定计算横地理坐标系下的系统误差状态:
其中,表示三维漂移误差角矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的漂移误差角;表示三维速度误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的速度误差;/>表示三维位置误差矢量在计算横地理坐标系c'下的投影,各分量分别为计算横地理坐标系c'下东向、北向、天向的位置误差;/>表示陀螺的零偏矢量,各分量分别为X、Y、Z轴陀螺的零偏;/>表示加速度计的零偏矢量,各分量分别为X、Y、Z轴加速度计的零偏;δk表示测速仪标度因数误差;δη、δγ表示测速仪的俯仰角安装误差、方位角安装误差;
(7.1.2)确定计算横地理坐标系下惯性导航系统的姿态、速度和位置误差微分方程:
式中,表示计算横地理坐标系c'下地球自转角速度,/>表示计算横地理坐标系c'下计算横地理坐标系c'相对地球坐标系e的角速度,/>表示从载体坐标系b到计算横地理坐标系c'的方向余弦矩阵,fc'表示计算横地理坐标系c'下表示的比力;
式中分别表示载体坐标系b下的陀螺误差和加速度计误差,表示为:
式中,和/>分别表示陀螺和加速度计的噪声;
(7.1.3)确定陀螺零偏、加速度计零偏、测速仪标度因数误差、测速仪俯仰角安装误差、测速仪方位角安装误差的误差微分方程:
式中,τε和分别表示陀螺和加速度计的一阶马尔可夫相关时间,wε和/>分别表示陀螺和加速度计的高斯白噪声;
(7.2)确定测速仪的速度观测方程:
其中,
式中,表示惯性导航系统的速度估计值与测速仪的速度输出值;vc'表示计算横地理坐标系c'下的速度矢量;
(7.3)确定系统误差状态修正方式:经滤波后的系统状态向量定义在计算横地理坐标系中,通过对系统状态进行修正使其定义在横地理坐标系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 cosLt(RM+h)δLtδh]T
位置误差δrt转换为经纬度误差表示为:
其中为位置误差δrt的天向分量。
2.如权利要求1所述的一种优化的地球椭球模型下横坐标系组合导航方法,其特征在于,组合导航滤波器对系统姿态误差、速度误差、位置误差、陀螺和加速度计零偏采用闭环反馈,测速仪标度因数误差和安装误差采用开环反馈,且每次闭环反馈的系统误差状态校正后置0。
3.如权利要求1所述的一种优化的地球椭球模型下横坐标系组合导航方法,其特征在于,若载体接收到其他传感器的位置信息,基于接收到的位置信息对转换关系或/>进行修正更新。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311492697.4A CN117470233B (zh) | 2023-11-10 | 2023-11-10 | 一种优化的地球椭球模型下横坐标系组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311492697.4A CN117470233B (zh) | 2023-11-10 | 2023-11-10 | 一种优化的地球椭球模型下横坐标系组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117470233A CN117470233A (zh) | 2024-01-30 |
CN117470233B true CN117470233B (zh) | 2024-06-21 |
Family
ID=89637680
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311492697.4A Active CN117470233B (zh) | 2023-11-10 | 2023-11-10 | 一种优化的地球椭球模型下横坐标系组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117470233B (zh) |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103389090A (zh) * | 2013-07-29 | 2013-11-13 | 哈尔滨工程大学 | 一种惯性导航系统极区导航模式初始速度的测量方法 |
CN115200574A (zh) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | 一种地球椭球模型下的极区横向组合导航方法 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103528584B (zh) * | 2013-11-04 | 2015-10-28 | 东南大学 | 基于横向地理坐标系的极区惯性导航方法 |
CN104215242B (zh) * | 2014-09-30 | 2017-03-15 | 东南大学 | 一种基于横向游移坐标系的极区惯性导航方法 |
CN108225325B (zh) * | 2017-12-29 | 2020-04-17 | 中国人民解放军海军工程大学 | 基于虚拟球模型的极地横向导航方法 |
CN108426575B (zh) * | 2018-02-12 | 2020-06-26 | 中国人民解放军海军工程大学 | 用地球椭球模型改进的捷联惯导极地横向导航方法 |
CN114923483A (zh) * | 2022-05-31 | 2022-08-19 | 中国人民解放军战略支援部队信息工程大学 | 一种基于状态变换卡尔曼滤波的dvl/sins组合导航方法 |
CN116734887B (zh) * | 2023-03-11 | 2024-01-30 | 中国人民解放军国防科技大学 | 基于速度误差修正模型的极地双惯导协同标定方法 |
-
2023
- 2023-11-10 CN CN202311492697.4A patent/CN117470233B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103389090A (zh) * | 2013-07-29 | 2013-11-13 | 哈尔滨工程大学 | 一种惯性导航系统极区导航模式初始速度的测量方法 |
CN115200574A (zh) * | 2022-07-03 | 2022-10-18 | 中国人民解放军国防科技大学 | 一种地球椭球模型下的极区横向组合导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN117470233A (zh) | 2024-01-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112097763B (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
CN115200574B (zh) | 一种地球椭球模型下的极区横向组合导航方法 | |
CN105043415B (zh) | 基于四元数模型的惯性系自对准方法 | |
CN100516775C (zh) | 一种捷联惯性导航系统初始姿态确定方法 | |
CN111380518B (zh) | 一种引入径向速度的sins/usbl紧组合导航定位方法 | |
CN111982106A (zh) | 导航方法、装置、存储介质及电子装置 | |
CN116734887B (zh) | 基于速度误差修正模型的极地双惯导协同标定方法 | |
Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
CN116481564B (zh) | 基于Psi角误差修正模型的极地双惯导协同标定方法 | |
CN104697520A (zh) | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 | |
CN117537811B (zh) | 一种优化的地球椭球模型下跨极区导航切换方法 | |
CN112747770B (zh) | 一种基于速度量测的载体机动中初始对准方法 | |
CN117470233B (zh) | 一种优化的地球椭球模型下横坐标系组合导航方法 | |
CN112798014A (zh) | 一种基于重力场球谐模型补偿垂线偏差的惯导自对准方法 | |
CN112083425A (zh) | 一种引入径向速度的sins/lbl紧组合导航方法 | |
CN111380520B (zh) | 一种引入径向速度的sins/usbl松组合导航定位方法 | |
CN110873577A (zh) | 一种水下快速动基座对准方法及装置 | |
CN111060140A (zh) | 一种地球椭球模型下的极区惯性导航误差获得方法 | |
CN117516520B (zh) | 一种优化的地球椭球模型下极区最优阻尼方法 | |
CN117470234B (zh) | 基于Psi角误差模型的舰船跨极区滤波切换方法 | |
CN117470235B (zh) | 一种优化的地球椭球模型下长航时跨极区阻尼切换方法 | |
CN117516518B (zh) | 一种地球椭球模型下的跨极区阻尼切换方法 | |
CN111473786A (zh) | 一种基于局部反馈的两层分布式多传感器组合导航滤波方法 | |
CN117516519B (zh) | 一种跨极区最优阻尼切换方法 | |
CN117705096A (zh) | 一种长航时跨极区阻尼切换方法 |
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 |