[發明專利]一種融合反光柱及柵格地圖的定位方法及裝置

有效

• 申請號: 202011230450.1
• 申請日: 2020-11-06
• 公開（公告）號: CN112344940B
• 公開（公告）日: 2022-05-17
• 發明（設計）人: 趙成偉;胡佳琪;馮艷曉;靳興來;朱世強;裴翔;王國成
• 申請（專利權）人: 杭州國辰機器人科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 蘇州中合知識產權代理事務所(普通合伙) 32266
• 代理人: 阮梅

本發明公開一種融合反光柱及柵格地圖的定位方法，包括如下步驟：獲取當前幀中的反光柱和對應的里程計位姿數據；檢測局部概率柵格地圖是否初始化，若為否，則以當前幀反光柱和對應的里程計位姿數據，更新局部概率柵格地圖；若為是，則以當前幀反光柱和對應的里程計位姿數據為初值，通過局部概率柵格地圖跟蹤，得到更準確的約束關系，通過約束關系優化待定位對象的位姿；獲取上一幀反光柱數據和當前幀反光柱數據的對應關系，通過局部概率柵格地圖跟蹤并利用最小二乘方法對待定位對象的位姿進行尋優；以尋優后待定位對象的位姿以及當前幀的反光柱數據，更新柵格地圖。本發明能夠實時連續的反饋位姿信息，保證建圖和定位的穩定性。

技術領域

本發明涉及定位技術領域，尤其涉及一種融合反光柱及柵格地圖的定位方法及裝置。

背景技術

基于激光的定位方式分為基于反光柱的定位方式和基于輪廓的定位方式；基于輪廓的定位方式計算量大，受環境影響嚴重，相對精度較差，無法適應移動機器人精密操作的要求；基于反光柱的定位方式計算量小，定位精度高、穩定性好。基于反光柱的定位方式關鍵是反光柱匹配，國內外現有的反光柱定位技術，均采用了幾何三角定位原理；通過檢測到的反光柱與反光柱地圖幾何匹配來獲取激光位姿，此種方案利用視野內反光柱形狀特征的唯一性，來保證重復定位的精度，且定位過程中需要保證環境部署中，反光柱不能出現對稱分布及局部分布相似，這種需求對反光柱部署有著較高的要求，特別是在大場景下，實施難度很大；且在實際應用中，由于存在各種復雜運動及復雜地面環境，很難保證基于幾何三角定位的定位位置的連續性和穩定性。

發明內容

為了解決上述技術問題，本發明提出一種融合反光柱及柵格地圖的定位方法及裝置。

為了達到上述目的，本發明的技術方案如下：

一種融合反光柱及柵格地圖的定位方法，包括如下步驟：

獲取當前幀中的反光柱數據和對應的里程計位姿數據；

檢測局部概率柵格地圖是否初始化，若檢測結果為否，則以當前幀中反光柱數據和對應的里程計位姿數據，更新局部概率柵格地圖；若檢測結果為是，則以當前幀中反光柱數據和當前里程計位姿數據為初值，通過局部概率柵格地圖跟蹤，得到更準確的約束關系，通過所述約束關系優化待定位對象的位姿；

獲取上一幀反光柱數據和當前幀反光柱數據的對應關系，通過局部概率柵格地圖跟蹤并利用最小二乘方法對待定位對象的位姿進行尋優，獲取尋優后待定位對象的位姿；

以尋優后待定位對象的位姿以及當前幀的反光柱數據，更新柵格地圖。

優選地，所述獲取當前幀中的反光柱數據和對應的里程計位姿數據，具體包括如下步驟：

基于反光柱反射強度，提取當前幀數據中的反光柱數據；

以當前激光雷達中心為原點，計算當前幀數據的反光柱位置。

優選地，所述獲取當前幀中的反光柱數據和對應的里程計位姿數據之后，還包括如下步驟：

檢測當前幀數據中提取的反光柱數目，若檢測到的相同的反光柱數目小于3個，則等待下一幀數據；若檢測到的相同反光柱數目不小于3個，則進入下一步，檢測局部概率柵格地圖是否初始化。

優選地，通過所述約束關系優化待定位對象的位姿，是采用極大似然估計計算得到當前時刻待定位對象的位姿。

優選地，所述采用極大似然估計計算得到當前時刻待定位對象的位姿，采用如下公式獲得：

式中，M^ref為局部反光柱地圖數據，p_i為當前幀中的反光柱數據，符號代表旋轉平移變換；符號||{M^ref,.}代表點到局部反光柱數據的歐氏距離，P為計算過程中的位姿。