CN111982105B - 一种基于sins/lbl紧组合的水下导航定位方法及系统 - Google Patents

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

Info

Publication number
CN111982105B
CN111982105B CN202010847141.2A CN202010847141A CN111982105B CN 111982105 B CN111982105 B CN 111982105B CN 202010847141 A CN202010847141 A CN 202010847141A CN 111982105 B CN111982105 B CN 111982105B
Authority
CN
China
Prior art keywords
difference
submersible
pitch
error
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.)
Active
Application number
CN202010847141.2A
Other languages
English (en)
Other versions
CN111982105A (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 CN202010847141.2A priority Critical patent/CN111982105B/zh
Publication of CN111982105A publication Critical patent/CN111982105A/zh
Application granted granted Critical
Publication of CN111982105B publication Critical patent/CN111982105B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
    • G01L19/00—Details of, or accessories for, apparatus for measuring steady or quasi-steady pressure of a fluent medium insofar as such details or accessories are not special to particular types of pressure gauges
    • 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/24—Position of single direction-finder fixed by determining direction of a plurality of spaced sources of known location
    • 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紧组合的水下导航定位方法，基于该方法的系统由捷联惯导系统、长基线水声定位系统、压力传感器和数据处理单元组成，将捷联惯导系统输出的斜距差、斜距率差和深度值分别与长基线水声定位系统输出的斜距差和斜距率差、压力传感器测量的潜器深度作差后进行卡尔曼滤波，滤波输出的导航误差估计值反馈修正捷联惯导系统。本发明方法利用斜距差之差、斜距率差之差进行捷联惯导系统/长基线水声定位系统紧组合，有效提高了水下潜器的导航定位精度。

Description

一种基于SINS/LBL紧组合的水下导航定位方法及系统
技术领域
本发明属于水下潜器导航定位领域，具体涉及一种基于SINS/LBL紧组合的水下导航定位方法及系统。
背景技术
随着海洋资源开发和海洋能源的利用，现代海洋高新技术已成为新技术领域的研究热点。水下潜器是完成水下勘探、侦测的主要载体，水下导航和定位是潜器正常工作的前提，长航时高精度是潜器导航的重要指标。
受到水下复杂环境因素的影响，长航时精确导航一直是潜器面临的难题之一，目前在水下导航定位技术方面，潜器主要以多普勒测速仪(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)，计算潜器与声学基阵中各水听器的斜距差为：
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，该坐标可通过阵形校准方法获取，为潜器相对于第j号水听器的斜距与其相对于第k号水听器的斜距之差，其中j、k为水听器编号，且j≠k；
将斜距差在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
式中，为第j号水听器到潜器的真实距离，为第k号水听器到潜器的真实距离，分别为水听器在直角坐标三个轴向的方向余弦之差，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差，潜器位置真值(x,y,z)由当前捷联惯导系统输出的位置(xI,yI,zI)提供；
对斜距差求导，得到潜器相对于声学基阵中各水听器的斜距率差为：
式中，为潜器相对于第j号水听器的斜距率与其相对于第k号水听器的斜距率之差，变量上方·表示变量的导数。
进一步的，本发明方法中，步骤4中长基线水声定位系统获取斜距差和斜距率差的具体过程如下：
潜器与声学基阵中各水听器的斜距差为：
式中，c为水中声速，τjk为潜器上声源到达第j号水听器的时延与其到达第k号水听器的时延之差，δtjk为斜距时延差误差，υδρ为斜距差观测噪声；
对上述斜距差求导，得到斜距率差为：
式中，为声波多普勒频移差，λ为声波相位波长，δfjk为斜距时延差变化率误差，/>为斜距率差观测噪声。
进一步的，所述步骤5中作差、滤波及修正的具体步骤如下：
步骤501，分别建立长基线水声定位系统和压力传感器状态方程及整个水下导航定位系统状态方程；
长基线水声定位系统状态参数选取与时间相关的两个误差δtjk和δfjk，其状态表示为：
式中，δtjk为斜距时延差误差，δfjk为斜距时延差变化率误差，wδt为驱动噪声，τδf和wδf分别为一阶马尔科夫过程的相关时间和驱动噪声；
压力传感器深度测量误差δhp状态方程为：
式中，τp为深度误差相关时间，wp为深度误差噪声；
整个水下导航定位系统的状态方程描述为：
式中，XI、XL、XP分别为捷联惯导系统、长基线水声定位系统和压力传感器的状态变量，FI、FL、FP分别为捷联惯导系统、长基线水声定位系统和压力传感器的状态转移矩阵，WI、WL、WP分别为捷联惯导系统、长基线水声定位系统和压力传感器的系统噪声；XI表达式为：
式中，φE、φN和φU为“数学平台”失准角，δvE、δvN和δvU分别表示东北天速度误差，δL、δλ和δh分别表示纬度误差、经度误差和深度误差，εx、εy和εz为陀螺常值漂移，和为加速度计常值偏置，上标T表示矩阵转置，FI可以由捷联惯导系统误差方程得到；
XL表达式为：
XL＝[δtjk δfjk]T
XP表达式为：
XP＝δhp
步骤502，建立整个水下导航定位系统观测方程；
斜距差之差观测方程为：
是将位置误差由球面坐标系转换到直角坐标系的转换矩阵，表达式为：
式中，RM表示地球卯酉圈曲率半径，L、λ、h分别表示潜器纬度、经度和深度，e表示地球偏心率；
将转换矩阵代入斜距差之差观测方程，得斜距差之差观测方程为：
Zδρ＝HδρX+Vδρ
式中，Zδρ＝[δρjk](N-1)×1，Hδρ＝[0(N-1)×6 Hδρ1 0(N-1)×6 Hδρ2 0(N-1)×1]，X＝[XI XL XP]T，Hδρ2＝[-c 0](N-1)×2，Vδρ＝[-υδρ](N-1)×1，N为潜器进入声学基阵工作区水听器的可用个数，N>2；
斜距率差之差观测方程为：
是速度误差由东北天坐标系转换到直角坐标系的转换矩阵，表达式为：
将转换矩阵代入斜距率差之差观测方程，得斜距率差之差观测方程为：
式中，X＝[XI XL XP]T，
深度之差观测方程为：
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
式中，观测量Zδρ、/>和ZP分别为捷联惯导系统获取的斜距差、斜距率差、潜器深度值与长基线水声定位系统获取的斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵/>X＝[XI XL XP]T，观测噪声/>
步骤503，将上述得到的斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波，并利用滤波器输出的当前误差最优估计修正捷联惯导系统中的状态量位置校正通过捷联惯导系统的位置解算值与位置误差估计值相减来修正；
式中，Xc是修正后的状态量。
一种基于SINS/LBL紧组合的水下导航定位系统，所述系统包括：
捷联惯导系统，用于获取潜器与声学基阵之间基于SINS的斜距差和斜距率差并输出至数据处理单元；
长基线水声定位系统，用于判断水听器的可用个数，并获取潜器与声学基阵之间基于LBL的斜距差和斜距率差并输出至数据处理单元；
压力传感器，用于获取潜器的深度值并输出至数据处理单元；
数据处理单元，用于对获取的数据进行权利要求1中步骤5所述的处理。
进一步的，所述捷联惯导系统包括惯性测量单元，用于采集潜器的角速率和比力信息，进行捷联惯导系统初始对准并获取潜器的初始姿态矩阵和初始速度信息。
进一步的，所述长基线水声定位系统包括布放在海底的声学基阵和安装在潜器上的声源；通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，判断水听器的可用个数。
进一步的，所述声学基阵包括至少三个水听器，用于接收声源信号。
进一步的，所述数据处理单元包括卡尔曼滤波器，用于对获取的斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波。
有益效果：本发明与现有技术相比，具有以下优点：
本发明通过捷联惯导系统获取的斜距差、斜距率差和深度值分别与长基线水声定位系统获取的斜距差和斜距率差、压力传感器获取的潜器深度值作差构建SINS/LBL紧组合导航观测模型，该方法充分利用各子系统提供的信息，当潜器经长航时而累积了较大位置误差时，进入声学基阵工作区，相比仅采用斜距差作为观测量的SINS/LBL紧组合导航，本发明所提方法的水下潜器导航定位误差收敛速度有明显提高。
附图说明
图1为本发明的原理框图；
图2为长基线水声定位系统LBL示意图；
图3为采用本发明方法后的水平位置定位误差仿真曲线。
具体实施方式
下面结合实施例和说明书附图对本发明作进一步的说明。
实施例1
如图1所示，一种基于SINS/LBL紧组合的水下导航定位方法，该方法步骤如下：
步骤1，初始化水下导航定位系统：获取潜器的初始位置参数，获取潜器的角速率和比力信息，进行捷联惯导系统初始对准，获取潜器的初始姿态矩阵和初始速度信息；
步骤2，判断声学基阵工作区水听器的可用个数，当水听器的可用个数大于两个时，迭代循环步骤3至步骤5，否则停止迭代，继续判断水听器的可用个数，具体判断方法如下：通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，当斜距小于声波传播距离时，该水听器为可用水听器。
步骤3，利用声学基阵和捷联惯导系统解算的潜器位置信息，计算出潜器与声学基阵之间的斜距差和斜距率差；
捷联惯导系统获取斜距差和斜距率差的具体过程如下：
将解算的潜器位置由球面坐标系转换到直角坐标系为(xI,yI,zI)，计算潜器与声学基阵中各水听器的斜距差为：
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，该坐标可通过阵形校准方法获取，为潜器相对于第j号水听器的斜距与其相对于第k号水听器的斜距之差，其中j、k为水听器编号，且j≠k；
将斜距差在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
式中，为第j号水听器到的真实距离，为第k号水听器到潜器的真实距离，分别为水听器在直角坐标三个轴向的方向余弦之差，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差，潜器位置真值(x,y,z)由当前捷联惯导系统输出的位置(xI,yI,zI)提供；
对斜距差求导，得到潜器相对于声学基阵中各水听器的斜距率差为：
式中，为潜器相对于第j号水听器的斜距率与其相对于第k号水听器的斜距率之差，变量上方·表示变量的导数。
步骤4，利用长基线水声定位系统计算出潜器与声学基阵之间的斜距差和斜距率差；
长基线水声定位系统获取斜距差和斜距率差的具体过程如下：
潜器与声学基阵中各水听器的斜距差为：
式中，c为水中声速，τjk为潜器上声源到达第j号水听器的时延与其到达第k号水听器的时延之差，δtjk为斜距时延差误差，υδρ为斜距差观测噪声；
对上述斜距差求导，得到斜距率差为：
式中，为声波多普勒频移差，λ为声波相位波长，δfjk为斜距时延差变化率误差，/>为斜距率差观测噪声。
步骤5，将所述步骤3中捷联惯导系统获取的斜距差和斜距率差以及捷联惯导系统解算的潜器深度值分别与所述步骤4中长基线水声定位系统获取的斜距差和斜距率差、压力传感器获取的潜器深度值作差后进行卡尔曼滤波，并将滤波后输出的导航误差估计值反馈修正捷联惯导系统的导航参数，并输出相应的潜器位置、姿态和速度。
作差、滤波及修正的具体步骤如下：
步骤501，分别建立长基线水声定位系统和压力传感器状态方程及整个水下导航定位系统状态方程：
长基线水声定位系统状态参数选取与时间相关的两个误差δtjk和δfjk，其状态表示为：
式中，δtjk为斜距时延差误差，δfjk为斜距时延差变化率误差，wδt为驱动噪声，τδf和wδf分别为一阶马尔科夫过程的相关时间和驱动噪声；
压力传感器深度测量误差δhp状态方程为：
式中，τp为深度误差相关时间，wp为深度误差噪声；
整个水下导航定位系统的状态方程描述为：
式中，XI、XL、XP分别为捷联惯导系统、长基线水声定位系统和压力传感器的状态变量，FI、FL、FP分别为捷联惯导系统、长基线水声定位系统和压力传感器的状态转移矩阵，WI、WL、WP分别为捷联惯导系统、长基线水声定位系统和压力传感器的系统噪声；XI表达式为：
式中，φE、φN和φU为“数学平台”失准角，δvE、δvN和δvU分别表示东北天速度误差，δL、δλ和δh分别表示纬度误差、经度误差和深度误差，εx、εy和εz为陀螺常值漂移，和为加速度计常值偏置，上标T表示矩阵转置，FI可以由捷联惯导系统误差方程得到；
XL表达式为：
XL＝[δtjk δfjk]T
XP表达式为：
XP＝δhp
步骤502，建立整个水下导航定位系统观测方程；
斜距差之差观测方程为：
是将位置误差由球面坐标系转换到直角坐标系的转换矩阵，表达式为：
式中，RM表示地球卯酉圈曲率半径，L、λ、h分别表示潜器纬度、经度和深度，e表示地球偏心率；
将转换矩阵代入斜距差之差观测方程，得斜距差之差观测方程为：
Zδρ＝HδρX+Vδρ
式中，Zδρ＝[δρjk](N-1)×1，Hδρ＝[0(N-1)×6 Hδρ1 0(N-1)×6 Hδρ2 0(N-1)×1]，X＝[XI XL XP]T，Hδρ2＝[-c 0](N-1)×2，Vδρ＝[-υδρ](N-1)×1，N为潜器进入声学基阵工作区水听器的可用个数，N>2；
斜距率差之差观测方程为：
是速度误差由东北天坐标系转换到直角坐标系的转换矩阵，表达式为：
将转换矩阵代入斜距率差之差观测方程，得斜距率差之差观测方程为：
式中，X＝[XI XL XP]T，
深度之差观测方程为：
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
式中，观测量Zδρ、/>和ZP分别为捷联惯导系统获取的斜距差、斜距率差、潜器深度值与长基线水声定位系统获取的斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵/>X＝[XI XL XP]T，观测噪声
步骤503，将上述得到的斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波，并利用滤波器输出的当前误差最优估计修正捷联惯导系统中的状态量位置校正通过捷联惯导系统的位置解算值与位置误差估计值相减来修正；
式中，Xc是修正后的状态量。
一种基于SINS/LBL紧组合的水下导航定位系统，所述系统包括：
捷联惯导系统，系统包括惯性测量单元，用于采集潜器的角速率和比力信息，进行捷联惯导系统初始对准并获取潜器的初始姿态矩阵和初始速度信息，还用于获取潜器与声学基阵之间基于SINS的斜距差和斜距率差并输出至数据处理单元；
长基线水声定位系统，系统包括布放在海底的声学基阵和安装在潜器上的声源，通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，判断水听器的可用个数；
声学基阵包括至少三个水听器，用于接收声源信号，进而获取潜器与声学基阵之间基于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获取的斜距差常值偏置5m，随机漂移5m，斜距率差常值偏置5m/s，随机漂移5m/s；
(4)压力传感器测量深度误差为1m；
(5)惯性传感器数据更新周期为10ms，滤波周期为1s，仿真时间40min；
(6)进入声学基阵工作区之前，潜器以基本导航系统为主，经长航时而累积了较大位置误差，水平定位误差约860m，通过水听器为四个的计算机仿真，采用本发明所提方法的水平位置定位误差曲线如图3所示。由图3中的对比可见，进入声学基阵工作区后，图中约2000s时刻处，导航定位误差迅速减小，比较可得，采用本发明所提方法的位置误差收敛速度更快，定位精度高。进一步地，基于该仿真可以推知水听器个数大于四个的结果。
以上示意性地对本发明创造及其实施方式进行了描述，该描述没有限制性，在不背离本发明的精神或者基本特征的情况下，能够以其他的具体形式实现本发明。附图中所示的也只是本发明创造的实施方式之一，实际的结构并不局限于此，权利要求中的任何附图标记不应限制所涉及的权利要求。所以，如果本领域的普通技术人员受其启示，在不脱离本创造宗旨的情况下，不经创造性的设计出与该技术方案相似的结构方式及实施例，均应属于本专利的保护范围。此外，“包括”一词不排除其他元件或步骤，在元件前的“一个”一词不排除包括“多个”该元件。产品权利要求中陈述的多个元件也可以由一个元件通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。

Claims (7)

1.一种基于SINS/LBL紧组合的水下导航定位方法，其特征在于，该方法步骤如下：
步骤1，初始化水下导航定位系统：获取潜器的初始位置参数，获取潜器的角速率和比力信息，进行捷联惯导系统初始对准，获取潜器的初始姿态矩阵和初始速度信息；
步骤2，判断声学基阵工作区水听器的可用个数，当水听器的可用个数大于两个时，迭代循环步骤3至步骤5，否则停止迭代，继续判断水听器的可用个数；
步骤3，利用声学基阵和捷联惯导系统解算的潜器位置信息，计算出潜器与声学基阵之间的斜距差和斜距率差；捷联惯导系统获取斜距差和斜距率差的具体过程如下：
将解算的潜器位置由球面坐标系转换到直角坐标系为(xI,yI,zI)，计算潜器与声学基阵中各水听器的斜距差为：
式中，(xa,ya,za)为组成声学基阵的水听器位置坐标，为潜器相对于第j号水听器的斜距与其相对于第k号水听器的斜距之差，其中j、k为水听器编号，且j≠k；
将斜距差在潜器位置真值(x,y,z)处泰勒级数展开，取其一次项得：
式中，为第j号水听器到潜器的真实距离，为第k号水听器到潜器的真实距离，分别为水听器在直角坐标三个轴向的方向余弦之差，δx＝xI-x，δy＝yI-y，δz＝zI-z分别为直角坐标三个轴向的位置误差；
对斜距差求导，得到潜器相对于声学基阵中各水听器的斜距率差为：
式中，为潜器相对于第j号水听器的斜距率与其相对于第k号水听器的斜距率之差，变量上方·表示变量的导数；
步骤4，利用长基线水声定位系统计算出潜器与声学基阵之间的斜距差和斜距率差；长基线水声定位系统获取斜距差和斜距率差的具体过程如下：
潜器与声学基阵中各水听器的斜距差为：
式中，c为水中声速，τjk为潜器上声源到达第j号水听器的时延与其到达第k号水听器的时延之差，δtjk为斜距时延差误差，υδρ为斜距差观测噪声；
对上述斜距差求导，得到斜距率差为：
式中， 为声波多普勒频移差，λ为声波相位波长，δfjk为斜距时延差变化率误差，/>为斜距率差观测噪声；
步骤5，将所述步骤3中捷联惯导系统获取的斜距差和斜距率差以及捷联惯导系统解算的潜器深度值分别与所述步骤4中长基线水声定位系统获取的斜距差和斜距率差、压力传感器获取的潜器深度值作差后进行卡尔曼滤波，并将滤波后输出的导航误差估计值反馈修正捷联惯导系统的导航参数，并输出相应的潜器位置、姿态和速度；
作差、滤波及修正的具体步骤如下：
步骤501，分别建立长基线水声定位系统和压力传感器状态方程及整个水下导航定位系统状态方程：
长基线水声定位系统状态参数选取与时间相关的两个误差δtjk和δfjk，其状态表示为：
式中，δtjk为斜距时延差误差，δfjk为斜距时延差变化率误差，wδt为驱动噪声，τδf和wδf分别为一阶马尔科夫过程的相关时间和驱动噪声；
压力传感器深度测量误差δhp状态方程为：
式中，τp为深度误差相关时间，wp为深度误差噪声；
整个水下导航定位系统的状态方程描述为：
式中，XI、XL、XP分别为捷联惯导系统、长基线水声定位系统和压力传感器的状态变量，FI、FL、FP分别为捷联惯导系统、长基线水声定位系统和压力传感器的状态转移矩阵，WI、WL、WP分别为捷联惯导系统、长基线水声定位系统和压力传感器的系统噪声；
XI表达式为：
式中，φE、φN和φU为“数学平台”失准角，δvE、δvN和δvU分别表示东北天速度误差，δL、δλ和δh分别表示纬度误差、经度误差和深度误差，εx、εy和εz为陀螺常值漂移，和/>为加速度计常值偏置，上标T表示矩阵转置，FI可以由捷联惯导系统误差方程得到；
XL表达式为：
XL＝[δtjkδfjk]T
XP表达式为：
XP＝δhp
步骤502，建立整个水下导航定位系统观测方程；
斜距差之差观测方程为：
是将位置误差由球面坐标系转换到直角坐标系的转换矩阵，表达式为：
式中，RM表示地球卯酉圈曲率半径，L、λ、h分别表示潜器纬度、经度和深度，e表示地球偏心率；
将转换矩阵代入斜距差之差观测方程，得斜距差之差观测方程为：
Zδρ＝HδρX+Vδρ
式中，Zδρ＝[δρjk](N-1)×1，Hδρ＝[0(N-1)×6 Hδρ1 0(N-1)×6 Hδρ2 0(N-1)×1]，X＝[XI XL XP]T，Hδρ2＝[-c 0](N-1)×2，Vδρ＝[-υδρ](N-1)×1，N为潜器进入声学基阵工作区水听器的可用个数，N>2；
斜距率差之差观测方程为：
是速度误差由东北天坐标系转换到直角坐标系的转换矩阵，表达式为：
将转换矩阵代入斜距率差之差观测方程，得斜距率差之差观测方程为：
式中，X＝[XI XL XP]T，
深度之差观测方程为：
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
式中，观测量Zδρ、/>和ZP分别为捷联惯导系统获取的斜距差、斜距率差、潜器深度值与长基线水声定位系统获取的斜距差和斜距率差、压力传感器获取的潜器深度值的差值，观测矩阵/>X＝[XI XL XP]T，观测噪声/>
步骤503，将上述得到的斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波，并利用滤波器输出的当前误差最优估计修正捷联惯导系统中的状态量位置校正通过捷联惯导系统的位置解算值与位置误差估计值相减来修正；
式中，Xc是修正后的状态量。
2.根据权利要求1所述的一种基于SINS/LBL紧组合的水下导航定位方法，其特征在于，所述步骤2中判断声学基阵工作区水听器的可用个数，具体判断方法如下：
通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，当斜距小于声波传播距离时，该水听器为可用水听器。
3.一种基于SINS/LBL紧组合的水下导航定位系统，其特征在于，所述系统包括：
捷联惯导系统，用于获取潜器与声学基阵之间基于SINS的斜距差和斜距率差并输出至数据处理单元；
长基线水声定位系统，用于判断水听器的可用个数，并获取潜器与声学基阵之间基于LBL的斜距差和斜距率差并输出至数据处理单元；
压力传感器，用于获取潜器的深度值并输出至数据处理单元；
数据处理单元，用于对获取的数据进行权利要求1中步骤5所述的处理。
4.根据权利要求3所述的一种基于SINS/LBL紧组合的水下导航定位系统，其特征在于，所述捷联惯导系统包括惯性测量单元，用于采集潜器的角速率和比力信息，进行捷联惯导系统初始对准并获取潜器的初始姿态矩阵和初始速度信息。
5.根据权利要求3所述的一种基于SINS/LBL紧组合的水下导航定位系统，其特征在于，所述长基线水声定位系统包括布放在海底的声学基阵和安装在潜器上的声源；通过潜器上声源获取潜器相对于声学基阵中各水听器的斜距，与声波传播距离相比较，判断水听器的可用个数。
6.根据权利要求5所述的一种基于SINS/LBL紧组合的水下导航定位系统，其特征在于，所述声学基阵包括至少三个水听器，用于接收声源信号。
7.根据权利要求3所述的一种基于SINS/LBL紧组合的水下导航定位系统，其特征在于，所述数据处理单元包括卡尔曼滤波器，用于对获取的斜距差之差、斜距率差之差和深度之差进行卡尔曼滤波。
CN202010847141.2A 2020-08-21 2020-08-21 一种基于sins/lbl紧组合的水下导航定位方法及系统 Active CN111982105B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010847141.2A CN111982105B (zh) 2020-08-21 2020-08-21 一种基于sins/lbl紧组合的水下导航定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010847141.2A CN111982105B (zh) 2020-08-21 2020-08-21 一种基于sins/lbl紧组合的水下导航定位方法及系统

Publications (2)

Publication Number Publication Date
CN111982105A CN111982105A (zh) 2020-11-24
CN111982105B true CN111982105B (zh) 2023-10-24

Family

ID=73442950

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010847141.2A Active CN111982105B (zh) 2020-08-21 2020-08-21 一种基于sins/lbl紧组合的水下导航定位方法及系统

Country Status (1)

Country Link
CN (1) CN111982105B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113156442A (zh) * 2021-03-22 2021-07-23 河海大学 一种基于长基线水声系统辅助导航的auv水下定位方法
CN113280812B (zh) * 2021-05-12 2023-06-02 中山大学 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置
CN114199234B (zh) * 2021-11-18 2023-09-12 湖南国天电子科技有限公司 一种容错式水下惯性组合导航方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
CN106767793A (zh) * 2017-01-19 2017-05-31 东南大学 一种基于sins/usbl紧组合的auv水下导航定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
CN106767793A (zh) * 2017-01-19 2017-05-31 东南大学 一种基于sins/usbl紧组合的auv水下导航定位方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
AUV positioning method based on tightly coupled SINS/LBL for underwater acoustic multipath propagation;T. Zhang 等;《Sensors》;全文 *
INS/USBL组合导航技术研究;白金磊;《中国优秀硕士学位论文全文数据库》;全文 *
基于SINS/LBL紧组合的AUV水下导航定位技术;张涛;石宏飞;徐晓苏;;中国惯性技术学报(第04期);全文 *
基于UKF的SINS/LBL水下AUV紧组合定位技术;张涛;石宏飞;陈立平;刘强;;中国惯性技术学报(第05期);全文 *
基于倒置声学基阵的INSUSBL组合导航算法研究;徐博;郝芮;王超;张勋;;海洋技术学报(第05期);全文 *

Also Published As

Publication number Publication date
CN111982105A (zh) 2020-11-24

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109443379B (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN111982105B (zh) 一种基于sins/lbl紧组合的水下导航定位方法及系统
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN101900558B (zh) 集成声纳微导航的自主式水下机器人组合导航方法
CN104316045A (zh) 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN111596333B (zh) 一种水下定位导航方法及系统
WO2008144139A1 (en) System and process for the precise positioning of subsea units
CN110274591B (zh) 深潜载人潜水器adcp辅助sins导航方法
CN106679662A (zh) 一种基于tma技术的水下机器人单信标组合导航方法
CN112284384A (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN111504324B (zh) 一种噪声自适应滤波的水下组合导航方法
CN104197939A (zh) 基于水下信息网络的多参考节点水下航行器组合导航方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
CN111220146B (zh) 一种基于高斯过程回归学习的水下地形匹配定位方法
Song et al. Underwater adaptive height-constraint algorithm based on SINS/LBL tightly coupled
Chen In-situ alignment calibration of attitude and ultra short baseline sensors for precision underwater positioning
CN111964684B (zh) 一种基于sins/lbl紧组合的水下导航混合定位方法及系统
CN112083425B (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN116222578B (zh) 基于自适应滤波和最优平滑的水下组合导航方法及系统
CN110873813B (zh) 一种水流速度估算方法、组合导航方法及装置
CN117029872A (zh) 一种基于ins/dvl/lbl紧组合的auv导航方法及导航系统
CN113670303B (zh) 一种基于rbf神经网络的sins/dvl组合导航流速补偿方法
CN111721284B (zh) 一种无源模式下的sins/usbl组合导航定位方法

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