[發明專利]一種基于粒子濾波算法的水聲導航定位方法

有效

• 申請號: 201910063247.0
• 申請日: 2019-01-23
• 公開（公告）號: CN109540154B
• 公開（公告）日: 2019-12-27
• 發明（設計）人: 王海斌;李瑨瑤;汪俊;韓一丁;陳曦
• 申請（專利權）人: 中國科學院聲學研究所
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 11457 北京律譜知識產權代理事務所(普通合伙)
• 代理人: 黃云鐸

1.一種基于粒子濾波算法的水聲導航定位方法，其特征在于，該方法包括：

步驟1，根據信標的第二位置坐標和航行器與所述信標之間的脈沖定位信號，構建聲學導航模型，其中，所述脈沖定位信號由所述信標向所述航行器發送，

所述步驟1具體包括：

步驟11，根據所述航行器的航行速度以及所述脈沖定位信號傳播的等效聲速改變量，構建第一矩陣方程，所述第一矩陣方程的計算公式為：

X_k＝X_k-1+V_k-1，

X_k＝[x_k y_k c_1k … c_sk … c_mk]^T，

式中，m為所述信標的數量，s＝1…m，c_sk為當前時刻k第s個信標與所述航行器之間的等效聲速，(x_k,y_k)為所述航行器的第一位置坐標，(v_xk,v_yk)為所述航行器的航行速度，n_k^cs為當前時刻k所述航行器與第s個信標之間所述脈沖定位信號傳播的所述等效聲速改變量；

步驟12，根據所述第二位置坐標、所述脈沖定位信號的傳播時間以及所述傳播時間的誤差參數，構建第二矩陣方程，所述第二矩陣方程的計算公式為：

Z_k＝H(X_k)+N_k，

Z_k＝[t_1k … t_sk … t_mk]^T，

N_k＝[n_1k … n_sk … n_mk]^T，

式中，(x_os,y_os)為第s個信標的所述第二位置坐標，t_sk為當前時刻k第s個信標的所述脈沖定位信號到達所述航行器的所述傳播時間，n_sk為所述誤差參數，所述誤差參數n_sk滿足零均值高斯分布其中，為所述零均值高斯分布中的方差參數；

步驟13，將所述第一矩陣方程和所述第二矩陣方程，記作所述聲學導航模型；

步驟2，采用加權算法，根據所述脈沖定位信號和所述聲學導航模型，計算所述航行器的所述第一位置坐標和等效聲速。

2.如權利要求1所述的基于粒子濾波算法的水聲導航定位方法，其特征在于，所述步驟2中，具體包括：根據所述航行器接收到的所述脈沖定位信號和所述聲學導航模型，計算所述加權算法中的權值，所述權值的計算公式為：

式中，為所述權值，為所述零均值高斯分布中的方差參數。

3.如權利要求1所述的基于粒子濾波算法的水聲導航定位方法，其特征在于，還包括：

步驟3，根據當前時刻的所述脈沖定位信號和預設修訂公式，修訂下一時刻接收到的所述脈沖定位信號，其中，所述修訂公式為：

式中，A為第一運動系數矩陣，U_k+1|k為第二運動系數矩陣，X_k|kⁱ為當前時刻的所述脈沖定位信號，X_k+1|kⁱ為下一時刻接收到的所述脈沖定位信號。

4.如權利要求1所述的基于粒子濾波算法的水聲導航定位方法，其特征在于，還包括：

步驟4，根據所述第一位置坐標和預設坐標，計算所述航行器的位置偏差；

步驟5，當判定所述位置偏差大于或等于第一預設閾值時，生成最近導航信息。

5.如權利要求4所述的基于粒子濾波算法的水聲導航定位方法，其特征在于，還包括：

步驟6，當判定所述位置偏差大于或等于第二預設閾值時，生成報警信息，其中，所述第二預設閾值大于所述第一預設閾值。