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

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

Info

Publication number
CN111323012A
CN111323012A CN202010334588.XA CN202010334588A CN111323012A CN 111323012 A CN111323012 A CN 111323012A CN 202010334588 A CN202010334588 A CN 202010334588A CN 111323012 A CN111323012 A CN 111323012A
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.)
Granted
Application number
CN202010334588.XA
Other languages
English (en)
Other versions
CN111323012B (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 (5)

1.一种载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，包括以下步骤：
S1：更新INS的姿态、速度和位置信息；
S2：在DVL发射时刻，记录INS姿态、速度信息；
S3：在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息；
S4：对步骤S3中校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束。
2.根据权利要求1所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S1具体包括以下过程：
更新INS的姿态、速度和位置信息：
Figure FDA0002466129080000011
Figure FDA0002466129080000012
Figure FDA0002466129080000013
Figure FDA0002466129080000014
Figure FDA0002466129080000015
Figure FDA0002466129080000016
Figure FDA0002466129080000017
Figure FDA0002466129080000018
其中，
Figure FDA0002466129080000019
为b系到n系的转换矩阵导数，
Figure FDA00024661290800000110
为b系到n系的转换矩阵，
Figure FDA00024661290800000111
为b系下b系到n系的角速度，(×)为叉乘，
Figure FDA00024661290800000112
为n系速度导数，Vn为n系速度，fb为比力，
Figure FDA00024661290800000113
为n系下地球自转角速度，
Figure FDA00024661290800000114
为n系下n系到e系的角速度，Gn为重力矢量，
Figure FDA00024661290800000115
为纬度导数，L为纬度，VN为n系北向速度，RM为子午圈半径，
Figure FDA00024661290800000116
为经度导数，λ为经度，VE为n系东向速度，RN为卯酉圈半径，
Figure FDA00024661290800000117
为高度导数，h为高度，VU为n系天向速度，
Figure FDA00024661290800000118
为b系下b系到i系的角速度，
Figure FDA00024661290800000119
为n系到b系的转换矩阵，Ω为地球自转角速度。
3.根据权利要求2所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S2具体包括以下过程：
在DVLt1发射时刻，记录t1时刻INS输出b系到n系的转换矩阵
Figure FDA0002466129080000025
与t1时刻INS输出速度信息
Figure FDA0002466129080000026
4.根据权利要求2所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S3具体包括以下过程：
在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息：
S3.1若DVL输出多普勒频移信息，则直接利用多普勒频移信息，若DVL输出测速信息，将测速信息转化为多普勒频移信息：
Figure FDA0002466129080000021
Figure FDA0002466129080000022
其中，[Δf1 Δf2 Δf3 Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，A为转换矩阵，
Figure FDA0002466129080000027
为DVL测速信息，C为超声波波速，α为波束倾角；
S3.2利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正t1发射时刻DVL测速信息：
Figure FDA0002466129080000023
其中，[Δf1 Δf2 Δf3 Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，C为超声波波速，A为转换矩阵，I为单位矩阵，
Figure FDA0002466129080000028
为t1时刻INS输出n系到b系的转换矩阵，
Figure FDA0002466129080000029
为t2时刻INS输出b系到n系的转换矩阵，
Figure FDA00024661290800000210
为t2时刻INS输出n系到b系的转换矩阵，
Figure FDA00024661290800000211
为t2时刻INS输出速度信息，
Figure FDA00024661290800000212
为t1时刻INS输出速度信息，
Figure FDA00024661290800000213
为t1发射时刻DVL测速信息；
S3.3利用校正后的t1发射时刻DVL测速信息计算t2接收时刻DVL测速信息：
Figure FDA0002466129080000024
其中，
Figure FDA00024661290800000214
为t2接收时刻DVL测速信息，
Figure FDA00024661290800000215
为t2时刻INS输出n系到b系的转换矩阵，
Figure FDA0002466129080000037
为t2时刻INS输出速度信息，
Figure FDA0002466129080000038
为t1时刻INS输出n系到b系的转换矩阵，
Figure FDA0002466129080000039
为t1时刻INS输出速度信息，
Figure FDA00024661290800000310
为t1发射时刻DVL测速信息。
5.根据权利要求2所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S4具体包括以下过程：
对步骤S3中校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束：
S4.1根据INS误差模型与校正后的DVL测速信息建立状态方程与量测方程：
选定状态向量为：
Figure FDA0002466129080000031
其中，X为状态向量，δVE,N,U为n系东北天向速度误差，φx,y,z为三轴安装误差角，
Figure FDA00024661290800000311
为加速度计偏差，εx,y,z为陀螺偏差；
建立离散化状态方程与量测方程：
Figure FDA0002466129080000032
其中，Xk为k时刻状态向量，φk,k-1为状态转移矩阵，Xk-1为k-1时刻状态向量，Gk-1为系统噪声输入矩阵，Wk-1为系统噪声向量，Zk为量测向量，Hk为量测矩阵，Vk为量测噪声向量；
S4.2卡尔曼滤波：
Figure FDA0002466129080000033
Figure FDA0002466129080000034
Figure FDA0002466129080000035
Figure FDA0002466129080000036
Pk＝(I-KkHk)Pk，k-1
其中，
Figure FDA00024661290800000312
为状态一步预测向量，φk,k-1为状态转移矩阵，
Figure FDA00024661290800000313
为初始状态向量，Pk,k-1为状态一步预测误差方差阵，Pk-1为初始状态误差方差阵，Gk-1为系统噪声输入矩阵，Qk-1为系统噪声误差方差阵，Kk为状态增益矩阵，Hk为量测矩阵，Rk为量测噪声误差方差阵，
Figure FDA00024661290800000314
为状态估计向量，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 true CN111323012A (zh) 2020-06-23
CN111323012B 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)

Cited By (5)

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

Citations (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
CN101900558A (zh) * 2010-06-04 2010-12-01 浙江大学 集成声纳微导航的自主式水下机器人组合导航方法
CN102829777A (zh) * 2012-09-10 2012-12-19 江苏科技大学 自主式水下机器人组合导航系统及方法
CN103090884A (zh) * 2013-02-19 2013-05-08 哈尔滨工程大学 基于捷联惯导系统的多普勒计程仪测速误差抑制方法
CN106908086A (zh) * 2017-04-14 2017-06-30 北京理工大学 一种多普勒计程仪测速误差的修正方法

Patent Citations (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
CN101900558A (zh) * 2010-06-04 2010-12-01 浙江大学 集成声纳微导航的自主式水下机器人组合导航方法
CN102829777A (zh) * 2012-09-10 2012-12-19 江苏科技大学 自主式水下机器人组合导航系统及方法
CN103090884A (zh) * 2013-02-19 2013-05-08 哈尔滨工程大学 基于捷联惯导系统的多普勒计程仪测速误差抑制方法
CN106908086A (zh) * 2017-04-14 2017-06-30 北京理工大学 一种多普勒计程仪测速误差的修正方法

Cited By (10)

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

Also Published As

Publication number Publication date
CN111323012B (zh) 2022-08-09

Similar Documents

Publication Publication Date Title
CN111323012B (zh) 载体高动态环境下的ins辅助dvl测速误差补偿方法
CN109443379B (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
US11754400B2 (en) Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering
US8224574B2 (en) System for multiple navigation components
US7844397B2 (en) Method and apparatus for high accuracy relative motion determination using inertial sensors
US5991692A (en) Zero motion detection system for improved vehicle navigation system
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航系统
CN103697910B (zh) 自主水下航行器多普勒计程仪安装误差的校正方法
JP4199553B2 (ja) ハイブリッド航法装置
CN110221332A (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN102252677A (zh) 一种基于时间序列分析的变比例自适应联邦滤波方法
CN111854747B (zh) 一种载体大机动情况下的dvl辅助sins粗对准方法
WO2016203744A1 (ja) 測位装置
CN111397603B (zh) 载体姿态动态情况下的惯性/多普勒动基座粗对准方法
de La Parra et al. Low cost navigation system for UAV's
CN104061930B (zh) 一种基于捷联惯性制导和多普勒计程仪的导航方法
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN113252031A (zh) 一种narx神经网络辅助组合导航的方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN115979253A (zh) 一种基于抗差滤波的水下机器人多传感器紧组合导航方法
CN103697887B (zh) 一种基于捷联惯导系统和多普勒计程仪的优化导航方法
CN117029872A (zh) 一种基于ins/dvl/lbl紧组合的auv导航方法及导航系统
JP6962040B2 (ja) 方位推定装置
CN106323226B (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