CN113776525A - 一种基于斜距差匹配的惯性/单声源被动组合导航方法 - Google Patents
一种基于斜距差匹配的惯性/单声源被动组合导航方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims description 49
- 238000005259 measurement Methods 0.000 claims description 24
- 239000000126 substance Substances 0.000 claims description 19
- 230000007704 transition Effects 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 5
- 230000007547 defect Effects 0.000 abstract description 3
- 230000005236 sound signal Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- 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
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling 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)中,建立惯性/单声源组合导航的状态方程模型:
将惯导的位置误差、速度误差、姿态误差作为系统状态量,得:
因此,组合导航的状态方程为:
其中,F(t)表示状态转移矩阵,根据捷联惯导误差方程设置,W(t)表示过程噪声矩阵;
(2)建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵;
步骤(2)中,建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵,具体为:
设置滑动窗窗口大小,窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵,以滑动窗内的最后一个惯导位置为参考位置,根据窗口内不同时刻位置下航行器接收到的声信号到达时间,计算相对于参考位置的到达时间差,根据已知的声速可以得到斜距差。在获得一组斜距差的基础上,确定一个最大斜距差以及其对应的信号接收时间,以最大斜距差作为滤波器的量测值。
(3)建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型;
步骤(3)中,建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型,具体为:
考虑惯导误差情况下,滑动窗内的最大斜距差表示为:
将pe转换到导航系下,其矢量表示为:
其中,RNh=RN+h,[L λ h]分别表示经度、纬度和高度。
BS表示为
其中,Sa=sin(a),Sa=cos(a),a=L或λ;
因此,斜距差表示为
其中,表示tk时刻位置误差,表示tk+i时刻位置误差,Ce,k+i表示tk+i时刻的Ce矩阵值,表示tk+i时刻的矩阵值,BS,k+i表示tk+i时刻的BS矩阵值, 表示tk+i时刻的pe矩阵值,Ce,k表示tk时刻的Ce矩阵值,表示tk时刻的矩阵值,BS,k表示tk时刻的BS矩阵值,
测量得到的斜距差表示为:
其中,δr表示测量的误差;
因此,基于斜距差的量测方程表示为:
(4)根据(1)-(3)建立的状态方程以及量测方程,利用卡尔曼滤波方法进行误差的更新与反馈。
作为本发明进一步改进,步骤(4)中,具体为:
其中,Qk-1表示过程噪声矩阵,Pk-1表示k-1时刻的均方根误差;
其中,Hk表示k时刻的量测矩阵,Rk表示量测噪声矩阵;
其中,zk表示观测量;
(e)状态估计均方误差:Pk=Pk-KkHkPk|(k-1)。
有益效果:
根据以上步骤所建立的基于斜距差匹配的惯性/单声源被动组合导航定位方法,可以克服传统主动式定位中,信息易暴露,定位隐蔽性差的缺点,提高特殊应用环境下的定位安全性、隐蔽性。
附图说明
图1为滑动窗内虚拟基阵示意图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述:
本发明旨在提供一种基于斜距差匹配的惯性/单声源被动组合导航方法,通过设置滑动窗,根据窗口内不同时刻接收到声信号的惯导位置建立虚拟基阵,选定滑动窗最后一帧窗口的位置为参考基元位置,并计算各个位置相对参考基元位置的时间达到差,根据时间到达差建立卡尔曼滤波的量测模型,以此达到高精度的组合导航的目的。
本发明针对传统主动式声学定位信息易暴露的缺点,设计了基于斜距差匹配的惯性/单声源被动组合导航定位方法,其中滑动窗内虚拟基阵示意图如图1所示,
具体方法如下:
步骤1:建立惯性/单声源组合导航的状态方程模型,具体为:
将惯导的位置误差、速度误差、姿态误差作为系统状态量,得:
因此,组合导航的状态方程为:
其中,F(t)表示状态转移矩阵,根据捷联惯导误差方程设置,W(t)表示过程噪声矩阵。
步骤2:建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵,具体为:
设置滑动窗窗口大小,窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵。以滑动窗内的最后一个惯导位置为参考位置,根据窗口内不同时刻位置下航行器接收到的声信号到达时间,计算相对于参考位置的到达时间差,根据已知的声速可以得到斜距差。在获得一组斜距差的基础上,确定一个最大斜距差以及其对应的信号接收时间,以最大斜距差作为滤波器的量测值。
步骤3:建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型,具体为;
考虑惯导误差情况下,滑动窗内的最大斜距差可以表示为:
将pe转换到导航系下,其矢量表示为:
其中,RNh=RN+h,[L λ h]分别表示经度、纬度和高度。
BS表示为
其中,Sa=sin(a),Sa=cos(a),a=L或λ。
因此,斜距差可以表示为
其中,表示tk时刻位置误差,表示tk+i时刻位置误差。Ce,k+i表示tk+i时刻的Ce矩阵值,表示tk+i时刻的矩阵值,BS,k+i表示tk+i时刻的BS矩阵值, 表示tk+i时刻的pe矩阵值。Ce,k表示tk时刻的Ce矩阵值,表示tk时刻的矩阵值,BS,k表示tk时刻的BS矩阵值, 表示tk时刻的pe矩阵值。r1表示声源与tk时刻船的位置之间的真实距离,r2表示声源与tk+i时刻船的位置之间的真实距离
测量得到的斜距差可以表示为:
其中,δr表示测量的误差。
因此,基于斜距差的量测方程可以表示为:
步骤4:根据步骤1-3建立的状态方程以及量测方程,利用卡尔曼滤波方法进行误差的更新与反馈,具体为;
其中,Qk-1表示过程噪声矩阵,Pk-1表示k-1时刻的均方根误差。
其中,Hk表示k时刻的量测矩阵,Rk表示量测噪声矩阵。
其中,zk表示观测量。
(e)状态估计均方误差:Pk=Pk-KkHkPk|(k-1)。
以上所述,仅是本发明的较佳实施例之一,并非是对本发明作任何其他形式的限制,而依据本发明的技术实质所作的任何修改或等同变化,仍属于本发明所要求保护的范围。
Claims (2)
1.一种基于斜距差匹配的惯性/单声源被动组合导航方法,其特征在于,包括如下步骤:
(1)建立惯性/单声源组合导航的状态方程模型;
步骤(1)中,建立惯性/单声源组合导航的状态方程模型:
将惯导的位置误差、速度误差、姿态误差作为系统状态量,得:
因此,组合导航的状态方程为:
其中,F(t)表示状态转移矩阵,根据捷联惯导误差方程设置,W(t)表示过程噪声矩阵;
(2)建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵;
步骤(2)中,建立滑动窗,在滑动窗内基于惯导的位置构建虚拟基阵,具体为:
设置滑动窗窗口大小,窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵,以滑动窗内的最后一个惯导位置为参考位置,根据窗口内不同时刻位置下航行器接收到的声信号到达时间,计算相对于参考位置的到达时间差,根据已知的声速可以得到斜距差。在获得一组斜距差的基础上,确定一个最大斜距差以及其对应的信号接收时间,以最大斜距差作为滤波器的量测值。
(3)建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型;
步骤(3)中,建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型,具体为:
考虑惯导误差情况下,滑动窗内的最大斜距差表示为:
将pe转换到导航系下,其矢量表示为:
其中,RNh=RN+h,[L λ h]分别表示经度、纬度和高度。
BS表示为
其中,Sa=sin(a),Sa=cos(a),a=L或λ;
因此,斜距差表示为
其中,表示tk时刻位置误差,表示tk+i时刻位置误差,Ce,k+i表示tk+i时刻的Ce矩阵值,表示tk+i时刻的矩阵值,BS,k+i表示tk+i时刻的BS矩阵值, 表示tk+i时刻的pe矩阵值,Ce,k表示tk时刻的Ce矩阵值,表示tk时刻的矩阵值,BS,k表示tk时刻的BS矩阵值,
测量得到的斜距差表示为:
其中,δr表示测量的误差;
因此,基于斜距差的量测方程表示为:
(4)根据(1)-(3)建立的状态方程以及量测方程,利用卡尔曼滤波方法进行误差的更新与反馈。
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)
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紧组合导航定位方法 |
-
2021
- 2021-09-01 CN CN202111019597.0A patent/CN113776525B/zh active Active
Patent Citations (4)
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)
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 |