[發明專利]基于改進粒子提議分布和自適應粒子重采樣的FastSLAM方法

有效

• 申請號: 201611001062.X
• 申請日: 2016-11-14
• 公開（公告）號: CN106599368B
• 公開（公告）日: 2019-06-25
• 發明（設計）人: 陳耀武;徐巍軍;黃余格
• 申請（專利權）人: 浙江大學
• 主分類號: G06F17/50
• 分類號: G06F17/50
• 代理公司: 杭州天勤知識產權代理有限公司 33224
• 代理人: 胡紅娟

本發明公開了一種基于改進粒子提議分布和自適應粒子重采樣的FastSLAM方法，包括：(1)利用平方根轉換無味卡爾曼濾波器對最優粒子提議分布進行估計，并對機器人位姿狀態進行采樣；(2)利用平方根容積卡爾曼濾波器更新每個粒子對應的特征地圖信息；(3)利用基于相對熵的自適應粒子重采樣方法確定當前時刻所需的粒子個數；(4)根據重采樣后的粒子集確定機器人的位姿狀態和路標特征地圖信息。本發明同時從采樣粒子的質量和數量兩個方面對傳統的FastSLAM算法進行改進：增強算法在估計過程中的數值穩定性和精度，提高采樣粒子的質量；在粒子重采樣過程中根據估計不確定度動態確定最少所需粒子個數，提高算法的計算效率。

技術領域

本發明屬于移動機器人自主導航領域，具體涉及一種基于改進粒子提議分布和自適應粒子重采樣的FastSLAM方法。

背景技術

移動機器人作為智能機器人的重要分支，在諸如制造工業、國防軍事、航天航空、衛生醫療、家庭服務等領域得到了廣泛的應用。為了實現移動機器人在復雜未知環境下，不經過人工干預而順利完成特定作業任務，移動機器人必須具備自主導航的能力。作為自主導航的關鍵前提，移動機器人需要通過攜帶的內部、外部傳感器對自身姿態進行估計，并且同時對未知環境進行地圖描述。移動機器人同時定位與地圖創建(SimultaneousLocalization and Mapping，SLAM)方法可以充分利用定位與地圖創建兩者之間的相關性，在沒有先驗環境地圖信息及GPS等輔助定位設備的情形下，實現未知環境的增量式地圖創建以及自身姿態的在線估計。

Montemerlo等人基于Rao-Blackwellized粒子濾波器思想提出了FastSLAM算法，有效地克服了基于高斯濾波器的SLAM算法計算復雜度高、數據關聯魯棒性差等缺點，實現了任意非線性非高斯隨機系統的狀態估計。在FastSLAM算法中，粒子提議分布的計算是影響算法性能的關鍵因素之一。

傳統的FastSLAM算法利用一階泰勒級數展開對機器人的非線性運動模型和測量模型進行線性化近似，忽略了二階以上統計項的信息，因而在處理非線性程度較高的模型時容易造成較大的累積誤差，最終導致算法的估計一致性較差。

Kim等人和Song等人分別將無味卡爾曼濾波和容積卡爾曼濾波器應用于FastSLAM的粒子提議分布的計算中，通過一系列特定選取的Sigma點來直接近似狀態的后驗概率密度函數，從而避免了對非線性函數的線性化操作。但是當狀態向量的維度較高時，無味卡爾曼濾波器的中心Sigma點的權值可能為負數，因而不能保證后驗協方差矩陣的正定性；容積卡爾曼濾波器用來限定容積點的球體半徑與狀態向量的維度成正比，因而出現所謂的非局部采樣問題。

另一方面，粒子重要性重采樣過程也是影響FastSLAM算法性能的重要因素之一。通常而言，采用較多的粒子數目能夠更加精確地逼近機器人位姿狀態的后驗概率分布，而較少的粒子數目能夠大幅度減少算法對計算資源的需求。因而，在FastSLAM的粒子重要性重采樣過程中，在保證算法估計精度的同時盡量提高算法的計算效率，應考慮如何選擇盡可能少的符合實際狀態后驗概率分布的粒子。

發明內容

本發明提供了一種基于改進粒子提議分布和自適應粒子重采樣的FastSLAM方法，旨在同時從采樣粒子的質量和數量兩個方面對傳統的FastSLAM算法進行改進，增強算法在估計過程中的數值穩定性和精度，提高采樣粒子的質量，同時在粒子重采樣過程中根據估計不確定度動態確定最少所需粒子個數，提高算法的計算效率。

一種基于改進粒子提議分布和自適應粒子重采樣的FastSLAM方法，包括以下步驟：

(1)將運動噪聲和測量噪聲同時增廣至機器人位姿狀態向量中，利用平方根轉換無味卡爾曼濾波器對最優粒子提議分布進行估計，并根據該粒子提議分布對機器人位姿狀態進行采樣；

(2)利用平方根容積卡爾曼濾波器更新每個粒子對應的路標特征地圖信息；