歐 芳 苗中華 李 楠 何創(chuàng)新 李云輝

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

0 引言

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

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

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

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

1 回環(huán)檢測系統(tǒng)與關(guān)鍵技術(shù)

1.1 回環(huán)檢測算法

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

圖1 回環(huán)檢測示意圖

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

1.2 關(guān)鍵技術(shù)

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

圖2 復(fù)雜果園場景

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

針對以上提及的復(fù)雜果園的非結(jié)構(gòu)化、大尺度、高度相似特性,本文提出如圖3所示的激光回環(huán)檢測系統(tǒng)。系統(tǒng)具備的關(guān)鍵技術(shù)包括:

圖3 基于密度二進制模式的回環(huán)檢測算法框圖

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

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

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

2 基于DBP的回環(huán)檢測算法

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

2.1 DBP點云描述子

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

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

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

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

(1)

(2)

hk=zk+ph

(3)

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

ph——雷達安裝高度

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

(4)

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

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

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

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

(5)

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

(6)

(7)

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

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

2.2 兩階段回環(huán)檢測

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

圖5 兩階段回環(huán)檢測算法原理框圖

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

(8)

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

因此,環(huán)因子V是關(guān)于DBP的Nr維列向量表示

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

(9)

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

圖6 DBP描述子對比

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

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

(10)

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

DBPc——候選點云的描述子

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

(11)

(12)

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

ck——所有候選幀索引

3 復(fù)雜果園環(huán)境回環(huán)檢測實驗

3.1 實驗設(shè)置

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

圖7 實地果園場景實驗

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

3.2 實驗結(jié)果分析

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

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

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

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

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

果園3中場景尺度為2 294個機器人位姿,實驗?zāi)M機器人實際采摘任務(wù)的頻繁轉(zhuǎn)向、抖動。DBP-LeGO-LOAM與ISC-LeGO-LOAM算法均使用了改進的回環(huán)檢測算法,相對于基準的LeGO-LOAM算法機器人位姿估計優(yōu)化效果顯著。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。

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

4 結(jié)論

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

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

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