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

有效

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

1.一種基于三維激光的定位恢復方法，其特征在于，包括以下步驟：

步驟1、機器人按照原始Global地圖進行定位導航；

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

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

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

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

所述步驟5中的定位恢復操作具體包括：

將匹配結果中的三維激光點云數據進行Ceres優化匹配，得到Ceres優化匹配結果；

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

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

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

2.根據權利要求1所述的基于三維激光的定位恢復方法，其特征在于，所述步驟4中的實時定量保存匹配結果具體包括：

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

3.根據權利要求2所述的基于三維激光的定位恢復方法，其特征在于，所述Ceres優化匹配具體為：

保存下來的若干幀的點云及其相關數據，通過原始Global地圖坐標系和局部坐標系及兩個坐標系之間的轉換關系在保存下來的三維激光點云數據之間建立位姿殘差模型，運用最小二乘的方法，對每一幀三維激光點云數據的位姿進行優化處理，得到準確的點云匹配結果。

4.根據權利要求3所述的基于三維激光的定位恢復方法，其特征在于，所述置信度在0到1之間，置信度閾值為0.4。

5.根據權利要求4所述的基于三維激光的定位恢復方法，其特征在于，所述實時定量保存的匹配結果中的原始Global地圖坐標系下的三維激光點云數據為連續300幀原始Global地圖坐標系下的三維激光點云數據；步驟5中所述的連續幀三維激光點云數據為連續100幀三維激光點云數據。