CN113776525A - 一种基于斜距差匹配的惯性/单声源被动组合导航方法 - Google Patents

一种基于斜距差匹配的惯性/单声源被动组合导航方法 Download PDF

Info

Publication number
CN113776525A
CN113776525A CN202111019597.0A CN202111019597A CN113776525A CN 113776525 A CN113776525 A CN 113776525A CN 202111019597 A CN202111019597 A CN 202111019597A CN 113776525 A CN113776525 A CN 113776525A
Authority
CN
China
Prior art keywords
time
error
matrix
navigation
sound source
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
CN202111019597.0A
Other languages
English (en)
Other versions
CN113776525B (zh
Inventor
张涛
张亮
夏茂栋
张佳宇
刘射德
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN202111019597.0A priority Critical patent/CN113776525B/zh
Publication of CN113776525A publication Critical patent/CN113776525A/zh
Application granted granted Critical
Publication of CN113776525B publication Critical patent/CN113776525B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; 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/1652Navigation; 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
    • YGENERAL 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
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02TCLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明公开了一种基于斜距差匹配的惯性/单声源被动组合导航方法,本发明针对传统声学主动定位方式下信息容易暴露的缺点,采用一种单声源被动定位方式辅助惯性导航的组合导航技术,声源按照固定周期向外发射信号,通过接收到信号的时间差完成定位。对于航行器来说,只接收外界的声信号,而可以在不对外辐射信号的情况下实现自主定位,有效地提高了水下航行器导航定位的隐蔽性和安全性。

Description

一种基于斜距差匹配的惯性/单声源被动组合导航方法
技术领域
本发明涉及水下基于时间到达差定位方式的被动导航技术领域,以一种斜距差匹配方式实现高精度的被动导航定位功能,特别是涉及一种基于斜距差匹配的惯性/单声源被动组合导航方法。
背景技术
惯性导航系统(Inertial Navigation System,INS)由于其自主性好、隐蔽性强、短时间内精度高等优点,已广泛应用于水下航行器的自主导航与定位。惯性导航可以全天候的提供姿态、速度、位置的全方位信息,拥有其他导航传感器不可比拟的优点,因此成为水下航行器导航定位的首选。虽然惯性导航技术日益成熟,然而其定位误差随时间积累需要定期重调以保证一定精度的这一根本特性却无法改变,因此,世界各国都在探索惯性导航的系统误差校准方法。
由于声波在水下的传输距离远,信号衰减损失小,因而水声导航被广泛应用于水下航行器的导航与定位中。传统基于时间到达方式,需要收发两端设备按照严格的时钟同步,时钟偏置误差将对定位结果产生较大的影响。而基于时间到达差的定位技术,不需要严格的时钟同步,而是以各水听器接收到的声源信号到达时间差作为计算依据,因此可消除对严格时钟同步的依赖性。由于航行器只接收声信号,不需要向外发送询问信号,因此,是一种低成本、高效的被动定位方式。
综上,基于时间到达差的方式对惯性导航的误差进行校正,可以有效提高水下的导航定位精度。但是其难点在于,根据一个信号周期内的时间到达差得到的斜距差,往往变化很小,难以实现对位置误差的精确校正作用。因此,本发明提出一种斜距差匹配方式的惯性/单声源被动组合导航定位方法。通过建立滑动窗构建合适长度的虚拟基阵,采用基阵内的变化较大的斜距差完成对惯性导航的位置误差校正功能。
发明内容
针以上问题,本发明旨在提供一种基于斜距差匹配的惯性/单声源被动组合导航方法,通过设置滑动窗,根据窗口内不同时刻接收到声信号的惯导位置建立虚拟基阵,选定滑动窗最后一帧窗口的位置为参考基元位置,并计算各个位置相对参考基元位置的时间达到差,根据时间到达差建立卡尔曼滤波的量测模型,以此达到高精度的组合导航的目的。
本发明提供一种基于斜距差匹配的惯性/单声源被动组合导航方法,包括如下步骤:
(1)建立惯性/单声源组合导航的状态方程模型;
步骤(1)中,建立惯性/单声源组合导航的状态方程模型:
将惯导的位置误差、速度误差、姿态误差作为系统状态量,得:
Figure RE-GDA0003327518930000021
其中,[φE φN φU]T表示姿态的失准角误差,[δVE δVN δVU]T表示速度误差,[δL δλ δh]T表示位置误差,[εx εy εz]T表示陀螺仪的零偏,
Figure RE-GDA0003327518930000022
表示加速度计的零偏;
因此,组合导航的状态方程为:
Figure RE-GDA0003327518930000023
其中,F(t)表示状态转移矩阵,根据捷联惯导误差方程设置,W(t)表示过程噪声矩阵;
(2)建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵;
步骤(2)中,建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵,具体为:
设置滑动窗窗口大小,窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵,以滑动窗内的最后一个惯导位置为参考位置,根据窗口内不同时刻位置下航行器接收到的声信号到达时间,计算相对于参考位置的到达时间差,根据已知的声速可以得到斜距差。在获得一组斜距差的基础上,确定一个最大斜距差以及其对应的信号接收时间,以最大斜距差作为滤波器的量测值。
(3)建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型;
步骤(3)中,建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型,具体为:
所确定的最大斜距差所在时刻为tk,参考位置的时间为tk+i,分别对应的惯导位置为
Figure RE-GDA0003327518930000024
Figure RE-GDA0003327518930000025
此时惯导对应的误差状态为
Figure RE-GDA0003327518930000026
Figure RE-GDA0003327518930000027
通过状态转移矩阵统一不同时刻的误差状态,得到:
Figure RE-GDA0003327518930000028
其中,
Figure RE-GDA0003327518930000031
考虑惯导误差情况下,滑动窗内的最大斜距差表示为:
Figure RE-GDA0003327518930000032
其中,
Figure RE-GDA0003327518930000033
表示声源位置;
Figure RE-GDA0003327518930000034
在地球直角坐标系中对位置进行求解,他们在地球坐标系e系下的相对位置矢量pe为:
Figure RE-GDA0003327518930000035
其中,
Figure RE-GDA0003327518930000036
RN为卯酉圈曲率半径,e为椭圆偏心率;
将pe转换到导航系下,其矢量表示为:
Figure RE-GDA0003327518930000037
其中,
Figure RE-GDA0003327518930000038
表示由地球坐标系到导航系的坐标转换矩阵;
考虑惯导位置误差的情况下,
Figure RE-GDA0003327518930000039
表示为:
Figure RE-GDA00033275189300000310
其中,
Figure RE-GDA00033275189300000311
表示
Figure RE-GDA00033275189300000312
的误差,δpn表示惯导位置误差,Ce表示为:
Figure RE-GDA00033275189300000313
其中,RNh=RN+h,[L λ h]分别表示经度、纬度和高度。
BS表示为
Figure RE-GDA00033275189300000314
其中,Sa=sin(a),Sa=cos(a),a=L或λ;
因此,斜距差表示为
Figure RE-GDA00033275189300000315
其中,
Figure RE-GDA0003327518930000041
表示tk时刻位置误差,
Figure RE-GDA0003327518930000042
表示tk+i时刻位置误差,Ce,k+i表示tk+i时刻的Ce矩阵值,
Figure RE-GDA0003327518930000043
表示tk+i时刻的
Figure RE-GDA0003327518930000044
矩阵值,BS,k+i表示tk+i时刻的BS矩阵值,
Figure RE-GDA0003327518930000045
Figure RE-GDA0003327518930000046
表示tk+i时刻的pe矩阵值,Ce,k表示tk时刻的Ce矩阵值,
Figure RE-GDA0003327518930000047
表示tk时刻的
Figure RE-GDA0003327518930000048
矩阵值,BS,k表示tk时刻的BS矩阵值,
Figure RE-GDA0003327518930000049
Figure RE-GDA00033275189300000410
Figure RE-GDA00033275189300000411
表示tk时刻的pe矩阵值,r1表示声源与tk时刻船的位置之间的真实距离,r2表示声源与tk+i时刻船的位置之间的真实距离;
测量得到的斜距差表示为:
Figure RE-GDA00033275189300000412
其中,δr表示测量的误差;
因此,基于斜距差的量测方程表示为:
Figure RE-GDA00033275189300000413
其中,H1=[01×6 HPD1 01×6],
Figure RE-GDA00033275189300000414
H2=[01×6 HPD2 01×6],
Figure RE-GDA00033275189300000415
Figure RE-GDA00033275189300000416
{*}7:9表示向量的第7-9列,
Figure RE-GDA00033275189300000417
(4)根据(1)-(3)建立的状态方程以及量测方程,利用卡尔曼滤波方法进行误差的更新与反馈。
作为本发明进一步改进,步骤(4)中,具体为:
(a)状态一步预测:
Figure RE-GDA00033275189300000418
中,
Figure RE-GDA00033275189300000419
表示k-1时刻的状态估计,Fk-1表示k-1时刻到k时刻的状态转移矩阵;
(b)状态一步预测均方误差:
Figure RE-GDA0003327518930000051
其中,Qk-1表示过程噪声矩阵,Pk-1表示k-1时刻的均方根误差;
(c)滤波增益:
Figure RE-GDA0003327518930000052
其中,Hk表示k时刻的量测矩阵,Rk表示量测噪声矩阵;
(d)状态估计:
Figure RE-GDA0003327518930000053
其中,zk表示观测量;
(e)状态估计均方误差:Pk=Pk-KkHkPk|(k-1)
有益效果:
根据以上步骤所建立的基于斜距差匹配的惯性/单声源被动组合导航定位方法,可以克服传统主动式定位中,信息易暴露,定位隐蔽性差的缺点,提高特殊应用环境下的定位安全性、隐蔽性。
附图说明
图1为滑动窗内虚拟基阵示意图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述:
本发明旨在提供一种基于斜距差匹配的惯性/单声源被动组合导航方法,通过设置滑动窗,根据窗口内不同时刻接收到声信号的惯导位置建立虚拟基阵,选定滑动窗最后一帧窗口的位置为参考基元位置,并计算各个位置相对参考基元位置的时间达到差,根据时间到达差建立卡尔曼滤波的量测模型,以此达到高精度的组合导航的目的。
本发明针对传统主动式声学定位信息易暴露的缺点,设计了基于斜距差匹配的惯性/单声源被动组合导航定位方法,其中滑动窗内虚拟基阵示意图如图1所示,
具体方法如下:
步骤1:建立惯性/单声源组合导航的状态方程模型,具体为:
将惯导的位置误差、速度误差、姿态误差作为系统状态量,得:
Figure RE-GDA0003327518930000054
其中,[φE φN φU]T表示姿态的失准角误差,[δVE δVN δVU]T表示速度误差,[δL δλ δh]T表示位置误差,[εx εy εz]T表示陀螺仪的零偏,
Figure RE-GDA0003327518930000061
表示加速度计的零偏。
因此,组合导航的状态方程为:
Figure RE-GDA0003327518930000062
其中,F(t)表示状态转移矩阵,根据捷联惯导误差方程设置,W(t)表示过程噪声矩阵。
步骤2:建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵,具体为:
设置滑动窗窗口大小,窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵。以滑动窗内的最后一个惯导位置为参考位置,根据窗口内不同时刻位置下航行器接收到的声信号到达时间,计算相对于参考位置的到达时间差,根据已知的声速可以得到斜距差。在获得一组斜距差的基础上,确定一个最大斜距差以及其对应的信号接收时间,以最大斜距差作为滤波器的量测值。
步骤3:建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型,具体为;
步骤2中所确定的最大斜距差所在时刻为tk,参考位置的时间为tk+i,分别对应的惯导位置为
Figure RE-GDA0003327518930000063
Figure RE-GDA0003327518930000064
此时惯导对应的误差状态为
Figure RE-GDA0003327518930000065
Figure RE-GDA0003327518930000066
通过状态转移矩阵统一不同时刻的误差状态,可以得到:
Figure RE-GDA0003327518930000067
其中,
Figure RE-GDA0003327518930000068
考虑惯导误差情况下,滑动窗内的最大斜距差可以表示为:
Figure RE-GDA0003327518930000069
其中,
Figure RE-GDA00033275189300000610
表示声源位置。
Figure RE-GDA00033275189300000611
在地球直角坐标系中对位置进行求解,他们在地球坐标系e系下的相对位置矢量pe为:
Figure RE-GDA00033275189300000612
其中,
Figure RE-GDA0003327518930000071
RN为卯酉圈曲率半径,e为椭圆偏心率。
将pe转换到导航系下,其矢量表示为:
Figure RE-GDA0003327518930000072
其中,
Figure RE-GDA0003327518930000073
表示由地球坐标系到导航系的坐标转换矩阵。
考虑惯导位置误差的情况下,
Figure RE-GDA0003327518930000074
可以表示为:
Figure RE-GDA0003327518930000075
其中,
Figure RE-GDA0003327518930000076
表示
Figure RE-GDA0003327518930000077
的误差,δpn表示惯导位置误差,Ce表示为:
Figure RE-GDA0003327518930000078
其中,RNh=RN+h,[L λ h]分别表示经度、纬度和高度。
BS表示为
Figure RE-GDA0003327518930000079
其中,Sa=sin(a),Sa=cos(a),a=L或λ。
因此,斜距差可以表示为
Figure RE-GDA00033275189300000710
其中,
Figure RE-GDA00033275189300000711
表示tk时刻位置误差,
Figure RE-GDA00033275189300000712
表示tk+i时刻位置误差。Ce,k+i表示tk+i时刻的Ce矩阵值,
Figure RE-GDA00033275189300000713
表示tk+i时刻的
Figure RE-GDA00033275189300000720
矩阵值,BS,k+i表示tk+i时刻的BS矩阵值,
Figure RE-GDA00033275189300000714
Figure RE-GDA00033275189300000715
表示tk+i时刻的pe矩阵值。Ce,k表示tk时刻的Ce矩阵值,
Figure RE-GDA00033275189300000716
表示tk时刻的
Figure RE-GDA00033275189300000717
矩阵值,BS,k表示tk时刻的BS矩阵值,
Figure RE-GDA00033275189300000718
Figure RE-GDA00033275189300000719
表示tk时刻的pe矩阵值。r1表示声源与tk时刻船的位置之间的真实距离,r2表示声源与tk+i时刻船的位置之间的真实距离
测量得到的斜距差可以表示为:
Figure RE-GDA0003327518930000081
其中,δr表示测量的误差。
因此,基于斜距差的量测方程可以表示为:
Figure RE-GDA0003327518930000082
其中,H1=[01×6 HPD1 01×6],
Figure RE-GDA0003327518930000083
H2=[01×6 HPD2 01×6],
Figure RE-GDA0003327518930000084
{*}7:9表示向量的第7-9列。
Figure RE-GDA0003327518930000085
步骤4:根据步骤1-3建立的状态方程以及量测方程,利用卡尔曼滤波方法进行误差的更新与反馈,具体为;
(a)状态一步预测:
Figure RE-GDA0003327518930000086
其中,
Figure RE-GDA0003327518930000087
表示k-1时刻的状态估计,Fk-1表示k-1时刻到k时刻的状态转移矩阵。
(b)状态一步预测均方误差:
Figure RE-GDA0003327518930000088
其中,Qk-1表示过程噪声矩阵,Pk-1表示k-1时刻的均方根误差。
(c)滤波增益:
Figure RE-GDA0003327518930000089
其中,Hk表示k时刻的量测矩阵,Rk表示量测噪声矩阵。
(d)状态估计:
Figure RE-GDA00033275189300000810
其中,zk表示观测量。
(e)状态估计均方误差:Pk=Pk-KkHkPk|(k-1)
以上所述,仅是本发明的较佳实施例之一,并非是对本发明作任何其他形式的限制,而依据本发明的技术实质所作的任何修改或等同变化,仍属于本发明所要求保护的范围。

Claims (2)

1.一种基于斜距差匹配的惯性/单声源被动组合导航方法,其特征在于,包括如下步骤:
(1)建立惯性/单声源组合导航的状态方程模型;
步骤(1)中,建立惯性/单声源组合导航的状态方程模型:
将惯导的位置误差、速度误差、姿态误差作为系统状态量,得:
Figure RE-FDA0003327518920000011
其中,[φE φN φU]T表示姿态的失准角误差,[δVE δVN δVU]T表示速度误差,[δL δλ δh]T表示位置误差,[εx εy εz]T表示陀螺仪的零偏,
Figure RE-FDA0003327518920000012
表示加速度计的零偏;
因此,组合导航的状态方程为:
Figure RE-FDA0003327518920000013
其中,F(t)表示状态转移矩阵,根据捷联惯导误差方程设置,W(t)表示过程噪声矩阵;
(2)建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵;
步骤(2)中,建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵,具体为:
设置滑动窗窗口大小,窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵,以滑动窗内的最后一个惯导位置为参考位置,根据窗口内不同时刻位置下航行器接收到的声信号到达时间,计算相对于参考位置的到达时间差,根据已知的声速可以得到斜距差。在获得一组斜距差的基础上,确定一个最大斜距差以及其对应的信号接收时间,以最大斜距差作为滤波器的量测值。
(3)建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型;
步骤(3)中,建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型,具体为:
所确定的最大斜距差所在时刻为tk,参考位置的时间为tk+i,分别对应的惯导位置为
Figure RE-FDA0003327518920000014
Figure RE-FDA0003327518920000015
此时惯导对应的误差状态为
Figure RE-FDA0003327518920000016
Figure RE-FDA0003327518920000017
通过状态转移矩阵统一不同时刻的误差状态,得到:
Figure RE-FDA0003327518920000018
其中,
Figure RE-FDA0003327518920000019
考虑惯导误差情况下,滑动窗内的最大斜距差表示为:
Figure RE-FDA0003327518920000021
其中,
Figure RE-FDA0003327518920000022
表示声源位置;
Figure RE-FDA0003327518920000023
在地球直角坐标系中对位置进行求解,他们在地球坐标系e系下的相对位置矢量pe为:
Figure RE-FDA0003327518920000024
其中,
Figure RE-FDA0003327518920000025
RN为卯酉圈曲率半径,e为椭圆偏心率;
将pe转换到导航系下,其矢量表示为:
Figure RE-FDA0003327518920000026
其中,
Figure RE-FDA0003327518920000027
表示由地球坐标系到导航系的坐标转换矩阵;
考虑惯导位置误差的情况下,
Figure RE-FDA0003327518920000028
表示为:
Figure RE-FDA0003327518920000029
其中,
Figure RE-FDA00033275189200000210
表示
Figure RE-FDA00033275189200000211
的误差,δpn表示惯导位置误差,Ce表示为:
Figure RE-FDA00033275189200000212
其中,RNh=RN+h,[L λ h]分别表示经度、纬度和高度。
BS表示为
Figure RE-FDA00033275189200000213
其中,Sa=sin(a),Sa=cos(a),a=L或λ;
因此,斜距差表示为
Figure RE-FDA00033275189200000214
其中,
Figure RE-FDA00033275189200000215
表示tk时刻位置误差,
Figure RE-FDA00033275189200000216
表示tk+i时刻位置误差,Ce,k+i表示tk+i时刻的Ce矩阵值,
Figure RE-FDA0003327518920000031
表示tk+i时刻的
Figure RE-FDA00033275189200000317
矩阵值,BS,k+i表示tk+i时刻的BS矩阵值,
Figure RE-FDA0003327518920000032
Figure RE-FDA0003327518920000033
表示tk+i时刻的pe矩阵值,Ce,k表示tk时刻的Ce矩阵值,
Figure RE-FDA0003327518920000034
表示tk时刻的
Figure RE-FDA0003327518920000035
矩阵值,BS,k表示tk时刻的BS矩阵值,
Figure RE-FDA0003327518920000036
Figure RE-FDA0003327518920000037
Figure RE-FDA0003327518920000038
表示tk时刻的pe矩阵值,r1表示声源与tk时刻船的位置之间的真实距离,r2表示声源与tk+i时刻船的位置之间的真实距离;
测量得到的斜距差表示为:
Figure RE-FDA0003327518920000039
其中,δr表示测量的误差;
因此,基于斜距差的量测方程表示为:
Figure RE-FDA00033275189200000310
其中,H1=[01×6 HPD1 01×6],
Figure RE-FDA00033275189200000311
H2=[01×6 HPD2 01×6],
Figure RE-FDA00033275189200000312
Figure RE-FDA00033275189200000313
{*}7:9表示向量的第7-9列,
Figure RE-FDA00033275189200000314
(4)根据(1)-(3)建立的状态方程以及量测方程,利用卡尔曼滤波方法进行误差的更新与反馈。
2.根据权利要求书1所述一种基于斜距差匹配的惯性/单声源被动组合导航方法,其特征在于:步骤(4)中,具体为:
(a)状态一步预测:
Figure RE-FDA00033275189200000315
中,
Figure RE-FDA00033275189200000316
表示k-1时刻的状态估计,Fk-1表示k-1时刻到k时刻的状态转移矩阵;
(b)状态一步预测均方误差:
Figure RE-FDA0003327518920000041
其中,Qk-1表示过程噪声矩阵,Pk-1表示k-1时刻的均方根误差;
(c)滤波增益:
Figure RE-FDA0003327518920000042
其中,Hk表示k时刻的量测矩阵,Rk表示量测噪声矩阵;
(d)状态估计:
Figure RE-FDA0003327518920000043
其中,zk表示观测量;
(e)状态估计均方误差:Pk=Pk-KkHkPk|(k-1)
CN202111019597.0A 2021-09-01 2021-09-01 一种基于斜距差匹配的惯性/单声源被动组合导航方法 Active CN113776525B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111019597.0A CN113776525B (zh) 2021-09-01 2021-09-01 一种基于斜距差匹配的惯性/单声源被动组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111019597.0A CN113776525B (zh) 2021-09-01 2021-09-01 一种基于斜距差匹配的惯性/单声源被动组合导航方法

Publications (2)

Publication Number Publication Date
CN113776525A true CN113776525A (zh) 2021-12-10
CN113776525B CN113776525B (zh) 2023-12-05

Family

ID=78840611

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111019597.0A Active CN113776525B (zh) 2021-09-01 2021-09-01 一种基于斜距差匹配的惯性/单声源被动组合导航方法

Country Status (1)

Country Link
CN (1) CN113776525B (zh)

Citations (4)

* 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水下导航定位方法
CN106908060A (zh) * 2017-02-15 2017-06-30 东南大学 一种基于mems惯性传感器的高精度室内定位方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法

Patent Citations (4)

* 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水下导航定位方法
CN106908060A (zh) * 2017-02-15 2017-06-30 东南大学 一种基于mems惯性传感器的高精度室内定位方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法

Non-Patent Citations (4)

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

Also Published As

Publication number Publication date
CN113776525B (zh) 2023-12-05

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN110031882B (zh) 一种基于sins/dvl组合导航系统的外量测信息补偿方法
CN107621264B (zh) 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN103217159B (zh) 一种sins/gps/偏振光组合导航系统建模及动基座初始对准方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN108594283B (zh) Gnss/mems惯性组合导航系统的自由安装方法
CN105910602B (zh) 一种组合导航方法
CN106643709B (zh) 一种海上运载体的组合导航方法及装置
CN103217699B (zh) 一种基于偏振信息的组合导航系统递推优化初始对准方法
CN110133692B (zh) 惯导技术辅助的高精度gnss动态倾斜测量系统及方法
CN104344836A (zh) 一种基于姿态观测的冗余惯导系统光纤陀螺系统级标定方法
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN111854747B (zh) 一种载体大机动情况下的dvl辅助sins粗对准方法
CN112050807B (zh) 一种基于时间同步补偿的sins_gnss组合导航方法
CN108303063B (zh) 一种高精度车载组合高程测量方法
CN115388884A (zh) 一种智能体位姿估计器联合初始化方法
CN111397603A (zh) 载体姿态动态情况下的惯性/多普勒动基座粗对准方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法
CN104501809A (zh) 一种基于姿态耦合的捷联惯导/星敏感器组合导航方法
CN116678406B (zh) 组合导航姿态信息确定方法、装置、终端设备及存储介质
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法

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