[发明专利]一种基于水声通信延迟的主从式多UUV协同定位方法

摘要

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

主权项

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＝[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为主UUVT_s时刻的定位信息，x_bk，y_bk和z_ak为从UUVT_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时刻系统状态量，Ak-1=100000001000000010000000100δt·Vb(k-1)cosφb(k-1)000010-δt·Vb(k-1)sinφb(k-1)00000100000001]]>为系统矩阵，δt为系统滤波周期，Gk-1=100000100000100000δt·sinφb(k-1)0000δt·cosφb(k-1)0000000000δt]]>为系统噪声驱动阵，W_k-1为系统噪声阵；且有：Wk=(Wxas,Wyas,Wzas,WVbk,Wwbk)T,]]>和分别为零均值且相互独立的高斯白噪声，其方差阵分别为：qxas2,qyas2,qzas2,qVbk2,qwbk2;]]>系统噪声方差阵：Qk=E[WkWkT]=qxas200000qyas200000qyas200000qVbk200000qwbk2;]]>步骤四、从UUV计算在T_k时刻下系统状态转移矩阵，并取出延迟阶段每一时刻的状态转移矩阵，得到延迟前后时刻系统状态量之间的关系；从UUV在T_k时刻系统下状态转移矩阵为：Φb(k)=100δt·Vbkcosφbk010-δt·Vbisinφbi00100001]]>从UUV延迟前后时刻系统状态量之间的关系为：Xbs=Φb(s,k)Xbk=Φb(k,s)-1Xbk=(Πi=skΦb(i))-1Xbk]]>其中为本次通信N_i个滤波周期延迟中每时刻的状态转移矩阵乘积，且有：Πi=skΦb(i)=100δtΣi=skVbicosφbi010-δtΣi=skVbisinφbi00100001;]]>步骤五、从UUV利用步骤四获得的延迟前后时刻系统状态量之间的关系，建立协同定位系统量测方程；步骤六、利用步骤五获得的系统量测方程，完成协同定位系统量测更新，保存系统滤波量测更新量；步骤七、利用步骤三获得的系统状态方程，完成协同定位系统状态更新，保存系统滤波状态更新量；步骤八、从UUV取出延迟阶段每一时刻的滤波量测更新量和状态更新量，对协同定位系统由于通信延时造成的状态估计误差进行补偿；完成一次基于水声通信延迟的主从式多UUV协同定位。