[發明專利]基于螢火蟲優化粒子濾波的機器人定位與建圖方法及裝置

有效

• 申請號: 201910473312.7
• 申請日: 2019-05-31
• 公開（公告）號: CN110263905B
• 公開（公告）日: 2021-03-02
• 發明（設計）人: 崔昊楊;張宇;劉昊;秦倫明;楊強強;卞正蘭;唐忠
• 申請（專利權）人: 上海電力學院
• 主分類號: G06N3/00
• 分類號: G06N3/00;G06T7/70
• 代理公司: 上海科盛知識產權代理有限公司 31225
• 代理人: 蔡彭君

本發明涉及一種基于螢火蟲優化粒子濾波的機器人定位與建圖方法及裝置，其中方法包括：步驟S1：獲得采樣初始時刻粒子；步驟S2：通過機器人運動模型和觀測模型得到t時刻的目標值，并通過重要性采樣得到t時刻的粒子集合；步驟S3：利用改進后的螢火蟲算法優化采樣粒子的位置，分別得到所有粒子t+1時刻優化后的位置；步驟S4：計算優化后的粒子權重并進行歸一化，篩選有效粒子，并基于有效粒子進行重采樣，直至粒子總數達到設定數目；步驟S5：根據步驟S4得到的粒子群進行機器人位姿與環境估計。與現有技術相比，本發明解決了固定步長因子導致的收斂速度慢、在極值點附近易“振蕩”等問題，提高了粒子全局尋優和局部尋優的精度與效率。

技術領域

本發明涉及智能移動設備環境感知與自動化作業領域，尤其是涉及一種基于螢火蟲優化粒子濾波的機器人定位與建圖方法及裝置。

背景技術

智能移動巡檢設備中的同時定位和地圖創建(Simultaneous Localization AndMapping,SLAM)技術是實現電力設備狀態信息智能感知與自動化作業的核心問題，SLAM既是移動機器人自主移動的前提，又是后續路徑規劃與運動控制的基礎，其難點在于如何通過自身攜帶的傳感器，采用濾波方法實現未知環境地圖構建和精準定位。

標準粒子濾波(Particle Filter,PF)在重要性采樣時遵循次優選擇原則，會導致樣本有效性的遺失，引起樣本貧化現象；同時標準PF是以犧牲小權值粒子為代價來進行重采樣，經過多次循環迭代會導致粒子退化，降低了采樣粒子的多樣性，導致定位預測精度難以滿足高性能機器人精確定位的要求。

發明內容

本發明的目的就是為了克服上述現有技術存在的缺陷而提供一種基于螢火蟲優化粒子濾波的機器人定位與建圖方法及裝置。

本發明的目的可以通過以下技術方案來實現：

一種基于螢火蟲優化粒子濾波的機器人定位與建圖方法，包括：

步驟S1：獲得采樣初始時刻粒子；

步驟S2：通過機器人運動模型和觀測模型得到t時刻的目標值，并通過重要性采樣得到t時刻的粒子集合；

步驟S3：利用改進后的螢火蟲算法優化采樣粒子的位置，分別得到所有粒子t+1時刻優化后的位置；

步驟S4：計算優化后的粒子權重并進行歸一化，篩選有效粒子，并基于有效粒子進行重采樣，直至粒子總數達到設定數目；

步驟S5：根據步驟S4得到的粒子群進行機器人位姿與環境估計。

所述步驟S3具體包括：

步驟S31：根據重定義后的相對亮度公式計算t+1時刻粒子i、j的相對亮度：

其中：I為相對亮度，pdist(·)_euc為歐氏距離，為第i粒子的預測觀測值，為第i粒子的最新觀測值，為第i粒子的預測狀態值，為第i粒子的最新狀態值；

步驟S32：計算粒子t+1時刻粒子i、j的歐式距離；

步驟S33：根據改進后的位置方程來更新粒子i的位置：

其中：x_i'為更新后的粒子i所在的空間位置，x_i為粒子i所在的空間位置，β₀為螢火蟲最大吸引度，γ為光強吸收系數，r_ij為粒子i、j的空間距離，x_j為粒子j所在的空間位置，R_t為觀測過程的噪聲值，I(i)為粒子i相對亮度，I(j)為粒子j相對亮度，rand為服從均值為0、方差為1的均勻分布隨機數。

所述步驟S4具體包括：