CN113124879A - 虚拟圆球模型下sins/dvl极区组合导航方法及系统 - Google Patents

虚拟圆球模型下sins/dvl极区组合导航方法及系统 Download PDF

Info

Publication number
CN113124879A
CN113124879A CN202110440859.4A CN202110440859A CN113124879A CN 113124879 A CN113124879 A CN 113124879A CN 202110440859 A CN202110440859 A CN 202110440859A CN 113124879 A CN113124879 A CN 113124879A
Authority
CN
China
Prior art keywords
representing
navigation
under
virtual sphere
error
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.)
Pending
Application number
CN202110440859.4A
Other languages
English (en)
Inventor
徐祥
李凤
陈洋豪
孙逸帆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou University
Original Assignee
Suzhou University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Suzhou University filed Critical Suzhou University
Priority to CN202110440859.4A priority Critical patent/CN113124879A/zh
Publication of CN113124879A publication Critical patent/CN113124879A/zh
Pending legal-status Critical Current

Links

Images

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/20—Instruments for performing navigational calculations
    • 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
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种虚拟圆球模型下SINS/DVL极区组合导航方法及系统，包括以下步骤：获取惯性传感器和声学多普勒测速仪的采集的实时数据；构建虚拟圆球模型下惯性导航力学编排；基于虚拟圆球模型下惯性导航力学编排，构造虚拟圆球模型下SINS/DVL组合定位系统误差模型；将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中，利用迭代优化算法实现的误差参数估计；定义极区长航时惯性导航定位时长为M，若导航时间k＜M，则标定过程未完成，重复上述步骤，直至导航定位结束。其采用虚拟圆球模型进行极区惯性导航更新，减小了算法复杂度，提高了惯导定位精度。

Description

虚拟圆球模型下SINS/DVL极区组合导航方法及系统
技术领域
本发明涉及导航技术领域，尤其是指一种虚拟圆球模型下SINS/DVL极区组合导航方法及系统。
背景技术
当前极区组合导航算法多采用横向坐标系或格网坐标系进行构建，而在地球模型上通常有圆球模型和椭球模型两种。其中圆球模型具有计算简单的优点，但是由于地球并非严格意义上的圆球，因此采用圆球模型进行组合导航存在原理性误差。椭球模型虽然可以消除原理性误差，但是却存在计算复杂度高的缺陷，难以在很多场合进行应用。
发明内容
为此，本发明所要解决的技术问题在于克服现有技术中圆球模型具有原理性误差，椭球模型计算复杂度高的技术缺陷。
为解决上述技术问题，本发明提供了一种虚拟圆球模型下SINS/DVL极区组合导航方法，包括以下步骤：
S1、获取惯性传感器和声学多普勒测速仪的采集的实时数据；
S2、构建虚拟圆球模型下惯性导航力学编排；
S3、基于虚拟圆球模型下惯性导航力学编排，构造虚拟圆球模型下SINS/DVL组合定位系统误差模型；
S4、将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中，利用迭代优化算法实现的误差参数估计；
S5、定义极区长航时惯性导航定位时长为M，若导航时间k＜M，则标定过程未完成，重复S1-S4，直至导航定位结束。
作为优选的，所述S1具体包括：
由惯性传感器测量模型可知：
Figure BDA0003035004340000021
其中，
Figure BDA0003035004340000022
表示加速度计测量值；fb表示真实比力；
Figure BDA0003035004340000023
表示加速度计零偏；wa表示加速度计随机噪声；
Figure BDA0003035004340000024
表示陀螺仪测量值；εb表示陀螺仪零偏；wg表示陀螺仪测量噪声；
DVL测量模型为：
Figure BDA0003035004340000025
其中，
Figure BDA0003035004340000026
表示DVL测量速度；
Figure BDA0003035004340000027
表示载体运动真实速度；
Figure BDA0003035004340000028
表示DVL测速误差。
作为优选的，所述S2具体包括：
由横向导航定位方法可知：
Figure BDA0003035004340000029
其中，
Figure BDA00030350043400000210
表示导航坐标系与横向导航坐标系之间的方向余弦矩阵；p表示导航系与横向导航系之间的夹角，采用下式计算：
Figure BDA0003035004340000031
其中，p表示导航系与横向导航系之间的夹角；Lt表示横向纬度；λt表示横向经度；所以虚拟圆球模型下速度可以计算为：
Figure BDA0003035004340000032
其中，
Figure BDA00030350043400000315
表示圆球模型下横向导航系速度；
Figure BDA0003035004340000033
表示导航坐标系与横向导航坐标系之间的方向余弦矩阵；
Figure BDA0003035004340000034
表示横向导航坐标系与导航坐标系之间的方向余弦矩阵；
Figure BDA0003035004340000035
表示虚拟圆球模型下横向导航系速度；
虚拟圆球模型下惯性导航力学编排为：
Figure BDA0003035004340000036
其中，
Figure BDA0003035004340000037
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure BDA0003035004340000038
表示载体姿态角速度；
Figure BDA0003035004340000039
表示圆球模型下横向导航系速度；
Figure BDA00030350043400000310
表示横向导航系相对于横向地球系的角速度；
Figure BDA00030350043400000311
表示地球系相对于惯性系角速度；
Figure BDA00030350043400000312
表示重力加速度；
Figure BDA00030350043400000313
表示虚拟圆球北向速度；
Figure BDA00030350043400000314
表示虚拟圆球东向速度；Rt表示圆球半径；h表示载体高度；Lt表示横向纬度；λt表示横向经度。
作为优选的，所述S3中，虚拟圆球模型下SINS/DVL组合定位系统误差模型，包括虚拟圆球模型下姿态误差方程、虚拟圆球模型下速度误差方程和虚拟圆球模型下位置误差方程。
作为优选的，所述虚拟圆球模型下姿态误差方程，具体包括：
由姿态误差可知：
Figure BDA0003035004340000041
其中，
Figure BDA0003035004340000042
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure BDA0003035004340000043
表示虚拟圆球模型下计算横向导航系与横向导航系之间的方向余弦矩阵；
Figure BDA0003035004340000044
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure BDA0003035004340000045
表示姿态误差；
对上式两边进行求导并整理可得虚拟圆球模型下姿态误差方程：
Figure BDA0003035004340000046
其中，
Figure BDA0003035004340000047
表示姿态误差微分；
Figure BDA0003035004340000048
表示姿态误差；
Figure BDA0003035004340000049
表示横向导航系相对于惯性系的角速度；
Figure BDA00030350043400000410
表示速度误差；
Figure BDA00030350043400000411
表示位置误差；
Figure BDA00030350043400000412
表示虚拟圆球模型下姿态方向余弦矩阵；εb表示陀螺零偏；Mav、M1和M2表示中间变量矩阵，采用下式计算：
Figure BDA00030350043400000413
Figure BDA0003035004340000051
Figure BDA0003035004340000052
其中，Rt表示地球圆球半径；h表示高度；ωie表示地球自转角速度；Lt表示横向纬度；λt表示横向经度；
Figure BDA0003035004340000053
表示虚拟圆球北向速度；
Figure BDA0003035004340000054
表示虚拟圆球东向速度。
作为优选的，所述虚拟圆球模型下速度误差方程，具体包括：
由比力方程可知虚拟圆球模型下比力方程为：
Figure BDA0003035004340000055
其中，
Figure BDA0003035004340000056
表示计算的横向导航系下的速度；
Figure BDA0003035004340000057
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure BDA0003035004340000058
表示加速度计测量值；
Figure BDA0003035004340000059
表示计算的横向导航系相对于横向地球系的角速度；
Figure BDA00030350043400000510
表示计算的地球系相对于惯性系角速度；
Figure BDA00030350043400000511
表示计算的重力加速度；
由虚拟圆球模型下的比力方程可以计算速度误差为：
Figure BDA00030350043400000512
其中，
Figure BDA00030350043400000513
表示横向坐标系下的比力；
Figure BDA00030350043400000514
表示姿态误差；
Figure BDA00030350043400000515
表示圆球模型下横向导航系速度；
Figure BDA0003035004340000061
表示横向导航系相对于横向地球系的角速度；
Figure BDA0003035004340000062
表示地球系相对于惯性系角速度；
Figure BDA0003035004340000063
表示位置误差；
Figure BDA0003035004340000064
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure BDA0003035004340000065
表示陀螺零偏；Mav、M1和M2表示中间变量矩阵。
作为优选的，所述虚拟圆球模型下位置误差方程，具体包括：
基于速度误差，位置误差为：
Figure BDA0003035004340000066
其中，
Figure BDA0003035004340000067
表示位置误差；
Figure BDA0003035004340000068
表示速度误差；Mpv和Mpp表示中间变量矩阵，可以计算为：
Figure BDA0003035004340000069
Figure BDA00030350043400000610
其中，Lt表示横向纬度；λt表示横向经度；
Figure BDA00030350043400000611
表示虚拟圆球北向速度；
Figure BDA00030350043400000612
表示虚拟圆球东向速度；Rt表示地球圆球半径；h表示高度。
作为优选的，所述S3中，构建虚拟圆球模型下SINS/DVL组合定位系统误差模型，具体包括：
构造组合导航系统方程：
Figure BDA0003035004340000071
其中，
Figure BDA0003035004340000072
表示系统；F表示系统矩阵；w表示系统噪声；
由DVL测速方程可以构造系统量测方程：
Figure BDA0003035004340000073
其中，y表示量测量；
Figure BDA0003035004340000074
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure BDA0003035004340000075
表示虚拟圆球模型下横向导航系速度；
Figure BDA0003035004340000076
表示DVL测量速度；
Figure BDA0003035004340000077
表示DVL测速误差；
Figure BDA0003035004340000078
表示虚拟圆球模型下姿态方向余弦矩阵的转置矩阵。
作为优选的，所述S4具体包括：
Figure BDA0003035004340000079
Figure BDA00030350043400000710
Figure BDA00030350043400000711
Figure BDA00030350043400000712
Pk＝(I-KkHk)Pk/k-1，
其中，
Figure BDA00030350043400000713
表示k-1时刻的状态估计；Fk/k-1表示k-1时刻到k时刻的状态转移矩阵；
Figure BDA00030350043400000714
表示状态一步预测；Pk-1表示k-1时刻状态估计的均方误差矩阵；Pk/k-1表示状态一步预测的均方误差矩阵；Qk-1表示系统噪声的协方差矩阵；Rk表示量测噪声的协方差矩阵；Hk表示状态和量测之间的测量矩阵；Kk表示滤波增益；zk表示k时刻的量测值；I表示单位矩阵；Pk表示k时刻状态估计的均方误差矩阵。
本发明还公开了一种虚拟圆球模型下SINS/DVL极区组合导航系统，包括：
数据采集模块，所述数据采集模块用于获取惯性传感器和声学多普勒测速仪的采集的实时数据；
惯性导航力学编排模块，所述惯性导航力学编排模块用于构建虚拟圆球模型下惯性导航力学编排；
定位系统误差模型构建模块，所述定位系统误差模型构建模块基于虚拟圆球模型下惯性导航力学编排，构造虚拟圆球模型下SINS/DVL组合定位系统误差模型；
迭代优化模块，所述导航定位模块将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中，利用迭代优化算法实现的误差参数估计；
导航定位模块，所述导航定位模块用于定义极区长航时惯性导航定位时长为M，若导航时间k＜M，则标定过程未完成，重复上述操作，直至导航定位结束。
本发明的上述技术方案相比现有技术具有以下优点：
1、本发明采用虚拟圆球模型进行极区惯性导航更新，减小了算法复杂度。
2、本发明通过虚拟圆球模型构造极区组合导航算法误差方程，减小了原理性误差，提高了惯导定位精度。
附图说明
图1是虚拟圆球模型下SINS/DVL极区组合导航算法流程图；
图2是组合导航算法姿态误差图，其中，图2(a)为纵摇误差，图2(b)为横摇误差，图2(c)为航向误差；
图3是组合导航算法速度误差图，其中，图3(a)为东向速度误差，图3(b)为北向速度误差；
图4是组合导航算法位置误差图，其中，图4(a)为维度误差，图4(b)为经度误差；
图5是三种算法计算时长统计图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
参照图1所示，本发明的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，包括以下步骤：
步骤一、获取惯性传感器和声学多普勒测速仪的采集的实时数据，具体包括：
由惯性传感器测量模型可知：
Figure BDA0003035004340000091
其中，
Figure BDA0003035004340000092
表示加速度计测量值；fb表示真实比力；
Figure BDA0003035004340000093
表示加速度计零偏；wa表示加速度计随机噪声；
Figure BDA0003035004340000094
表示陀螺仪测量值；εb表示陀螺仪零偏；wg表示陀螺仪测量噪声；
DVL测量模型为：
Figure BDA00030350043400001012
其中，
Figure BDA0003035004340000101
表示DVL测量速度；
Figure BDA0003035004340000102
表示载体运动真实速度；
Figure BDA0003035004340000103
表示DVL测速误差。
步骤二、构建虚拟圆球模型下惯性导航力学编排，具体包括：
由横向导航定位方法可知：
Figure BDA0003035004340000104
其中，
Figure BDA0003035004340000105
表示导航坐标系与横向导航坐标系之间的方向余弦矩阵；p表示导航系与横向导航系之间的夹角，采用下式计算：
Figure BDA0003035004340000106
其中，p表示导航系与横向导航系之间的夹角；Lt表示横向纬度；λt表示横向经度；所以虚拟圆球模型下速度可以计算为：
Figure BDA0003035004340000107
其中，
Figure BDA0003035004340000108
表示圆球模型下横向导航系速度；
Figure BDA0003035004340000109
表示导航坐标系与横向导航坐标系之间的方向余弦矩阵；
Figure BDA00030350043400001010
表示横向导航坐标系与导航坐标系之间的方向余弦矩阵；
Figure BDA00030350043400001011
表示虚拟圆球模型下横向导航系速度；
虚拟圆球模型下惯性导航力学编排为：
Figure BDA0003035004340000111
其中，
Figure BDA0003035004340000112
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure BDA0003035004340000113
表示载体姿态角速度；
Figure BDA0003035004340000114
表示圆球模型下横向导航系速度；
Figure BDA0003035004340000115
表示横向导航系相对于横向地球系的角速度；
Figure BDA0003035004340000116
表示地球系相对于惯性系角速度；
Figure BDA0003035004340000117
表示重力加速度；
Figure BDA0003035004340000118
表示虚拟圆球北向速度；
Figure BDA0003035004340000119
表示虚拟圆球东向速度；Rt表示圆球半径；h表示载体高度；Lt表示横向纬度；λt表示横向经度。
步骤三、基于虚拟圆球模型下惯性导航力学编排，构造虚拟圆球模型下SINS/DVL组合定位系统误差模型。
其中，虚拟圆球模型下SINS/DVL组合定位系统误差模型，包括虚拟圆球模型下姿态误差方程、虚拟圆球模型下速度误差方程和虚拟圆球模型下位置误差方程。
由姿态误差可知：
Figure BDA00030350043400001110
其中，
Figure BDA00030350043400001111
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure BDA00030350043400001112
表示虚拟圆球模型下计算横向导航系与横向导航系之间的方向余弦矩阵；
Figure BDA00030350043400001113
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure BDA00030350043400001114
表示姿态误差；
对上式两边进行求导并整理可得虚拟圆球模型下姿态误差方程：
Figure BDA0003035004340000121
其中，
Figure BDA0003035004340000122
表示姿态误差微分；
Figure BDA0003035004340000123
表示姿态误差；
Figure BDA0003035004340000124
表示横向导航系相对于惯性系的角速度；
Figure BDA0003035004340000125
表示速度误差；
Figure BDA0003035004340000126
表示位置误差；
Figure BDA0003035004340000127
表示虚拟圆球模型下姿态方向余弦矩阵；εb表示陀螺零偏；Mav、M1和M2表示中间变量矩阵，采用下式计算：
Figure BDA0003035004340000128
Figure BDA0003035004340000129
Figure BDA00030350043400001210
其中，Rt表示地球圆球半径；h表示高度；ωie表示地球自转角速度；Lt表示横向纬度；λt表示横向经度；
Figure BDA00030350043400001211
表示虚拟圆球北向速度；
Figure BDA00030350043400001212
表示虚拟圆球东向速度。
由比力方程可知虚拟圆球模型下比力方程为：
Figure BDA00030350043400001213
其中，
Figure BDA0003035004340000131
表示计算的横向导航系下的速度；
Figure BDA0003035004340000132
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure BDA0003035004340000133
表示加速度计测量值；
Figure BDA0003035004340000134
表示计算的横向导航系相对于横向地球系的角速度；
Figure BDA0003035004340000135
表示计算的地球系相对于惯性系角速度；
Figure BDA0003035004340000136
表示计算的重力加速度；
由虚拟圆球模型下的比力方程可以计算速度误差为：
Figure BDA0003035004340000137
其中，
Figure BDA0003035004340000138
表示横向坐标系下的比力；
Figure BDA0003035004340000139
表示姿态误差；
Figure BDA00030350043400001310
表示圆球模型下横向导航系速度；
Figure BDA00030350043400001311
表示横向导航系相对于横向地球系的角速度；
Figure BDA00030350043400001312
表示地球系相对于惯性系角速度；
Figure BDA00030350043400001313
表示位置误差；
Figure BDA00030350043400001314
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure BDA00030350043400001315
表示陀螺零偏；Mav、M1和M2表示中间变量矩阵。
基于速度误差，位置误差为：
Figure BDA00030350043400001316
其中，
Figure BDA00030350043400001317
表示位置误差；
Figure BDA00030350043400001318
表示速度误差；Mpv和Mpp表示中间变量矩阵，可以计算为：
Figure BDA00030350043400001319
Figure BDA0003035004340000141
其中，Lt表示横向纬度；λt表示横向经度；
Figure BDA0003035004340000142
表示虚拟圆球北向速度；
Figure BDA0003035004340000143
表示虚拟圆球东向速度；Rt表示地球圆球半径；h表示高度。
构造组合导航系统方程：
Figure BDA0003035004340000144
其中，
Figure BDA0003035004340000145
表示系统；F表示系统矩阵；w表示系统噪声；
由DVL测速方程可以构造系统量测方程：
Figure BDA0003035004340000146
其中，y表示量测量；
Figure BDA0003035004340000147
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure BDA0003035004340000148
表示虚拟圆球模型下横向导航系速度；
Figure BDA0003035004340000149
表示DVL测量速度；
Figure BDA00030350043400001410
表示DVL测速误差；
Figure BDA00030350043400001411
表示虚拟圆球模型下姿态方向余弦矩阵的转置矩阵。
步骤四、将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中，利用迭代优化算法实现的误差参数估计，具体包括：
Figure BDA00030350043400001412
Figure BDA00030350043400001413
Figure BDA0003035004340000151
Figure BDA0003035004340000152
Pk＝(I-KkHk)Pk/k-1，
其中，
Figure BDA0003035004340000153
表示k-1时刻的状态估计；Fk/k-1表示k-1时刻到k时刻的状态转移矩阵；
Figure BDA0003035004340000154
表示状态一步预测；Pk-1表示k-1时刻状态估计的均方误差矩阵；Pk/k-1表示状态一步预测的均方误差矩阵；Qk-1表示系统噪声的协方差矩阵；Rk表示量测噪声的协方差矩阵；Hk表示状态和量测之间的测量矩阵；Kk表示滤波增益；zk表示k时刻的量测值；I表示单位矩阵；Pk表示k时刻状态估计的均方误差矩阵。
步骤五、定义极区长航时惯性导航定位时长为M，若导航时间k＜M，则标定过程未完成，重复步骤一至步骤四，直至导航定位结束。
MATLAB仿真实验，在以下的仿真条件下，对该方法进行仿真实验：陀螺仪量测常值漂移误差为εb＝[0.02 0.02 0.02]T°/h，陀螺仪量测随机游走误差为
Figure BDA0003035004340000155
输出频率为200Hz；加速度计量测常值漂移误差为
Figure BDA0003035004340000156
陀螺仪量测随机游走误差为
Figure BDA0003035004340000157
输出频率为200Hz。DVL采样周期为1s，测速误差为0.1m/s。步骤4中，M＝72小时。
本实施例将本发明提出的一种虚拟圆球模型下SINS/DVL极区组合导航方法通过Matlab仿真软件进行仿真验证。仿真硬件环境均为Intel(R)Core(TM)T9600 CPU 2.80GHz，4G RAM，Windows 7操作系统。如图2所示，为组合导航算法姿态误差图，图3为组合导航算法速度误差图，图4为组合导航算法位置误差图，图5是三种算法计算时长统计图。从图中可以看出，采用虚拟圆球模型进行组合导航算法，可以达到与椭球模型相同的定位精度，而计算量却小于椭球模型。
本发明公开了一种虚拟圆球模型下SINS/DVL极区组合导航系统，包括数据采集模块、惯性导航力学编排模块、定位系统误差模型构建模块、迭代优化模块和导航定位模块。
所述数据采集模块用于获取惯性传感器和声学多普勒测速仪的采集的实时数据。
所述惯性导航力学编排模块用于构建虚拟圆球模型下惯性导航力学编排。
所述定位系统误差模型构建模块基于虚拟圆球模型下惯性导航力学编排，构造虚拟圆球模型下SINS/DVL组合定位系统误差模型。
所述导航定位模块将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中，利用迭代优化算法实现的误差参数估计。
所述导航定位模块用于定义极区长航时惯性导航定位时长为M，若导航时间k＜M，则标定过程未完成，重复上述操作，直至导航定位结束。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (10)

1.一种虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，包括以下步骤：
S1、获取惯性传感器和声学多普勒测速仪的采集的实时数据；
S2、构建虚拟圆球模型下惯性导航力学编排；
S3、基于虚拟圆球模型下惯性导航力学编排，构造虚拟圆球模型下SINS/DVL组合定位系统误差模型；
S4、将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中，利用迭代优化算法实现的误差参数估计；
S5、定义极区长航时惯性导航定位时长为M，若导航时间k＜M，则标定过程未完成，重复S1-S4，直至导航定位结束。
2.根据权利要求1所述的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，所述S1具体包括：
由惯性传感器测量模型可知：
Figure FDA0003035004330000011
其中，
Figure FDA0003035004330000012
表示加速度计测量值；fb表示真实比力；
Figure FDA0003035004330000013
表示加速度计零偏；wa表示加速度计随机噪声；
Figure FDA0003035004330000014
表示陀螺仪测量值；εb表示陀螺仪零偏；wg表示陀螺仪测量噪声；
DVL测量模型为：
Figure FDA0003035004330000021
其中，
Figure FDA0003035004330000022
表示DVL测量速度；
Figure FDA0003035004330000023
表示载体运动真实速度；
Figure FDA0003035004330000024
表示DVL测速误差。
3.根据权利要求1所述的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，所述S2具体包括：
由横向导航定位方法可知：
Figure FDA0003035004330000025
其中，
Figure FDA0003035004330000026
表示导航坐标系与横向导航坐标系之间的方向余弦矩阵；p表示导航系与横向导航系之间的夹角，采用下式计算：
Figure FDA0003035004330000027
其中，p表示导航系与横向导航系之间的夹角；Lt表示横向纬度；λt表示横向经度；所以虚拟圆球模型下速度可以计算为：
Figure FDA0003035004330000028
其中，
Figure FDA0003035004330000029
表示圆球模型下横向导航系速度；
Figure FDA00030350043300000210
表示导航坐标系与横向导航坐标系之间的方向余弦矩阵；
Figure FDA00030350043300000211
表示横向导航坐标系与导航坐标系之间的方向余弦矩阵；
Figure FDA00030350043300000212
表示虚拟圆球模型下横向导航系速度；
虚拟圆球模型下惯性导航力学编排为：
Figure FDA0003035004330000031
其中，
Figure FDA0003035004330000032
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure FDA0003035004330000033
表示载体姿态角速度；
Figure FDA0003035004330000034
表示圆球模型下横向导航系速度；
Figure FDA0003035004330000035
表示横向导航系相对于横向地球系的角速度；
Figure FDA0003035004330000036
表示地球系相对于惯性系角速度；
Figure FDA0003035004330000037
表示重力加速度；
Figure FDA0003035004330000038
表示虚拟圆球北向速度；
Figure FDA0003035004330000039
表示虚拟圆球东向速度；Rt表示圆球半径；h表示载体高度；Lt表示横向纬度；λt表示横向经度。
4.根据权利要求3所述的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，所述S3中，虚拟圆球模型下SINS/DVL组合定位系统误差模型，包括虚拟圆球模型下姿态误差方程、虚拟圆球模型下速度误差方程和虚拟圆球模型下位置误差方程。
5.根据权利要求4所述的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，所述虚拟圆球模型下姿态误差方程，具体包括：
由姿态误差可知：
Figure FDA00030350043300000310
其中，
Figure FDA00030350043300000311
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure FDA00030350043300000312
表示虚拟圆球模型下计算横向导航系与横向导航系之间的方向余弦矩阵；
Figure FDA00030350043300000313
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure FDA00030350043300000314
表示姿态误差；
对上式两边进行求导并整理可得虚拟圆球模型下姿态误差方程：
Figure FDA0003035004330000041
其中，
Figure FDA0003035004330000042
表示姿态误差微分；
Figure FDA0003035004330000043
表示姿态误差；
Figure FDA0003035004330000044
表示横向导航系相对于惯性系的角速度；
Figure FDA0003035004330000045
表示速度误差；
Figure FDA0003035004330000046
表示位置误差；
Figure FDA0003035004330000047
表示虚拟圆球模型下姿态方向余弦矩阵；εb表示陀螺零偏；Mav、M1和M2表示中间变量矩阵，采用下式计算：
Figure FDA0003035004330000048
Figure FDA0003035004330000049
Figure FDA00030350043300000410
其中，Rt表示地球圆球半径；h表示高度；ωie表示地球自转角速度；Lt表示横向纬度；λt表示横向经度；
Figure FDA00030350043300000411
表示虚拟圆球北向速度；
Figure FDA00030350043300000412
表示虚拟圆球东向速度。
6.根据权利要求5所述的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，所述虚拟圆球模型下速度误差方程，具体包括：
由比力方程可知虚拟圆球模型下比力方程为：
Figure FDA0003035004330000051
其中，
Figure FDA0003035004330000052
表示计算的横向导航系下的速度；
Figure FDA0003035004330000053
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure FDA0003035004330000054
表示加速度计测量值；
Figure FDA0003035004330000055
表示计算的横向导航系相对于横向地球系的角速度；
Figure FDA0003035004330000056
表示计算的地球系相对于惯性系角速度；
Figure FDA0003035004330000057
表示计算的重力加速度；
由虚拟圆球模型下的比力方程可以计算速度误差为：
Figure FDA0003035004330000058
其中，
Figure FDA0003035004330000059
表示横向坐标系下的比力；
Figure FDA00030350043300000510
表示姿态误差；
Figure FDA00030350043300000511
表示圆球模型下横向导航系速度；
Figure FDA00030350043300000512
表示横向导航系相对于横向地球系的角速度；
Figure FDA00030350043300000513
表示地球系相对于惯性系角速度；
Figure FDA00030350043300000514
表示位置误差；
Figure FDA00030350043300000515
表示虚拟圆球模型下姿态方向余弦矩阵；
Figure FDA00030350043300000516
表示陀螺零偏；Mav、M1和M2表示中间变量矩阵。
7.根据权利要求6所述的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，所述虚拟圆球模型下位置误差方程，具体包括：
基于速度误差，位置误差为：
Figure FDA00030350043300000517
其中，
Figure FDA00030350043300000518
表示位置误差；
Figure FDA00030350043300000519
表示速度误差；Mpv和Mpp表示中间变量矩阵，可以计算为：
Figure FDA0003035004330000061
Figure FDA0003035004330000062
其中，Lt表示横向纬度；λt表示横向经度；
Figure FDA0003035004330000063
表示虚拟圆球北向速度；
Figure FDA0003035004330000064
表示虚拟圆球东向速度；Rt表示地球圆球半径；h表示高度。
8.根据权利要求7所述的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，所述S3中，构建虚拟圆球模型下SINS/DVL组合定位系统误差模型，具体包括：
构造组合导航系统方程：
Figure FDA0003035004330000065
其中，
Figure FDA0003035004330000066
表示系统；F表示系统矩阵；w表示系统噪声；
由DVL测速方程可以构造系统量测方程：
Figure FDA0003035004330000067
其中，y表示量测量；
Figure FDA0003035004330000071
表示虚拟圆球模型下计算姿态方向余弦矩阵；
Figure FDA0003035004330000072
表示虚拟圆球模型下横向导航系速度；
Figure FDA0003035004330000073
表示DVL测量速度；
Figure FDA0003035004330000074
表示DVL测速误差；
Figure FDA0003035004330000075
表示虚拟圆球模型下姿态方向余弦矩阵的转置矩阵。
9.根据权利要求8所述的虚拟圆球模型下SINS/DVL极区组合导航方法，其特征在于，所述S4具体包括：
Figure FDA0003035004330000076
Figure FDA0003035004330000077
Figure FDA0003035004330000078
Figure FDA0003035004330000079
Pk＝(I-KkHk)Pk/k-1，
其中，
Figure FDA00030350043300000710
表示k-1时刻的状态估计；Fk/k-1表示k-1时刻到k时刻的状态转移矩阵；
Figure FDA00030350043300000711
表示状态一步预测；Pk-1表示k-1时刻状态估计的均方误差矩阵；Pk/k-1表示状态一步预测的均方误差矩阵；Qk-1表示系统噪声的协方差矩阵；Rk表示量测噪声的协方差矩阵；Hk表示状态和量测之间的测量矩阵；Kk表示滤波增益；zk表示k时刻的量测值；I表示单位矩阵；Pk表示k时刻状态估计的均方误差矩阵。
10.一种虚拟圆球模型下SINS/DVL极区组合导航系统，其特征在于，包括：
数据采集模块，所述数据采集模块用于获取惯性传感器和声学多普勒测速仪的采集的实时数据；
惯性导航力学编排模块，所述惯性导航力学编排模块用于构建虚拟圆球模型下惯性导航力学编排；
定位系统误差模型构建模块，所述定位系统误差模型构建模块基于虚拟圆球模型下惯性导航力学编排，构造虚拟圆球模型下SINS/DVL组合定位系统误差模型；
迭代优化模块，所述导航定位模块将实时数据代入至虚拟圆球模型下SINS/DVL组合定位系统误差模型中，利用迭代优化算法实现的误差参数估计；
导航定位模块，所述导航定位模块用于定义极区长航时惯性导航定位时长为M，若导航时间k＜M，则标定过程未完成，重复上述操作，直至导航定位结束。
CN202110440859.4A 2021-04-23 2021-04-23 虚拟圆球模型下sins/dvl极区组合导航方法及系统 Pending CN113124879A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110440859.4A CN113124879A (zh) 2021-04-23 2021-04-23 虚拟圆球模型下sins/dvl极区组合导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110440859.4A CN113124879A (zh) 2021-04-23 2021-04-23 虚拟圆球模型下sins/dvl极区组合导航方法及系统

Publications (1)

Publication Number Publication Date
CN113124879A true CN113124879A (zh) 2021-07-16

Family

ID=76779289

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110440859.4A Pending CN113124879A (zh) 2021-04-23 2021-04-23 虚拟圆球模型下sins/dvl极区组合导航方法及系统

Country Status (1)

Country Link
CN (1) CN113124879A (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20150080209A (ko) * 2013-12-31 2015-07-09 부경대학교 산학협력단 북극항로 자동 안내 시스템 및 이를 이용한 북극항로 자동 안내 방법
CN108225325A (zh) * 2017-12-29 2018-06-29 中国人民解放军海军工程大学 基于虚拟球模型的极地横向导航方法
CN110196066A (zh) * 2019-05-10 2019-09-03 西北工业大学 基于格网姿态速度信息不变的虚拟极区方法
CN110457813A (zh) * 2019-08-08 2019-11-15 西北工业大学 基于横向地理坐标系的虚拟极区方法
AU2020101525A4 (en) * 2020-07-28 2020-09-03 Xi`an University of Architecture and Technology Inertial basis combined navigation method for indirect grid frame in polar zone
CN111928848A (zh) * 2020-09-24 2020-11-13 中国人民解放军国防科技大学 一种基于虚拟圆球法向量模型的极区惯性导航方法
AU2020103939A4 (en) * 2020-12-08 2021-02-11 Xi'an University Of Architecture And Technology Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20150080209A (ko) * 2013-12-31 2015-07-09 부경대학교 산학협력단 북극항로 자동 안내 시스템 및 이를 이용한 북극항로 자동 안내 방법
CN108225325A (zh) * 2017-12-29 2018-06-29 中国人民解放军海军工程大学 基于虚拟球模型的极地横向导航方法
CN110196066A (zh) * 2019-05-10 2019-09-03 西北工业大学 基于格网姿态速度信息不变的虚拟极区方法
CN110457813A (zh) * 2019-08-08 2019-11-15 西北工业大学 基于横向地理坐标系的虚拟极区方法
AU2020101525A4 (en) * 2020-07-28 2020-09-03 Xi`an University of Architecture and Technology Inertial basis combined navigation method for indirect grid frame in polar zone
CN111928848A (zh) * 2020-09-24 2020-11-13 中国人民解放军国防科技大学 一种基于虚拟圆球法向量模型的极区惯性导航方法
AU2020103939A4 (en) * 2020-12-08 2021-02-11 Xi'an University Of Architecture And Technology Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LING HUANG ET AL.: "Transverse SINS/DVL Integrated Polar Navigation Algorithm Based on Virtual Sphere Model", 《MATHEMATICAL PROBLEMS IN ENGINEERING》 *
张福斌等: "基于横坐标系的捷联惯性导航系统/多普勒速度仪极区组合导航算法", 《兵工学报》 *
覃方君等: "基于虚拟圆球模型的横向极区导航方法（英文）", 《中国惯性技术学报》 *

Similar Documents

Publication Publication Date Title
CN105300379B (zh) 一种基于加速度的卡尔曼滤波姿态估计方法及系统
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN105180937B (zh) 一种mems‑imu初始对准方法
CN109141475B (zh) 一种dvl辅助sins鲁棒行进间初始对准方法
US9683865B2 (en) In-use automatic calibration methodology for sensors in mobile devices
US10267646B2 (en) Method and system for varying step length estimation using nonlinear system identification
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN108007477B (zh) 一种基于正反向滤波的惯性行人定位系统误差抑制方法
CN110715659A (zh) 零速检测方法、行人惯性导航方法、装置及存储介质
US10533874B2 (en) Inertial positioning and navigation device featuring a novel walk detection method
CN108132053B (zh) 一种行人轨迹构建方法、系统及惯性测量装置
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
CN103644910A (zh) 基于分段rts平滑算法的个人自主导航系统定位方法
CN104613965A (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
CN104613966B (zh) 一种地籍测量事后数据处理方法
Leppäkoski et al. Error analysis of step length estimation in pedestrian dead reckoning
WO2021218731A1 (zh) Imu与刚体的位姿融合方法、装置、设备及存储介质
Hasan et al. Smart phone based sensor fusion by using Madgwick filter for 3D indoor navigation
CN109931952A (zh) 未知纬度条件下捷联惯导直接解析式粗对准方法
CN113340298A (zh) 一种惯导和双天线gnss外参标定方法
CN111121820B (zh) 基于卡尔曼滤波的mems惯性传感器阵列融合方法
CN114001731B (zh) 虚拟圆球模型下极区惯性导航相位调制阻尼方法及系统
Llorach et al. Position estimation with a low-cost inertial measurement unit
CN109084765B (zh) 一种行人室内行走定位方法、装置及存储介质
CN113124879A (zh) 虚拟圆球模型下sins/dvl极区组合导航方法及系统

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210716