CN108614258B - 一种基于单水声信标距离量测的水下定位方法 - Google Patents

Abstract

本发明公开了一种基于单水声信标距离量测的水下定位方法：首先，水下航行器搭载水声通信机与固定位置的水声信标以固定频率进行斜距测量，水声信标将自身水深发送给航行器，航行器结合水声信标和自身的实时水深，计算得到航行器与水声信标的深度差与水平距离；其次，在航行过程中，航行器利用罗经和多普勒测速仪得到自身的准确航向和速度，从而得到某一时间段内航行器准确的航行轨迹；最后，航行器以测距序列和航向为约束，以水声信标位置为参照，对实时相对航行轨迹在水声信标覆盖范围内进行匹配定位，确定当前位置。本发明能够弥补传统水声定位技术放置的基元多、结构复杂、成本较高，以及航位推算法中误差累积和需要起始点位置信息的不足。

Description

一种基于单水声信标距离量测的水下定位方法

技术领域

本发明属于海洋导航定位技术领域，具体涉及一种基于单水声信标距离量测的水下导航定位方法。该方法主要应用于航行器在未知水下环境中航行作业时的定位估计。

背景技术

水下导航定位技术是海洋科学考察、资源勘查及开发的必要技术，而水声定位技术是目前水下导航定位研究的主要方向之一。

传统的航位推算法，具有短时间内高精度定位的特点，但是其定位误差会随着时间推移而累积增大，且在航位推算的过程中需要已知起始点位置信息。在航行器进行长距离航行时，航位推算法的缺点就变得十分明显，而且该方法产生的累积误差是无界的，随着时间增长，定位精度越来越差。

传统的水声定位技术包括长基线定位***、短基线定位***以及超短基线定位***三种方式。这些传统方法可以在覆盖区域内提供高精度的定位更新信息，但需要的水声基元较多、***布设复杂、成本昂贵，无法广泛应用于水下航行器的导航定位需求。

目前对于单信标水声定位的研究，都是将测距作为辅助信息，为惯性导航***等提供观测量，从而提高位置估计精度，但这种方式下，导航***的可观性要求对航行器与水声信标之间的相对运动方向具有严格的要求，当相对航向过于单一时，辅助效果将大打折扣，导航估计准确性迅速降低。

发明内容

本发明针对水下航行器在航行过程中的定位需求，提供一种物理结构简单、成本低、定位准确的定位方法。能够弥补传统水声定位技术放置的基元多、结构复杂、成本较高，以及航位推算法中误差累积和需要起始点位置信息的不足。利用本发明定位方法，航行器通过与固定水声信标进行连续的距离量测，结合自身的航向和深度，以及潜标的实时深度信息，即可确定自身位置，实现实时定位。

本发明所采用的技术方案是：一种基于单水声信标距离量测的水下定位方法，包括以下步骤：

步骤A，水下航行器搭载水声通信机与固定位置的水声信标以固定频率进行斜距测量，水声信标将自身水深发送给航行器，航行器结合水声信标和航行器自身的实时水深，计算得到航行器与水声信标的深度差与水平距离；

步骤B，在航行过程中，航行器利用罗经和多普勒测速仪得到自身的准确航向和速度，从而得到某一时间段内航行器准确的航行轨迹；

步骤C，航行器以测距序列和航向为约束，以水声信标位置为参照，对实时相对航行轨迹在水声信标覆盖范围内进行匹配定位，确定航行器当前位置。

进一步的，步骤A具体包括：

步骤A1，水下航行器在航行过程中，利用自身携带的磁罗经或惯性陀螺测量实时的航向，利用多普勒测速仪测量自身的绝对航行速度，结合航向，得到速度矢量；

步骤A2，当水下航行器进入单信标覆盖区域后，接收水声信标发送的位置、深度和测距信号；

步骤A3，航行器在t₁,t₂,…,t_n共n个时刻测量与水声信标之间的斜距d₁,d₂,…,d_n，利用接收到的水声信标深度和航行器自身深度计算深度差h₁,h₂,…,h_n，进而，利用勾股定理，计算航行器与水声信标之间的水平距离R₁,R₂,…,R_n，其中，

i＝1,2,…,n，以R_i为半径，以水声信标位置为圆心，得到n个同心圆C₁,C₂,…,C_n。

进一步的，步骤B具体为：航行器利用航位推算法得到t₁,t₂,…,t_n这段时间内的相对航迹，设航迹上t₁,t₂,…,t_n时刻对应的航迹点分别为P₁,P₂,…,P_n，P₁,P₂,…,P_n轨迹点的相应坐标分别为(X_P1,Y_P1),(X_P2,Y_P2),…,(X_Pn,Y_Pn)。

其中，所述的航位推算法是在知道当前时刻位置的条件之下，通过测量移动的距离和方位，推算下一时刻位置的方法，具体方法为：

假设航线的起始点位置为(X(0),Y(0))，根据航行器上搭载的多普勒测速仪测得每一时刻航行器的航行速度Vel_measure(i)，利用惯性陀螺和磁罗经得到每一时刻航行器准确的航行方向Direction(i)；

通过航行速度Vel_measure(i)和航行方向Direction(i)，根据航位推算公式，推得每一时刻航行器的位置，如下：

其中，X(i+1),Y(i+1)分别表示通过航位推算法推得的i+1时刻航行器所处的位置信息；X(i),Y(i)为前一时刻，即第i时刻航行器所处的位置信息；dt表示测量的时间间隔。

进一步的，步骤C具体包括：

步骤C1，将半径为R_n的同心圆划分为N等份，从而得到N个点位，依次将P_n放置在这N个点位上，根据相对航迹上P₁,P₂,…,P_n这n个航迹点彼此之间的相对位置，计算得到P₁,P₂,…,P_n这n个航迹点的具***置；当P_n放置在第j(1≤j≤N)个点位时，将所有航迹点对应的点位定义为P_1j,P_2j,…,P_nj；设P_1j坐标为(X_P1j,Y_P1j)，则其他航迹点的位置为(X_Pij,Y_Pij)，其中，X_Pij＝X_Pi+(X_P1j-X_P1)，Y_Pij＝Y_Pi+(Y_P1j-Y_P1)；

步骤C2，设P_ij(i＝1,2,…,n；1≤j≤N)与C_i之间的距离为S_ij，

则当P_n位于第j个点位时，n个航迹点与该轨迹点对应的同心圆之间的距离之和为S_j＝S_1j+S_2j+…+S_nj；

步骤C3，在同心圆C_n上所有N个点位中，使得S_j最小的点位确定为航迹点P_n当前位置的最佳匹配点，设最佳匹配点的对应序号为j_min，将航迹点P_n放置在此最佳匹配点上，即为

即为航行器的当前位置，其他对应航迹点则为

进一步的，在步骤C获得航行器当前位置之后，对航行器当前位置进行修正。

其中，所述的对航行器当前位置进行修正具体包括：

步骤(1)，计算测距误差补偿量D，D＝|S_j/n|；

步骤(2)，确定测距误差补偿方向，确定水声信标与最佳匹配点之间的连线方向为误差补偿方向；

步骤(3)，进行测距误差补偿，当S_j>0时，将

向靠近圆心方向整体移动D距离；当S_j<0时，将

向远离圆心方向整体移动D距离；当S_j＝0时，保持位置不变。

本发明的有益效果是：一种基于单水声信标距离量测的水下定位方法，本发明针对于海洋环境中的水下航行器，提供一种物理结构简单、定位准确、成本低的基于单信标距离量测的水下定位方案。相比于测距辅助的导航方式，本方法不受可观性的限制，在任意航行轨迹上都能够实现定位。而且，基于单水声信标距离量测的水下定位方法，只需一个固定的水声信标，依据连续时间内水下信标和航行器之间的距离量测就可对航行器进行定位解算，相对传统的水声定位技术降低了物理复杂性，与航位推算方法相比提高了定位精度，在航行器的远距离航行中具有明显的优势。

附图说明

图1为本发明的算法流程图。

图2为本发明的基于单信标距离量测的定位方法原理示意图。

图3为本发明水下航行器目标航行轨迹示意图。

图4为本发明水下航行器推位轨迹示意图。

图5为本发明公开的方法与传统长基线定位、航位推算定位的对比图。

图6为本发明公开的方法与传统长基线定位、航位推算定位的对比细节图。

图7为本发明公开的方法进行误差补偿之后与单信标定位的对比图。

图8为本发明公开的方法进行误差补偿之后与单信标定位的对比图。

具体实施方式

为能进一步了解本发明的发明内容、特点及功效，兹例举以下实施例，并配合附图详细说明如下：

图1为本发明的算法流程图，图2为基于单信标距离量测的定位方法原理示意图，如附图1和图2所示，一种基于单水声信标距离量测的水下定位方法，包括以下步骤：

步骤A，水下航行器搭载水声通信机与固定位置的水声信标以固定频率进行斜距测量，水声信标将自身水深发送给航行器，航行器结合水声信标和航行器自身的实时水深，计算得到航行器与水声信标的深度差与水平距离。具体包括：

步骤A1，水下航行器在航行过程中，利用自身携带的磁罗经或惯性陀螺测量实时的航向，利用多普勒测速仪测量自身的绝对航行速度，结合航向，得到速度矢量；

步骤A2，当水下航行器进入单信标覆盖区域后，接收水声信标发送的位置、深度和测距信号；

步骤A3，航行器在t₁,t₂,…,t_n共n个时刻测量与水声信标之间的斜距d₁,d₂,…,d_n，利用接收到的水声信标深度和航行器自身深度计算深度差h₁,h₂,…,h_n，进而，利用勾股定理，计算航行器与水声信标之间的水平距离R₁,R₂,…,R_n，其中，

i＝1,2,…,n，以R_i为半径，以水声信标位置为圆心，得到n个同心圆C₁,C₂,…,C_n。

步骤B，在航行过程中，航行器利用罗经和多普勒测速仪得到自身的准确航向和速度，从而得到某一时间段内航行器准确的航行轨迹。具体为：航行器利用航位推算法得到t₁,t₂,…,t_n这段时间内的相对航迹，设航迹上t₁,t₂,…,t_n时刻对应的航迹点分别为P₁,P₂,…,P_n，P₁,P₂,…,P_n轨迹点的相应坐标分别为(X_P1,Y_P1),(X_P2,Y_P2),…,(X_Pn,Y_Pn)。

步骤C，航行器以测距序列和航向为约束，以水声信标位置为参照，对实时相对航行轨迹在水声信标覆盖范围内进行匹配定位，确定航行器当前位置。具体包括：

步骤C1，将半径为R_n的同心圆划分为N等份，从而得到N个点位，依次将P_n放置在这N个点位上，根据相对航迹上P₁,P₂,…,P_n这n个航迹点彼此之间的相对位置，计算得到P₁,P₂,…,P_n这n个航迹点的具***置；当P_n放置在第j(1≤j≤N)个点位时，将所有航迹点对应的点位定义为P_1j,P_2j,…,P_nj；设P_1j坐标为(X_P1j,Y_P1j)，则其他航迹点的位置为(X_Pij,Y_Pij)，其中，X_Pij＝X_Pi+(X_P1j-X_P1)，Y_Pij＝Y_Pi+(Y_P1j-Y_P1)；

步骤C2，设P_ij(i＝1,2,…,n；1≤j≤N)与C_i之间的距离为S_ij，

则当P_n位于第j个点位时，n个航迹点与该轨迹点对应的同心圆之间的距离之和为S_j＝S_1j+S_2j+…+S_nj；

步骤C3，在同心圆C_n上所有N个点位中，使得S_j最小的点位确定为航迹点P_n当前位置的最佳匹配点，设最佳匹配点的对应序号为j_min，将航迹点P_n放置在此最佳匹配点上，即为

即为航行器的当前位置，其他对应航迹点则为

步骤D，在步骤C获得航行器当前位置之后，对航行器当前位置进行修正，具体包括：

步骤D1，计算测距误差补偿量D，D＝|S_j/n|；

步骤D2，确定测距误差补偿方向，确定水声信标与最佳匹配点之间的连线方向为误差补偿方向；

步骤D3，进行测距误差补偿，当S_j>0时，将

向靠近圆心方向整体移动D距离；当S_j<0时，将

向远离圆心方向整体移动D距离；当S_j＝0时，保持位置不变。

图3为水下航行器目标航行轨迹示意图，在实际航行中，该轨迹的位置信息是未知的，需要我们进行定位。

图4为水下航行器推位轨迹示意图，航位推算法是在知道当前时刻位置的条件之下，通过测量移动的距离和方位，推算下一时刻位置的方法。

在此，假设航线的起始点位置为(X(0),Y(0))，根据航行器上搭载的多普勒测速仪测得每一时刻航行器的航行速度Vel_measure(i)，利用惯性陀螺和磁罗经得到每一时刻航行器准确的航行方向Direction(i)；

通过航行速度Vel_measure(i)和航行方向Direction(i)，根据航位推算公式，推得每一时刻航行器的位置，如下：

其中，X(i+1),Y(i+1)分别表示通过航位推算法推得的i+1时刻航行器所处的位置信息；X(i),Y(i)为前一时刻，即第i时刻航行器所处的位置信息；dt表示测量的时间间隔。

图5、6为本发明单水声信标定位结果与长基线定位、航位推算定位的对比图。通过图5可以看出，在远距离作业时，基于单信标距离量测的定位方法的定位效果明显于长基线水声定位。同时，通过实验可以验证，基于单信标距离量测的定位方法的定位效果在一定程度上优于航位推算法的定位效果，在一定程度上解决了航位推算法误差累积的缺点。

图7、8为进行误差补偿的定位效果图，可以看出，在基于单信标距离量测的定位基础之上，采用本发明的方法对其进行位置修正，修正之后的定位效果明显好于基于单信标的距离量测的定位效果。

尽管上面结合附图对本发明的优选实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，并不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可以做出很多形式，这些均属于本发明的保护范围之内。

Claims (7)

1.一种基于单水声信标距离量测的水下定位方法，其特征在于，包括以下步骤：

步骤A，水下航行器搭载水声通信机与固定位置的水声信标以固定频率进行斜距测量，水声信标将自身水深发送给航行器，航行器结合水声信标和航行器自身的实时水深，计算得到航行器与水声信标的深度差与水平距离；

步骤B，在航行过程中，航行器利用罗经和多普勒测速仪得到自身的准确航向和速度，从而得到某一时间段内航行器准确的航行轨迹；

步骤C，航行器以测距序列和航向为约束，以水声信标位置为参照，对实时相对航行轨迹在水声信标覆盖范围内进行匹配定位，确定航行器当前位置。

2.根据权利要求1所述的一种基于单水声信标距离量测的水下定位方法，其特征在于，步骤A具体包括：

步骤A1，水下航行器在航行过程中，利用自身携带的磁罗经或惯性陀螺测量实时的航向，利用多普勒测速仪测量自身的绝对航行速度，结合航向，得到速度矢量；

步骤A2，当水下航行器进入单信标覆盖区域后，接收水声信标发送的位置、深度和测距信号；

步骤A3，航行器在t₁，t₂，…，t_n共n个时刻测量与水声信标之间的斜距d₁，d₂，…，d_n，利用接收到的水声信标深度和航行器自身深度计算深度差h₁，h₂，…，h_n，进而，利用勾股定理，计算航行器与水声信标之间的水平距离R₁，R₂，…，R_n，其中，

以R_i为半径，以水声信标位置为圆心，得到n个同心圆C₁，C₂，…，C_n。

3.根据权利要求2所述的一种基于单水声信标距离量测的水下定位方法，其特征在于，步骤B具体为：航行器利用航位推算法得到t₁，t₂，…，t_n这段时间内的相对航迹，设航迹上t₁，t₂，…，t_n时刻对应的航迹点分别为P₁，P₂，…，P_n，P₁，P₂，…，P_n轨迹点的相应坐标分别为(X_P1，Y_P1)，(X_P2，Y_P2)，…，

4.根据权利要求3所述的一种基于单水声信标距离量测的水下定位方法，其特征在于，所述的航位推算法是在知道当前时刻位置的条件之下，通过测量移动的距离和方位，推算下一时刻位置的方法，具体方法为：

假设航线的起始点位置为(X(0)，Y(0))，根据航行器上搭载的多普勒测速仪测得每一时刻航行器的航行速度Vel_measure(i)，利用惯性陀螺和磁罗经得到每一时刻航行器准确的航行方向Direction(i)；

通过航行速度Vel_measure(i)和航行方向Direction(i)，根据航位推算公式，推得每一时刻航行器的位置，如下：

其中，X(i+1)，Y(i+1)分别表示通过航位推算法推得的i+1时刻航行器所处的位置信息；X(i)，Y(i)为前一时刻，即第i时刻航行器所处的位置信息；dt表示测量的时间间隔。

5.根据权利要求3所述的一种基于单水声信标距离量测的水下定位方法，其特征在于，步骤C具体包括：

步骤C1，将半径为R_n的同心圆划分为N等份，从而得到N个点位，依次将P_n放置在这N个点位上，根据相对航迹上P₁，P₂，…，P_n这n个航迹点彼此之间的相对位置，计算得到P₁，P₂，…，P_n这n个航迹点的具***置；当P_n放置在第j(1≤j≤N)个点位时，将所有航迹点对应的点位定义为P_1j，P_2j，…，P_nj；设P_1j坐标为(X_P1j，Y_P1j)，则其他航迹点的位置为(X_Pij，Y_Pij)，其中，X_Pij＝X_Pi+(X_P1j-X_P1)，Y_Pij＝Y_Pi+(Y_P1j-Y_P1)；

步骤C2，设P_ij(i＝1，2，…，n；1≤j≤N)与C_i之间的距离为S_ij，

则当P_n位于第j个点位时，n个航迹点与该轨迹点对应的同心圆之间的距离之和为S_j＝S_1j+S_2j+…+S_nj；

步骤C3，在同心圆C_n上所有N个点位中，使得S_j最小的点位确定为航迹点P_n当前位置的最佳匹配点，设最佳匹配点的对应序号为J_min，将航迹点P_n放置在此最佳匹配点上，即为

即为航行器的当前位置，其他对应航迹点则为

6.根据权利要求5所述的一种基于单水声信标距离量测的水下定位方法，其特征在于，在步骤C获得航行器当前位置之后，对航行器当前位置进行修正。

7.根据权利要求6所述的一种基于单水声信标距离量测的水下定位方法，其特征在于，所述的对航行器当前位置进行修正具体包括：

步骤(1)，计算测距误差补偿量D，D＝|S_j/n|；

步骤(2)，确定测距误差补偿方向，确定水声信标与最佳匹配点之间的连线方向为误差补偿方向；

步骤(3)，进行测距误差补偿，当S_j＞0时，将

向靠近圆心方向整体移动D距离；当S_j＜0时，将

向远离圆心方向整体移动D距离；当S_j＝0时，保持位置不变。

Images