CN116337115A - 一种基于声呐标校惯性传感器的方法及*** - Google Patents

一种基于声呐标校惯性传感器的方法及*** Download PDF

Info

Publication number
CN116337115A
CN116337115A CN202310628706.1A CN202310628706A CN116337115A CN 116337115 A CN116337115 A CN 116337115A CN 202310628706 A CN202310628706 A CN 202310628706A CN 116337115 A CN116337115 A CN 116337115A
Authority
CN
China
Prior art keywords
error
sonar
inertial navigation
inertial
navigation
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
CN202310628706.1A
Other languages
English (en)
Other versions
CN116337115B (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.)
Laoshan National Laboratory
Shandong University
Original Assignee
Laoshan National Laboratory
Shandong 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 Laoshan National Laboratory, Shandong University filed Critical Laoshan National Laboratory
Priority to CN202310628706.1A priority Critical patent/CN116337115B/zh
Publication of CN116337115A publication Critical patent/CN116337115A/zh
Application granted granted Critical
Publication of CN116337115B publication Critical patent/CN116337115B/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
    • 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
    • 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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • G01S15/08—Systems for measuring distance only
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/52—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S15/00
    • G01S7/52004—Means for monitoring or calibrating
    • 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)
  • Computer Networks & Wireless Communication (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Navigation (AREA)

Abstract

本发明提出一种基于声呐标校惯性传感器的方法及***，涉及导航领域。包括建立惯性传感器测量误差模型；建立导航***状态方程；将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，构建量测方程；更新声学观测量、惯导推算观测量；采用卡尔曼滤波更新惯导误差，在没有声学观测量时，惯导进行独立推算位置；当声学观测量更新时，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，进行量测更新，直至惯导误差收敛。本发明利用观测到的声呐信号对惯导误差在线标校，使得水下潜器在没有声呐信号时依靠纯惯导位置推算驶向下一个海底基准站网。

Description

一种基于声呐标校惯性传感器的方法及***
技术领域
本发明属于水下组合导航技术领域，尤其涉及一种基于声呐标校惯性传感器的方法及***。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。
随着海洋强国战略的提出，水下导航的重要性愈发明显。基于无线电传播的全球卫星导航***GNSS在水下无法应用，而声学导航克服了电磁波在海水中衰减较快的缺点，因此声呐信号成为水下导航的重要信号。目前水下导航技术有声学导航、惯性导航以及地球物理场匹配导航等。随着水下声学定位的广泛应用，以惯导SINS/声学LBL为主的组合导航***成为发展趋势。
目前国内对使用声呐标校惯导的研究尚少，但在使用GPS标校惯导的工作中已经取得了一定的研究成果。李海强等人在2006年提出了对整弹中的捷联惯性传感器进行标定，实现了对主要器件误差的标定；陆志东等人设计了一种包括陀螺和加表的随机常值误差、刻度因子误差在内的17维卡尔曼滤波器，并将GPS提供的信息作为外部观测量，通过规划合适的飞行路径来激励相关误差项，进而实现对惯性器件基本误差的标定；北航的张小跃等人提出将IMU的输出误差分解为若干部分，并建立33维的***误差模型，对包括随机常值误差、刻度因子误差、安装误差和随机白噪声等在内的误差项进行了在线标定和补偿；北航的房建成等人提出建立一种包含安装误差的标定残差、刻度因子误差等误差项的39维高阶误差模型，并通过实验证明其在POS***上使用之后与传统误差模型相比，误差精度有较大提高。
水下组合导航中，声呐是主要的导航信息源，水下潜器能接收到声呐信号的前提是海底布设有相应的基准站。在声呐信号观测良好的情况下，单一声学导航精度可达米级，但当声呐信号入射角太小时，会导致导航定位精度急剧恶化。惯导SINS采用加表和陀螺仪获取的数据经过积分运算可获得姿态、速度和位置信息，作为一种自主导航***，具备测量更新频率快、测量信息完备、不受外界干扰等优点，但是其在工作前需要外部提供初始化信息，导航误差也会随着时间递增。因此，SINS常与声呐联合应用于水下导航，通过松、紧组合算法可以提高组合导航的精度与稳定度。
发明人发现，SINS/LBL的组合导航***，具有高精度和高稳定性的特点，并且可靠性和隐蔽性较好。但是SINS/LBL的组合导航***需要可靠的海洋基准，即需要在海底布设大量的基准站，而大量海底基准站的布设与维护需要大量的人力财力。
发明人还发现，虽然已有的方法在水下环境可以提供一定的导航定位能力，但还存在着各种各样的问题。声呐信号能测出水下潜器到海底基准点距离，对潜器进行导航定位，受限于海底基准站数量较少，在水下潜器远离海底基准点时，由于声呐信号入射角太低、信号传输距离远，使得声呐信号衰弱导致接收信号数量减少或者由于环境遮挡根本无法接收到声呐信号等原因，导致单一声学导航定位结果差或根本无法提供定位服务。惯性导航虽然有自主推算能力，不需要外界观测量便可提供定位结果，但由于其导航误差会不断累积，一般精度的惯导推算结果会快速偏离真实值，无法满足为水下潜器长时间提供导航定位的需求。惯导与声呐组合导航确实能够提高定位解算的精度，但受限于海底基准点布设数量较少，无法进行长时间组合导航，为水下潜器提供长期稳定的导航定位服务。
发明内容
为克服上述现有技术的不足，本发明提供了一种基于声呐标校惯性传感器的方法及***，在水下环境中，利用观测到的声呐信号对惯导误差在线标校，抑制惯性导航位置误差发散速度，在有海底基准站时使用接收到的声呐信号标校惯导，减少惯导自身误差，使得水下潜器能够在没有声呐信号时依靠纯惯导位置推算驶向下一个海底基准站网，既解决了海底基准站少无法长时间提供声呐信号的问题，也避免了惯导长时间独立工作导致推算结果误差过大的问题，大幅度提高惯导推算位置精度，提高适应海底基准站较少现状的能力。
为实现上述目的，本发明的一个或多个实施例提供了如下技术方案：
本发明第一方面提供了一种基于声呐标校惯性传感器的方法。
一种基于声呐标校惯性传感器的方法，包括以下步骤：
步骤一：建立惯性传感器测量误差模型；
步骤二：基于惯性传感器测量误差模型，在当地导航坐标系下构建状态向量，基于状态向量建立导航***状态方程；
步骤三：获取声呐测距信息，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，构建量测方程；
步骤四：基于当前历元观测到的声呐信号传播速度和传播时间，更新声学观测量；基于当前历元惯导量测更新位置和海底基准站位置，更新惯导推算观测量；
步骤五：采用卡尔曼滤波更新惯导误差，在没有声学观测量时，惯导进行独立推算位置；当声学观测量更新、获取新的声呐测距信息时，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，使用步骤三中构建的量测方程进行量测更新；
步骤六：循环步骤四至步骤五，直至惯导误差收敛，完成惯导误差标校；
步骤七：建立误差反馈模型，将惯导误差收敛时刻的状态向量代入至误差反馈模型中，得到经惯导误差补偿后的陀螺仪和加速度计的理论输出。
本发明第二方面提供了一种基于声呐标校惯性传感器的***。
一种基于声呐标校惯性传感器的***，包括：
测量误差模型建立模块，被配置为：建立惯性传感器测量误差模型；
状态方程建立模块，被配置为：基于惯性传感器测量误差模型，在当地导航坐标系下构建状态向量，基于状态向量建立导航***状态方程；
量测方程建立模块，被配置为：获取声呐测距信息，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，构建量测方程；
更新模块，被配置为：基于当前历元观测到的声呐信号传播速度和传播时间，更新声学观测量；基于当前历元惯导量测更新位置和海底基准站位置，更新惯导推算观测量；
卡尔曼滤波模块，被配置为：采用卡尔曼滤波更新惯导误差，在没有声学观测量时，惯导进行独立推算位置；当声学观测量更新、获取新的声呐测距信息时，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，使用量测方程建立模块中构建的量测方程进行量测更新；
循环模块，被配置为：循环步骤四至步骤五，直至惯导误差收敛，完成惯导误差标校；
反馈模块，被配置为：建立误差反馈模型，将惯导误差收敛时刻的状态向量代入至误差反馈模型中，得到经惯导误差补偿后的陀螺仪和加速度计的理论输出。
本发明第三方面提供了计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明第一方面所述的基于声呐标校惯性传感器的方法中的步骤。
本发明第四方面提供了电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明第一方面所述的基于声呐标校惯性传感器的方法中的步骤。
以上一个或多个技术方案存在以下有益效果：
为了维持水下导航的精度，本发明提出了一种基于声呐标校惯性传感器的方法及***，在能接收到海底基准站的声呐信号时，利用声呐测距信号标校惯导***，减少惯导累积误差，使得水下载体能够在无法接收声呐信号时，依靠纯惯导，依然能维持一定的精度，并导航至下一组海底基准站网，以此类推，可以在海底基准站布设数量较少的前提下为水下载体提供较高精度的导航位置服务。
本发明在有海底基准站时，将接收到的声呐测距信息与惯导解算结果反推出的测距信息之差作为观测向量构建量测方程，将惯导误差作为状态向量一部分，使用卡尔曼滤波技术在线标定惯导***误差，将惯导***误差反馈给惯性导航***，抑制惯导位置误差发散速度，实验测试结果表明，对于微机械级惯导该方法可以有效抑制惯导误差发散的速度，使得惯导位置误差发散一公里维持时间提高121%，大幅度提高惯导推算位置精度，提高适应海底基准站较少现状的能力。
本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
图1为第一个实施例的方法流程图。
图2为第二个实施例的***结构图。
具体实施方式
应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。
在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
术语解释：
DVL：多普勒计程仪(Doppler Velocity Log）；
SINS：捷联惯性导航***(Strap-down Inertial Navigation System)
GNSS：全球卫星导航***(Global Navigation Satellite System)
LBL：长基线定位***Long BaseLine positioning system
GPS:全球定位***Global Positioning System
IMU：惯性测量单元(Inertial Measurement Unit)
POS：位置和定向***(Position and Orientation System)
实施例一
本实施例公开了一种基于声呐标校惯性传感器的方法。
如图1所示，一种基于声呐标校惯性传感器的方法，包括以下步骤：
步骤一：建立惯性传感器测量误差模型；
步骤二：基于惯性传感器测量误差模型，在当地导航坐标系下构建状态向量，基于状态向量建立导航***状态方程；
步骤三：获取声呐测距信息，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，构建量测方程；
步骤四：基于当前历元观测到的声呐信号传播速度和传播时间，更新声学观测量；基于当前历元惯导量测更新位置和海底基准站位置，更新惯导推算观测量；
步骤五：采用卡尔曼滤波更新惯导误差，在没有声学观测量时，惯导进行独立推算位置；当声学观测量更新、获取新的声呐测距信息时，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，使用到步骤三中构建的量测方程进行量测更新；
步骤六：循环步骤四至步骤五，直至惯导误差收敛，完成惯导误差标校；
步骤七：建立误差反馈模型，将惯导误差收敛时刻的状态向量代入至误差反馈模型中，得到经惯导误差补偿后的陀螺仪和加速度计的理论输出。
本发明为了解决惯导独立推算结果误差发散快和精度低的问题，提出一种基于卡尔曼滤波的声呐标校惯导算法，详细步骤如下：
（一）惯导***误差建模。
水下载体在运行过程中，惯性传感器的测量误差将明显偏离其实验室环境下的离线标定值。为了对水下载体运行环境下的惯性传感器误差进行有效标定及补偿，建立如下惯性传感器测量误差模型。
Figure SMS_1
(1)
式中，
Figure SMS_4
和/>
Figure SMS_8
分别为陀螺仪和加速度计在载体坐标系的量测误差；/>
Figure SMS_11
和/>
Figure SMS_5
分别为陀螺仪和加速度计的安装误差矩阵，其中/>
Figure SMS_7
为三个陀螺仪的安装误差系数，/>
Figure SMS_9
为三个加速度计的安装误差系数，表示j轴对i轴的影响；/>
Figure SMS_13
和
Figure SMS_2
分别为陀螺仪和加速度计的刻度因子误差矩阵；
Figure SMS_6
和/>
Figure SMS_10
分别为陀螺仪和加速度计的随机常值误差矩阵；/>
Figure SMS_12
和/>
Figure SMS_3
分别为陀螺仪和加速度计在载体坐标系下的理想输出。
（二）构建***状态方程。
***状态方程的建立与惯性传感器误差类型、组合方式以及坐标系的选择有关。对于多源传感器，还应考虑对不同状态向量的公共部分进行融合。在当地导航坐标系（local navigation frame）下构建的27维状态向量为：
Figure SMS_14
(2)
其中，
Figure SMS_17
向量表示当地导航坐标系下沿东北天方向上的姿态角误差；/>
Figure SMS_20
向量表示当地导航坐标系下沿东北天方向上的速度误差；/>
Figure SMS_22
表示为大地坐标系下三维位置误差；/>
Figure SMS_16
和/>
Figure SMS_19
分别表示载体坐标系下沿X、Y、Z方向上的陀螺仪和加速度的随机常值误差矩阵；/>
Figure SMS_21
和/>
Figure SMS_23
分别表示载体坐标系下沿X、Y、Z方向上的陀螺仪和加速度计的刻度因子误差矩阵；
Figure SMS_15
和/>
Figure SMS_18
分别是载体坐标系下沿X、Y、Z方向上的陀螺仪和加速度计的安装误差矩阵。
导航***的状态方程为
Figure SMS_24
(3)
式中，
Figure SMS_25
为状态转移矩阵；/>
Figure SMS_26
为过程噪声向量，
Figure SMS_27
为陀螺仪和加速度计的随机白噪声，/>
Figure SMS_28
为陀螺仪和加速度计的随机游走过程噪声。
（三）构建***量测方程。
声呐LBL-SINS滤波器通过量测方程可以对***状态向量进行修正。
LBL的观测向量为载体到海底基准点间的距离，当进行量测更新时，与通过SINS推算得到的观测值作差得到观测信息。
LBL-SINS紧组合导航***的量测方程为：
Figure SMS_29
(4)
式中，
Figure SMS_31
为/>
Figure SMS_34
时刻SINS推算的第/>
Figure SMS_37
个海底基准站至惯性传感器之间的距离，/>
Figure SMS_32
为/>
Figure SMS_35
时刻观测到的第/>
Figure SMS_38
个海底基准站至水下潜器换能器之间的距离；/>
Figure SMS_40
为量测噪声向量，/>
Figure SMS_30
为/>
Figure SMS_33
时刻观测到的第/>
Figure SMS_36
个海底基准站至水下潜器换能器之间的距离误差；
Figure SMS_39
为量测关系矩阵，表达式为：
Figure SMS_41
(5)
式中
Figure SMS_42
，/>
Figure SMS_43
，/>
Figure SMS_44
，
Figure SMS_45
，/>
Figure SMS_46
Figure SMS_49
、/>
Figure SMS_54
和/>
Figure SMS_61
分别表示/>
Figure SMS_50
时刻坐标点的所在卯酉圈曲率半径、子午圈曲率半径以及高程；/>
Figure SMS_59
表示/>
Figure SMS_63
时刻坐标点对应的纬度值；/>
Figure SMS_68
表示/>
Figure SMS_53
时刻坐标点对应的经度值；/>
Figure SMS_57
表示/>
Figure SMS_65
时刻载体坐标系至当地导航坐标系的坐标旋转矩阵；/>
Figure SMS_69
表示惯性传感器与水下潜器换能器之间的杆臂；/>
Figure SMS_48
表示地球第一偏心率；/>
Figure SMS_55
系为载体坐标系，/>
Figure SMS_62
系为当地导航坐标系，/>
Figure SMS_67
系为大地坐标系，/>
Figure SMS_52
系为地心地固坐标系；/>
Figure SMS_58
为将/>
Figure SMS_64
系转化到/>
Figure SMS_70
系的坐标转换矩阵；/>
Figure SMS_47
为将/>
Figure SMS_60
系转化为/>
Figure SMS_66
系的坐标转换矩阵；/>
Figure SMS_71
为/>
Figure SMS_51
系转化到/>
Figure SMS_56
系的坐标转换矩阵。
（四）更新声学观测量。
声学观测量可由当前历元观测到的声呐信号传播速度以及传播时间获得。通过现场实测或者已有的声速场可以获得水下声速；通过记录水下潜器换能器发射声呐信号到海底基准接收到水下潜器换能器的时间，可以获得声呐信号传播时间；将水下声速和声呐信号传播时间两者积分即可获得声学观测量。
（五）更新SINS推算观测量。
根据当前历元SINS量测更新位置和海底基准站位置，经过杆臂改正后，反推得到海底基准点至水下潜器换能器的距离。
（六）卡尔曼滤波更新惯导误差。
将步骤（2）和（3）给出的状态方程和量测方程离散化，滤波器状态更新过程为：
Figure SMS_72
(6)
滤波器量测更新过程为：
Figure SMS_73
(7)
式中，
Figure SMS_75
、/>
Figure SMS_79
、/>
Figure SMS_81
分别表示***离散化之后的前一时刻状态估计、状态一步预测和这一时刻状态估计；/>
Figure SMS_77
、/>
Figure SMS_80
、/>
Figure SMS_83
分别表示***离散化之后前一时刻状态均方误差阵、状态一步预测均方误差阵和这一时刻状态均方误差阵；/>
Figure SMS_85
表示***离散化之后上一时刻***噪声矩阵；/>
Figure SMS_74
表示***离散化之后状态一步转移矩阵；/>
Figure SMS_78
、/>
Figure SMS_82
、/>
Figure SMS_84
、/>
Figure SMS_76
分别表示***离散化之后这一时刻卡尔曼滤波增益矩阵、量测更新矩阵、观测噪声矩阵和观测量。
此步骤中卡尔曼滤波更新分为两部分，第一部分是状态更新，对应于公式（6）；第二部分是量测更新，对应于公式（7）。由于惯性导航的更新频率为100HZ，即1秒内更新100次，声学观测量的更新频率为0.5HZ，即两秒内更新1次。所以在声学观测量更新的间隙中惯性数据按照公式（6）进行独立推算，实现状态更新；当步骤四中声学观测量更新时，此时进行步骤五，使用惯导推算出的位置与海底基准解算出海底基准到水下潜器换能器的距离，将步骤四与步骤五更新结果带入公式（7）中实现量测更新，以此类推使用卡尔曼滤波实现惯导误差的更新，当惯导误差收敛时即可完成惯导误差标校。
（七）建立误差反馈模型并反馈惯导误差。
误差反馈模型为：
Figure SMS_86
(8)
Figure SMS_87
和/>
Figure SMS_88
为陀螺仪和加速度计的测量输出；
步骤（六）中，利用卡尔曼滤波技术估计出下一时刻状态向量
Figure SMS_89
和状态向量均方误差阵/>
Figure SMS_90
，当持续更新一段时间之后状态向量均方误差阵/>
Figure SMS_91
逐渐趋于零，这也意味着估计出的状态向量/>
Figure SMS_92
趋于真实值，将此值带入到公式（8），得到经惯导误差补偿后的陀螺仪和加速度计的理论输出/>
Figure SMS_93
和/>
Figure SMS_94
。
实施例二
本实施例公开了一种基于声呐标校惯性传感器的***。
如图2所示，一种基于声呐标校惯性传感器的***，包括：
测量误差模型建立模块，被配置为：建立惯性传感器测量误差模型；
状态方程建立模块，被配置为：基于惯性传感器测量误差模型，在当地导航坐标系下构建状态向量，基于状态向量建立导航***状态方程；
量测方程建立模块，被配置为：获取声呐测距信息，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，构建量测方程；
更新模块，被配置为：基于当前历元观测到的声呐信号传播速度和传播时间，更新声学观测量；基于当前历元惯导量测更新位置和海底基准站位置，更新惯导推算观测量；
卡尔曼滤波模块，被配置为：采用卡尔曼滤波更新惯导误差，在没有声学观测量时，惯导进行独立推算位置；当声学观测量更新、获取新的声呐测距信息时，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，使用量测方程建立模块构建的量测方程进行量测更新；
循环模块，被配置为：循环步骤四至步骤五，直至惯导误差收敛，完成惯导误差标校；
反馈模块，被配置为：建立误差反馈模型，将惯导误差收敛时刻的状态向量代入至误差反馈模型中，得到经惯导误差补偿后的陀螺仪和加速度计的理论输出。
实施例三
本实施例的目的是提供计算机可读存储介质。
计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如本公开实施例1所述的基于声呐标校惯性传感器的方法中的步骤。
实施例四
本实施例的目的是提供电子设备。
电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本公开实施例1所述的基于声呐标校惯性传感器的方法中的步骤。
以上实施例二、三和四的装置中涉及的各步骤与方法实施例一相对应，具体实施方式可参见实施例一的相关说明部分。术语“计算机可读存储介质”应该理解为包括一个或多个指令集的单个介质或多个介质；还应当被理解为包括任何介质，所述任何介质能够存储、编码或承载用于由处理器执行的指令集并使处理器执行本发明中的任一方法。
本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种基于声呐标校惯性传感器的方法，其特征在于，包括以下步骤：
步骤一：建立惯性传感器测量误差模型；
步骤二：基于惯性传感器测量误差模型，在当地导航坐标系下构建状态向量，基于状态向量建立导航***状态方程；
步骤三：获取声呐测距信息，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，构建量测方程；
步骤四：基于当前历元观测到的声呐信号传播速度和传播时间，更新声学观测量；基于当前历元惯导量测更新位置和海底基准站位置，更新惯导推算观测量；
步骤五：采用卡尔曼滤波更新惯导误差，在没有声学观测量时，惯导进行独立推算位置；当声学观测量更新、获取新的声呐测距信息时，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，使用到步骤三中构建的量测方程进行量测更新；
步骤六：循环步骤四至步骤五，直至惯导误差收敛，完成惯导误差标校；
步骤七：建立误差反馈模型，将惯导误差收敛时刻的状态向量代入至误差反馈模型中，得到经惯导误差补偿后的陀螺仪和加速度计的理论输出。
2.如权利要求1所述的基于声呐标校惯性传感器的方法，其特征在于，所述惯性传感器测量误差模型为：
Figure QLYQS_1
其中，
Figure QLYQS_4
和/>
Figure QLYQS_5
分别为陀螺仪和加速度计在载体坐标系的量测误差；/>
Figure QLYQS_8
和/>
Figure QLYQS_3
分别为陀螺仪和加速度计的安装误差矩阵，/>
Figure QLYQS_7
和/>
Figure QLYQS_10
分别为陀螺仪和加速度计的刻度因子误差矩阵；/>
Figure QLYQS_11
和/>
Figure QLYQS_2
分别为陀螺仪和加速度计的随机常值误差矩阵；/>
Figure QLYQS_6
和/>
Figure QLYQS_9
分别为陀螺仪和加速度计在载体坐标系下的理想输出。
3.如权利要求1所述的基于声呐标校惯性传感器的方法，其特征在于，所述导航***状态方程为：
Figure QLYQS_12
Figure QLYQS_13
其中，
Figure QLYQS_15
向量表示当地导航坐标系下沿东北天方向上的姿态角误差；/>
Figure QLYQS_18
向量表示当地导航坐标系下沿东北天方向上的速度误差；/>
Figure QLYQS_21
表示为大地坐标系下三维位置误差；/>
Figure QLYQS_16
和
Figure QLYQS_17
分别表示载体坐标系下沿X、Y、Z方向上的陀螺仪和加速度的随机常值误差矩阵；/>
Figure QLYQS_20
和
Figure QLYQS_22
分别表示载体坐标系下沿X、Y、Z方向上的陀螺仪和加速度计的刻度因子误差矩阵；/>
Figure QLYQS_14
和/>
Figure QLYQS_19
分别是载体坐标系下沿X、Y、Z方向上的陀螺仪和加速度计的安装误差矩阵。
4.如权利要求1所述的基于声呐标校惯性传感器的方法，其特征在于，所述量测方程为：
Figure QLYQS_23
其中，
Figure QLYQS_25
为/>
Figure QLYQS_29
时刻惯导推算的第/>
Figure QLYQS_32
个海底基准站至惯性传感器之间的距离，/>
Figure QLYQS_26
为/>
Figure QLYQS_28
时刻观测到的第/>
Figure QLYQS_31
个海底基准站至水下潜器换能器之间的距离；/>
Figure QLYQS_34
为量测噪声向量，/>
Figure QLYQS_24
为/>
Figure QLYQS_27
时刻观测到的第/>
Figure QLYQS_30
个海底基准站至水下潜器换能器之间的距离误差；/>
Figure QLYQS_33
为量测关系矩阵。
5.如权利要求1所述的基于声呐标校惯性传感器的方法，其特征在于，采用卡尔曼滤波更新惯导误差，具体为：
滤波器状态更新过程为：
Figure QLYQS_35
滤波器量测更新过程为：
Figure QLYQS_36
其中，
Figure QLYQS_38
、/>
Figure QLYQS_43
、/>
Figure QLYQS_46
分别表示***离散化之后的前一时刻状态估计、状态一步预测和这一时刻状态估计；/>
Figure QLYQS_39
、/>
Figure QLYQS_41
、/>
Figure QLYQS_44
分别表示***离散化之后前一时刻状态均方误差阵、状态一步预测均方误差阵和这一时刻状态均方误差阵；/>
Figure QLYQS_47
表示***离散化之后上一时刻***噪声矩阵；/>
Figure QLYQS_37
表示***离散化之后状态一步转移矩阵；/>
Figure QLYQS_42
、/>
Figure QLYQS_45
、/>
Figure QLYQS_48
、/>
Figure QLYQS_40
分别表示***离散化之后这一时刻卡尔曼滤波增益矩阵、量测更新矩阵、观测噪声矩阵和观测量。
6.如权利要求1所述的基于声呐标校惯性传感器的方法，其特征在于，误差反馈模型为：
Figure QLYQS_49
其中，
Figure QLYQS_50
和/>
Figure QLYQS_51
分别为陀螺仪和加速度计的测量输出；/>
Figure QLYQS_52
和/>
Figure QLYQS_53
分别为经惯导误差补偿后的陀螺仪和加速度计的理论输出。
7.如权利要求1所述的基于声呐标校惯性传感器的方法，其特征在于，得到经惯导误差补偿后的陀螺仪和加速度计的理论输出之后，在没有声呐信号的情况下，依靠纯惯导位置推算驶向下一个海底基准站网。
8.一种基于声呐标校惯性传感器的***，其特征在于：包括：
测量误差模型建立模块，被配置为：建立惯性传感器测量误差模型；
状态方程建立模块，被配置为：基于惯性传感器测量误差模型，在当地导航坐标系下构建状态向量，基于状态向量建立导航***状态方程；
量测方程建立模块，被配置为：获取声呐测距信息，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，构建量测方程；
更新模块，被配置为：基于当前历元观测到的声呐信号传播速度和传播时间，更新声学观测量；基于当前历元惯导量测更新位置和海底基准站位置，更新惯导推算观测量；
卡尔曼滤波模块，被配置为：采用卡尔曼滤波更新惯导误差，在没有声学观测量时，惯导进行独立推算位置；当声学观测量更新、获取新的声呐测距信息时，将声呐测距信息与使用惯导独立推算的位置和海底基准站位置反推出的测距信息之差作为观测向量，使用到量测方程建立模块构建的量测方程进行量测更新；
循环模块，被配置为：循环步骤四至步骤五，直至惯导误差收敛，完成惯导误差标校；
反馈模块，被配置为：建立误差反馈模型，将惯导误差收敛时刻的状态向量代入至误差反馈模型中，得到经惯导误差补偿后的陀螺仪和加速度计的理论输出。
9.计算机可读存储介质，其上存储有程序，其特征在于，该程序被处理器执行时实现如权利要求1-7任一项所述的基于声呐标校惯性传感器的方法中的步骤。
10.电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7任一项所述的基于声呐标校惯性传感器的方法中的步骤。
CN202310628706.1A 2023-05-31 2023-05-31 一种基于声呐标校惯性传感器的方法及*** Active CN116337115B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310628706.1A CN116337115B (zh) 2023-05-31 2023-05-31 一种基于声呐标校惯性传感器的方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310628706.1A CN116337115B (zh) 2023-05-31 2023-05-31 一种基于声呐标校惯性传感器的方法及***

Publications (2)

Publication Number Publication Date
CN116337115A true CN116337115A (zh) 2023-06-27
CN116337115B CN116337115B (zh) 2023-08-29

Family

ID=86880878

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310628706.1A Active CN116337115B (zh) 2023-05-31 2023-05-31 一种基于声呐标校惯性传感器的方法及***

Country Status (1)

Country Link
CN (1) CN116337115B (zh)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007210402A (ja) * 2006-02-08 2007-08-23 Kawasaki Heavy Ind Ltd 自律型無人潜水機およびその水中航行方法
US20100246322A1 (en) * 2009-03-27 2010-09-30 Welker Kenneth E Determining a position of a survey receiver in a body of water
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法
CN110207698A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种极区格网惯导/超短基线紧组合导航方法
CN110207694A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法
US20200311514A1 (en) * 2019-04-01 2020-10-01 Honeywell International Inc. Deep neural network-based inertial measurement unit (imu) sensor compensation method
CN112083425A (zh) * 2020-09-14 2020-12-15 湖南航天机电设备与特种材料研究所 一种引入径向速度的sins/lbl紧组合导航方法
CN113670302A (zh) * 2021-09-01 2021-11-19 东南大学 一种运动效应影响下的惯性/超短基线组合导航方法
CN115307643A (zh) * 2022-08-24 2022-11-08 东南大学 一种双应答器辅助的sins/usbl组合导航方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007210402A (ja) * 2006-02-08 2007-08-23 Kawasaki Heavy Ind Ltd 自律型無人潜水機およびその水中航行方法
US20100246322A1 (en) * 2009-03-27 2010-09-30 Welker Kenneth E Determining a position of a survey receiver in a body of water
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109828296A (zh) * 2019-03-08 2019-05-31 哈尔滨工程大学 一种ins/usbl非线性紧组合综合校正方法
US20200311514A1 (en) * 2019-04-01 2020-10-01 Honeywell International Inc. Deep neural network-based inertial measurement unit (imu) sensor compensation method
CN110207698A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种极区格网惯导/超短基线紧组合导航方法
CN110207694A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法
CN112083425A (zh) * 2020-09-14 2020-12-15 湖南航天机电设备与特种材料研究所 一种引入径向速度的sins/lbl紧组合导航方法
CN113670302A (zh) * 2021-09-01 2021-11-19 东南大学 一种运动效应影响下的惯性/超短基线组合导航方法
CN115307643A (zh) * 2022-08-24 2022-11-08 东南大学 一种双应答器辅助的sins/usbl组合导航方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
XINGMEI WANG; GUOQIANG WANG; YANXIA WU: "An Adaptive Particle Swarm Optimization for Underwater Target Tracking in Forward Looking Sonar Image Sequences", IEEE ACCESS *
王彬;翁海娜;梁瑾;王彦国;王桂如;: "一种惯性/水声单应答器距离组合导航方法", 中国惯性技术学报, vol. 25, no. 01 *
王银涛，贾晓宝，崔荣鑫，严卫生: "移动USBL测距辅助的UUV协同导航定位方法", 控制理论与应用, vol. 39, no. 11 *
陈建华;朱海;郭正东;栾禄雨;: "基于水声传播时延补偿的惯导误差修正方法", 舰船科学技术, vol. 38, no. 3 *

Also Published As

Publication number Publication date
CN116337115B (zh) 2023-08-29

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN110095800B (zh) 一种多源融合的自适应容错联邦滤波组合导航方法
US7979231B2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
Morgado et al. Tightly coupled ultrashort baseline and inertial navigation system for underwater vehicles: An experimental validation
US8860609B2 (en) Loosely-coupled integration of global navigation satellite system and inertial navigation system
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航***
Hasan et al. A review of navigation systems (integration and algorithms)
CN112378400A (zh) 一种双天线gnss辅助的捷联惯导组合导航方法
CN108594272B (zh) 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN109443379A (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN111323050B (zh) 一种捷联惯导和多普勒组合***标定方法
De Agostino et al. Performances comparison of different MEMS-based IMUs
Mandt et al. Integrating DGPS-USBL position measurements with inertial navigation in the HUGIN 3000 AUV
CN112146655B (zh) 一种BeiDou/SINS紧组合导航***弹性模型设计方法
WO2008056861A1 (en) Apparatus for integrated navigation based on multi filter fusion and method for providing navigation information using the same
CN102252677A (zh) 一种基于时间序列分析的变比例自适应联邦滤波方法
CN113834483B (zh) 一种基于可观测度的惯性/偏振/地磁容错导航方法
CN111965685B (zh) 一种基于多普勒信息的低轨卫星/惯性组合导航定位方法
CN104344836A (zh) 一种基于姿态观测的冗余惯导***光纤陀螺***级标定方法
Troni et al. Preliminary experimental evaluation of a Doppler-aided attitude estimator for improved Doppler navigation of underwater vehicles
CN115096302A (zh) 捷联惯性基导航***信息滤波鲁棒对准方法、***及终端
CN111722295A (zh) 一种水下捷联式重力测量数据处理方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
US8566055B1 (en) Gyro indexing compensation method and system

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