CN115031724A - Sins/dvl紧组合***dvl波束故障处理方法 - Google Patents
Sins/dvl紧组合***dvl波束故障处理方法 Download PDFInfo
- Publication number
- CN115031724A CN115031724A CN202210279711.1A CN202210279711A CN115031724A CN 115031724 A CN115031724 A CN 115031724A CN 202210279711 A CN202210279711 A CN 202210279711A CN 115031724 A CN115031724 A CN 115031724A
- Authority
- CN
- China
- Prior art keywords
- dvl
- fault
- coordinate system
- information
- sins
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 238000012545 processing Methods 0.000 title claims abstract description 11
- 238000005259 measurement Methods 0.000 claims abstract description 59
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000009434 installation Methods 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 6
- 230000002159 abnormal effect Effects 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 230000007704 transition Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 7
- 238000006073 displacement reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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
-
- 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
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
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)
- Navigation (AREA)
Abstract
本发明公开了一种SINS/DVL紧组合***DVL波束故障处理方法,步骤一、进行捷联惯性导航***初始对准,然后将声学多普勒计程仪波束量测信息传送到捷联惯性导航***;步骤二:构建捷联惯性导航***状态参量X及状态方程;步骤三、选取捷联惯性导航***量测量Z并构建量测方程,步骤四:利用基于新息的χ2检验方法对接收到的DVL波束量测信息进行故障检测,步骤五、根据不同波束故障情况,重构故障波束速度信息。本发明直接使用了DVL原始的波束量测信息,能够更加充分的获得可利用的有效信息,当部分波束量测出现故障时,本发明通过相应的波束信息故障处理方法使组合导航***依旧维持较高的导航精度。
Description
技术领域
本发明属于导航技术领域,涉及一种SINS/DVL紧组合***DVL波束故障处理方法。
背景技术
近年来,水下航行器应用在国民经济、海洋开发和军事等领域都起到了至关重要的作用。然而复杂的水下环境给航行器长时间、高精度导航带来了巨大挑战,在某些特殊应用领域又要求水下导航***具有隐蔽性和自主性。捷联惯性导航***(SINS)因其具有可靠性高、自主性强、输出的导航信息全面等优势被广泛应用于水下导航领域,但因其特殊的导航原理决定了导航参数的误差会随时间累积,长航时的导航精度无法保证。由于水下环境电磁信号衰减严重,因此,SINS/全球卫星导航***(GNSS)组合导航***在水下导航难以实现。相对电磁信号,声波在水下环境中具有衰减小、传播范围广等优势,因此声学导航设备在水下环境得到大量应用。多普勒计程仪(DVL)可以利用发射信号与接收信号之间的多普勒频移测量载体的速度,具备相对稳定的误差特性,且不受到工作范围约束、自主性强、可靠性高。目前,SINS/DVL组合导航***为水下导航的主要导航方式。当应用在复杂水下环境导航时,DVL可能会出现故障情况和测速异常等情况,现有的SINS/DVL组合导航研究多为通过完善模型提高导航精度,所提解决方案也无法对误差量精准的建模,同时会引入新的速度量测噪声等问题。长时间的先验信息与噪声统计特性不匹配将导致组合导航结果精度变低。
发明内容
针对上述现有技术,本发明要解决的技术问题是提供一种SINS/DVL紧组合***DVL波束故障处理方法,当部分波束量测出现故障时,通过相应的波束信息故障处理方法使组合导航***依旧维持较高的导航精度。
为解决上述技术问题,本发明的一种SINS/DVL紧组合***DVL波束故障处理方法,包括:
步骤一、进行捷联惯性导航***初始对准,然后将声学多普勒计程仪波束量测信息传送到捷联惯性导航***;
步骤二:构建捷联惯性导航***状态参量X及状态方程;
步骤三、选取捷联惯性导航***量测量Z并构建量测方程,以四波束詹纳斯配置结构坐标系B下的DVL四波束方向的速度误差作为量测量,该量测量由捷联惯性导航***输出的速度信息在DVL四波束方向上的投影与DVL四波束速度量测信息之差组成;
步骤四:利用基于新息的χ2检验方法对接收到的DVL波束量测信息进行故障检测,SINS/DVL紧组合导航***离散化***方程和量测方程表示如下:
Xk=Φk/k-1Xk-1+Γk/k-1Wk-1
Zk=HkXk与Vk
其中,Xk,Zk为状态量和量测量的离散化表示;Φk/k-1,Γk/k-1和Hk分别为状态一步转移矩阵、***噪声分配矩阵及量测矩阵;Wk-1是***噪声向量,Vk是量测噪声向量;SINS/DVL紧组合***卡尔曼滤波残差为rk=Zk-HkXk/k-1,式中,Xk/k-1为状态一步预测,残差服从零均值高斯分布,其方差为:
其中,Pk/k-1=E[Xk/k-1Xk/k-1 T]为一步预测协方差矩阵,为量测噪声协方差矩阵;各波束多维残差独立不相关,即rk(i)~N(0,Sk(i,i)),i=1,2,3,4,选取各波束样本数据为ηk(i)=rk(i)rk(i)T/Sk(i,i),(i=1,2,3,4),其总体服从卡方分布,选取阈值ηv,当样本数据小于阈值时认为对应波束速度信息正常,当样本值大于阈值时认为对应波束速度信息存在异常,存在故障情况,执行步骤五;
步骤五、根据不同波束故障情况,重构故障波束速度信息。
进一步的,步骤二构建捷联惯性导航***状态参量及状态方程具体为:
选取状态参量为:
n为导航坐标系,与当地地理坐标系重合,x、y、z轴分别指向东、北、天;δP=[δL δλδh]T为位置误差矢量,δL为纬度位置误差量、δλ为经度位置误量、δh高度位置误差量,δvn=[δvE δvN δvU]T为速度误差矢量,δvE为东向速度误差、δvN为北向速度误差、δvU天向速度误差,φ=[φe φu φu]T为失准角矢量,φe、φn、φu分别为东、北、天三个方向的失准角;b系为载体坐标系,εb=[εx εy εz]T为载体坐标系下陀螺常值漂移矢量,εx、εy、εz分别为x、y、z轴向的陀螺常值漂移,为载体坐标系下加速度计常值偏置矢量,为别为x、y、z轴向的加速度计常值偏置;
根据选取状态参量得到捷联惯性导航***的状态方程为:
其中,状态量上的点代表一阶导,为导航系相对惯性系的旋转角速度在导航系的投影,包括两个矢量部分:其中为地球自转角速度在导航坐标系的投影矢量,为载体在导航系相对地固坐标系的旋转角速度在导航坐标系的投影矢量,ωie为地球自转角速率标量,L为当地纬度,Rh为载体与地球中心的距离,其中Rh=Re+h,Re为地球半径,h为载体所在海拔高度,vn=[vE vN vU]T为载体速度矢量在导航坐标系的投影,vE、vN、vU分别为东向速度、北向速度及天向速度,fb是加速度计输出载体坐标系的比力矢量;
为捷联惯导的姿态矩阵,T11、T12、T13、T21、T22、T23、T31、T32、T33是姿态矩阵的各元素、为载体坐标系下陀螺量测的高斯白噪声矢量,为载体坐标系下x、y、z轴向陀螺量测高斯白噪声、为载体坐标系下加速度计量测高斯白噪声矢量,为载体坐标系下x、y、z轴向加速度计量测高斯白噪声。
进一步的,步骤三具体为:
SINS输出导航坐标系下的速度,利用惯性测速信息构造DVL波束方向速度信息,根据SINS输出导航参数有 为SINS输出速度在载体坐标系的投影,为载体真实速度在导航坐标系下的投影,为SINS输出导航坐标系下的速度误差;SINS导航信息构造的B坐标系下DVL波束方向速度信息为因此,以DVL输出速度为匹配量的SINS/DVL紧组合量测方程可以表示为其中为DVL测量获得的波束方向速度信息,为波束方向测速误差。
进一步的,步骤五中根据不同波束故障情况,重构故障波束速度信息具体为:
当单个波束发生故障时,重构故障波束速度信息Vres为:
Vres=Vi+Vj-Vn
其中,i、j表示与故障波束相邻的波束,n为与故障波束构成对角线关系的波束,Vi,Vj,Vn分别表示对应DVL波束的量测速度信息,适用于四波束DVL的“+”和“×”型配置方式,当四波数Janus配置安装角β为0度时,是“+”型配置方式,当四波数Janus配置安装角β为45度时,是“×”型配置方式;
当两个相邻波束速度信息Vn、Vl故障时(n,l∈{[1,2],[2,3],[3,4],[4,1]}),构造故障波束速度信息为:
n、l分别为与i、j呈对角线关系的故障波束,α为由DVL结构决定的固定波束发射角度;
当n、l∈{[1,3],[2,4]}且DVL采用“×”配置时,构造故障波束速度信息为:
当n、l∈{[2,4]}且DVL采用“+”配置时,构造故障波束速度信息为:
当n、l∈{[1,3]}且DVL采用“+”配置时,构造故障波束速度信息为:
本发明的有益效果:本发明是一种声学多普勒计程仪与捷联惯性导航***紧组合在多普勒计程仪波束信息发生故障时的补偿方法。本发明以多普勒计程仪的波束量测信息为基础,考虑到复杂水下环境多普勒计程仪波束信息可能出现故障,通过卡方检测故障情况并提出相应的波束信息重构方法。与传统的SINS/DVL松组合导航***相比,SINS/DVL紧组合导航***直接使用了DVL原始的波束量测信息,与松组合对于信息的降维使用不同,能够更加充分的获得可利用的有效信息,当部分波束量测出现故障时,松组合导航***DVL无法提供辅助导航作用,紧组合导航***可通过相应的波束信息故障处理方法使组合导航***依旧维持较高的导航精度。本发明在紧组合研究通常是以完善模型的方式提高导航精度的情况下,对DVL波束量测出现故障时提供新的解决方案,具有较高的工程应用价值。
附图说明
图1是本发明实例中的四波束Janus阵列配置结构图。
图2是本发明实例中的四波数Janus配置波束故障处理方案图。
图3(a)是本发明实例中的DVL故障情况合速度误差及统计情况图。
图3(b)是本发明实例中的DVL故障情况纯惯性合速度误差统计情况图。
图3(c)是本发明实例中的DVL故障情况case3合速度误差统计情况图。
图3(d)是本发明实例中的DVL故障情况case2合速度误差统计情况图。
图3(e)是本发明实例中的DVL故障情况case1合速度误差统计情况图。
图4是本发明实例中的DVL故障情况下径向定位误差图。
图5是本发明实例中的DVL故障情况下东向、北向定位误差图。
具体实施方式
下面结合说明书附图和实施例对本发明做进一步说明。
如图1所示,为本发明适用的DVL四波束Janus阵列配置结构图,其中b系为载体坐标系,y轴指向载体纵轴方向,x轴指向载体横轴方向且与y轴垂直,z轴与x轴、y轴构成右手坐标系。α为由DVL结构决定的固定波束发射角度,β是四波数Janus配置安装角,“+”配置时为0度,“×”配置时为45度。忽略刻度系数误差及安装偏差角影响,载体坐标系三维速度和每个波束方向速度的关系为
上式中表示由波束方向测量速度构成的速度矢量信息,上角标T表示向量转置,为DVL四个波束方向的速度矢量信息;表示DVL输出速度信息在载体坐标系下的投影;C为波束方向矩阵。本发明具有波束信息构造易实现,故障处理效果优良等优势。包括如下步骤:
步骤一:航行器上配置多普勒计程仪,进行安装偏差角的标定,这里认为安装偏差角标定准确。
步骤二:进行捷联惯性导航***初始对准后,将声学多普勒计程仪波束量测信息传送到捷联惯性导航***,这里认为捷联惯导提供的姿态矩阵满足精度要求。
步骤三:构建捷联惯性导航***状态参量及状态方程。选取状态量为
n为导航坐标系,与当地地理坐标系重合,x、y、z轴分别指向东、北、天。δP=[δL δλδh]T为位置误差矢量,δL为纬度位置误差量、δλ为经度位置误量、δh高度位置误差量,δvn=[δvE δvN δvU]T为速度误差矢量,δvE为东向速度误差、δvN为北向速度误差、δvU天向速度误差,φ=[φe φn φu]T为失准角矢量,φe、φn、φu分别为东、北、天三个方向的失准角,εb=[εx εy εz]T为陀螺常值漂移矢量,εx、εy、εz分别为x、y、z轴向的陀螺常值漂移,为加速度计常值偏置矢量,为别是x、y、z轴向的加速度计常值偏置。
进而根据所选各状态参量可得捷联惯性导航***的状态方程为:
其中状态量上的点代表一阶导,为导航系相对惯性系的旋转角速度在导航系的投影,它包括两个矢量部分:其中为地球自转角速度在导航坐标系的投影矢量,为载体在地球表面运动引起的导航系相对地固坐标系的旋转角速度在导航坐标系的投影矢量,ωie为地球自转角速率标量,L为当地纬度,Rh为载体与地球中心的距离,其中Rh=Re+h,Re为地球半径,h为载体所在海拔高度,vn=[vE vN vU]T为载体速度矢量在导航坐标系的投影,vE、vN、vU分别为东向速度、北向速度及天向速度,fb是加速度计输出载体坐标系的比力矢量,
为捷联惯导的姿态矩阵,T11、T12、T13、T21、T22、T23、T31、T32、T33是姿态矩阵的各元素、为陀螺量测的高斯白噪声矢量,为x、y、z轴向陀螺量测高斯白噪声、为加速度计量测高斯白噪声矢量,为x、y、z轴向加速度计量测高斯白噪声。
步骤四:选取捷联惯性导航***量测参量及构建量测方程。传统的SINS/DVL松组合导航***以载体坐标系三维速度误差量作为量测,本发明采取的紧组合方式以DVL四波束方向的速度误差作为量测量,该量测量由捷联惯性导航***输出的速度信息在DVL四波束方向上的投影与DVL四波束速度量测信息之差组成,具体构造方法如下:
SINS输出导航坐标系下的速度,需要利用惯性测速信息构造DVL波束方向速度信息,根据SINS输出导航参数有 为SINS输出速度在载体坐标系的投影,为载体真实速度在导航坐标系下的投影,为SINS输出导航坐标系下的速度误差;SINS导航信息构造的DVL波束方向速度信息为因此,以DVL输出速度为匹配量的SINS/DVL紧组合量测方程可以表示为其中为DVL测量获得的波束方向速度信息,为波束方向测速误差。
步骤五:利用基于新息的χ2检验方法对接收到的DVL波束量测信息进行检测,并对发生故障的波束信息进行重构。SINS/DVL紧组合导航***离散化***方程和量测方程表示如下:
Xk=Φk/k-1Xk-1+Γk/k-1Wk-1
Zk=HkXk+Vk
其中,Xk,Zk为上述状态参量和量测量的离散化表示;Φk/k-1,Γk/k-1和Hk分别称为状态一步转移矩阵、***噪声分配矩阵及量测矩阵;Wk-1是***噪声向量,Vk是量测噪声向量。SINS/DVL紧组合***卡尔曼滤波残差为rk=Zk-HkXk/k-1,式中,Xk/k-1为状态一步预测,在理想情况下,残差服从零均值高斯分布,其方差为:
其中,Pk/k-1=E[Xk/k-1Xk/k-1 T]为一步预测协方差矩阵,为量测噪声协方差矩阵。因多维残差独立不相关,即rk(i)~N(0,Sk(i,i)),i=1,2,3,4,选取样本数据为ηk(i)=rk(i)rk(i)T/Sk(i,i),(i=1,2,3,4),其总体服从卡方分布,根据自由度和需求的虚警率参照卡方分布表来选取合适的阈值ηv,当样本小于阈值时认为对应波束速度信息正常,当样本值大于阈值时认为波束速度信息存在异常。进而判断出波束故障的情况并进行相应的故障处理,本发明提出的SINS/DVL紧组合导航DVL波束故障容错处理方案如下所示:
(1)单波束故障
在单个波束故障时,故障波束测速信息可以等价为其他三波束测速信息的线性组合,可以重构故障波束方向测速Vres为
Vres=Vi+Vj-Vn
i、j表示与故障波束相邻的波束,n为与故障波束构成对角线关系的波束,即Vi,Vj,Vn分别表示对应DVL波束的量测速度信息。这一方法可以适用于四波束DVL的“+”和“×”型配置方式。
(2)多波束故障
当两个相邻波束速度信息Vn、Vl故障时(n,l∈{[1,2],[2,3],[3,4],[4,1]}),C([n,l],:)的秩等于2,DVL垂向速度信息等于SINS输出的载体垂向信息构造故障波束速度信息为:
n、l分别为与i、j呈对角线关系的故障波束。当n、l∈{[1,3],[2,4]}且DVL采用“×”配置时,此时考虑到载体侧向速度及SINS输出侧向速度误差足够小,即DVL侧向速度等于SINS输出则构造故障波束速度信息为:
当一个波束方向速度信息有效时,波束方向矩阵C(i,:),(i∈{1,2,3,4})的秩等于1,需要同时引入两个维度的SINS速度信息才能重构所有的波束速度,考虑载体在垂向及侧向速度较小,所造成侧向速度误差及垂向速度误差也较小,所以通常情况下仅采用引入来重构四波束DVL速度,而在“+”型Janus配置结构波束2或4波束方向信息有效时,此时有效波束信息中不包含载体前向速度信息,故需引入进行波束速度信息重构。综上所述,四波束Janus配置波束故障处理方案如图2所示,其中不同颜色的线型描述了不同重构方案及所用的SINS速度信息。
仿真验证
仿真条件:惯性导航设备初始位置误差设置为惯性测量单元参数如下:陀螺常值漂移0.01°h,随机游走系数为加速度计常值偏置3×10-4g,随机游走系数为采样时间间隔为10ms。多普勒计程仪测速误差为0.1%航速±0.3ms,在14000秒至17600秒设置一小时的波束故障情况。
图3(a)-3(e)为DVL故障情况合速度误差及统计情况图,利用所提出的故障检测及处理方案对故障进行处理,并与在此区间内利用纯惯性导航的方案进行对比。Case1表示DVL波束2发生故障,Case2表示DVL波束2、3发生故障,Case3表示DVL波束2、3、4发生故障,可以看出在故障数据段采用惯性导航方案的速度产生较大偏移,在分布概率图中,速度误差在-0.2m/s至-0.4m/s区间段内存在较大分布概率,即速度误差存在偏置,从误差分布图中可以看出,经过所提波束补偿方案的处理之后,组合***的速度误差呈现出零均值的高斯分布,即估计速度近似为真实速度的无偏估计。图4为DVL故障情况下径向定位误差图,可以看出在波束故障时段内采用惯性导航方式,在短时间内定位误差呈正弦趋势增长,一个小时定位误差增长约550m。同时在速度信息恢复时,位置误差并没有减小。本文提出的故障处理方案在定位精度上相较于采用惯性导航方案有了较大提高,其中Case 1在故障时间段误差几乎没有增加,在Case 2中定位误差约增长55m,Case 3中定位误差增长约90m,约为纯惯性方案定位误差的6.96%、12.95%、19.95%。图5为东向、北向定位误差图,可以看出故障Case 2的应对方案是将天向速度认为是零,天向速度误差经俯仰角的投影转为载体前向的速度误差,由此造成前向定位误差,载体在东向位移较大,北向位移较小,所以定位误差主要表现在东向定位误差约为75m;Case 3的应对方案将载体侧向及天向速度认为是零,此时造成了侧向及前向速度的不准确,东向及北向定位误差分别接近100m及50m。
Claims (4)
1.一种SINS/DVL紧组合***DVL波束故障处理方法,其特征在于,包括:
步骤一、进行捷联惯性导航***初始对准,然后将声学多普勒计程仪波束量测信息传送到捷联惯性导航***;
步骤二:构建捷联惯性导航***状态参量X及状态方程;
步骤三、选取捷联惯性导航***量测量Z并构建量测方程,以四波束詹纳斯配置结构坐标系B下的DVL四波束方向的速度误差作为量测量,该量测量由捷联惯性导航***输出的速度信息在DVL四波束方向上的投影与DVL四波束速度量测信息之差组成;
步骤四:利用基于新息的χ2检验方法对接收到的DVL波束量测信息进行故障检测,SINS/DVL紧组合导航***离散化***方程和量测方程表示如下:
Xk=Φk/k-1Xk-1+Γk/k-1Wk-l
Zk=HkXk+Vk
其中,Xk,Zk为状态量和量测量的离散化表示;Φk/k-1,Γk/k-1和Hk分别为状态一步转移矩阵、***噪声分配矩阵及量测矩阵;Wk-1是***噪声向量,Vk是量测噪声向量;SINS/DVL紧组合***卡尔曼滤波残差为rk=Zk-HkXk/k-1,式中,Xk/k-1为状态一步预测,残差服从零均值高斯分布,其方差为:
其中,Pk/k-1=E[Xk/k-1Xk/k-1 T]为一步预测协方差矩阵,为量测噪声协方差矩阵;各波束多维残差独立不相关,即rk(i)~N(0,Sk(i,i)),i=1,2,3,4,选取各波束样本数据为ηk(i)=rk(i)rk(i)T/Sk(i,i),(i=1,2,3,4),其总体服从卡方分布,选取阈值ηv,当样本数据小于阈值时认为对应波束速度信息正常,当样本值大于阈值时认为对应波束速度信息存在异常,存在故障情况,执行步骤五;
步骤五、根据不同波束故障情况,重构故障波束速度信息。
2.根据权利要求1所述的一种SINS/DVL紧组合***DVL波束故障处理方法,其特征在于:步骤二所述构建捷联惯性导航***状态参量及状态方程具体为:
选取状态参量为:
n为导航坐标系,与当地地理坐标系重合,x、y、z轴分别指向东、北、天;δP=[δL δλ δh]T为位置误差矢量,δL为纬度位置误差量、δλ为经度位置误量、δh高度位置误差量,δvn=[δvEδvN δvU]T为速度误差矢量,δvE为东向速度误差、δvN为北向速度误差、δvU天向速度误差,φ=[φe φu φu]T为失准角矢量,φe、φn、φu分别为东、北、天三个方向的失准角;b系为载体坐标系,εb=[εx εy εz]T为载体坐标系下陀螺常值漂移矢量,εx、εy、εz分别为x、y、z轴向的陀螺常值漂移,为载体坐标系下加速度计常值偏置矢量,为别为x、y、z轴向的加速度计常值偏置;
根据选取状态参量得到捷联惯性导航***的状态方程为:
其中,状态量上的点代表一阶导,为导航系相对惯性系的旋转角速度在导航系的投影,包括两个矢量部分:其中为地球自转角速度在导航坐标系的投影矢量,为载体在导航系相对地固坐标系的旋转角速度在导航坐标系的投影矢量,ωie为地球自转角速率标量,L为当地纬度,Rh为载体与地球中心的距离,其中Rh=Re+h,Re为地球半径,h为载体所在海拔高度,vn=[vE vN vU]T为载体速度矢量在导航坐标系的投影,vE、vN、vU分别为东向速度、北向速度及天向速度,fb是加速度计输出载体坐标系的比力矢量;
4.根据权利要求1所述的一种SINS/DVL紧组合***DVL波束故障处理方法,其特征在于:步骤五所述根据不同波束故障情况,重构故障波束速度信息具体为:
当单个波束发生故障时,重构故障波束速度信息Vres为:
Vres=Vi+Vj-Vn
其中,i、j表示与故障波束相邻的波束,n为与故障波束构成对角线关系的波束,Vi,Vj,Vn分别表示对应DVL波束的量测速度信息,适用于四波束DVL的“+”和“×”型配置方式,当四波数Janus配置安装角β为0度时,是“+”型配置方式,当四波数Janus配置安装角β为45度时,是“×”型配置方式;
当两个相邻波束速度信息Vn、Vl故障时(n,l∈{[1,2],[2,3],[3,4],[4,1]}),构造故障波束速度信息为:
n、l分别为与i、j呈对角线关系的故障波束,α为由DVL结构决定的固定波束发射角度;
当n、l∈{[1,3],[2,4]}且DVL采用“×”配置时,构造故障波束速度信息为:
当n、l∈{[2,4]}且DVL采用“+”配置时,构造故障波束速度信息为:
当n、l∈{[1,3]}且DVL采用“+”配置时,构造故障波束速度信息为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210279711.1A CN115031724A (zh) | 2022-03-21 | 2022-03-21 | Sins/dvl紧组合***dvl波束故障处理方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210279711.1A CN115031724A (zh) | 2022-03-21 | 2022-03-21 | Sins/dvl紧组合***dvl波束故障处理方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115031724A true CN115031724A (zh) | 2022-09-09 |
Family
ID=83118477
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210279711.1A Pending CN115031724A (zh) | 2022-03-21 | 2022-03-21 | Sins/dvl紧组合***dvl波束故障处理方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115031724A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115792873A (zh) * | 2023-02-14 | 2023-03-14 | 杭州聆巡科技有限公司 | 声呐***和基于声呐***的故障监测方法 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US3414899A (en) * | 1967-07-18 | 1968-12-03 | Gen Precision Systems Inc | Apparatus for calibrating doppler-inertial navigation systems |
JP2000111358A (ja) * | 1998-10-07 | 2000-04-18 | Mitsubishi Electric Corp | ドプラ航法装置 |
US20090238042A1 (en) * | 2008-03-19 | 2009-09-24 | Honeywell International Inc. | Methods and systems for underwater navigation |
CN103674034A (zh) * | 2013-12-26 | 2014-03-26 | 北京控制工程研究所 | 多波束测速测距修正的鲁棒导航方法 |
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN110261814A (zh) * | 2019-03-22 | 2019-09-20 | 中国人民解放***箭军工程大学 | 基于空间谱重构和导向矢量直接估计的波束形成方法 |
CN110487301A (zh) * | 2019-09-18 | 2019-11-22 | 哈尔滨工程大学 | 一种雷达辅助机载捷联惯性导航***初始对准方法 |
CN110567454A (zh) * | 2019-09-08 | 2019-12-13 | 东南大学 | 一种复杂环境下sins/dvl紧组合导航方法 |
CN112507281A (zh) * | 2020-11-19 | 2021-03-16 | 东南大学 | 一种基于双状态多因子抗差估计的sins/dvl紧组合*** |
WO2021170993A1 (en) * | 2020-02-24 | 2021-09-02 | Sonardyne International Limited | Vessel station maintenance control system and method of maintaining station for a vessel |
CN113819906A (zh) * | 2021-09-23 | 2021-12-21 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
-
2022
- 2022-03-21 CN CN202210279711.1A patent/CN115031724A/zh active Pending
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US3414899A (en) * | 1967-07-18 | 1968-12-03 | Gen Precision Systems Inc | Apparatus for calibrating doppler-inertial navigation systems |
JP2000111358A (ja) * | 1998-10-07 | 2000-04-18 | Mitsubishi Electric Corp | ドプラ航法装置 |
US20090238042A1 (en) * | 2008-03-19 | 2009-09-24 | Honeywell International Inc. | Methods and systems for underwater navigation |
CN103674034A (zh) * | 2013-12-26 | 2014-03-26 | 北京控制工程研究所 | 多波束测速测距修正的鲁棒导航方法 |
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN110261814A (zh) * | 2019-03-22 | 2019-09-20 | 中国人民解放***箭军工程大学 | 基于空间谱重构和导向矢量直接估计的波束形成方法 |
CN110567454A (zh) * | 2019-09-08 | 2019-12-13 | 东南大学 | 一种复杂环境下sins/dvl紧组合导航方法 |
CN110487301A (zh) * | 2019-09-18 | 2019-11-22 | 哈尔滨工程大学 | 一种雷达辅助机载捷联惯性导航***初始对准方法 |
WO2021170993A1 (en) * | 2020-02-24 | 2021-09-02 | Sonardyne International Limited | Vessel station maintenance control system and method of maintaining station for a vessel |
CN112507281A (zh) * | 2020-11-19 | 2021-03-16 | 东南大学 | 一种基于双状态多因子抗差估计的sins/dvl紧组合*** |
CN113819906A (zh) * | 2021-09-23 | 2021-12-21 | 哈尔滨工程大学 | 一种基于统计相似度量的组合导航鲁棒滤波方法 |
Non-Patent Citations (1)
Title |
---|
徐博 等: "量测野值与波束故障条件下SINS/DVL紧组合导航方法", 《中国惯性技术学报》 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115792873A (zh) * | 2023-02-14 | 2023-03-14 | 杭州聆巡科技有限公司 | 声呐***和基于声呐***的故障监测方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109324330B (zh) | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 | |
CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
US6496778B1 (en) | Real-time integrated vehicle positioning method and system with differential GPS | |
EP0856747A1 (en) | Method and apparatus for attitude determination utilizing an inertial measurement unit and a plurality of satellite transmitters | |
CN105806339B (zh) | 一种基于gnss、ins和守时***的组合导航方法和设备 | |
US8224574B2 (en) | System for multiple navigation components | |
US20020120400A1 (en) | Fully-coupled vehicle positioning method and system thereof | |
CN109613585A (zh) | 一种对基站天线超短基线gnss双天线实时测向的方法 | |
US8082099B2 (en) | Aircraft navigation using the global positioning system and an attitude and heading reference system | |
CN109471146B (zh) | 一种基于ls-svm的自适应容错gps/ins组合导航方法 | |
CN110793518B (zh) | 一种海上平台的定位定姿方法及*** | |
CN111380518B (zh) | 一种引入径向速度的sins/usbl紧组合导航定位方法 | |
CN106405592B (zh) | 车载北斗载波相位周跳检测与修复方法及*** | |
KR100443550B1 (ko) | 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법 | |
CN115096303B (zh) | 一种gnss多天线与ins紧组合定位定姿方法和设备 | |
CN115327588A (zh) | 一种基于网络rtk的无人自动化作业特种车高精度定位方法 | |
CN111399020A (zh) | 一种定向测姿***及方法 | |
CN114894181A (zh) | 一种实时自主组合导航定位方法及装置 | |
CN113916225A (zh) | 一种基于稳健权因子系数的组合导航粗差抗差估计方法 | |
US7406379B2 (en) | System for interferometric sensing | |
CN115031724A (zh) | Sins/dvl紧组合***dvl波束故障处理方法 | |
CN112697154B (zh) | 一种基于矢量分配的自适应多源融合导航方法 | |
CN112083425B (zh) | 一种引入径向速度的sins/lbl紧组合导航方法 | |
CN117804443A (zh) | 一种北斗卫星多模融合定位监测方法及*** | |
Maklouf et al. | Performance evaluation of GPS\INS main integration approach |
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 | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20220909 |
|
WD01 | Invention patent application deemed withdrawn after publication |