CN114485646A - 一种集成超短基线系统的uuv集群协同定位方法 - Google Patents

一种集成超短基线系统的uuv集群协同定位方法 Download PDF

Info

Publication number
CN114485646A
CN114485646A CN202210102709.7A CN202210102709A CN114485646A CN 114485646 A CN114485646 A CN 114485646A CN 202210102709 A CN202210102709 A CN 202210102709A CN 114485646 A CN114485646 A CN 114485646A
Authority
CN
China
Prior art keywords
uuv
usbl
slave
positioning
result
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
CN202210102709.7A
Other languages
English (en)
Other versions
CN114485646B (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 CN202210102709.7A priority Critical patent/CN114485646B/zh
Publication of CN114485646A publication Critical patent/CN114485646A/zh
Application granted granted Critical
Publication of CN114485646B publication Critical patent/CN114485646B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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/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
    • 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/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • 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)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种集成超短基线系统的UUV集群协同定位方法。本发明方法包括：主从UUV内部时钟驯服；主UUV融合INS和DVL数据，周期性将自身位置和位置估计协方差矩阵打包成协同定位数据报文广播给集群中的从UUV；从UUV接收协同定位报文，提取所需的协同定位数据和USBL定位数据；从UUV建立结合状态向量，根据距离测量构建测量方程，利用扩展卡尔曼滤波进行位置更新，得到更新后的从UUV位置；从UUV的USBL阵列与处理模块利用USBL定位数据计算输出USBL定位结果，并利用从UUV定位结果进行第二级滤波处理，完成协同定位。本发明方法采用两级滤波处理设计，提升集群整体的定位性能。

Description

一种集成超短基线系统的UUV集群协同定位方法
技术领域
本发明属于声学定位技术领域，尤其是水下航行器集群声学定位技术领域，具体涉及一种集成超短基线系统的UUV集群协同定位方法。
背景技术
随着海洋探索日益发展，无人水下航行器(Unmanned underwater vehicle,UUV)越来越多地承担了数据采集、环境监控等任务。单体UUV存在着覆盖范围小、载荷少等缺陷，无法胜任日益繁重且复杂的探索任务。因此UUV集群的工作模式逐渐发展起来。这种方式允许多个UUV独立进行工作，以编队的形式完成任务。定位技术是UUV集群协同工作的关键技术。受到水下环境的限制，水下定位不像陆地上那么容易。一般来说，水下大多使用声信号作为通信和定位的信号载体，在此基础上也发展了很多基于声学的定位方法。利用声学方法来发展高精度的UUV集群协同定位方法正逐步成为一个热门的研究话题，具有较高的现实意义。
UUV集群可以根据其内部的UUV结构，分为主从式、分层式和并行式三种工作模式。主从模式是目前最为主流的UUV集群结构。对于主从式结构的UUV集群来说，存在一个或者多个领航者UUV，这类UUV被称为主UUV，通常体积大、负载能力强，搭载着数量更多、精度更高的载荷；跟随领航者航行的UUV，被称为从UUV，从UUV相对的体积较小、负载能力较弱。主UUV利用惯性导航系统(Inertial navigation system,INS)和多普勒测速仪(Dopplervelocity log,DVL)可以具有较高的定位精度，而从UUV的定位能力相比于主UUV要差得多，甚至部分从UUV不具备定位能力。如果从UUV可以和主UUV存在相对观测，那么可以通过和主UUV的信息交互实现定位信息共享，提高从UUV的定位精度或使其中不具备定位能力的从UUV获得定位能力，进而提升整个集群的定位性能。
发明内容
本发明的一个目的就是针对现有主从UUV集群协同定位的需求，提供了一种集成超短基线系统(Ultrashort baseline system,USBL)的UUV集群协同定位方法。
本发明方法的系统由一台主UUV和多台从UUV构成；主UUV搭载多普勒测速仪DVL、高精度惯性导航系统INS、卫星单元模块、水声通信机、主控单元模块、动力系统、数据处理模块和时钟模块；每台从UUV搭载低精度惯性导航系统INS、卫星单元模块、水声通信机、USBL阵列与处理模块、主控单元模块、动力系统、协同定位处理模块和时钟模块。
具体协同定位方法是：
步骤(1)主UUV和从UUV进行内部时钟驯服，完成驯服后开始水下工作。主、从UUV的卫星单元模块工作，主、从UUV位置信息初始化。
步骤(2)主UUV的数据处理模块融合INS和DVL数据，输出高精度的定位结果，并以固定周期T′生成协同定位数据报文，协同定位数据报文包括经度LM,k、纬度BM,k、深度dM,k、航速vM,k、航向
Figure BDA0003492980210000021
X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时间
Figure BDA0003492980210000022
下标M表示主UUV，下标k表示在第k定位时隙，协同定位数据报文广播给集群中的从UUV。
步骤(3)从UUV的数据处理模块获取自身INS的航行数据，包括INS估计的经度LS,k、纬度BS,k、深度dS,k、航速vS,k和航向
Figure BDA0003492980210000023
对其中从UUV经度和纬度进行处理，下标S表示从UUV；转换成XY坐标系位置(xS-INS,k,yS-INS,k)＝LB2XY(LS,k,BS,k)，LB2XY(·,·)表示经纬度和XY坐标系的转换运算；将该位置和剩余信息输入给协同定位处理模块。
从UUV的水声通信机接收到协同定位数据报文后，记录该报文的到达时刻
Figure BDA0003492980210000024
并解析其中包含的数据。对协同定位数据报文中的主UUV经度和纬度进行处理，转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)；构建主UUV状态向量
Figure BDA0003492980210000025
和主UUV状态协方差矩阵
Figure BDA0003492980210000026
[·]T表示矩阵或矢量的转置运算；将ψM,k、PM,k、剩余协同定位报文数据和报文到达时刻输入给协同定位处理模块。
步骤(4)从UUV的协同定位处理模块建立状态转移方程和测量方程，利用步骤(3)中的信息进行第一级滤波处理。具体是：
(4-1)建立状态向量和状态转移方程：
从UUV第k定位时隙的运动状态表示为XY坐标系位置和航向的组合
Figure BDA0003492980210000027
在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态
Figure BDA0003492980210000028
运动方程表示为：
Figure BDA0003492980210000029
将当前状态和前序缓存的状态相结合，构建组合状态向量,
Figure BDA0003492980210000031
其中N表示缓存的阶数，相应的协方差矩阵为
Figure BDA0003492980210000032
组合后的运动预测模型
Figure BDA0003492980210000033
其中，uS,k表示作为测量输入的从UUV的运动速度，nk表示估计误差，服从高斯分布。状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fk，预测模型线性化处理表示为
Figure BDA0003492980210000034
其中，B表示输入控制矩阵。
Figure BDA0003492980210000035
相应的协方差矩阵
Figure BDA0003492980210000036
表示为
Figure BDA0003492980210000037
Qk为引入的状态转移误差，
Figure BDA0003492980210000038
其中，
Figure BDA0003492980210000039
和
Figure BDA00034929802100000310
分别表示测速标准差和测向标准差。
当完成一次状态向量增广操作后，
Figure BDA00034929802100000311
其协方差矩阵同步进行维度增广，修改表示为
Figure BDA00034929802100000312
Pk-1[1,1]、Pk-1[1,2]、Pk-1[2,1]、Pk-1[2,2]分别表示协方差矩阵Pk-1的左上、右上、左下和右下子矩阵。
(4-2)结合ψM,k信息对状态向量进行增广：
从UUV定位与协同定位处理模块在原有状态的基础上增添上接收到的主UUV信息ψM,k，构建增广状态向量
Figure BDA00034929802100000313
相应的协方差矩阵为
Figure BDA00034929802100000314
其中PM,k表示主UUV状态协方差矩阵。
(4-3)建立测量方程：
根据解析协同定位数据报文得到的发送时间
Figure BDA00034929802100000315
和本地记录的到达时刻
Figure BDA00034929802100000316
从UUV计算出传播距离
Figure BDA00034929802100000317
其中c为等效声速。
在第k个定位时隙，结合步骤(4-1)输出的
Figure BDA00034929802100000318
中提取出从UUV的XY位置信息和步骤(3)的深度信息，得到从UUV的空间位置为
Figure BDA00034929802100000319
主UUV的空间信息表示为(xM,k,yM,k,dM,k)。
关于状态向量构建测距方程
Figure BDA00034929802100000320
假设测量受一个零均值的高斯白噪声影响ηS,k，则测量模型表示为
Figure BDA0003492980210000041
测量偏差矩阵为Rk。
(4-4)滤波更新：
测量关于状态量是一个非线性函数，可以将其线性化处理，求其Jacobi矩阵H。结合上述步骤(4-1)-(4-3)，以扩展卡尔曼滤波(extended Kalman filter,EKF)为基础，引入新息增益K吸收新息，以更新预测的状态：
Figure BDA0003492980210000042
其中，
Figure BDA0003492980210000043
为更新后预测的状态，Kk为时隙k的新息增益，
Figure BDA0003492980210000044
表示测距方程，Hk为测量方程的Jacobi矩阵。
新息增益通过测量矩阵和测量偏差矩阵得到：
Figure BDA0003492980210000045
由状态更新和新息增益，同步状态更新的协方差矩阵
Figure BDA0003492980210000046
I为单位矩阵。
(4-5)输出利用测距信息得到的定位结果：
从
Figure BDA0003492980210000047
中提取出更新的从UUV位置
Figure BDA0003492980210000048
与步骤(3)的定位结果(xS-INS,k,yS-INS,k)进行比较：当两者偏差小于设定阈值时，此时更新正常时，将该结果与相应的协方差矩阵进行输出；当两者偏差大于设定阈值时，舍弃此时更新后的结果，从
Figure BDA0003492980210000049
提取出从UUV位置
Figure BDA00034929802100000410
将该结果与相应的协方差矩阵进行输出。
第一级滤波处理的结果即为从UUV位置ψfirst,k和相应的协方差矩阵Pfirst,k。
步骤(5)从UUV的USBL阵列与处理模块接收到协同定位数据报文后，将USBL定位数据，包括USBL阵元的间距、信号时延差，输入给协同定位处理模块，得到USBL计算结果，并利用步骤(4)中得到的从UUV第一级滤波结果进行第二级滤波处理，输出从UUV的协同定位结果。具体是：
(5-1)从UUV协同定位处理模块根据输入的USBL定位数据，得到从UUV的USBL定位结果(xUSBL,k,yUSBL,k)＝getUSBL(xM,k,yM,k,zM,k,rk,d,τx,τy)；其中，getUSBL(·)表示USBL定位运算，d为USBL阵元的间距，τx和τy为USBL的X方向和Y方向阵元接收到的信号到达时延差。
从第一级滤波处理的结果ψfirst,k中提取从UUV的位置(xfirst,k,yfirst,k)，与USBL定位结果(xUSBL,k,yUSBL,k)进行比较；当两者偏差大于设定阈值时，将当前USBL的定位结果作为野值进行剔除，并将(xfirst,k,yfirst,k)转换成经纬度坐标(Lfirst,k,Bfirst,k)＝XY2LB(xfirst,k,yfirst,k),作为协同定位结果进行输出，跳过后续步骤，完成协同定位；否则执行(5-2)。
(5-2)将(xUSBL,k,yUSBL,k)作为测量值zUSBL,k，相应的测量协方差矩阵RUSBL,k表示为
Figure BDA0003492980210000051
σx,k和σy,k为USBL定位的X、Y方向估计标准差。此时的测量方程关于状态向量是线性的，测量矩阵HUSBL,k表示为
Figure BDA0003492980210000052
(5-3)由第一级滤波处理的结果ψfirst,k和Pfirst,k，和第k-1定位时隙第二级滤波输出的状态更新
Figure BDA0003492980210000053
和协方差矩阵
Figure BDA0003492980210000054
得到第二级滤波器的预测，表示为：
Figure BDA0003492980210000055
构建更新方程
Figure BDA0003492980210000056
其中，新息增益
Figure BDA0003492980210000057
更新后协方差
Figure BDA0003492980210000058
(5-4)从
Figure BDA0003492980210000059
中提取出更新的从UUV位置
Figure BDA00034929802100000510
将其转换成经纬度坐标
Figure BDA00034929802100000511
当更新的经纬度正常时，进行协同定位结果输出；当结果异常时，舍弃此时更新后的结果，将(4-5)输出的结果作为协同定位结果，将其转换成经纬度坐标后进行输出，完成从UUV的协同定位。
本发明方法利用协同定位数据报文构建主UUV和从UUV之间的协同定位模型，结合USBL定位结果构建滤波器，提升集群整体的定位性能。本发明中的协同定位数据报文同时可以作为USBL定位信号，从UUV可以使用该协同定位数据报文建立测距方程，进行第一级滤波处理更新自身位置，并进一步利用USBL计算结果对第一级滤波处理结果进行再处理。相比于传统方法将USBL安装于主UUV上的方式，本发明将USBL安装于从UUV上，方便从UUV及时利用USBL定位自身位置。此外，本发明的两级滤波处理设计，利用第一级的处理结果对USBL定位结果进行判定，较好的解决了USBL野值无法剔除的问题。
附图说明
图1是本发明中主、从UUV的协同定位处理逻辑示意图。
具体实施方式
以下结合具体实施方式和附图对本发明做进一步说明。图1给出了一种本发明的主、从UUV的定位处理逻辑。
集成超短基线系统的UUV集群协同定位方法，其协同定位系统构成包括：
(1)一台主UUV；搭载多普勒测速仪DVL、高精度惯性导航系统INS、卫星单元模块、水声通信机、主控单元模块、动力系统、数据处理模块和时钟模块。其中：
多普勒测速仪DVL进行对水底锁定，用于测量主UUV航速；
高精度惯性导航系统INS用于估计主UUV自身位置和相应的航行数据；
卫星单元模块用于主UUV浮出海面时，接收卫星位置更新和与控制中心的通信；
水声通信机用于主从UUV在水下的通信，主UUV向控制中心汇报状态、接收控制中心的指令与数据以及发送USBL定位信号，具有通信和定位一体化功能；
主控单元模块用于控制主UUV的上浮、下潜、任务切换；
动力系统用于输出主UUV所需动力；
数据处理模块用于结合DVL和INS数据，输出主UUV位置，并产生协同定位报文；
时钟模块用于提供时钟基准。
(2)三台从UUV；每台从UUV搭载低精度惯性导航系统INS、卫星单元模块、水声通信机、USBL阵列与处理模块、主控单元模块、动力系统、协同定位处理模块和时钟模块。其中：
低精度惯性导航系统INS输出从UUV的航行数据和位置信息；
卫星单元模块用于从UUV浮出海面时，接收卫星位置更新和与控制中心的通信；
水声通信机用于从主UUV在水下的通信，以及接收协同定位数据报文；
USBL阵列与处理模块用于接收USBL定位信号，输出USBL定位数据；
主控单元模块用于控制从UUV的上浮、下潜、任务切换；
动力系统用于输出从UUV所需动力；
协同定位处理模块是协同定位系统的核心模块，用于从UUV的协同定位算法执行，输出协同定位结果；内含利用INS数据进行位置估计的主导航单元和由利用USBL数据和协同定位报文数据的两级滤波处理器构成的副导航单元；
时钟模块用于提供时钟基准。
该协同定位系统的工作模式为：主UUV融合DVL和高精度INS，输出高精度的主UUV参考位置，并将自身的位置状态、状态协方差矩阵、发送时间戳等关键信息打包成协同定位数据报文，以固定的时间间隔通过水声通信机广播给集群中的从UUV；从UUV的水声通信机接收到协同定位数据报文之后，记录该协同定位数据报文的到达时间戳，并结合解析的协同定位报文数据一同打包输入给协同定位处理模块；协同定位处理模块根据输入的协同定位报文数据和到达时间戳，建立第一级滤波处理回路，将从UUV第一级滤波更新后的定位结果输入给第二级滤波处理回路；同时，从UUV的USBL阵列与处理模块接收到主UUV的协同定位数据报文后，得到USBL定位数据，将USBL定位数据输入给协同定位处理模块；当协同定位处理模块接收到USBL定位数据后，进行USBL定位计算，并将USBL定位结果和第一级滤波处理输入的数据联合进行第二级滤波处理，输出协同定位结果，完成协同定位。
高精度惯性导航系统INS与低精度惯性导航系统INS为精度相对高低。
UUV集群包括一台主UUV和三台从UUV，从UUV使用相同的协同定位方法，其中第i台从UUV，i∈{1，2，3}，具体协同定位方法如图1所示：
步骤(1)主(下标M)、从UUV(下标Si，i∈{1，2，3})进行内部时钟驯服，完成驯服后开始水下工作。主、从UUV的卫星单元模块工作，主、从UUV对定位信息进行初始化。
步骤(2)主UUV的数据处理模块融合INS和DVL数据，输出高精度的定位结果，并以固定周期T′生成协同定位数据报文，在第k定位时隙中，协同定位数据报文包括经度LM,k、纬度BM,k、深度DM,k、航速vM,k、航向
Figure BDA0003492980210000071
X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时间
Figure BDA0003492980210000072
协同定位数据报文广播给集群中的从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_TIME以及2个字节的纠错冗余FCC，如下表所示，随后广播给集群中的从UUV。
SYN LEN M_LON M_LAT M_DEP M_VEL M_HEAD M_UNCX M_UNCY M_TIME FCC
1 1 4 4 2 2 2 2 2 4 2
步骤(3)第i台从UUV的数据处理模块获取自身INS的航行数据，包括INS估计的经度LSi,k、纬度BSi,k、深度DSi,k、航速vSi,k、航向
Figure BDA0003492980210000073
对其中从UUV经度和纬度进行处理，转换成XY坐标系位置(xSi-INS,k,ySi-INS,k)＝LB2XY(LSi,k,BSi,k)，将该位置和剩余信息打包输入给协同定位处理模块。
第i台从UUV的水声通信机接收到协同定位数据报文后，记录该报文的到达时刻
Figure BDA0003492980210000081
并解析其中包含的数据。对协同定位数据报文中的主UUV经度和纬度进行处理，转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)。转换完成后，构建主UUV状态向量
Figure BDA0003492980210000082
和主UUV状态协方差矩阵
Figure BDA0003492980210000083
[·]T表示矩阵或者矢量的转置；将ψM,k、PM,k、剩余协同定位报文数据和到达时刻一同打包输入给协同定位处理模块。
步骤(4)第i台从UUV的协同定位处理模块建立状态转移方程和测量方程，利用步骤(3)中的信息进行第一级滤波处理。具体方法是：
(4-1)建立状态向量和状态转移方程：
第i台从UUV第k定位时隙的运动状态表示为XY坐标系位置和航向的组合
Figure BDA0003492980210000084
在固定周期T′的更新频率之下，第i台从UUV第k定位时隙的运动方程表示为：
Figure BDA0003492980210000085
将当前状态和前序缓存的状态相结合，构建组合状态向量,
Figure BDA0003492980210000086
其中N表示缓存的阶数，相应的协方差矩阵为
Figure BDA0003492980210000087
组合后的运动预测模型
Figure BDA0003492980210000088
其中，uSi,k表示作为测量输入的第i台从UUV的第k定位时隙的运动速度，ni,k表示估计误差，服从高斯分布。状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fi,k，预测模型线性化处理表示为
Figure BDA0003492980210000089
其中，Bi表示输入控制矩阵。
Figure BDA00034929802100000810
相应的协方差矩阵
Figure BDA00034929802100000811
表示为
Figure BDA00034929802100000812
其中，Qi,k为引入的状态转移误差，
Figure BDA00034929802100000813
其中,
Figure BDA00034929802100000814
和
Figure BDA00034929802100000815
分别表示第i台从UUV的测速标准差和测向标准差。
当完成一次状态向量增广操作后，
Figure BDA0003492980210000091
其协方差矩阵同步进行维度增广，修改表示为
Figure BDA0003492980210000092
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)结合ψM,k信息对状态向量进行增广：
第i台从UUV的协同定位处理模块在原有状态的基础上增添上接收到的主UUV信息ψM,k，构建增广状态向量
Figure BDA0003492980210000093
相应的协方差矩阵为
Figure BDA0003492980210000094
Figure BDA0003492980210000095
其中，PM,k表示主UUV状态协方差矩阵。
(4-3)建立测量方程：
根据解析出的发送时间
Figure BDA0003492980210000096
和本地记录的到达时刻
Figure BDA0003492980210000097
计算出传播距离
Figure BDA0003492980210000098
其中，c为等效声速。
在第k个定位时隙，结合步骤(4-1)输出的
Figure BDA0003492980210000099
中提取出第i台从UUV的XY位置信息和步骤(3)的深度信息，得到第i台从UUV的空间位置
Figure BDA00034929802100000910
主UUV的空间信息表示为(xM,k,yM,k,dM,k)。
关于状态向量构建测距方程
Figure BDA00034929802100000911
设测量受一个零均值的高斯白噪声ηSi,k影响，则测量模型表示为
Figure BDA00034929802100000912
测量偏差矩阵为Ri,k。
(4-4)滤波更新：
由于测量关于状态量是一个非线性函数，因此可以将其线性化处理，求其Jacobi矩阵H。结合上述步骤(4-1)-(4-3)，以扩展卡尔曼滤波(extended Kalman filter,EKF)为基础，引入新息增益K吸收新息，以更新预测的状态：
Figure BDA00034929802100000913
Figure BDA00034929802100000914
为更新后预测的状态，Ki,k为k时刻新息增益，
Figure BDA00034929802100000915
表示测距方程，Hi,k为测量方程的Jacobi矩阵。
新息增益K通过测量矩阵和测量偏差R得到：
Figure BDA0003492980210000101
由状态更新和新息增益，同步状态更新的协方差矩阵
Figure BDA0003492980210000102
I为单位矩阵。
(4-5)输出利用测距信息得到的定位结果
从
Figure BDA0003492980210000103
中提取出第i台从UUV的位置
Figure BDA0003492980210000104
与步骤(3)的定位结果(xSi-INS,k,ySi-INS,k)进行比较：当两者偏差小于设定阈值时，此时更新正常时，将该结果与相应的协方差矩阵进行输出；当两者偏差大于设定阈值时，舍弃此时更新后的结果，从
Figure BDA0003492980210000105
提取出第i台从UUV位置
Figure BDA0003492980210000106
将该结果与相应的协方差矩阵进行输出。
第一级滤波处理的结果即为第i台从UUV的位置ψi,first,k和相应的协方差矩阵Pi,first,k。
步骤(5)第i台从UUV的USBL阵列与处理模块接收到协同定位数据报文后，将USBL定位数据，包括USBL阵元的间距、信号时延差，输入给协同定位处理模块，得到USBL计算结果，并利用步骤(4)中得到的从UUV第一级滤波结果进行第二级滤波处理，输出从UUV的协同定位结果。具体是：
(5-1)第i台从UUV的协同定位处理模块根据输入的USBL定位数据，得到从UUV的USBL定位结果(xi,USBL,k,yi,USBL,k)＝getUSBL(xM,k,yM,k,zM,k,ri,k,di,τi,x,τi,y)；其中，getUSBL()表示USBL定位操作，di为第i台从UUV的USBL阵元的间距，τi,x和τi,y为第i台从UUV的USBL的X方向和Y方向阵元接收到的信号时延差。
从第一级滤波处理的结果ψi,first,k中提取第i台从UUV的位置(xi,first,k,yi,first,k)，与USBL定位结果(xi,USBL,k,yi,USBL,k)进行比较；当两者偏差大于设定阈值时，将当前USBL的定位结果作为野值进行剔除，并将(xi,first,k,yi,first,k)转换成经纬度坐标(Li,first,k,Bi,first,k)＝XY2LB(xi,first,k,yi,first,k),作为协同定位结果进行协同定位输出，跳过后续步骤，完成协同定位；否则执行(5-2)。
(5-2)将(xi,USBL,k,yi,USBL,k)作为测量值zi,USBL,k，相应的测量协方差矩阵Ri,USBL,k表示为
Figure BDA0003492980210000107
σx,k和σy,k为USBL定位的XY方向估计标准差。此时的测量方程关于状态向量是线性的，测量矩阵Hi,USBL,k表示为
Figure BDA0003492980210000111
(5.3)由第一级滤波处理的结果ψi,first,k和Pi,first,k，和第k-1定位时隙第二级滤波输出的状态更新
Figure BDA0003492980210000112
和协方差矩阵
Figure BDA0003492980210000113
得到第二级滤波器的预测，表示为：
Figure BDA0003492980210000114
建立更新方程
Figure BDA0003492980210000115
其中，新息增益
Figure BDA0003492980210000116
更新后协方差
Figure BDA0003492980210000117
(5.4)从
Figure BDA0003492980210000118
中提取出更新的第i台从UUV位置
Figure BDA0003492980210000119
将其转换成经纬度坐标
Figure BDA00034929802100001110
当更新的经纬度正常时，进行协同定位结果输出；当结果异常时，舍弃此时更新后的结果，将(4.5)输出的结果作为协同定位结果，将其转换成经纬度坐标后进行输出，完成第i台从UUV的协同定位。

Claims (1)

1.一种集成超短基线系统的UUV集群协同定位方法，其系统由一台主UUV和多台从UUV构成；主UUV搭载多普勒测速仪DVL、高精度惯性导航系统INS、卫星单元模块、水声通信机、主控单元模块、动力系统、数据处理模块和时钟模块；每台从UUV搭载低精度惯性导航系统INS、卫星单元模块、水声通信机、USBL阵列与处理模块、主控单元模块、动力系统、协同定位处理模块和时钟模块；其特征在于，具体方法是：
步骤(1)主UUV和从UUV进行内部时钟驯服，完成驯服后开始水下工作；主、从UUV的卫星单元模块工作，主、从UUV位置信息初始化；
步骤(2)主UUV的数据处理模块融合INS和DVL数据，输出高精度的定位结果，并以固定周期T′生成协同定位数据报文，协同定位数据报文包括经度LM,k、纬度BM,k、深度dM,k、航速vMk、航向
Figure FDA0003492980200000011
X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时间
Figure FDA0003492980200000012
下标M表示主UUV，下标k表示在第k定位时隙；协同定位数据报文广播给集群中的从UUV；
步骤(3)从UUV的数据处理模块获取自身INS的航行数据，包括INS估计的经度LS,k、纬度BS，k、深度dS，k、航速vS，k和航向
Figure FDA0003492980200000013
对其中从UUV经度和纬度进行处理，下标S表示从UUV；转换成XY坐标系位置(xS-INS,k,yS-INS,k)＝LB2XY(LS,k,BS,k)，LB2XY(·,·)表示经纬度和XY坐标系的转换运算；将该位置和剩余信息输入给协同定位处理模块；
从UUV的水声通信机接收到协同定位数据报文后，记录该报文的到达时刻
Figure FDA0003492980200000014
并解析其中包含的数据；对协同定位数据报文中的主UUV经度和纬度进行处理，转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)；构建主UUV状态向量
Figure FDA0003492980200000015
和主UUV状态协方差矩阵
Figure FDA0003492980200000016
[·]T表示矩阵或矢量的转置运算；将ψM,k、PM,k、剩余协同定位报文数据和报文到达时刻输入给协同定位处理模块；
步骤(4)从UUV的协同定位处理模块建立状态转移方程和测量方程，并进行第一级滤波处理；
(4-1)建立状态向量和状态转移方程：
从UUV第k定位时隙的运动状态表示为XY坐标系位置和航向的组合
Figure FDA0003492980200000021
在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态
Figure FDA0003492980200000022
运动方程表示为：
Figure FDA0003492980200000023
将当前状态和前序缓存的状态相结合，构建组合状态向量,
Figure FDA0003492980200000024
其中N表示缓存的阶数，相应的协方差矩阵为
Figure FDA0003492980200000025
组合后的运动预测模型
Figure FDA0003492980200000026
其中，uS,k表示作为测量输入的从UUV的运动速度，nk表示估计误差，服从高斯分布；状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fk，预测模型线性化处理表示为
Figure FDA0003492980200000027
其中，B表示输入控制矩阵；
Figure FDA0003492980200000028
相应的协方差矩阵
Figure FDA0003492980200000029
表示为
Figure FDA00034929802000000210
Qk为引入的状态转移误差，
Figure FDA00034929802000000211
其中，
Figure FDA00034929802000000212
和
Figure FDA00034929802000000213
分别表示测速标准差和测向标准差；
当完成一次状态向量增广操作后，
Figure FDA00034929802000000214
其协方差矩阵同步进行维度增广，修改表示为
Figure FDA00034929802000000215
Pk-1[1,1]、Pk-1[1,2]、Pk-1[2,1]、Pk-1[2,2]分别表示协方差矩阵Pk-1的左上、右上、左下和右下子矩阵；
(4-2)结合ψM,k信息对状态向量进行增广：
从UUV定位与协同定位处理模块在原有状态的基础上增添上接收到的主UUV信息ψM,k，构建增广状态向量
Figure FDA00034929802000000216
相应的协方差矩阵为
Figure FDA00034929802000000217
其中PM,k表示主UUV状态协方差矩阵；
(4-3)建立测量方程：
根据解析协同定位数据报文得到的发送时间
Figure FDA0003492980200000031
和本地记录的到达时刻
Figure FDA0003492980200000032
从UUV计算出传播距离
Figure FDA0003492980200000033
其中c为等效声速；
在第k个定位时隙，结合步骤(4-1)输出的
Figure FDA0003492980200000034
中提取出从UUV的XY位置信息和步骤(3)的深度信息，得到从UUV的空间位置为
Figure FDA0003492980200000035
主UUV的空间信息表示为(xM,k,yM,k,dM,k)；
关于状态向量构建测距方程
Figure FDA0003492980200000036
假设测量受一个零均值的高斯白噪声影响ηS,k，则测量模型表示为
Figure FDA0003492980200000037
测量偏差矩阵为Rk；
(4-4)滤波更新：
以扩展卡尔曼滤波为基础，引入新息增益K吸收新息，以更新预测的状态：更新后预测的状态
Figure FDA0003492980200000038
其中，Kk为时隙k的新息增益，
Figure FDA0003492980200000039
表示测距方程，Hk为测量方程的Jacobi矩阵；
新息增益通过测量矩阵和测量偏差矩阵得到：
Figure FDA00034929802000000310
由状态更新和新息增益，同步状态更新的协方差矩阵
Figure FDA00034929802000000311
I为单位矩阵；
(4-5)输出利用测距信息得到的定位结果：
从
Figure FDA00034929802000000312
中提取出更新的从UUV位置
Figure FDA00034929802000000313
与定位结果(xS-INS,k,yS-INS,k)进行比较：当两者偏差小于设定阈值时，此时更新正常时，将该结果与相应的协方差矩阵进行输出；当两者偏差大于设定阈值时，舍弃此时更新后的结果，从
Figure FDA00034929802000000314
提取出从UUV位置
Figure FDA00034929802000000315
将该结果与相应的协方差矩阵进行输出；
第一级滤波处理的结果即为从UUV位置ψfirst,k和相应的协方差矩阵Pfirst,k；
步骤(5)从UUV的USBL阵列与处理模块接收到协同定位数据报文后，将USBL定位数据，包括USBL阵元的间距、信号时延差，输入给协同定位处理模块，得到USBL计算结果，并利用从UUV第一级滤波结果进行第二级滤波处理，输出从UUV的协同定位结果；具体是：
(5-1)从UUV协同定位处理模块根据输入的USBL定位数据，得到从UUV的USBL定位结果(xUSBL,k,yUSBL,k)＝getUSBL(xM,k,yM,k,zM,k,rk,d,τx,τy)；其中，getUSBL(·)表示USBL定位运算，d为USBL阵元的间距，τx和τy为USBL的X方向和Y方向阵元接收到的信号到达时延差；
从第一级滤波处理的结果ψfirst,k中提取从UUV的位置(xfirst,k,yfirst,k)，与USBL定位结果(xUSBL,k,yUSBL,k)进行比较；当两者偏差大于设定阈值时，将当前USBL的定位结果作为野值进行剔除，并将(xfirst,k,yfirst,k)转换成经纬度坐标(Lfirst,k,Bfirst,k)＝XY2LB(xfirst,k,yfirst,k),作为协同定位结果进行输出，跳过后续步骤，完成协同定位；否则执行(5-2)；
(5-2)将(xUSBL,k,yUSBL,k)作为测量值zUSBL,k，相应的测量协方差矩阵RUSBL,k表示为
Figure FDA0003492980200000041
σx,k和σy,k为USBL定位的X、Y方向估计标准差；此时的测量方程关于状态向量是线性的，测量矩阵HUSBL，k表示为
Figure FDA0003492980200000042
(5-3)由第一级滤波处理的结果ψfirst,k和Pfirst,k，和第k-1定位时隙第二级滤波输出的状态更新
Figure FDA0003492980200000043
和协方差矩阵
Figure FDA0003492980200000044
得到第二级滤波器的预测，表示为：
Figure FDA0003492980200000045
构建更新方程
Figure FDA0003492980200000046
其中，新息增益
Figure FDA0003492980200000047
更新后协方差
Figure FDA0003492980200000048
(5-4)从
Figure FDA0003492980200000049
中提取出更新的从UUV位置
Figure FDA00034929802000000410
将其转换成经纬度坐标
Figure FDA00034929802000000411
当更新的经纬度正常时，进行协同定位结果输出；当结果异常时，舍弃此时更新后的结果，将(4-5)输出的结果作为协同定位结果，将其转换成经纬度坐标后进行输出，完成从UUV的协同定位。
CN202210102709.7A 2022-01-27 2022-01-27 一种集成超短基线系统的uuv集群协同定位方法 Active CN114485646B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210102709.7A CN114485646B (zh) 2022-01-27 2022-01-27 一种集成超短基线系统的uuv集群协同定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210102709.7A CN114485646B (zh) 2022-01-27 2022-01-27 一种集成超短基线系统的uuv集群协同定位方法

Publications (2)

Publication Number Publication Date
CN114485646A true CN114485646A (zh) 2022-05-13
CN114485646B CN114485646B (zh) 2024-04-26

Family

ID=81476598

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210102709.7A Active CN114485646B (zh) 2022-01-27 2022-01-27 一种集成超短基线系统的uuv集群协同定位方法

Country Status (1)

Country Link
CN (1) CN114485646B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105910603A (zh) * 2016-04-20 2016-08-31 北京理工大学 一种通信延迟下的多auv协同导航滤波方法
CN112697145A (zh) * 2020-11-17 2021-04-23 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) 一种基于ckf的主从式多水下无人潜器协同定位方法
CN112763979A (zh) * 2020-12-27 2021-05-07 西北工业大学 基于双移动声信标测距辅助的水下航行器协同定位方法
CN112945224A (zh) * 2021-02-05 2021-06-11 哈尔滨工程大学 一种采用因子图和和积算法的多auv协同导航方法
CN113311388A (zh) * 2021-05-27 2021-08-27 湖南国天电子科技有限公司 一种水下机器人的超短基线定位系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105910603A (zh) * 2016-04-20 2016-08-31 北京理工大学 一种通信延迟下的多auv协同导航滤波方法
CN112697145A (zh) * 2020-11-17 2021-04-23 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) 一种基于ckf的主从式多水下无人潜器协同定位方法
CN112763979A (zh) * 2020-12-27 2021-05-07 西北工业大学 基于双移动声信标测距辅助的水下航行器协同定位方法
CN112945224A (zh) * 2021-02-05 2021-06-11 哈尔滨工程大学 一种采用因子图和和积算法的多auv协同导航方法
CN113311388A (zh) * 2021-05-27 2021-08-27 湖南国天电子科技有限公司 一种水下机器人的超短基线定位系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
MATTEO BRESCIANI, ET AL: "Cooperative ASV/AUV system exploiting active acoustic localization", 《2021 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》, 1 October 2021 (2021-10-01) *
严浙平;王璐;: "UUV水下定位方法的研究现状与进展", 哈尔滨工程大学学报, no. 07, 31 July 2017 (2017-07-31) *
徐博;李盛新;张奂;: "一种基于双模型的低成本多AUV协同定位方法", 中国舰船研究, no. 03, 30 June 2020 (2020-06-30) *
闫敬;张立;罗小元;濮彬;关新平;: "异步时钟下基于信息物理融合的水下潜器协同定位算法", 自动化学报, no. 04, 21 November 2018 (2018-11-21) *

Also Published As

Publication number Publication date
CN114485646B (zh) 2024-04-26

Similar Documents

Publication Publication Date Title
CN113048984B (zh) 一种用于水下无人机器人集群的动态定位信息融合方法
Xiong et al. Robust GPS/INS/DVL navigation and positioning method using adaptive federated strong tracking filter based on weighted least square principle
CN107270894B (zh) 基于维数约简的gnss/sins深组合导航系统
CN110646822B (zh) 一种基于惯导辅助的整周模糊度Kalman滤波算法
CN112284384A (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
Havyarimana et al. Achieving reliable intervehicle positioning based on redheffer weighted least squares model under multi-GNSS outages
CN114485645B (zh) 基于测距与信息交互的uuv集群协同定位系统和方法
CN113156368B (zh) 一种基于因子图的误差参数辨识协同定位方法
Yan et al. Optimal rigid graph-based cooperative formation control of AUVs in anchor-free environments
Wang et al. Robust filter method for SINS/DVL/USBL tight integrated navigation system
CN114485646B (zh) 一种集成超短基线系统的uuv集群协同定位方法
CN111928851B (zh) 基于tma技术的多自主水下机器人集群协同导航方法
Pan et al. AUV Tightly Coupled Terrain Aided Navigation Strategy Based on Isogonal MBES Modeling Method
CN110940999A (zh) 一种基于误差模型下的自适应无迹卡尔曼滤波方法
CN109931935B (zh) 基于距离和环境特征的导航系统及其参数摄动解决方法
Li et al. Cooperative positioning algorithm of swarm UAVs based on posterior linearization belief propagation
CN108226976B (zh) 一种RTK用自适应渐消Kalman滤波算法
Tang et al. Factor graph weight particles aided distributed underwater cooperative positioning algorithm
CN115183771A (zh) 一种基于距离与到达角复合测量的多潜器协同定位方法
Chen et al. Overview of the integrated navigation system research status and its development integrated with 5G communication
Zhang et al. A integrated navigation algorithm based on distributed Kalman filter
CN113671442B (zh) 基于矢量水听器技术的水下无人集群导航定位方法
Lin et al. An adaptive square-root unscented Kalman filter for underwater vehicle navigation
Mi et al. A Performance Enhancement Method for Redundant IMU Based on Neural Network and Geometric Constraint
CN118192387B (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