CN114485645B - 基于测距与信息交互的uuv集群协同定位***和方法 - Google Patents

基于测距与信息交互的uuv集群协同定位***和方法 Download PDF

Info

Publication number
CN114485645B
CN114485645B CN202210102334.4A CN202210102334A CN114485645B CN 114485645 B CN114485645 B CN 114485645B CN 202210102334 A CN202210102334 A CN 202210102334A CN 114485645 B CN114485645 B CN 114485645B
Authority
CN
China
Prior art keywords
uuv
location
main
information
slave
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
Application number
CN202210102334.4A
Other languages
English (en)
Other versions
CN114485645A (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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202210102334.4A priority Critical patent/CN114485645B/zh
Publication of CN114485645A publication Critical patent/CN114485645A/zh
Application granted granted Critical
Publication of CN114485645B publication Critical patent/CN114485645B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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
    • 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
    • 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/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Acoustics & Sound (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了基于测距与信息交互的UUV集群协同定位***和方法。本发明***包括一台主UUV、多台从UUV和安装于从UUV上的协同定位装置，协同定位装置利用从UUV的INS数据进行位置估计作为主导航单元，利用声学测距和协同定位报文滤波处理作为副导航单元。协同定位方法包括：主从UUV内部时钟驯服；主UUV周期性将自身位置和位置估计协方差矩阵打包成协同定位报文，并广播给集群的从UUV；从UUV接收协同定位报文；从UUV的协同定位装置根据数据构建状态和测量方程，利用扩展卡尔曼滤波计算得到协同定位结果。本发明考虑主UUV自身的位置估计不确定性，针对从UUV的状态向量进行了重构，提升了集群整体的定位性能。

Description

基于测距与信息交互的UUV集群协同定位***和方法
技术领域
本发明属于声学定位技术领域，尤其是水下航行器集群声学定位技术领域，涉及一种基于测距与信息交互的UUV集群协同定位***和方法。
背景技术
海洋蕴含着丰富的资源，随着陆地上资源的日益枯竭，全世界越来越多的国家也逐渐将目光投向了海洋的开发与保护。作为未来国家海洋安全、经济发展的重要竞争领域，海洋研究的重要性不言而喻。近年来，水下航行器的发展，如水下无人自主航行器(unmanned underwater vehicles,UUV)等，也加快了海洋探索和研究的进程。但相比于陆地环境，水下环境往往更加恶劣，陆地常用的光波、无线电等信号在水下的衰减或色散大，因此陆地上众多成熟的商用通信、导航方式在水下并不适用。
声波由于其更小的水下衰减、更长的水下传播距离，因此比光波、无线电等更适合于作为水下通信与定位导航的信号载体。一系列的声学理论、技术、产品也逐渐发展，在海洋研究领域做出了重要的贡献。
对于水下探索、任务执行而言，单体UUV存在着覆盖范围小、搭载的任务载荷少等缺陷，所以参考陆地上狼群、海洋里鲸群等工作的模式，UUV集群的工作模式逐渐发展起来。这种方式允许多个UUV以编队的形式执行任务，克服了单体UUV的缺陷。UUV集群分为三种工作模式：主从式、分层式和并行式。主从模式是最为常见的工作形式，在这种模式下，集群中存在单个或者多个领航者，这些作为领航者的主航行器搭载了更多的载荷，具有更为全面的探测、定位、处理能力。从航行器跟随主航行器进行工作，通过水声通信实现和主航行器的信息交互，实现任务分配、编队控制、数据汇聚等功能。分层模式下，弱化了主从航行器的概念，根据不同航行器搭载的载荷精度级别，将整个编队的航行器分属于不同的精度层级，相同层级的航行器可以进行信息交互，而低精度层的航行器受控于高精度层的航行器。并行模式下，没有主从和高低精度的区别，所有的航行器均可互相实现信息交互。
对于主从式UUV集群来说，主航行器作为集群的核心，承担着任务分配、信息汇聚、导航协助等重要功能；从航行器受体积、成本等限制，装备精度较差的传感器，往往定位精度较差。协同定位的内涵是，如果主从水下航行器可以实现相互的信息共享，从航行器可以利用主航行器的定位信息提升自身的导航定位精度，从而提升集群整体的定位能力。
发明内容
本发明的一个目的是针对现有主从UUV集群协同定位的需求，提供一种基于测距与信息交互的UUV集群协同定位***，考虑主航行器定位不确定性时，利用测距和信息交互进行集群内协同定位。
本发明的协同定位***，包括主UUV、从UUV和协同定位装置，具体构成包括：
(1)一台主UUV，包括但不限于大型的自主式水下航行器(Autonomous underwatervehicles,AUV)等，搭载多普勒测速仪DVL(Doppler velocity log)、高精度惯性导航***INS(Inertial navigation system)、第一卫星单元模块、第一水声通信机、第一主控单元模块、第一动力***、第一数据处理模块和第一时钟模块。其中：
多普勒测速仪进行对水和对水底锁定，用于测量主UUV航速；
高精度惯性导航***INS用于估计主UUV自身位置和相应的运动姿态；
第一卫星单元模块用于主UUV浮出海面时，接收卫星位置更新和与控制中心的通信；
第一水声通信机用于主从UUV在水下的通信，主UUV通过水声通信机向控制中心汇报状态以及接收控制中心的指令与数据；
第一主控单元模块用于控制主UUV的上浮、下潜、任务切换；
第一动力***用于输出主UUV所需动力；
第一数据处理模块用于结合DVL和INS数据，输出主UUV位置，并产生协同定位报文；
第一时钟模块用于提供时钟基准。
(2)多台从UUV，包括但不限于小型的自主式水下航行器等，每台从UUV搭载低精度惯性导航***INS、第二卫星单元模块、第二水声通信机、第二主控单元模块、第二动力***、第二数据处理模块和第二时钟模块。其中：
低精度惯性导航***INS输出从UUV的定位结果与相应的运动姿态；
第二卫星单元模块用于从UUV浮出海面时，接收卫星位置更新和与控制中心的通信；
第二水声通信机用于从主UUV在水下的通信，以及接收协同定位数据报文；
第二主控单元模块用于控制从UUV的上浮、下潜、任务切换，具有通信和定位一体化功能；
第二动力***用于输出从UUV所需动力；
第二数据处理模块用于从UUV的内部数据处理；
第二时钟模块用于提供时钟基准。
主UUV搭载的高精度惯性导航***INS与从UUV搭载的低精度惯性导航***INS为精度相对高低。
(3)协同定位装置，安装于从UUV上，用于协同定位的算法执行和输出协同定位结果。
协同定位装置包含主导航单元和副导航单元，利用从UUV的INS数据进行位置估计作为主导航单元；利用声学测距和协同定位报文滤波处理作为副导航单元。
协同定位装置向外提供两个接口，与数据处理模块的通信接口和与水声通信机的通信接口；接口通信采用串口通信，发送TX、接收RX与地线连接的方式，使用软件握手XON/XOFF方式建立通信连接。需要通信时，接收端向发送端发送XON，开始接收数据；发送端接收到XON后开始发送数据；当不需要通信时，接收端反馈XOFF结束通信。
(3-1)与数据处理模块的通信接口：从UUV默认以1Hz的频率向协同定位装置输入INS采集的经度、纬度、航速、航向、深度、时间戳数据；协同定位装置向从UUV输出协同定位后的经度、纬度数据。
协同定位装置和从UUV的信息交互如下：协同定位装置需要状态信息时，向第二数据处理模块发送XON开始接收信息，直至不需要时，发送XOFF结束通信；第二数据处理模块接收到协同定位装置的XON后，开始发送包含经度、纬度的状态信息，直至接收到XOFF后停止发送。第二数据处理模块需要协同定位结果时，向协同定位装置发送XON，直至不需要该结果后，发送XOFF停止通信；协同定位装置接收到从UUV的XON后开始发送协同定位结果，直至接收到XOFF后停止发送。
(3-2)与水声通信机的通信接口：第二水声通信机接收到主UUV协同定位报文后，将解析后的报文信息与水声通信机记录的该协同定位报文的到达时刻输入给协同定位装置。
协同定位装置和声学通信接收机的信息交互如下：协同定位装置需要第二水声通信机接收的信息时，向第二水声通信机发送XON，直至不需要该信息，发送XOFF；第二水声通信机接收到XON后，接收到主UUV的协同定位报文，向协同定位装置输入协同定位报文数据和其到达时刻，直至接收到XOFF停止发送。
该协同定位***的工作模式为：
主UUV融合多普勒测速仪和高精度惯性导航***INS数据，输出高精度的主UUV参考位置，并将关键信息打包成协同定位数据报文，以固定的时间间隔通过第一水声通信机广播给集群中的从UUV，关键信息包括自身的位置、状态协方差矩阵信息、发送时间戳。
从UUV的低精度惯性导航***INS更新频率高，将其采集的包括经度、纬度的数据作为协同定位装置中的主导航单元；协同定位装置中副导航单元更新频率低。当第二水声通信机接收到主UUV的协同定位报文之后，记录协同定位报文的到达时间戳，并结合解析的协同定位报文内容输入给协同定位装置；协同定位装置根据第二水声通信机输入的信息，经过协同定位处理，输出给从UUV更新后的定位信息，完成协同定位。
本发明的另一个目的是提供一种基于测距与信息交互的UUV集群协同定位方法，具体步骤是：
步骤(1)主UUV、从UUV下水前进行内部时钟驯服，保持时钟同步，完成驯服后开始水下工作。主UUV的第一卫星单元模块和从UUV的第二卫星单元模块开始工作，主UUV和从UUV的定位信息进行初始化。
步骤(2)进入稳定工作状态后，主UUV的第一数据处理模块以固定周期T′生成协同定位数据报文，协同定位数据报文包括经度LM,k、纬度BM,k、深度dM,k、航速vM,k、航向X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时刻/>下标M表示主UUV，下标k表示在第k定位时隙。协同定位数据报文广播给集群中的从UUV。
步骤(3)第二水声通信机接收到协同定位数据报文后，记录协同定位数据报文的到达时刻解析出协同定位数据报文中的信息，对其中的主UUV的经度和纬度进行处理，转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)，LB2XY(·,·)表示经纬度和XY坐标系的转换运算。第二数据处理模块获取INS的数据，包括从UUV的经度LS,k、纬度BS,k、深度dS,k、航速vS,k、航向/>角加速度ωS,k，下标S表示从UUV；对其中的从UUV的经度和纬度进行处理，转换成XY坐标系位置(xS,k,yS,k)＝LB2XY(LS,k,BS,k)，将主UUV位置、从UUV位置、解析的协同定位数据信息，以及剩余的从UUV第二数据处理模块输出信息，输入给协同定位装置。
步骤(4)协同定位装置建立状态转移方程和测量方程，利用解析出协同定位数据报文中的信息进行滤波处理，完成协同定位。具体方法是：
(4-1)建立状态向量和状态转移方程：
从UUV处于稳态运行的状态时，第k定位时隙的从UUV的运动状态ψS,k表示为XY坐标系位置和航向的组合[·]T表示转置运算。
在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态运动方程表示为：/>
将当前状态和前序缓存的状态相结合，构建组合状态向量其中N表示缓存的阶数，相应的协方差矩阵为/>
组合后的运动预测模型其中，uS,k-1表示作为测量输入的从UUV的运动速度，nk表示估计误差，服从高斯分布。状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fk，预测模型线性化处理表示为/>其中，B表示输入控制矩阵。
相应的协方差矩阵/>表示为/>其中，Qk为引入的状态转移误差，其中，/>和/>分别表示从UUV测速标准差和测向标准差。
当完成一次状态向量增广操作后，其协方差矩阵同步进行维度增广，修改表示为/>Pk-1[1,1]、Pk-1[1,2]、Pk-1[2,1]、Pk-1[2,2]分别表示协方差矩阵Pk-1的左上、右上、左下和右下子矩阵。
(4-2)对状态向量进行增广：
协同定位装置在原有状态的基础上增添上接收到的主UUV信息ψM,k构建增广状态向量相应的协方差矩阵为/>其中，PM,k表示主UUV状态协方差矩阵。
(4-3)建立测量方程：
根据解析出的发送时间和本地记录的到达时刻/>计算出传播距离/>其中，c为等效声速。
在第k定位时隙，从UUV的空间位置信息表示为主UUV的空间位置信息表示为(xM,k,yM,k,dM,k)。
关于状态向量构建测距方程设测量受一个零均值的高斯白噪声ηS,k影响，则测量模型表示为/>测量偏差矩阵为Rk。
(4-4)滤波更新：
由于测量关于状态量是一个非线性函数，因此可以将其线性化处理，求其Jacobi矩阵H。结合上述步骤(4-1)～(4-3)，以扩展卡尔曼滤波(extended Kalman filter,EKF)为基础，引入新息增益吸收新息，以更新预测的状态：为更新后预测的状态，Kk为时隙k的新息增益，/>表示测距方程，Hk为测量方程的Jacobi矩阵。
新息增益K通过测量矩阵和测量偏差得到，
由状态更新和新息增益，同步状态更新的协方差矩阵I为单位矩阵。
(4-5)利用测距信息得到的定位结果：
从中提取出更新的从UUV位置/>与步骤(3)的(xS,k,yS,k)进行比较：当两者差值小于等于设定阈值时，则更新正常，将其转换成经纬度坐标进行协同定位结果输出；当两者差值大于设定阈值时，舍弃此时更新后的结果，或协同定位装置无法接收到水声通信机输入数据时，从/>提取出从UUV位置/>将其转换成经纬度坐标/>进行协同定位结果输出。
本发明利用航行器之间的测距和信息交互构建主航行器和从航行器之间的协同定位模型，结合滤波的方法提升集群整体的定位性能。相比于传统方法，本发明考虑了主UUV自身定位的不确定性，将其打包进协同定位报文中发送给从UUV，从UUV可以利用主UUV的状态信息和自身的状态信息联合建立新的状态向量，在接收到协同定位后对该状态向量进行更新，获得协同定位的结果，进一步减小了主UUV自身定位的不确定性对整个集群定位性能影响。
附图说明
图1是本发明中主、从UUV的协同定位处理逻辑示意图。
具体实施方式
以下结合具体实施方式和附图对本发明做进一步说明。
基于测距与信息交互的UUV集群协同定位***，
包括主UUV、从UUV和协同定位装置，具体构成包括：
(1)一台主UUV，可以是自主式水下航行器(Autonomous underwater vehicles,AUV)等，搭载多普勒测速仪DVL、高精度惯性导航***INS、第一卫星单元模块、第一水声通信机、第一主控单元模块、第一动力***、第一数据处理模块和第一时钟模块。其中：
多普勒测速仪进行对水和对水底锁定，用于测量主UUV航速；
高精度惯性导航***INS用于估计主UUV自身位置和相应的运动姿态；
第一卫星单元模块用于主UUV浮出海面时，接收卫星位置更新和与控制中心的通信；
第一水声通信机用于主从UUV在水下的通信，主UUV通过水声通信机向控制中心汇报状态以及接收控制中心的指令与数据；
第一主控单元模块用于控制主UUV的上浮、下潜、任务切换；
第一动力***用于输出主UUV所需动力；
第一数据处理模块用于结合DVL和INS数据，输出主UUV位置，并产生协同定位报文；
第一时钟模块用于提供时钟基准。
(2)三台从UUV，可以是小型的自主式水下航行器等，每台从UUV搭载低精度惯性导航***INS、第二卫星单元模块、第二水声通信机、第二主控单元模块、第二动力***、第二数据处理模块和第二时钟模块。其中：
低精度惯性导航***INS输出从UUV的定位结果与相应的运动姿态；
第二卫星单元模块用于从UUV浮出海面时，接收卫星位置更新和与控制中心的通信；
第二水声通信机用于从主UUV在水下的通信，以及接收协同定位数据报文；
第二主控单元模块用于控制从UUV的上浮、下潜、任务切换，具有通信和定位一体化功能；
第二动力***用于输出从UUV所需动力；
第二数据处理模块用于从UUV的内部数据处理；
第二时钟模块用于提供时钟基准。
主UUV搭载的高精度惯性导航***INS与从UUV搭载的低精度惯性导航***INS为精度相对高低。
(3)协同定位装置，安装于从UUV上，用于协同定位的算法执行和输出协同定位结果。
协同定位装置包含主导航单元和副导航单元，利用从UUV的INS数据进行位置估计作为主导航单元；利用声学测距和协同定位报文滤波处理作为副导航单元。
协同定位装置向外提供两个接口，与数据处理模块的通信接口和与水声通信机的通信接口；接口通信采用串口通信，发送TX、接收RX与地线连接的方式，使用软件握手XON/XOFF方式建立通信连接。需要通信时，接收端向发送端发送XON，开始接收数据；发送端接收到XON后开始发送数据；当不需要通信时，接收端反馈XOFF结束通信。
(3-1)与数据处理模块的通信接口：从UUV默认以1Hz的频率向协同定位装置输入INS采集的经度、纬度、航速、航向、深度、时间戳数据；协同定位装置向从UUV输出协同定位后的经度、纬度数据。
协同定位装置和从UUV的信息交互如下：协同定位装置需要状态信息时，向第二数据处理模块发送XON开始接收信息，直至不需要时，发送XOFF结束通信；第二数据处理模块接收到协同定位装置的XON后，开始发送包含经度、纬度的状态信息，直至接收到XOFF后停止发送。第二数据处理模块需要协同定位结果时，向协同定位装置发送XON，直至不需要该结果后，发送XOFF停止通信；协同定位装置接收到从UUV的XON后开始发送协同定位结果，直至接收到XOFF后停止发送。
(3-2)与水声通信机的通信接口：第二水声通信机接收到主UUV协同定位报文后，将解析后的报文信息与水声通信机记录的该协同定位报文的到达时刻输入给协同定位装置。
协同定位装置和声学通信接收机的信息交互如下：协同定位装置需要第二水声通信机接收的信息时，向第二水声通信机发送XON，直至不需要该信息，发送XOFF；第二水声通信机接收到XON后，接收到主UUV的协同定位报文，向协同定位装置输入协同定位报文数据和其到达时刻，直至接收到XOFF停止发送。
该协同定位***的工作模式为：
主UUV融合多普勒测速仪和高精度惯性导航***INS数据，输出高精度的主UUV参考位置，并将关键信息打包成协同定位数据报文，以固定的时间间隔通过第一水声通信机广播给集群中的从UUV，关键信息包括自身的位置、状态协方差矩阵信息、发送时间戳。
从UUV的低精度惯性导航***INS更新频率高，将其采集的包括经度、纬度的数据作为协同定位装置中的主导航单元；协同定位装置中副导航单元更新频率低。当第二水声通信机接收到主UUV的协同定位报文之后，记录协同定位报文的到达时间戳，并结合解析的协同定位报文内容输入给协同定位装置；协同定位装置根据第二水声通信机输入的信息，经过协同定位处理，输出给从UUV更新后的定位信息，完成协同定位。
图1给出了主、从UUV的协同定位处理逻辑，如图1，三台从UUV使用相同的协同定位方法，其中第i台从UUV，i∈{1，2，3}，具体的协同定位方法如下：
步骤(1)主UUV(以下标M表示)、第i台从UUV(以下标Si表示)下水前进行内部时钟驯服，保持时钟同步，完成驯服后开始水下工作。第一卫星单元模块和第二卫星单元模块开始工作，分别对主UUV和第i台从UUV的定位信息进行初始化。
步骤(2)进入稳定工作状态后，主UUV的第一数据处理模块以固定周期T'生成协同定位数据报文，在第k定位时隙中，协同定位数据报文包括经度LM,k、纬度BM,k、深度dM,k、航速vM,k、航向X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时间/>协同定位数据报文广播给集群中的从UUV。
协同定位数据报文包括1个字节的数据报文起始头SYN、1个字节的有效报文长度LEN、4个字节的经度M_LON、4个字节的纬度M_LAT、2个字节的深度M_DEP、2个字节的航速M_VEL、2个字节的航向M_HEAD、4个字节的X方向和Y方向位置定位标准差M_UNCX和M_UNCY、4个字节的报文生成时间M_TIMET以及2个字节的纠错冗余FCC，协同定位数据报文的格式如下表，随后广播给集群中的从UUV。
SYN LEN M_LON M_LAT M_DEP M_VEL M_HEAD M_UNCX M_UNCY M_TIMET FCC
1 1 4 4 2 2 2 2 2 4 2
步骤(3)第i台从UUV的第二数据处理模块获取INS的数据，对其中的从UUV的经度和纬度进行处理转换成XY坐标系位置(xSi,k,ySi,k)＝LB2XY(LSi,k,BSi,k)，LB2XY()表示经纬度和XY坐标系的转换运算，将该位置和剩余信息打包输入给协同定位装置，从UUV第二数据处理模块输入协同定位处理装置的数据格式如下表,包括4个字节的经度INS_LON、4个字节的纬度INS_LAT、2个字节的深度INS_DEP、2个字节的航速INS_VEL、2个字节的航向INS_HEAD以及4个字节的时间INS_TIME。
SYN INS_LON INS_LAT INS_DEP INS_VEL INS_HEAD INS_TIME
1 4 4 2 2 2 4
当第i台从UUV的水声通信机接收到定位报文后，记录定位报文的到达时刻解析出协同定位报文中的信息，对其中的主UUV的经度和纬度进行处理转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)。完成数据打包后，输入给协同定位处理装置。声学通信机输入协同定位处理装置的数据格式如下表,包括解析后的4个字节的主UUV经度M_LON，4个字节的主UUV纬度M_LAT，2个字节的主UUV深度M_DEP，2个字节主UUV航速M_VEL，2个字节的航向M_HEAD，4个字节的X方向和Y方向位置定位标准差M_UNCX和M_UNCY、4个字节的报文生成时间M_TIMET以及4个字节的报文到达时间M_TIMER。
SYN M_LON M_LAT M_DEP M_VEL M_HEAD M_UNCX M_UNCY M_TIMET M_TIMER
1 4 4 2 2 2 2 2 4 4
步骤(4)协同定位装置建立状态转移方程和测量方程，利用解析出协同定位数据报文中的信息进行滤波处理，完成协同定位。具体方法是：
(4-1)建立状态向量和状态转移方程：
第i台从UUV处于稳态运行的状态时，第k定位时隙的从UUV的运动状态ψSi,k表示为XY坐标系位置和航向的组合[·]T表示转置运算。
在固定周期T′的更新频率之下，第i台从UUV的第k定位时隙的估计状态运动方程表示为：/>
将当前状态和前序缓存的状态相结合，构建组合状态向量其中N表示缓存的阶数，相应的协方差矩阵为/>组合后的运动预测模型/>其中，uSi,k-1表示作为测量输入的从UUV的运动速度，nk表示估计误差，服从高斯分布。状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fi,k，预测模型线性化处理表示为/>其中，Bi表示输入控制矩阵。
相应的协方差矩阵/>表示为/>其中，Qi,k为引入的状态转移误差，/>其中，/>和/>分别表示从UUV测速标准差和测向标准差。
当完成一次状态向量增广操作后，其协方差矩阵同步进行维度增广，修改表示为/>Pi,k-1[1,1]、Pi,k-1[1,2]、Pi,k-1[2,1]、Pi,k-1[2,2]分别表示协方差矩阵Pi,k-1的左上、右上、左下和右下子矩阵。
(4-2)对状态向量进行增广：
协同定位装置在原有状态的基础上增添上接收到的主UUV信息ψM,k构建增广状态向量相应的协方差矩阵为/>其中，PM,k表示主UUV状态协方差矩阵。
(4-3)建立测量方程：
根据解析出的发送时间和本地记录的到达时刻/>计算出传播距离其中，c为等效声速。
在第k定位时隙，第i台从UUV的空间位置信息表示为主UUV的空间位置信息表示为(xM,k,yM,k,dM,k)。
关于状态向量构建测距方程设测量受一个零均值的高斯白噪声ηSi,k影响，则测量模型表示为/>测量偏差矩阵为Ri,k。
(4-4)滤波更新：
由于测量关于状态量是一个非线性函数，因此可以将其线性化处理，求其Jacobi矩阵H。结合上述步骤(4-1)～(4-3)，以扩展卡尔曼滤波(extended Kalman filter,EKF)为基础，引入新息增益K吸收新息，以更新预测的状态： 为更新后预测的状态，Ki,k为时隙k的新息增益，/>表示测距方程，Hi,k为测量方程的Jacobi矩阵。
新息增益K通过测量矩阵和测量偏差得到：
由状态更新和新息增益，同步状态更新的协方差矩阵I为单位矩阵。
(4-5)利用测距信息得到的定位结果：
从中提取出更新的第i台从UUV位置/>与步骤(3)的(xSi-INS,k,ySi-INS,k)进行比较：当两者差值小于设定阈值时，则更新正常，将其转换成经纬度坐标进行协同定位结果输出；当两者差值大于设定阈值时，舍弃此时更新后的结果，或者协同定位装置无法接收到水声通信机输入数据时，从/>提取出从UUV位置/>将其转换成经纬度坐标/>进行协同定位结果输出。

Claims (3)

1.基于测距与信息交互的UUV集群协同定位***，包括主UUV、从UUV和协同定位装置，其特征在于，具体构成包括：
(1)一台主UUV，搭载多普勒测速仪DVL、高精度惯性导航***INS、第一卫星单元模块、第一水声通信机、第一主控单元模块、第一动力***、第一数据处理模块和第一时钟模块；其中：
所述的多普勒测速仪进行对水和对水底锁定，用于测量主UUV航速；
所述的高精度惯性导航***INS用于估计主UUV自身位置和相应的运动姿态；
所述的第一卫星单元模块用于主UUV浮出海面时，接收卫星位置更新和与控制中心的通信；
所述的第一水声通信机用于主从UUV在水下的通信，主UUV通过水声通信机向控制中心汇报状态以及接收控制中心的指令与数据；
所述的第一主控单元模块用于控制主UUV的上浮、下潜、任务切换；
所述的第一动力***用于输出主UUV所需动力；
所述的第一数据处理模块用于结合DVL和INS数据，输出主UUV位置，并产生协同定位报文；
所述的第一时钟模块用于提供时钟基准；
(2)多台从UUV，每台从UUV搭载低精度惯性导航***INS、第二卫星单元模块、第二水声通信机、第二主控单元模块、第二动力***、第二数据处理模块和第二时钟模块；其中：
所述的低精度惯性导航***INS输出从UUV的定位结果与相应的运动姿态；
所述的第二卫星单元模块用于从UUV浮出海面时，接收卫星位置更新和与控制中心的通信；
所述的第二水声通信机用于从主UUV在水下的通信，以及接收协同定位数据报文；
所述的第二主控单元模块用于控制从UUV的上浮、下潜、任务切换，具有通信和定位一体化功能；
所述的第二动力***用于输出从UUV所需动力；
所述的第二数据处理模块用于从UUV的内部数据处理；
所述的第二时钟模块用于提供时钟基准；
主UUV搭载的高精度惯性导航***INS与从UUV搭载的低精度惯性导航***INS为精度相对高低；
(3)协同定位装置，安装于从UUV上，用于协同定位的算法执行和输出协同定位结果；
所述的协同定位装置包含主导航单元和副导航单元，利用从UUV的INS数据进行位置估计作为主导航单元；利用声学测距和协同定位报文滤波处理作为副导航单元；
协同定位装置向外提供两个接口：与数据处理模块的通信接口和与水声通信机的通信接口；接口通信采用串口通信，发送TX、接收RX与地线连接的方式，使用软件握手XON/XOFF方式建立通信连接；需要通信时，接收端向发送端发送XON，开始接收数据；发送端接收到XON后开始发送数据；不需要通信时，接收端反馈XOFF结束通信；
(3-1)与数据处理模块的通信接口：从UUV默认以1Hz的频率向协同定位装置输入INS采集的经度、纬度、航速、航向、深度、时间戳数据；协同定位装置向从UUV输出协同定位后的经度、纬度数据；
协同定位装置和从UUV的信息交互如下：协同定位装置需要状态信息时，向第二数据处理模块发送XON开始接收信息，直至不需要时，发送XOFF结束通信；第二数据处理模块接收到协同定位装置的XON后，开始发送包含经度、纬度的状态信息，直至接收到XOFF后停止发送；第二数据处理模块需要协同定位结果时，向协同定位装置发送XON，直至不需要该结果后，发送XOFF停止通信；协同定位装置接收到从UUV的XON后开始发送协同定位结果，直至接收到XOFF后停止发送；
(3-2)与水声通信机的通信接口：第二水声通信机接收到主UUV协同定位报文后，将解析后的报文信息与水声通信机记录的该协同定位报文的到达时刻输入给协同定位装置；
协同定位装置和声学通信接收机的信息交互如下：协同定位装置需要第二水声通信机接收的信息时，向第二水声通信机发送XON，直至不需要该信息，发送XOFF；第二水声通信机接收到XON后，接收到主UUV的协同定位报文，向协同定位装置输入协同定位报文数据和其到达时刻，直至接收到XOFF停止发送。
2.如权利要求1所述的基于测距与信息交互的UUV集群协同定位***，其特征在于，该协同定位***的工作模式为：
主UUV融合多普勒测速仪和高精度惯性导航***INS数据，输出高精度的主UUV参考位置，并将关键信息打包成协同定位数据报文，以固定的时间间隔通过第一水声通信机广播给集群中的从UUV，关键信息包括自身的位置、状态协方差矩阵信息、发送时间戳；
从UUV的低精度惯性导航***INS更新频率高，将其采集的包括经度、纬度的数据作为协同定位装置中的主导航单元；协同定位装置中副导航单元更新频率低；当第二水声通信机接收到主UUV的协同定位报文之后，记录协同定位报文的到达时间戳，并结合解析的协同定位报文内容输入给协同定位装置；协同定位装置根据第二水声通信机输入的信息，经过协同定位处理，输出给从UUV更新后的定位信息，完成协同定位。
3.采用权利要求1所述UUV集群协同定位***的协同定位方法，其特征在于，具体是：
步骤(1)主UUV、从UUV下水前进行内部时钟驯服，保持时钟同步，完成驯服后开始水下工作；主UUV的第一卫星单元模块和从UUV的第二卫星单元模块开始工作，主UUV和从UUV的定位信息进行初始化；
步骤(2)进入稳定工作状态后，主UUV的第一数据处理模块以固定周期T′生成协同定位数据报文，协同定位数据报文包括经度LM,k、纬度BM,k、深度dM,k、航速vM,k、航向X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时刻/>下标M表示主UUV，下标k表示在第k定位时隙；协同定位数据报文广播给集群中的从UUV；
步骤(3)第二水声通信机接收到协同定位数据报文后，记录协同定位数据报文的到达时刻解析出协同定位数据报文中的信息，对其中的主UUV的经度和纬度进行处理，转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)，LB2XY(·,·)表示经纬度和XY坐标系的转换运算；第二数据处理模块获取INS的数据，包括从UUV的经度LS,k、纬度BS,k、深度dS,k、航速vS,k、航向/>角加速度ωS,k，下标S表示从UUV；对其中的从UUV的经度和纬度进行处理，转换成XY坐标系位置(xS,k,yS,k)＝LB2XY(LS,k,BS,k)，将主UUV位置、从UUV位置、解析的协同定位数据信息，以及剩余的从UUV第二数据处理模块输出信息，输入给协同定位装置；
步骤(4)协同定位装置建立状态转移方程和测量方程，利用解析出协同定位数据报文中的信息进行滤波处理，完成协同定位；具体是：
(4-1)建立状态向量和状态转移方程：
从UUV处于稳态运行的状态时，第k定位时隙的从UUV的运动状态ψS,k表示为XY坐标系位置和航向的组合[·]T表示转置运算；
在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态运动方程表示为：/>
将当前状态和前序缓存的状态相结合，构建组合状态向量其中N表示缓存的阶数，相应的协方差矩阵为/>
组合后的运动预测模型其中，uS,k-1表示作为测量输入的从UUV的运动速度，nk表示估计误差，服从高斯分布；状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fk，预测模型线性化处理表示为/>其中，B表示输入控制矩阵；
相应的协方差矩阵/>表示为/>其中，Qk为引入的状态转移误差，其中，/>和/>分别表示从UUV测速标准差和测向标准差；
当完成一次状态向量增广操作后，其协方差矩阵同步进行维度增广，修改表示为/>Pk-1[1,1]、Pk-1[1,2]、Pk-1[2,1]、Pk-1[2,2]分别表示协方差矩阵Pk-1的左上、右上、左下和右下子矩阵；
(4-2)对状态向量进行增广：
协同定位装置在原有状态的基础上增添上接收到的主UUV信息ψM,k构建增广状态向量相应的协方差矩阵为/>其中，PM,k表示主UUV状态协方差矩阵；
(4-3)建立测量方程：
根据解析出的发送时间和本地记录的到达时刻/>计算出传播距离/>其中，c为等效声速；
在第k定位时隙，从UUV的空间位置信息表示为主UUV的空间位置信息表示为(xM,k,yM,k,dM,k)；
关于状态向量构建测距方程设测量受一个零均值的高斯白噪声ηS,k影响，则测量模型表示为/>测量偏差矩阵为Rk；
(4-4)滤波更新：
以扩展卡尔曼滤波为基础，引入新息增益吸收新息，更新预测的状态，更新后预测的状态其中，Kk为时隙k的新息增益，/>表示测距方程，Hk为测量方程的Jacobi矩阵；
新息增益K通过测量矩阵和测量偏差得到，
由状态更新和新息增益，同步状态更新的协方差矩阵I为单位矩阵；
(4-5)利用测距信息得到的定位结果：
从中提取出更新的从UUV位置/>与步骤(3)的(xS,k,yS,k)进行比较：当两者差值小于等于设定阈值时，则更新正常，将其转换成经纬度坐标/>进行协同定位结果输出；当两者差值大于设定阈值，或者协同定位装置无法接收到水声通信机输入数据时，从/>提取出从UUV位置/>将其转换成经纬度坐标进行协同定位结果输出。
CN202210102334.4A 2022-01-27 2022-01-27 基于测距与信息交互的uuv集群协同定位***和方法 Active CN114485645B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210102334.4A CN114485645B (zh) 2022-01-27 2022-01-27 基于测距与信息交互的uuv集群协同定位***和方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210102334.4A CN114485645B (zh) 2022-01-27 2022-01-27 基于测距与信息交互的uuv集群协同定位***和方法

Publications (2)

Publication Number Publication Date
CN114485645A CN114485645A (zh) 2022-05-13
CN114485645B true CN114485645B (zh) 2024-04-30

Family

ID=81476139

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210102334.4A Active CN114485645B (zh) 2022-01-27 2022-01-27 基于测距与信息交互的uuv集群协同定位***和方法

Country Status (1)

Country Link
CN (1) CN114485645B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116401618B (zh) * 2023-03-03 2023-12-01 南京航空航天大学 基于几何分布采样的跨域无人集群协同导航信息融合方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103336267A (zh) * 2013-07-02 2013-10-02 哈尔滨工程大学 一种基于水声通信延迟的主从式多uuv协同定位方法
CN105319534A (zh) * 2015-11-09 2016-02-10 哈尔滨工程大学 一种基于水声双程测距的多auv协同定位方法
CN108303715A (zh) * 2017-12-19 2018-07-20 浙江大学 基于北斗信标的水下移动节点无源定位方法及其***
CN112525218A (zh) * 2020-11-23 2021-03-19 哈尔滨工程大学 一种ins/dvl组合导航***鲁棒智能协同校准方法
CN113203411A (zh) * 2021-04-23 2021-08-03 南京理工大学 一种弹性嵌入式gnss/惯性组合导航***及方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108955678B (zh) * 2018-08-03 2019-08-20 国家深海基地管理中心 一种深海运载器通信定位导航授时一体化方法及***

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103336267A (zh) * 2013-07-02 2013-10-02 哈尔滨工程大学 一种基于水声通信延迟的主从式多uuv协同定位方法
CN105319534A (zh) * 2015-11-09 2016-02-10 哈尔滨工程大学 一种基于水声双程测距的多auv协同定位方法
CN108303715A (zh) * 2017-12-19 2018-07-20 浙江大学 基于北斗信标的水下移动节点无源定位方法及其***
CN112525218A (zh) * 2020-11-23 2021-03-19 哈尔滨工程大学 一种ins/dvl组合导航***鲁棒智能协同校准方法
CN113203411A (zh) * 2021-04-23 2021-08-03 南京理工大学 一种弹性嵌入式gnss/惯性组合导航***及方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种基于无迹卡尔曼滤波的UUV协同定位方法;刘明雍;沈俊元;张加全;胡俊伟;;鱼雷技术;20110615(03);全文 *
通信延迟下的多UUV协同定位――基于航迹预测的实时更新算法;姚尧;徐德民;张立川;严卫生;;机器人;20110315(02);全文 *

Also Published As

Publication number Publication date
CN114485645A (zh) 2022-05-13

Similar Documents

Publication Publication Date Title
CN105319534B (zh) 一种基于水声双程测距的多auv协同定位方法
Xiong et al. Robust GPS/INS/DVL navigation and positioning method using adaptive federated strong tracking filter based on weighted least square principle
Munafo et al. An acoustic network navigation system
CN110837854B (zh) 一种基于因子图的auv多源信息融合方法和设备
US8423289B2 (en) Inter-moving body interferometric positioning system, device and method thereof
CN109814069B (zh) 一种基于单定位信标的水下移动节点无源定位方法及其***
CN105806339B (zh) 一种基于gnss、ins和守时***的组合导航方法和设备
CN111595348A (zh) 一种自主水下航行器组合导航***的主从式协同定位方法
Allotta et al. Cooperative navigation of AUVs via acoustic communication networking: field experience with the Typhoon vehicles
CN109974706A (zh) 一种基于双运动模型的主从式多auv协同导航方法
CN114485645B (zh) 基于测距与信息交互的uuv集群协同定位***和方法
CN111578944B (zh) 一种基于单信标的水下滑翔机定位方法
CN110865333B (zh) 洋流影响下水下滑翔机单信标无源声学定位方法
Choi et al. Broadband acoustic communication aided underwater inertial navigation system
Jiang et al. Improved decentralized multi-sensor navigation system for airborne applications
US11719784B2 (en) Method for positioning underwater glider based on virtual time difference of arrival of single beacon
CN114485646B (zh) 一种集成超短基线***的uuv集群协同定位方法
US20240098451A1 (en) Systems and methods for differential and non-differential navigation with cellular signals
CN111801595A (zh) 用于全球导航卫星***的多径管理
CN113639762B (zh) 一种近场水下固定式多元直线阵列三维校正方法
CN109931935B (zh) 基于距离和环境特征的导航***及其参数摄动解决方法
CN113671442B (zh) 基于矢量水听器技术的水下无人集群导航定位方法
Chen et al. Overview of the integrated navigation system research status and its development integrated with 5G communication
Wang et al. Distributed collaborative navigation based on rank-defect free network
Sandru et al. Kalman and Butterworth filtering for GNSS/INS data

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