[發明專利]移動機器人導航實時定位方法、系統、介質及電子設備

在審

• 申請號: 202010425218.7
• 申請日: 2020-05-19
• 公開（公告）號: CN111578958A
• 公開（公告）日: 2020-08-25
• 發明（設計）人: 李文廣;陳偉;黃金瑞;王子昂;岳敏敏
• 申請（專利權）人: 山東金惠新達智能制造科技有限公司
• 主分類號: G01C21/32
• 分類號: G01C21/32;G05D1/02;G01C21/20
• 代理公司: 濟南圣達知識產權代理有限公司 37221
• 代理人: 祖之強

1.一種移動機器人導航實時定位方法，其特征在于，包括以下步驟：

獲取機器人測得的位姿數據和激光雷達數據，構建機器人所在環境的平面地圖；

根據實時獲取的機器人位移變化、姿態角度變化以及里程計的模型參數，對當前定位粒子進行旋轉和平移處理，得到處理后的粒子集；

根據處理后的粒子集中的定位粒子包含的位姿數據、平面地圖以及當前激光雷達數據，更新處理后的粒子集中的定位粒子的權重；

以處理后的粒子集中權重最大的粒子對應的位姿數據作為位姿均值，根據原定位粒子集的方差得到位姿方差，根據得到的位姿均值和位姿方差構建新的高斯粒子集；

從新的高斯粒子集中隨機抽取粒子，更新到機器人的當前定位粒子集中，取當前粒子集所包含的粒子的均值作為機器人的當前位姿。

2.如權利要求1所述的移動機器人導航實時定位方法，其特征在于，通過機器人的位姿變化數據生成定位粒子集。

3.如權利要求1所述的移動機器人導航實時定位方法，其特征在于，采用GMapping建圖算法，根據機器人位移變化、姿態角度變化以及里程計的模型參數，構建機器人所在環境的平面地圖。

4.如權利要求1所述的移動機器人導航實時定位方法，其特征在于，新的高斯粒子集，具體為：

其中，sqrt為求平方根函數，cov為粒子集的位姿方差，pose為機器人的位姿數據，為粒子群的位姿均值。

5.如權利要求1所述的移動機器人導航實時定位方法，其特征在于，更新后的定位粒子，具體為：

其中，δ_trans代表兩點間的平移分量，δ_rot1，δ_rot2表示兩點的轉動分量。

6.如權利要求1所述的移動機器人導航實時定位方法，其特征在于，該粒子集的方差，具體為：

cov＝(1～3)*cov_cur

其中，cov_cur為當前定位粒子集的方差cov_cur＝(cov_x,cov_y,cov_θ)。

7.如權利要求1所述的移動機器人導航實時定位方法，其特征在于，基于處理后的粒子集中的定位粒子包含的位姿數據，結合當前激光雷達數據得到激光末端點數據，并與構建的平面地圖數據進行比較，根據對比結果更新處理后的粒子集中的定位粒子的權重。

8.一種移動機器人導航實時定位系統，其特征在于，包括：

地圖構建模塊，被配置為：獲取機器人測得的位姿數據和激光雷達數據，構建機器人所在環境的平面地圖；

粒子處理模塊，被配置為：根據實時獲取的機器人位移變化、姿態角度變化以及里程計的模型參數，對當前定位粒子進行旋轉和平移處理，得到處理后的粒子集；

權重計算模塊，被配置為：根據處理后的粒子集中的定位粒子包含的位姿數據、平面地圖以及當前激光雷達數據，更新處理后的粒子集中的定位粒子的權重；

新粒子集構建模塊，被配置為：以處理后的粒子集中權重最大的粒子對應的位姿數據作為位姿均值，根據原定位粒子集的方差得到位姿方差，根據得到的位姿均值和位姿方差構建新的高斯粒子集；

定位模塊，被配置為：從新的高斯粒子集中隨機抽取粒子，更新到機器人的當前定位粒子集中，取當前粒子集所包含的粒子的均值作為機器人的當前位姿。

9.一種介質，其上存儲有程序，其特征在于，該程序被處理器執行時實現如權利要求1-7任一項所述的移動機器人導航實時定位方法中的步驟。

10.一種電子設備，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的程序，其特征在于，所述處理器執行所述程序時實現如權利要求1-7任一項所述的移動機器人導航實時定位方法中的步驟。