[发明专利]基于双移动声信标测距辅助的水下航行器协同定位方法

权利要求书

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：X₁(0)＝[x₁(0) y₁(0) ψ₁(0)]^T；

USV2：X₂(0)＝[x₂(0) y₂(0) ψ₂(0)]^T，

其中x(0)、y(0)表示UUV在水平面中的位置初值，x₁(0)、y₁(0)表示USV1在水平面中的位置初值，x₂(0)、y₂(0)表示USV2在水平面中的位置初值；ψ(0)、ψ₁(0)、ψ₂(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的位置，分别为：

X_a1(k)＝[x_a1(k) y_a1(k) z_a1(k)]

X_a2(k)＝[x_a2(k) y_a2(k) z_a2(k)]

式中：X_a1(.)为USV1测得的UUV的位置，X_a2(.)为USV2测得的UUV的位置；x_a1(.)、y_a1(.)和z_a1(.)分别为USV1测得的UUV的位置在xyz三个方向上的坐标，x_a2(k)、y_a2(k)和z_a2(k)分别为USV2测得的UUV的位置在xyz三个方向上的坐标；

去除X_a1(k)和X_a2(k)中的深度信息，分别表示为：

X_b1(k)＝[x_a1(k) y_a1(k)]

X_b2(k)＝[x_a2(k) y_a2(k)]

通过式(3)计算USV1、USV2与UUV的相对位置：

ρ₁(k)＝X₁(k)-X_b1(k)

ρ₂(k)＝X₂(k)-X_b2(k) (3)

其中，ρ₁(k)、ρ₂(k)分别为USV1、USV2与UUV的相对位置，X₁(k)＝[x₁(k)，y₁(k)]、X₂(k)＝[x₂(k)，y₂(k)]分别为USV1和USV2在k时刻的位置；

将|ρ₁(k)|、|ρ₂(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间的距离与最小安全距离之间的误差：

式中，r_mini为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的速度分别为：

式中，K₁，K₂∈R⁺为预设的控制器增益；ψ₁(k)、ψ₂(k)分别为USV1和USV2在k时刻的航向；

步骤10-7：控制USV1和USV2的航向角速度分别为：

式中，d₁＞0，d₂＞0且d₁和d₂分别为USV1和USV2的USBL沿纵轴距USV的重心的距离；

步骤11：令k加1，返回步骤5，开始新的循环，实现对UUV的连续定位。