基于特征提取改進的LeGO-LOAM方法

楊書濤,郁漢琪,戴紅衛,李佩娟,杜俊峰,李 睿

(1. 南京工程學院工業中心/創新創業學院, 江蘇 南京 211167;2. 南京綠航生態農業有限公司, 江蘇 南京 211500)

隨著移動機器人的快速發展,其在物流、無人駕駛和智能家居等多個領域獲得廣泛應用.這些機器人通過使用傳感器,如攝像頭、激光雷達,能夠實現環境感知和自主移動,并成功完成各種復雜任務[1].機器人自主移動前需要進行同步定位和建圖(simultaneous localization and mapping,SLAM),以實現環境感知和機器人定位[2].

目前激光SLAM研究已成為熱點,研究主要集中在特征提取、點云分割、回環檢測等方面,特征提取是激光SLAM中的一個關鍵環節[3];在激光雷達獲取的原始點云數據中存在大量的噪聲和冗余信息,通過特征提取,可以保留最具代表性的關鍵特征,過濾掉無用信息和干擾信號,從而為后續的定位和建圖提供更加穩定可靠的數據源[4-5].特征提取可以幫助SLAM算法識別并匹配當前幀與歷史幀之間的對應關系,從而確定機器人在空間中的位置和姿態,并計算出其相對運動關系,提高定位精度.最早期的激光SLAM方法主要使用歐式距離或者點之間的其他幾何信息來計算匹配程度,從而構建地圖,但該方法未能提取有意義的特征點,導致匹配誤差大,且需要大量的計算資源與運算時間[6];文獻[7]提出基于特征匹配的激光雷達建圖(lidar odometry and mapping,LOAM)算法,通過對掃描點云數據進行高頻操作提取邊緣和平面特征點,但其對地面判斷不夠準確;文獻[8]在LOAM算法的基礎上提出輕量化激光雷達建圖(lightweight and groud-optimized lidar odometry and mapping,LeGO-LOAM)算法,采用地面分割方法和點云聚類方法來提取特征點,并在LOAM算法的基礎上加入IMU減小地圖匹配誤差.在特征選擇與匹配方面,這些算法雖然可以通過各種特征提取方法獲得關鍵特征點,但處理并不完善,存在匹配誤差較大的問題.

本文提出一種基于特征提取改進的LeGO-LOAM方法,采用自適應特征提取方法在不同距離下保證特征提取的準確性,通過基于隨機一致性采樣優化的地面特征濾除方法來去除不可靠特征點,通過相鄰幀中相同聚類對象的移動距離去除動態目標,使得后期提取的特征點更加合理.

1 特征點提取優化算法設計

1.1 自適應特征點提取

激光雷達是基于線掃描獲取點云信息,本文移動機器人安裝Velodyne16線激光雷達,雷達基礎坐標系定義為:Y軸正方向水平向前,Z軸正方向垂直向上,如圖1所示.其水平視場為360°,水平掃描分辨率為0.2°,垂直掃描分辨率僅為2°,所以整個環境點云是稀疏的.

圖1 Velodyne16線激光雷達坐標系的定義

傳統的特征提取方法通常采用關鍵點前后5個點的深度差值和的平方作為判斷指標[9],這種方法容易導致特征點的錯誤識別和遺漏.通過研究分組策略,本文采用一種自適應特征提取算法來解決這個問題[10].在計算關鍵點與鄰近點的深度差值時,先根據關鍵點的深度值進行對比,如果誤差超過一定閾值,則將其直接視為噪點并剔除;關鍵點i的深度越小,就需要更多的鄰近點,相反,關鍵點的深度越大,則需要較少的鄰近點.鄰近點數S在范圍(4,10)內進行自適應波動,對應的水平包角θ在0.8°～2°.圖2為基于深度變化的自適應曲率計算示意圖.由圖2可見:當深度R1較小時,相應的角度θ1包含的距離l1較短,為了獲取更多的臨近點來計算曲率,需要擴大角度θ2來增強曲率信息的可靠性;當深度R2較大時,θ1包含的距離l2過長,此時需要縮小θ3來計算曲率.當關鍵點的曲率小于閾值時,判定其為平面點,而當曲率大于閾值時,則判定為角點.

圖2 基于深度變化的自適應曲率計算示意圖

關鍵點i的水平曲率計算式為:

(1)

式中:ri為點i的三維坐標,m;S為同一行激光掃描線上的連續點集.

點集S在關鍵點i兩側平均分布,臨近點集S的值與關鍵點i的深度值Ri存在自適應關系:

S∈[(Ri≤10,10),(8,10

(6,2030)]

(2)

平面特征的閾值設置為0.1,角點特征的閾值設置為0.5[11].當計算得到的水平曲率C<0.1時,關鍵點i被歸類為平面特征點;而當C>0.5時,則被歸類為角特征點.

1.2 地面點云特征點濾除

在特征提取過程中會得到許多點云特征點,過多的特征點并不能顯著提高精度,反而會增加系統資源的消耗.在結構豐富的室外環境中,平面特征點的數量較多,這些平面特征點通常聚集在地面點云上;同時,地面上的特征點分布相似,這種相似性可能導致閉環檢測時的誤匹配.基于以上考慮,本文采用基于隨機一致性采樣(RANSAC)的地面點云特征濾除方法[12].

Velodyne16線激光雷達直發射角在±15°之間,在垂直發射角為正的激光線上不會出現地面點.但實際戶外環境地面傾斜或不平,激光線的垂直發射角度選擇在[-15°,1°]范圍內搜索地面點,包含地面平面模型的內點和外點,符合平面模型的點稱為內點,不符合的點稱為外點.在每幀點云中選擇3個點P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3)作為內點進行計算,并假設它們滿足某個平面模型方程xcosα+ycosβ+zcosγ=D,其中,(cosα,cosβ,cosγ)為地面的法向量,D為激光原點到地面的距離,地面點云的描述如圖3所示,距離誤差在預設的閾值內(距離閾值設置為0.1 m),將剩余點云帶入模型進行求解,就認為滿足平面模型,并將其添加到內點集合I={p1,…,pi,…,pn};進行k次迭代,比較每次產生的數學模型所包含的內點數量,保存內點數最多的模型,得到相對更合理的地面數學模型解;最后將所有的內點標記為地面點云.識別出地面點云后,不對地面點云進行后續操作,在處理最后一幀點云數據時,地面點云以黑色標記顯示,圖4(a)為LeGO-LOAM提取地面特征點,圖4(b)為優化后地面特征點.由圖4可見,優化后濾除地面特征有一定效果.

圖3 地面點云和地平面法向量

1.3 動態點濾除

在室外環境下,由于行人和行駛車輛等原因導致動態點較多,在去除動態點之前,需要先對點云數據進行分割聚類,并區分出動態對象和靜態對象.在真實的物理世界中,很多物體通過地面相互連接.如果在分割時不單獨考慮接地點,這些通過地面連接的物體可以被劃分為一類物體.去除接地點則可以根據點云的連接情況對三維世界中未連接的物體進行分割.

相鄰點示意圖如圖5所示,圖5中:O為LiDAR的坐標原點;A為點云中的任意點;B為相鄰激光線上的相同方位角點;C為同一激光線上的相鄰點;β為OA與AC之間的夾角,β為判斷A與C是否位于同一物體上提供有價值信息,計算式為:

圖5 相鄰點示意圖

(3)

根據文獻[13]將鄰角閾值定義為60°,如果相鄰點之間的夾角大于60°則可以認為它們來自于同一物體.通過上述分割,最終點云中的點數大于30,則認為分割有效[13];如果點數小于30,則可以將這些點視為離群點或干擾點,在進行特征匹配前將特征點去除.這種方法可以有效地去除離群點和干擾點.聚類后進行動態消除,具體步驟為:

1) 對物體進行聚類后,即可知道每種類型的物體所包含點的個數,該物體質心計算式為:

(4)

2) 激光雷達采樣頻率非?？?即使激光雷達隨著底盤不斷移動,也可以認為每次聚類后物體所含的點數基本保持不變,或者在一定范圍內發生變化,根據點的數量,可以匹配相鄰兩幀之間的對象.

3) 匹配對象可以計算出移動距離,移動距離用質心之間的變化量來表示,此時假設:a) 靜止物體的數量應遠遠多于運動物體的數量;b) 不同靜止物體的移動距離應基本保持不變或在一定范圍內變化;c) 靜止物體和運動物體之間的距離是可分辨的.基于以上假設,可以通過移動距離消除動態物體,對每個物體的移動距離進行計算和排序,首先去除最大的10%,因為這些可能是移動的物體;然后去除最小的10%,這些可能是匹配異常的物體.

2 仿真分析與室外試驗驗證

2.1 仿真分析

KITTI數據集[14-15]由德國卡爾斯魯厄理工學院和美國豐田理工學院共同創建.該數據集被廣泛用于評估自動駕駛場景下計算機視覺算法的性能.KITTI數據集不僅提供原始點云數據,還包含軌跡真值數據(ground truth),可以用來分析LiDAR里程表的精度.選取的序列為09,時間約為165 s.

通過軌跡精度評測軟件EVO可直觀體現SLAM算法的里程計與真實里程計的整體偏差.本文在KITTI09數據集上將各類算法和改進算法進行全局里程計偏差對比,結果如圖6、圖7所示.為進一步深入分析,將LeGO-LOAM、LIO-SAM軌跡數據中的X、Y、Z軸坐標分別與ground truth進行詳細對比,結果見表1.

表1 激光雷達里程計的誤差數據 m

圖6 生成軌跡對比

圖7 X、Y、Z軸運動估計的比較

由圖6可見,本文改進算法比其他算法真實里程計偏差較低.由于KITTI09數據集具備64線激光雷達,點云數據豐富,因此,所有算法均能夠實現低漂移的運動估計;由圖7可見,所有算法誤差主要集中在Y軸上,即垂直方向的偏差較大,與其他算法相比,本文算法在降低3個軸向上的最大誤差方面表現出良好效果,其中對Y軸誤差的改善尤為顯著;由表1可見,本文算法比加入了IMU預積分約束的LIO-SAM能獲得更小的地圖匹配誤差,計算得出的最大誤差相較于初始算法減少了23.9%,平均誤差也呈現出下降,降幅達到19.1%.

綜上所述,通過多重觀察角度的圖像分析和數值數據對比,本文算法不僅在降低三軸最大誤差方面表現出優越性能,尤其是在Y軸方向上取得了顯著的改善.這些結果為該算法在實際應用中的準確性和可靠性提供了有力支持.為了驗證優化后運動估計的穩定性,進行5組運動估計試驗,結果如圖8所示.由圖8可見,三軸坐標基本不變,說明本文方法具有良好的魯棒性和穩點性.

圖8 5組試驗運動估計比較

2.2 實際戶外環境試驗

試驗使用松靈公司研制的BUNKER底盤,車身尺寸為1 023 mm×778 mm×400 mm.車身前端配置Velodyne16線激光雷達,該雷達支持TCP/IP協議,通過網線將激光雷達與計算機連接.運行算法的計算機配置為英偉達公司生產的Jeston TX2,系統為Ubuntu18.04.試驗平臺框架如圖9所示.

圖9 試驗平臺框架

為了評估優化后的SLAM效果,在戶外環境中進行軌跡測試,測試結果如圖10所示.由圖10可見,標記出的底盤實際運動路徑具有復雜多變的路況,地面坡度變化頻繁,路邊有許多車輛、樹木和行人,這些動態元素對于點云處理過程中的特征點去除造成了重大干擾.如果特征點誤判為擺動的葉子,運動估計結果將產生偏差.生成的全局點云圖如圖11所示.圖11能夠高度還原室外環境,包括道路兩側的建筑物和臺階,輪廓清晰可見.

圖10 實際運動路徑圖

圖11 全局點云圖

為了進一步評估系統性能,將記錄的LiDAR點云數據保存為Bag文件,并進行5次離線試驗,對比每次試驗產生的軌跡發現,5次重復試驗生成的軌跡高度重疊.將LiDAR點云數據記錄過程中計算的里程表數據作為真值,并對比記錄5次試驗的里程表誤差,結果如表2所示.由表2可見,與在線生成的里程數據表相比,5個離線試驗計算出的最大位置誤差小于1 m,表現出良好的魯棒性和穩定性.這些結果進一步證明了本文算法的可靠性和魯棒性.

表2 五組試驗與激光里程表誤差 m

3 結語

本文提出了基于自適應特征提取及濾除地面不可靠特征點和動態點的即時定位建圖算法.在特征提取方面,該算法能夠準確、高效地提取出地圖中的關鍵特征點,為后續的定位和建圖任務奠定基礎.同時,可有效解決地面特征點的濾除和動態點的消除,確保定位建圖的精度和穩定性.仿真結果表明,優化后計算出的最大誤差降低了23.1%,平均誤差降低了19.1%.5個離線試驗計算出的最大位置誤差小于1 m,表現了良好的魯棒性和穩定性.然而,當前的算法在閉環檢測方面可能還存在一定的局限性,需要進一步的研究和改進,提升定位建圖算法的性能和可靠性.