[發(fā)明專利]室內動態(tài)環(huán)境下基于激光雷達的定位方法及計算機存儲介質

有效

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

1.一種室內動態(tài)環(huán)境下基于激光雷達的定位方法，其特征在于，所述方法包括以下步驟：

S1、給定機器人初始位姿，并對機器人初始位姿進行采樣生成多個初始粒子；

S2、根據(jù)運動模型更新粒子狀態(tài)；

S3、采用激光雷達進行數(shù)據(jù)采樣，獲得點云數(shù)據(jù)；

S4、根據(jù)點云數(shù)據(jù)更新粒子權重；

S5、根據(jù)粒子權重對粒子進行重采樣；

S6、更新機器人位姿；

S7、判斷粒子集是否收斂，若未收斂，則跳轉至步驟S2，若收斂，則進入步驟S8；

S8、對步驟S6更新后的機器人位姿進行二次重采樣；

S9、跳轉至步驟S2。

2.根據(jù)權利要求1所述的方法，其特征在于，在步驟S1中，給定機器人初始位姿x_t，基于高斯分布對x_t進行采樣生成N個初始粒子，記為：

其中，粒子權重初始化為

3.根據(jù)權利要求2所述的方法，其特征在于，在步驟S2中，根據(jù)運動模型更新粒子狀態(tài)，記為

其中，u_t為運動控制量，v_t為噪聲。

4.根據(jù)權利要求3所述的方法，其特征在于，步驟S3包括：

S31、將所述激光雷達的數(shù)據(jù)按掃描角度分塊，每20度為一塊區(qū)域，最后不足以構成一塊區(qū)域的激光雷達數(shù)據(jù)與前一組數(shù)據(jù)合并為一組；

S32、提取每塊區(qū)域的激光雷達數(shù)據(jù)中距離最大的5個數(shù)據(jù)作為點云數(shù)據(jù)。

5.根據(jù)權利要求4所述的方法，其特征在于，在步驟S4中，根據(jù)步驟S3獲得的點云數(shù)據(jù)，依據(jù)各個粒子的位姿，將點云數(shù)據(jù)映射到地圖并計算點云與地圖的匹配值，

點云匹配評價函數(shù)為：

其中，σ＝0.1，d為點云在地圖中與障礙點最近的距離，

記點云數(shù)據(jù)為m個，在地圖中第j個點云離障礙點最近距離記為d_j，則第i個粒子的點云匹配值為

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

歸一化權重，則

6.根據(jù)權利要求5所述的方法，其特征在于，在步驟S5中，根據(jù)粒子權重對粒子進行重采樣，并重新設置權重為

7.根據(jù)權利要求6所述的方法，其特征在于，在步驟S6中，更新機器人位姿，記為：

t＝t+1。

8.根據(jù)權利要求7所述的方法，其特征在于，在步驟S8中，基于高斯分布對機器人位姿進行二次重采樣以更新粒子集，即X_t～N(x_t,Σ₁)，其中，X_t為更新后的粒子集，x_t為步驟S6更新后的機器人位姿，Σ₁為采樣協(xié)方差。

9.根據(jù)權利要求8所述的方法，其特征在于，在步驟S8中，根據(jù)更新后的粒子集，計算位姿x_t的點云匹配值，記為s1，若s10.8，則采樣協(xié)方差為：

否則采樣協(xié)方差為：

設置粒子權重為

10.一種計算機存儲介質，其特征在于，包括一條或多條計算機指令，所述一條或多條計算機指令在執(zhí)行時實現(xiàn)如權利要求1-9中任一項所述的方法。