[發明專利]一種粒子濾波BSLAM方法

有效

• 申請號: 202110028795.7
• 申請日: 2021-01-11
• 公開（公告）號: CN112732854B
• 公開（公告）日: 2023-03-31
• 發明（設計）人: 李曄;張千一;馬騰;叢正;貢雨森
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G06F16/29
• 分類號: G06F16/29;G06F17/10;G01S15/89;G01S15/00
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種粒子濾波BSLAM方法，其特征在于，包括以下步驟：

步驟一：算法初始化，包括：

1)設定粒子數N、粒子閉環檢測半徑R、粒子匹配子地圖M_p的測線數目N_p、粒子軌跡子地圖M_h的測線數目N_h、算法總步數T；

2)初始化粒子集P_(1:N)，對于任一粒子P_(n)，n為粒子序數，其內部儲存粒子當前狀態粒子自身歷史軌跡/粒子權值被設定為默認值w_n＝1/N，t為時間步長；

3)t＝1時刻，P_(1:N)的狀態和自身歷史軌跡被初始化為由GPS獲得的先驗狀態GPS_t＝1，/轉入步驟二；

步驟二：在每一時刻t，2≤t≤T輸入對海底的觀測值z_(t)與里程計更新值v_(t)以及里程計DR_(t)后，對P_(1:N)中所有粒子進行操作，具體為：

1)運動更新：使用含有噪聲ω的里程計模型更新

2)閉環檢測：計算與粒子自身歷史軌跡的最小歐氏距離r，記錄其對應的時刻t_r，若r＜R,則進行粒子觀測更新，否則該粒子n跳過觀測更新且不加入重采樣步驟；

3)觀測更新：以與里程計的軌跡/結合對應時刻觀測值/構成M_p；以/與里程計的軌跡/結合對應時刻觀測值/構成M_h，使用反距離加權算法對M_p在M_h相應位置進行插值，并計算w_n，具體為：

步驟2.1：輸入粒子t、t_r時刻狀態觀測值z_(1:t)，里程計軌跡DR_(1:t)，轉入步驟2.2；

步驟2.2：生成粒子匹配子地圖軌跡：

并將x_p與其對應的觀測值結合生成粒子匹配子地圖M_p；同樣的，生成粒子軌跡子地圖軌跡：

并將x_h與其對應的觀測值結合生成粒子軌跡子地圖M_h，轉入步驟2.3；

步驟2.3：使用反距離加權算法對M_p進行插值，插值位置為M_h中測點的位置，獲得包含L個共有有效測點的觀測值序列定義為Z_p,定義M_h中相同位置的觀測值序列為Z_h，計算粒子權值：

其中：與/分別為Z_p與Z_h的第l個有效測點的觀測值；σ²為傳感器的測量方差；/

4)粒子歷史軌跡更新：將加入粒子自身歷史軌跡/轉入步驟三；

步驟三：當t時刻所有粒子都進行了步驟二后，判斷是否需要進行重采樣，定義進行觀測更新的粒子總數為N_th，定義有效粒子數：

其中為第i個粒子的歸一化權值，若N_eff滿足設定重采樣條件，將所有經過觀測更新的粒子進行重采樣，t＝t+1，轉入步驟二，否則t＝t+1，轉入步驟二；

步驟四：當t＝T時，以最終粒子集中每個粒子軌跡的平均值結合對應時刻的觀測值z_(1:t)生成海底地形圖并輸出，結束。