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

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′生成协同定位数据报文，协同定位数据报文包括经度L_M,k、纬度B_M,k、深度d_M,k、航速v_M,k、航向

X方向位置定位标准差σ_Mx,k、Y方向位置定位标准差σ_My,k、报文发送时刻

下标M表示主UUV，下标k表示在第k定位时隙。协同定位数据报文广播给集群中的从UUV。

步骤(3)第二水声通信机接收到协同定位数据报文后，记录协同定位数据报文的到达时刻

解析出协同定位数据报文中的信息，对其中的主UUV的经度和纬度进行处理，转换成XY坐标系位置(x_M,k,y_M,k)＝LB2XY(L_M,k,B_M,k)，LB2XY(·,·)表示经纬度和XY坐标系的转换运算。第二数据处理模块获取INS的数据，包括从UUV的经度L_S,k、纬度B_S,k、深度d_S,k、航速v_S,k、航向

角加速度ω_S,k，下标S表示从UUV；对其中的从UUV的经度和纬度进行处理，转换成XY坐标系位置(x_S,k,y_S,k)＝LB2XY(L_S,k,B_S,k)，将主UUV位置、从UUV位置、解析的协同定位数据信息，以及剩余的从UUV第二数据处理模块输出信息，输入给协同定位装置。

步骤(4)协同定位装置建立状态转移方程和测量方程，利用解析出协同定位数据报文中的信息进行滤波处理，完成协同定位。具体方法是：

(4-1)建立状态向量和状态转移方程：

从UUV处于稳态运行的状态时，第k定位时隙的从UUV的运动状态ψ_S,k表示为XY坐标系位置和航向的组合

[·]^T表示转置运算。

在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态

运动方程表示为：

将当前状态和前序缓存的状态相结合，构建组合状态向量

其中N表示缓存的阶数，相应的协方差矩阵为

组合后的运动预测模型

其中，u_S,k-1表示作为测量输入的从UUV的运动速度，n_k表示估计误差，服从高斯分布。状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为F_k，预测模型线性化处理表示为

其中，B表示输入控制矩阵。

相应的协方差矩阵

表示为

其中，Q_k为引入的状态转移误差，

其中，

和

分别表示从UUV测速标准差和测向标准差。

当完成一次状态向量增广操作后，

其协方差矩阵同步进行维度增广，修改表示为

P_k-1[1,1]、P_k-1[1,2]、P_k-1[2,1]、P_k-1[2,2]分别表示协方差矩阵P_k-1的左上、右上、左下和右下子矩阵。

(4-2)对状态向量进行增广：

协同定位装置在原有状态的基础上增添上接收到的主UUV信息ψ_M,k构建增广状态向量

相应的协方差矩阵为

其中，P_M,k表示主UUV状态协方差矩阵。

(4-3)建立测量方程：

根据解析出的发送时间

和本地记录的到达时刻

计算出传播距离

其中，c为等效声速。

在第k定位时隙，从UUV的空间位置信息表示为

主UUV的空间位置信息表示为(x_M,k,y_M,k,d_M,k)。

关于状态向量构建测距方程

设测量受一个零均值的高斯白噪声η_S,k影响，则测量模型表示为

测量偏差矩阵为R_k。

(4-4)滤波更新：

由于测量关于状态量是一个非线性函数，因此可以将其线性化处理，求其Jacobi矩阵H。结合上述步骤(4-1)～(4-3)，以扩展卡尔曼滤波(extended Kalman filter,EKF)为基础，引入新息增益吸收新息，以更新预测的状态：

为更新后预测的状态，K_k为时隙k的新息增益，

表示测距方程，H_k为测量方程的Jacobi矩阵。

新息增益K通过测量矩阵和测量偏差得到，

由状态更新和新息增益，同步状态更新的协方差矩阵

I为单位矩阵。

(4-5)利用测距信息得到的定位结果：

从

中提取出更新的从UUV位置

与步骤(3)的(x_S,k,y_S,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定位时隙中，协同定位数据报文包括经度L_M,k、纬度B_M,k、深度d_M,k、航速v_M,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坐标系位置(x_Si,k,y_Si,k)＝LB2XY(L_Si,k,B_Si,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坐标系位置(x_M,k,y_M,k)＝LB2XY(L_M,k,B_M,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表示缓存的阶数，相应的协方差矩阵为

组合后的运动预测模型

其中，u_Si,k-1表示作为测量输入的从UUV的运动速度，n_k表示估计误差，服从高斯分布。状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为F_i,k，预测模型线性化处理表示为

其中，B_i表示输入控制矩阵。

相应的协方差矩阵

表示为

其中，Q_i,k为引入的状态转移误差，

其中，

和

分别表示从UUV测速标准差和测向标准差。

当完成一次状态向量增广操作后，

其协方差矩阵同步进行维度增广，修改表示为

P_i,k-1[1,1]、P_i,k-1[1,2]、P_i,k-1[2,1]、P_i,k-1[2,2]分别表示协方差矩阵P_i,k-1的左上、右上、左下和右下子矩阵。

(4-2)对状态向量进行增广：

协同定位装置在原有状态的基础上增添上接收到的主UUV信息ψ_M,k构建增广状态向量

相应的协方差矩阵为

其中，P_M,k表示主UUV状态协方差矩阵。

(4-3)建立测量方程：

根据解析出的发送时间

和本地记录的到达时刻

计算出传播距离

其中，c为等效声速。

在第k定位时隙，第i台从UUV的空间位置信息表示为

主UUV的空间位置信息表示为(x_M,k,y_M,k,d_M,k)。

关于状态向量构建测距方程

设测量受一个零均值的高斯白噪声η_Si,k影响，则测量模型表示为

测量偏差矩阵为R_i,k。

(4-4)滤波更新：

由于测量关于状态量是一个非线性函数，因此可以将其线性化处理，求其Jacobi矩阵H。结合上述步骤(4-1)～(4-3)，以扩展卡尔曼滤波(extended Kalman filter,EKF)为基础，引入新息增益K吸收新息，以更新预测的状态：

为更新后预测的状态，K_i,k为时隙k的新息增益，

表示测距方程，H_i,k为测量方程的Jacobi矩阵。

新息增益K通过测量矩阵和测量偏差得到：

由状态更新和新息增益，同步状态更新的协方差矩阵

I为单位矩阵。

(4-5)利用测距信息得到的定位结果：

从

中提取出更新的第i台从UUV位置

与步骤(3)的(x_Si-INS,k,y_Si-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′生成协同定位数据报文，协同定位数据报文包括经度L_M,k、纬度B_M,k、深度d_M,k、航速v_M,k、航向

X方向位置定位标准差σ_Mx,k、Y方向位置定位标准差σ_My,k、报文发送时刻

下标M表示主UUV，下标k表示在第k定位时隙；协同定位数据报文广播给集群中的从UUV；

步骤(3)第二水声通信机接收到协同定位数据报文后，记录协同定位数据报文的到达时刻

解析出协同定位数据报文中的信息，对其中的主UUV的经度和纬度进行处理，转换成XY坐标系位置(x_M,k,y_M,k)＝LB2XY(L_M,k,B_M,k)，LB2XY(·,·)表示经纬度和XY坐标系的转换运算；第二数据处理模块获取INS的数据，包括从UUV的经度L_S,k、纬度B_S,k、深度d_S,k、航速v_S,k、航向

角加速度ω_S,k，下标S表示从UUV；对其中的从UUV的经度和纬度进行处理，转换成XY坐标系位置(x_S,k,y_S,k)＝LB2XY(L_S,k,B_S,k)，将主UUV位置、从UUV位置、解析的协同定位数据信息，以及剩余的从UUV第二数据处理模块输出信息，输入给协同定位装置；

步骤(4)协同定位装置建立状态转移方程和测量方程，利用解析出协同定位数据报文中的信息进行滤波处理，完成协同定位；具体是：

(4-1)建立状态向量和状态转移方程：

从UUV处于稳态运行的状态时，第k定位时隙的从UUV的运动状态ψ_S,k表示为XY坐标系位置和航向的组合

[·]^T表示转置运算；

在固定周期T′的更新频率之下，从UUV的第k定位时隙的估计状态

运动方程表示为：

将当前状态和前序缓存的状态相结合，构建组合状态向量

其中N表示缓存的阶数，相应的协方差矩阵为

组合后的运动预测模型

其中，u_S,k-1表示作为测量输入的从UUV的运动速度，n_k表示估计误差，服从高斯分布；状态方程f(·,·)是关于状态向量的非线性函数，其Jacobi矩阵为F_k，预测模型线性化处理表示为

其中，B表示输入控制矩阵；

相应的协方差矩阵

表示为

其中，Q_k为引入的状态转移误差，

其中，

和

分别表示从UUV测速标准差和测向标准差；

当完成一次状态向量增广操作后，

其协方差矩阵同步进行维度增广，修改表示为

P_k-1[1,1]、P_k-1[1,2]、P_k-1[2,1]、P_k-1[2,2]分别表示协方差矩阵P_k-1的左上、右上、左下和右下子矩阵；

(4-2)对状态向量进行增广：

协同定位装置在原有状态的基础上增添上接收到的主UUV信息ψ_M,k构建增广状态向量

相应的协方差矩阵为

其中，P_M,k表示主UUV状态协方差矩阵；

(4-3)建立测量方程：

根据解析出的发送时间

和本地记录的到达时刻

计算出传播距离

其中，c为等效声速；

在第k定位时隙，从UUV的空间位置信息表示为

主UUV的空间位置信息表示为(x_M,k,y_M,k,d_M,k)；

关于状态向量构建测距方程

设测量受一个零均值的高斯白噪声η_S,k影响，则测量模型表示为

测量偏差矩阵为R_k；

(4-4)滤波更新：

以扩展卡尔曼滤波为基础，引入新息增益吸收新息，更新预测的状态，更新后预测的状态

其中，K_k为时隙k的新息增益，

表示测距方程，H_k为测量方程的Jacobi矩阵；

新息增益K通过测量矩阵和测量偏差得到，

由状态更新和新息增益，同步状态更新的协方差矩阵

I为单位矩阵；

(4-5)利用测距信息得到的定位结果：

从

中提取出更新的从UUV位置

与步骤(3)的(x_S,k,y_S,k)进行比较：当两者差值小于等于设定阈值时，则更新正常，将其转换成经纬度坐标

进行协同定位结果输出；当两者差值大于设定阈值，或者协同定位装置无法接收到水声通信机输入数据时，从

提取出从UUV位置

将其转换成经纬度坐标

进行协同定位结果输出。

Images