一种基于双移动信标的AUV水下声学定位方法

摘要

本发明涉及一种基于双移动信标的AUV水下声学定位方法，其特征在于双移动信标由两艘USV来担任，在两艘USV上分别安装有差分GPS、航向与航向角速度传感器及声定位与通讯声纳，AUV上安装有航向、航速、航向角速度传感器及声定位与通讯声纳。两艘USV上的声定位与通讯声纳周期性地发出声定位信号，并将自身的位置发送给AUV。当AUV接收到USV的声定位信号和位置坐标后，利用扩展卡尔曼滤波器进行水下定位；同时，AUV将测量和定位的结果通过声定位与通讯声纳发送给两艘USV，使两艘USV以AUV为领航者随AUV一起编队运动，并与AUV之间的距离稳定到最小安全距离，与AUV连线之间的夹角稳定到π/2。

主权项

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\left(k\right)=\mathrm{diag}\left[\begin{array}{ccc}{\sigma }_{V\left(k\right)}^2& {\sigma }_{\psi \left(k\right)}^2& {\sigma }_{\omega \left(k\right)}^2\end{array}\right],$Q(k)可根据AUV上航速、航向、航向角速度传感器的性能事先设定为常值矩阵；步骤3：AUV利用扩展卡尔曼滤波器对AUV的状态进行预测，其中AUV扩展卡尔曼滤波器的滤波初值AUV状态估计误差协方差矩阵的初值为$P\left(0\right|0)=\mathrm{diag}\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_y^2& {\sigma }_{\psi }^2\end{array}\right]>0$且事先设定为常值矩阵，采样周期为T。具体步骤为：计算AUV在第k个采样时刻的状态预测值：$\hat{X}\left(k\right|k-1)=\hat{X}(k-1|k-1)+\left[\begin{array}{c}\mathrm{TV}\left(k\right)\cos \psi \left(k\right)\\ \mathrm{TV}\left(k\right)\sin \psi \left(k\right)\\ \mathrm{T\omega }\left(k\right)\end{array}\right]$计算AUV在第k个采样时刻状态预测误差协方差矩阵：$P\left(k\right|k-1)=F_X\left(k\right)P(k-1|k-1)F_X^T\left(k\right)+F_U\left(k\right)Q\left(k\right)F_U^T\left(k\right)$式中：$F_X\left(k\right)=\left[\begin{array}{ccc}1& 0& -\mathrm{TV}\left(k\right)\sin \psi \left(k\right)\\ 0& 1& \mathrm{TV}\left(k\right)\cos \psi \left(k\right)\\ 0& 0& 1\end{array}\right],$$F_U\left(k\right)=\left[\begin{array}{ccc}T\cos \psi \left(k\right)& -\mathrm{TV}\left(k\right)\sin \psi \left(k\right)& 0\\ T\sin \psi \left(k\right)& \mathrm{TV}\left(k\right)\cos \psi \left(k\right)& 0\\ 0& 1& T\end{array}\right]$步骤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)·Cr₂(k)＝Δt₂(k)·C式中，C为声信号在水中的速度；步骤5：AUV利用距离量测对AUV的状态进行滤波，令距离测量噪声协方差矩阵$R\left(k\right)=\mathrm{diag}\left[\begin{array}{cc}{\sigma }_{r_1\left(k\right)}^2& {\sigma }_{r_2\left(k\right)}^2\end{array}\right]=N\times \mathrm{diag}\left[\begin{array}{cc}{r}_1\left(k\right)& r_2\left(k\right)\end{array}\right],$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\left(k\right)=\left[\begin{array}{ccc}\frac{\hat{x}\left(k\right|k-1)-x_{u1}\left(k\right)}{\sqrt{{(\hat{x}\left(k\right|k-1)-x_{u1}\left(k\right))}^2}+{(\hat{y}\left(k\right|k-1)-y_{u1}\left(k\right))}^2}& \frac{\hat{y}\left(k\right|k-1)-y_{u1}\left(k\right)}{\sqrt{{(\hat{x}\left(k\right|k-1)-x_{u1}\left(k\right))}^2+{(\hat{y}\left(k\right|k-1)-y_{u1}\left(k\right))}^2}}& 0\\ \frac{\hat{x}\left(k\right|k-1)-x_{u2}\left(k\right)}{\sqrt{{(\hat{x}\left(k\right|k-1)-x_{u2}\left(k\right))}^2+{(\hat{y}\left(k\right|k-1)-y_{u2}\left(k\right))}^2}}& \frac{\hat{y}\left(k\right|k-1)-y_{u2}\left(k\right)}{\sqrt{{\left(\hat{x}\left(k\right|k-1){-x}_{u2}\left(k\right)\right)}^2+{(\hat{y}\left(k\right|k-1)-y_{u2}\left(k\right))}^2}}& 0\end{array}\right]$步骤(2)：计算AUV在第k个采样时刻状态滤波值：$\hat{X}\left(k\right|k)=\hat{X}\left(k\right|k-1)+K\left(k\right)(Z\left(k\right)-h(\hat{X}\left(k\right|k-1))$式中：$h\left(\hat{X}\left(k\right|k-1)\right)=\left[\begin{array}{c}{(\hat{x}\left(k\right|k-1)-x_{u1}\left(k\right))}^2+{(\hat{y}\left(k\right|k-1)-y_{u1}\left(k\right))}^2\\ {(\hat{x}\left(k\right|k-1)-x_{u2}\left(k\right))}^2+{(\hat{y}\left(k\right|k-1)-y_{u2}\left(k\right))}^2\end{array}\right]$步骤(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之间的距离：${\rho }_1\left(k\right)=\sqrt{{(x_{u1}\left(k\right)-\hat{x}\left(k\right|k))}^2+{(y_{u1}\left(k\right)-\hat{y}\left(k\right|k))}^2}$${\rho }_2\left(k\right)=\sqrt{{(x_{u2}\left(k\right)-\hat{x}\left(k\right|k))}^2+{(y_{u2}\left(k\right)-\hat{y}\left(k\right|k))}^2}$计算两艘USV与AUV之间的夹角：${\alpha }_1\left(k\right)={\tan }^{-1}\frac{y_{u1}\left(k\right)-\hat{y}\left(k\right|k)}{x_{u1}\left(k\right)-\hat{x}\left(k\right|k)}-\hat{\psi }\left(k\right|k)$${\alpha }_2\left(k\right)={\tan }^{-1}\frac{y_{u2}\left(k\right)-\hat{y}\left(k\right|k)}{x_{u2}\left(k\right)-\hat{x}\left(k\right|k)}-\hat{\psi }\left(k\right|k)$计算两艘USV与AUV间距离与最小航行安全距离之间的误差：${\tilde{\rho }}_1\left(k\right)={\rho }_1\left(k\right)-r_{\min }$${\tilde{\rho }}_2\left(k\right)={\rho }_2\left(k\right)-r_{\min }$式中，r_min为USV与AUV之间的最小航行安全距离；计算两艘USV与AUV之间的夹角与期望夹角之间的误差：${\tilde{\alpha }}_1\left(k\right)={\alpha }_1\left(k\right)-{\alpha }_1^{*}\left(k\right)$${\tilde{\alpha }}_2\left(k\right)={\alpha }_2\left(k\right)-{\alpha }_2^{*}\left(k\right)$式中，${\alpha }_1^{*}\left(k\right)=ϵ,$${\alpha }_2^{*}\left(k\right)=\frac{3\pi }{2}+ϵ,$其中ε为0～2π的常值；计算两艘USV和AUV之间的航向角偏差：${\psi }_1\left(k\right)=\hat{\psi }\left(k\right|k)-{\psi }_{u1}\left(k\right)$${\psi }_2\left(k\right)=\hat{\psi }\left(k\right|k)-{\psi }_{u2}\left(k\right)$控制两艘USV的速度为：$V_{u1}\left(k\right)=-k_1{\tilde{\rho }}_1\left(k\right)\cos ({\alpha }_1\left(k\right)+{\psi }_1\left(k\right))-{\rho }_1\left(k\right)\sin ({\alpha }_1\left(k\right)+{\psi }_1\left(k\right))(-k_2{\alpha }_1\left(k\right)+\omega \left(k\right))+V\left(k\right)\cos {\psi }_1\left(k\right)$$V_{u2}\left(k\right)=-k_1{\tilde{\rho }}_2\left(k\right)\cos ({\alpha }_2\left(k\right)+{\psi }_2\left(k\right))-{\rho }_2\left(k\right)\sin ({\alpha }_2\left(k\right)+{\psi }_2\left(k\right))(-k_2{\alpha }_2\left(k\right)+\omega \left(k\right))+V\left(k\right)\cos {\psi }_2\left(k\right)$式中，k₁，k₂∈R⁺为设计的控制增益；控制两艘USV的航向角速度为：${\omega }_{u1}\left(k\right)=\frac{-k_1{\tilde{\rho }}_1\left(k\right)\sin ({\alpha }_1\left(k\right)+{\psi }_1\left(k\right))+{\rho }_1\left(k\right)\cos ({\alpha }_1\left(k\right)+{\psi }_1\left(k\right))(-k_2{\tilde{\alpha }}_1\left(k\right)+\omega \left(k\right))+V\left(k\right)\sin {\psi }_1\left(k\right)}{d_1}$${\omega }_{u2}\left(k\right)=\frac{-k_1{\tilde{\rho }}_2\left(k\right)\sin ({\alpha }_2\left(k\right)+{\psi }_2\left(k\right))+{\rho }_2\left(k\right)\cos ({\alpha }_2\left(k\right)+{\psi }_2\left(k\right))(-k_2{\tilde{\alpha }}_2\left(k\right)+\omega \left(k\right))+V\left(k\right)\sin {\psi }_2\left(k\right)}{d_2}$式中，d₁＞0，d₂＞0，且d₁和d₂分别为两艘USV声定位与通讯声纳沿纵轴距USV重心的距离；返回至步骤2开始新的循环，直至定位结束，使两艘USV以AUV为领航者随AUV一起编队运动，并与AUV之间的距离稳定到最小航行安全距离，与AUV连线之间的夹角稳定到π/2。