[發明專利]一種激光建圖定位方法、裝置、存儲介質及電子設備

有效

• 申請號: 202010965430.2
• 申請日: 2020-09-14
• 公開（公告）號: CN112068154B
• 公開（公告）日: 2022-12-13
• 發明（設計）人: 尹靖淳;王運志
• 申請（專利權）人: 中科院軟件研究所南京軟件技術研究院
• 主分類號: G01S17/89
• 分類號: G01S17/89;G01C21/20
• 代理公司: 北京三聚陽光知識產權代理有限公司 11250
• 代理人: 李靜玉

本發明公開了一種激光建圖定位方法、裝置、存儲介質及電子設備，包括采用占據柵格地圖來進行環境表示；運動跟蹤線程中相鄰兩幀激光雷達進行掃描匹配，計算優化的幀間位姿運動矩陣；當前幀激光雷達數據與占據柵格地圖匹配，調整幀間位姿運動矩陣。建圖線程中由新插入的關鍵幀和關鍵幀隊列構建共視幀約束，檢測到閉環時，構建閉環幀約束，采用增量式優化位姿因子圖更新關鍵幀隊列位姿，實現全局地圖的更新。本發明通過兩次匹配，確保機器人運動過程中局部建圖準確可靠；對于原始位姿圖非線性最小二乘優化問題的求解，將圖式結構轉換為樹狀結構，并將非線性優化問題的求解轉換為最新幀相關的共視幀的增量式求解，大大提高計算效率，減小資源的耗費。

技術領域

本發明涉及定位和建圖技術領域，具體涉及一種激光建圖定位方法、裝置、存儲介質及電子設備。

背景技術

激光SLAM(simultaneous localization and mapping，即時定位與地圖構建)一般是指裝備平面單線激光雷達的輪式移動機器人，通過運動來使用激光雷達掃描周圍室內環境進行建圖，同時在此地圖中估算機器人位姿的技術。所使用的傳感器除了激光雷達，還可以使用輪速里程計，慣導陀螺儀等。現有方案一般通過激光掃描匹配來計算幀間位姿變換，再通過粒子濾波進行狀態更新迭代，或通過位姿圖優化來消除大范圍建圖過程中的積累誤差。

粒子濾波方法采用一系列的粒子來表示機器人的位姿和地圖信息，通過里程計信息作為運動先驗和掃描匹配激光到地圖上的匹配度來推算概率分布函數(proposaldistribution)。同時對粒子根據激光匹配度來進行權重系數的評估，再根據這些評估權重系統的大小，來對于粒子進行篩選，確保雷達數據與地圖匹配好的粒子能夠在多次迭代篩選中大概率的存活下來，從而完成建圖。該方法不足之處在于無法消除建圖過程中的累計誤差，且通過激光掃描匹配強依賴于里程計信息。一旦里程計信息失效，如出現輪子打滑，空轉，胎壓不穩，或者機器人被意外推動，挪移，就會出現建圖花掉的情形。

位姿圖優化方法在局部通過掃描匹配來計算幀間位姿，使用的是相關度匹配和基于高斯牛頓的最小化地圖灰度差的方法；全局通過優化整個位姿圖來優化整體軌跡，閉環檢測使用分支定界加速的相關度匹配和基于高斯牛頓的最小化地圖灰度差的方法來構造位姿圖的閉環運動約束。該方案可以單獨直接使用雷達數據(主要對于雷達數據源質量較高或機器人運動速度較低情形)，也可以使用輪速里程計及/或慣導來提供運動先驗，同時使用雷達數據。不足之處在于一方面幀間匹配依然存在錯誤估算的情況，對于邊建圖邊導航的探索模式中容易導致運動規劃控制無法正常進行；另一方面全局位姿圖優化過程中所使用的加速分支定界來根據相關度匹配尋找相似子圖并計算幀間運動而構建運動約束的位姿圖優化方法，對于計算資源的耗費是非常顯著的。就算使用多線程計算的方式，計算負載也很難降下來。

發明內容

有鑒于此，本發明實施例提供了一種激光建圖定位方法、裝置、存儲介質及電子設備，以解決現有技術在激光建圖定位過程中機器人的運動跟蹤過程抗干擾能力不夠強問題，以及位姿圖優化過程存在非線性優化問題不夠高效耗時過多的問題。

本發明提出的技術方案如下：

本發明實施例第一方面提供一種激光建圖定位方法，該方法包括：根據獲取的第一幀激光雷達數據構建局部的占據柵格地圖；根據運動跟蹤線程中當前幀激光雷達數據與上一幀激光雷達數據進行匹配，計算得到優化的幀間位姿運動矩陣；根據當前幀激光雷達數據與占據柵格地圖進行匹配，對所述優化的幀間位姿運動矩陣進行調整，得到精確的當前幀位姿；根據當前幀和當前關鍵幀之間的距離更新局部地圖或重新構建局部地圖；根據新插入的關鍵幀構建的運動約束，增量式優化更新關鍵幀隊列中各個關鍵幀的位姿，得到更新后的全局地圖。