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

有效

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

1.一種融合反光柱及柵格地圖的定位方法，其特征在于，包括如下步驟：

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

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

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

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

2.根據權利要求1所述的一種融合反光柱及柵格地圖的定位方法，其特征在于，所述獲取當前幀中的反光柱數據和對應的里程計位姿數據，具體包括如下步驟：

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

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

3.根據權利要求1所述的一種融合反光柱及柵格地圖的定位方法，其特征在于，所述獲取當前幀中的反光柱數據和對應的里程計位姿數據之后，還包括如下步驟：

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

4.根據權利要求1所述的一種融合反光柱及柵格地圖的定位方法，其特征在于，通過所述約束關系優化待定位對象的位姿，是采用極大似然估計計算得到當前時刻待定位對象的位姿。

5.根據權利要求4所述的一種融合反光柱及柵格地圖的定位方法，其特征在于，所述采用極大似然估計計算得到當前時刻待定位對象的位姿，采用如下公式獲得：

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

6.根據權利要求1所述的一種融合反光柱及柵格地圖的定位方法，其特征在于，所述通過局部概率柵格地圖跟蹤并利用最小二乘方法對待定位對象的位姿進行尋優，包括如下步驟：

檢測上一幀中反光柱數據和當前幀中反光柱數據中相同反光柱的情況，若檢測到的相同反光柱數目小于3個，則等待下一幀數據；若檢測到的相同反光柱數目不小于3個，則利用最小二乘方法對前后兩幀數據的待定位對象的運動量進行尋優，以尋優后的運動量進一步優化當前待定位對象的位姿。

7.根據權利要求6所述的一種融合反光柱及柵格地圖的定位方法，其特征在于，所述檢測上一幀中反光柱數據和當前幀中反光柱數據中相同反光柱的情況，包括如下步驟：

獲取上一幀中反光柱數據生成的概率柵格地圖；

根據當前幀待定位對象的位姿，計算當前幀數據中反光柱在概率柵格地圖上的概率；

判斷概率是否低于預設閾值，若是，則當前幀中反光柱與上一幀中反光柱數據為不相同觀測數據；若否，且當前幀中反光柱數據離當前激光雷達中心位置及上一幀中激光雷達中心的位置、距離和夾角均滿足預設閾值條件，則當前幀中反光柱與上一幀中反光柱數據為相同觀測數據。

8.根據權利要求7所述的一種融合反光柱及柵格地圖的定位方法，其特征在于，還包括如下步驟：當當前幀定位位姿與柵格地圖第一幀數據的相對運動量達到預設閾值時，重置柵格地圖，所述重置柵格地圖包括重置柵格地圖的概率值、重置地圖原點和更新局部概率柵格地圖。