基于双移动声信标测距辅助的水下航行器协同定位方法
阅读说明：本技术 基于双移动声信标测距辅助的水下航行器协同定位方法 (Underwater vehicle cooperative positioning method based on double-mobile-acoustic-beacon ranging assistance ) 是由 王银涛 贾晓宝 韩正卿 严卫生 崔荣鑫 张守旭 李宏 王崇武 于 2020-12-27 设计创作，主要内容包括：本发明公开了一种基于双移动声信标测距辅助的水下航行器协同定位方法,用于提高水下航行器的定位精度,以解决水下导航定位难的问题。无人船携带USBL水声定位和通信设备,UUV上安装有航向、航速和航向角速度传感器以及OEM应答器。无人船周期性将自身的位置发送给UUV携带的应答器,USBL同时测得UUV相对于无人船USV的位置。当UUV接收到USV的位置坐标后,利用扩展卡尔曼滤波的方法进行自身定位,同时,UUV将定位结果通过应答器发送给两个USV,使两个USV以UUV作为领航者,随UUV一起编队运行。编队队形保持与UUV之间的距离为设定的安全距离,且两个USV与UUV之间的夹角为90°。本发明利用了无人船携带USBL辅助UUV定位以及编队控制算法,解决了水下航行器UUV定位精度不高的问题。(The invention discloses an underwater vehicle cooperative positioning method based on double-mobile-acoustic-beacon ranging assistance, which is used for improving the positioning precision of an underwater vehicle and solving the problem of difficult underwater navigation positioning. The unmanned ship carries USBL underwater sound positioning and communication equipment, and a course, navigational speed and course angular speed sensor and an OEM transponder are installed on the UUV. The unmanned ship periodically sends the position of the unmanned ship to a transponder carried by the UUV, and the USBL simultaneously measures the position of the UUV relative to the USV of the unmanned ship. After the UUV receives the position coordinates of the USV, the UUV carries out self-positioning by using an extended Kalman filtering method, and meanwhile, the UUV sends positioning results to the two USVs through the responder, so that the two USVs form a formation to operate together with the UUV by taking the UUV as a pilot. The formation keeps the distance between the formation and the UUV at a set safe distance, and the included angle between the two USVs and the UUV is 90 degrees. The invention utilizes the USBL carried by the unmanned ship to assist UUV positioning and formation control algorithm, and solves the problem of low positioning accuracy of the UUV.)
技术领域
本发明属于水下航行器技术领域，具体涉及一种水下航行器协同定位方法。
背景技术
UUV(Unmanned Underwater Vehicle，以下简称UUV)是新一代水下机器人，具有活动范围大、机动性好、安全、智能化等优点，在海底探测数据和海军反水雷作战方面发挥极其重要的作用。然而，UUV完成上述任务的前提是需要知道自身的精确位置信息。UUV由于一般在水下航行，因此UUV水下定位属于三维问题，算法设计较为困难，但是UUV自身可以安装深度传感器，因此可以精确测量UUV的深度，所以可以将水下三维定位问题转化为二维平面的定位问题。
超短基线定位系统(USBL，Ultra Short Base Line)是一种水下声学定位技术，被普遍应用于海洋石油勘探开发、海洋打捞等海洋生产开发方面，主要用于确定ROV、AUV、潜水员、水下其他载体的精确位置。超短基线定位系统由发射换能器、OEM应答器、接收基阵组成。发射换能器和接收基阵安装在船上，应答器固定在水下载体UUV上。发射换能器发出一个声脉冲，应答器收到后，回发声脉冲，接收基阵收到后，测量出X、Y两个方向的相位差，并根据声波的到达时间计算出水下装置到基阵的距离R，从而计算得到UUV在平面坐标上的位置和水下探测器的深度。
目前，可以用作UUV水下高精度定位的主要是声学方法。主要有：
(1)短基线的声学定位方法。此方法需要事先将一个信标固定在海底，并测出信标的位置。使用时，将修订后的信标的位置提前置于UUV中，UUV通过实时测量其与信标之间的距离，从而可解算出UUV相对于信标的位置。但是该方法UUV仅能在信标覆盖的有限范围内使用，并且布放信标需要较高的费用且不易维护。该方法观测性能较差，必须UUV机动才能进行定位。
(2)基于多个固定信标的长基线声学定位方法。此方法需要事先将3～4个信标固定在海底，并测出信标的位置。使用时，将修订后的信标位置存储在UUV中，UUV通过实时测量其与各个信标之间的距离，从而解算出UUV相对于各个信标的位置。该方法的不足是，UUV仅能在信标覆盖的有限区域内进行定位，且信标的校准和布放过程繁琐。
发明内容
为了克服现有技术的不足，本发明提供了一种基于双移动声信标测距辅助的水下航行器协同定位方法，主要用于提高水下航行器的定位精度，以解决水下导航定位难的问题。无人船携带USBL水声定位和通信设备，UUV上安装有航向、航速和航向角速度传感器以及OEM应答器。无人船周期性将自身的位置发送给UUV携带的应答器，USBL同时测得UUV相对于无人船USV的位置。当UUV接收到USV的位置坐标后，利用扩展卡尔曼滤波的方法进行自身定位，同时，UUV将定位结果通过应答器发送给两个USV，使两个USV以UUV作为领航者，随UUV一起编队运行。编队队形保持与UUV之间的距离为设定的安全距离，且两个USV与UUV之间的夹角为90°。本发明利用了无人船携带USBL辅助UUV定位以及编队控制算法，解决了水下航行器UUV定位精度不高的问题。
本发明解决其技术问题所采用的技术方案包括以下步骤：
步骤1：在将两个安装有USBL的USV1、USV2和安装有OEM应答器的UUV布放入水之前，利用GPS同步时钟信号对UUV、USV1和USV2三者进行时钟同步；
步骤2：将USV1、USV2和UUV布放入水中，初始位置向量分别定义为：
UUV：X(0)＝[x(0) y(0) ψ(0)]T；
USV1：X1(0)＝[x1(0) y1(0) ψ1(0)]T；
USV2：X2(0)＝[x2(0) y2(0) ψ2(0)]T，
其中x(0)、y(0)表示UUV在水平面中的位置初值，x1(0)、y1(0)表示USV1在水平面中的位置初值，x2(0)、y2(0)表示USV2在水平面中的位置初值；ψ(0)、ψ1(0)、ψ2(0)分别表示UUV、USV1和USV2的航向角初值；
步骤3：定义UUV的位置向量X＝[x y ψ]T，并设定卡尔曼滤波的位置滤波初始值X(0|0)＝X(0)，UUV的初始状态误差协方差矩阵采样时间间隔为T，分别为预设的UUV在x、y、z三个方向的方差；
步骤4：UUV、USV1和USV2开始航行；令k＝1；
步骤5：在第k个采样时刻，安装在USV1上和USV2上的USBL同时发出声定位信号，安装在UUV上的OEM接收到两个声定位信号后发出返回信号；USV1和USV2上的USBL接收返回信号；同时UUV上安装的航向传感器、速度传感器和航向角速度传感器分别测量出UUV在k时刻的航向ψ(k)、航速v(k)和航向角速度ω(k)；测量噪声协方差矩阵为 分别为预设的UUV航向、航速和航向角速度的方差；
步骤6：利用扩展卡尔曼滤波算法对UUV的状态进行预测；
步骤6-1：利用式(1)计算UUV在k时刻的位置滤波预测值：
其中，X(k-1|k-1)表示k-1时刻UUV位置滤波的准确值；
将表示为：
其中，和分别表示UUV在k时刻的位置滤波在x方向、y方向和航向角的预测值；
步骤6-2：利用式(2)计算UUV在k时刻的状态误差协方差矩阵的预测值：
其中：
P(k-1|k-1)表示UUV在k-1时刻的状态误差协方差矩阵的准确值；
步骤7：USV1和USV2通过USBL得到UUV的位置，发送给UUV，并计算出斜距，步骤如下：
USV1和USV2通过自身携带的USBL测得UUV的位置，分别为：
Xa1(k)＝[xa1(k) ya1(k) za1(k)]
Xa2(k)＝[xa2(k) ya2(k) za2(k)]
式中：Xa1(.)为USV1测得的UUV的位置，Xa2(.)为USV2测得的UUV的位置；xa1(.)、ya1(.)和za1(.)分别为USV1测得的UUV的位置在xyz三个方向上的坐标，xa2(k)、ya2(k)和za2(k)分别为USV2测得的UUV的位置在xyz三个方向上的坐标；
去除Xa1(k)和Xa2(k)中的深度信息，分别表示为：
Xb1(k)＝[xa1(k) ya1(k)]
Xb2(k)＝[xa2(k) ya2(k)]
通过式(3)计算USV1、USV2与UUV的相对位置：
ρ1(k)＝X1(k)-Xb1(k)
ρ2(k)＝X2(k)-Xb2(k) (3)
其中，ρ1(k)、ρ2(k)分别为USV1、USV2与UUV的相对位置，X1(k)＝[x1(k)，y1(k)]、X2(k)＝[x2(k)，y2(k)]分别为USV1和USV2在k时刻的位置；
将|ρ1(k)|、|ρ2(k)|作为斜距通过USBL发送给UUV，设定距离测量误差协方差矩阵R(k)＝diag[0.01 0.01]；
步骤8：利用距离量测对UUV的状态进行滤波，步骤如下：
步骤8-1：利用式(4)计算UUV在k时刻卡尔曼滤波增益矩阵：
其中，
步骤8-2：利用式(5)计算UUV在k时刻位置滤波准确值：
其中：
将X(k|k)表示为：
X(k|k)＝[x(k|k)y(k|k)ψ(k|k)]
其中，x(k|k)、y(k|k)和ψ(k|k)分别表示UUV在k时刻的位置滤波在x方向、y方向和航向角的准确值；
步骤8-3：利用式(6)计算UUV在k时刻状态误差协方差矩阵的准确值：
步骤9：UUV将k时刻的速度v(k)、航向角速度ω(k)及位置滤波值X(k|k)通过USBL发送给USV1和USV2；
步骤10：USV1和USV2以UUV作为领航者，并分别采用如下的主从编队控制律实现编队航行；
步骤10-1：利用式(7)分别计算USV1、USV2与UUV之间的距离：
步骤10-2：利用式(8)分别计算USV1、USV2与UUV之间的夹角：
步骤10-3：利用式(9)分别计算USV1、USV2与UUV间的距离与最小安全距离之间的误差：
式中，rmin i为USV与UUV之间的最小安全距离；
步骤10-4：利用式(10)分别计算USV1、USV2与UUV之间的夹角与期望夹角之间的误差：
式中，
步骤10-5：利用式(11)分别计算USV1、USV2和UUV之间的航向角偏差：
ψi(k)＝ψ(k|k)-ψi(k)(i＝1，2) (11)
步骤10-6：控制USV1和USV2的速度分别为：
式中，K1，K2∈R+为预设的控制器增益；ψ1(k)、ψ2(k)分别为USV1和USV2在k时刻的航向；
步骤10-7：控制USV1和USV2的航向角速度分别为：
式中，d1＞0，d2＞0且d1和d2分别为USV1和USV2的USBL沿纵轴距USV的重心的距离；
步骤11：令k加1，返回步骤5，开始新的循环，实现对UUV的连续定位。
优选地，所述USV1、USV2和UUV的航向角由通过安装在自身上的SBG姿态航向传感器进行测量。
优选地，所述USV1和USV2的位置由通过自身安装的RTK GPS流动站设备进行测量。
优选地，所述ε为0～2π之间的常值。
本发明的有益效果如下：
由于USBL安装在移动的USV上，且USV上安装有高精度的RTK GPS流动站，而且两个USV以UUV为领航者，随UUV一起编队航行，从而可以实现较大范围内的导航和定位，保持两艘USV与UUV之间的距离均等于最小安全距离，且两艘USV与UUV连线的夹角保持90度，且UUV和USV在这种编队队形的定位精度优于普通队形。
附图说明
图1为本发明方法USBL定位原理图。
图2为本发明方法基于双USBL的UUV水下定位系统原理示意图。
图3为本发明方法基于双USBL的UUV定位示意图。
图4为本发明方法UUV与USV之间的相对运动学关系。
图5为本发明方法最优观测配置定位队形结构。
图6为本发明方法UUV在地面坐标系中运动状态。
图7为本发明方法USV与UUV之间的距离与定位误差关系图。
图8为本发明方法USV与UUV间距不同时定位误差对比图。
图9为本发明方法两艘USV与UUV连线之间的夹角与定位误差关系图。
图10为本发明方法两艘USV与UUV连线之间的夹角不同时定位误差对比图。
具体实施方式
下面结合附图和实施例对本发明进一步说明。
本发明提出了一种基于双USBL的UUV水下定位方法，其特征在于将两个USBL设备分别固定在两艘USV(Unmanned Surface Vehicle)上，如图2所示，该超短基线设备既可以主动定位UUV的位置，又可以作为通信装置周期性的交互USV的位置坐标和USV与UUV之间的距离。UUV上安装有航向、航速、航向角速度传感器，同时也安装了OEM应答器。如图3所示，当UUV接收到USV的声定位信息和位置坐标后，利用扩展卡尔曼滤波的方法进行水下定位；同时，UUV将定位结果通过OEM发送给两个USV，使两个USV以UUV作为领航者，随UUV一起编队运行，并保持与UUV之间的距离均为最小安全距离，且两个USV与UUV之间的夹角为如图5所示。
如图1所示，一种基于双移动声信标测距辅助的水下航行器协同定位方法，包括以下步骤：
步骤1：在将两个安装有USBL的USV1、USV2和安装有OEM应答器的UUV布放入水之前，利用GPS同步时钟信号对UUV、USV1和USV2三者进行时钟同步；
步骤2：将USV1、USV2和UUV布放入水中，初始位置向量分别定义为：
UUV：X(0)＝[x(0) y(0) ψ(0)]T；
USV1：X1(0)＝[x1(0) y1(0) ψ1(0)]T；
USV2：X2(0)＝[x2(0) y2(0) ψ2(0)]T，
其中x(0)、y(0)表示UUV在水平面中的位置初值，x1(0)、y1(0)表示USV1在水平面中的位置初值，x2(0)、y2(0)表示USV2在水平面中的位置初值；ψ(0)、ψ1(0)、ψ2(0)分别表示UUV、USV1和USV2的航向角初值；
步骤3：定义UUV的位置向量X＝[x y ψ]T，并设定卡尔曼滤波的位置滤波初始值X(0|0)＝X(0)，UUV的初始状态误差协方差矩阵采样时间间隔为T，分别为预设的UUV在x、y、z三个方向的方差；
步骤4：UUV、USV1和USV2开始航行；令k＝1；
步骤5：在第k个采样时刻，安装在USV1上和USV2上的USBL同时发出声定位信号，安装在UUV上的OEM接收到两个声定位信号后发出返回信号；USV1和USV2上的USBL接收返回信号；同时UUV上安装的航向传感器、速度传感器和航向角速度传感器分别测量出UUV在k时刻的航向ψ(k)、航速v(k)和航向角速度ω(k)；测量噪声协方差矩阵为 分别为预设的UUV航向、航速和航向角速度的方差；
步骤6：利用扩展卡尔曼滤波算法对UUV的状态进行预测；
步骤6-1：利用式(1)计算UUV在k时刻的位置滤波预测值：
其中，X(k-1|k-1)表示k-1时刻UUV位置滤波的准确值；
将表示为：
其中，和分别表示UUV在k时刻的位置滤波在x方向、y方向和航向角的预测值；
步骤6-2：利用式(2)计算UUV在k时刻的状态误差协方差矩阵的预测值：
其中：
P(k-1|k-1)表示UUV在k-1时刻的状态误差协方差矩阵的准确值；
步骤7：USV1和USV2通过USBL得到UUV的位置，发送给UUV，并计算出斜距，步骤如下：
USV1和USV2通过自身携带的USBL测得UUV的位置，分别为：
Xa1(k)＝[xa1(k) ya1(k) za1(k)]
Xa2(k)＝[xa2(k) ya2(k) za2(k)]
式中：Xa1(.)为USV1测得的UUV的位置，Xa2(.)为USV2测得的UUV的位置；xa1(.)、ya1(.)和za1(.)分别为USV1测得的UUV的位置在xyz三个方向上的坐标，xa2(k)、ya2(k)和za2(k)分别为USV2测得的UUV的位置在xyz三个方向上的坐标；
去除Xa1(k)和Xa2(k)中的深度信息，分别表示为：
Xb1(k)＝[xa1(k) ya1(k)]
Xb2(k)＝[xa2(k) ya2(k)]
通过式(3)计算USV1、USV2与UUV的相对位置：
ρ1(k)＝X1(k)-Xb1(k)
ρ2(k)＝X2(k)-Xb2(k) (3)
其中，ρ1(k)、ρ2(k)分别为USV1、USV2与UUV的相对位置，X1(k)＝[x1(k)，y1(k)]、X2(k)＝[x2(k)，y2(k)]分别为USV1和USV2在k时刻的位置；
将|ρ1(k)|、|ρ2(k)|作为斜距通过USBL发送给UUV，设定距离测量误差协方差矩阵R(k)＝diag[0.01 0.01]；
步骤8：利用距离量测对UUV的状态进行滤波，步骤如下：
步骤8-1：利用式(4)计算UUV在k时刻卡尔曼滤波增益矩阵：
其中，
步骤8-2：利用式(5)计算UUV在k时刻位置滤波准确值：
其中：
将X(k|k)表示为：
X(k|k)＝[x(k|k)y(k|k)ψ(k|k)]
其中，x(k|k)、y(k|k)和ψ(k|k)分别表示UUV在k时刻的位置滤波在x方向、y方向和航向角的准确值；
步骤8-3：利用式(6)计算UUV在k时刻状态误差协方差矩阵的准确值：
步骤9：UUV将k时刻的速度v(k)、航向角速度ω(k)及位置滤波值X(k|k)通过USBL发送给USV1和USV2；
步骤10：如图4所示，USV1和USV2以UUV作为领航者，并分别采用如下的主从编队控制律实现编队航行；
步骤10-1：利用式(7)分别计算USV1、USV2与UUV之间的距离：
步骤10-2：利用式(8)分别计算USV1、USV2与UUV之间的夹角：
步骤10-3：利用式(9)分别计算USV1、USV2与UUV间的距离与最小安全距离之间的误差：
式中，rmin i为USV与UUV之间的最小安全距离；
步骤10-4：利用式(10)分别计算USV1、USV2与UUV之间的夹角与期望夹角之间的误差：
式中，
步骤10-5：利用式(11)分别计算USV1、USV2和UUV之间的航向角偏差：
ψi(k)＝ψ(k|k)-ψi(k)(i＝1，2) (11)
步骤10-6：控制USV1和USV2的速度分别为：
式中，K1，K2∈R+为预设的控制器增益；ψ1(k)、ψ2(k)分别为USV1和USV2在k时刻的航向；
步骤10-7：控制USV1和USV2的航向角速度分别为：
式中，d1＞0，d2＞0且d1和d2分别为USV1和USV2的USBL沿纵轴距USV的重心的距离；如图3所示；
步骤11：令k加1，返回步骤5，开始新的循环，实现对UUV的连续定位。
优选地，所述USV1、USV2和UUV的航向角由通过安装在自身上的SBG姿态航向传感器进行测量。
优选地，所述USV1和USV2的位置由通过自身安装的RTK GPS流动站设备进行测量。
优选地，所述ε为0～2π之间的常值。
具体实施例：
实施例一：分析USV与UUV之间的距离对定位性能的影响。如图6所示，假设UUV运动速度为3节，从坐标为(20,20)处开始出发以航向角出发作直线运动。另外假设两艘USV运动速度为3节，初始航向角为并分别从表1给出的不同初始位置出发，此时两个USV与UUV之间的夹角为90度，USV和UUV之间的距离从最小安全距离60米增加到1000米。实验中假设Q＝diag([0.1 0.005 0.005])，P(0|0)＝diag([1000 10001000])，T＝0.5s，k1＝5，k2＝1，d1＝d2＝0.1m。表1给出了不同USV初始运动位置条件下定位系统的平均定位误差，图7给出了USV与UUV之间间距从80增加到1000m时的定位误差曲线图，图8给出了USV和UUV之间的距离分别为400m和80m时定位误差对比图。
表1不同USV初始位置条件下定位系统的平均定位误差
由表1及图7和图8可以看出，当保持两个USV与UUV间的夹角为90度时，改变两个USV与UUV间的间距，随着间距的增大，平均定位误差增大。
实施例二：分析USV与UUV之间的夹角对定位性能的影响。假设UUV运动速度为3节，从坐标为(20,20)处开始出发以航向角出发作直线运动。首先假设两艘USV运动速度为3节，初始航向角为此时两个USV与UUV间的夹角为距离为最小安全距离。然后，并分别从表2给出的不同初始位置出发，此时两个USV与UUV之间的夹角改变。实验中假设Q＝diag([0.1 0.005 0.005])，P(0|0)＝diag([1000 10001000])，T＝0.5s，k1＝5，k2＝1，d1＝d2＝0.1m。表2给出了不同USV与UUV连线间的夹角条件下定位系统的平均定位误差，图9给出了USV与UUV之间夹角从40度增加到160度时的定位误差曲线图，图10给出了USV和UUV之间的夹角分别为90度和160度时定位误差对比图。
表2不同USV与UUV连线夹角条件定位系统的平均定位误差
由表2及图9和图10可以看出，当保持两个USV与UUV间的距离为400m时，改变两个USV与UUV间的夹角，平均定位误差在90度时最小。
由以上两个实验可以得出，本发明设计的基于双移动声信标辅助的UUV水下定位方法具有最优定位性能。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种基于方位角及其变化率的目标运动分析方法