CN103336267B - 一种基于水声通信延迟的主从式多uuv协同定位方法 - Google Patents

Abstract

一种基于水声通信延迟的主从式多UUV协同定位方法，涉及一种协同定位方法。它解决了现有主从式多UUV协同定位系统的定位误差大的问题。其方法：主、从UUV进行时钟同步准备，主UUV通过水声测距测量主、从UUV之间的距离；主UUV通过水声设备向从UUV发送自身的定位信息、间距信息以及信息发送时刻的时间戳；从UUV接收到主UUV水声信息后计算本次水声通信的延迟时长，建立协同定位系统的状态方程；从UUV利用系统状态转移矩阵，建立协同定位系统量测方程；利用系统量测方程，完成协同定位系统量测更新；利用系统状态方程，完成协同定位系统状态更新；从UUV对状态估计误差进行补偿。本发明适用于主从式多UUV协同定位。

Description

一种基于水声通信延迟的主从式多UUV协同定位方法

技术领域

本发明涉及一种协同定位方法，具体涉及一种主从式多UUV协同定位方法。

背景技术

多UUV(Unmanned Underwater Vehicle，无人水下航行器)协同系统在海洋探索及开发、军事作战等方面有着重要的作用。定位技术是协同作业的前提条件和关键技术。但由于复杂的水下环境以及UUV自身的体积、质量等限制，目前实现多UUV定位难度较大。多UUV协同定位是指多UUV在作业过程中少量UUV(主UUV)配备高精度导航设备能够完成自主定位，并且为其他UUV(从UUV)提供精确相对定位信息，从UUV利用主UUV提供的相对定位信息对自身低成本、低精度的航推定位误差进行修正，从而提高定位精度。在主从式多UUV协同定位系统中，由于各UUV的信息处理能力不同，并且水声信息传输需要一定时间。所以协同定位所需要的相对量测信息普遍存在时间延迟，导致定位误差增大。

发明内容

本发明是为了解决现有主从式多UUV协同定位系统的定位误差大的问题，从而提供一种基于水声通信延迟的主从式多UUV协同定位方法。

一种基于水声通信延迟的主从式多UUV协同定位方法，它由以下步骤实现：

步骤一、设置主UUV和从UUV为时钟同步；主UUV通过水声测距的方法测量主UUV与从UUV之间的距离r；

步骤二、设平行于地球纬线切线方向为x轴，东向为正方向，以平行于地球经线切线方向为y轴，北向为正方向，以同时垂直x轴和y轴的方向为z轴，向上为正方向，建立三维直角坐标系；主UUV通过水声设备向从UUV发送自身的定位信息x_a、y_a、z_a、主UUV与从UUV之间的距离r以及信息发送时刻的时间戳T；

步骤三、从UUV接收到主UUV水声信息后计算本次水声通信的延迟时长N_i，并结合从UUV自身航推定位信息建立协同定位系统的状态方程；从UUV自身航推定位信息包括航速V_b以及航向角速度w_b；

建立协同定位系统的状态方程的具体方法为：

从UUV接收到主UUV水声信息后，记录计算机时钟的接收时刻，与主UUV发送时刻相减得到本次水声通信的延迟时长为N_i；

系统状态量为：

X_k＝[X_ak ^T X_bk ^T]^T＝(x_as，y_as，z_as，x_bk，y_bk，z_ak，φ_bk)^T

其中下标a、b分别代表主UUV和从UUV，下标s、k分别代表主UUV信息发送时刻T_s和从UUV信息接收时刻T_k；

X_k为系统状态量，X_ak为主UUV状态量，X_bk为从UUV状态量，x_as，y_as和z_as为主UUV T_s时刻的定位信息，x_bk，y_bk和z_ak为从UUV T_k时刻的航推定位信息，φ_bk为从UUV的航向角；

系统输入为：

u_k＝[u_ak u_bk]^T＝(x_as,y_as,z_as,V_bk,w_bk)^T

式中u_k为系统输入值；

u_ak＝[x_as,y_as,z_as]^T为主UUV在T_k时刻的输入值，

u_bk＝[V_bk,w_bk]^T为从UUV在T_k时刻的输入值；

则系统状态方程为：

X_k＝A_k-1X_k-1+G_k-1W_k-1

其中：X_k-1为T_k-1时刻系统状态量，

$A_{k-1}=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& \mathrm{\δt}\·V_{b(k-1)}\cos {\mathrm{\φ}}_{b(k-1)}\\ 0& 0& 0& 0& 1& 0& -\mathrm{\δt}\·V_{b(k-1)}\sin {\mathrm{\φ}}_{b(k-1)}\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

为系统矩阵，

δt为系统滤波周期，

$G_{k-1}=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& \mathrm{\δt}\·\sin {\mathrm{\φ}}_{b(k-1)}& 0\\ 0& 0& 0& \mathrm{\δt}\·\cos {\mathrm{\φ}}_{b(k-1)}& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& \mathrm{\δt}\end{array}\right]$

为系统噪声驱动阵，W_k-1为系统噪声阵；

且有：

$W_k={(W_{x_{\mathrm{as}}},W_{y_{\mathrm{as}}},W_{z_{\mathrm{as}}},W_{V_{\mathrm{bk}}},W_{w_{\mathrm{bk}}})}^T,$

和分别为零均值且相互独立的高斯白噪声，其方差阵分别为：

系统噪声方差阵：

$Q_k=E\left[W_k{W_k}^T\right]=\left[\begin{array}{ccccc}{q}_{x_{\mathrm{as}}}^2& 0& 0& 0& 0\\ 0& q_{y_{\mathrm{as}}}^2& 0& 0& 0\\ 0& 0& q_{y_{\mathrm{as}}}^2& 0& 0\\ 0& 0& 0& q_{V_{\mathrm{bk}}}^2& 0\\ 0& 0& 0& 0& q_{w_{\mathrm{bk}}}^2\end{array}\right];$

步骤四、从UUV计算在T_k时刻下系统状态转移矩阵，并取出延迟阶段每一时刻的状态转移矩阵，得到延迟前后时刻系统状态量之间的关系；

从UUV在T_k时刻系统下状态转移矩阵为：

${\mathrm{\Φ}}_b\left(k\right)=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\δt}\·V_{\mathrm{bk}}\cos {\mathrm{\φ}}_{\mathrm{bk}}\\ 0& 1& 0& -\mathrm{\δt}\·V_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

从UUV延迟前后时刻系统状态量之间的关系为：

$X_{\mathrm{bs}}={\mathrm{\Φ}}_b(s,k)X_{\mathrm{bk}}={\mathrm{\Φ}}_b{(k,s)}^{-1}{X}_{\mathrm{bk}}={\left(\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b\left(i\right)\right)}^{-1}{X}_{\mathrm{bk}}$

其中为本次通信延迟时长N_i中每时刻的状态转移矩阵乘积，且有：

$\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b\left(i\right)=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\δt}\underset{i=s}{\overset{k}{\mathrm{\Σ}}}{V}_{\mathrm{bi}}\cos {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 1& 0& -\mathrm{\δt}\underset{i=s}{\overset{k}{\mathrm{\Σ}}}{V}_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

步骤五、从UUV利用步骤四获得的延迟前后时刻系统状态量之间的关系，建立协同定位系统量测方程；

步骤六、利用步骤五获得的系统量测方程，完成协同定位系统量测更新，保存系统滤波量测更新量；

步骤七、利用步骤三获得的系统状态方程，完成协同定位系统状态更新，保存系统滤波状态更新量；

步骤八、从UUV取出延迟阶段每一时刻的滤波量测更新量和状态更新量，对协同定位系统由于通信延时造成的状态估计误差进行补偿；完成一次基于水声通信延迟的主从式多UUV协同定位。

步骤五中建立协同定位系统量测方程为：

Z_k＝r_s+v_k＝C_kX_k+v_k

其中Z_k为系统量测量，r_s为T_s时刻主UUV测得的间距，v_k为系统量测噪声，C_k为系统量测矩阵：

C_k＝[c_ak c_bsΦ_b(k，s)^-1]

其中主UUV量测矩阵：

$c_{\mathrm{ak}}=\left[\begin{array}{ccc}\frac{(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}& \frac{(z_{\mathrm{as}}-z_{\mathrm{bs}})}{r_s}\end{array}\right],$

从UUV量测矩阵：

$c_{\mathrm{bs}}=\left[\begin{array}{cccc}\frac{-(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{-(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}& \frac{-(z_{\mathrm{as}}-z_{\mathrm{bs}})}{r_s}& 0\end{array}\right].$

步骤六中利用步骤五获得的系统量测方程，完成协同定位系统量测更新的方法为：

系统量测更新过程为：

$\left\{\begin{array}{c}{P}_{k,k-1}=A_{k-1}{P}_{k-1}{A_{k-1}}^T+G_{k-1}{Q}_{k-1}{G_{k-1}}^T\\ K_k=P_{k,k-1}{C}_k^T{[C_k{P}_{k,k-1}{C}_k^T+v_k]}^{-1}\\ P_k=(I-K_k{C}_k)P_{k,k-1}\\ Q_k=A_k(I-K_k{C}_k)\end{array}\right.$

其中：P_k,k-1为系统状态估计一步预测均方误差，K_k为系统T_k时刻滤波增益，P_k为系统状态估计均方误差；Q_k为保存的系统T_k时刻滤波量测更新量。

步骤七中利用步骤五获得的系统量测方程，完成协同定位系统状态更新的方法为：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}^{\′}=A_{k-1}{\hat{X}}_{k-1}\\ M_k=A_k{K}_k^d[Z_k-C_k{\hat{X}}_{k,k-1}^d]\end{array}\right.$

其中：为系统状态估计一步预测，M_k为保存的系统T_k时刻滤波状态更新量。

步骤八所述从UUV取出延迟阶段每一时刻的滤波量测更新量和状态更新量，对协同定位系统由于通信延时造成的状态估计误差进行补偿的具体方法为：

从UUV取出本次通信延迟时长N_i中系统每一时刻的滤波量测更新量Q_i和信息发送时刻的滤波状态更新量M_s，得到系统状态估计一步预测误差：

$\mathrm{\δ}{\hat{X}}_{k,k-1}=\left(\underset{i=s}{\overset{k}{\mathrm{\Π}}}{Q}_i\right)M_s$

系统T_k时刻状态估计为：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\hat{X}}_{k,k-1}^{\′}+\mathrm{\δ}{\hat{X}}_{k,k-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-C_k{\hat{X}}_{k,k-1}]\end{array}\right.$

其中δ为误差符号，为考虑误差补偿后的系统状态估计一步预测，为T_k时刻系统状态估计。

发明提出了一种基于水声通信延迟的主从式多UUV协同定位方法，该方法既能够利用主UUV的精确相对定位信息和从UUV航推定位信息，又能有效的补偿由于水声通信延迟造成的定位误差，提高从UUV的定位精度。因此本发明提出的多UUV协同定位方法具有定位成本低、定位精度高的显著进步。

附图说明

图1是本发明方法流程示意图；

图2是本发明中主UUV与从UUV的运动轨迹示意图；

图3是本发明提出的方法对水声通信延迟补偿后的定位误差仿真示意图。

具体实施方式

具体实施方式一、结合图1说明本具体实施方式，一种基于水声通信延迟的主从式多UUV协同定位方法，其特征包括以下步骤：

步骤一、主、从UUV进行时钟同步准备，主UUV通过水声测距测量主、从UUV之间的距离；

步骤二、主UUV通过水声设备向从UUV发送自身的定位信息、间距信息以及信息发送时刻的时间戳；

步骤三、从UUV接收到主UUV水声信息后利用计算机时钟计算本次水声通信的延迟时长，采集自身航推定位信息，建立协同定位系统的状态方程；

步骤四、从UUV计算本时刻系统状态转移矩阵，并取出延迟阶段每一时刻的状态转移矩阵，得到延迟前后时刻系统状态量之间的关系；

步骤五、从UUV利用步骤四得到的延迟前后时刻系统状态量之间的关系，建立协同定位系统量测方程；

步骤六、利用步骤四的系统量测方程，完成协同定位系统量测更新，保存系统滤波量测更新量；

步骤七、利用步骤三的系统状态方程，完成协同定位系统状态更新，保存系统滤波状态更新量；

步骤八、从UUV取出延迟阶段每一时刻的滤波量测更新量和状态更新量，对协同定位系统由于通信延时造成的状态估计误差进行补偿。

本发明的工作原理是：

第一步：主、从UUV进行时钟同步准备，即在航行试验开始之前将主UUV计算机时钟、水声设备时钟、从UUV计算机时钟调整到同一时间，精度准确到0.1秒。时钟同步后，主UUV通过水声测距测量主、从UUV之间的距离r；

第二步：主UUV通过水声设备向从UUV发送自身的定位信息x_a、y_a、z_a，x轴平行于地球纬线切线方向，东向为正方向，y轴平行于地球经线切线方向，北向为正方向。间距信息r以及信息发送时刻的时间戳T；

第三步：从UUV接收到主UUV水声信息后利用计算机时钟计算本次水声通信的延迟时长N_i，结合自身航推定位信息：航速V_b以及航向角速度w_b，建立协同定位系统的状态方程；

从UUV接收到主UUV水声信息后，记录计算机时钟的接收时刻，与主UUV发送时刻相减得到本次水声通信的延迟时长为N_i。

系统状态量为：

X_k＝[X_ak ^T X_bk ^T]^T＝(x_as，y_as，z_as，x_bk，y_bk，z_ak，φ_bk)^T

其中下标a、b分别代表主UUV和从UUV，下标s、k分别代表主UUV信息发送时刻T_s和从UUV信息接收时刻T_k。X_k为系统状态量，X_ak为主UUV状态量，X_bk为从UUV状态量，x_as，y_as，z_as为主UUV T_s时刻的定位信息，x_bk，y_bk，z_ak为从UUV T_k时刻的航推定位信息，φ_bk为从UUV的航向角。

系统输入为：

u_k＝[u_ak u_bk]^T＝(x_as,y_as,z_as,V_bk,w_bk)^T

式中u_k为系统输入值，u_ak＝[x_as,y_as,z_as]^T为主UUVT_k时刻的输入值，u_bk＝[V_bk,w_bk]^T为从UUVT_k时刻的输入值。

系统状态方程为：

X_k＝A_k-1X_k-1+G_k-1W_k-1

其中X_k-1为T_k-1时刻系统状态量，

$A_{k-1}=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& \mathrm{\δt}\·V_{b(k-1)}\cos {\mathrm{\φ}}_{b(k-1)}\\ 0& 0& 0& 0& 1& 0& -\mathrm{\δt}\·V_{b(k-1)}\sin {\mathrm{\φ}}_{b(k-1)}\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]$ 为系统矩阵，

δt为系统滤波周期，

$G_{k-1}=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& \mathrm{\δt}\·\sin {\mathrm{\φ}}_{b(k-1)}& 0\\ 0& 0& 0& \mathrm{\δt}\·\cos {\mathrm{\φ}}_{b(k-1)}& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& \mathrm{\δt}\end{array}\right]$ 为系统噪声驱动阵，

W_k-1为系统噪声阵。

且有 $W_k={(W_{x_{\mathrm{as}}},W_{y_{\mathrm{as}}},W_{z_{\mathrm{as}}},W_{V_{\mathrm{bk}}},W_{w_{\mathrm{bk}}})}^T,$ 和分别为零均值且相互独立的高斯白噪声，其方差阵分别为

系统噪声方差阵：

$Q_k=E\left[W_k{W_k}^T\right]=\left[\begin{array}{ccccc}{q}_{x_{\mathrm{as}}}^2& 0& 0& 0& 0\\ 0& q_{y_{\mathrm{as}}}^2& 0& 0& 0\\ 0& 0& q_{y_{\mathrm{as}}}^2& 0& 0\\ 0& 0& 0& q_{V_{\mathrm{bk}}}^2& 0\\ 0& 0& 0& 0& q_{w_{\mathrm{bk}}}^2\end{array}\right].$

第四步：从UUV计算本时刻系统状态转移矩阵，并取出延迟阶段每一时刻的状态转移矩阵，得到延迟前后时刻系统状态量之间的关系；

从UUVT_k时刻系统状态转移矩阵为：

${\mathrm{\Φ}}_b\left(k\right)=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\δt}\·V_{\mathrm{bk}}\cos {\mathrm{\φ}}_{\mathrm{bk}}\\ 0& 1& 0& -\mathrm{\δt}\·V_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

从UUV延迟前后时刻系统状态量之间的关系为：

$X_{\mathrm{bs}}={\mathrm{\Φ}}_b(s,k)X_{\mathrm{bk}}={\mathrm{\Φ}}_b{(k,s)}^{-1}{X}_{\mathrm{bk}}={\left(\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b\left(i\right)\right)}^{-1}{X}_{\mathrm{bk}}$

其中为本次通信延长时长N_i中每时刻的状态转移矩阵乘积，且有：

$\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b\left(i\right)=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\δt}\underset{i=s}{\overset{k}{\mathrm{\Σ}}}{V}_{\mathrm{bi}}\cos {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 1& 0& -\mathrm{\δt}\underset{i=s}{\overset{k}{\mathrm{\Σ}}}{V}_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

第五步：从UUV利用第四步得到的延迟前后时刻系统状态量之间的关系，建立协同定位系统量测方程；

系统量测方程为：

Z_k＝r_s+v_k＝C_kX_k+v_k

其中Z_k为系统量测量，r_s为T_s时刻主UUV测得的间距，v_k为系统量测噪声，C_k为系统量测矩阵：

C_k＝[c_ak c_bsΦ_b(k，s)^-1]

其中主UUV量测矩阵：

$c_{\mathrm{ak}}=\left[\begin{array}{ccc}\frac{(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}& \frac{(z_{\mathrm{as}}-z_{\mathrm{bs}})}{r_s}\end{array}\right],$

从UUV量测矩阵：

$c_{\mathrm{bs}}=\left[\begin{array}{cccc}\frac{-(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{-(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}& \frac{-(z_{\mathrm{as}}-z_{\mathrm{bs}})}{r_s}& 0\end{array}\right].$

第六步：利用第五步的系统量测方程，完成协同定位系统量测更新，保存系统滤波量测更新量；

系统量测更新过程为：

$\left\{\begin{array}{c}{P}_{k,k-1}=A_{k-1}{P}_{k-1}{A_{k-1}}^T+G_{k-1}{Q}_{k-1}{G_{k-1}}^T\\ K_k=P_{k,k-1}{C}_k^T{[C_k{P}_{k,k-1}{C}_k^T+v_k]}^{-1}\\ P_k=(I-K_k{C}_k)P_{k,k-1}\\ Q_k=A_k(I-K_k{C}_k)\end{array}\right.$

其中P_k,k-1为系统状态估计一步预测均方误差，K_k为系统T_k时刻滤波增益，P_k为系统状态估计均方误差。Q_k为保存的系统T_k时刻滤波量测更新量。

第七步：利用第三步的系统量测方程，完成协同定位系统状态更新，保存系统滤波状态更新量；

系统状态更新过程为：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}^{\′}=A_{k-1}{\hat{X}}_{k-1}\\ M_k=A_k{K}_k^d[Z_k-C_k{\hat{X}}_{k,k-1}^d]\end{array}\right.$

其中为系统状态估计一步预测，M_k为保存的系统T_k时刻滤波状态更新量。

第八步：从UUV取出延迟阶段每一时刻的滤波量测更新量和状态更新量，对协同定位系统由于通信延时造成的状态估计误差进行补偿。

从UUV取出本次通信延迟时长N_i中系统每一时刻的滤波量测更新量Q_i和信息发送时刻的滤波状态更新量M_s，得到系统状态估计一步预测误差：

$\mathrm{\δ}{\hat{X}}_{k,k-1}=\left(\underset{i=s}{\overset{k}{\mathrm{\Π}}}{Q}_i\right)M_s$

系统T_k时刻状态估计为：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\hat{X}}_{k,k-1}^{\′}+\mathrm{\δ}{\hat{X}}_{k,k-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-C_k{\hat{X}}_{k,k-1}]\end{array}\right.$

其中δ为误差符号，为考虑误差补偿后的系统状态估计一步预测，为T_k时刻系统状态估计。

本发明提供的基于水声通信延迟定位误差补偿的多UUV协同定位方法能够有效地降低由于水声通信通信延迟造成的定位误差，提高从UUV定位精度。

本发明提供的基于水声通信延迟的主从式多UUV协同定位方法具有以下优点：

一方面，充分利用主UUV的高精度定位能力，提高了主UUV的高成本导航设备利用效率，从而降低了系统成本；另一方面，降低了了水声通信延迟对协同定位的影响，使得多UUV系统的可靠性增强。因此，在实际工程使用中，利用上述步骤对水声通信延迟误差进行补偿，能够大大提高从UUV的定位精度，抑制从UUV航推定位误差发散。

为了进一步说明所述方法的有益效果，在以下初始条件下分别对从UUV航推定位、水声通信延迟误差不补偿、水声通信延迟误差补偿三种情况进行仿真，仿真结果如图2、图3所示，并对其进行了分析比较。

初始条件：主UUV的起点坐标是(0,1000,2000)，单位为米，航速为1.5m/s；从UUV的起点坐标是(0,0,2000)，单位为米，初始航速为0.5m/s，加速度为0.0005m/s²；通信延时计算公式为其中1500m/s为信号在水中传播速度，5秒为设备处理信号时间，水声通信延迟随机误差δt服从均值为0s，方差为0.1s的正态分布；滤波周期为1秒，仿真时长为1小时；接收艇测速误差为0.2m/s，航向角速度测量误差10°/h，两艇之间距离测量误差为5m。

分析比较：图2给出了发送艇和接收艇的运动轨迹，这种相对运动基本上包括了实际工作过程中所有的相对运动方式(直线、拐弯运动)，所以仿真结果具有代表性。根据图2可以看出，如果存在水声通信延迟而不进行误差补偿，会产生非常大的定位误差，从而导致协同定位失效。通过对通信延迟的补偿，可以提高从UUV协同定位精度。这证明了本发明提供的误差补偿方法能够有效处理水声通信延迟对多UUV协同定位的影响。

本发明提出了一种基于水声通信延迟的主从式多UUV协同定位技术，既能够利用主UUV的精确相对定位信息和从UUV航推定位信息，又能有效的补偿由于水声通信延迟造成的定位误差，提高从UUV的定位精度。本发明提出的多UUV协同定位方法具有降低定位成本、提高定位精度等显著优点。

Claims (5)

1.一种基于水声通信延迟的主从式多UUV协同定位方法，其特征是：它由以下步骤实现：

步骤一、设置主UUV和从UUV为时钟同步；主UUV通过水声测距的方法测量主UUV与从UUV之间的距离r；

步骤二、设平行于地球纬线切线方向为x轴，东向为正方向，以平行于地球经线切线方向为y轴，北向为正方向，以同时垂直x轴和y轴的方向为z轴，向上为正方向，建立三维直角坐标系；主UUV通过水声设备向从UUV发送自身的定位信息x_a、y_a、z_a、主UUV与从UUV之间的距离r以及信息发送时刻的时间戳T；

步骤三、从UUV接收到主UUV水声信息后计算本次水声通信的延迟时长N_i，并结合从UUV自身航推定位信息建立协同定位系统的状态方程；从UUV自身航推定位信息包括航速V_b以及航向角速度w_b；

建立协同定位系统的状态方程的具体方法为：

从UUV接收到主UUV水声信息后，记录计算机时钟的接收时刻，与主UUV发送时刻相减得到本次水声通信的延迟时长N_i；

系统状态量为：

$X_k={\left[{X_{\mathrm{ak}}}^T{X_{\mathrm{bk}}}^T\right]}^T={(x_{\mathrm{as}},y_{\mathrm{as}},z_{\mathrm{as}},x_{\mathrm{bk}},y_{\mathrm{bk}},z_{\mathrm{ak}},{\mathrm{\φ}}_{\mathrm{bk}})}^T$

其中下标a、b分别代表主UUV和从UUV，下标s、k分别代表主UUV信息发送时刻T_s和从UUV信息接收时刻T_k；

X_k为系统状态量，X_ak为主UUV状态量，X_bk为从UUV状态量，x_as，y_as和z_as为主UUV T_s时刻的定位信息，x_bk，y_bk和z_ak为从UUV T_k时刻的航推定位信息，φ_bk为从UUV的航向角；

系统输入为：

u_k＝[u_ak u_bk]^T＝(x_as,y_as,z_as,V_bk,w_bk)^T

式中u_k为系统输入值；

u_ak＝[x_as,y_as,z_as]^T为主UUV在T_k时刻的输入值，

u_bk＝[V_bk,w_bk]^T为从UUV在T_k时刻的输入值；

则系统状态方程为：

X_k＝A_k-1X_k-1+G_k-1W_k-1

其中：X_k-1为T_k-1时刻系统状态量，

$A_{k-1}=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& \mathrm{\δt}\·V_{b(k-1)}\cos {\mathrm{\φ}}_{b(k-1)}\\ 0& 0& 0& 0& 1& 0& -\mathrm{\δt}\·V_{b(k-1)}\sin {\mathrm{\φ}}_{b(k-1)}\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

为系统矩阵，

δt为系统滤波周期，

$G_{k-1}=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& \mathrm{\δt}\·\sin {\mathrm{\φ}}_{b(k-1)}& 0\\ 0& 0& 0& \mathrm{\δt}\·\cos {\mathrm{\φ}}_{b(k-1)}& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& \mathrm{\δt}\end{array}\right]$

为系统噪声驱动阵，W_k-1为系统噪声阵；

且有：

$W_k={(W_{x_{\mathrm{as}}},W_{y_{\mathrm{as}}},W_{z_{\mathrm{as}}},W_{V_{\mathrm{bk}}},W_{w_{\mathrm{bk}}})}^T,$

和分别为零均值且相互独立的高斯白噪声，其方差阵分别为：

系统噪声方差阵：

$Q_k=E\left[W_k{W_k}^T\right]=\left[\begin{array}{ccccc}{q}_{x_{\mathrm{as}}}^2& 0& 0& 0& 0\\ 0& q_{y_{\mathrm{as}}}^2& 0& 0& 0\\ 0& 0& q_{y_{\mathrm{as}}}^2& 0& 0\\ 0& 0& 0& q_{V_{\mathrm{bk}}}^2& 0\\ 0& 0& 0& 0& q_{w_{\mathrm{bk}}}^2\end{array}\right];$

步骤四、从UUV计算在T_k时刻下系统状态转移矩阵，并取出延迟阶段每一时刻的状态转移矩阵，得到延迟前后时刻系统状态量之间的关系；

从UUV在T_k时刻系统下状态转移矩阵为：

${\mathrm{\Φ}}_b\left(k\right)=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\δt}\·V_{\mathrm{bk}}\cos {\mathrm{\φ}}_{\mathrm{bk}}\\ 0& 1& 0& -\mathrm{\δt}\·V_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

从UUV延迟前后时刻系统状态量之间的关系为：

$X_{\mathrm{bs}}={\mathrm{\Φ}}_b(s,k)X_{\mathrm{bk}}={\mathrm{\Φ}}_b{(k,s)}^{-1}{X}_{\mathrm{bk}}={\left(\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b\left(i\right)\right)}^{-1}{X}_{\mathrm{bk}}$

其中为本次通信延迟时长N_i中每时刻的状态转移矩阵乘积，且有：

$\underset{i=s}{\overset{k}{\mathrm{\Π}}}{\mathrm{\Φ}}_b\left(i\right)=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\δt}\underset{i=s}{\overset{k}{\mathrm{\Σ}}}{V}_{\mathrm{bi}}\cos {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 1& 0& -\mathrm{\δt}\underset{i=s}{\overset{k}{\mathrm{\Σ}}}{V}_{\mathrm{bi}}\sin {\mathrm{\φ}}_{\mathrm{bi}}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

步骤五、从UUV利用步骤四获得的延迟前后时刻系统状态量之间的关系，建立协同定位系统量测方程；

步骤六、利用步骤五获得的系统量测方程，完成协同定位系统量测更新，保存系统滤波量测更新量；

步骤七、利用步骤三获得的系统状态方程，完成协同定位系统状态更新，保存系统滤波状态更新量；

步骤八、从UUV取出延迟阶段每一时刻的滤波量测更新量和状态更新量，对协同定位系统由于通信延时造成的状态估计误差进行补偿；完成一次基于水声通信延迟的主从式多UUV协同定位。

2.根据权利要求1所述的一种基于水声通信延迟的主从式多UUV协同定位方法，其特征在于步骤五中建立协同定位系统量测方程为：

Z_k＝r_s+v_k＝C_kX_k+v_k

其中Z_k为系统量测量，r_s为T_s时刻主UUV测得的间距，v_k为系统量测噪声，C_k为系统量测矩阵：

C_k＝[c_ak c_bsΦ_b(k，s)^-1]

其中主UUV量测矩阵：

$c_{\mathrm{ak}}=\left[\begin{array}{ccc}\frac{(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}& \frac{(z_{\mathrm{as}}-z_{\mathrm{bs}})}{r_s}\end{array}\right],$

从UUV量测矩阵：

$c_{\mathrm{bs}}=\left[\begin{array}{cccc}\frac{-(x_{\mathrm{as}}-x_{\mathrm{bs}})}{r_s}& \frac{-(y_{\mathrm{as}}-y_{\mathrm{bs}})}{r_s}& \frac{-(z_{\mathrm{as}}-z_{\mathrm{bs}})}{r_s}& 0\end{array}\right].$

3.根据权利要求2所述的一种基于水声通信延迟的主从式多UUV协同定位方法，其特征在于步骤六中利用步骤五获得的系统量测方程，完成协同定位系统量测更新的方法为：

系统量测更新过程为：

$\left\{\begin{array}{c}{P}_{k,k-1}=A_{k-1}{P}_{k-1}{A_{k-1}}^T+G_{k-1}{Q}_{k-1}{G_{k-1}}^T\\ K_k=P_{k,k-1}{C}_k^T{[C_k{P}_{k,k-1}{C}_k^T+v_k]}^{-1}\\ P_k=(I-K_k{C}_k)P_{k,k-1}\\ Q_k=A_k(I-K_k{C}_k)\end{array}\right.$

其中：P_k,k-1为系统状态估计一步预测均方误差，K_k为系统T_k时刻滤波增益，P_k为系统状态估计均方误差；Q_k为保存的系统T_k时刻滤波量测更新量。

4.根据权利要求3所述的一种基于水声通信延迟的主从式多UUV协同定位方法，其特征在于步骤七中利用步骤五获得的系统量测方程，完成协同定位系统状态更新的方法为：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}^{\text{'}}=A_{k-1}{\hat{X}}_{k-1}\\ M_k=A_k{K}_k^d[Z_k-C_k{\hat{X}}_{k,k-1}^d]\end{array}\right.$

其中：为系统状态估计一步预测，M_k为保存的系统T_k时刻滤波状态更新量。

5.根据权利要求4所述的一种基于水声通信延迟的主从式多UUV协同定位方法，其特征在于步骤八所述从UUV取出延迟阶段每一时刻的滤波量测更新量和状态更新量，对协同定位系统由于通信延时造成的状态估计误差进行补偿的具体方法为：

从UUV取出本次通信延迟时长N_i中系统每一时刻的滤波量测更新量Q_i和信息发送时刻的滤波状态更新量M_s，得到系统状态估计一步预测误差：

$\mathrm{\δ}{\hat{X}}_{k,k-1}=\left(\underset{i=s}{\overset{k}{\mathrm{\Π}}}{Q}_i\right)M_s$

系统T_k时刻状态估计为：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\hat{X}}_{k,k-1}^{\text{'}}+\mathrm{\δ}{\hat{X}}_{k,k-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-C_k{\hat{X}}_{k,k-1}]\end{array}\right.$

其中δ为误差符号，为考虑误差补偿后的系统状态估计一步预测，为T_k时刻系统状态估计。