[發明專利]基于粒子自收斂的移動機器人重定位方法

有效

• 申請號: 201911403712.7
• 申請日: 2019-12-31
• 公開（公告）號: CN111061287B
• 公開（公告）日: 2022-05-27
• 發明（設計）人: 陳智君;伍永健;郝奇;曹雛清;高云峰
• 申請（專利權）人: 蕪湖哈特機器人產業技術研究院有限公司
• 主分類號: G05D1/08
• 分類號: G05D1/08
• 代理公司: 蕪湖安匯知識產權代理有限公司 34107
• 代理人: 鐘雪

1.一種基于粒子自收斂的移動機器人重定位方法，其特征在于，所述方法具體包括如下步驟：

S1、基于柵格地圖生成格網粒子集合set_a，格網粒子集合set_a由格網粒子組成，格網粒子即為各格網點p_i(x_i,y_i)的最優位姿；

S2、更新格網粒子集合set_a中的粒子及對應的打分；

S3、對格網粒子集合set_a中的粒子進行重采樣，將重采樣的粒子復制至新的粒子集合set_b，粒子集合set_b中的粒子權重相等；

S4、計算粒子集合set_b中所有粒子的位姿標準差，檢測是否所有粒子的位姿標準差都小于預設的閾值，若檢測結果為 是，將粒子集合中set_b所有粒子的平均位姿作為機器人的當前重定位的位姿，若檢測結果為否，則執行步驟S2；

格網點p_i(x_i,y_i)最優位姿的獲取方法具體如下：

從0度到360度，每個設定的角度步進Δθ取一個角度θ_j，將角度θ_j與格網點p_i(x_i,y_i)組成一個位姿p_i,j(x_i,y_i,θ_j)；

利用似然域模型對位姿p_i,j(x_i,y_i,θ_j)進行打分，遍歷格網點p_i(x_i,y_i)所有的角度，最高打分位姿即為格網點p_i(x_i,y_i)的最優位姿；

格網粒子集合set_a中的粒子更新方法具體包括如下步驟：

S21、格網粒子集合set_a中順序選擇一個粒子p_i(x_i,y_i,θ_i)，并在粒子p_i(x_i,y_i,θ_i)周圍生成k個高斯隨機位姿；

S22、利用似然域模型計算粒子p_i(x_i,y_i,θ_i)及k個隨機位姿集合的得分，得到最高得分w_max和對應的位姿p_max，令當前位姿等于得分最高的位姿p_i＝p_max，當前位姿的權重等于最高得分，即w_i＝w_max；

S23、遍歷粒子格網粒子集合set_a中所有的粒子，完成格網粒子集合set_a中的更新與打分。

2.如權利要求1所述基于粒子自收斂的移動機器人重定位方法，其特征在于，格網粒子集合set_a的生成方法具體包括如下步驟：

S11、在柵格地圖上每隔設定距離Δd取一個格網點p_i(x_i,y_i)；

S12、檢測取格網點所在的柵格是否被障礙物占據；

S13、若檢測結果為否，則獲取格網點p_i(x_i,y_i)的最優位姿，若檢測結果為是，則基于設定距離Δd在柵格地圖上取下一個格網點，并執行步驟S12；

S14、遍歷柵格地圖上的所有格網點，輸出格網粒子集合set_a。

3.如權利要求1所述基于粒子自收斂的移動機器人重定位方法，其特征在于，粒子重采樣過程具體如下：

S31、對格網粒子集合中的粒子權重進行降序排列；

S32、基于粒子復制比率計算待復制粒子的數量copy_count，設定粒子復制比率為r，當前格網粒子集合中的粒子數set_count，計算得到待復制粒子的數copy_count＝r*set_count；

S33、構建空的粒子集合set_b，將排名靠前的copy_count個粒子復制到粒子集合set_b，并將粒子集合set_b中的所有粒子權重設為1/copy_count；

S34、將粒子集合set_b替代格網粒子集合set_a。