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

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

Info

Publication number
CN114485646B
CN114485646B CN202210102709.7A CN202210102709A CN114485646B CN 114485646 B CN114485646 B CN 114485646B CN 202210102709 A CN202210102709 A CN 202210102709A CN 114485646 B CN114485646 B CN 114485646B
Authority
CN
China
Prior art keywords
uuv
usbl
positioning
result
location
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
CN202210102709.7A
Other languages
English (en)
Other versions
CN114485646A (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

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、航向X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时间/>下标M表示主UUV，下标k表示在第k定位时隙，协同定位数据报文广播给集群中的从UUV。
步骤(3)从UUV的数据处理模块获取自身INS的航行数据，包括INS估计的经度LS,k、纬度BS,k、深度dS,k、航速vS,k和航向对其中从UUV经度和纬度进行处理，下标S表示从UUV；转换成XY坐标系位置(xS-INS,k,yS-INS,k)＝LB2XY(LS,k,BS,k)，LB2XY(·,·)表示经纬度和XY坐标系的转换运算；将该位置和剩余信息输入给协同定位处理模块。
从UUV的水声通信机接收到协同定位数据报文后，记录该报文的到达时刻并解析其中包含的数据。对协同定位数据报文中的主UUV经度和纬度进行处理，转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)；构建主UUV状态向量/>和主UUV状态协方差矩阵/>[·]T表示矩阵或矢量的转置运算；将ψM,k、PM,k、剩余协同定位报文数据和报文到达时刻输入给协同定位处理模块。
步骤(4)从UUV的协同定位处理模块建立状态转移方程和测量方程，利用步骤(3)中的信息进行第一级滤波处理。具体是：
(4-1)建立状态向量和状态转移方程：
从UUV第k定位时隙的运动状态表示为XY坐标系位置和航向的组合
在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态运动方程表示为：/>
将当前状态和前序缓存的状态相结合，构建组合状态向量,其中N表示缓存的阶数，相应的协方差矩阵为/>组合后的运动预测模型/>其中，uS,k表示作为测量输入的从UUV的运动速度，nk表示估计误差，服从高斯分布。状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fk，预测模型线性化处理表示为/>其中，B表示输入控制矩阵。
相应的协方差矩阵/>表示为/>Qk为引入的状态转移误差，其中，/>和/>分别表示测速标准差和测向标准差。
当完成一次状态向量增广操作后，其协方差矩阵同步进行维度增广，修改表示为/>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，构建增广状态向量相应的协方差矩阵为/>其中PM,k表示主UUV状态协方差矩阵。
(4-3)建立测量方程：
根据解析协同定位数据报文得到的发送时间和本地记录的到达时刻/>从UUV计算出传播距离/>其中c为等效声速。
在第k个定位时隙，结合步骤(4-1)输出的中提取出从UUV的XY位置信息和步骤(3)的深度信息，得到从UUV的空间位置为/>主UUV的空间信息表示为(xM,k,yM,k,dM,k)。
关于状态向量构建测距方程假设测量受一个零均值的高斯白噪声影响ηS,k，则测量模型表示为/>测量偏差矩阵为Rk。
(4-4)滤波更新：
测量关于状态量是一个非线性函数，可以将其线性化处理，求其Jacobi矩阵H。结合上述步骤(4-1)-(4-3)，以扩展卡尔曼滤波(extended Kalman filter,EKF)为基础，引入新息增益K吸收新息，以更新预测的状态：其中，/>为更新后预测的状态，Kk为时隙k的新息增益，/>表示测距方程，Hk为测量方程的Jacobi矩阵。
新息增益通过测量矩阵和测量偏差矩阵得到：
由状态更新和新息增益，同步状态更新的协方差矩阵I为单位矩阵。
(4-5)输出利用测距信息得到的定位结果：
从中提取出更新的从UUV位置/>与步骤(3)的定位结果(xS-INS,k,yS-INS,k)进行比较：当两者偏差小于设定阈值时，此时更新正常时，将该结果与相应的协方差矩阵进行输出；当两者偏差大于设定阈值时，舍弃此时更新后的结果，从/>提取出从UUV位置/>将该结果与相应的协方差矩阵进行输出。
第一级滤波处理的结果即为从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表示为σx,k和σy,k为USBL定位的X、Y方向估计标准差。此时的测量方程关于状态向量是线性的，测量矩阵HUSBL,k表示为/>
(5-3)由第一级滤波处理的结果ψfirst,k和Pfirst,k，和第k-1定位时隙第二级滤波输出的状态更新和协方差矩阵/>得到第二级滤波器的预测，表示为：
构建更新方程
其中，新息增益
更新后协方差
(5-4)从中提取出更新的从UUV位置/>将其转换成经纬度坐标当更新的经纬度正常时，进行协同定位结果输出；当结果异常时，舍弃此时更新后的结果，将(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、航向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_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、航向对其中从UUV经度和纬度进行处理，转换成XY坐标系位置(xSi-INS,k,ySi-INS,k)＝LB2XY(LSi,k,BSi,k)，将该位置和剩余信息打包输入给协同定位处理模块。
第i台从UUV的水声通信机接收到协同定位数据报文后，记录该报文的到达时刻并解析其中包含的数据。对协同定位数据报文中的主UUV经度和纬度进行处理，转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)。转换完成后，构建主UUV状态向量和主UUV状态协方差矩阵/>[·]T表示矩阵或者矢量的转置；将ψM,k、PM,k、剩余协同定位报文数据和到达时刻一同打包输入给协同定位处理模块。
步骤(4)第i台从UUV的协同定位处理模块建立状态转移方程和测量方程，利用步骤(3)中的信息进行第一级滤波处理。具体方法是：
(4-1)建立状态向量和状态转移方程：
第i台从UUV第k定位时隙的运动状态表示为XY坐标系位置和航向的组合
在固定周期T′的更新频率之下，第i台从UUV第k定位时隙的运动方程表示为：
将当前状态和前序缓存的状态相结合，构建组合状态向量,其中N表示缓存的阶数，相应的协方差矩阵为/>组合后的运动预测模型/>其中，uSi,k表示作为测量输入的第i台从UUV的第k定位时隙的运动速度，ni,k表示估计误差，服从高斯分布。状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fi,k，预测模型线性化处理表示为/>其中，Bi表示输入控制矩阵。
相应的协方差矩阵/>表示为/>其中，Qi,k为引入的状态转移误差，/>其中,/>和分别表示第i台从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)结合ψM,k信息对状态向量进行增广：
第i台从UUV的协同定位处理模块在原有状态的基础上增添上接收到的主UUV信息ψM,k，构建增广状态向量相应的协方差矩阵为/> 其中，PM,k表示主UUV状态协方差矩阵。
(4-3)建立测量方程：
根据解析出的发送时间和本地记录的到达时刻/>计算出传播距离/>其中，c为等效声速。
在第k个定位时隙，结合步骤(4-1)输出的中提取出第i台从UUV的XY位置信息和步骤(3)的深度信息，得到第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通过测量矩阵和测量偏差R得到：
由状态更新和新息增益，同步状态更新的协方差矩阵I为单位矩阵。
(4-5)输出利用测距信息得到的定位结果
从中提取出第i台从UUV的位置/>与步骤(3)的定位结果(xSi-INS,k,ySi-INS,k)进行比较：当两者偏差小于设定阈值时，此时更新正常时，将该结果与相应的协方差矩阵进行输出；当两者偏差大于设定阈值时，舍弃此时更新后的结果，从/>提取出第i台从UUV位置/>将该结果与相应的协方差矩阵进行输出。
第一级滤波处理的结果即为第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表示为σx,k和σy,k为USBL定位的XY方向估计标准差。此时的测量方程关于状态向量是线性的，测量矩阵Hi,USBL,k表示为/>
(5.3)由第一级滤波处理的结果ψi,first,k和Pi,first,k，和第k-1定位时隙第二级滤波输出的状态更新和协方差矩阵/>得到第二级滤波器的预测，表示为：
建立更新方程
其中，新息增益
更新后协方差
(5.4)从中提取出更新的第i台从UUV位置/>将其转换成经纬度坐标/>当更新的经纬度正常时，进行协同定位结果输出；当结果异常时，舍弃此时更新后的结果，将(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、航向X方向位置定位标准差σMx,k、Y方向位置定位标准差σMy,k、报文发送时间/>下标M表示主UUV，下标k表示在第k定位时隙；协同定位数据报文广播给集群中的从UUV；
步骤(3)从UUV的数据处理模块获取自身INS的航行数据，包括INS估计的经度LS,k、纬度BS，k、深度dS，k、航速vS，k和航向对其中从UUV经度和纬度进行处理，下标S表示从UUV；转换成XY坐标系位置(xS-INS,k,yS-INS,k)＝LB2XY(LS,k,BS,k)，LB2XY(·,·)表示经纬度和XY坐标系的转换运算；将该位置和剩余信息输入给协同定位处理模块；
从UUV的水声通信机接收到协同定位数据报文后，记录该报文的到达时刻并解析其中包含的数据；对协同定位数据报文中的主UUV经度和纬度进行处理，转换成XY坐标系位置(xM,k,yM,k)＝LB2XY(LM,k,BM,k)；构建主UUV状态向量/>和主UUV状态协方差矩阵/>[·]T表示矩阵或矢量的转置运算；将ψM,k、PM,k、剩余协同定位报文数据和报文到达时刻输入给协同定位处理模块；
步骤(4)从UUV的协同定位处理模块建立状态转移方程和测量方程，并进行第一级滤波处理；
(4-1)建立状态向量和状态转移方程：
从UUV第k定位时隙的运动状态表示为XY坐标系位置和航向的组合
在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态运动方程表示为：/>
将当前状态和前序缓存的状态相结合，构建组合状态向量,其中N表示缓存的阶数，相应的协方差矩阵为/>组合后的运动预测模型/>其中，uS,k表示作为测量输入的从UUV的运动速度，nk表示估计误差，服从高斯分布；状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为Fk，预测模型线性化处理表示为/>其中，B表示输入控制矩阵；
相应的协方差矩阵/>表示为/>Qk为引入的状态转移误差，其中，/>和/>分别表示测速标准差和测向标准差；
当完成一次状态向量增广操作后，其协方差矩阵同步进行维度增广，修改表示为/>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，构建增广状态向量相应的协方差矩阵为/>其中PM,k表示主UUV状态协方差矩阵；
(4-3)建立测量方程：
根据解析协同定位数据报文得到的发送时间和本地记录的到达时刻/>从UUV计算出传播距离/>其中c为等效声速；
在第k个定位时隙，结合步骤(4-1)输出的中提取出从UUV的XY位置信息和步骤(3)的深度信息，得到从UUV的空间位置为/>主UUV的空间信息表示为(xM,k,yM,k,dM,k)；
关于状态向量构建测距方程假设测量受一个零均值的高斯白噪声影响ηS,k，则测量模型表示为/>测量偏差矩阵为Rk；
(4-4)滤波更新：
以扩展卡尔曼滤波为基础，引入新息增益K吸收新息，以更新预测的状态：更新后预测的状态其中，Kk为时隙k的新息增益，/>表示测距方程，Hk为测量方程的Jacobi矩阵；
新息增益通过测量矩阵和测量偏差矩阵得到：
由状态更新和新息增益，同步状态更新的协方差矩阵I为单位矩阵；
(4-5)输出利用测距信息得到的定位结果：
从中提取出更新的从UUV位置/>与定位结果(xS-INS,k,yS-INS,k)进行比较：当两者偏差小于设定阈值时，此时更新正常时，将该结果与相应的协方差矩阵进行输出；当两者偏差大于设定阈值时，舍弃此时更新后的结果，从/>提取出从UUV位置/>将该结果与相应的协方差矩阵进行输出；
第一级滤波处理的结果即为从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表示为σx,k和σy,k为USBL定位的X、Y方向估计标准差；此时的测量方程关于状态向量是线性的，测量矩阵HUSBL，k表示为/>
(5-3)由第一级滤波处理的结果ψfirst,k和Pfirst,k，和第k-1定位时隙第二级滤波输出的状态更新和协方差矩阵/>得到第二级滤波器的预测，表示为：
构建更新方程
其中，新息增益
更新后协方差
(5-4)从中提取出更新的从UUV位置/>将其转换成经纬度坐标当更新的经纬度正常时，进行协同定位结果输出；当结果异常时，舍弃此时更新后的结果，将(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 CN114485646A (zh) 2022-05-13
CN114485646B true 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)》.2021,全文. *
UUV水下定位方法的研究现状与进展;严浙平;王璐;;哈尔滨工程大学学报;20170731(第07期);全文 *
一种基于双模型的低成本多AUV协同定位方法;徐博;李盛新;张奂;;中国舰船研究;20200630(第03期);全文 *
异步时钟下基于信息物理融合的水下潜器协同定位算法;闫敬;张立;罗小元;濮彬;关新平;;自动化学报;20181121(第04期);全文 *

Also Published As

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

Similar Documents

Publication Publication Date Title
CN113048984B (zh) 一种用于水下无人机器人集群的动态定位信息融合方法
CN110837854B (zh) 一种基于因子图的auv多源信息融合方法和设备
CN111595348B (zh) 一种自主水下航行器组合导航系统的主从式协同定位方法
CN112284384B (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN109856638B (zh) 一种特定水下目标自动搜索定位的方法
CN109974706B (zh) 一种基于双运动模型的主从式多auv协同导航方法
Allotta et al. Cooperative navigation of AUVs via acoustic communication networking: field experience with the Typhoon vehicles
CN113819906A (zh) 一种基于统计相似度量的组合导航鲁棒滤波方法
Xu et al. A novel measurement information anomaly detection method for cooperative localization
CN110646822B (zh) 一种基于惯导辅助的整周模糊度Kalman滤波算法
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
CN114485645B (zh) 基于测距与信息交互的uuv集群协同定位系统和方法
CN116295386A (zh) 一种基于多传感器融合的水下航行器导航控制系统及方法
CN114485646B (zh) 一种集成超短基线系统的uuv集群协同定位方法
Ma et al. Multi-source information fusion based on factor graph in autonomous underwater vehicles navigation systems
CN110764533A (zh) 一种多水下机器人协同目标搜索方法
CN110793516A (zh) 基于车辆运动模型的组合导航装置、算法及方法
Qin et al. Robust interactive multimodel INS/DVL intergrated navigation system with adaptive model set
CN109582914B (zh) 噪声相关的带偏差系统的并行式融合估计方法
Ma et al. A novel Rauch–Tung–Streibel smoothing scheme based on the factor graph for autonomous underwater vehicles
CN111025364A (zh) 一种基于卫星辅助的机器视觉定位系统及方法
CN109931935B (zh) 基于距离和环境特征的导航系统及其参数摄动解决方法
XU et al. Low-cost multi-AUV cooperative localization method based on dual-model
CN115183771A (zh) 一种基于距离与到达角复合测量的多潜器协同定位方法
Tang et al. Factor graph weight particles aided distributed underwater cooperative positioning algorithm

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