[發明專利]實現基于粒子濾波SLAM算法的方法及系統

在審

• 申請號: 201611128122.4
• 申請日: 2016-12-08
• 公開（公告）號: CN106597435A
• 公開（公告）日: 2017-04-26
• 發明（設計）人: 劉雪楠;沈剛
• 申請（專利權）人: 北京康力優藍機器人科技有限公司
• 主分類號: G01S13/89
• 分類號: G01S13/89
• 代理公司: 北京超凡志成知識產權代理事務所(普通合伙)11371
• 代理人: 張海洋

技術領域

本發明涉及數字圖象處理技術領域，尤其是涉及實現基于粒子濾波SLAM算法的方法及系統。

背景技術

基于粒子濾波的SLAM(Simultaneous Localization And Mapping，即時定位與地圖構建)是一種強調實時性與準確性的算法，其在匹配優化等方面運算量很大，現行的基于軟件的SLAM方案對硬件配置的要求很高。現行的SLAM方案一般只能順序執行。例如，對于一個多粒子的SLAM算法來說，軟件的處理方式就要依據粒子數量進行同等數量的循環處理。而一次的處理就需要經過運動位姿的匹配優化、將點插入地圖，權重的計算等一系列處理，而循環多次其處理時間將大大增加。如果使用高性能的CPU當然可以解決這個問題，但是硬件成本也一樣提高了。對于一個強調實時性的多傳感器系統來說，處理時間十分重要。

發明內容

有鑒于此，本發明的目的在于提供實現基于粒子濾波SLAM算法的方法及系統，以降低成本和提高數據處理效率。

第一方面，本發明實施例提供了一種實現基于粒子濾波SLAM算法的方法，包括：

獲取激光雷達數據和里程計數據；

根據原始地圖的多個原始位姿信息，對所述里程計數據進行運動估計，同時得到多個第一位姿信息；

根據所述原始地圖，對所述多個第一位姿信息同時進行匹配優化，得到多個第二位姿信息，并根據所述多個第二位姿信息與所述激光雷達數據計算得到第一權重值；

根據所述第一權重值對所述原始地圖進行粒子濾波，得到多個更新粒子數據組；

根據所述多個更新粒子數據組，對所述原始地圖進行更新，生成更新地圖。

結合第一方面，本發明實施例提供了第一方面的第一種可能的實施方式，其中，所述原始地圖包括多個原始粒子數據組，所述根據所述第一權重值對所述原始地圖進行粒子濾波，得到多個更新粒子數據組包括：

根據所述第一權重值設置第一閾值和第二閾值；

判斷每個所述原始粒子數據組是否滿足預設條件，其中，預設條件包括第一預設條件、第二預設條件或第三預設條件；

如果每個所述原始粒子數據組滿足所述第一預設條件，則將滿足所述第一預設條件的原始粒子數據組進行淘汰，其中，所述第一預設條件為每個所述原始粒子數據組對應的第二權重值小于所述第一閾值；

如果每個所述原始粒子數據組滿足所述第二預設條件，則將滿足所述第二預設條件的原始粒子數據組按照預設的次數進行復制，其中，所述第二預設條件為每個所述原始粒子數據組對應的第二權重值大于所述第二閾值；

如果每個所述原始粒子數據組滿足所述第三預設條件，則將滿足所述第三預設條件的原始粒子數據組進行保留，其中，所述第三預設條件為每個所述原始粒子數據組對應的第二權重值位于所述第一閾值與第二閾值之間；

將復制的所述原始粒子數據組和保留的所述原始粒子數據組構成所述多個更新粒子數據組。

結合第一方面的第一種可能的實施方式，本發明實施例提供了第一方面的第二種可能的實施方式，其中，所述多個原始粒子數據組包括所述多個原始位姿信息和原始激光雷達數據。

結合第一方面的第一種可能的實施方式，本發明實施例提供了第一方面的第三種可能的實施方式，其中，所述預設次數是根據淘汰的原始粒子數據組的個數進行設置。

結合第一方面，本發明實施例提供了第一方面的第四種可能的實施方式，其中，所述方法還包括：

如果不存在所述原始地圖，則直接根據所述激光雷達數據和所述里程計數據生成地圖。

第二方面，本發明實施例還提供一種實現基于粒子濾波SLAM算法的系統，包括：數據接收模塊、運動估計模塊、匹配優化模塊、粒子濾波模塊和更新地圖模塊；

所述數據接收模塊，用于獲取激光雷達數據和里程計數據，并將所述里程計數據發送給所述運動估計模塊，將所述激光雷達數據發送給所述匹配優化模塊；

所述運動估計模塊，用于根據原始地圖的多個原始位姿信息，對所述里程計數據進行運動估計，同時得到多個第一位姿信息；

所述匹配優化模塊，用于根據所述原始地圖，對所述多個第一位姿信息同時進行匹配優化，得到多個第二位姿信息，并根據所述多個第二位姿信息與所述激光雷達數據計算得到第一權重值；

所述粒子濾波模塊，用于根據所述第一權重值對所述原始地圖進行粒子濾波，得到多個更新粒子數據組；

所述更新地圖模塊，用于根據所述多個更新粒子數據組，對所述原始地圖進行更新，生成更新地圖。