[發(fā)明專利]一種基于幾何先驗信息下的室內(nèi)機器人快速重定位的方法

有效

• 申請?zhí)枺?/td>: 201910301095.3
• 申請日: 2019-04-15
• 公開（公告）號: CN110006432B
• 公開（公告）日: 2021-02-02
• 發(fā)明（設(shè)計）人: 柏林;于泠汰;劉彪
• 申請（專利權(quán)）人: 廣州高新興機器人有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 廣州國鵬知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 44511
• 代理人: 何志軍

1.一種基于幾何先驗信息下的室內(nèi)機器人快速重定位的方法，其特征在于，包括如下步驟：

(1)提取環(huán)境幾何特征，通過激光雷達(dá)對環(huán)境信息進(jìn)行感知，按照一定的采樣頻率將離散化的點云數(shù)據(jù)信息收集起來；

(2)環(huán)境結(jié)構(gòu)化信息分塊，以測量開始的第一個點為待處理的第一個點，依次計算順序測得的兩個點之間的距離并根據(jù)距離值進(jìn)行區(qū)域分塊，直至處理到周期測量的最后一個點；

(3)基于幾何先驗信息進(jìn)行重定位，將分塊后的環(huán)境結(jié)構(gòu)化信息與幾何地圖進(jìn)行匹配，獲得粗匹配點，基于粗匹配點和點云數(shù)據(jù)進(jìn)行重定位；

在所述步驟(2)中，進(jìn)一步包括如下步驟：

(a)計算兩個連續(xù)測量點之間的距離；

(b)計算分塊，設(shè)定動態(tài)閾值，如果上述兩個連續(xù)測量點之間的距離大于所述動態(tài)閾值，則認(rèn)定這兩個點是不連續(xù)的，以這個點為分割點，將這個點分割成和前一個點所在區(qū)域不同的區(qū)域；

(c)判斷每個區(qū)域內(nèi)的測量點的個數(shù)是否少于預(yù)設(shè)值，如果是，將該區(qū)域的測量點放棄；

(d)判斷所有的區(qū)域塊中的第一個點與前一個區(qū)域塊中的最后一個點的距離是否小于所述閾值,如果是，則將后一個區(qū)域塊和前一個區(qū)域塊進(jìn)行合并；

(e)預(yù)先設(shè)定一個最大偏離閾值，并基于最小二乘擬合和所述閾值對步驟(d)中區(qū)域塊繼續(xù)分割，形成一個統(tǒng)計線性區(qū)域M；

(f)對步驟(e)中獲得的區(qū)域M采用最小二乘法擬合為直線；

在所述步驟(a)中，計算兩個連續(xù)測量點之間的距離的公式為：

其中j_max為激光雷達(dá)在圓周360°內(nèi)的最多可見點數(shù)；

在所述步驟(e)中，進(jìn)一步包括，采用最小二乘擬合算法對區(qū)域塊繼續(xù)分割，預(yù)先設(shè)定一個最大偏離值的閾值，步驟如下：

S1：在區(qū)域Qi中，j＝區(qū)域中第一個點Ps，k＝區(qū)域中最后一個點Pc，其中i為區(qū)域的編號；

S2：經(jīng)過j點和k點做一條直線Lj,k；

S3：從j+1點開始尋找，判斷是否有到直線Lj,k的距離大于閾值的點；若是，k＝距離大于閾值且到直線距離最大的點，返回步驟S2；否則進(jìn)入步驟S4；

S4：j到k的點形成一個統(tǒng)計線性區(qū)域M；

S5：判斷j是否為Pc；若否，j＝k+1為新的起點，k＝Pc，返回步驟S2；若是，流程結(jié)束。

2.根據(jù)權(quán)利要求1所述的基于幾何先驗信息下的室內(nèi)機器人快速重定位的方法，其特征在于，所述激光雷達(dá)安裝在機器人上。

3.根據(jù)權(quán)利要求1所述的基于幾何先驗信息下的室內(nèi)機器人快速重定位的方法，其特征在于，將上述擬合為直線的區(qū)域幾何結(jié)構(gòu)信息保存在機器人存儲器中。

4.根據(jù)權(quán)利要求3所述的基于幾何先驗信息下的室內(nèi)機器人快速重定位的方法，其特征在于，機器人不斷重復(fù)執(zhí)行上述步驟(1)-(2)，并根據(jù)SLAM模塊給出的定位合并重復(fù)的地圖信息且添加新的地圖信息，最終可以形成一個包含著幾何結(jié)構(gòu)信息的幾何特征地圖。

5.根據(jù)權(quán)利要求1所述的基于幾何先驗信息下的室內(nèi)機器人快速重定位的方法，其特征在于，在所述步驟(3)中，進(jìn)一步包括步驟：

(3.1)機器人到達(dá)一個新的環(huán)境中，先旋轉(zhuǎn)一周，獲取周圍的所述分塊后的環(huán)境結(jié)構(gòu)信息，并且采用區(qū)域分割的方式將其歸一化到多條直線的狀態(tài)；

(3.2)將機器人周圍的所述分塊后的環(huán)境結(jié)構(gòu)化信息與幾何地圖進(jìn)行匹配，獲得粗匹配點；

(3.3)將所述的點云數(shù)據(jù)散落在粗匹配點之上，利用粒子濾波來進(jìn)行重定位。

6.根據(jù)權(quán)利要求5所述的基于幾何先驗信息下的室內(nèi)機器人快速重定位的方法，其特征在于，在步驟(3.2)中，所述的匹配為非精確匹配。

7.一種非易失性存儲介質(zhì)，其特征在于，包括一條或多條計算機指令，所述一條或多條計算機指令在執(zhí)行時實現(xiàn)上述權(quán)利要求1-6任一項所述的方法。