CN110567454B - 一种复杂环境下sins/dvl紧组合导航方法 - Google Patents

一种复杂环境下sins/dvl紧组合导航方法 Download PDF

Info

Publication number
CN110567454B
CN110567454B CN201910847266.2A CN201910847266A CN110567454B CN 110567454 B CN110567454 B CN 110567454B CN 201910847266 A CN201910847266 A CN 201910847266A CN 110567454 B CN110567454 B CN 110567454B
Authority
CN
China
Prior art keywords
dvl
representing
sins
measurement
information
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
CN201910847266.2A
Other languages
English (en)
Other versions
CN110567454A (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 CN201910847266.2A priority Critical patent/CN110567454B/zh
Publication of CN110567454A publication Critical patent/CN110567454A/zh
Application granted granted Critical
Publication of CN110567454B publication Critical patent/CN110567454B/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

Landscapes

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

Abstract

本发明公开了一种基于SINS/DVL紧组合的水下航行器导航方法，具体包括以下步骤：步骤1：根据SINS/DVL系统的采集数据，建立状态方程；步骤2：根据步骤1中的状态方程建立量测方程；步骤3：对DVL波束信息缺失处理；步骤4：DVL波束信息受到洋流影响处理；步骤5：基于卡尔曼滤波的数据融合处理：根据步骤1和步骤2中的状态方程和量测方程，建立卡尔曼递推估计，将卡尔曼递推估计得到的状态向量反馈到SINS结果中便得到了最终的位置和速度信息。本发明能够在复杂的环境下抑制组合导航误差发散，为未来SINS/DVL紧组合方向的研究提供参考价值。

Description

一种复杂环境下SINS/DVL紧组合导航方法
技术领域：
本发明属于水下航行器组合导航技术，具体涉及一种复杂环境下SINS/DVL紧组合导航方法。
背景技术：
自主水下航行器(AUV)作为人类探索和发展海洋的重要工具，在海洋发展战略中发挥着重要作用。AUV导航系统提供诸如位置，速度和姿态之类的信息。导航信息的准确性和可靠性是AUV成功执行的保证之一。在水下组合导航系统中，辅助传感器通常包括多普勒计程仪(DVL)，测斜仪，压力传感器(PS)，捷联惯性导航系统(SINS)，陀螺罗经。IMU，即惯性测量单元，包括三轴陀螺仪和三轴加速度计，是完成SINS捷联惯性导航系统的传感器。尽管辅助传感器长时间以高精度测量，但它们在独立应用中存在若干问题。基于SINS/DVL的组合导航具有高自主性和隐蔽性，是未来AUV组合导航的发展方向之一。
目前，研究人员对SINS/DVL组合导航方法进行了大量研究。已经提出了许多数据融合算法以提高组合导航的精度。针对SINS/DVL组合系统设计了一些自适应卡尔曼滤波(AKF)，如异步自适应直接卡尔曼滤波算法，具有递归噪声估计的自适应卡尔曼滤波，以及自适应联邦强跟踪滤波算法。考虑到系统模型非线性特点。一些学者比较了用于SINS/DVL组合导航问题的EKF和UKF算法，这些算法表明EKF具有更高的估计精度。为了解决SINS/DVL组合导航中常规滤波算法精度低的问题，研究者介绍了平方根无迹信息滤波器(SR-UIF)。同时结合高斯混合密度模型和Cubature卡尔曼滤波器(CKF)，设计了基于高斯混合立方卡尔曼滤波器(GM-CKF)的导航定位算法，并通过湖试验证明了GM-CKF方法优于EKF，UKF和CKF。研究人员还将交互式多模型滤波算法引入SINS/DVL组合导航，并通过仿真验证了其优越性。根据上述研究，在较好的水下环境中，数据融合算法可以在一定程度上提高SINS/DVL组合导航的精度。目前，该领域的研究相对成熟，并成功应用于实际系统。但是针对复杂环境下的SINS/DVL组合导航问题研究较少。
发明内容：
针为提高复杂环境下的容错性，本发明从系统模型角度出发，提出一种基于SINS/DVL紧组合的水下航行器导航方法。该方法利用DVL四通道波束测量信息，能够在非完整性波束测量情况下提供连续、准确的导航信息。该方法能够在复杂的环境下抑制组合导航误差发散，为未来SINS/DVL紧组合方向的研究提供参考价值。
本发明的上述目的可通过以下技术方案来实现：
一种基于SINS/DVL紧组合的水下航行器导航方法，具体包括以下步骤：
步骤1：根据SINS/DVL系统的采集数据，建立状态方程；
步骤2：根据步骤1中的状态方程建立量测方程；
步骤3：对DVL波束信息缺失处理；
步骤4：DVL波束信息受到洋流影响处理；
步骤5：基于卡尔曼滤波的数据融合处理：根据步骤1和步骤2中的状态方程和量测方程，建立卡尔曼递推估计，将卡尔曼递推估计得到的状态向量反馈到SINS结果中便得到了最终的位置和速度信息。
所述的基于SINS/DVL紧组合的水下航行器导航方法，步骤1中所述的SINS/DVL系统的采集数据，包括：IMU采集的光纤陀螺数据；
Figure BDA0002195044440000021
其中：/>
Figure BDA0002195044440000022
表示载体坐标系下x轴的角速度值；/>
Figure BDA0002195044440000023
表示载体坐标系下y轴的角速度值；/>
Figure BDA0002195044440000024
表示载体坐标系下z轴的角速度值；
IMU采集的加速度计数据
Figure BDA0002195044440000025
其中：：/>
Figure BDA0002195044440000026
表示载体系下x轴的加速度值；/>
Figure BDA0002195044440000027
表示载体系下y轴的加速度值；/>
Figure BDA0002195044440000028
表示载体系下z轴的加速度值。
DVL采集的四个波束通道的数据：
Figure BDA0002195044440000029
其中：/>
Figure BDA00021950444400000210
表示DVL波束通道1输出的速度信息；/>
Figure BDA00021950444400000211
表示DVL波束通道2输出的速度信息；/>
Figure BDA00021950444400000212
表示DVL波束通道3输出的速度信息；/>
Figure BDA00021950444400000213
表示DVL波束通道4输出的速度信息；输出量：载体的速度信息：
Figure BDA00021950444400000214
其中：/>
Figure BDA00021950444400000215
表示导航系下x轴的速度信息；/>
Figure BDA00021950444400000216
表示导航系下y轴的速度信息；/>
Figure BDA00021950444400000217
表示导航系下z轴的速度信息；
载体的位置信息：λ,L,h，其中：λ表示地理经度；L表示地理纬度；h表示高度；
所述的基于SINS/DVL紧组合的水下航行器导航方法，步骤1中所述的建立状态方程，具体包括如下步骤；
定义建立的状态方程如下：
Figure BDA00021950444400000218
其中，F表示状态转移矩阵；X表示状态向量；G表示系统噪声矩阵；W表示系统噪声矢量，状态向量为21维，表示如下：
Figure BDA00021950444400000219
其中，[φx φy φz]T表示俯仰、横滚、航向姿态角误差；
Figure BDA0002195044440000031
表示东向、北向、天向速度误差；[δλ δL δh]T表示经度、纬度、高度误差；[▽x ▽y ▽z]T表示加速度计零偏；[εx εy εz]T表示陀螺零偏；[bD1 bD2 bD3 bD4]T表示DVL四个波束零偏；KD表示DVL刻度因子；bPS表示深度计零偏；状态转移矩阵F表示为：
Figure BDA0002195044440000032
其中，
Figure BDA0002195044440000033
Figure BDA0002195044440000034
Figure BDA0002195044440000035
Figure BDA0002195044440000036
Figure BDA0002195044440000037
Figure BDA0002195044440000041
其中：
Figure BDA0002195044440000042
表示b系到n系的姿态转移矩阵；β表示重力扁率；/>
Figure BDA0002195044440000043
表示加速度计噪声；
Figure BDA0002195044440000044
表示陀螺噪声；ωie表示地球自转角速率；VE表示东向速度；VN表示北向速度；VU表示天向速度；/>
Figure BDA0002195044440000045
表示导航系下天向加速度；/>
Figure BDA0002195044440000046
表示导航系下东向加速度；/>
Figure BDA0002195044440000047
表示导航系下北向加速度；L表示地理纬度；h表示高度；β1＝(1/8)(2βf+f2)，β2＝3.08×10-6s-2；f表示椭球扁率；
所述的基于SINS/DVL紧组合的水下航行器导航方法，步骤2中所述的建立量测方程，具体包括如下步骤：
定义建立的量测方程如下：
Z＝HX+V
其中，H为量测方程转移矩阵，X为状态向量；V为量测噪声，具体计算流程如下：
DVL坐标系定义为d系；载体坐标系定义为b系；导航坐标系定位为n系，在忽略杆臂误差情况下，以水下航行器为研究对象；
在忽略传感器误差情况下，定义载体坐标系下SINS的速度为
Figure BDA0002195044440000048
导航系下SINS的速度为/>
Figure BDA0002195044440000049
DVL坐标系下SINS的速度为
Figure BDA00021950444400000410
DVL的输出速度为/>
Figure BDA00021950444400000411
则可以得到如下式子：
Figure BDA00021950444400000412
其中，
Figure BDA00021950444400000413
表示导航系到载体系的转移矩阵；/>
Figure BDA00021950444400000414
表示载体系到DVL坐标系的转移矩阵可表示为：
Figure BDA00021950444400000415
其中，α表示波束与水下航行器的水平夹角；
定义深度计误差模型为：
Figure BDA00021950444400000416
其中，
Figure BDA00021950444400000417
为传感器测量值；HPS为真实值；δbPS为深度计零偏；wPS为测量噪声；
DVL测量误差模型定义为：
Figure BDA00021950444400000418
其中，
Figure BDA0002195044440000051
为DVL波束测量值；KD为刻度因子；/>
Figure BDA0002195044440000052
为波束真值；δbD为波束零偏；wD为波束噪声；
考虑到IMU和DVL之间存在安装角误差，假设经过标定补偿后的安装角误差为：
Figure BDA0002195044440000053
则载体坐标系到DVL坐标系的计算值为：
Figure BDA0002195044440000054
根据以上分析，SINS解算得到的速度转换到DVL坐标系下的计算公式为：
Figure BDA0002195044440000055
进一步可以得到：
Figure BDA0002195044440000056
则量测信息的误差方程为：
Figure BDA0002195044440000057
根据以上分析，量测方程中的量测值Z、转移矩阵H、和噪声V分别为：
Figure BDA0002195044440000058
Figure BDA0002195044440000059
V＝[wD wPS]T。
所述的基于SINS/DVL紧组合的水下航行器导航方法，步骤3中所述的DVL波束信息缺失处理，具体包括以下步骤：
首先引入流速固有误差公式，评估数据质量是否有效，具体为：
Figure BDA00021950444400000510
通过分析可知，只要所测水底均匀，则无论系统怎样摇摆Verror都基本趋近于零，且四路回波认为是有效数据；如果所测流场不均匀，也要看Verror来确定不均匀的程度，用于判断四路回波数据的有效性；
对于四通道DVL波束配置的DVL设备其波束信息存在以下特点：
Figure BDA0002195044440000061
因此，当DVL某个通道信息缺失时采用对角线方向的数据取反来替代，定义为虚拟波束法；当DVL信息缺失情况下采用虚拟波束法进行处理。
所述的基于SINS/DVL紧组合的水下航行器导航方法，步骤4中所述的DVL波束信息受到洋流影响处理，具体包括如下步骤：
步骤4.1：在d系下计算SINS和DVL之间的速度差；
Figure BDA0002195044440000062
步骤4.2：判断波束是否受到洋流影响，当光束测量不受水流影响时，该光束的波束测量值服从零均值高斯分布；当存在洋流时，波束测量值将发生很大变化，但SINS的速度精度保持不变，因此，不再满足零均值高斯分布，设定阈值用于确定光束测量是否受洋流影响：
Figure BDA0002195044440000063
/>
步骤4.3：用虚拟波束法处理受到洋流影响的波束测量信息，如果无法使用虚拟波束法，则将隔离波束测量。
所述的基于SINS/DVL紧组合的水下航行器导航方法，步骤5中所述的卡尔曼递推估计具体是：
Xk,k-1＝FXk-1
Figure BDA0002195044440000064
Kk＝Pk,k-1HT(HPk,k-1H+R)-1
Xk＝Xk,k-1+Kk(Zk-HXk,k-1)
Pk＝(I-Kk)Pk,k-1。
有益效果：
与现有技术相比，本发明具有如下优点：
(1)本发明充分利用了DVL四个波束测量信息，将量测值扩展到5维，当DVL波束少于3个情况下仍能够完成组合导航功能。
(2)本发明提出针对DVL波束信息缺失和DVL信息受到影响下的容错处理机制。同时引入虚拟波束方法，提高了系统的抗干扰能力。
(3)本发明将DVL和PS等辅助传感器引入建模中，实现了传感器误差参数的估计与补偿，进一步提高了组合导航精度。
(4)目前关于SINS/DVL紧组合的研究相对较少，本发明为紧组合的研究方向提供了理论参考价值。
附图说明
图1为本发明所描述的复杂环境下SINS/DVL紧组合导航方法原理图；
图2为本发明实现流程图。
具体实施方式
以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
1、一种复杂环境下SINS/DVL紧组合导航方法，其特征为：
已知量：IMU采集的光纤陀螺数据；
Figure BDA0002195044440000071
IMU采集的加速度计数据
Figure BDA0002195044440000072
DVL采集的四个波束通道的数据：
Figure BDA0002195044440000073
输出量：载体的速度信息：
Figure BDA0002195044440000074
载体的位置信息：λ,L,h
步骤1：状态方程建立。
定义建立的状态方程如下：
Figure BDA0002195044440000075
/>
其中，F表示状态转移矩阵；X表示状态向量；G表示系统噪声矩阵；W表示系统噪声矢量。状态向量为21维，可表示如下：
Figure BDA0002195044440000076
其中，[φx φy φz]T表示俯仰、横滚、航向姿态角误差；
Figure BDA0002195044440000077
表示东向、北向、天向速度误差；[δλ δL δh]T表示经度、纬度、高度误差；[▽x ▽y ▽z]T表示加速度计零偏；[εx εy εz]T表示陀螺零偏；[bD1 bD2 bD3 bD4]T表示DVL四个波束零偏；KD表示DVL刻度因子；bPS表示深度计零偏。状态转移矩阵F可表示为：
Figure BDA0002195044440000078
其中，
Figure BDA0002195044440000081
Figure BDA0002195044440000082
Figure BDA0002195044440000083
Figure BDA0002195044440000084
/>
Figure BDA0002195044440000085
Figure BDA0002195044440000086
步骤2：量测方程建立。
定义建立的量测方程如下：
Z＝HX+V
其中，H为量测方程转移矩阵，具体计算流程如下：
DVL坐标系定义为d系；载体坐标系定义为b系；导航坐标系定位为n系。在忽略杆臂误差情况下，以水下航行器为研究对象。
在忽略传感器误差情况下，我们定义载体坐标系下SINS的速度为
Figure BDA0002195044440000091
导航系下SINS的速度为/>
Figure BDA0002195044440000092
DVL坐标系下SINS的速度为/>
Figure BDA0002195044440000093
DVL的输出速度为/>
Figure BDA0002195044440000094
则可以得到如下式子：
Figure BDA0002195044440000095
其中，
Figure BDA0002195044440000096
表示导航系到载体系的转移矩阵；/>
Figure BDA0002195044440000097
表示载体系到DVL坐标系的转移矩阵可表示为：
Figure BDA0002195044440000098
其中，α表示波束与水下航行器的水平夹角。
我们定义深度计误差模型为：
Figure BDA0002195044440000099
其中，
Figure BDA00021950444400000910
为传感器测量值；HPS为真实值；δbPS为深度计零偏；wPS为测量噪声。
DVL测量误差模型定义为：
Figure BDA00021950444400000911
其中，
Figure BDA00021950444400000912
为DVL波束测量值；KD为刻度因子；/>
Figure BDA00021950444400000913
为波束真值；δbD为波束零偏；wD为波束噪声。
考虑到IMU和DVL之间存在安装角误差，假设经过标定补偿后的安装角误差为：
Figure BDA00021950444400000914
/>
则载体坐标系到DVL坐标系的计算值为：
Figure BDA00021950444400000915
根据以上分析，SINS解算得到的速度转换到DVL坐标系下的计算公式为：
Figure BDA00021950444400000916
进一步可以得到：
Figure BDA00021950444400000917
则量测信息的误差方程为：
Figure BDA0002195044440000101
根据以上分析，量测方程中的量测值Z、转移矩阵H、和噪声V分别为：
Figure BDA0002195044440000102
Figure BDA0002195044440000103
V＝[wD wPS]T
步骤3：DVL波束信息缺失处理。
DVL一般有两种工作模式：低跟踪模式和水跟踪模式。当工作在低跟踪模式下，由于复杂水下环境会导致DVL存在波束信息缺失问题。当工作在对水模式下，由于海洋洋流的影响，DVL波束信息容易受到干扰，因此针对这两种情况，基于SINS/DVL紧组合导航方法解决如下：
实际上，所测水底很少是完全均匀的，即DVL在同一深度上所测相对水底的速度大小与方向往往不同。水底的不均匀性会引入不同程度的速度固有误差。四波束Janus阵型结构可以可以验证水底的均匀性情况。为了量化水底不均匀性造成的影响，可以引入流速固有误差公式，这是评估数据质量是否有效的重要因素。具体为：
Figure BDA0002195044440000104
通过分析可知，只要所测水底均匀，则无论系统怎样摇摆Verror都基本趋近于零，且四路回波认为是有效数据；如果所测流场不均匀，也要看Verror来确定不均匀的程度，用于判断四路回波数据的有效性。
对于四通道DVL波束配置的DVL设备其波束信息存在以下特点：
Figure BDA0002195044440000105
因此，当DVL某个通道信息缺失时可以采用对角线方向的数据取反来替代。如当DVL波束通道1的信息缺失时可以用过通道3的信息取反来替代通道1的信息，从而提高了组合导航的容错性。定义这种方法为虚拟波束法。当DVL信息缺失情况下采用虚拟波束法进行处理。
步骤4：DVL波束信息受到洋流影响处理。
①在d系下计算SINS和DVL之间的速度差；
Figure BDA0002195044440000111
②判断波束是否受到洋流影响。以波束1测量为例，当光束测量不受水流影响时，该值服从零均值高斯分布。当存在洋流时，波束测量值将发生很大变化(误差变大)，但SINS的速度精度保持不变。因此，不再满足零均值高斯分布。设定阈值可用于确定光束测量是否受洋流影响。
Figure BDA0002195044440000112
③用虚拟波束法处理受到洋流影响的波束测量信息。如果无法使用虚拟波束法，则将隔离波束测量。
步骤5：基于卡尔曼滤波的数据融合处理。
根据步骤1和步骤2中的状态方程和量测方程，建立卡尔曼递推估计如下：
Xk,k-1＝FXk-1
Figure BDA0002195044440000113
Kk＝Pk,k-1HT(HPk,k-1H+R)-1
Xk＝Xk,k-1+Kk(Zk-HXk,k-1)
Pk＝(I-Kk)Pk,k-1
将卡尔曼估计得到的状态向量反馈到SINS结果中便得到了最终的位置和速度信息。

Claims (5)

1.一种基于SINS/DVL紧组合的水下航行器导航方法，其特征在于，具体包括以下步骤：
步骤1：根据SINS/DVL系统的采集数据，建立状态方程；
步骤2：根据步骤1中的状态方程建立量测方程；
步骤3：对DVL波束信息缺失处理；
步骤4：DVL波束信息受到洋流影响处理；
步骤5：基于卡尔曼滤波的数据融合处理：根据步骤1和步骤2中的状态方程和量测方程，建立卡尔曼递推估计，将卡尔曼递推估计得到的状态向量反馈到SINS结果中便得到了最终的位置和速度信息；
步骤1中所述的SINS/DVL系统的采集数据，包括：IMU采集的光纤陀螺数据；
Figure FDA0003875390310000011
其中：
Figure FDA0003875390310000012
表示载体坐标系下x轴的角速度；
Figure FDA0003875390310000013
表示载体坐标系下y轴的角速度；
Figure FDA0003875390310000014
表示载体坐标系下z轴的角速度；IMU采集的加速度计数据
Figure FDA0003875390310000015
其中：
Figure FDA0003875390310000016
表示载体系下x轴的加速度值；
Figure FDA0003875390310000017
表示载体系下y轴的加速度值；
Figure FDA0003875390310000018
表示载体系下z轴的加速度值；DVL采集的四个波束通道的数据：
Figure FDA0003875390310000019
其中：
Figure FDA00038753903100000110
表示DVL波束通道1输出的速度信息；
Figure FDA00038753903100000111
表示DVL波束通道2输出的速度信息；
Figure FDA00038753903100000112
表示DVL波束通道3输出的速度信息；
Figure FDA00038753903100000113
表示DVL波束通道4输出的速度信息；
输出量：载体的速度信息：
Figure FDA00038753903100000114
其中：
Figure FDA00038753903100000115
表示导航系下载体x轴的速度值；
Figure FDA00038753903100000116
表示导航系下载体y轴的速度值；
Figure FDA00038753903100000117
表示导航系下载体z轴的速度值；
载体的位置信息：λ,L,h，其中：λ表示地理经度值；L表示地理纬度值；h表示高度值；
步骤1中所述的建立状态方程，具体包括如下步骤；
定义建立的状态方程如下：
Figure FDA00038753903100000118
其中，F表示状态转移矩阵；X表示状态向量；G表示系统噪声矩阵；W表示系统噪声矢量，状态向量为21维，表示如下：
Figure FDA00038753903100000119
其中，[φx φy φz]T表示俯仰、横滚、航向姿态角误差；
Figure FDA00038753903100000120
表示东向、北向、天向速度误差；[δλ δL δh]T表示经度误差、纬度误差、高度误差；[▽x ▽y ▽z]T表示加速度计零偏；[εx εy εz]T表示陀螺零偏；[bD1 bD2 bD3 bD4]T表示DVL四个波束零偏；KD表示DVL刻度因子；bPS表示深度计零偏；状态转移矩阵F表示为：
Figure FDA0003875390310000021
其中，
Figure FDA0003875390310000022
Figure FDA0003875390310000023
Figure FDA0003875390310000024
Figure FDA0003875390310000025
Figure FDA0003875390310000026
Figure FDA0003875390310000031
其中：
Figure FDA0003875390310000032
表示b系到n系的姿态转移矩阵；β表示重力扁率；
Figure FDA0003875390310000033
表示加速度计噪声；
Figure FDA0003875390310000034
表示陀螺噪声；ωie表示地球自转角速率；VE表示东向速度；VN表示北向速度；VU表示天向速度；
Figure FDA0003875390310000035
表示导航系下天向加速度；
Figure FDA0003875390310000036
表示导航系下东向加速度；
Figure FDA0003875390310000037
表示导航系下北向加速度；L表示地理纬度；h表示高度；β1＝(1/8)(2βf+f2)，β2＝3.08×10-6s-2；f表示椭球扁率；
2.根据权利要求1所述的基于SINS/DVL紧组合的水下航行器导航方法，其特征在于，步骤2中所述的建立量测方程，具体包括如下步骤：
定义建立的量测方程如下：
Z＝HX+V
其中，H为量测方程转移矩阵，X表示状态向量；V表示量测噪声；具体计算流程如下：
DVL坐标系定义为d系；载体坐标系定义为b系；导航坐标系定位为n系，在忽略杆臂误差情况下，以水下航行器为研究对象；
在忽略传感器误差情况下，定义载体坐标系下SINS的速度为
Figure FDA0003875390310000038
导航系下SINS的速度为
Figure FDA0003875390310000039
DVL坐标系下SINS的速度为
Figure FDA00038753903100000310
DVL的输出速度为
Figure FDA00038753903100000311
则可以得到如下式子：
Figure FDA00038753903100000312
其中，
Figure FDA00038753903100000313
表示导航系到载体系的转移矩阵；
Figure FDA00038753903100000314
表示载体系到DVL坐标系的转移矩阵可表示为：
Figure FDA00038753903100000315
α＝70°
其中，α表示波束与水下航行器的水平夹角；
定义深度计误差模型为：
Figure FDA00038753903100000316
其中，
Figure FDA00038753903100000317
为传感器测量值；HPS为真实值；δbPS为深度计零偏；wPS为测量噪声；
DVL测量误差模型定义为：
Figure FDA0003875390310000041
其中，
Figure FDA0003875390310000042
为DVL波束测量值；KD为刻度因子；
Figure FDA0003875390310000043
为波束真值；δbD为波束零偏；wD为波束噪声；
考虑到IMU和DVL之间存在安装角误差，假设经过标定补偿后的安装角误差为：
Figure FDA0003875390310000044
其中，
Figure FDA0003875390310000045
表示x轴安装角误差；
Figure FDA0003875390310000046
表示y轴安装角误差；
Figure FDA0003875390310000047
表示z轴安装角误差；则载体坐标系到DVL坐标系的计算值为：
Figure FDA0003875390310000048
根据以上分析，SINS解算得到的速度转换到DVL坐标系下的计算公式为：
Figure FDA0003875390310000049
进一步可以得到：
Figure FDA00038753903100000410
则量测信息的误差方程为：
Figure FDA00038753903100000411
根据以上分析，量测方程中的量测值Z、转移矩阵H、和噪声V分别为：
Figure FDA00038753903100000412
Figure FDA00038753903100000413
V＝[wD wPS]T。
3.根据权利要求2所述的基于SINS/DVL紧组合的水下航行器导航方法，其特征在于，步骤3中所述的DVL波束信息缺失处理，具体包括以下步骤：
首先引入流速固有误差公式，评估数据质量是否有效，具体为：
Figure FDA0003875390310000051
通过分析可知，只要所测水底均匀，则无论系统怎样摇摆Verror都基本趋近于零，且四路回波认为是有效数据；如果所测流场不均匀，也要看Verror来确定不均匀的程度，用于判断四路回波数据的有效性；
对于四通道DVL波束配置的DVL设备其波束信息存在以下特点：
Figure FDA0003875390310000052
因此，当DVL某个通道信息缺失时采用对角线方向的数据取反来替代，定义为虚拟波束法；当DVL信息缺失情况下采用虚拟波束法进行处理。
4.根据权利要求3所述的基于SINS/DVL紧组合的水下航行器导航方法，其特征在于，步骤4中所述的DVL波束信息受到洋流影响处理，具体包括如下步骤：
步骤4.1：在d系下计算SINS和DVL之间的速度差；
Figure FDA0003875390310000053
步骤4.2：判断波束是否受到洋流影响，当光束测量不受水流影响时，该光束的波束测量值服从零均值高斯分布；当存在洋流时，波束测量值将发生很大变化，但SINS的速度精度保持不变，因此，不再满足零均值高斯分布，设定阈值用于确定光束测量是否受洋流影响：
Figure FDA0003875390310000054
步骤4.3：用虚拟波束法处理受到洋流影响的波束测量信息，如果无法使用虚拟波束法，则将隔离波束测量。
5.根据权利要求4所述的基于SINS/DVL紧组合的水下航行器导航方法，其特征在于，步骤5中所述的卡尔曼递推估计具体是：
Xk,k-1＝FXk-1
Figure FDA0003875390310000055
Kk＝Pk,k-1HT(HPk,k-1H+R)-1
Xk＝Xk,k-1+Kk(Zk-HXk,k-1)
Pk＝(I-Kk)Pk,k-1。
CN201910847266.2A 2019-09-08 2019-09-08 一种复杂环境下sins/dvl紧组合导航方法 Active CN110567454B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910847266.2A CN110567454B (zh) 2019-09-08 2019-09-08 一种复杂环境下sins/dvl紧组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910847266.2A CN110567454B (zh) 2019-09-08 2019-09-08 一种复杂环境下sins/dvl紧组合导航方法

Publications (2)

Publication Number Publication Date
CN110567454A CN110567454A (zh) 2019-12-13
CN110567454B true CN110567454B (zh) 2023-03-24

Family

ID=68778392

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910847266.2A Active CN110567454B (zh) 2019-09-08 2019-09-08 一种复杂环境下sins/dvl紧组合导航方法

Country Status (1)

Country Link
CN (1) CN110567454B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111141281A (zh) * 2020-01-03 2020-05-12 中国船舶重工集团公司第七0七研究所 一种sins/dvl组合导航数据后处理误差估计方法
CN111964684B (zh) * 2020-08-21 2023-11-17 运城学院 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN112507281B (zh) * 2020-11-19 2024-01-02 东南大学 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法
CN112504298B (zh) * 2020-11-25 2024-03-15 东南大学 一种gnss辅助的dvl误差标定方法
CN112987067B (zh) * 2021-02-24 2023-06-27 博雅工道（北京）机器人科技有限公司 组合导航的洋流速度估计方法、装置和一种处理器
CN114440878B (zh) * 2022-01-27 2023-08-15 湖南大学无锡智能控制研究院 一种sins和dvl组合导航方法、设备与系统
CN114459476B (zh) * 2022-03-09 2024-03-01 东南大学 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法
CN115031724A (zh) * 2022-03-21 2022-09-09 哈尔滨工程大学 Sins/dvl紧组合系统dvl波束故障处理方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103744098B (zh) * 2014-01-23 2017-03-15 东南大学 基于sins/dvl/gps的auv组合导航系统
KR101990405B1 (ko) * 2017-06-28 2019-06-18 국방과학연구소 시간지연 적응 보상기능을 갖는 관성항법장치 및 그의 급속 초기정렬 방법
CN110031882B (zh) * 2018-08-02 2023-05-30 哈尔滨工程大学 一种基于sins/dvl组合导航系统的外量测信息补偿方法
GB2579415B (en) * 2018-11-30 2021-11-10 Thales Holdings Uk Plc Method and apparatus for determining a position of a vehicle

Also Published As

Publication number Publication date
CN110567454A (zh) 2019-12-13

Similar Documents

Publication Publication Date Title
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN110031882B (zh) 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN109724599B (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN109945860B (zh) 一种基于卫星紧组合的ins和dr惯性导航方法与系统
CN102169184B (zh) 组合导航系统中测量双天线gps安装失准角的方法和装置
CN111323050B (zh) 一种捷联惯导和多普勒组合系统标定方法
CN112507281B (zh) 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN112762961B (zh) 一种车载惯性里程计组合导航在线标定方法
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN104697520B (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN104344837A (zh) 一种基于速度观测的冗余惯导系统加速度计系统级标定方法
CN113203418A (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN109708663B (zh) 基于空天飞机sins辅助的星敏感器在线标定方法
CN111829512A (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN108931791A (zh) 卫惯紧组合钟差修正系统和方法
CN105547289A (zh) 一种水下航行器组合导航系统及导航信息融合方法
CN113834483A (zh) 一种基于可观测度的惯性/偏振/地磁容错导航方法
CN113340298A (zh) 一种惯导和双天线gnss外参标定方法
CN105988129A (zh) 一种基于标量估计算法的ins/gnss组合导航方法
CN109974695B (zh) 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法
CN115657097A (zh) 一种基于轨道约束的leo几何法定轨模糊度快速重收敛方法

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