[發明專利]室內動態環境下基于激光雷達的定位方法及計算機存儲介質

有效

• 申請號: 201910048913.3
• 申請日: 2019-01-18
• 公開（公告）號: CN109633666B
• 公開（公告）日: 2021-02-02
• 發明（設計）人: 劉彪;李振;宿凱
• 申請（專利權）人: 廣州高新興機器人有限公司
• 主分類號: G01S17/06
• 分類號: G01S17/06;G01S17/50
• 代理公司: 廣州國鵬知識產權代理事務所(普通合伙) 44511
• 代理人: 寧尚國

本發明提供一種室內動態環境下基于激光雷達的定位方法及計算機存儲介質，所述方法包括以下步驟：S1、給定機器人初始位姿，并對機器人初始位姿進行采樣生成多個初始粒子；S2、根據運動模型更新粒子狀態；S3、采用激光雷達進行數據采樣，獲得點云數據；S4、根據點云數據更新粒子權重；S5、根據粒子權重對粒子進行重采樣；S6、更新機器人位姿；S7、判斷粒子集是否收斂，若未收斂，則跳轉至步驟S2，若收斂，則進入步驟S8；S8、對機器人位姿進行二次重采樣；S9、跳轉至步驟S2。根據本發明實施例的定位方法，通過對基于粒子濾波的定位方法進行改進，使其適用于室內的動態環境。

技術領域

本發明涉及激光定位領域，更具體地，涉及一種室內動態環境下基于激光雷達的定位方法及計算機存儲介質。

背景技術

目前，基于粒子濾波的定位方法通常是通過運動模型對機器人位姿進行采樣生成大量粒子，依據傳感器的觀測結果更新粒子的權重并進行重采樣，最終粒子會收斂至正確位姿。但是傳統的基于粒子濾波的定位方法假設機器人是在一個靜態環境下運動，因此應用在商場、醫院展會等人流較大的動態環境下易發生定位丟失現象。

發明內容

有鑒于此，本發明提供一種室內動態環境下基于激光雷達的定位方法及計算機存儲介質，定位效果好，應用范圍廣。

為解決上述技術問題，一方面，本發明提供一種室內動態環境下基于激光雷達的定位方法，所述方法包括以下步驟：S1、給定機器人初始位姿，并對機器人初始位姿進行采樣生成多個初始粒子；S2、根據運動模型更新粒子狀態；S3、采用激光雷達進行數據采樣，獲得點云數據；S4、根據點云數據更新粒子權重；S5、根據粒子權重對粒子進行重采樣；S6、更新機器人位姿；S7、判斷粒子集是否收斂，若未收斂，則跳轉至步驟S2，若收斂，則進入步驟S8；S8、對機器人位姿進行二次重采樣；S9、跳轉至步驟S2。

根據本發明的一些實施例，給定機器人初始位姿x_t，基于高斯分布對x_t進行采樣生成N個初始粒子，記為：其中，粒子權重初始化為

根據本發明的一些實施例，在步驟S2中，根據運動模型更新粒子狀態，記為其中，u_t為運動控制量，v_t為噪聲。

根據本發明的一些實施例，步驟S3包括：S31、將所述激光雷達的數據按掃描角度分塊，每20度為一塊區域，最后不足以構成一塊區域的激光雷達數據與前一組數據合并為一組；S32、提取每塊區域的激光雷達數據中距離最大的5個數據作為點云數據。

根據本發明的一些實施例，在步驟S4中，根據步驟S3獲得的點云數據，依據各個粒子的位姿，將點云數據映射到地圖并計算點云與地圖的匹配值，點云匹配評價函數為：其中，σ＝0.1，d為點云在地圖中與障礙點最近的距離，記點云數據為m個，在地圖中第j個點云離障礙點最近距離記為d_j，則第i個粒子的點云匹配值為

取點云的匹配值為粒子的權重，則

歸一化權重，則

根據本發明的一些實施例，在步驟S5中，根據粒子權重對粒子進行重采樣，并重新設置權重為

根據本發明的一些實施例，在步驟S6中，更新機器人位姿，記為：

t＝t+1。