復雜果園場景中基于DBP的激光回環檢測算法

歐 芳 苗中華 李 楠 何創新 李云輝

(上海大學機電工程與自動化學院, 上海 200444)

0 引言

機器人在果園場景中基于雷達點云數據在定位的同時構建地圖,即激光同步定位建圖 (Simultaneous localization and mapping, SLAM)技術,是果園機器人領域的核心問題。在SLAM過程中加入回環檢測能夠實時矯正位姿估計誤差,提高定位精度[1-2]。目前回環檢測主要采用融合基準的全球定位系統 (Global positioning system,GPS)與視覺傳感器的方法[3-4]。然而,視覺傳感器對長期的光照變化敏感,往往不能成功地跟蹤視覺特征。并且,在果園、森林等樹冠茂密的場景中,衛星信號容易被阻攔,導致GPS失效[5-6]。相較而言,點云數據不受室外光照變化以及信號遮擋影響,更適用于復雜果園場景[7-9]。

回環檢測是機器人SLAM技術中的關鍵內容,常見的回環檢測算法包括基于手工特征與深度學習的方法。文獻[10-11]提取關于點云的直方圖表示用于點云配準,然而直方圖提取需要大量的算力消耗,不適用于在大尺度場景中檢測點云幀的回環對。文獻[1]提出了基于點云掃描上下文信息的回環檢測方法,通過提取區域點云的最大高度值,將單幀雷達掃描投影到鳥瞰圖矩陣得到回環檢測特征Scan Context。文獻[12]利用點云幾何分布與強度投影提取ISC描述子,實現兩階段快速回環檢索,避免了直接暴力匹配。文獻[13]基于高通濾波提取具有旋轉不變性的點云描述子。這些手工描述子將三維空間點云表示為二維矩陣,提高了回環檢測效率。然而,單一的點云強度、高度最值并不具備點云掃描空間的全局代表性,在具有多個高度相似場景的果園,無法從細節區分場景目標,導致回環檢測的高失敗率。

深度學習為回環檢測帶來了新的可能性。文獻[14]提出了一種聯合在線 LiDAR 掃描和強度圖并通過卷積匹配實現回環檢測的方法。文獻[15]利用高斯混合模型對多個可能的回環檢測結果建模,有效排除錯誤回環索引。文獻[16]直接對點云進行處理,對輸入點云中的每一個點,學習其對應的空間編碼,最后集成所有局部點的特征得到一個全局的點云特征。文獻[17]提出了一個多層次點云特征提取結構Pointnet++,有效提取局部特征。然而,深度學習模型訓練需要大量的差異性數據,且通常只對特定場景有效,不適用于高度相似的果園場景。

本文面向非結構化、大尺度、高度相似的復雜果園場景,提出一種基于密度二進制模式(Density binary pattern,DBP)描述子的回環檢測算法。將當前幀點云分割成Nr×Ns×Nh個體素塊,并進一步地將單元塊的點云數據編碼投影到Nr×Ns矩陣,以此構建具有區分性的密度二進制模式全局DBP。DBP描述子為鳥瞰圖矩陣,矩陣元素值由編碼函數決定,值區間為0～255,為提高回環檢索效率,以兩階段搜索方法實現快速回環檢測?；跉v史幀的DBP環因子構造KD-Tree(K-dimensional tree,K維度樹),從KD-Tree中初步檢索候選(可能的)回環幀。對所有候選幀的DBP描述子進行列平移,計算得到與當前幀最相似的候選幀作為回環目標幀。

1 回環檢測系統與關鍵技術

1.1 回環檢測算法

如圖1所示,在SLAM問題中,機器人位姿估計是一個遞推的過程,即由上一幀位姿解算當前幀位姿。隨著時間推移,位姿誤差會不斷累積導致軌跡漂移。成功的回環檢測,可以顯著地減小建圖與定位累積誤差,使機器人更精準、快速地進行避障導航等工作。

圖1 回環檢測示意圖

因此,在大尺度SLAM任務上執行回環檢測實現軌跡優化是非常必要的?；丨h檢測的實質是為了求兩幀點云的相似性配準,在機器人的運動過程中實時查找與當前幀相似的歷史點云幀,構建回環對之間的位姿約束?；丨h檢測過程中,當前幀xi(i=1,2,…,N)在歷史幀xj(j=1,2,…,N-C)數據庫中查詢相似的場景并配準。為保證回環檢測的可靠性,查詢的歷史幀需與當前幀點云保持一定距離C?？紤]到在大尺度場景中,點云逐幀配準查詢將導致巨大的計算量,因此,為減少回環檢測時間,保證SLAM任務的實時性,通常將回環檢測分為兩階段檢索方法。首先,將每幀點云抽象表示為詞袋模型[18]、鳥瞰圖[1,12]等特征描述子;其次,在候選回環幀中查找最終回環。

1.2 關鍵技術

近年來,隨著傳感器技術的快速發展,果園機器人的使用已顯著增加。果園機器人任務主要包括果園物境信息獲取、剪枝套袋、對靶噴藥、疏花疏果、果實采摘等,它們對機器人移動過程中建圖與定位具有高精度要求[19-21]。為了緩解SLAM過程中由累積誤差導致的軌跡 “大漂移”,引入回環檢測構造位姿估計約束。然而,在復雜的果園環境中回環檢測需要克服非結構化、大尺度、場景高度相似的挑戰。如圖2所示,在復雜果園場景中,果樹的長期生長變化、枝葉運動以及果樹的無規則布局,決定了果園的非結構化特性。

圖2 復雜果園場景

不同于成行成列的半結構化/結構化果園,機器人可以以等間距的行距與列距作為回環檢測的先驗信息。復雜果園的非結構化特性給回環檢測帶來巨大挑戰。機器人建圖與定位是增量式累積的過程,回環檢測過程中當前雷達幀在所有歷史幀中遍歷查詢最相似場景。隨著作業場景尺度的增加,歷史幀數據庫內容劇烈增加,回環檢測將帶來巨大的計算消耗。不同于常見的戶外場景可以人、車、樓宇等特征明顯的目標輔助回環檢測。果園場景中目標單一且主要為果木。同一個果園可能包含多處相似的具有弱可區分性的場景,從而給回環檢測中成對相似場景的識別帶來巨大挑戰。

針對以上提及的復雜果園的非結構化、大尺度、高度相似特性,本文提出如圖3所示的激光回環檢測系統。系統具備的關鍵技術包括:

圖3 基于密度二進制模式的回環檢測算法框圖

(1)針對非結構化特性,將點云掃描空間分割為多個3D體素塊,并編碼體素塊為鳥瞰圖描述子DBP。描述子只考慮目標點云的整體輪廓,不受樹葉運動等導致的細微動態環境變化影響。

(2)針對果園弱可區分特性,全局描述子能體現點云的高度與密度信息,具備對果樹、雜草以及低矮灌木的細節區分能力。

(3)針對果園的大尺度特性,避免直接的關鍵幀匹配,以有效的兩步法提高回環檢測效率。

2 基于DBP的回環檢測算法

本文面向果園場景的回環檢算法主要包括:提取點云DBP描述子與兩階段回環檢索。

2.1 DBP點云描述子

文獻[13,22]表明,將無序點云有序化是最常見的提高計算速度的方法。本文定義了一種在果園場景中回環檢測的密度二進制模式描述子DBP,一種關于單幀雷達掃描的全局描述子,提取流程如圖4所示。

圖4 提取果園中單幀雷達掃描的密度二進制模式

DBP描述子將雷達360°掃描的球體空間投影為鳥瞰圖矩陣。以單幀雷達掃描中的果樹為例,將果樹點云分割到垂直方向8個3D體素塊,若體素塊內存在點云占位,則其二進制值為1,否則為0。進一步將得到的8位二進制值基于DBP編碼函數投影為鳥瞰圖矩陣元素。鳥瞰圖以雷達為原點,沿掃描半徑與方位角方向將雷達的3D掃描空間展開。DBP描述子提取的具體流程描述為:

首先,將單個點云pk=(xk,yk,zk)在極坐標下表示為Γ=(rk,θk,hk),其中

(1)

(2)

hk=zk+ph

(3)

式中rk、θk、hk——點云半徑、方位角、實際高度

ph——雷達安裝高度

將點云沿垂直高度方向分割成Nr×Ns×Nh個體素塊P,設置雷達最大有效掃描半徑Rmax=80 m、最大有效高度Hmax=8 m,以及雷達掃描一周的方位角為2π。

(4)

式中Nr、Ns、Nh——點云在半徑、方位角、高度的單元塊分割數量,分別取20、60、7

i、j、ξ——點云分割到單元塊中對應掃描半徑、方位角、高度方向的序號

Pijξ——點云分割到單元塊的表示

可能有多個點落在同一個相同i、j、ξ序號的單元塊P內。

(5)

在全局描述子DBP編碼過程中,沿高度方向將點云分割到8位體素塊,同時將體素塊二進制值編碼投影到DBP矩陣,以此構建具有區分性的空間二進制模式全局表示

(6)

(7)

式中δ(P)——體素塊賦值函數,由塊內點云占位決定

δ(P)函數對體素塊賦二進制值,塊內存在點云則為有效掃描,表示為占位;反之為無效掃描,非占位。在描述子編碼函數中,對點云占位體素塊賦值為1,否則為0。由于體素塊分割考慮到點云的幾何信息,同時點云占位與果樹點云密度關聯。因此,全局描述子DBP元素內容同時解釋了點云的幾何與密度信息。并且,由于二進制編碼函數的指數特性,占據高位體素塊的果樹編碼值明顯大于低矮雜草、灌木的特征編碼值。因此,描述子能對不同高度與密度分布的植被進行有效區分。

2.2 兩階段回環檢測

回環檢測需要在所有歷史幀中檢索與當前場景最相似的雷達掃描幀作為目標回環幀。然而,隨著機器人經過的路程越長,復雜果園場景尺度不斷增加,增量式建圖與定位的同時伴隨著巨大計算量。激光回環檢測作為SLAM系統中的優化環節,應盡可能減少時間占比,保證機器人任務的實時性。本文基于兩階段搜索算法實現在大尺度場景中高效地檢測回環。如圖5所示,基于歷史幀DBP描述子的環因子構造KD-Tree,從歷史幀中檢索K近鄰候選幀;計算候選回環幀與當前查詢幀的DBP描述子相似度,確定最終目標回環。

圖5 兩階段回環檢測算法原理框圖

(1)提取候選(可能的)回環幀。提取關于DBP的低維向量表示,引入環因子向量概念,統計DBP每行非零元素的占有率

(8)

式中 ‖ri‖0——DBP矩陣行向量的零范數

因此,環因子V是關于DBP的Nr維列向量表示

V=(φ(r1),φ(r2),…,φ(rNr)),whereφ:ri→R

(9)

如圖6所示,將以雷達為中心的360°掃描空間投影到DBP鳥瞰圖矩陣,矩陣的行對應掃描的水平方位角方向。

圖6 DBP描述子對比

同一場景中,不同雷達掃描幀只存在水平方位角偏差,導致投影到鳥瞰圖時產生列平移。列平移不影響行向量中非零元素的統計,因此環因子不受視角變化影響。為保證高效候選幀檢索,基于歷史幀描述子的環因子構造KD-Tree。當前掃描(查詢)幀在KD-Tree中查詢K近鄰幀作為候選回環幀(K=25,K為候選回環幀數量)。雖然相比于全局描述子DBP,環因子的信息含量更低,但是低維環因子可實現快速回環幀初步檢索。

(2)當前幀與候選回環幀描述子相似度計算。由于同一場景下的不同掃描幀存在水平偏差。因此,對所有候選幀平移n列計算與當前幀的相似度

(10)

式中DBPq——查詢點云的描述子

DBPc——候選點云的描述子

確定候選幀與當前幀之間的最大相似度為d的最小值

(11)

(12)

式中c*——與查詢幀距離最小的候選幀索引

ck——所有候選幀索引

3 復雜果園環境回環檢測實驗

3.1 實驗設置

果園實驗平臺為圖7a所示的果園采摘機器人。機器人搭載內存為500 GB的Intel i7 NUC微型工控機與16線速騰激光雷達。實驗將3種回環檢測算法都集成到機器人操作系統(Robot operating system, ROS),評估算法在3個不同特點果園的性能。

圖7 實地果園場景實驗

經典的LeGO-LOAM[23]是一種可搭載于嵌入式系統的輕量化激光SLAM,適用于大尺度戶外環境。系統由點云分割、特征提取、雷達里程計、雷達建圖、位姿估計5個環節組成。在雷達建圖環節中基于點云幀之間的距離與時間閾值構建簡單回環約束。本文以ISC[12]與本文算法改進LeGO-LOAM的回環檢測內容,構造ISC-LeGO-LOAM與DBP-LeGO-LOAM激光SLAM系統。實驗以運行時間與位姿估計誤差評估LeGO-LOAM、ISC-LeGO-LOAM與DBP-LeGO-LOAM 3個算法在不同尺度果園場景的性能。實驗結果證明了DBP-LeGO-LOAM算法在果園場景回環檢測的可行性,特別是在大尺度多回環場景具有顯著優越性。

3.2 實驗結果分析

在果園3的回環檢測實驗中,機器人基于LeGO-LOAM算法生成的實際果園環境點云地圖如圖7b所示。機器人重新回到起點時回環識別失敗,雷達里程計終點未與起點構成閉環,位姿估計的累積誤差導致機器人運動軌跡“大漂移”?；诒疚腄BP-LeGO-LOAM算法生成的實際果園環境的點云地圖如圖7c所示。機器人識別曾經到達的場景,成功執行了位姿回環矯正,使得軌跡閉環。如圖8所示, 實地果園實驗中基于DBP-LeGO-LOAM算法生成的軌跡與真值(Ground truth)軌跡基本保持一致。為直觀量化算法性能,本文以算法軌跡與真值軌跡的絕對誤差(Absolute trajectory error, ATE)評估3種激光SLAM系統。表1呈現了絕對軌跡誤差的均方根誤差(Root mean square error, RMSE)、標準差(Standard deviation, STD)與算法用時,反映了算法的魯棒性、穩定性與實時性。

表1 3種算法的軌跡誤差與時間消耗對比

圖8 實地果園實驗軌跡與真值軌跡

在果園1中,場景尺度為1 604個位姿。本文算法的軌跡誤差最小,均方根誤差與標準差分別為0.12 m與0.08 m。DBP-LeGO-LOAM算法相比于LeGO-LOAM與ISC-LeGO-LOAM算法定位軌跡的均方根誤差分別減少33%和40%。LeGO-LOAM的全局回環檢測算法取得最小運行時間44.94 s,本文的DBP-LeGO-LOAM算法運行時間為52 s,提取回環檢測描述子使算法運行時間增加7.06 s。

果園2場景尺度為10 458個機器人位姿,場景中地勢平坦,機器人運動平滑無大抖動,本文DBP-LeGO-LOAM算法的均方根誤差為0.23 m,相比LeGO-LOAM算法軌跡減少32%。隨著場景尺度的增加,全局回環檢測算法的時間消耗為125.44 s,LeGO-LOAM算法已不再適用。兩階段回環檢測DBP-LeGO-LOAM算法的時間消耗為118.38 s,比LeGO-LOAM算法減少7.06 s。

果園3中場景尺度為2 294個機器人位姿,實驗模擬機器人實際采摘任務的頻繁轉向、抖動。DBP-LeGO-LOAM與ISC-LeGO-LOAM算法均使用了改進的回環檢測算法,相對于基準的LeGO-LOAM算法機器人位姿估計優化效果顯著。DBP-LeGO-LOAM算法取得了最好的實驗效果,位姿估計軌跡的均方根誤差與標準差分別為0.24 m與0.09 m,減少81%與91%。LeGO-LOAM、ISC-LeGO-LOAM與DBP-LeGO-LOAM算法的時間消耗分別為75.43、63.19、73.60 s。

在小尺度的具有單一回環的果園中(果園1),機器人位姿估計累積誤差小,可優化空間有限。DBP-LeGO-LOAM算法以時間代價取得定位精度的提升。然而,在復雜的非結構化、大尺度、多回環果園場景中(果園3)DBP-LeGO-LOAM算法既能保證算法的實時性,同時多次成功的回環檢測也保證了機器人定位精度。算法在復雜場景的優越性可解釋為:DBP描述子對高度相似的場景有效編碼,保證了回環檢測的正確性。在大尺度果園場景中,多次成功回環檢測與矯正使機器人“大漂移”誤差顯著減少。同時,相比LeGO-LOAM中直接基于幀間距離與時間閾值的回環檢測算法,以兩步法執行回環檢測,可有效提高算法效率,保證了算法可靠性與實時性,使算法不受場景尺度限制。

4 結論

(1)提出DBP描述子包含高度與密度信息,利用空間中點云占位區分高度相似的場景。

(2)將大尺度果園場景的回環檢測過程分成兩步,改進基于距離的回環檢測方法,提高檢索效率。

(3)在實際工程實驗中,驗證了本文算法在復雜果園場景的優勢,有效消除了位姿估計的累積誤差,使機器人運動軌跡與真值軌跡保持一致。本文改進算法在多回環場景的均方根誤差與標準差分別為0.24 m與0.09 m,相比LeGO-LOAM算法分別減少81%與91%。