CN117553787B - 水下无人航行器的协同导航方法、装置及系统 - Google Patents
水下无人航行器的协同导航方法、装置及系统 Download PDFInfo
- Publication number
- CN117553787B CN117553787B CN202410027822.2A CN202410027822A CN117553787B CN 117553787 B CN117553787 B CN 117553787B CN 202410027822 A CN202410027822 A CN 202410027822A CN 117553787 B CN117553787 B CN 117553787B
- Authority
- CN
- China
- Prior art keywords
- filter
- uuv
- navigation
- state
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000004891 communication Methods 0.000 claims abstract description 55
- 238000001914 filtration Methods 0.000 claims abstract description 49
- 238000005259 measurement Methods 0.000 claims description 20
- 238000004364 calculation method Methods 0.000 claims description 12
- 238000004422 calculation algorithm Methods 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 description 7
- 238000012545 processing Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000001934 delay Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000014509 gene expression Effects 0.000 description 2
- 238000011835 investigation Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 239000002131 composite material Substances 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 238000012217 deletion Methods 0.000 description 1
- 230000037430 deletion Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000000827 velocimetry Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
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
- 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/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04B—TRANSMISSION
- H04B13/00—Transmission systems characterised by the medium used for transmission, not provided for in groups H04B3/00 - H04B11/00
- H04B13/02—Transmission systems in which the medium consists of the earth or a large mass of water thereon, e.g. earth telegraphy
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/024—Guidance services
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明涉及水下无人航行器导航技术领域，具体公开了一种水下无人航行器的协同导航方法、装置及系统，包括：接收主UUV发送的协同信息；根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，当第一滤波器接收到的协同信息为脉冲信号时将第一滤波器根据协同信息获得的第一滤波器状态参数输出至第二滤波器，当第一滤波器接收到的协同信息为通信波信号时，将通信波信号传输的主UUV位置信息输出至第二滤波器；根据第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对第二滤波器进行扩展卡尔曼滤波更新，获得最终导航结果。本发明提供的水下无人航行器的协同导航方法能够有效提升导航精度并提升实时性。
Description
技术领域
本发明涉及水下无人航行器导航技术领域，尤其涉及一种水下无人航行器的协同导航方法、水下无人航行器的协同导航装置及协同导航系统。
背景技术
随着水下无人航行器（Unmanned Underwater Vehicle，UUV）的快速发展，其在海洋调查、海洋生态监测、水下探测与测绘等任务中得到广泛应用。且越来越多的任务需要多个UUV同时进行协调调查，推进了UUV集群协同的必要性。为了确保UUV的安全操作，获取可靠的定位导航信息至关重要。为了实现UUV集群最优的协同导航性能，UUV配备了复杂的惯性导航系统（Inertial Navigation System，INS）来获得可靠的自身定位信息。在没有外部参考的情况下，INS引起的定位导航误差会无限积累，需要融合如多普勒测速仪（DopplerVelocity Log，DVL）、USBL等外部传感器信息进行误差修正。然而，水下环境的时变与空变性、声速传播时延、信号多途等原因导致水下传输的量测信息具有一定延迟性和不固定性，这使得UUV集群协同导航无法实时使用量测信息进行误差修正，导航结果会出现精度波动大、可靠性差等的缺点，严重影响UUV 协同导航系统的工程应用。因此，解决UUV协同通信时延问题对高精度导航尤为重要。
为了克服水下UUV集群通信时延带来的导航精度低和不稳定的问题，现有技术中也存在多种方式，例如，（1）在卡尔曼滤波算法的框架下，将UUV集群通信时延转化为观测方程中的量测偏差，通过具有延迟的组合导航误差估计方法来提高导航定位精度；（2）用稀疏信息滤波代替卡尔曼滤波来保留过去状态的历史信息，并允许UUV根据历史信息更新当前位置，补偿通信延迟带来的定位误差；（3）采用联邦滤波器，能够实现延迟信息等多源信息融合，有效解决UUV间量测时延问题。
由于针对水下集群通信时延的大部分协同导航算法都要求存储时延期间的信息，再集中处理大量历史数据，这使得计算压力较大，上述现有技术中的算法无法解决该问题，从而可能影响导航解算的实时性；另外，上述现有技术中的部分算法针对特定情况，泛化性较差。
因此，如何能够同时提升水下无人航行器的协同导航的精度和实时性成为本领域技术人员亟待解决的技术问题。
发明内容
本发明提供了一种水下无人航行器的协同导航方法、水下无人航行器的协同导航装置及协同导航系统，解决相关技术中存在的水下无人航行器的协同导航精度低和且实时性差的问题。
作为本发明的第一个方面，提供一种水下无人航行器的协同导航方法，其中，应用于协同导航系统，所述协同导航系统包括主UUV和与主UUV通信连接的多个从UUV，针对每个从UUV，所述水下无人航行器的协同导航方法包括：
接收主UUV发送的协同信息，所述协同信息至少包括用于计算与主UUV之间距离信息的脉冲信号和包含主UUV位置信息的通信波信号；
根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，以及当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，当所述第一滤波器接收到的所述协同信息为所述通信波信号时，将所述通信波信号传输的主UUV位置信息输出至第二滤波器；
根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，获得最终导航结果。
进一步地，当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，包括：
当所述第一滤波器接收到的所述协同信息为所述脉冲信号时，根据所述脉冲信号计算与主UUV之间的距离；
将接收到所述脉冲信号时的第一滤波器的自身状态参数、自身惯性导航系统实时推算出的当前位置信息以及与主UUV之间的距离作为所述第一滤波器状态参数，并将所述第一滤波器状态参数输出至第二滤波器。
进一步地，根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，包括：
根据所述第一滤波状态参数中自身惯性导航系统实时推算出的当前位置信息与主UUV位置信息计算相对距离信息；
将所述相对距离信息和所述第一滤波状态参数中与主UUV之间的距离两者之间的差值与预设门限值进行比较；
若所述相对距离信息和所述第一滤波状态参数中与主UUV之间的距离两者之间的差值小于预设门限值，则判定所述协同信息中的脉冲信号和所述通信波信号相匹配；
当所述协同信息中的脉冲信号和所述通信波信号相匹配时，根据所述第一滤波器状态参数和所述主UUV位置信息对所述第二滤波器进行扩展卡尔曼滤波量测更新；
根据所述自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波状态更新。
进一步地，还包括：
将所述第一滤波器状态参数和主UUV位置信息存储至第二滤波器，且将自身惯性导航系统的实时导航结果按照接收时间进行持续存储。
进一步地，还包括：
判断第二滤波器的状态是否更新至最新状态；
若所述第二滤波器的状态不是更新至最新状态，则根据存储的自身惯性导航系统的实时导航结果对所述第二滤波器的状态进行更新；
若所述第二滤波器的状态更新至最新状态，则停止对所述自身惯性导航系统的实时导航结果的存储，并将所述第二滤波器中存储的当级数据均删除。
进一步地，还包括：
若所述第二滤波器的状态更新至最新状态，则根据第二滤波器的状态参数对第一滤波器的状态进行更新，获得最终导航结果。
进一步地，还包括：
若所述相对距离信息和所述第一滤波状态参数中与主UUV之间的距离两者之间的差值大于预设门限值，则判断所述第一滤波状态参数中与主UUV之间的距离的存储时间是否大于预设时间；
若所述第一滤波状态参数中与主UUV之间的距离的存储时间大于预设时间，则停止对所述自身惯性导航系统的实时导航结果的存储，并将所述第二滤波器中存储的当级数据均删除。
进一步地，根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，包括：
根据自身惯性导航系统输出的加速度计值和陀螺值进行惯性解算算法计算，获得导航解算信息；
根据所述导航解算信息对所述第一滤波器进行扩展卡尔曼滤波状态更新。
作为本发明的另一个方面，提供一种水下无人航行器的协同导航装置，其中，应用于协同导航系统，所述协同导航系统包括主UUV和与主UUV通信连接的多个从UUV，针对每个从UUV，所述水下无人航行器的协同导航装置包括：
接收模块，用于接收主UUV发送的协同信息，所述协同信息至少包括用于计算与主UUV之间距离信息的脉冲信号和包含主UUV位置信息的通信波信号；
第一滤波器更新模块，用于根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，以及当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，当所述第一滤波器接收到的所述协同信息为所述通信波信号时，将所述通信波信号传输的主UUV位置信息输出至第二滤波器；
第二滤波器更新模块，用于根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，获得最终导航结果。
作为本发明的另一个方面，提供一种协同导航系统，其中，包括主UUV和主UUV通信连接的多个从UUV，每个所述从UUV均包括前文所述的水下无人航行器的协同导航装置。
本发明提供的水下无人航行器的协同导航方法，通过从UUV自身惯性导航系统的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，并实时输出导航结果，从而能够保证导航的实时性；在第一滤波器的基础上对第二滤波器进行扩展卡尔曼滤波更新，从而能够保证导航的精度。因此本发明的这种水下无人航行器的协同导航方法能够面对复杂的通信时延形式，如协同信息顺序、错序和丢失等情况，能够在多种场景下实现协同导航的高精度和实时性。
附图说明
附图是用来提供对本发明的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明，但并不构成对本发明的限制。
图1为现有技术中协同信息存在时延传播的示意图。
图2为本发明提供的水下无人航行器的协同导航方法的流程图。
图3为本发明提供的协同导航系统的架构图。
图4为本发明提供的对第一滤波器进行扩展卡尔曼滤波状态更新的流程图。
图5为本发明提供的第一滤波器接收到脉冲信号时的处理流程图。
图6为本发明提供的第二滤波器和第二滤波器的具体工作流程图。
图7为本发明提供的对第二滤波器进行扩展卡尔曼滤波更新的流程图。
图8为本发明提供的水下无人航行器的协同导航装置的结构框图。
图9为本发明提供的协同导航系统的工作流程图。
具体实施方式
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互结合。下面将参考附图并结合实施例来详细说明本发明。
为了使本领域技术人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包括，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
在协同导航系统中，如图1所示，作为主节点的主UUV向作为从节点的从UUV传输的数据包括声脉冲信号和通信波信号，两类信号的通信时延都受到传输时延和信号处理等影响。脉冲信号信息量小，处理简单，故信号处理时延可忽略；通信波信号因其较大的数据量等因素，使得信号处理时间较长，通信时延较大，极易发生信号错序以及因错序导致的信号丢失情况。
因此，在本实施例中提供了一种水下无人航行器的协同导航方法，图1是根据本发明实施例提供的水下无人航行器的协同导航方法的流程图，应用于协同导航系统，所述协同导航系统包括主UUV和与主UUV通信连接的多个从UUV，针对每个从UUV，如图2所示，所述水下无人航行器的协同导航方法包括：
S100、接收主UUV发送的协同信息，所述协同信息至少包括用于计算与主UUV之间距离信息的脉冲信号和包含主UUV位置信息的通信波信号；
在本发明实施例中，结合图3所示，所述协同导航系统包括主UUV和与主UUV通信连接的多个从UUV，此处需要说明的是，在协同导航系统中，通常将配备高精度的惯性导航系统的UUV作为主UUV，而配备精度低的惯性导航系统的UUV作为从UUV，因此，从UUV需要借助于主UUV发送的数据进行导航定位。
主UUV能够向从UUV发送的协同信息具体可以包括主UUV的位置信息以及主从UUV之间的距离相关信息，该协同信息具体通过两类水声信号传递，其中一类是用于形成距离量测的声脉冲信号，另一类是包含有主UUV位置信息的通信波。
应当理解的是，由于声脉冲信号的信号周期短且实时性高，因此能够根据声脉冲信号的发射与接收时间计算出主从UUV之间的距离。
S200、根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，以及当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，当所述第一滤波器接收到的所述协同信息为所述通信波信号时，将所述通信波信号传输的主UUV位置信息输出至第二滤波器；
在本发明实施例中，作为从UUV，其自身惯性导航系统能够实时输出实时导航结果，根据该实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新。此处针对第一滤波器的更新仅限于状态更新。
此外，还需要根据第一滤波器接收到的协同信息是脉冲信号还是通信波信号分别做不同的处理。
当第一滤波器接收到的协同信息为脉冲信号时，是能够根据该脉冲信号获得包括主从UUV距离在内的第一滤波器状态参数，并将该第一滤波器状态参数输出至第二滤波器；当第一滤波器接收到的协同信息为通信波信号时，根据该通信波信号能够获得主UUV位置信息，因此将主UUV位置信息输出至第二滤波器。
S300、根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，获得最终导航结果。
在第二滤波器接收到第一滤波器状态参数以及主UUV位置信息后，还能够结合自身惯性导航系统的实时导航结果对第二滤波器进行扩展卡尔曼滤波更新，此处针对第二滤波器的更新既包量测更新也包括状态更新，获得最终导航结果。
综上，本发明提供的水下无人航行器的协同导航方法，通过从UUV自身惯性导航系统的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，并实时输出导航结果，从而能够保证导航的实时性；在第一滤波器的基础上对第二滤波器进行扩展卡尔曼滤波更新，从而能够保证导航的精度。因此本发明的这种水下无人航行器的协同导航方法能够面对复杂的通信时延形式，如协同信息顺序、错序和丢失等情况，能够在多种场景下实现协同导航的高精度和实时性。
具体地，如图4所示，根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，包括：
S210、根据自身惯性导航系统输出的加速度计值和陀螺值进行惯性解算算法计算，获得导航解算信息；
在本发明实施例中，根据从UUV自身配备的自身惯性导航系统输出的加速度计值和陀螺值进行惯性解算，获得导航结算信息，并依据该解算信息对第一滤波器进行状态更新。
S220、根据所述导航解算信息对所述第一滤波器进行扩展卡尔曼滤波状态更新。
在本发明实施例中，从UUV通过惯性导航系统输出的实时导航结果，并同步对第一滤波器进行扩展卡尔曼滤波状态更新。令从UUV的位置为分别表示从UUV的经度、纬度和深度。其中深度可由深度传感器获取，故不进行状态误差估计，仅在计算主从UUV间的距离时需使用。假设每隔时间T进行从UUV的位置更新，则从UUV的运动方程可表示为：
，
其中，、/>和/>分别表示/>时刻从UUV的经度、纬度和航向，/>和/>分别表示在/>时刻从UUV的前向合成速度和航向角速度。由于实际中存在噪声影响，假设噪声为高斯白噪声，令/>和/>的合并向量为：
，
扩展卡尔曼滤波非线性系统状态空间模型为：
，
其中，，/>表示扩展卡尔曼滤波非线性系统噪声转移矩阵，/>表示零均值高斯噪声向量，且/>表示非线性项。扩展卡尔曼滤波非线性系统噪声/>的协方差矩阵/>定义为：
，
在第一滤波器上进行扩展卡尔曼滤波状态更新：
，
其中，表示根据/>和扩展卡尔曼滤波非线性系统状态方程对当前时刻状态/>的最优估计。/>表示最优估计值的均方误差阵，/>表示/>关于/>的雅可比矩阵，/>表示/>关于/>的雅可比矩阵，表达式分别如下：
，
。
具体地，如图5所示，当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，包括：
S230、当所述第一滤波器接收到的所述协同信息为所述脉冲信号时，根据所述脉冲信号计算与主UUV之间的距离；
S240、将接收到所述脉冲信号时的第一滤波器的自身状态参数、自身惯性导航系统实时推算出的当前位置信息以及与主UUV之间的距离作为所述第一滤波器状态参数，并将所述第一滤波器状态参数输出至第二滤波器。
结合图6所示，若从UUV接收到主UUV发送的脉冲信号，则利用脉冲信号计算出与主UUV即发送端之间的距离，并将接收到脉冲信号时刻的第一滤波器A的自身状态参数（如误差状态/>和均方根误差值/>）、自身惯性导航系统实时推算出的从UUV位置/>和距离/>在本发明实施例中作为第一滤波器状态参数均均发送给第二滤波器B；若从UUV接收到包含发送端位置/>的通信波信号时，则将/>发送给第二滤波器B。其中作为发送端的主UUV经度、纬度和深度位置/>，作为接收端的从UUV的位置/>。
在本发明实施例中，如图7所示，根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，包括：
S310、根据所述第一滤波状态参数中自身惯性导航系统实时推算出的当前位置信息与主UUV位置信息计算相对距离信息；
结合图6所示，利用第一滤波器A自身惯性导航系统实时推算出的当前位置信息和主UUV位置信息/>计算相对距离信息/>。
具体地，在本发明实施例中，从UUV通过接收到的延迟通信波进行第二滤波器B的扩展卡尔曼滤波异步量测更新，获取异步量测的组合导航结果。若第二滤波器B接收到第一滤波器A的滤波状态、从UUV状态和收发端距离，则进行存储并开始持续存储加速度计值和陀螺值/>。若第二滤波器B接收到发送端位置/>，则利用/>和接收端位置/>计算相对距离信息/>：
。
S320、将所述相对距离信息和所述第一滤波状态参数中与主UUV之间的距离两者之间的差值与预设门限值进行比较；
在本发明实施例中，将上述相对距离信息和利用脉冲信号计算出的与主UUV即发送端之间的距离之间的差值与预设门限值进行/>比较。
S330、若所述相对距离信息和所述第一滤波状态参数中与主UUV之间的距离两者之间的差值小于预设门限值，则判定所述协同信息中的脉冲信号和所述通信波信号相匹配；
应当理解的是，在存储数据中寻找是否有与对应的/>。由于从UUV根据推算位置计算的与主UUV的距离存在一定误差，因此根据预设门限值/>作为粗差判断，当距离误差小于/>时，如下：
，
则认为与/>对应，即脉冲信号与通信波信号相匹配。
S340、当所述协同信息中的脉冲信号和所述通信波信号相匹配时，根据所述第一滤波器状态参数和所述主UUV位置信息对所述第二滤波器进行扩展卡尔曼滤波量测更新；
在本发明实施例中，在脉冲信号和通信波信号相匹配时，则对第二滤波器进行两侧更新。
具体地，若数据找到对应数据/>，则在接收到的第一滤波器A状态基础上对第二滤波器B进行卡尔曼滤波量测更新，量测模型如下所示：
，
其中，表示一个/>的量测矩阵，是/>关于/>的雅可比矩阵，表达式如下：
，
表示零均值高斯量测噪声向量，其协方差矩阵/>定义为观测噪声矩阵，由下式给出：
，
根据存储第一滤波器A的误差状态和均方根误差值/>进行第二滤波器B的卡尔曼滤波量测更新：
。
在本发明实施例中，需要说明的是，数据和第一滤波器A自身状态的存储时间为，若超过时间/>未有数据/>与之对应，则停止对加速度计值和陀螺值的存储，并将数据/>、第一滤波器A的自身状态以及持续存储的加速度计值和陀螺值进行删除。若数据/>未找到对应数据/>，则对该通信波数据进行删除，不再使用。
S350、根据所述自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波状态更新。
在本发明实施例中，还包括：
若所述第二滤波器的状态更新至最新状态，则根据第二滤波器的状态参数对第一滤波器的状态进行更新，获得最终导航结果。
量测更新完毕后对接收脉冲时刻自身惯性导航系统INS推导的从UUV位置进行反馈更新。利用持续存储的加速度计值/>和陀螺值/>对反馈更新后的从UUV的状态进行航位推算，并同步对第二滤波器B进行状态更新。若当前无储存的陀螺、加速度计数据可用，则将推算的导航结果和第二滤波器B状态传输给第一滤波器A并覆盖，即此时第一滤波器A输出的导航结果/>与滤波器状态/>、/>为第二滤波器B所提供，并在此基础上进行后续更新。
在本发明实施例中，所述水下无人航行器的协同导航方法还包括：
将所述第一滤波器状态参数和主UUV位置信息存储至第二滤波器，且将自身惯性导航系统的实时导航结果按照接收时间进行持续存储。
应当理解的是，在本发明实施例中，为解决信号错序以及错序引起的信号丢失问题，对第二滤波器B增加了信息的多级暂存器功能，即第二滤波器B收到脉冲信号和第一滤波器A传输的参数后分级存储所需参数。若第二滤波器B接收的下一帧数据仍是脉冲信号的/>与第一滤波器A参数，则继续分级存储，若第二滤波器B接收到通信波信号的位置信息，则利用位置信息与/>寻找相对应的/>值，并进行量测更新与状态更新。
在本发明实施例中，为了防止存储数据占用过多内存，还包括：
判断第二滤波器的状态是否更新至最新状态；
若所述第二滤波器的状态不是更新至最新状态，则根据存储的自身惯性导航系统的实时导航结果对所述第二滤波器的状态进行更新；
若所述第二滤波器的状态更新至最新状态，则停止对所述自身惯性导航系统的实时导航结果的存储，并将所述第二滤波器中存储的当级数据均删除。
此处需要说明的是，由于第二滤波器对接收到的第一滤波器A的参数、从UUV的位置、相对距离L以及加速度计值和陀螺仪值均是分级存储的，此处在进行删除时只是删除当级的存储数据。
具体地，还包括：
若所述相对距离信息和所述第一滤波状态参数中与主UUV之间的距离两者之间的差值大于预设门限值，则判断所述第一滤波状态参数中与主UUV之间的距离的存储时间是否大于预设时间；
若所述第一滤波状态参数中与主UUV之间的距离的存储时间大于预设时间，则停止对所述自身惯性导航系统的实时导航结果的存储，并将所述第二滤波器中存储的当级数据均删除。
应当理解的是，为防止占用过多内存，当第二滤波器B更新至最新状态将数据传输给第一滤波器A后，就停止对加速度计值和陀螺值/>的持续存储，并将相应存储的当级的第一滤波器A状态参数、节点状态、距离值/>与/>和/>与/>删除，进行空间释放。
第一滤波器A负责导航的实时位置输出，第二滤波器B负责利用过期数据提高导航精度，并利用多级暂存器功能解决了信号错序以及错序引起的信号丢失情况，实现协同导航系统的高精度实时定位解算。
本发明提供的水下无人航行器的协同导航方法，在自身惯性导航系统和扩展卡尔曼滤波状态更新的基础上，实现UUV集群协同导航结果的实时解算和输出，避免了因存储大量数据而导致导航结果输出实时性差的问题。同时，利用第二滤波器进行数据存储和延迟扩展卡尔曼滤波量测更新。该第二滤波器能够通过存储数据来处理接收的错序信息，并利用具有延迟性的主从UUV相对位置信息对从节点位置进行异步量测更新反馈，提高导航定位精度。通过INS与两个滤波器的联合使用，能够在保证导航结果实时输出的基础上减少数据丢失，提高导航定位精度。
另外，本发明的这种水下无人航行器的协同导航方法，与现有技术中为解决通信延迟而进行的信息融合的协同导航算法不同，本发明采用的延迟扩展卡尔曼滤波技术能够解决算法泛化能力差的问题，可以应对通信时延顺序延迟、错序延迟和丢失等情况，并采用双滤波器，在保证水下UUV集群实时导航的情况下提高导航定位精度。
作为本发明的另一实施例，提供一种水下无人航行器的协同导航装置，其中，应用于协同导航系统，所述协同导航系统包括主UUV和与主UUV通信连接的多个从UUV，针对每个从UUV，如图8所示，所述水下无人航行器的协同导航装置100包括：
接收模块110，用于接收主UUV发送的协同信息，所述协同信息至少包括用于计算与主UUV之间距离信息的脉冲信号和包含主UUV位置信息的通信波信号；
第一滤波器更新模块120，用于根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，以及当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，当所述第一滤波器接收到的所述协同信息为所述通信波信号时，将所述通信波信号传输的主UUV位置信息输出至第二滤波器；
第二滤波器更新模块130，用于根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，获得最终导航结果。
在本发明实施例中，所述水下无人航行器的协同导航装置100中的接收模块110主要接收主UUV发送的脉冲信号和通信波信号；第一滤波器更新模块能够根据自身惯性导航系统的实时导航结果对第一滤波器进行扩展卡尔曼状态更新；第二滤波器更新模块能够根据通信波信号和第一滤波器输出的状态参数等对第二滤波器进行扩展卡尔曼量测更新和状态更新，实现信息融合，得到组合导航结果，提高定位精度。
因此，本发明提供的水下无人航行器的协同导航装置，通过从UUV自身惯性导航系统的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，并实时输出导航结果，从而能够保证导航的实时性；在第一滤波器的基础上对第二滤波器进行扩展卡尔曼滤波更新，从而能够保证导航的精度。因此本发明的这种水下无人航行器的协同导航方法能够面对复杂的通信时延形式，如协同信息顺序、错序和丢失等情况，能够在多种场景下实现协同导航的高精度和实时性。
关于本发明的水下无人航行器的协同导航装置的具体工作原理可以参照前文的水下无人航行器的协同导航方法的描述，此处不再赘述。
作为本发明的另一实施例，提供一种协同导航系统，其中，包括主UUV和主UUV通信连接的多个从UUV，每个所述从UUV均包括前文所述的水下无人航行器的协同导航装置。
在本发明实施例中，结合图3和图9所示，针对主从式UUV水下集群协同导航情况，本发明通过延迟扩展卡尔曼滤波算法，其具有较强泛化性，并兼顾导航精度和实时性。具体地，在本发明实施例中，延迟扩展卡尔曼滤波具体可以首先通过第一滤波器A根据惯导解算的实时结果进行扩展卡尔曼滤波状态更新，并实时输出导航结果，保证算法的实时性；然后第二滤波器B在第一滤波器A的基础上进行扩展卡尔曼滤波的量测和状态更新，对通信时延信息进行部分存储并利用，保证导航精度，数据使用完毕后进行空间释放，减小内存占用。本发明的这种协同导航系统能够面对复杂的通信时延形式，如协同信息顺序、错序和丢失等情况，且能够在多种场景下实现协同导航的高精度和实时性。
关于本发明提供的协同导航系统的具体工作原理可以参照前文的水下无人航行器的协同导航方法的描述，此处不再赘述。
可以理解的是，以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式，然而本发明并不局限于此。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。
Claims (9)
1.一种水下无人航行器的协同导航方法，其特征在于，应用于协同导航系统，所述协同导航系统包括主UUV和与主UUV通信连接的多个从UUV，针对每个从UUV，所述水下无人航行器的协同导航方法包括：
接收主UUV发送的协同信息，所述协同信息至少包括用于计算与主UUV之间距离信息的脉冲信号和包含主UUV位置信息的通信波信号；
根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，以及当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，当所述第一滤波器接收到的所述协同信息为所述通信波信号时，将所述通信波信号传输的主UUV位置信息输出至第二滤波器；
根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，获得最终导航结果；
根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，包括：
根据所述第一滤波器状态参数中自身惯性导航系统实时推算出的当前位置信息与主UUV位置信息计算相对距离信息；
将所述相对距离信息和所述第一滤波器状态参数中与主UUV之间的距离两者之间的差值与预设门限值进行比较；
若所述相对距离信息和所述第一滤波器状态参数中与主UUV之间的距离两者之间的差值小于预设门限值，则判定所述协同信息中的脉冲信号和所述通信波信号相匹配；
当所述协同信息中的脉冲信号和所述通信波信号相匹配时，根据所述第一滤波器状态参数和所述主UUV位置信息对所述第二滤波器进行扩展卡尔曼滤波量测更新；
根据所述自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波状态更新。
2.根据权利要求1所述的水下无人航行器的协同导航方法，其特征在于， 当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，包括：
当所述第一滤波器接收到的所述协同信息为所述脉冲信号时，根据所述脉冲信号计算与主UUV之间的距离；
将接收到所述脉冲信号时的第一滤波器的自身状态参数、自身惯性导航系统实时推算出的当前位置信息以及与主UUV之间的距离作为所述第一滤波器状态参数，并将所述第一滤波器状态参数输出至第二滤波器。
3.根据权利要求1所述的水下无人航行器的协同导航方法，其特征在于，还包括：
将所述第一滤波器状态参数和主UUV位置信息存储至第二滤波器，且将自身惯性导航系统的实时导航结果按照接收时间进行持续存储。
4.根据权利要求3所述的水下无人航行器的协同导航方法，其特征在于，还包括：
判断第二滤波器的状态是否更新至最新状态；
若所述第二滤波器的状态不是更新至最新状态，则根据存储的自身惯性导航系统的实时导航结果对所述第二滤波器的状态进行更新；
若所述第二滤波器的状态更新至最新状态，则停止对所述自身惯性导航系统的实时导航结果的存储，并将所述第二滤波器中存储的当级数据均删除。
5.根据权利要求4所述的水下无人航行器的协同导航方法，其特征在于，还包括：
若所述第二滤波器的状态更新至最新状态，则根据第二滤波器的状态参数对第一滤波器的状态进行更新，获得最终导航结果。
6.根据权利要求3所述的水下无人航行器的协同导航方法，其特征在于，还包括：
若所述相对距离信息和所述第一滤波器状态参数中与主UUV之间的距离两者之间的差值大于预设门限值，则判断所述第一滤波器状态参数中与主UUV之间的距离的存储时间是否大于预设时间；
若所述第一滤波器状态参数中与主UUV之间的距离的存储时间大于预设时间，则停止对所述自身惯性导航系统的实时导航结果的存储，并将所述第二滤波器中存储的当级数据均删除。
7.根据权利要求1所述的水下无人航行器的协同导航方法，其特征在于，根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，包括：
根据自身惯性导航系统输出的加速度计值和陀螺值进行惯性解算算法计算，获得导航解算信息；
根据所述导航解算信息对所述第一滤波器进行扩展卡尔曼滤波状态更新。
8.一种水下无人航行器的协同导航装置，用于实现权利要求1至7中任意一项所述的水下无人航行器的协同导航方法，其特征在于，应用于协同导航系统，所述协同导航系统包括主UUV和与主UUV通信连接的多个从UUV，针对每个从UUV，所述水下无人航行器的协同导航装置包括：
接收模块，用于接收主UUV发送的协同信息，所述协同信息至少包括用于计算与主UUV之间距离信息的脉冲信号和包含主UUV位置信息的通信波信号；
第一滤波器更新模块，用于根据自身惯性导航系统输出的实时导航结果对第一滤波器进行扩展卡尔曼滤波状态更新，以及当所述第一滤波器接收到的所述协同信息为所述脉冲信号时将所述第一滤波器根据所述协同信息获得的第一滤波器状态参数输出至第二滤波器，当所述第一滤波器接收到的所述协同信息为所述通信波信号时，将所述通信波信号传输的主UUV位置信息输出至第二滤波器；
第二滤波器更新模块，用于根据所述第一滤波器状态参数、主UUV位置信息以及自身惯性导航系统的实时导航结果对所述第二滤波器进行扩展卡尔曼滤波更新，获得最终导航结果。
9.一种协同导航系统，其特征在于，包括主UUV和主UUV通信连接的多个从UUV，每个所述从UUV均包括权利要求8所述的水下无人航行器的协同导航装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410027822.2A CN117553787B (zh) | 2024-01-09 | 2024-01-09 | 水下无人航行器的协同导航方法、装置及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410027822.2A CN117553787B (zh) | 2024-01-09 | 2024-01-09 | 水下无人航行器的协同导航方法、装置及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117553787A CN117553787A (zh) | 2024-02-13 |
| CN117553787B true CN117553787B (zh) | 2024-03-26 |
Family
ID=89820798
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410027822.2A Active CN117553787B (zh) | 2024-01-09 | 2024-01-09 | 水下无人航行器的协同导航方法、装置及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117553787B (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101755307B1 (ko) * | 2016-05-11 | 2017-07-10 | 한국해양과학기술원 | 수중 이동체의 위치 측정 오차 보정 방법 |
| CN107117276A (zh) * | 2017-04-18 | 2017-09-01 | 西北工业大学 | 一种便携式自主水下航行器的推进与操纵一体化控制装置 |
| KR101789188B1 (ko) * | 2016-05-11 | 2017-10-24 | 한국해양과학기술원 | 수중 이동체의 위치 추적을 위한 수중 통합 항법 시스템 |
| CN108444476A (zh) * | 2018-02-05 | 2018-08-24 | 哈尔滨工程大学 | 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法 |
| CN109693774A (zh) * | 2018-12-29 | 2019-04-30 | 中国科学院声学研究所 | 一种水下航行器轨迹的控制方法和系统 |
| CN110332933A (zh) * | 2019-07-09 | 2019-10-15 | 西安中兴物联软件有限公司 | 车辆定位方法、终端及计算机可读存储介质 |
| WO2022028286A1 (zh) * | 2020-08-04 | 2022-02-10 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
| WO2022135070A1 (zh) * | 2020-12-24 | 2022-06-30 | 北京紫光展锐通信技术有限公司 | 惯性导航方法及设备 |
-
2024
- 2024-01-09 CN CN202410027822.2A patent/CN117553787B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101755307B1 (ko) * | 2016-05-11 | 2017-07-10 | 한국해양과학기술원 | 수중 이동체의 위치 측정 오차 보정 방법 |
| KR101789188B1 (ko) * | 2016-05-11 | 2017-10-24 | 한국해양과학기술원 | 수중 이동체의 위치 추적을 위한 수중 통합 항법 시스템 |
| CN107117276A (zh) * | 2017-04-18 | 2017-09-01 | 西北工业大学 | 一种便携式自主水下航行器的推进与操纵一体化控制装置 |
| CN108444476A (zh) * | 2018-02-05 | 2018-08-24 | 哈尔滨工程大学 | 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法 |
| CN109693774A (zh) * | 2018-12-29 | 2019-04-30 | 中国科学院声学研究所 | 一种水下航行器轨迹的控制方法和系统 |
| CN110332933A (zh) * | 2019-07-09 | 2019-10-15 | 西安中兴物联软件有限公司 | 车辆定位方法、终端及计算机可读存储介质 |
| WO2022028286A1 (zh) * | 2020-08-04 | 2022-02-10 | 东南大学 | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 |
| WO2022135070A1 (zh) * | 2020-12-24 | 2022-06-30 | 北京紫光展锐通信技术有限公司 | 惯性导航方法及设备 |
Non-Patent Citations (2)
| Title |
|---|
| 基于EKF 的UUV 单信标水声 导航定位方法研究;曹方方等;《计算机测量与控制》;20111231;第19卷(第9期);第2234-2236页 * |
| 基于距离量测的主从式A U V 协同定位方法;张永清等;《火力与指挥控制》;20111130;第36卷(第11期);第206-208页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117553787A (zh) | 2024-02-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110906923B (zh) | 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆 | |
| Kepper et al. | A navigation solution using a MEMS IMU, model-based dead-reckoning, and one-way-travel-time acoustic range measurements for autonomous underwater vehicles | |
| US11305854B2 (en) | Deep-sea low-cost long-endurance collaborative navigation and positioning system | |
| Webster et al. | Decentralized extended information filter for single-beacon cooperative acoustic navigation: Theory and experiments | |
| CN110057365B (zh) | 一种大潜深auv下潜定位方法 | |
| CN111595348B (zh) | 一种自主水下航行器组合导航系统的主从式协同定位方法 | |
| Webster et al. | Preliminary deep water results in single-beacon one-way-travel-time acoustic navigation for underwater vehicles | |
| CN110837854B (zh) | 一种基于因子图的auv多源信息融合方法和设备 | |
| CN107677272B (zh) | 一种基于非线性信息滤波的auv协同导航方法 | |
| CN110186461B (zh) | 一种基于重力梯度信息测距的协同导航方法 | |
| Allotta et al. | Cooperative navigation of AUVs via acoustic communication networking: field experience with the Typhoon vehicles | |
| Al-Khatib et al. | The widely scalable mobile underwater sonar technology (WiMUST) project: an overview | |
| Ridao et al. | USBL/DVL navigation through delayed position fixes | |
| CN107943859B (zh) | 海量传感器数据收集处理及反馈的系统和方法 | |
| Wiktor et al. | Collaborative multi-robot localization in natural terrain | |
| CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
| KR101296190B1 (ko) | 지형 정보 활용 항법 및 그 구동 방법 | |
| CN117553787B (zh) | 水下无人航行器的协同导航方法、装置及系统 | |
| CN111708008B (zh) | 一种基于imu和tof的水下机器人单信标导航方法 | |
| Chen et al. | Underwater terrain positioning method using maximum a posteriori estimation and PCNN model | |
| CN114088104B (zh) | 一种自动驾驶场景下的地图生成方法 | |
| Yao et al. | Cooperative localization with communication delays for MAUVs | |
| Li et al. | Research on the algorithm of multi-autonomous underwater vehicles navigation and localization based on the extended kalman filter | |
| CN114777767A (zh) | 基于无人机定位预测方法、装置和无人机 | |
| Zhao et al. | A single beacon-aided cooperative localization algorithm based on maximum correntropy criterion |
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 |