[發(fā)明專利]一種基于三維激光的定位恢復(fù)方法

有效

• 申請?zhí)枺?/td>: 201911257085.0
• 申請日: 2019-12-10
• 公開（公告）號: CN111077495B
• 公開（公告）日: 2022-02-22
• 發(fā)明（設(shè)計）人: 蔡香玉;林歡;李栗;程敏;許春山
• 申請（專利權(quán)）人: 億嘉和科技股份有限公司
• 主分類號: G01S5/16
• 分類號: G01S5/16
• 代理公司: 南京知識律師事務(wù)所 32207
• 代理人: 劉豐

本發(fā)明公開了一種基于三維激光的定位恢復(fù)方法，包括：步驟1、機器人按照原始Global地圖進行定位導航；步驟2、機器人通過激光傳感器接收新一幀三維激光點云數(shù)據(jù)；步驟3、新一幀三維激光點云數(shù)據(jù)與原始Global地圖進行實時匹配，計算機器人的位姿、原始Global地圖坐標系下的三維激光點云數(shù)據(jù)和匹配置信度作為匹配結(jié)果；步驟4、將實時匹配得到的機器人的位姿信息進行發(fā)布；實時定量保存匹配結(jié)果；步驟5、對定位導航中的置信度進行判斷，若連續(xù)幀三維激光點云數(shù)據(jù)的置信度小于設(shè)定的置信度閾值，則判定定位丟失，觸發(fā)機器人原地待命機制，啟動定位恢復(fù)操作；否則返回步驟2，即機器人接收新一幀三維激光點云數(shù)據(jù)，繼續(xù)進行定位導航。本發(fā)明能有效防止機器人因定位丟失而導致?lián)p壞，提高了機器人的自主定位導航的可靠性和安全性，增強了機器人對環(huán)境變化的適應(yīng)性。

技術(shù)領(lǐng)域

本發(fā)明涉及巡檢機器人領(lǐng)域，具體涉及一種基于三維激光的定位恢復(fù)方法。

背景技術(shù)

近年來，隨著科技的發(fā)展，巡檢機器人已經(jīng)成為諸如變電站等特殊工程場所設(shè)備巡檢的重要輔助，巡檢機器人可以利用傳感器設(shè)備感知周邊環(huán)境實現(xiàn)對高精度環(huán)境地圖的構(gòu)建并根據(jù)該地圖實現(xiàn)機器人可重復(fù)的自主導航。隨著技術(shù)的不斷發(fā)展進步，3D激光較2D激光，由于具有點云信息豐富、定位更加穩(wěn)定的優(yōu)勢，在機器人構(gòu)建地圖與自主定位導航方面得到更加廣泛的應(yīng)用。目前，國內(nèi)外已有的主流3D激光SLAM(即時定位與建圖)技術(shù)，已經(jīng)實現(xiàn)了機器人在理想場景環(huán)境中基于激光傳感器采集數(shù)據(jù)構(gòu)建地圖以及根據(jù)地圖自主定位的功能。然而，上述技術(shù)構(gòu)建的地圖是靜態(tài)的，但實際的環(huán)境可能會發(fā)生動態(tài)的改變，例如環(huán)境中隨季節(jié)變化的植物生長與凋零、人為的場景改造等因素都會造成地圖與實際環(huán)境存在較大的不吻合。圖2是現(xiàn)有主流SLAM自主定位導航的技術(shù)流程，依據(jù)之前的地圖在發(fā)生變化的場景區(qū)域中行進，機器人定位精度會受到影響，不穩(wěn)定且不可控，乃至發(fā)生定位丟失的情況，造成機器人的損壞或耗費較多人力輔助機器人，費時費力。

發(fā)明內(nèi)容

為了解決上述問題，本發(fā)明提出了一種基于三維激光的定位恢復(fù)方法，包括以下步驟：步驟1、機器人按照原始Global地圖進行定位導航；

步驟2、機器人通過激光傳感器接收新一幀三維激光點云數(shù)據(jù)；

步驟3、新一幀三維激光點云數(shù)據(jù)與原始Global地圖進行實時匹配，計算機器人的位姿、原始Global地圖坐標系下的三維激光點云數(shù)據(jù)和匹配置信度作為匹配結(jié)果；

步驟4、將實時匹配得到的機器人的位姿信息進行發(fā)布；實時定量保存匹配結(jié)果；

步驟5、對定位導航中的置信度進行判斷，若連續(xù)幀三維激光點云數(shù)據(jù)的置信度小于設(shè)定的置信度閾值，則判定定位丟失，觸發(fā)機器人原地待命機制，啟動定位恢復(fù)操作；否則返回步驟2，即機器人接收新一幀三維激光點云數(shù)據(jù)，繼續(xù)進行定位導航。

進一步地，所述步驟5中的定位恢復(fù)操作具體包括：

將匹配結(jié)果中的三維激光點云數(shù)據(jù)進行Ceres優(yōu)化匹配，得到Ceres優(yōu)化匹配結(jié)果；

將Ceres優(yōu)化匹配結(jié)果保存并形成局部子圖，生成具有位置信息的局部子圖文件、局部子圖描述文件和導航點文件；

終止當前定位導航過程，以局部子圖文件作為導航指引地圖進行定位導航，機器人回退至可信區(qū)域；

機器人終止以局部子圖文件作為導航指引地圖進行的定位導航過程，再次以原始Global地圖作為導航指引地圖繼續(xù)進行定位導航。

進一步地，所述步驟4中的實時定量保存匹配結(jié)果具體包括：

建立一個緩存池，用于數(shù)量可配置地保存匹配結(jié)果，以待定位丟失后啟用。

進一步地，所述Ceres優(yōu)化匹配具體為：