CN111964684A - 一种基于sins/lbl紧组合的水下导航混合定位方法及*** - Google Patents

一种基于sins/lbl紧组合的水下导航混合定位方法及*** Download PDF

Info

Publication number
CN111964684A
CN111964684A CN202010847109.4A CN202010847109A CN111964684A CN 111964684 A CN111964684 A CN 111964684A CN 202010847109 A CN202010847109 A CN 202010847109A CN 111964684 A CN111964684 A CN 111964684A
Authority
CN
China
Prior art keywords
difference
distance
slant
submersible
hydrophone
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
CN202010847109.4A
Other languages
English (en)
Other versions
CN111964684B (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.)
Yuncheng University
Original Assignee
Yuncheng 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 Yuncheng University filed Critical Yuncheng University
Priority to CN202010847109.4A priority Critical patent/CN111964684B/zh
Publication of CN111964684A publication Critical patent/CN111964684A/zh
Application granted granted Critical
Publication of CN111964684B publication Critical patent/CN111964684B/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/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
    • 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
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • 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)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明公开一种基于SINS/LBL紧组合的水下导航混合定位方法及***，针对现有技术中存在的水下导航定位误差收敛速度慢的问题，本发明提供一种基于SINS/LBL紧组合的水下导航混合定位方法及***，基于该方法的***由SINS、LBL、压力传感器和数据处理单元组成，LBL根据可用水听器数量采用TOA和TDOA混合定位方法或直接采用TDOA定位方法辅助SINS紧组合导航，将SINS输出的参数分别与LBL输出的对应参数作差后进行卡尔曼滤波，滤波输出的导航误差估计值反馈修正SINS。本发明方法利用斜距差、斜距率差、斜距差之差、斜距率差之差进行SINS/LBL紧组合，有效提高了水下潜器的导航定位精度。

Description

一种基于SINS/LBL紧组合的水下导航混合定位方法及***
技术领域
本发明属于水下潜器导航定位领域，具体涉及一种基于SINS/LBL紧组合的水下导航混合定位方法及***。
背景技术
海洋占地球表面积的70.8％，蕴藏着丰富的生物、矿产、化学和动力资源，探索和开发海洋成为人类主要的生产活动。水下潜器在科学考察、商业及军事方面均有着广阔的应用前景，水下长航时高精度导航是发展水下潜器必须突破的关键技术之一，也是目前导航技术领域研究的热点和难点问题。
受到水下复杂环境因素的影响，长航时精确导航一直是潜器面临的难题之一，目前在水下导航定位技术方面，潜器主要以多普勒测速仪(DVL：Doppler Velocity Log)辅助捷联惯导***(SINS/DVL：Strapdown Inertial Navigation System/Doppler VelocityLog)为基本导航***，根据基本导航***的航位推算原理，潜器经过长时间航行后，基本导航***输出的位置已经积累较大误差，考虑到安全性和长时间水下作业的实际情况，采用长基线水声定位***(LBL：Long Base Line)，LBL定位***堪称“水下GNSS高精度定位导航***”，具有作用范围广和定位精度高的优点，因而得到广泛研究与应用。
SINS/LBL组合导航类似SINS/GNSS组合导航模式，SINS/LBL松组合导航模式已研究成熟，为直接利用长基线水声定位***获取的斜距信息，张涛等研究了SINS/LBL紧组合导航模式(中国公开专利：一种基于SINS/LBL紧组合的AUV水下导航定位方法，公开号CN104457754B)。该紧组合模式在水听器可用数目为四个时，导航定位效果良好，但当水听器可用数目小于四个时，紧组合导航的定位误差仍较大，不利于潜器的快速定位。
发明内容
技术问题：当潜器进入声学基阵工作区时，针对现有导航技术定位误差收敛速度慢的问题，本发明提出一种基于SINS/LBL紧组合的水下导航混合定位方法及***，它可实现定位误差的快速收敛，进一步提高水下潜器的导航定位精度。
技术方案：本发明的一种基于SINS/LBL紧组合的水下导航混合定位方法，该方法步骤如下：
步骤1，初始化水下导航定位***：获取潜器的初始位置参数，获取潜器的角速率和比力信息，进行捷联惯导***初始对准，获取潜器的初始姿态矩阵和初始速度信息；
步骤2，判断声学基阵工作区水听器的可用个数，当水听器的可用个数大于等于两个时，迭代循环步骤3至步骤5，否则停止迭代，继续判断水听器的可用个数；
步骤3，当水听器的可用个数等于两个时，利用声学基阵和捷联惯导***解算的潜器位置信息，计算出潜器与声学基阵之间的斜距、斜距率、斜距差和斜距率差；
当水听器的可用个数大于两个时，利用声学基阵和捷联惯导***解算的潜器位置信息，计算出潜器与声学基阵之间的斜距差和斜距率差；
步骤4，当水听器的可用个数等于两个时，长基线水声定位***计算出潜器与声学基阵之间的斜距、斜距率、斜距差和斜距率差；
当水听器的可用个数大于两个时，长基线水声定位***计算出潜器与声学基阵之间的斜距差和斜距率差；
步骤5，将所述步骤3中捷联惯导***获取的斜距和斜距率、斜距差和斜距率差以及捷联惯导***解算的潜器深度值分别与所述步骤4中长基线水声定位***获取的斜距和斜距率、斜距差和斜距率差、压力传感器获取的潜器深度值作差后进行卡尔曼滤波，并将滤波后输出的导航误差估计值反馈修正捷联惯导***的导航参数，并输出相应的潜器位置、姿态和速度。
进一步的，所述步骤2中判断声学基阵工作区水听器的可用个数，具体判断方法如下：
通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，当斜距小于声波传播距离时，该水听器为可用水听器。
进一步的，本发明方法中，所述步骤3中捷联惯导***获取斜距和斜距率的具体过程如下：
将解算的潜器位置由球面坐标系转换到直角坐标系为(xI,yI,zI)，计算潜器与声学基阵中各水听器的斜距
Figure BDA0002643418750000021
为：
Figure BDA0002643418750000022
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，
Figure BDA0002643418750000023
为潜器相对于第j号水听器的斜距，其中j为水听器编号；
将斜距在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
Figure BDA0002643418750000031
式中，
Figure BDA0002643418750000032
为第j号水听器到潜器的真实距离，
Figure BDA0002643418750000033
分别为水听器在直角坐标三个轴向的方向余弦，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差，潜器位置真值(x,y,z)由当前捷联惯导***输出的位置(xI,yI,zI)提供；
对斜距求导，得到潜器相对于声学基阵中各水听器的斜距率为：
Figure BDA0002643418750000035
式中，
Figure BDA0002643418750000036
为潜器相对于第j号水听器的斜距率，变量上方·表示变量的导数；
捷联惯导***获取斜距差和斜距率差的具体过程如下：
将解算的潜器位置由球面坐标系转换到直角坐标系为(xI,yI,zI)，计算潜器与声学基阵中各水听器的斜距差
Figure BDA0002643418750000037
为：
Figure BDA0002643418750000038
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，
Figure BDA0002643418750000039
为潜器相对于第j号水听器的斜距与其相对于第k号水听器的斜距之差，其中j、k为水听器编号，且j≠k；
将斜距差在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
Figure BDA00026434187500000310
式中，
Figure BDA00026434187500000311
为第j号水听器到潜器的真实距离，
Figure BDA00026434187500000312
为第k号水听器到潜器的真实距离，
Figure BDA00026434187500000313
分别为水听器在直角坐标三个轴向的方向余弦之差，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差，潜器位置真值(x,y,z)由当前捷联惯导***输出的位置(xI,yI,zI)提供；
对斜距差求导，得到潜器相对于声学基阵中各水听器的斜距率差
Figure BDA0002643418750000041
为：
Figure BDA0002643418750000042
式中，
Figure BDA0002643418750000043
为潜器相对于第j号水听器的斜距率与其相对于第k号水听器的斜距率之差，变量上方·表示变量的导数。
进一步的，本发明方法中，所述步骤4中
长基线水声定位***获取斜距和斜距率的具体过程如下：
潜器与声学基阵中各水听器的斜距
Figure BDA0002643418750000044
为：
Figure BDA0002643418750000045
式中，
Figure BDA0002643418750000046
c为水中声速，τj为潜器上声源到达第j号水听器的时延，δtj为斜距时延误差，υδρ为斜距观测噪声；
对上述斜距求导，得到斜距率
Figure BDA0002643418750000047
为：
Figure BDA0002643418750000048
式中，
Figure BDA0002643418750000049
为声波多普勒频移，λ为声波相位波长，δfj为斜距时延变化率误差，
Figure BDA00026434187500000410
为斜距差观测噪声；
长基线水声定位***获取斜距差和斜距率差的具体过程如下：
潜器与声学基阵中各水听器的斜距差
Figure BDA00026434187500000411
为：
Figure BDA00026434187500000412
式中，
Figure BDA00026434187500000413
c为水中声速，τjk为潜器上声源到达第j号水听器的时延与其到达第k号水听器的时延之差，δtjk为斜距时延差误差，υδρ为斜距差观测噪声；
对上述斜距差求导，得到斜距率差
Figure BDA00026434187500000414
为：
Figure BDA00026434187500000415
式中，
Figure BDA00026434187500000416
为声波多普勒频移差，λ为声波相位波长，δfjk为斜距时延差变化率误差，
Figure BDA00026434187500000417
为斜距率差观测噪声。
进一步的，所述步骤5中作差、滤波及修正的具体步骤如下：
步骤501，分别建立长基线水声定位***和压力传感器状态方程及整个水下导航定位***状态方程：
长基线水声定位***状态参数选取与时间相关的误差δtj和δfj，其状态表示为：
Figure BDA0002643418750000051
式中，δtj为斜距时延误差，δfjk为斜距时延变化率误差，wtj为时延误差驱动噪声，τfj和wfj分别为一阶马尔科夫过程的时延变化率误差相关时间和驱动噪声；
长基线水声定位***状态参数选取与时间相关的误差δtjk和δfjk，其状态表示为：
Figure BDA0002643418750000052
式中，δtjk为斜距时延差误差，δfjk为斜距时延差变化率误差，wδt为时延差误差驱动噪声，τδf和wδf分别为一阶马尔科夫过程的时延差变化率误差相关时间和驱动噪声；
压力传感器深度测量误差δhp状态方程为：
Figure BDA0002643418750000053
式中，τp为深度误差相关时间，wp为深度误差噪声；
整个水下导航定位***的状态方程描述为：
Figure BDA0002643418750000054
式中，XI、XL、XP分别为捷联惯导***、长基线水声定位***和压力传感器的状态变量，FI、FL、FP分别为捷联惯导***、长基线水声定位***和压力传感器的状态转移矩阵，WI、WL、WP分别为捷联惯导***、长基线水声定位***和压力传感器的***噪声；
XI表达式为：
Figure BDA0002643418750000055
式中，φE、φN和φU为“数学平台”失准角，δvE、δvN和δvU分别表示东北天速度误差，δL、δλ和δh分别表示纬度误差、经度误差和深度误差，x、εy和εz为陀螺常值漂移，
Figure BDA0002643418750000056
和
Figure BDA0002643418750000057
为加速度计常值偏置，上标T表示矩阵转置，FI可以由捷联惯导***误差方程得到；
XL表达式为
XL＝[δtj δfj δtjk δfjk]T
Figure BDA0002643418750000061
XP表达式为：
XP＝δhp
Figure BDA0002643418750000062
步骤502，建立整个水下导航定位***观测方程；
斜距差观测方程：
Figure BDA0002643418750000063
Figure BDA0002643418750000064
是将位置误差由球面坐标系转换到直角坐标系的转换矩阵，表达式为：
Figure BDA0002643418750000065
式中，RM表示地球卯酉圈曲率半径，L、λ、h分别表示潜器纬度、经度和深度，e表示地球偏心率；
将转换矩阵
Figure BDA0002643418750000066
代入斜距差观测方程，得斜距差观测方程为：
Zρ＝HρX+Vρ
式中，Zρ＝[δρj]N×1，Hρ＝[0N×6 Hρ1 0N×6 Hρ2 0N×3]，X＝[XI XL XP]T，
Figure BDA0002643418750000067
Hρ2＝[-c 0]N×2，Vρ＝[-υρ]N×1，N为可用水听器个数，此处N＝2；
斜距率差观测方程：
Figure BDA0002643418750000071
Figure BDA0002643418750000072
是速度误差由东北天坐标系转换到直角坐标系的转换矩阵，表达式为：
Figure BDA0002643418750000073
将转换矩阵
Figure BDA0002643418750000074
代入斜距率差观测方程，得斜距率差观测方程为：
Figure BDA0002643418750000075
式中，
Figure BDA0002643418750000076
X＝[XI XL XP]T，
Figure BDA0002643418750000077
斜距差之差观测方程为：
Figure BDA0002643418750000078
将转换矩阵
Figure BDA0002643418750000079
代入斜距差之差观测方程，得斜距差之差观测方程为：
Zδρ＝HδρX+Vδρ
式中，Zδρ＝[δρjk](N-1)×1，Hδρ＝[0(N-1)×6Hδρ10(N-1)×8Hδρ20(N-1)×1]，X＝[XIXLXP]T，
Figure BDA00026434187500000710
Hδρ2＝[-c0](N-1)×2，Vδρ＝[-υδρ](N-1)×1，N≥2；
斜距率差之差观测方程为：
Figure BDA00026434187500000711
将转换矩阵
Figure BDA00026434187500000712
代入斜距率差之差观测方程，得斜距率差之差观测方程为：
Figure BDA00026434187500000713
式中，
Figure BDA00026434187500000714
X＝[XI XL XP]T，
Figure BDA00026434187500000715
深度之差观测方程为：
ZP＝HPX+VP
hI-hp＝(h+δh)-(h+δhp+υp)＝δh-δhp-υp
式中，ZP＝hI-hP，HP＝[01×8 1 01×10 -1]，X＝[XI XL XP]T，VP＝-υP，hI为捷联惯导***解算的潜器深度值，hp为压力传感器测量的潜器深度，h为潜器的深度真值，υp为深度观测噪声；
整个水下导航定位***的观测方程为：
Z＝HX+V
式中，当N＝2时，观测量
Figure BDA0002643418750000081
为捷联惯导***获取的斜距、斜距率、斜距差、斜距率差和潜器深度值分别与长基线水声定位***获取的斜距和斜距率、斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵
Figure BDA0002643418750000082
X＝[XI XL XP]T，观测噪声
Figure BDA0002643418750000083
当N>2时，观测量
Figure BDA0002643418750000084
Zδρ、
Figure BDA0002643418750000085
和ZP分别为捷联惯导***获取的斜距差、斜距率差、潜器深度值与长基线水声定位***获取的斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵
Figure BDA0002643418750000086
X＝[XI XL XP]T，观测噪声
Figure BDA0002643418750000087
步骤503，将得到的斜距差、斜距率差、斜距差之差、斜距率差之差和深度之差进行Kalman滤波，并利用滤波器输出的当前误差最优估计修正捷联惯导***中的状态量
Figure BDA0002643418750000088
位置校正通过捷联惯导***的位置解算值与位置误差估计值相减来修正；
Figure BDA0002643418750000089
式中，Xc是修正后的状态量。
一种基于SINS/LBL紧组合的水下导航混合定位***，所述***包括：
捷联惯导***，用于获取潜器与声学基阵之间基于SINS的斜距、斜距率、斜距差和斜距率差并输出至数据处理单元；
长基线水声定位***，用于判断水听器的可用个数，并获取潜器与声学基阵之间基于LBL的斜距、斜距率、斜距差和斜距率差并输出至数据处理单元；
压力传感器，用于获取潜器的深度值并输出至数据处理单元；
数据处理单元，用于对获取的数据进行权利要求1中步骤5所述的处理。
进一步的，所述捷联惯导***包括惯性测量单元，用于采集潜器的角速率和比力信息，进行捷联惯导***初始对准并获取潜器的初始姿态矩阵和初始速度信息。
进一步的，所述长基线水声定位***包括布放在海底的声学基阵和安装在潜器上的声源；通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，判断水听器的可用个数。
进一步的，所述声学基阵包括至少两个水听器，用于接收声源信号。
进一步的，所述数据处理单元包括卡尔曼滤波器，用于对获取的斜距差、斜距率差、斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波。
有益效果：本发明与现有技术相比，具有以下优点：
针对声学基阵水听器的可用数目不同，本发明选择不同方式的SINS/LBL紧组合定位方法，当有两个水听器接收到声源时，***采用斜距差、斜距率差以及斜距差之差、斜距率差之差的混合定位方法，当有三个以上水听器接收到声源时，***采用斜距差之差和斜距率差之差的TDOA定位方法，结合潜器深度之差构建SINS/LBL紧组合导航观测模型，该方法充分利用各子***提供的信息，相比仅采用单一定位方法的SINS/LBL紧组合导航，本发明所提方法的水下潜器导航定位误差收敛速度快，定位精度有明显提高。
附图说明
图1为本发明的原理框图；
图2为长基线水声定位***示意图；
图3为采用本发明方法后的水平位置定位误差仿真曲线。
具体实施方式
下面结合实施例和说明书附图对本发明作进一步的说明。
实施例1
在潜器进行相关检测的时候，在潜器刚进入声学基阵时，潜器导航可用的水听器数量为两个时，SINS/LBL松组合导航失效，SINS/LBL紧组合导航可将获得的TDOA测量信息融合到载体定位设备中，修正SINS定位误差，但定位误差仍然较大。为充分利用声学基阵提供的信息，当有两个水听器接收到声源时，本发明提出LBL定位***采用到达时间差定位方法和到达时间定位方法的混合定位方法辅助SINS紧组合导航，当有三个以上水听器接收到声源时，LBL定位***采用到达时间差定位方法辅助SINS紧组合导航。当潜器经长航时而累积了较大位置误差时，进入声学基阵工作区，本发明所提方法的导航定位误差收敛速度快、定位精度高，在进一步提高水下潜器组合导航定位精度方面是很有意义的。
如图1所示，一种基于SINS/LBL紧组合的水下导航混合定位方法，该方法步骤如下：
步骤1，初始化水下导航定位***：获取潜器的初始位置参数，获取潜器的角速率和比力信息，进行捷联惯导***初始对准，获取潜器的初始姿态矩阵和初始速度信息；
步骤2，判断声学基阵工作区水听器的可用个数，当水听器的可用个数大于等于两个时，迭代循环步骤3至步骤5，否则停止迭代，继续判断水听器的可用个数；
具体判断方法如下：通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，当斜距小于声波传播距离时，该水听器为可用水听器。
步骤3，当水听器的可用个数等于两个时，利用声学基阵和捷联惯导***解算的潜器位置信息，计算出潜器与声学基阵之间的斜距、斜距率、斜距差和斜距率差；
当水听器的可用个数大于两个时，利用声学基阵和捷联惯导***解算的潜器位置信息，计算出潜器与声学基阵之间的斜距差和斜距率差；
捷联惯导***获取斜距和斜距率的具体过程如下：
将解算的潜器位置由球面坐标系转换到直角坐标系为(xI,yI,zI)，计算潜器与声学基阵中各水听器的斜距
Figure BDA0002643418750000101
为：
Figure BDA0002643418750000102
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，
Figure BDA0002643418750000103
为潜器相对于第j号水听器的斜距，其中j为水听器编号；
将斜距在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
Figure BDA0002643418750000104
式中，
Figure BDA0002643418750000105
为第j号水听器到潜器的真实距离，
Figure BDA0002643418750000106
分别为水听器在直角坐标三个轴向的方向余弦，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差，潜器位置真值(x,y,z)由当前捷联惯导***输出的位置(xI,yI,zI)提供；
对斜距求导，得到潜器相对于声学基阵中各水听器的斜距率
Figure BDA0002643418750000111
为：
Figure BDA0002643418750000112
式中，
Figure BDA0002643418750000113
为潜器相对于第j号水听器的斜距率，变量上方·表示变量的导数；
捷联惯导***获取斜距差和斜距率差的具体过程如下：
将解算的潜器位置由球面坐标系转换到直角坐标系为(xI,yI,zI)，计算潜器与声学基阵中各水听器的斜距差
Figure BDA0002643418750000114
为：
Figure BDA0002643418750000115
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，
Figure BDA0002643418750000116
为潜器相对于第j号水听器的斜距与其相对于第k号水听器的斜距之差，其中j、k为水听器编号，且j≠k；
将斜距差在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
Figure BDA0002643418750000117
式中，
Figure BDA0002643418750000118
为第j号水听器到潜器的真实距离，
Figure BDA0002643418750000119
为第k号水听器到潜器的真实距离，
Figure BDA00026434187500001110
分别为水听器在直角坐标三个轴向的方向余弦之差，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差，潜器位置真值(x,y,z)由当前捷联惯导***输出的位置(xI,yI,zI)提供；
对斜距差求导，得到潜器相对于声学基阵中各水听器的斜距率差
Figure BDA00026434187500001111
为：
Figure BDA00026434187500001112
式中，
Figure BDA00026434187500001113
为潜器相对于第j号水听器的斜距率与其相对于第k号水听器的斜距率之差，变量上方·表示变量的导数。
步骤4，当水听器的可用个数等于两个时，长基线水声定位***计算出潜器与声学基阵之间的斜距、斜距率、斜距差和斜距率差；
当水听器的可用个数大于两个时，长基线水声定位***计算出潜器与声学基阵之间的斜距差和斜距率差；
长基线水声定位***获取斜距和斜距率的具体过程如下：
潜器与声学基阵中各水听器的斜距
Figure BDA0002643418750000121
为：
Figure BDA0002643418750000122
式中，
Figure BDA0002643418750000123
c为水中声速，τj为潜器上声源到达第j号水听器的时延，δtj为斜距时延误差，υδρ为斜距观测噪声；
对上述斜距求导，得到斜距率
Figure BDA0002643418750000124
为：
Figure BDA0002643418750000125
式中，
Figure BDA0002643418750000126
为声波多普勒频移，λ为声波相位波长，δfj为斜距时延变化率误差，
Figure BDA0002643418750000127
为斜距差观测噪声；
长基线水声定位***获取斜距差和斜距率差的具体过程如下：
潜器与声学基阵中各水听器的斜距差
Figure BDA0002643418750000128
为：
Figure BDA0002643418750000129
式中，
Figure BDA00026434187500001210
c为水中声速，τjk为潜器上声源到达第j号水听器的时延与其到达第k号水听器的时延之差，δtjk为斜距时延差误差，υδρ为斜距差观测噪声；
对上述斜距差求导，得到斜距率差
Figure BDA00026434187500001211
为：
Figure BDA00026434187500001212
式中，
Figure BDA00026434187500001213
为声波多普勒频移差，λ为声波相位波长，δfjk为斜距时延差变化率误差，
Figure BDA00026434187500001214
为斜距率差观测噪声。
步骤5，将所述步骤3中捷联惯导***获取的斜距和斜距率、斜距差和斜距率差以及捷联惯导***解算的潜器深度值分别与所述步骤4中长基线水声定位***获取的斜距和斜距率、斜距差和斜距率差、压力传感器获取的潜器深度值作差后进行卡尔曼滤波，并将滤波后输出的导航误差估计值反馈修正捷联惯导***的导航参数，并输出相应的潜器位置、姿态和速度。
作差、滤波及修正的具体步骤如下：
进一步的，所述步骤5中作差、滤波及修正的具体步骤如下：
步骤501，分别建立长基线水声定位***和压力传感器状态方程及整个水下导航定位***状态方程：
长基线水声定位***状态参数选取与时间相关的误差δtj和δfj，其状态表示为：
Figure BDA0002643418750000131
式中，δtj为斜距时延误差，δfjk为斜距时延变化率误差，wtj为时延误差驱动噪声，τfj和wfj分别为一阶马尔科夫过程的时延变化率误差相关时间和驱动噪声；
长基线水声定位***状态参数选取与时间相关的误差δtjk和δfjk，其状态表示为：
Figure BDA0002643418750000132
式中，δtjk为斜距时延差误差，δfjk为斜距时延差变化率误差，wδt为时延差误差驱动噪声，τδf和wδf分别为一阶马尔科夫过程的时延差变化率误差相关时间和驱动噪声；
压力传感器深度测量误差δhp状态方程为：
Figure BDA0002643418750000133
式中，τp为深度误差相关时间，wp为深度误差噪声；
整个水下导航定位***的状态方程描述为：
Figure BDA0002643418750000134
式中，XI、XL、XP分别为捷联惯导***、长基线水声定位***和压力传感器的状态变量，FI、FL、FP分别为捷联惯导***、长基线水声定位***和压力传感器的状态转移矩阵，WI、WL、WP分别为捷联惯导***、长基线水声定位***和压力传感器的***噪声；
XI表达式为：
Figure BDA0002643418750000135
式中，φE、φN和φU为“数学平台”失准角，δvE、δvN和δvU分别表示东北天速度误差，δL、δλ和δh分别表示纬度误差、经度误差和深度误差，εx、εy和εz为陀螺常值漂移，
Figure BDA0002643418750000141
和
Figure BDA0002643418750000142
为加速度计常值偏置，上标T表示矩阵转置，FI可以由捷联惯导***误差方程得到；
XL表达式为
XL＝[δtj δfj δtjk δfjk]T
Figure BDA0002643418750000143
XP表达式为：
XP＝δhp
Figure BDA0002643418750000144
步骤502，建立整个水下导航定位***观测方程；
斜距差观测方程：
Figure BDA0002643418750000145
Figure BDA0002643418750000146
是将位置误差由球面坐标系系转换到直角坐标系的转换矩阵，表达式为：
Figure BDA0002643418750000147
式中，RM表示地球卯酉圈曲率半径，L、λ、h分别表示潜器纬度、经度和深度，e表示地球偏心率；
将转换矩阵
Figure BDA0002643418750000148
代入斜距差观测方程，得斜距差观测方程为：
Zρ＝HρX+Vρ
式中，Zρ＝[δρj]N×1，Hρ＝[0N×6 Hρ1 0N×6 Hρ2 0N×3]，X＝[XI XL XP]T，
Figure BDA0002643418750000151
Hρ2＝[-c 0]N×2，Vρ＝[-υρ]N×1，N为可用水听器个数，此处N＝2；
斜距率差观测方程：
Figure BDA0002643418750000152
Figure BDA0002643418750000153
是速度误差由东北天坐标系转换到直角坐标系的转换矩阵，表达式为：
Figure BDA0002643418750000154
将转换矩阵
Figure BDA0002643418750000155
代入斜距率差观测方程，得斜距率差观测方程为：
Figure BDA0002643418750000156
式中，
Figure BDA0002643418750000157
X＝[XI XL XP]T，
Figure BDA0002643418750000158
斜距差之差观测方程为：
Figure BDA0002643418750000159
将转换矩阵
Figure BDA00026434187500001510
代入斜距差之差观测方程，得斜距差之差观测方程为：
Zδρ＝HδρX+Vδρ
式中，Zδρ＝[δρjk](N-1)×1，Hδρ＝[0(N-1)×6 Hδρ1 0(N-1)×8 Hδρ2 0(N-1)×1]，X＝[XI XL XP]T，
Figure BDA00026434187500001511
Hδρ2＝[-c 0](N-1)×2，Vδρ＝[-υδρ](N-1)×1，N≥2；
斜距率差之差观测方程为：
Figure BDA00026434187500001512
将转换矩阵
Figure BDA00026434187500001513
代入斜距率差之差观测方程，得斜距率差之差观测方程为：
Figure BDA00026434187500001514
式中，
Figure BDA0002643418750000161
X＝[XI XL XP]T，
Figure BDA0002643418750000162
深度之差观测方程为：
ZP＝HPX+VP
hI-hp＝(h+δh)-(h+δhp+υp)＝δh-δhp-υp
式中，ZP＝hI-hP，HP＝[01×8 1 01×8 -1]，X＝[XI XL XP]T，VP＝-υP，hI为捷联惯导***解算的潜器深度值，hp为压力传感器测量的潜器深度，h为潜器的深度真值，υp为深度观测噪声；
整个水下导航定位***的观测方程为：
Z＝HX+V
式中，当N＝2时，观测量
Figure BDA0002643418750000163
为捷联惯导***获取的斜距、斜距率、斜距差、斜距率差和潜器深度值分别与长基线水声定位***获取的斜距和斜距率、斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵
Figure BDA0002643418750000164
X＝[XI XL XP]T，观测噪声
Figure BDA0002643418750000165
当N>2时，观测量
Figure BDA0002643418750000166
Zδρ、
Figure BDA0002643418750000167
和ZP分别为捷联惯导***获取的斜距差、斜距率差、潜器深度值与长基线水声定位***获取的斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵
Figure BDA0002643418750000168
X＝[XI XL XP]T，观测噪声
Figure BDA0002643418750000169
步骤503，将得到的斜距差、斜距率差、斜距差之差、斜距率差之差和深度之差进行Kalman滤波，并利用滤波器输出的当前误差最优估计修正捷联惯导***中的状态量
Figure BDA00026434187500001610
位置校正通过捷联惯导***的位置解算值与位置误差估计值相减来修正；
Figure BDA00026434187500001611
式中，Xc是修正后的状态量。
一种基于SINS/LBL紧组合的水下导航混合定位***，所述***包括：
捷联惯导***，***包括惯性测量单元，用于采集潜器的角速率和比力信息，进行捷联惯导***初始对准并获取潜器的初始姿态矩阵和初始速度信息，还用于获取潜器与声学基阵之间基于SINS的斜距、斜距率、斜距差和斜距率差并输出至数据处理单元；
长基线水声定位***，用于判断水听器的可用个数，并获取潜器与声学基阵之间基于LBL的斜距、斜距率、斜距差和斜距率差并输出至数据处理单元；***包括布放在海底的声学基阵和安装在潜器上的声源，通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，判断水听器的可用个数；
声学基阵包括至少两个水听器，用于接收声源信号，进而获取潜器与声学基阵之间基于LBL的斜距差和斜距率差并输出至数据处理单元；
压力传感器，用于获取潜器的深度值并输出至数据处理单元；
数据处理单元包括卡尔曼滤波器，用于对获取的斜距差、斜距率差、斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波。
本发明的可行性通过如下仿真加以验证：
(1)长基线水声定位***、压力传感器辅助捷联惯导***，构成SINS/LBL紧组合导航***；
(2)陀螺仪常值漂移0.06°/h，随机漂移为0.06°/h，加速度计常值偏置0.1mg，随机漂移0.05mg，初始姿态误差为1.5°，初始速度误差为0.1m/s，初始位置误差为东向10m，北向10m，深度1m，初始航向角为北偏东45°；
(3)长基线水声定位***LBL示意图如图2所示，声学基阵布放在海面以下1000m，相邻水听器间距为东向500m，北向500m，声波传输距离范围2500m，水中声速1500m/s，LBL定位***采用TOA定位方法获取的斜距常值偏置10m，随机漂移10m，斜距率常值偏置5m/s，随机漂移5m/s，采用TDOA定位方法获取的斜距差常值偏置5m，随机漂移5m，斜距率差常值偏置5m/s，随机漂移5m/s；
(4)压力传感器测量深度误差为1m；
(5)惯性传感器数据更新周期为10ms，滤波周期为1s，仿真时间40min；
(6)进入声学基阵工作区之前，潜器以基本导航***为主，经长航时而累积了较大位置误差，水平定位误差约570m，通过水听器为四个的计算机仿真，采用本发明所提混合定位方法后的水平位置定位误差曲线如图3所示。由图3中的曲线对比可见，进入声学基阵工作区后，图中约1300s时刻处，导航定位误差迅速减小，比较可得，采用本发明所提混合定位方法的定位误差收敛速度更快，定位精度高。进一步地，基于该仿真可以推知水听器个数大于四个的结果。
以上示意性地对本发明创造及其实施方式进行了描述，该描述没有限制性，在不背离本发明的精神或者基本特征的情况下，能够以其他的具体形式实现本发明。附图中所示的也只是本发明创造的实施方式之一，实际的结构并不局限于此，权利要求中的任何附图标记不应限制所涉及的权利要求。所以，如果本领域的普通技术人员受其启示，在不脱离本创造宗旨的情况下，不经创造性的设计出与该技术方案相似的结构方式及实施例，均应属于本专利的保护范围。此外，“包括”一词不排除其他元件或步骤，在元件前的“一个”一词不排除包括“多个”该元件。产品权利要求中陈述的多个元件也可以由一个元件通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。

Claims (10)

1.一种基于SINS/LBL紧组合的水下导航混合定位方法及***，其特征在于，该方法步骤如下：
步骤1，初始化水下导航定位***：获取潜器的初始位置参数，获取潜器的角速率和比力信息，进行捷联惯导***初始对准，获取潜器的初始姿态矩阵和初始速度信息；
步骤2，判断声学基阵工作区水听器的可用个数，当水听器的可用个数大于等于两个时，迭代循环步骤3至步骤5，否则停止迭代，继续判断水听器的可用个数；
步骤3，当水听器的可用个数等于两个时，利用声学基阵和捷联惯导***解算的潜器位置信息，计算出潜器与声学基阵之间的斜距、斜距率、斜距差和斜距率差；
当水听器的可用个数大于两个时，利用声学基阵和捷联惯导***解算的潜器位置信息，计算出潜器与声学基阵之间的斜距差和斜距率差；
步骤4，当水听器的可用个数等于两个时，长基线水声定位***计算出潜器与声学基阵之间的斜距、斜距率、斜距差和斜距率差；
当水听器的可用个数大于两个时，长基线水声定位***计算出潜器与声学基阵之间的斜距差和斜距率差；
步骤5，将所述步骤3中捷联惯导***获取的斜距和斜距率、斜距差和斜距率差以及捷联惯导***解算的潜器深度值分别与所述步骤4中长基线水声定位***获取的斜距和斜距率、斜距差和斜距率差、压力传感器获取的潜器深度值作差后进行卡尔曼滤波，并将滤波后输出的导航误差估计值反馈修正捷联惯导***的导航参数，并输出相应的潜器位置、姿态和速度。
2.根据权利要求1所述的一种基于SINS/LBL紧组合的水下导航混合定位方法，其特征在于，所述步骤2中判断声学基阵工作区水听器的可用个数，具体判断方法如下：
通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，当斜距小于声波传播距离时，该水听器为可用水听器。
3.根据权利要求1所述的一种基于SINS/LBL紧组合的水下导航混合定位方法，其特征在于，所述步骤3中捷联惯导***获取斜距和斜距率的具体过程如下：
将解算的潜器位置由球面坐标系转换到直角坐标系为(xI,yI,zI)，计算潜器与声学基阵中各水听器的斜距
Figure FDA0002643418740000011
为：
Figure FDA0002643418740000012
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，
Figure FDA0002643418740000013
为潜器相对于第j号水听器的斜距，其中j为水听器编号；
将斜距在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
Figure FDA0002643418740000021
式中，
Figure FDA0002643418740000022
为第j号水听器到潜器的真实距离，
Figure FDA0002643418740000023
分别为水听器在直角坐标三个轴向的方向余弦，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差；
对斜距求导，得到潜器相对于声学基阵中各水听器的斜距率
Figure FDA0002643418740000024
为：
Figure FDA0002643418740000025
式中，
Figure FDA0002643418740000026
为潜器相对于第j号水听器的斜距率，变量上方·表示变量的导数；
捷联惯导***获取斜距差和斜距率差的具体过程如下：
将解算的潜器位置由球面坐标系转换到直角坐标系为(xI,yI,zI)，计算潜器与声学基阵中各水听器的斜距差
Figure FDA0002643418740000027
为：
Figure FDA0002643418740000028
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，
Figure FDA0002643418740000029
为潜器相对于第j号水听器的斜距与其相对于第k号水听器的斜距之差，其中j、k为水听器编号，且j≠k；
将斜距差在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
Figure FDA00026434187400000210
式中，
Figure FDA00026434187400000211
为第j号水听器到潜器的真实距离，
Figure FDA00026434187400000212
为第k号水听器到潜器的真实距离，
Figure FDA00026434187400000213
分别为水听器在直角坐标三个轴向的方向余弦之差，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差；
对斜距差求导，得到潜器相对于声学基阵中各水听器的斜距率差
Figure FDA0002643418740000031
为：
Figure FDA0002643418740000032
式中，
Figure FDA0002643418740000033
为潜器相对于第j号水听器的斜距率与其相对于第k号水听器的斜距率之差，变量上方·表示变量的导数。
4.根据权利要求1所述的一种基于SINS/LBL紧组合的水下导航混合定位方法，其特征在于，所述步骤4中长基线水声定位***获取斜距和斜距率的具体过程如下：
潜器与声学基阵中各水听器的斜距
Figure FDA0002643418740000034
为：
Figure FDA0002643418740000035
式中，
Figure FDA0002643418740000036
c为水中声速，τj为潜器上声源到达第j号水听器的时延，δtj为斜距时延误差，υρ为斜距观测噪声；
对上述斜距求导，得到斜距率
Figure FDA0002643418740000037
为：
Figure FDA0002643418740000038
式中，
Figure FDA0002643418740000039
Figure FDA00026434187400000310
为声波多普勒频移，λ为声波相位波长，δfj为斜距时延变化率误差，
Figure FDA00026434187400000318
为斜距差观测噪声；
长基线水声定位***获取斜距差和斜距率差的具体过程如下：
潜器与声学基阵中各水听器的斜距差
Figure FDA00026434187400000311
为：
Figure FDA00026434187400000312
式中，
Figure FDA00026434187400000313
c为水中声速，τjk为潜器上声源到达第j号水听器的时延与其到达第k号水听器的时延之差，δtjk为斜距时延差误差，υδρ为斜距差观测噪声；
对上述斜距差求导，得到斜距率差
Figure FDA00026434187400000314
为：
Figure FDA00026434187400000315
式中，
Figure FDA00026434187400000316
Figure FDA00026434187400000317
为声波多普勒频移差，λ为声波相位波长，δfjk为斜距时延差变化率误差，
Figure FDA00026434187400000319
为斜距率差观测噪声。
5.根据权利要求1所述的一种基于SINS/LBL紧组合的水下导航混合定位方法，其特征在于，所述步骤5中作差、滤波及修正的具体步骤如下：
步骤501，分别建立长基线水声定位***和压力传感器状态方程及整个水下导航定位***状态方程：
长基线水声定位***状态参数选取与时间相关的误差δtj和δfj，其状态表示为：
Figure FDA0002643418740000041
式中，δtj为斜距时延误差，δfjk为斜距时延变化率误差，wtj为时延误差驱动噪声，τfj和wfj分别为一阶马尔科夫过程的时延变化率误差相关时间和驱动噪声；
长基线水声定位***状态参数选取与时间相关的误差δtjk和δfjk，其状态表示为：
Figure FDA0002643418740000042
式中，δtjk为斜距时延差误差，δfjk为斜距时延差变化率误差，wδt为时延差误差驱动噪声，τδf和wδf分别为一阶马尔科夫过程的时延差变化率误差相关时间和驱动噪声；
压力传感器深度测量误差δhp状态方程为：
Figure FDA0002643418740000043
式中，τp为深度误差相关时间，wp为深度误差噪声；
整个水下导航定位***的状态方程描述为：
Figure FDA0002643418740000044
式中，XI、XL、XP分别为捷联惯导***、长基线水声定位***和压力传感器的状态变量，FI、FL、FP分别为捷联惯导***、长基线水声定位***和压力传感器的状态转移矩阵，WI、WL、WP分别为捷联惯导***、长基线水声定位***和压力传感器的***噪声；
XI表达式为：
Figure FDA0002643418740000045
式中，φE、φN和φU为“数学平台”失准角，δvE、δvN和δvU分别表示东北天速度误差，δL、δλ和δh分别表示纬度误差、经度误差和深度误差，x、εy和εz为陀螺常值漂移，
Figure FDA0002643418740000051
和
Figure FDA0002643418740000052
为加速度计常值偏置，上标T表示矩阵转置，FI可以由捷联惯导***误差方程得到；
XL表达式为
XL＝[δtj δfj δtjk δfjk]T
Figure FDA0002643418740000053
XP表达式为：
XP＝δhp
Figure FDA0002643418740000054
步骤502，建立整个水下导航定位***观测方程；
斜距差观测方程：
Figure FDA0002643418740000055
Figure FDA0002643418740000056
是将位置误差由球面坐标系转换到直角坐标系的转换矩阵，表达式为：
Figure FDA0002643418740000057
式中，RM表示地球卯酉圈曲率半径，L、λ、h分别表示潜器纬度、经度和深度，e表示地球偏心率；
将转换矩阵
Figure FDA0002643418740000058
代入斜距差观测方程，得斜距差观测方程为：
Zρ＝HρX+Vρ
式中，Zρ＝[δρj]N×1，Hρ＝[0N×6 Hρ1 0N×6 Hρ2 0N×3]，X＝[XI XL XP]T，
Figure FDA0002643418740000061
Hρ2＝[-c 0]N×2，Vρ＝[-υρ]N×1，N为可用水听器个数，此处N＝2；
斜距率差观测方程：
Figure FDA0002643418740000062
Figure FDA0002643418740000063
是速度误差由东北天坐标系转换到直角坐标系的转换矩阵，表达式为：
Figure FDA0002643418740000064
将转换矩阵
Figure FDA0002643418740000065
代入斜距率差观测方程，得斜距率差观测方程为：
Figure FDA00026434187400000615
式中，
Figure FDA0002643418740000066
X＝[XI XL XP]T，
Figure FDA0002643418740000067
斜距差之差观测方程为：
Figure FDA0002643418740000068
将转换矩阵
Figure FDA0002643418740000069
代入斜距差之差观测方程，得斜距差之差观测方程为：
Zδρ＝HδρX+Vδρ
式中，Zδρ＝[δρjk](N-1)×1，Hδρ＝[0(N-1)×6 Hδρ1 0(N-1)×8 Hδρ2 0(N-1)×1]，X＝[XI XL XP]T，
Figure FDA00026434187400000610
Hδρ2＝[-c 0](N-1)×2，Vδρ＝[-υδρ](N-1)×1，N≥2；
斜距率差之差观测方程为：
Figure FDA00026434187400000611
将转换矩阵
Figure FDA00026434187400000612
代入斜距率差之差观测方程，得斜距率差之差观测方程为：
Figure FDA00026434187400000613
式中，
Figure FDA00026434187400000614
X＝[XI XL XP]T，
Figure FDA0002643418740000071
深度之差观测方程为：
ZP＝HPX+VP
hI-hp＝(h+δh)-(h+δhp+υp)＝δh-δhp-υp
式中，ZP＝hI-hP，HP＝[01×8 1 01×10 -1]，X＝[XI XL XP]T，VP＝-υP，hI为捷联惯导***解算的潜器深度值，hp为压力传感器测量的潜器深度，h为潜器的深度真值，υp为深度观测噪声；
整个水下导航定位***的观测方程为：
Z＝HX+V
式中，当N＝2时，观测量
Figure FDA0002643418740000072
为捷联惯导***获取的斜距、斜距率、斜距差、斜距率差和潜器深度值分别与长基线水声定位***获取的斜距和斜距率、斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵
Figure FDA0002643418740000073
X＝[XI XL XP]T，观测噪声
Figure FDA0002643418740000074
当N>2时，观测量
Figure FDA0002643418740000075
Zδρ、
Figure FDA0002643418740000076
和ZP分别为捷联惯导***获取的斜距差、斜距率差、潜器深度值与长基线水声定位***获取的斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵
Figure FDA0002643418740000077
X＝[XI XL XP]T，观测噪声
Figure FDA0002643418740000078
步骤503，将得到的斜距差、斜距率差、斜距差之差、斜距率差之差和深度之差进行Kalman滤波，并利用滤波器输出的当前误差最优估计修正捷联惯导***中的状态量
Figure FDA0002643418740000079
位置校正通过捷联惯导***的位置解算值与位置误差估计值相减来修正；
Figure FDA00026434187400000710
式中，Xc是修正后的状态量。
6.一种基于SINS/LBL紧组合的水下导航混合定位***，其特征在于，所述***包括：
捷联惯导***，用于获取潜器与声学基阵之间基于SINS的斜距、斜距率、斜距差和斜距率差并输出至数据处理单元；
长基线水声定位***，用于判断水听器的可用个数，并获取潜器与声学基阵之间基于LBL的斜距、斜距率、斜距差和斜距率差并输出至数据处理单元；
压力传感器，用于获取潜器的深度值并输出至数据处理单元；
数据处理单元，用于对获取的数据进行权利要求1中步骤5所述的处理。
7.根据权利要求6所述的一种基于SINS/LBL紧组合的水下导航混合定位***，其特征在于，所述捷联惯导***包括惯性测量单元，用于采集潜器的角速率和比力信息，进行捷联惯导***初始对准并获取潜器的初始姿态矩阵和初始速度信息。
8.根据权利要求6所述的一种基于SINS/LBL紧组合的水下导航混合定位***，其特征在于，所述长基线水声定位***包括布放在海底的声学基阵和安装在潜器上的声源；通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，判断水听器的可用个数。
9.根据权利要求8所述的一种基于SINS/LBL紧组合的水下导航混合定位***，其特征在于，所述声学基阵包括至少两个水听器，用于接收声源信号。
10.根据权利要求6所述的一种基于SINS/LBL紧组合的水下导航混合定位***，其特征在于，所述数据处理单元包括卡尔曼滤波器，用于对获取的斜距差、斜距率差、斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波。
CN202010847109.4A 2020-08-21 2020-08-21 一种基于sins/lbl紧组合的水下导航混合定位方法及*** Active CN111964684B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010847109.4A CN111964684B (zh) 2020-08-21 2020-08-21 一种基于sins/lbl紧组合的水下导航混合定位方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010847109.4A CN111964684B (zh) 2020-08-21 2020-08-21 一种基于sins/lbl紧组合的水下导航混合定位方法及***

Publications (2)

Publication Number Publication Date
CN111964684A true CN111964684A (zh) 2020-11-20
CN111964684B CN111964684B (zh) 2023-11-17

Family

ID=73389879

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010847109.4A Active CN111964684B (zh) 2020-08-21 2020-08-21 一种基于sins/lbl紧组合的水下导航混合定位方法及***

Country Status (1)

Country Link
CN (1) CN111964684B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113280812A (zh) * 2021-05-12 2021-08-20 中山大学 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2456634C1 (ru) * 2011-03-16 2012-07-20 Юрий Николаевич Жуков Способ навигации подводного объекта посредством гидроакустической навигационной системы
CN106767793A (zh) * 2017-01-19 2017-05-31 东南大学 一种基于sins/usbl紧组合的auv水下导航定位方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN110567454A (zh) * 2019-09-08 2019-12-13 东南大学 一种复杂环境下sins/dvl紧组合导航方法
CN111380518A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl紧组合导航定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2456634C1 (ru) * 2011-03-16 2012-07-20 Юрий Николаевич Жуков Способ навигации подводного объекта посредством гидроакустической навигационной системы
CN106767793A (zh) * 2017-01-19 2017-05-31 东南大学 一种基于sins/usbl紧组合的auv水下导航定位方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN110567454A (zh) * 2019-09-08 2019-12-13 东南大学 一种复杂环境下sins/dvl紧组合导航方法
CN111380518A (zh) * 2020-03-04 2020-07-07 哈尔滨工程大学 一种引入径向速度的sins/usbl紧组合导航定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
TAO ZHANG等: "Underwater Positioning Algorithm Based on SINS/LBL Integrated System", IEEE ACCESS *
张涛;石宏飞;徐晓苏;: "基于SINS/LBL紧组合的AUV水下导航定位技术", 中国惯性技术学报, no. 04 *
徐博;郝芮;王超;张勋;: "基于倒置声学基阵的INSUSBL组合导航算法研究", 海洋技术学报, no. 05 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113280812A (zh) * 2021-05-12 2021-08-20 中山大学 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置
CN113280812B (zh) * 2021-05-12 2023-06-02 中山大学 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置

Also Published As

Publication number Publication date
CN111964684B (zh) 2023-11-17

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN109443379B (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN106767793A (zh) 一种基于sins/usbl紧组合的auv水下导航定位方法
CN101900558B (zh) 集成声纳微导航的自主式水下机器人组合导航方法
CN104316045A (zh) 一种基于sins/lbl的auv水下交互辅助定位***及定位方法
Chen et al. Review of AUV underwater terrain matching navigation
CN111982105B (zh) 一种基于sins/lbl紧组合的水下导航定位方法及***
CN106679662B (zh) 一种基于tma技术的水下机器人单信标组合导航方法
CN104180804A (zh) 基于水下信息网络的单参考节点水下航行器组合导航方法
CN105486313A (zh) 一种基于usbl辅助低成本sins***的定位方法
CN110274591B (zh) 深潜载人潜水器adcp辅助sins导航方法
RU2563332C2 (ru) Способ навигации автономного необитаемого подводного аппарата
CN104197939A (zh) 基于水下信息网络的多参考节点水下航行器组合导航方法
Yona et al. Compensating for partial Doppler velocity log outages by using deep-learning approaches
Kinsey et al. Towards in-situ calibration of gyro and doppler navigation sensors for precision underwater vehicle navigation
CN104280024B (zh) 一种深水机器人组合导航装置和方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
Chen In-situ alignment calibration of attitude and ultra short baseline sensors for precision underwater positioning
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法
CN111735455A (zh) 基于改进的高斯距离迭代算法对接回收组合导航方法
Liu et al. SINS/DVL integrated system with current and misalignment estimation for midwater navigation
CN113155134B (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
Allotta et al. Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements
CN111964684A (zh) 一种基于sins/lbl紧组合的水下导航混合定位方法及***

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