[發明專利]一種基于LeGO-LOAM的分步式幀間位姿估計算法

有效

• 申請號: 202110317982.7
• 申請日: 2021-03-25
• 公開（公告）號: CN112907610B
• 公開（公告）日: 2022-04-29
• 發明（設計）人: 潘樹國;章輝;高旺;趙濤;譚涌
• 申請（專利權）人: 東南大學
• 主分類號: G06T7/13
• 分類號: G06T7/13;G06T7/20;G06K9/62;G06V10/764
• 代理公司: 南京眾聯專利代理有限公司 32206
• 代理人: 蔣昱

本發明公開了一種基于LeGO?LOAM的分步式幀間位姿估計算法。首先讀入激光雷達的點云信息，并將點云劃分為地面點和非地面點，再標記小集群點。在對點云進行分類之后，在地面點中提取地面特征點，在非地面點中提取邊緣特征點。之后對位姿進行分步求解，步驟一通過地面特點計算得到幀間位姿的翻滾角、俯仰角和豎直方向的位移，并將計算得到的三自由度位姿代入步驟二的邊緣特征點計算偏航角以及水平面上位移的過程，在步驟二中對六自由度位姿同時進行優化，最后再與地圖點共同優化位姿。應用本發明的方法求解的軌跡精度較LeGO?LOAM算法的軌跡精度提升最高達45.4％。

技術領域

本發明涉及車輛平臺的SLAM技術方案，尤其涉及一種基于LeGO-LOAM的分步式幀間位姿估計算法。

背景技術

無人駕駛技術中，作為車輛的感知器官，激光雷達有著舉足輕重的地位，而激光SLAM(simultaneouslocalizationandmapping)正是車輛對環境與自身感知的一種手段。相較于視覺SLAM，激光雷達有著更高的精度、更強魯棒性、更大的視角和更精準的尺度信息。

三維激光雷達可以捕獲更多環境的細節，所以本文重點研究了三維激光雷達在室外環境下的實時定位與建圖技術。尋找激光雷達相鄰幀間轉換的典型方法是迭代最近點(ICP)，ICP算法需要計算所有獲得的激光點云，所以計算量巨大。為了提高ICP算法的效率和精度，提出了多種改進ICP算法，其中廣義ICP提出了一種匹配相鄰幀間局部平面塊的方法，此后基于特征匹配的方法受到越來越多的關注，相繼提出了大量的利用特征進行點云配準的算法。

基于特征匹配的LOAM算法是3D激光SLAM中最為經典的算法，LOAM算法中的特征提取算法原理為計算點在其局部區域的曲率，進而將激光雷達獲取的點云通過曲率分為平面特征點和邊緣特征點，根據這兩類特征點完成相鄰幀間的特征匹配，并開創性的提出了將位姿估計劃分為高頻的位姿估計和低頻的位姿優化。LOAM算法作為一種普適性算法，未對于無人車平臺，做出針對性的優化，所以在無人車平臺上存在著運算速度和精度仍有提升空間。2018年發布的LeGO-LOAM算法針對無人車平臺對LOAM算法進行了改進，該算法將平面點進一步細化為地面特征點，并且在幀間位姿求解時采用了分步求解的策略，充分利用了道路的特征，簡化了幀間位姿估計的運算復雜度。但是LeGO-LOAM的幀間位姿估計策略中，在根據邊緣特征點估計位姿估計時，僅將在不影響算法運行速度的前提下，對通過地面特征匹配求得的三自由度的位姿變化結果進行優化，從而達到提升整體位姿估計精度的效果。

發明內容

發明目的：針對以上問題，本發明提出一種基于LeGO-LOAM的分步式幀間位姿估計算法。在分步計算車輛幀間的位姿時，將(rx，rz，ty)也代入由特征點求取位姿的最小二乘計算中，在對(ry，tx，tz)的同時，對(rx，rz，ty)也進行優化，提高分步求解位姿的精度。

技術方案：為實現本發明的目的，本發明所采用的技術方案是：一種基于LeGO-LOAM的分步式幀間位姿估計算法，具體方法如下：

(1)讀入激光點云數據；

系統通過激光雷達獲取周圍環境的點云信息，并將點云信息進行處理，將激光束編號，從仰角最小的激光束開始編號，作為點云的縱坐標，激光的水平編號，從旋角為0的點開始編號，作為其橫坐標，對點云進行編號，最后記錄點云的深度信息、水平旋角和仰角；

(2)對點云進行分類；

點云分類通過兩步完成，第一步通過地面點計算以及公式(1)的原理完成地面點的標記；

θ＝atan 2(ΔZ-ΔX)

ΔX＝R_r-1 cosα-R_r cosβ

ΔZ＝R_r-1 sinα-R_r sinβ(1)