CN114136323A - 基于运动补偿下tdoa量测的auv协同导航方法 - Google Patents
基于运动补偿下tdoa量测的auv协同导航方法 Download PDFInfo
- Publication number
- CN114136323A CN114136323A CN202111430697.2A CN202111430697A CN114136323A CN 114136323 A CN114136323 A CN 114136323A CN 202111430697 A CN202111430697 A CN 202111430697A CN 114136323 A CN114136323 A CN 114136323A
- Authority
- CN
- China
- Prior art keywords
- auv
- slave
- time
- collaborative navigation
- main
- 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
- 238000005259 measurement Methods 0.000 title claims abstract description 38
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000005236 sound signal Effects 0.000 claims abstract description 31
- 238000001914 filtration Methods 0.000 claims abstract description 22
- 238000004364 calculation method Methods 0.000 claims description 8
- 239000013598 vector Substances 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 5
- 230000001360 synchronised effect Effects 0.000 description 10
- 238000004891 communication Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000002457 bidirectional effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000003631 expected effect Effects 0.000 description 1
- 230000008092 positive 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/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
- 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/1654—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 electromagnetic compass
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
-
- 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
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Electromagnetism (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明涉及基于运动补偿下TDOA量测的AUV协同导航方法,能够在无精确时钟同步的条件下,以从AUV的时钟***为基准,测量任意两个主AUV到从AUV水声信号到达时刻的时间差并根据水下声速计算距离差,并考虑从AUV运动的影响,根据从AUV的实测运动状态补偿由于载体运动对距离差引入的误差,以此建立协同导航基于载体运动补偿下的TDOA量测模型,最后根据从AUV传感器误差模型或者运动学模型建立协同导航状态方程,两者共同构成协同导航的非线性滤波模型。
Description
技术领域
本发明属于水下航行器协同导航技术领域,尤其是基于运动补偿下TDOA量测的AUV协同导航方法。
背景技术
随着人类对海洋探索和开发越来越深入,以及未来对无人化作战的需求,单兵作业和作战逐渐无法适应未来海洋开发和海战的需求,因此集群AUV协同作业和作战越来越引起人们的重视。为了给集群AUV精确制导提供准确的位置保障,精准的协同导航技术是前提条件。当集群AUV出现时钟不同步时,如果时钟误差超过一定量级,会使得基于同步测距的协同导航量测误差急剧提升甚至无效,而本文所提的基于运动补偿下TDOA量测的AUV协同导航方法可以有效解决集群AUV之间时钟不同步的问题。
水声测距的同步信标模式需要控制时钟同步在一定误差范围内,当集群AUV之间出现较大时钟偏差时,会导致水声时延数据产生较大误差,此时直接利用水声时延和水下声速将无法准确得到精确的距离信息。对于时钟不同步的解决办法,现有的方案为水声测距采用双程应答模式,从AUV发出应答请求,主AUV接收到应答信号后回发声信号,由于受双向水声通信链路高时延、通信链路不稳定以及载体隐蔽性因素的影响,采取主动发声的应答模式往往无法获得预期效果,而采取基于同步信标模式的被动接收模式在各AUV之间时钟偏差较大的情况下,将会出现较大的距离测量误差。由于水下声速相比陆地无线电的速度小了5个数量级,所以水下TDOA量测无法忽略载体运动的影响,需补偿由于从AUV运动而引入距离差计算的误差。
发明内容
本发明的目的在于克服现有技术的不足,提出基于运动补偿下TDOA量测的AUV协同导航方法,利用从AUV作为同一时间基准,以从AUV接收到任意两个主AUV水声信号的时差作为量测,并补偿由于从AUV运动而引入的距离差计算的误差,该方法可以很好解决时钟不同步的问题。
本发明解决其技术问题是采取以下技术方案实现的:
基于运动补偿下TDOA量测的AUV协同导航方法,包括以下步骤:
步骤1、从AUV采集来自多个主AUV的水声信号并标以时间戳,同时解码各个主AUV的位置信息;
步骤2、将从AUV最先收到声信号的时刻作为基准,同时将最先收到声信号的主AUV设定为主AUV1,分别计算声信号从其它主AUV到达从AUV的时刻与声信号从主AUV1到达从AUV时刻的时间差;根据声速和时间差,得到主AUV1到从AUV之间的距离与其它主AUV到从AUV之间距离的差值;
步骤3、考虑从AUV在此过程中由于自身运动而引入的距离差计算的误差,对距离差计算进行补偿,并将补偿后的距离差作为建立从AUV协同导航非线性滤波模型的量测方程;
步骤4、根据AUV的传感器误差模型或者运动学模型建立状态方程,并结合步骤3中的量测方程建立基于时差量测的协同导航非线性滤波模型;
步骤5、根据步骤4得到的滤波模型选择任意一种非线性滤波算法对对下一时刻位置进行估计滤波,得到协同导航估计结果。
而且,所述步骤1的具体实现方法为:各个主AUV向外发射声信号,并广播自身位置信息,从AUV接收到各个主AUV的声信号后,解码获得各个主AUV的位置信息以及声信号到达时刻的时间戳其中k为时间序列,m表示主AUV。
而且,所述步骤4中建立的状态方程和量测方程为:
其中,m表示主AUV,s表示从AUV,为时刻第i个从AUV的位置状态,为tk时刻第i个从AUV的位置状态,通过从AUV传感器误差模型或者运动学模型建立,为过程噪声序列,为量测噪声序列,ΔL为补偿后的距离差,针对上述的状态方程和量测方程采用非线性滤波的方法对从AUV的状态进行估计,从而得到tk+1时刻从AUV状态的估计结果
本发明的优点和积极效果是:
本发明能够在无精确时钟同步的条件下,以从AUV的时钟***为基准,测量任意两个主AUV到从AUV水声信号到达时刻的时间差并根据水下声速计算距离差,并考虑从AUV运动的影响,根据从AUV的实测运动状态补偿由于载体运动对距离差引入的误差,以此建立协同导航基于载体运动补偿下的TDOA量测模型,最后根据从AUV传感器误差模型或者运动学模型建立协同导航状态方程,两者共同构成协同导航的非线性滤波模型。
附图说明
图1为本发明的流程图;
图2为本发明的原理图;
图3为本发明距离差补偿原理图:
图4为本发明和传统方式下基于同步信标模式下的协同导航定位误差对比图。
具体实施方式
以下结合附图对本发明做进一步详述。
基于运动补偿下TDOA量测的AUV协同导航方法,如图1和图2所示,包括以下步骤:
步骤1、协同导航初始阶段,集群AUV时钟同步。
本步骤中,在协同导航的初始阶段,对所有AUV的本地时钟是否严格同步不做要求,即所有AUV本地时钟同步和非同步的基于运动补偿下TDOA量测的协同导航均属于本专利的保护范围,因为本地时钟是否同步对本专利所提方案的协同导航定位精度没有影响。
步骤2、集群AUV群体中,从AUV和各个主AUV之间正常通信,以从AUV自身时钟***为基准,从AUV采集来自多个主AUV的水声信号并标以时间戳,同时解码各个主AUV的位置信息。
步骤2、将从AUV最先收到声信号的时刻作为基准,同时将最先收到声信号的主AUV设定为主AUV1,分别计算声信号从其它主AUV到达从AUV的时刻与声信号从主AUV1到达从AUV时刻的时间差;根据声速和时间差,得到主AUV1到从AUV之间的距离与其它主AUV到从AUV之间距离的差值。
本步骤的具体实现方法为:将从AUV最先收到主AUV发来声信号的时刻作为基准同时将最先收到声信号的主AUV设定为主AUV1,分别计算从AUV接收到其它主AUV声信号到达时刻与声信号从主AUV1到从AUV到达时刻的差值:根据声速c和步骤2中的时间差,得到主AUV1到从AUV之间的距离与其它主AUV到从AUV之间距离的差值,即
步骤4、考虑从AUV在此过程中由于自身运动而引入的距离差计算的误差,对距离差计算进行补偿,并将补偿后的距离差作为建立从AUV协同导航非线性滤波模型的量测方程。
如图3所示,计算从AUV到任意两个主AUV之间的距离差时,需要补偿由于从AUV运动而引入的距离差计算的误差。从AUV接收到距离较近的主AUV1水声信号的时刻为且从AUV和主AUV1的方向向量可由主AUV1的位置和当前从AUV的预测位置计算得到。而接收到距离较远的主AUV2的水声信号时刻为且假设在与期间从AUV的运动近似为直线航行,从AUV的运动方向向量并可由从AUV携带的惯性导航设备或者其他导航设备计算得到,那么和夹角的余弦表示为:
步骤5、根据AUV的传感器误差模型或者运动学模型建立状态方程,结合量测方程建立基于时差量测的协同导航非线性滤波模型。
采用扩展卡尔曼滤波的协同导航算法,将量测方程线性化,得到:
其中,s表示从AUV,为tk+1时刻第i个从AUV的位置状态,为tk时刻第i个从AUV的位置状态,通过从AUV传感器误差模型或者运动学模型建立,为过程噪声序列,为量测噪声序列,ΔL为补偿后的距离差,针对上述的状态方程和量测方程采用非线性滤波的方法对从AUV的状态进行估计,从而得到tk+1时刻从AUV状态的估计结果
采用扩展卡尔曼滤波的协同导航算法,将量测方程线性化:
即:
建立协同导航从AUV的状态方程如下,在此以装备DVL和磁罗经为例,若载体装备惯性导航设备,则状态方程则为惯导误差方程。
以扩展卡尔曼滤波(EKF)为例,一步状态预测为:
一步协方差预测为:
求取滤波增益:
步骤6、根据步骤5得到的滤波模型选择任意一种非线性滤波算法对对下一时刻位置进行估计滤波,得到协同导航估计结果。
状态更新方程为:
设置仿真时间:20s;滤波周期:1s;从AUV初始位置:(1000,100);时钟漂移:0.1s、0.3s;主AUV数量:6条;仿真结果如图4所示,实线为采用基于同步测距的协同导航方法得到的定位曲线,虚线则采用基于运动补偿下TDOA量测得到的定位曲线,通过仿真结果对比可知,当集群AUV之间存在较大的时钟偏差时,采用本专利的一种基于运动补偿下TDOA量测的协同导航方法可以比基于同步测距的协同导航方法得到更高的定位精度,且时钟偏差大小对于本专利的定位精度没有影响,而时钟偏差程度越大,采用基于同步测距的协同导航方法的定位误差将越大。
需要强调的是,本发明所述的实施例是说明性的,而不是限定性的,因此本发明包括并不限于具体实施方式中所述的实施例,凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式,同样属于本发明保护的范围。
Claims (4)
1.基于运动补偿下TDOA量测的AUV协同导航方法,其特征在于:包括以下步骤:
步骤1、从AUV采集来自多个主AUV的水声信号并标以时间戳,同时解码各个主AUV的位置信息;
步骤2、将从AUV最先收到声信号的时刻作为基准,同时将最先收到声信号的主AUV设定为主AUV1,分别计算声信号从其它主AUV到达从AUV的时刻与声信号从主AUV1到达从AUV时刻的时间差;根据声速和时间差,得到主AUV1到从AUV之间的距离与其它主AUV到从AUV之间距离的差值;
步骤3、考虑从AUV在此过程中由于自身运动而引入的距离差计算的误差,对距离差计算进行补偿,并将补偿后的距离差作为建立从AUV协同导航非线性滤波模型的量测方程;
步骤4、根据AUV的传感器误差模型或者运动学模型建立状态方程,并结合步骤3中的量测方程建立基于时差量测的协同导航非线性滤波模型;
步骤5、根据步骤4得到的滤波模型选择任意一种非线性滤波算法对对下一时刻位置进行估计滤波,得到协同导航估计结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111430697.2A CN114136323B (zh) | 2021-11-29 | 2021-11-29 | 基于运动补偿下tdoa量测的auv协同导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111430697.2A CN114136323B (zh) | 2021-11-29 | 2021-11-29 | 基于运动补偿下tdoa量测的auv协同导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114136323A true CN114136323A (zh) | 2022-03-04 |
CN114136323B CN114136323B (zh) | 2023-06-20 |
Family
ID=80388856
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111430697.2A Active CN114136323B (zh) | 2021-11-29 | 2021-11-29 | 基于运动补偿下tdoa量测的auv协同导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114136323B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116222578A (zh) * | 2023-05-04 | 2023-06-06 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101520502A (zh) * | 2009-03-24 | 2009-09-02 | 中国航空无线电电子研究所 | 对无线传感器网络移动节点的跟踪定位方法 |
CN103781095A (zh) * | 2012-10-23 | 2014-05-07 | 华为技术有限公司 | 一种tdoa测量误差的校正方法、传输点和*** |
CN105445722A (zh) * | 2015-11-09 | 2016-03-30 | 哈尔滨工程大学 | 一种适用于多auv协同导航的动态条件下水声双程测距误差补偿方法 |
CN105929405A (zh) * | 2016-04-15 | 2016-09-07 | 燕山大学 | 异步时钟下水下移动目标协同定位方法 |
CN109905846A (zh) * | 2019-02-18 | 2019-06-18 | 天津城建大学 | 一种基于自主水下航行器的水下无线传感器网络定位方法 |
CN109974706A (zh) * | 2019-03-08 | 2019-07-05 | 哈尔滨工程大学 | 一种基于双运动模型的主从式多auv协同导航方法 |
CN110087307A (zh) * | 2019-04-28 | 2019-08-02 | 青岛科技大学 | 基于测距修正的水下传感器网络定位方法 |
-
2021
- 2021-11-29 CN CN202111430697.2A patent/CN114136323B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101520502A (zh) * | 2009-03-24 | 2009-09-02 | 中国航空无线电电子研究所 | 对无线传感器网络移动节点的跟踪定位方法 |
CN103781095A (zh) * | 2012-10-23 | 2014-05-07 | 华为技术有限公司 | 一种tdoa测量误差的校正方法、传输点和*** |
CN105445722A (zh) * | 2015-11-09 | 2016-03-30 | 哈尔滨工程大学 | 一种适用于多auv协同导航的动态条件下水声双程测距误差补偿方法 |
CN105929405A (zh) * | 2016-04-15 | 2016-09-07 | 燕山大学 | 异步时钟下水下移动目标协同定位方法 |
CN109905846A (zh) * | 2019-02-18 | 2019-06-18 | 天津城建大学 | 一种基于自主水下航行器的水下无线传感器网络定位方法 |
CN109974706A (zh) * | 2019-03-08 | 2019-07-05 | 哈尔滨工程大学 | 一种基于双运动模型的主从式多auv协同导航方法 |
CN110087307A (zh) * | 2019-04-28 | 2019-08-02 | 青岛科技大学 | 基于测距修正的水下传感器网络定位方法 |
Non-Patent Citations (1)
Title |
---|
葛斌 等: "基于TDOA改进的矿山井下机车定位方法" * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116222578A (zh) * | 2023-05-04 | 2023-06-06 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
CN116222578B (zh) * | 2023-05-04 | 2023-08-29 | 山东大学 | 基于自适应滤波和最优平滑的水下组合导航方法及*** |
Also Published As
Publication number | Publication date |
---|---|
CN114136323B (zh) | 2023-06-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CA2565143C (en) | System and method for aligning multiple navigation components | |
CN111189441B (zh) | 一种多源自适应容错联邦滤波组合导航***及导航方法 | |
CN110057365B (zh) | 一种大潜深auv下潜定位方法 | |
CN105319534B (zh) | 一种基于水声双程测距的多auv协同定位方法 | |
Eustice et al. | Recent advances in synchronous-clock one-way-travel-time acoustic navigation | |
CN111595348B (zh) | 一种自主水下航行器组合导航***的主从式协同定位方法 | |
CN111366962A (zh) | 一种深远海低成本长航时协同导航定位*** | |
CN112097763A (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
JP2002090439A (ja) | Gps受信装置 | |
US20220342086A1 (en) | Position measurement system, positioning calculation apparatus, position measurement method and program | |
CN111505584B (zh) | 基于相对速度和多普勒效应的音频信号到达时间修正方法 | |
CN104197939A (zh) | 基于水下信息网络的多参考节点水下航行器组合导航方法 | |
CN105066993A (zh) | Lbl/mins组合导航***及其导航信息融合方法 | |
CN105445722A (zh) | 一种适用于多auv协同导航的动态条件下水声双程测距误差补偿方法 | |
CN111578944B (zh) | 一种基于单信标的水下滑翔机定位方法 | |
US7406379B2 (en) | System for interferometric sensing | |
CN114136323B (zh) | 基于运动补偿下tdoa量测的auv协同导航方法 | |
CN111174774A (zh) | 一种某深度水层面模式下的导航信息融合方法及*** | |
Zhang et al. | Integration of communication, positioning, navigation and timing for deep-sea vehicles | |
CN110764533A (zh) | 一种多水下机器人协同目标搜索方法 | |
CN108007457B (zh) | 一种基于细分时间片的监控导航***异步数据融合方法 | |
CN103968842A (zh) | 一种提高基于mems陀螺的无人艇协同导航定位精度的方法 | |
Willners et al. | Moving baseline localization for multi-vehicle maritime operations | |
CN113280812B (zh) | 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 | |
CN111174813B (zh) | 一种基于外积补偿的auv动基座对准方法及*** |
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 |