[發(fā)明專利]一種粒子濾波BSLAM方法

有效

• 申請?zhí)枺?/td>: 202110028795.7
• 申請日: 2021-01-11
• 公開（公告）號: CN112732854B
• 公開（公告）日: 2023-03-31
• 發(fā)明（設(shè)計）人: 李曄;張千一;馬騰;叢正;貢雨森
• 申請（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號: G06F16/29
• 分類號: G06F16/29;G06F17/10;G01S15/89;G01S15/00
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發(fā)明公開了一種粒子濾波BSLAM方法，包括：步驟一：算法初始化；步驟二：在每個t時刻輸入對海底地形的深度測量值z_(t)與里程計更新值v_(t)以及里程計DR_(t)后，對P_(1:N)中所有粒子進行運動更新、閉環(huán)檢測、觀測更新、粒子歷史軌跡更新；步驟三：當(dāng)t時刻所有粒子都進行了步驟二后，判斷是否需要進行重采樣，若是：將所有經(jīng)過觀測更新的粒子進行重采樣，t＝t+1，轉(zhuǎn)入步驟二，否則t＝t+1，轉(zhuǎn)入步驟二；步驟四：當(dāng)t＝T時，以最終粒子集中每個粒子軌跡的平均值結(jié)合對應(yīng)時刻的觀測數(shù)據(jù)z_(1:t)生成海底地形圖并輸出。本發(fā)明只需要在每一時刻輸入里程計數(shù)據(jù)和由多波束聲納獲得的地形測深，即可在沒有先驗地形圖的情況下實現(xiàn)AUV的同步定位與建圖。

技術(shù)領(lǐng)域

本發(fā)明涉及一種粒子濾波BSLAM方法，特別是一種使用粒子自身儲存軌跡進行權(quán)值更新的粒子濾波BSLAM方法，屬于海底地形同步定位與建圖領(lǐng)域。

背景技術(shù)

BSLAM以多波束聲納為傳感器收集海底地形信息，從而實現(xiàn)智能水下機器人不依賴于外界提供信息和先驗地形數(shù)據(jù)的情況下進行同步定位與建圖，粒子濾波是實現(xiàn)這一方法的重要手段之一。但目前的粒子濾波BSLAM依賴于柵格進行地形匹配，每個粒子都需要儲存一副柵格地圖，這導(dǎo)致了算法在運行中占用了很大的內(nèi)存開銷，且粒子在重采樣時需要復(fù)制其內(nèi)部的整個柵格地圖，同樣造成了很大的時間開銷。因此，需要一種可以在小內(nèi)存和時間消耗的情況下進行BSLAM的方法。

目前針對使用粒子自身儲存軌跡進行權(quán)值更新的粒子濾波BSLAM沒有相關(guān)方法，較為相似的是：公開日為2020年1月24日，公開號為CN110726415A，發(fā)明名稱為“一種自適應(yīng)的水下多波束同步定位與建圖方法”，該方法使用粒子集均值的軌跡作為算法生成的地圖，但是無法處理粒子在重采樣后聚類，由粒子集均值所生成軌跡不是最優(yōu)軌跡的情況，而且需要在重采樣后加入更新均值地圖部分，算法較為復(fù)雜。

發(fā)明內(nèi)容

針對上述現(xiàn)有技術(shù)，本發(fā)明要解決的技術(shù)問題是提供一種使用粒子自身儲存軌跡進行權(quán)值更新的粒子濾波BSLAM方法，能夠減少計算開銷，且算法結(jié)構(gòu)簡單，有利于在計算資源受限的水下機器人工作。

為解決上述技術(shù)問題，本發(fā)明的一種粒子濾波BSLAM方法，包括以下步驟：

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

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

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

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

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

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

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