復(fù)雜果園場(chǎng)景中基于DBP的激光回環(huán)檢測(cè)算法

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

(上海大學(xué)機(jī)電工程與自動(dòng)化學(xué)院, 上海 200444)

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 DBP點(diǎn)云描述子

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

圖4 提取果園中單幀雷達(dá)掃描的密度二進(jìn)制模式

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

首先,將單個(gè)點(diǎn)云pk=(xk,yk,zk)在極坐標(biāo)下表示為Γ=(rk,θk,hk),其中

(1)

(2)

hk=zk+ph

(3)

式中rk、θk、hk——點(diǎn)云半徑、方位角、實(shí)際高度

ph——雷達(dá)安裝高度

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

(4)

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

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

Pijξ——點(diǎn)云分割到單元塊的表示

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

(5)

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

(6)

(7)

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

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

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

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

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

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

(8)

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

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

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

(9)

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

圖6 DBP描述子對(duì)比

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

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

(10)

式中DBPq——查詢點(diǎn)云的描述子

DBPc——候選點(diǎn)云的描述子

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

(11)

(12)

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

ck——所有候選幀索引

3 復(fù)雜果園環(huán)境回環(huán)檢測(cè)實(shí)驗(yàn)

3.1 實(shí)驗(yàn)設(shè)置

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

圖7 實(shí)地果園場(chǎng)景實(shí)驗(yàn)

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

3.2 實(shí)驗(yàn)結(jié)果分析

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

表1 3種算法的軌跡誤差與時(shí)間消耗對(duì)比

圖8 實(shí)地果園實(shí)驗(yàn)軌跡與真值軌跡

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

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

果園3中場(chǎng)景尺度為2 294個(gè)機(jī)器人位姿,實(shí)驗(yàn)?zāi)M機(jī)器人實(shí)際采摘任務(wù)的頻繁轉(zhuǎn)向、抖動(dòng)。DBP-LeGO-LOAM與ISC-LeGO-LOAM算法均使用了改進(jìn)的回環(huán)檢測(cè)算法,相對(duì)于基準(zhǔn)的LeGO-LOAM算法機(jī)器人位姿估計(jì)優(yōu)化效果顯著。DBP-LeGO-LOAM算法取得了最好的實(shí)驗(yàn)效果,位姿估計(jì)軌跡的均方根誤差與標(biāo)準(zhǔn)差分別為0.24 m與0.09 m,減少81%與91%。LeGO-LOAM、ISC-LeGO-LOAM與DBP-LeGO-LOAM算法的時(shí)間消耗分別為75.43、63.19、73.60 s。

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

4 結(jié)論

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

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

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