[發明專利]基于粒子濾波與圖優化的海底地形同步定位與建圖方法

有效

• 申請號: 202110233287.2
• 申請日: 2021-03-03
• 公開（公告）號: CN113008223B
• 公開（公告）日: 2022-10-28
• 發明（設計）人: 李曄;張千一;馬騰;叢正;貢雨森
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/20;G01S15/89;G01S19/45;G06F17/15
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發明屬于海底地形同步定位與建圖技術領域，具體涉及一種基于粒子濾波與圖優化的海底地形同步定位與建圖方法。本發明設計了一種使用位姿圖結構對粒子自身儲存軌跡進行更新，并使用位姿圖在每一時刻結束時生成此時刻輸出地圖的粒子濾波BSLAM算法。本發明只需要在每一時刻輸入里程計數據和由多波束聲獲得的地形測深，在沒有先驗地形圖的情況下實現AUV的同步定位與建圖。本發明所提出的圖優化混合粒子濾波BSLAM算法相較于基于子地圖匹配的粒子濾波BSLAM算法，在大累積誤差下仍能實時準確的提供定位與建圖結果。

技術領域

本發明屬于海底地形同步定位與建圖技術領域，具體涉及一種基于粒子濾波與圖優化的海底地形同步定位與建圖方法。

背景技術

測深信息同步定位與建圖(BathymetricSimultaneousLocalizationandMapping,BSLAM)以多波束聲吶為傳感器收集海底地形信息，能夠實現智能水下機器人在不依賴于外界提供信息的情況下，獨立長時間的準確定位與建圖，粒子濾波是實現這一方法的重要手段之一。但因為粒子濾波只能求解當前時刻的最優狀態，在大累積誤差的情況下會產生性能下降的問題。除此之外，因為基于子地圖匹配的粒子濾波BSLAM中每一個粒子都會儲存一副地圖，選擇這些粒子中最優地圖存在問題?，F有方法中使用粒子儲存地圖重復區域的最小均方誤差作為選擇的標準，但是存在著計算復雜度高，結果不穩定，誤差較大的問題。因為BSLAM輸出的地圖需要為其它功能(如路徑規劃)提供信息，因此，需要一種在大累積誤差下，可以準確實時生成地圖的BSLAM方法。本發明在子地圖匹配粒子濾波BSLAM的基礎上提出了一種基于圖優化理論的粒子地圖更新方法與最優地圖生成算法，能實現智能水下機器人在航位推算累積誤差較大的情況下，能實時的提供準確的建圖結果。

目前針對使用圖優化理論進行地圖更新與最終地圖生成的粒子濾波BSLAM沒有相關方法，較為相似的是：公開日為2020年1月24日，公開號為CN110726415A，發明名稱為“一種自適應的水下多波束同步定位與建圖方法”，該方法雖然可以使用在大累積誤差下實時準確的進行定位與建圖，但是無法處理粒子在重采樣后聚類，由粒子集均值所生成軌跡不是最優軌跡的情況，而且需要在重采樣后加入更新均值地圖部分，算法較為復雜。除此之外，這種方法存在設定參數較多的問題。

發明內容

本發明的目的在于提供在沒有先驗地形圖的情況下實現AUV的同步定位與建圖的一種基于粒子濾波與圖優化的海底地形同步定位與建圖方法。

本發明的目的通過如下技術方案來實現：包括以下步驟：

步驟1：設定粒子數N、粒子閉環檢測半徑R、粒子匹配子地圖M_p的測線數目N_p、粒子軌跡子地圖M_h的測線數目N_h、算法總步數T；初始化粒子集P_(1:N)；對于任一粒子P_(n)，其內部儲存粒子當前狀態粒子自身歷史軌跡粒子權值w_n＝1/N、粒子儲存的閉環集合L_n；n為粒子序數；t為時間步長；

t₀＝1時刻粒子集P_(1:N)的狀態和自身歷史軌跡根據GPS獲得的先驗狀態設定，將t₀＝1時刻以及對應的粒子位姿加入粒子集P_(1:N)中各粒子儲存的閉環集合L_n，初始化t＝2；

步驟2：獲取當前t時刻輸入的對海底的深度測量值z_(t)、里程計更新值v_(t)以及里程計的軌跡DR_(t)，更新粒子集P_(1:N)中各粒子的當前狀態

其中，ω為里程計的噪聲；