CN111323012B - 载体高动态环境下的ins辅助dvl测速误差补偿方法 - Google Patents

载体高动态环境下的ins辅助dvl测速误差补偿方法 Download PDF

Info

Publication number
CN111323012B
CN111323012B CN202010334588.XA CN202010334588A CN111323012B CN 111323012 B CN111323012 B CN 111323012B CN 202010334588 A CN202010334588 A CN 202010334588A CN 111323012 B CN111323012 B CN 111323012B
Authority
CN
China
Prior art keywords
dvl
ins
information
time
speed
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
Application number
CN202010334588.XA
Other languages
English (en)
Other versions
CN111323012A (zh
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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN202010334588.XA priority Critical patent/CN111323012B/zh
Publication of CN111323012A publication Critical patent/CN111323012A/zh
Application granted granted Critical
Publication of CN111323012B publication Critical patent/CN111323012B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种载体高动态情况下的INS辅助DVL测速误差补偿方法，包括：首先更新INS的姿态、速度和位置信息；其次在DVL发射时刻，记录INS姿态、速度信息；在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息；最后对校正后的DVL测速信息与INS输出信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束。本发明能有效地提高载体高动态情况下的DVL测速精度，从而提高INS/DVL组合导航精度。

Description

载体高动态环境下的INS辅助DVL测速误差补偿方法
技术领域
本发明涉及一种载体高动态环境下的INS辅助DVL测速误差补偿方法，属于DVL动态误差补偿技术，特别适用于INS/DVL水下组合导航领域。
背景技术
在水下组合导航系统中，INS(即：惯性导航系统)/DVL(即：多普勒测速仪)组合导航方式凭借其高自主性和隐蔽性逐渐成为主流。在INS/DVL组合导航系统中，DVL精度是决定整个组合导航系统精度的关键因素，而载体动态情况下DVL信号发射接收时刻速度不一致性会引起DVL测速误差。
现有技术中减少载体动态情况下的DVL误差方法主要有纯惯性法，自适应滤波法和DVL测速原理校正法。纯惯性法在载体高动态情况下，直接摒弃DVL量测信息，以减少DVL误差对系统影响，短时内有一定效果，但长时误差会随时间积累；自适应滤波法根据当前载体动态大小调整量测噪声协方差矩阵，仅能从一定程度缓解DVL动态测速误差的影响；DVL测速原理校正法通过建立载体动态下的测速误差模型对DVL量测误差进行校正补偿。然而现有DVL测速原理校正技术多基于运动学假设，在载体复杂高动态时与真实情况不符，导致DVL测速误差补偿方法不准确。针对以上情况亟需一种更有效的DVL测速误差补偿校正方法。
发明内容
发明目的：为了减少动态环境下的DVL量测误差，本发明提出了一种载体高动态环境下的INS辅助DVL测速误差补偿方法。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：
一种载体高动态环境下的INS辅助DVL测速误差补偿方法，该方法包括如下步骤：
S1：更新INS的姿态、速度和位置信息；
S2：在DVL发射时刻，记录INS姿态、速度信息；
S3：在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息；
S4：对步骤S3中校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束。
所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，所述步骤S1具体包括以下过程：
更新INS的姿态、速度和位置信息：
Figure BDA0002466129090000021
Figure BDA0002466129090000022
Figure BDA0002466129090000023
Figure BDA0002466129090000024
Figure BDA0002466129090000025
Figure BDA0002466129090000026
Figure BDA0002466129090000027
Figure BDA0002466129090000028
其中，
Figure BDA0002466129090000029
为b系到n系的转换矩阵导数，
Figure BDA00024661290900000210
为b系到n系的转换矩阵，
Figure BDA00024661290900000211
为b系下b系到n系的角速度，(×)为叉乘，
Figure BDA00024661290900000212
为n系速度导数，Vn为n系速度，fb为比力，
Figure BDA00024661290900000213
为n系下地球自转角速度，
Figure BDA00024661290900000214
为n系下n系到e系的角速度，Gn为重力矢量，
Figure BDA00024661290900000215
为纬度导数，L为纬度，VN为n系北向速度，RM为子午圈半径，
Figure BDA00024661290900000216
为经度导数，λ为经度，VE为n系东向速度，RN为卯酉圈半径，
Figure BDA00024661290900000217
为高度导数，h为高度，VU为n系天向速度，
Figure BDA00024661290900000218
为b系下b系到i系的角速度，
Figure BDA00024661290900000219
为n系到b系的转换矩阵，Ω为地球自转角速度。所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，所述步骤S2具体包括以下过程：
在DVLt1发射时刻，记录t1时刻INS输出b系到n系的转换矩阵
Figure BDA00024661290900000220
与t1时刻INS输出速度信息
Figure BDA00024661290900000221
所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，所述步骤S3具体包括以下过程：
在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息：
S3.1若DVL输出多普勒频移信息，则直接利用多普勒频移信息，若DVL输出测速信息，将测速信息转化为多普勒频移信息：
Figure BDA00024661290900000222
Figure BDA00024661290900000223
其中，[Δf1 Δf2 Δf3 Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，A为转换矩阵，
Figure BDA0002466129090000031
为DVL测速信息，C为超声波波速，α为波束倾角；
S3.2利用DVL发射时刻与DVL接收时刻INS分别输出的姿态速度信息补偿校正t1发射时刻DVL测速信息：
Figure BDA0002466129090000032
其中，[Δf1 Δf2 Δf3 Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，C为超声波波速，A为转换矩阵，I为单位矩阵，
Figure BDA0002466129090000033
为t1时刻INS输出n系到b系的转换矩阵，
Figure BDA0002466129090000034
为t2时刻INS输出b系到n系的转换矩阵，
Figure BDA0002466129090000035
为t2时刻INS输出n系到b系的转换矩阵，
Figure BDA0002466129090000036
为t2时刻INS输出速度信息，
Figure BDA0002466129090000037
为t1时刻INS输出速度信息，
Figure BDA0002466129090000038
为t1发射时刻DVL测速信息；
S3.3利用校正后的t1发射时刻DVL测速信息计算t2接收时刻DVL测速信息：
Figure BDA0002466129090000039
其中，
Figure BDA00024661290900000310
为t2接收时刻DVL测速信息，
Figure BDA00024661290900000311
为t2时刻INS输出n系到b系的转换矩阵，
Figure BDA00024661290900000312
为t2时刻INS输出速度信息，
Figure BDA00024661290900000313
为t1时刻INS输出n系到b系的转换矩阵，
Figure BDA00024661290900000314
为t1时刻INS输出速度信息，
Figure BDA00024661290900000315
为t1发射时刻DVL测速信息。
所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，所述步骤S4具体包括以下过程：
对步骤S3校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束：
S4.1根据INS误差模型与校正后的DVL测速信息建立状态方程与量测方程：
选定状态向量为：
Figure BDA00024661290900000316
其中，X为状态向量，δVE,N,U为n系东北天向速度误差，φx,y,z为三轴安装误差角，
Figure BDA00024661290900000317
为加速度计偏差，εx,y,z为陀螺偏差；
建立离散化状态方程与量测方程：
Figure BDA00024661290900000318
其中，Xk为k时刻状态向量，φk，k-1为状态转移矩阵，Xk-1为k-1时刻状态向量，Gk-1为系统噪声输入矩阵，Wk-1为系统噪声向量，Zk为量测向量，Hk为量测矩阵，Vk为量测噪声向量；
S4.2卡尔曼滤波：
Figure BDA0002466129090000041
Figure BDA0002466129090000042
Figure BDA0002466129090000043
Figure BDA0002466129090000044
Pk＝(I-KkHk)Pk,k-1
其中，
Figure BDA0002466129090000045
为状态一步预测向量，φk，k-1为状态转移矩阵，
Figure BDA0002466129090000046
为初始状态向量，Pk，k-1为状态一步预测误差方差阵，Pk-1为初始状态误差方差阵，Gk-1为系统噪声输入矩阵，Qk-1为系统噪声误差方差阵，Kk为状态增益矩阵，Hk为量测矩阵，Rk为量测噪声误差方差阵，
Figure BDA00024661290900000410
为状态估计向量，Zk为量测向量，Pk为估计误差方差阵；
S4.3输出组合导航信息，并对INS进行反馈校正，然后重复上述步骤，直到导航任务结束
有益效果：
本发明利用INS输出信息校正DVL量测信息，减少了动态环境下的DVL量测误差。
附图说明
图1为本发明的载体高动态环境下的INS辅助DVL测速误差补偿方法流程图。
具体实施方式
下面结合附图，对本发明的技术方案作进一步说明。应当了解，以下提供的实施例仅是为了详尽地且完全地公开本发明，并且向所属技术领域的技术人员充分传达本发明的技术构思，本发明还可以用许多不同的形式来实施，并且不局限于此处描述的实施例。对于表示在附图中的示例性实施方式中的术语并不是对本发明的限定。
本发明提供的一种载体高动态环境下的INS辅助DVL测速误差补偿方法，实现原理如图1所示。其流程主要包括以下步骤：
步骤S1，更新INS的姿态、速度和位置信息。具体包括以下过程：
更新INS的姿态、速度和位置信息：
Figure BDA0002466129090000047
Figure BDA0002466129090000048
Figure BDA0002466129090000049
Figure BDA0002466129090000051
Figure BDA0002466129090000052
Figure BDA0002466129090000053
Figure BDA0002466129090000054
Figure BDA0002466129090000055
其中，
Figure BDA0002466129090000056
为b系到n系的转换矩阵导数，
Figure BDA0002466129090000057
为b系到n系的转换矩阵，
Figure BDA0002466129090000058
为b系下b系到n系的角速度，(×)为叉乘，
Figure BDA0002466129090000059
为n系速度导数，Vn为n系速度，fb为比力，
Figure BDA00024661290900000510
为n系下地球自转角速度，
Figure BDA00024661290900000511
为n系下n系到e系的角速度，Gn为重力矢量，
Figure BDA00024661290900000512
为纬度导数，L为纬度，VN为n系北向速度，RM为子午圈半径，
Figure BDA00024661290900000521
为经度导数，λ为经度，VE为n系东向速度，RN为卯酉圈半径，
Figure BDA00024661290900000513
为高度导数，h为高度，VU为n系天向速度，
Figure BDA00024661290900000514
为b系下b系到i系的角速度，
Figure BDA00024661290900000515
为n系到b系的转换矩阵，Ω为地球自转角速度。
步骤S2，在DVL发射时刻，记录1NS姿态、速度信息。具体包括以下过程：
在DVLt1发射时刻，记录t1时刻1NS输出b系到n系的转换矩阵
Figure BDA00024661290900000516
与t1时刻INS输出速度信息
Figure BDA00024661290900000517
步骤S3，在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息。具体包括以下过程：
S3.1若DVL输出多普勒频移信息，则直接利用多普勒频移信息，若DVL输出测速信息，将测速信息转化为多普勒频移信息：
Figure BDA00024661290900000518
Figure BDA00024661290900000519
其中，[Δf1 Δf2 Δf3 Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，A为转换矩阵，
Figure BDA00024661290900000522
为DVL测速信息，C为超声波波速，α为波束倾角；
S3.2利用DVL发射时刻与DVL接收时刻INS分别输出的姿态速度信息补偿校正t1发射时刻DVL测谏信息：
Figure BDA00024661290900000520
其中，[Δf1 Δf2 Δf3 Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，C为超声波波速，A为转换矩阵，I为单位矩阵，
Figure BDA0002466129090000061
为t1时刻INS输出n系到b系的转换矩阵，
Figure BDA0002466129090000062
为t2时刻INS输出b系到n系的转换矩阵，
Figure BDA0002466129090000063
为t2时刻INS输出n系到b系的转换矩阵，
Figure BDA0002466129090000064
为t2时刻INS输出速度信息，
Figure BDA0002466129090000065
为t1时刻INS输出速度信息，
Figure BDA0002466129090000066
为t1发射时刻DVL测速信息；
S3.3利用校正后的t1发射时刻DVL测速信息计算t2接收时刻DVL测速信息：
Figure BDA0002466129090000067
其中，
Figure BDA0002466129090000068
为t2接收时刻DVL测速信息，
Figure BDA0002466129090000069
为t2时刻INS输出n系到b系的转换矩阵，
Figure BDA00024661290900000610
为t2时刻INS输出速度信息，
Figure BDA00024661290900000611
为t1时刻INS输出n系到b系的转换矩阵，
Figure BDA00024661290900000612
为t1时刻INS输出速度信息，
Figure BDA00024661290900000613
为t1发射时刻DVL测速信息。
步骤S4，对步骤S3中校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束。具体包括以下过程：
S4.1根据INS误差模型与校正后的DVL测速信息建立状态方程与量测方程：
选定状态向量为：
Figure BDA00024661290900000614
其中，X为状态向量，δVE，N，U为n系东北天向速度误差，φx，y，z为三轴安装误差角，
Figure BDA00024661290900000615
为加速度计偏差，εx，y，z为陀螺偏差；
建立离散化状态方程与量测方程：
Figure BDA00024661290900000616
其中，Xk为k时刻状态向量，φk，k-1为状态转移矩阵，Xk-1为k-1时刻状态向量，Gk-1为系统噪声输入矩阵，Wk-1为系统噪声向量，Zk为量测向量，Hk为量测矩阵，Vk为量测噪声向量；
S4.2卡尔曼滤波：
Figure BDA00024661290900000617
Figure BDA00024661290900000618
Figure BDA00024661290900000619
Figure BDA00024661290900000620
Pk＝(I-KkHk)Pk，k-1
其中，
Figure BDA0002466129090000071
为状态一步预测向量，φk，k-1为状态转移矩阵，
Figure BDA0002466129090000072
为初始状态向量，Pk，k-1为状态一步预测误差方差阵，Pk-1为初始状态误差方差阵，Gk-1为系统噪声输入矩阵，Qk-1为系统噪声误差方差阵，Kk为状态增益矩阵，Hk为量测矩阵，Rk为量测噪声误差方差阵，
Figure BDA0002466129090000073
为状态估计向量，Zk为量测向量，Pk为估计误差方差阵；
S4.3输出组合导航信息，并对INS进行反馈校正，然后重复上述步骤，直到导航任务结束。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (4)

1.一种载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，包括以下步骤：
S1：更新INS的姿态、速度和位置信息；
S2：在DVL发射时刻，记录INS姿态、速度信息；
S3：在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息；
S4：对步骤S3中校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束；
所述步骤S1具体包括以下过程：
更新INS的姿态、速度和位置信息：
Figure FDA0003636402410000011
Figure FDA0003636402410000012
Figure FDA0003636402410000013
Figure FDA0003636402410000014
Figure FDA0003636402410000015
Figure FDA0003636402410000016
Figure FDA0003636402410000017
Figure FDA0003636402410000018
其中，
Figure FDA0003636402410000019
为b系到n系的转换矩阵导数，
Figure FDA00036364024100000110
为b系到n系的转换矩阵，
Figure FDA00036364024100000111
为b系下b系到n系的角速度，(×)为叉乘，
Figure FDA00036364024100000112
为n系速度导数，Vn为n系速度，fb为比力，
Figure FDA00036364024100000113
为n系下地球自转角速度，
Figure FDA00036364024100000114
为n系下n系到e系的角速度，Gn为重力矢量，
Figure FDA00036364024100000115
为纬度导数，L为纬度，VN为n系北向速度，RM为子午圈半径，
Figure FDA00036364024100000116
为经度导数，λ为经度，VE为n系东向速度，RN为卯酉圈半径，
Figure FDA00036364024100000117
为高度导数，h为高度，VU为n系天向速度，
Figure FDA00036364024100000118
为b系下b系到i系的角速度，
Figure FDA00036364024100000119
为n系到b系的转换矩阵，Ω为地球自转角速度。
2.根据权利要求1所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S2具体包括以下过程：
在DVLt1发射时刻，记录t1时刻INS输出b系到n系的转换矩阵
Figure FDA00036364024100000120
与t1时刻INS输出速度信息
Figure FDA0003636402410000021
3.根据权利要求1所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S3具体包括以下过程：
在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息：
S3.1若DVL输出多普勒频移信息，则直接利用多普勒频移信息，若DVL输出测速信息，将测速信息转化为多普勒频移信息：
Figure FDA0003636402410000022
Figure FDA0003636402410000023
其中，[Δf1 Δf2 Δf3 Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，A为转换矩阵，
Figure FDA0003636402410000024
为DVL测速信息，C为超声波波速，α为波束倾角；
S3.2利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正t1发射时刻DVL测速信息：
Figure FDA0003636402410000025
其中，[Δf1 Δf2 Δf3 Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，C为超声波波速，A为转换矩阵，I为单位矩阵，
Figure FDA0003636402410000026
为t1时刻INS输出n系到b系的转换矩阵，
Figure FDA0003636402410000027
为t2时刻INS输出b系到n系的转换矩阵，
Figure FDA0003636402410000028
为t2时刻INS输出n系到b系的转换矩阵，
Figure FDA0003636402410000029
为t2时刻INS输出速度信息，
Figure FDA00036364024100000210
为t1时刻INS输出速度信息，
Figure FDA00036364024100000211
为t1发射时刻DVL测速信息；
S3.3利用校正后的t1发射时刻DVL测速信息计算t2接收时刻DVL测速信息：
Figure FDA00036364024100000212
其中，
Figure FDA00036364024100000213
为t2接收时刻DVL测速信息，
Figure FDA00036364024100000214
为t2时刻INS输出n系到b系的转换矩阵，
Figure FDA00036364024100000215
为t2时刻INS输出速度信息，
Figure FDA00036364024100000216
为t1时刻INS输出n系到b系的转换矩阵，
Figure FDA0003636402410000031
为t1时刻INS输出速度信息，
Figure FDA0003636402410000032
为t1发射时刻DVL测速信息。
4.根据权利要求1所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S4具体包括以下过程：
对步骤S3中校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束：
S4.1根据INS误差模型与校正后的DVL测速信息建立状态方程与量测方程：
选定状态向量为：
Figure FDA00036364024100000313
其中，X为状态向量，δVE,N,U为n系东北天向速度误差，φx,y,z为三轴安装误差角，
Figure FDA0003636402410000034
为加速度计偏差，εx,y,z为陀螺偏差；
建立离散化状态方程与量测方程：
Figure FDA0003636402410000035
其中，Xk为k时刻状态向量，φk,k-1为状态转移矩阵，Xk-1为k-1时刻状态向量，Gk-1为系统噪声输入矩阵，Wk-1为系统噪声向量，Zk为量测向量，Hk为量测矩阵，Vk为量测噪声向量；
S4.2卡尔曼滤波：
Figure FDA0003636402410000036
Figure FDA0003636402410000037
Figure FDA0003636402410000038
Figure FDA0003636402410000039
Pk＝(I-KkHk)Pk,k-1
其中，
Figure FDA00036364024100000310
为状态一步预测向量，φk,k-1为状态转移矩阵，
Figure FDA00036364024100000311
为初始状态向量，Pk,k-1为状态一步预测误差方差阵，Pk-1为初始状态误差方差阵，Gk-1为系统噪声输入矩阵，Qk-1为系统噪声误差方差阵，Kk为状态增益矩阵，Hk为量测矩阵，Rk为量测噪声误差方差阵，
Figure FDA00036364024100000312
为状态估计向量，Zk为量测向量，Pk为估计误差方差阵；
S4.3输出组合导航信息，并对INS进行反馈校正，然后重复上述步骤，直到导航任务结束。
CN202010334588.XA 2020-04-24 2020-04-24 载体高动态环境下的ins辅助dvl测速误差补偿方法 Active CN111323012B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010334588.XA CN111323012B (zh) 2020-04-24 2020-04-24 载体高动态环境下的ins辅助dvl测速误差补偿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010334588.XA CN111323012B (zh) 2020-04-24 2020-04-24 载体高动态环境下的ins辅助dvl测速误差补偿方法

Publications (2)

Publication Number Publication Date
CN111323012A CN111323012A (zh) 2020-06-23
CN111323012B true CN111323012B (zh) 2022-08-09

Family

ID=71168151

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010334588.XA Active CN111323012B (zh) 2020-04-24 2020-04-24 载体高动态环境下的ins辅助dvl测速误差补偿方法

Country Status (1)

Country Link
CN (1) CN111323012B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111854747B (zh) * 2020-08-25 2022-08-12 东南大学 一种载体大机动情况下的dvl辅助sins粗对准方法
CN112504298B (zh) * 2020-11-25 2024-03-15 东南大学 一种gnss辅助的dvl误差标定方法
CN112729291B (zh) * 2020-12-29 2022-03-04 东南大学 一种深潜长航潜水器sins/dvl洋流速度估计方法
CN114396938B (zh) * 2021-12-07 2023-08-08 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种舰船捷联惯导系统的高精度初始对准方法
CN116068540B (zh) * 2023-02-17 2023-09-12 哈尔滨工程大学 一种声学多普勒测速径向波束角校正方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US3925642A (en) * 1974-05-13 1975-12-09 United Technologies Corp Strapdown gyro drift calculator
CN101900558B (zh) * 2010-06-04 2012-11-28 浙江大学 集成声纳微导航的自主式水下机器人组合导航方法
CN102829777B (zh) * 2012-09-10 2015-09-16 江苏科技大学 自主式水下机器人组合导航系统及方法
CN103090884B (zh) * 2013-02-19 2015-05-20 哈尔滨工程大学 基于捷联惯导系统的多普勒计程仪测速误差抑制方法
CN106908086A (zh) * 2017-04-14 2017-06-30 北京理工大学 一种多普勒计程仪测速误差的修正方法

Also Published As

Publication number Publication date
CN111323012A (zh) 2020-06-23

Similar Documents

Publication Publication Date Title
CN111323012B (zh) 载体高动态环境下的ins辅助dvl测速误差补偿方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
US11754400B2 (en) Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering
WO2020062791A1 (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
US8224574B2 (en) System for multiple navigation components
US5991692A (en) Zero motion detection system for improved vehicle navigation system
US7844397B2 (en) Method and apparatus for high accuracy relative motion determination using inertial sensors
EP1221586A2 (en) Position and heading error-correction method and apparatus for vehicle navigation systems
CN109724599A (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
WO2014002210A1 (ja) 測位装置
CN112505737B (zh) 一种gnss/ins组合导航方法
WO2015186309A1 (ja) 移動距離推定装置
CN111854747B (zh) 一种载体大机动情况下的dvl辅助sins粗对准方法
WO2016203744A1 (ja) 測位装置
de La Parra et al. Low cost navigation system for UAV's
CN111397603B (zh) 载体姿态动态情况下的惯性/多普勒动基座粗对准方法
CN111982105B (zh) 一种基于sins/lbl紧组合的水下导航定位方法及系统
JP3218876B2 (ja) 車両用現在位置検出装置
CN110133695A (zh) 一种双天线gnss位置延迟时间动态估计系统及方法
JP2000502801A (ja) 多重軸加速度計を使用する改良された車両ナビゲーションシステム及びその方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN115356754A (zh) 一种基于gnss和低轨卫星的组合导航定位方法
CN112083425B (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN116222578B (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