[发明专利]一种基于双移动信标的AUV水下声学定位方法

权利要求书

1.一种基于双移动信标的AUV水下声学定位方法，其特征在于：双移动信标由两艘USV来担任，在两艘USV上分别安装有差分GPS、航向与航向角速度传感器及声定位与通讯声纳，AUV上安装有航向、航速、航向角速度传感器及声定位与通讯声纳；两艘USV上的声定位与通讯声纳周期性发出声定位信号，同时将自身的位置发送给AUV；具体步骤如下：

步骤1：利用GPS的同步时钟信号PPS对AUV和两艘USV进行时钟同步，然后将AUV与两艘USV布放入水；所述AUV的初始状态向量为X(0)＝[x(0)y(0)ψ(0)]^T，其中：x、y为AUV水平面内的位置，ψ为AUV的航向角，(x(0)，y(0))可根据AUV布放时GPS的测量值进行事先设定，ψ(0)由AUV上的航向传感器测量得到；所述第一艘USV的初始状态向量为x_u1(0)＝[x_u1(0)y_u1(0)ψ_u1(0)]^T，其中：x_u1、y_u1为第一艘USV水平面内的位置，ψ_u1为第一艘USV的航向角；所述第二艘USV的初始状态向量为x_u2(0)＝[x_u2(0)y_u2(0)ψ_u2(0)]^T，其中：x_u2、y_u2为第二艘USV水平面内的位置，ψ_u2为第二艘USV的航向角。X_u1(0)和X_u2(0)可由两艘USV上的差分GPS和航向传感器测量得到；

步骤2：AUV开始下潜航行，在第k个采样时刻，两艘USV通过声定位与通讯声纳发出声定位信号，并将自身的位置信息(x_u1(k)，y_u1(k))、(x_u2(k)，y_u2(k))发送给AUV；同时AUV利用自身的航速、航向、航向角速度传感器分别测量出在k采样时刻的航速V(k)、航向ψ(k)、航向角速度ω(k)，其测量噪声协方差矩阵为Q(k)=diagσV(k)2σψ(k)2σω(k)2,]]>Q(k)可根据AUV上航速、航向、航向角速度传感器的性能事先设定为常值矩阵；

步骤3：AUV利用扩展卡尔曼滤波器对AUV的状态进行预测，其中AUV扩展卡尔曼滤波器的滤波初值AUV状态估计误差协方差矩阵的初值为P(0|0)=diagσx2σy2σψ2>0]]>且事先设定为常值矩阵，采样周期为T。具体步骤为：

计算AUV在第k个采样时刻的状态预测值：

X^(k|k-1)=X^(k-1|k-1)+TV(k)cosψ(k)TV(k)sinψ(k)Tω(k)]]>

计算AUV在第k个采样时刻状态预测误差协方差矩阵：

P(k|k-1)=FX(k)P(k-1|k-1)FXT(k)+FU(k)Q(k)FUT(k)]]>

式中：FX(k)=10-TV(k)sinψ(k)01TV(k)cosψ(k)001,]]>FU(k)=Tcosψ(k)-TV(k)sinψ(k)0Tsinψ(k)TV(k)cosψ(k)001T]]>

步骤4：AUV利用声定位与通讯声纳，计算接收到两艘USV发出的声定位信号的时延，确定AUV与两艘USV之间的距离，得到距离测量向量Z(k)＝[r₁(k)r₂(k)]^T，步骤如下：

步骤a：AUV计算两艘USV发出的声定位信号的接收时延：

Δt₁(k)＝t₁(k)-t(k)

Δt₂(k)＝t₂(k)-t(k)

式中，t₁(k)为AUV接收到第一艘USV发出的声定位信号的时间，t₂(k)为AUV接收到第二艘USV发出的声定位信号的时间，t(k)为两艘USV发出声定位信号的时间；

步骤b：计算AUV与两艘USV之间的距离：

r₁(k)＝Δt₁(k)·C

r₂(k)＝Δt₂(k)·C

式中，C为声信号在水中的速度；

步骤5：AUV利用距离量测对AUV的状态进行滤波，令距离测量噪声协方差矩阵R(k)=diagσr1(k)2σr2(k)2=N×diagr1(k)r2(k),]]>N为常数，步骤如下：

步骤(1)：计算AUV在第k个采样时刻卡尔曼滤波增益矩阵：

K(k)＝P(k|k-1)H^T(k)[H(k)P(k|k-1)H^T(k)+R(k)]^-1

H(k)=x^(k|k-1)-xu1(k)(x^(k|k-1)-xu1(k))2+(y^(k|k-1)-yu1(k))2y^(k|k-1)-yu1(k)(x^(k|k-1)-xu1(k))2+(y^(k|k-1)-yu1(k))20x^(k|k-1)-xu2(k)(x^(k|k-1)-xu2(k))2+(y^(k|k-1)-yu2(k))2y^(k|k-1)-yu2(k)(x^(k|k-1)-xu2(k))2+(y^(k|k-1)-yu2(k))20]]>

步骤(2)：计算AUV在第k个采样时刻状态滤波值：

X^(k|k)=X^(k|k-1)+K(k)(Z(k)-h(X^(k|k-1))]]>

式中：h(X^(k|k-1))=(x^(k|k-1)-xu1(k))2+(y^(k|k-1)-yu1(k))2(x^(k|k-1)-xu2(k))2+(y^(k|k-1)-yu2(k))2]]>

步骤(3)：计算AUV在第k个采样时刻状态滤波误差协方差矩阵：

P(k|k)＝P(k|k-1)-K(k)H(k)P(k|k-1)；

步骤6：AUV将第k个采样时刻的速度V(k)、航向角速度ω(k)及状态滤波值通过声定位与通讯声纳发送给两艘USV；

步骤7：两艘USV以AUV作为领航者，并分别采用如下的主从式编队控制律以最优定位队形编队航行，具体步骤为：

计算两艘USV与AUV之间的距离：

ρ1(k)=(xu1(k)-x^(k|k))2+(yu1(k)-y^(k|k))2]]>

ρ2(k)=(xu2(k)-x^(k|k))2+(yu2(k)-y^(k|k))2]]>

计算两艘USV与AUV之间的夹角：

α1(k)=tan-1yu1(k)-y^(k|k)xu1(k)-x^(k|k)-ψ^(k|k)]]>

α2(k)=tan-1yu2(k)-y^(k|k)xu2(k)-x^(k|k)-ψ^(k|k)]]>

计算两艘USV与AUV间距离与最小航行安全距离之间的误差：

ρ~1(k)=ρ1(k)-rmin]]>

ρ~2(k)=ρ2(k)-rmin]]>

式中，r_min为USV与AUV之间的最小航行安全距离；

计算两艘USV与AUV之间的夹角与期望夹角之间的误差：

α~1(k)=α1(k)-α1*(k)]]>

α~2(k)=α2(k)-α2*(k)]]>

式中，α1*(k)=ϵ,]]>α2*(k)=3π2+ϵ,]]>其中ε为0～2π的常值；计算两艘USV和AUV之间的航向角偏差：

ψ1(k)=ψ^(k|k)-ψu1(k)]]>

ψ2(k)=ψ^(k|k)-ψu2(k)]]>

控制两艘USV的速度为：

Vu1(k)=-k1ρ~1(k)cos(α1(k)+ψ1(k))-ρ1(k)sin(α1(k)+ψ1(k))(-k2α1(k)+ω(k))+V(k)cosψ1(k)]]>

Vu2(k)=-k1ρ~2(k)cos(α2(k)+ψ2(k))-ρ2(k)sin(α2(k)+ψ2(k))(-k2α2(k)+ω(k))+V(k)cosψ2(k)]]>

式中，k₁，k₂∈R⁺为设计的控制增益；

控制两艘USV的航向角速度为：

ωu1(k)=-k1ρ~1(k)sin(α1(k)+ψ1(k))+ρ1(k)cos(α1(k)+ψ1(k))(-k2α~1(k)+ω(k))+V(k)sinψ1(k)d1]]>

ωu2(k)=-k1ρ~2(k)sin(α2(k)+ψ2(k))+ρ2(k)cos(α2(k)+ψ2(k))(-k2α~2(k)+ω(k))+V(k)sinψ2(k)d2]]>

式中，d₁＞0，d₂＞0，且d₁和d₂分别为两艘USV声定位与通讯声纳沿纵轴距USV重心的距离；

返回至步骤2开始新的循环，直至定位结束，使两艘USV以AUV为领航者随AUV一起编队运动，并与AUV之间的距离稳定到最小航行安全距离，与AUV连线之间的夹角稳定到π/2。