[發明專利]一種基于多伯努利濾波器的同時定位與建圖方法

在審

• 申請號: 202211041346.7
• 申請日: 2022-08-29
• 公開（公告）號: CN115307645A
• 公開（公告）日: 2022-11-08
• 發明（設計）人: 章飛;張子菁;姬傳堂
• 申請（專利權）人: 江蘇科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06T7/277;G06T7/73
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 袁姝

本發明公開一種基于多伯努利濾波器的同時定位與建圖方法，具體步驟包括(1)參數初始化；(2)獲得輸入數據；(3)獲得機器人位置預測值；(4)獲得機器人的觀測集；(5)通過勢均衡多伯努利濾波方法獲得機器人在第k時刻用于表示地圖特征的伯努利項；(6)對獲得伯努利項進行目標提??；(7)記錄k時刻得到的地圖特征數目及位姿；(8)通過自適應信息控制法，判斷是否執行圖優化過程；(9)通過圖優化方法t個時刻所對應的機器人位姿，然后執行步驟(2)；(10)判斷是否達到最大運行時刻數，來決定是否輸出機器人位姿和地圖特征的狀態估計值。本發明的方法，改善了同時定位與建圖方法中的機器人位姿估計精度，提高了實時性。

技術領域

本發明涉及一種同時定位與建圖方法，尤其涉及一種基于多伯努利濾波器的同時定位與建圖方法。

背景技術

導航技術對于移動機器人在未知環境中工作必不可少，而同步定位與地圖構建(同時定位與建圖)是近年來的主要導航技術。導航算法精度、速度、實時性和穩定性的不斷提高一直是研究的重點。在一些復雜的環境中，如水下勘探環境、室內消防救援環境等，由于雜波密集，傳統同時定位與建圖算法的數據關聯精度會降低，計算量會大大增加，導致傳統同時定位與建圖方法的準確性下降。為了解決復雜的數據關聯問題，一種方法是避免數據關聯問題，另一種方法是使用更有效的算法，比如基于圖的同時定位與建圖。

基于隨機有限集(RFS)的多目標貝葉斯濾波器通過對測量更新中所有可能的關聯進行平均來避免數據關聯的問題。RFS是一個設置值的隨機變量，即狀態向量的個數和狀態向量本身都是隨機的，所以RFS自然獲得了地圖的不確定性。在隨后的研究中，將RFS應用到同時定位與建圖領域，提出了假設概率密度濾波器-同時定位與建圖(PHD-SLAM)算法。假設概率密度濾波器-同時定位與建圖算法在線性條件下通過高斯混合實現，在非線性條件下通過順序蒙特卡羅實現，但這兩種方法都使用近似策略計算粒子權重。雖然避免了數據關聯，但其算法估計誤差較大。且由于基于RFS理論的傳統同時定位與建圖方法在機器人位姿估計部分使用粒子濾波方法，這導致機器人在復雜室內環境下需要大量粒子近似機器人位姿的后驗分布，導致計算量大，實時性不高，以及機器人位姿估計精度低的問題。

發明內容

本發明目的是為了解決機器人位姿估計精度低，實時性不好的問題，而提供一種基于位姿圖優化的勢均衡多伯努利濾波同時定位與建圖方法。

為達到上述目的，本發明采用的技術方案是：

一種基于多伯努利濾波器的同時定位與建圖方法，包括以下步驟：

(1)對運行時刻數參數、優化時刻數參數、多伯努利存在參數、最大運行時刻數參數進行初始化；

(2)通過機器人所攜帶的傳感器獲得機器人的運動速度v和方向角度θ、地圖特征與機器人的直接距離d和方位角所述傳感器為慣導元件或激光雷達；

(3)基于步驟(2)獲得的機器人運行時的運動速度v和方向角度θ，通過機器人的運動方程f(v,θ,k)計算獲得第k時刻的機器人位姿預測值；

(4)基于步驟(3)獲得第k時刻的機器人位姿預測值，以及基于步驟(2)的直接距離d和方位角通過觀測方程獲得機器人在第k時刻所對應的觀測集；

(5)基于步驟(3)、步驟(4)所獲得的信息，通過勢均衡多伯努利濾波方法對地圖特征進行狀態估計，獲得機器人在第k時刻用于表示地圖特征的伯努利項；

(6)對步驟(5)中獲得的伯努利項，根據其存在概率參數r的值來進行目標提取，其結果作為地圖特征的狀態估計值，該狀態估計值包括地圖特征的數目和地圖特征的位姿；

(7)基于步驟(6)所獲得的地圖特征的狀態估計值，記錄k時刻所得到的地圖特征的數目和地圖特征的位姿；