载体高动态环境下的INS辅助DVL测速误差补偿方法技术

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

Error compensation method of INS assisted DVL in high dynamic environment

【技术实现步骤摘要】
载体高动态环境下的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的姿态、速度和位置信息：其中，为b系到n系的转换矩阵导数，为b系到n系的转换矩阵，为b系下b系到n系的角速度，(×)为叉乘，为n系速度导数，Vn为n系速度，fb为比力，为n系下地球自转角速度，为n系下n系到e系的角速度，Gn为重力矢量，为纬度导数，L为纬度，VN为n系北向速度，RM为子午圈半径，为经度导数，λ为经度，VE为n系东向速度，RN为卯酉圈半径，为高度导数，h为高度，VU为n系天向速度，为b系下b系到i系的角速度，为n系到b系的转换矩阵，Ω为地球自转角速度。所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，所述步骤S2具体包括以下过程：在DVLt1发射时刻，记录t1时刻INS输出b系到n系的转换矩阵与t1时刻INS输出速度信息所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，所述步骤S3具体包括以下过程：在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息：S3.1若DVL输出多普勒频移信息，则直接利用多普勒频移信息，若DVL输出测速信息，将测速信息转化为多普勒频移信息：其中，[Δf1Δf2Δf3Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，A为转换矩阵，为DVL测速信息，C为超声波波速，α为波束倾角；S3.2利用DVL发射时刻与DVL接收时刻INS分别输出的姿态速度信息补偿校正t1发射时刻DVL测速信息：其中，[Δf1Δf2Δf3Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，C为超声波波速，A为转换矩阵，I为单位矩阵，为t1时刻INS输出n系到b系的转换矩阵，为t2时刻INS输出b系到n系的转换矩阵，为t2时刻INS输出n系到b系的转换矩阵，为t2时刻INS输出速度信息，为t1时刻INS输出速度信息，为t1发射时刻DVL测速信息；S3.3利用校正后的t1发射时刻DVL测速信息计算t2接收时刻DVL测速信息：其中，为t2接收时刻DVL测速信息，为t2时刻INS输出n系到b系的转换矩阵，为t2时刻INS输出速度信息，为t1时刻INS输出n系到b系的转换矩阵，为t1时刻INS输出速度信息，为t1发射时刻DVL测速信息。所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，所述步骤S4具体包括以下过程：对步骤S3校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束：S4.1根据INS误差模型与校正后的DVL测速信息建立状态方程与量测方程：选定状态向量为：其中，X为状态向量，δVE,N,U为n系东北天向速度误差，φx,y,z为三轴安装误差角，为加速度计偏差，εx,y,z为陀螺偏差；建立离散化状态方程与量测方程：其中，Xk为k时刻状态向量，φk，k-1为状态转移矩阵，Xk-1为k-1时刻状态向量，Gk-1为系统噪声输入矩阵，Wk-1为系统噪声向量，Zk为量测向量，Hk为量测矩阵，Vk为量测噪声向量；S4.2卡尔曼滤波：Pk＝(I-KkHk)Pk,k-1其中，为状态一步预测向量，φk，k-1为状态转移矩阵，为初始状态向量，Pk，k-1为状态一步预测误差方差阵，Pk-1为初始状态误差方差阵，Gk-1为系统噪声输入矩阵，Qk-1为系统噪声误差方差阵，Kk为状态增益矩阵，Hk为量测矩阵，Rk为量测噪声误差方差阵，为状态估计向量，Zk为量测向量，Pk为估计误差方差阵；S4.3输出组合导航信息，并对INS进行反馈校正，然后重复上述步骤，直到导航任务结束有益效果：本专利技术利用INS输出信息校正DVL量测信息，减少了动态环境下的DVL量测误差。附图说明图1为本专利技术的载体高动态环境下的INS辅助DVL测速误差补偿方法流程图。具体实施方式下面结合附图，对本专利技术的技术方案作进一步说明。应当了解，以下提供的实施例仅是为了详尽地且完全地公开本专利技术，并且向所属
的技术人员充分传达本专利技术的技术构思，本专利技术还可以用许多不同的形式来实施，并且不局限于此处描述的实施例。对于表示在附图中的示例性实施方式中的术语并不是对本专利技术的限定。本专利技术提供的一种载体高动态环境下的INS辅助DVL测速误差补偿方法，实现原理如图1所示。其流程主要包括以下步骤：步骤S1，更新INS的本文档来自技高网...

【技术保护点】
1.一种载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，包括以下步骤：/nS1：更新INS的姿态、速度和位置信息；/nS2：在DVL发射时刻，记录INS姿态、速度信息；/nS3：在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息；/nS4：对步骤S3中校正后的DVL测速信息与步骤S1中更新后INS的姿态、速度和位置信息进行卡尔曼滤波融合，输出组合导航结果，同时对INS进行反馈校正，重复上述步骤，直到导航任务结束。/n

【技术特征摘要】
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的姿态、速度和位置信息：
























其中，为b系到n系的转换矩阵导数，为b系到n系的转换矩阵，为b系下b系到n系的角速度，(×)为叉乘，为n系速度导数，Vn为n系速度，fb为比力，为n系下地球自转角速度，为n系下n系到e系的角速度，Gn为重力矢量，为纬度导数，L为纬度，VN为n系北向速度，RM为子午圈半径，为经度导数，λ为经度，VE为n系东向速度，RN为卯酉圈半径，为高度导数，h为高度，VU为n系天向速度，为b系下b系到i系的角速度，为n系到b系的转换矩阵，Ω为地球自转角速度。


3.根据权利要求2所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S2具体包括以下过程：
在DVLt1发射时刻，记录t1时刻INS输出b系到n系的转换矩阵与t1时刻INS输出速度信息


4.根据权利要求2所述的载体高动态环境下的INS辅助DVL测速误差补偿方法，其特征在于，所述步骤S3具体包括以下过程：
在DVL接收时刻，利用DVL发射时刻与DVL接收时刻INS分别输出的姿态、速度信息补偿校正DVL测速信息：
S3.1若DVL输出多普勒频移信息，则直接利用多普勒频移信息，若DVL输出测速信息，将测速信息转化为多普勒频移信息：






其中，[Δf1Δf2Δf3Δf4]T为DVL输出多普勒频移信息，f0为DVL发射超声波信号频率，A为转换矩阵，为DVL测速信息，C为超声波波速，α为波束倾角；
S3.2利用DVL发射时刻与DVL接收时...

【专利技术属性】
技术研发人员：姚逸卿，侯岚华，徐晓苏，王迪，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏;32