CN112285652B - 利用单信标虚拟到达时刻差的水下滑翔机定位方法 - Google Patents

Abstract

本发明公开了利用单信标虚拟到达时刻差的水下滑翔机定位方法。本发明方法中定位信标周期性地发送定位报文，定位报文包含定位信标的经度、纬度和深度信息；水下滑翔机接收到定位报文，同时记录每个定位报文到达声学信号定位接收模块的到达时刻；水下滑翔机解析定位报文，储存定位报文数据；结合信标信息和水下滑翔机航位信息，构建虚拟信标，对到达时刻进行修正后，利用到达时刻差解算方法计算得到水下滑翔机的位置。采用本发明方法，水下滑翔机被动侦听与接收定位报文，无需向定位信标发送定位请求，整个定位过程节能，也具有一定的隐蔽性。

Description

利用单信标虚拟到达时刻差的水下滑翔机定位方法

技术领域

本发明属于水下声学定位技术领域，涉及一种基于单信标的水下滑翔机定位方法，具体是利用单信标虚拟到达时刻差的水下滑翔机定位方法，将定位报文的达到时刻与水下滑翔机的航位信息相结合构建两组虚拟信标，实现水下滑翔机的位置估计。

背景技术

海洋占据地球将近70％的面积，蕴含着丰富的自然资源。随着陆地上以石油为代表的能源储备逐渐减少，越来越多的国家开始将目光转移到海洋。因此，近年来海洋资源勘探、海洋环境监测、水下设备研发等也逐渐成为热点方向。水下机器人，如自主水下航行器(Autonomous Underwater Vehicle,AUV)、水下滑翔机(Underwater Glider,UG)、遥控水下航行器(Remote Operated Vehicles,ROV)等，越来越多地被应用于海洋环境监测与水下目标探测中。这类水下设备常需要机动到指定区域执行相应水下任务，设备的定位精度一定程度上会影响着任务的实施。随着海洋探测、水下目标识别要求越来越高，任务越来越精细，水下机器人如何获取在水下相对准确的位置就显得越来越重要。

由于电磁波信号在水中会受到较大的衰减，陆地上较为成熟的定位系统(如美国的全球定位系统，中国的北斗定位系统等)并不适合在海域工作的水下机器人定位。不同于电磁波信号，声信号在海水中的衰减较小，可以实现水下信号的较长距离的传输。因此，水声信号作为水下信息传输的主要载体，常被用于水下设备的通信与定位领域。

近几十年来，水下声学定位发展了以长基线、短基线和超短基线为代表的水下定位方法及其系统。传统长基线定位系统可用于水下设备的自主定位。一般需要在海底布设声学信标阵列，采用应答机制进行测距，进而得到较高的定位精度。但长基线系统需要在水域内部署较多的水声应答装置，而且每一个应答器位置标定程序复杂，其实现代价较高。短基线水声定位系统需要在载体平台上布放传感器阵列，水下设备安装相应的应答器，可对水下设备进行定位，但是基线长度较短，作用距离有限。超短基线水声定位系统也需要在载体平台上布放声学基线阵列，水下设备安装应答器，采用应答机制实现对水下设备定位。同时，超短基线定位系统还存在着复杂的传感器校准问题。在此基础上，也有一些组合形式的定位方案，兼具长基线的精度和超短基线的便携性。但是，上述水声定位方法均需要安装应答器以完成距离测量或实现角度测量，定位系统较为复杂；同时需要待定位水下设备安装应答器，也不具有节能、隐蔽的特点。

在长基线定位的思路上发展了基于多信标的水下机器人定位方法，即利用多个信标和水下机器人进行信息交互实现测距，结合多个信标的位置与测距信息实现水下机器人的位置解算。根据工作方式的不同，水下机器人可以主动向信标发送定位请求，也可以被动接收信标广播的定位报文获取测距信息。中国专利申请号CN201711371839.6公开了一种基于北斗信标的水下移动节点自主定位方法，海面3个信标组成定位阵列，同步广播定位报文，水下机器人被动侦听报文，当接收到3条定位报文之后完成位置解算。

相较于基于多信标的水下机器人定位方法，基于单信标的水下机器人定位技术是水声定位的一个新方向。这种定位方式水下机器人只需要利用一个信标节点发送的定位报文信息，将声学测距定位设备与机器人平台运动传感器组合使用，实现水下机器人自身位置的估计。

目前，已有一些基于单信标的水下机器人定位跟踪方法。专利申请号

CN201710599165.9和专利申请号CN201710599155.5分别发明公开了一种基于虚拟信标的AUV定位跟踪方法和一种基于虚拟收发信标的单信标测距定位方法。这两种跟踪方法的局限在于利用定位信标到AUV的斜距进行节点定位，要求节点与信标保持着较为严格的时钟同步。

水下滑翔机作为一种靠浮力驱动的水下机器人，通过改过浮力实现上浮或者下潜，利用翼板控制运动，一个完整的上浮和下潜运动轨迹类似正弦函数形状。由于水下滑翔机不装备有推进器，这类水下机器人的能耗远小于AUV之类的水下机器人，因此可以在水下工作较长时间。

和大部分现有的水下机器人定位方法类似，水下滑翔机的水声定位方法大多也存在以下问题：

1、水下机器人与信标需要保持时钟同步。在复杂的水下环境中，水下机器人的时钟漂移难以修正，无法保持和信标节点的精确同步。

2、应答式方式下水下机器人需要装备应答器。出于节能的考虑，一些水下机器人未配备信号发射装置，无法发送应答信号。另外，在特殊安全需求的场景中，出于隐蔽性考虑，水下机器人不能发送信号。

3、水下机器人内部搭载的航位推算模块估计的位置与实际位置偏差较大。

发明内容

本发明的目的就是针对现有技术的不足，提供一种利用单信标虚拟到达时刻差的水下滑翔机定位方法。

本发明方法具体是：

步骤(1).定位信标按固定的时间间隔T周期性地发送定位报文，定位报文包含定位信标的经度、纬度和深度信息。

步骤(2).水下滑翔机接收到定位报文，同时记录每个定位报文到达声学信号定位接收模块的到达时刻。

步骤(3).水下滑翔机解析定位报文，储存定位报文数据；具体是：

(3-1).首先解析定位报文，得到定位信标的经度L_RB、纬度B_RB和深度Z_RB；

(3-2).然后读取水下滑翔机当前的航位信息，包括t_n时刻的俯仰角θ_n、航向角φ_n和深度z_n，所述的俯仰角为水下滑翔机移动方向与水平面的夹角，航向角水下滑翔机移动方向的水平分量与正东方向的夹角；t_n时刻即第n个定位报文到达水下滑翔机的时刻。

步骤(4).进行水下滑翔机定位解算，具体方法是：

(4-1).从定位报文数据中提取第n-1个、第n个和第n+1个定位报文的报文到达时刻t_n-1、t_n和t_n+1，以及相应时刻的水下滑翔机的俯仰角θ_n-1、θ_n、θ_n+1，航向角φ_n-1、φ_n、φ_n+1，和深度z_n-1、z_n、z_n+1；

(4-2).构建虚拟信标：

a.根据t_n-1和t_n时刻的定位报文数据和水下滑翔机航位信息，构建一个虚拟信标VB_n-1；具体是：

以正东为X轴正方向、正北为Y轴正方向，将t_n时刻解析的真实定位信标的经纬度坐标(L_RB,B_RB)转换为平面坐标(X_RB,Y_RB)＝LB2XY(L_RB,B_RB)，LB2XY表示由经纬度坐标向平面坐标转换；VB_n-1的平面坐标中，

VB_n-1的深度Z_VB,n-1＝Z_RB+z_n-z_n-1；

再将平面坐标(X_VB,n-1,Y_VB,n-1)转换为经纬度坐标(L_VB,n-1,B_VB,n-1)＝XY2LB(X_VB,n-1,Y_VB,n-1)，XY2LB表示由平面坐标向经纬度坐标转换。

b.根据t_n和t_n+1时刻的定位报文数据和水下滑翔机航位信息，构建另一个虚拟信标VB_n+1；VB_n+1的平面坐标为(X_VB,n+1,Y_VB,n+1)、深度为Z_VB,n+1，经纬度坐标为(L_VB,n+1,B_VB,n+1)；

Z_VB,n+1＝Z_RB-z_n+1+z_n；(L_VB,n+1,B_VB,n+1)＝XY2LB(X_VB,n+1,Y_VB,n+1)。

(4-3).基于到达时刻差进行水下滑翔机定位解算：

首先对虚拟信标VB_n-1和VB_n+1的到达时刻进行修正，得到修正到达时刻t_VB,n-1和t_VB,n+1：

ri表示四舍五入取整函数；

然后计算得到t_n时刻水下滑翔机的经纬度坐标(L_n,B_n)，具体是：

(4-3-1).构建最小二乘定位方程AX＝H+Kd，A、H和K为已知矩阵，X为待求位置矩阵，d为中间变量，其中：

其中，c表示有效声速，x_n和y_n表示水下滑翔机在t_n时刻的平面坐标位置；

(4-3-2).求解中间变量d：

构建关于中间变量d的一元二次方程a₁d²+a₂d+a₃＝0，方程中间变量a₁、a₂和a₃为：

a₁＝N²(1,1)+N²(2,1)-1，

a₂＝2(Μ(1,1)-X_RB)N(1,1)+2(Μ(2,1)-Y_RB)N(2,1)，

a₃＝(Μ(1,1)-X_RB)²+(Μ(2,1)-Y_RB)²+(z_n-Z_RB)²；其中，Μ和N由A、H、K表示为：Μ＝(A^ΤA)^-1A^ΤK，N＝(A^ΤA)^-1A^ΤH，Τ表示转置；求解方程，则

舍去奇异值，得到中间变量d；

(4-3-3).求解位置矩阵X，得到水下滑翔机在t_n时刻的水平位置(L_n,B_n)：

将平面坐标(x_n,y_n)转换为经纬度坐标，获得水下滑翔机t_n时刻的水平位置(L_n,B_n)＝XY2LB(x_n,y_n)，完成水下滑翔机定位。

本发明方法利用单信标广播的定位报文和水下滑翔机的姿态信息，构建两组虚拟信标，利用虚拟信标的到达时刻差实现水下滑翔机的位置估计，定位系统构建较为简单。水下滑翔机被动侦听与接收定位报文，无需向定位信标发送定位请求，整个定位过程节能，也具有一定的隐蔽性。

附图说明

图1是本发明方法中虚拟信标示意图。

具体实施方式

以下结合具体实施方式和附图对本发明做进一步说明。

利用单信标虚拟到达时刻差的水下滑翔机定位方法，该方法采用的定位系统包括部署于海底的定位信标，以及安装在水下滑翔机内的声学定位信号接收模块、定位处理模块、压力传感器、姿态传感器。

定位信标包含定位声学收发机，用于信标位置的标定以及广播定位报文；海底信标布放后，首先处于位置标定模式，在获取自身位置后，再切换至信标工作模式。

声学定位信号接收模块包括水听器和信号接收处理模块，用于侦听定位信标发送的定位报文、记录定位报文的内容及其到达时刻；压力传感器用于获取水下滑翔机的实时深度信息；姿态传感器用于获取水下滑翔机的实时航位信息；定位处理模块用于执行定位算法，估计水下滑翔机的位置。

定位报文共128比特，依次包括：12比特的同步码SYN(Synchronize SequenceNumbers，同步序列编号)，用于指示定位报文的起始位；4比特的类型码TYP，用于指示定位报文的类型；8比特的信标标识符ID，用于指示发送定位报文的信标标识符；8比特的保留位RES，用于控制功能的保留位；32比特的经度信息LON，32比特的纬度信息LAT，16比特的高程(深度)信息HEI；16比特的纠错冗余位FCC，用于报文的帧纠错。

该方法具体是：

步骤(1).定位信标按固定的时间间隔T周期性地发送定位报文，定位报文包含定位信标的经度、纬度和深度信息。

步骤(2).水下滑翔机通过声学定位信号接收模块接收到定位信标发送的定位报文，同时记录每个定位报文到达声学信号定位接收模块的到达时刻。

步骤(3).水下滑翔机通过定位处理模块，解析定位报文，储存定位报文数据；具体方法是：

(3-1).定位处理模块解析定位报文，得到定位信标的经度L_RB、纬度B_RB和深度Z_RB。

(3-2).定位处理模块从姿态传感器读取水下滑翔机当前的航位信息，包括t_n时刻的俯仰角θ_n和航向角φ_n，所述的俯仰角为水下滑翔机移动方向与水平面的夹角，航向角水下滑翔机移动方向的水平分量与正东方向的夹角；从压力传感器读取水下滑翔机t_n时刻的深度z_n；t_n时刻即第n个定位报文到达声学信号定位接收模块的到达时刻。

步骤(4).水下滑翔机定位解算，具体方法是：

(4-1).从定位处理模块存储的定位报文数据中提取第n-1个、第n个和第n+1个定位报文的报文到达时刻t_n-1、t_n和t_n+1，以及相应时刻的水下滑翔机的俯仰角θ_n-1、θ_n、θ_n+1，航向角φ_n-1、φ_n、φ_n+1，和水下滑翔机的深度z_n-1、z_n、z_n+1。

(4-2).如图1，构建虚拟信标：

a.根据t_n-1和t_n时刻的定位报文数据和水下滑翔机航位信息，构建一个虚拟信标VB_n-1：VB_n-1的平面坐标为(X_VB,n-1,Y_VB,n-1)、深度为Z_VB,n-1，经纬度坐标为(L_VB,n-1,B_VB,n-1)；具体是：

以正东为X轴正方向、正北为Y轴正方向，将t_n时刻解析的真实定位信标的经纬度坐标(L_RB,B_RB)转换为平面坐标(X_RB,Y_RB)：(X_RB,Y_RB)＝LB2XY(L_RB,B_RB)，LB2XY表示由经纬度坐标向平面坐标转换；VB_n-1的平面坐标中，

VB_n-1的深度Z_VB,n-1＝Z_RB+z_n-z_n-1；

将平面坐标(X_VB,n-1,Y_VB,n-1)转换为经纬度坐标(L_VB,n-1,B_VB,n-1)：

(L_VB,n-1,B_VB,n-1)＝XY2LB(X_VB,n-1,Y_VB,n-1)，XY2LB表示由平面坐标向经纬度坐标转换。

b.根据t_n和t_n+1时刻的定位报文数据和水下滑翔机航位信息，构建另一个虚拟信标VB_n+1；VB_n+1的平面坐标为(X_VB,n+1,Y_VB,n+1)、深度为Z_VB,n+1，经纬度坐标为(L_VB,n+1,B_VB,n+1)；

Z_VB,n+1＝Z_RB-z_n+1+z_n；(L_VB,n+1,B_VB,n+1)＝XY2LB(X_VB,n+1,Y_VB,n+1)。

(4-3).基于到达时刻差进行水下滑翔机定位解算：

由于水下滑翔机收到的第n-1个、第n个和第n+1个定位报文的报文到达时刻不同，首先对虚拟信标VB_n-1和VB_n+1的到达时刻进行修正，得到修正到达时刻t_VB,n-1和t_VB,n+1：

ri表示四舍五入取整函数。

然后计算得到t_n时刻水下滑翔机的经纬度坐标(L_n,B_n)，具体是：

(4-3-1).构建最小二乘定位方程AX＝H+Kd，A、H和K为已知矩阵，X为待求位置矩阵，d为中间变量，其中：

其中，c表示有效声速，x_n和y_n表示水下滑翔机在t_n时刻的平面坐标位置。

(4-3-2).求解中间变量d：

构建关于中间变量d的一元二次方程a₁d²+a₂d+a₃＝0，方程中间变量a₁、a₂和a₃为：

a₁＝N²(1,1)+N²(2,1)-1，

a₂＝2(Μ(1,1)-X_RB)N(1,1)+2(Μ(2,1)-Y_RB)N(2,1)，a₃＝(Μ(1,1)-X_RB)²+(Μ(2,1)-Y_RB)²+(z_n-Z_RB)²；其中，Μ和N由A、H、K表示为：Μ＝(A^ΤA)^-1A^ΤK，N＝(A^ΤA)^-1A^ΤH，Τ表示转置；求解方程，则

舍去奇异值，得到中间变量d。

(4-3-3).求解位置矩阵X，得到水下滑翔机在t_n时刻的水平位置(L_n,B_n)：

将平面坐标(x_n,y_n)转换为经纬度坐标，获得水下滑翔机t_n时刻的水平位置(L_n,B_n)＝XY2LB(x_n,y_n)，完成水下滑翔机定位。

Claims (2)

1.利用单信标虚拟到达时刻差的水下滑翔机定位方法，其特征在于，该方法具体是：

步骤(1).定位信标按固定的时间间隔T周期性地发送定位报文，定位报文包含定位信标的经度、纬度和深度信息；

步骤(2).水下滑翔机接收到定位报文，同时记录每个定位报文到达声学信号定位接收模块的到达时刻；

步骤(3).水下滑翔机解析定位报文，储存定位报文数据；

步骤(4).进行水下滑翔机定位解算，具体方法是：

(4-1).从定位报文数据中提取第n-1个、第n个和第n+1个定位报文的报文到达时刻t_n-1、t_n和t_n+1，以及相应时刻的水下滑翔机的俯仰角θ_n-1、θ_n、θ_n+1，航向角φ_n-1、φ_n、φ_n+1，和深度z_n-1、z_n、z_n+1；

(4-2).构建虚拟信标VB_n-1和VB_n+1：VB_n-1的平面坐标为(X_VB,n-1,Y_VB,n-1)、深度为Z_VB,n-1，经纬度坐标为(L_VB,n-1,B_VB,n-1)；VB_n+1的平面坐标为(X_VB,n+1,Y_VB,n+1)、深度为Z_VB,n+1，经纬度坐标为(L_VB,n+1,B_VB,n+1)；具体是：

a.根据t_n-1和t_n时刻的定位报文数据和水下滑翔机航位信息，构建一个虚拟信标VB_n-1：

以正东为X轴正方向、正北为Y轴正方向，将t_n时刻解析的真实定位信标的经纬度坐标(L_RB,B_RB)转换为平面坐标(X_RB,Y_RB)＝LB2XY(L_RB,B_RB)，LB2XY表示由经纬度坐标向平面坐标转换，L_RB和B_RB为定位信标的经度和纬度；VB_n-1的平面坐标中，

VB_n-1的深度Z_VB,n-1＝Z_RB+z_n-z_n-1；

再将平面坐标(X_VB,n-1,Y_VB,n-1)转换为经纬度坐标(L_VB,n-1,B_VB,n-1)＝XY2LB(X_VB,n-1,Y_VB,n-1)，XY2LB表示由平面坐标向经纬度坐标转换；

b.根据t_n和t_n+1时刻的定位报文数据和水下滑翔机航位信息，构建另一个虚拟信标VB_n+1：

Z_VB,n+1＝Z_RB-z_n+1+z_n；(L_VB,n+1,B_VB,n+1)＝XY2LB(X_VB,n+1,Y_VB,n+1)；

(4-3).基于到达时刻差进行水下滑翔机定位解算：

首先对虚拟信标VB_n-1和VB_n+1的到达时刻进行修正，得到修正到达时刻t_VB,n-1和t_VB,n+1：

ri表示四舍五入取整函数；

然后计算得到t_n时刻水下滑翔机的经纬度坐标(L_n,B_n)，完成水下滑翔机定位；

解算方法具体是：

(4-3-1).构建最小二乘定位方程AX＝Η+Kd，A、Η和K为已知矩阵，X为待求位置矩阵，d为中间变量，其中：

其中，c表示有效声速，x_n和y_n表示水下滑翔机在t_n时刻的平面坐标位置，Z_RB为定位信标的深度；

(4-3-2).求解中间变量d：

构建关于中间变量d的一元二次方程a₁d²+a₂d+a₃＝0，方程中间变量a₁、a₂和a₃为：

a₁＝Ν²(1,1)+Ν²(2,1)-1，

a₂＝2(Μ(1,1)-X_RB)Ν(1,1)+2(Μ(2,1)-Y_RB)Ν(2,1)，

a₃＝(Μ(1,1)-X_RB)²+(Μ(2,1)-Y_RB)²+(z_n-Z_RB)²；其中，Μ和Ν由A、Η、K表示为：Μ＝(A^ΤA)^-1A^ΤK，Ν＝(A^ΤA)^-1A^ΤΗ，Τ表示转置；求解方程，则

舍去奇异值，得到中间变量d；

(4-3-3).求解位置矩阵X，得到水下滑翔机在t_n时刻的水平位置(L_n,B_n)：

将平面坐标(x_n,y_n)转换为经纬度坐标，获得水下滑翔机t_n时刻的水平位置(L_n,B_n)＝XY2LB(x_n,y_n)。

2.如权利要求1所述利用单信标虚拟到达时刻差的水下滑翔机定位方法，其特征在于：步骤(3)具体是：

(3-1).首先解析定位报文，得到定位信标的经度L_RB、纬度B_RB和深度Z_RB；

(3-2).然后读取水下滑翔机当前的航位信息，包括t_n时刻的俯仰角θ_n、航向角φ_n和深度z_n，所述的俯仰角为水下滑翔机移动方向与水平面的夹角，航向角水下滑翔机移动方向的水平分量与正东方向的夹角；t_n时刻即第n个定位报文到达水下滑翔机的时刻。

Images