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

在審

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

1.一種實現基于粒子濾波SLAM算法的方法，其特征在于，包括：

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

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

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

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

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

2.根據權利要求1所述的實現基于粒子濾波SLAM算法的方法，其特征在于，所述原始地圖包括多個原始粒子數據組，所述根據所述第一權重值對所述原始地圖進行粒子濾波，得到多個更新粒子數據組包括：

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

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

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

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

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

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

3.根據權利要求2所述的實現基于粒子濾波SLAM算法的方法，其特征在于，所述多個原始粒子數據組包括所述多個原始位姿信息和原始激光雷達數據。

4.根據權利要求2所述的實現基于粒子濾波SLAM算法的方法，其特征在于，所述預設次數是根據淘汰的原始粒子數據組的個數進行設置。

5.根據權利要求1所述的實現基于粒子濾波SLAM算法的方法，其特征在于，所述方法還包括：

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

6.一種實現基于粒子濾波SLAM算法的系統，其特征在于，包括：數據接收模塊、運動估計模塊、匹配優化模塊、粒子濾波模塊和更新地圖模塊；

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

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

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

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

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