基于改進(jìn)Gmapping算法的果園二維環(huán)境地圖精準(zhǔn)構(gòu)建

薛金林 王培曉 周 俊 程 峰

(南京農(nóng)業(yè)大學(xué)工學(xué)院, 南京 210031)

0 引言

果園作業(yè)機(jī)器人的定位與導(dǎo)航技術(shù)研究主要集中于基于SLAM算法的果園環(huán)境地圖構(gòu)建研究,果園環(huán)境地圖能否實(shí)現(xiàn)精準(zhǔn)構(gòu)建直接影響后續(xù)定位與導(dǎo)航作業(yè)的效果。

在眾多環(huán)境信息獲取傳感器中,激光雷達(dá)以其不易受光照影響、精度和實(shí)時(shí)性高等優(yōu)點(diǎn)被廣泛應(yīng)用[1-3]。針對(duì)激光SLAM算法,典型的三維環(huán)境地圖構(gòu)建算法有基于3D特征匹配的LOAM算法[4-5]和LeGO-LOAM算法[6]。LOAM算法通過(guò)提取環(huán)境中面特征和線特征進(jìn)行幀間匹配,特征匹配效果良好,地圖構(gòu)建較為精準(zhǔn)。LeGO-LOAM算法憑借基于線的地面分割方法提取地面點(diǎn),有效提升了算法整體的速度。上述三維地圖構(gòu)建算法雖然實(shí)現(xiàn)了果園環(huán)境的精準(zhǔn)構(gòu)建,但果樹(shù)樹(shù)冠的季節(jié)性變化及果樹(shù)的成長(zhǎng)和衰老造成的果樹(shù)特征變化不利于后續(xù)的地圖匹配,為解決此問(wèn)題有部分研究針對(duì)果園中樹(shù)干這一不變特征進(jìn)行二維環(huán)境地圖構(gòu)建。針對(duì)較小的簡(jiǎn)單果園場(chǎng)景,張瑩瑩等[7]對(duì)DBSCAN算法采用自適應(yīng)密度閾值和聚類半徑實(shí)現(xiàn)對(duì)果園樹(shù)干地圖的精準(zhǔn)構(gòu)建;GRISETTI等[8]提出的Gmapping算法以較小的計(jì)算量實(shí)現(xiàn)了穩(wěn)定且較高精度的地圖構(gòu)建。但對(duì)于特征復(fù)雜的大場(chǎng)景果園,張瑩瑩提出的改進(jìn)DBSCAN算法在作業(yè)機(jī)器人轉(zhuǎn)向角度過(guò)大時(shí)可能超出閾值范圍造成樹(shù)木的漏檢;Gmapping算法基于3D點(diǎn)云匹配方法的ICP前端里程計(jì)在后期累計(jì)誤差較大[9],并且后端優(yōu)化部分對(duì)前端里程計(jì)的初始定位信息的優(yōu)化效果有限,導(dǎo)致最終的定位精度較低,不利于精準(zhǔn)二維地圖的構(gòu)建。LIM等[10]采用了魯棒的基于多平面的R-GPF地面分割方法,有效提高了前端里程計(jì)精度,但該方法在面對(duì)果園內(nèi)較為豎直的果樹(shù)樹(shù)干和低矮平緩的草坪或灌木時(shí)可能分割失敗。針對(duì)后端優(yōu)化,ROH等[11]提出的改進(jìn)PSO粒子群優(yōu)化算法的前期優(yōu)化效果穩(wěn)定,但在迭代后期由于無(wú)法權(quán)衡全局搜索的程度易陷入局部最優(yōu)的困境。

本文對(duì)Gmapping算法進(jìn)行改進(jìn),前端里程計(jì)以LeGO-LOAM測(cè)程框架為底層,針對(duì)果園實(shí)際環(huán)境對(duì)R-GPF方法的探測(cè)區(qū)模型和地面點(diǎn)估計(jì)方法進(jìn)行相應(yīng)改進(jìn),提高果園內(nèi)地面分割效果,進(jìn)而獲得高精高頻的激光雷達(dá)里程計(jì)。后端優(yōu)化部分采用一種受蝙蝠尋找獵物行為啟發(fā)的自適應(yīng)重采樣方法實(shí)現(xiàn)對(duì)初始定位信息的有效和合理程度的優(yōu)化,以提高最終的定位精度,從而獲得精準(zhǔn)的果園環(huán)境地圖。

1 精準(zhǔn)果園環(huán)境地圖構(gòu)建方法

本文提出的精準(zhǔn)果園環(huán)境地圖的構(gòu)建算法需要對(duì)Gmapping算法的前端里程計(jì)和后端優(yōu)化部分進(jìn)行改進(jìn),其中前端里程計(jì)用基于改進(jìn)R-GPF方法的激光雷達(dá)里程計(jì)模塊進(jìn)行替換,后端優(yōu)化Rao-Blackwellised粒子濾波器[12]的重采樣方式采用BAT啟發(fā)式自適應(yīng)重采樣,基于改進(jìn)Gmapping算法的精準(zhǔn)地圖構(gòu)建算法框架如圖1所示。

圖1 精準(zhǔn)地圖構(gòu)建算法框架

1.1 精準(zhǔn)地圖構(gòu)建算法整體流程

基于改進(jìn)Gmapping算法的整體流程就是實(shí)現(xiàn)作業(yè)平臺(tái)的位姿預(yù)估和利用預(yù)估位姿最終實(shí)現(xiàn)地圖的獲取。位姿預(yù)估即作業(yè)平臺(tái)依靠激光雷達(dá)觀測(cè)信息z1:t=z1,z2,…,zt與前端里程計(jì)信息u1:t-1=u1,u2,…,ut-1進(jìn)行后端優(yōu)化得到最終定位信息x1:t=x1,x2,…,xt,即求解自身位姿的后驗(yàn)概率分布p(x1:t|z1:t,u1:t-1),后端優(yōu)化步驟概括如下:

(1)

再進(jìn)行權(quán)重歸一化處理

(2)

(4)重采樣。根據(jù)歸一化后得到的權(quán)重進(jìn)行重采樣,權(quán)重較大的粒子得以保留,權(quán)重較小的粒子予以剔除。重采樣結(jié)束后,可計(jì)算獲得最終定位信息,同時(shí)所有粒子的權(quán)重都重新設(shè)置為1/nr,nr為剩余粒子數(shù)。

最后進(jìn)行地圖更新。根據(jù)歷史觀測(cè)信息z1:t和步驟(4)獲得的最終定位信息計(jì)算p(m|x1:t,z1:t),實(shí)現(xiàn)地圖更新。

1.2 基于改進(jìn)R-GPF地面分割方法的激光雷達(dá)里程計(jì)

圖1里程計(jì)模塊以LeGO-LOAM框架為底層,并將LeGO-LOAM基于線的地面分割方法用改進(jìn)R-GPF方法代替,測(cè)程過(guò)程概括為以下4個(gè)步驟:①基于改進(jìn)R-GPF方法進(jìn)行地面分割,識(shí)別得到地面點(diǎn)。②將全部點(diǎn)云和地面點(diǎn)分別進(jìn)行點(diǎn)云投影。③基于投影圖像中的平滑度提取邊緣特征和平面特征。④利用特征的對(duì)應(yīng)關(guān)系,獲得連續(xù)掃描之間的變換矩陣來(lái)估計(jì)測(cè)程。激光雷達(dá)里程計(jì)框架如圖2所示。

圖2 激光雷達(dá)里程計(jì)框架

1.2.1地面分割問(wèn)題定義

(3)

1.2.2探測(cè)區(qū)模型

LeGO-LOAM使用激光雷達(dá)位于不同水平分辨率上同一線上任意兩點(diǎn)之間的角度差來(lái)提取地面,它往往對(duì)某些噪聲很敏感,如崎嶇不平的地面,草坪或灌木叢也會(huì)阻礙地面分割,因?yàn)樗鼈兊男螤钐荻仁侨我獾摹ｈb于基于線的地面分割方法易受干擾,因此提出了魯棒的基于多平面的R-GPF地面分割方法,該方法以激光雷達(dá)為坐標(biāo)原點(diǎn)建立二維圓形探測(cè)網(wǎng)格,再根據(jù)點(diǎn)云的三維坐標(biāo)進(jìn)行空間上點(diǎn)云的多平面分布。

R-GPF方法的探測(cè)網(wǎng)格M沿徑向和周向進(jìn)行規(guī)則劃分,最終形成環(huán)區(qū)和扇區(qū),其中Nr和Nθ分別是環(huán)數(shù)和扇區(qū)的數(shù)目,這樣M最終將分為徑向距離為Rmax/Nr,對(duì)應(yīng)軸向角為2π/Nθ的Nr×Nθ個(gè)網(wǎng)格,其中Rmax為最大邊界,探測(cè)網(wǎng)格M劃分結(jié)果如圖3a所示。

圖3 探測(cè)區(qū)模型示意圖

然而在語(yǔ)義KITTI數(shù)據(jù)集上對(duì)所有序列的各幀點(diǎn)云數(shù)據(jù)進(jìn)行測(cè)量的實(shí)驗(yàn)數(shù)據(jù)[14]表明,有90%以上的地面點(diǎn)位于激光雷達(dá)原點(diǎn)附近20 m范圍內(nèi)。因此,R-GPF方法的探測(cè)網(wǎng)格M的劃分方式面臨以下兩個(gè)問(wèn)題:①當(dāng)點(diǎn)云距離坐標(biāo)原點(diǎn)過(guò)遠(yuǎn)時(shí),對(duì)應(yīng)網(wǎng)格內(nèi)的點(diǎn)云過(guò)于稀疏,因此難以擬合出地平面。②由于靠近坐標(biāo)原點(diǎn)的網(wǎng)格太小并且原點(diǎn)附近點(diǎn)云過(guò)于密集,這種情況有時(shí)會(huì)導(dǎo)致地平面的法向向量估計(jì)失敗[15]。

(4)

其中

θk=arctan2(yk,xk)

(5)

ΔRm=Rmax,m-Rmin,m

(6)

Rmax,m=Rmin,m+1

(7)

進(jìn)行參數(shù)的合理設(shè)定,取{Nr,1,Nr,3}={4,6},{Nθ,1,Nθ,3}={30,40}。同時(shí),Rmax,3=Rmax,Rmin,1=Rmin,Rmax=80.0 m,且全局最小邊界Rmin取1.0 m來(lái)去除掉移動(dòng)平臺(tái)對(duì)點(diǎn)云數(shù)據(jù)的影響。

同時(shí)Z2中區(qū)的網(wǎng)格尺寸根據(jù)實(shí)時(shí)點(diǎn)云密度進(jìn)行自適應(yīng)調(diào)整,其中整數(shù)Nr,2和Nθ,2變化范圍分別為[6,8]和[36,60]。改進(jìn)的探測(cè)區(qū)模型如圖3b所示。

與R-GPF方法的探測(cè)網(wǎng)格劃分方式相比,改進(jìn)的探測(cè)區(qū)模型中,Z1內(nèi)區(qū)和Z3外區(qū)的網(wǎng)格更大,可以有效地解決外側(cè)的稀疏性問(wèn)題,同時(shí)可以實(shí)現(xiàn)對(duì)原點(diǎn)周圍地平面法向量的穩(wěn)健估計(jì),加以Z2中區(qū)的自適應(yīng)調(diào)整策略,最終有助于提高地面分割的效果;此外,探測(cè)區(qū)模型網(wǎng)格的數(shù)量減少到576～840個(gè),有助于里程計(jì)的高頻輸出。

1.2.3地面點(diǎn)似然估計(jì)

R-GPF地面分割方法將點(diǎn)云分配至各個(gè)網(wǎng)格對(duì)應(yīng)的空間后,擬合出該空間內(nèi)符合設(shè)定條件的地平面,進(jìn)而得到分割的地面點(diǎn)。代替常用的平面擬合方法RANSAC[16],R-GPF方法使用了主成分分析PCA[17]方法,雖然RANSAC對(duì)異常值的敏感性往往較低,但PCA的處理速度相較RANSAC更快,并能達(dá)到可接受的擬合效果[18]。

R-GPF方法獲取地面點(diǎn)的步驟如下:①任一網(wǎng)格空間內(nèi)高度最低的點(diǎn)最有可能屬于地面點(diǎn)[19],選擇最低高度的點(diǎn)作為初始種子,根據(jù)高度閾值得到初始估計(jì)地面點(diǎn)。②結(jié)合平面距離閾值,對(duì)初始估計(jì)地面點(diǎn)進(jìn)行多次PCA平面擬合迭代,獲取最終估計(jì)地面點(diǎn),設(shè)定迭代次數(shù)為3次[20]。

(8)

同時(shí)假設(shè)每個(gè)網(wǎng)格的對(duì)應(yīng)空間都相互獨(dú)立。

(9)

σ(u3,n)=

(10)

式中z——單位豎直向量

case(·)——條件判斷函數(shù)

在式(10)中,case(·)滿足條件時(shí)返回1,否則返回0。一般情況下激光雷達(dá)周圍的地平面相較參考X-Y平面變化不大,但較遠(yuǎn)處的地面可能為陡峭的坡面,故水平度閾值φ2的設(shè)定應(yīng)更加保守,故取φ1=30°,φ2=45°。

(2)高度:經(jīng)水平度評(píng)價(jià)函數(shù)評(píng)估后,一些FPs如較為豎直的果樹(shù)樹(shù)干和灌木側(cè)面已經(jīng)被剔除,但灌木等物體的平緩頂端未被過(guò)濾,故針對(duì)此類FPs提出高度評(píng)價(jià)函數(shù)為

(11)

因此,最終估計(jì)的地面點(diǎn)可以直接表示為

(12)

1.2.4改進(jìn)R-GPF地面分割方法的有效性驗(yàn)證

驗(yàn)證改進(jìn)R-GPF地面分割方法的有效性,在語(yǔ)義KITTI數(shù)據(jù)集00序列第5幀附近的地面估計(jì)結(jié)果如圖4所示。由圖4可以看到,原始R-GPF方法錯(cuò)誤地將一些較為豎直的樹(shù)干、灌木側(cè)面和墻體識(shí)別為地面點(diǎn),同時(shí)灌木和汽車的平緩頂端也未能正確分割,而改進(jìn)R-GPF方法成功地過(guò)濾掉了錯(cuò)誤估計(jì)的地面部分,從而顯著地減少了FPs,驗(yàn)證了較為豎直的樹(shù)干、灌木側(cè)面和墻體以及灌木和汽車的平緩頂端可以分別被水平度和高度過(guò)濾器過(guò)濾。

圖4 在語(yǔ)義KITTI數(shù)據(jù)集00序列第5幀附近的地面分割結(jié)果

比較基于原始R-GPF地面分割方法的激光雷達(dá)里程計(jì)和基于改進(jìn)R-GPF地面分割方法的激光雷達(dá)里程計(jì)的測(cè)程效果,直觀比較如圖5所示,具體數(shù)據(jù)如圖6和表1所示?？梢悦黠@看出,本文提出的改進(jìn)激光雷達(dá)里程計(jì)顯示出更好的性能,在整個(gè)過(guò)程中都保持較小且穩(wěn)定的絕對(duì)誤差產(chǎn)出,同時(shí)可以得出,精確的地面分割可以有效提高激光雷達(dá)里程計(jì)的性能。

表1 絕對(duì)誤差對(duì)應(yīng)數(shù)據(jù)

圖5 在語(yǔ)義KITTI數(shù)據(jù)集00序列上的測(cè)程結(jié)果與實(shí)際軌跡的絕對(duì)誤差

圖6 絕對(duì)誤差對(duì)應(yīng)數(shù)據(jù)圖

1.3 BAT啟發(fā)式自適應(yīng)重采樣方法

Gmapping算法由于其后端較為粗暴的重采樣方式和重采樣過(guò)程中對(duì)粒子收斂程度的較少干預(yù)分別導(dǎo)致其粒子的多樣性表述能力和定位準(zhǔn)確性難以適應(yīng)果園等大場(chǎng)景下的環(huán)境地圖構(gòu)建。受蝙蝠尋找獵物行為啟發(fā)本文提出了一種重采樣方法來(lái)解決這兩個(gè)問(wèn)題從而實(shí)現(xiàn)在果園等大場(chǎng)景下環(huán)境地圖的精準(zhǔn)構(gòu)建。

BAT啟發(fā)式自適應(yīng)重采樣方法的流程概括為圖7,rand為在[0,1]范圍內(nèi)均勻分布的隨機(jī)數(shù)[21]。首先篩選權(quán)重最高的粒子作為最佳粒子,再自適應(yīng)生成重采樣粒子集并將每個(gè)粒子對(duì)應(yīng)為蝙蝠,最佳粒子對(duì)應(yīng)為目標(biāo)獵物,每個(gè)蝙蝠隨機(jī)提取固定頻率,并將其定位和分配速度,根據(jù)其與目標(biāo)獵物的接近程度自動(dòng)調(diào)整脈沖的發(fā)射率和響度來(lái)搜索獵物,并不斷靠近獵物,同時(shí)不斷判斷是否到達(dá)自適應(yīng)調(diào)整的迭代時(shí)間,以生成最終定位信息。

圖7 BAT啟發(fā)式自適應(yīng)重采樣流程圖

1.3.1重采樣粒子集的自適應(yīng)生成

執(zhí)行完后端優(yōu)化的權(quán)重歸一化處理步驟后,對(duì)整個(gè)粒子群進(jìn)行權(quán)重差異性評(píng)估,判斷當(dāng)前是否需要執(zhí)行重采樣步驟,粒子群權(quán)重差異性指標(biāo)Neff的計(jì)算公式為

(13)

如果權(quán)重差異性指標(biāo)計(jì)算值大于設(shè)定重采樣閾值,說(shuō)明差異性很小,不需要執(zhí)行重采樣步驟;反之則表明差異性較大,易造成定位不準(zhǔn),需執(zhí)行自適應(yīng)重采樣步驟。

相比于權(quán)重較大的粒子,權(quán)重較小的粒子并不能很好地反映真實(shí)的后驗(yàn)分布,并且由于部分高權(quán)重粒子已經(jīng)接近于真實(shí)狀態(tài),并不需要過(guò)度優(yōu)化。故為提高整個(gè)粒子群的定位準(zhǔn)確性并保證算法的實(shí)時(shí)性,該改進(jìn)算法的重采樣步驟僅作用于權(quán)重較小的部分粒子。

根據(jù)粒子權(quán)重將整個(gè)粒子群進(jìn)行降序排序,篩選權(quán)重最高的粒子作為最佳粒子,其次設(shè)定重采樣粒子集自適應(yīng)生成閾值為粒子群權(quán)重差異性指標(biāo)計(jì)算值向下舍入得到的最近整數(shù)。將集合中粒子次序大于自適應(yīng)生成閾值的粒子放入新生成的粒子集中。該方法可以根據(jù)粒子群權(quán)重差異性程度來(lái)自適應(yīng)調(diào)整重采樣粒子集的大小來(lái)獲得更合理的重采樣粒子覆蓋范圍。

1.3.2迭代時(shí)間的自適應(yīng)調(diào)整

由重采樣粒子集的自適應(yīng)生成過(guò)程可知需要重新采樣的粒子已經(jīng)接近于真實(shí)狀態(tài),所以重采樣不需要過(guò)長(zhǎng)時(shí)間的迭代優(yōu)化,過(guò)長(zhǎng)時(shí)間的迭代優(yōu)化會(huì)導(dǎo)致粒子的退化[22],故應(yīng)根據(jù)重采樣前作業(yè)平臺(tái)預(yù)估位姿粒子與最佳粒子之間的實(shí)際差異程度指標(biāo)dt來(lái)自適應(yīng)調(diào)整迭代時(shí)間

(14)

zt——全局最佳粒子對(duì)應(yīng)的測(cè)量值

同時(shí)結(jié)合重采樣粒子集自適應(yīng)生成閾值,提出了一種能夠在重采樣步驟中自適應(yīng)調(diào)整粒子優(yōu)化迭代時(shí)間的策略

(15)

式中T——迭代時(shí)間Tmin——迭代時(shí)間下限

Tmax——迭代時(shí)間上限

l——自適應(yīng)生成閾值

基于以上分析,BAT啟發(fā)式自適應(yīng)重采樣方法可以在保證粒子多樣性的前提下獲得更好的粒子群定位效果,即獲得更準(zhǔn)確的最終定位信息來(lái)實(shí)現(xiàn)果園等大場(chǎng)景地圖的精準(zhǔn)構(gòu)建。

2 果園環(huán)境地圖構(gòu)建試驗(yàn)

2.1 試驗(yàn)環(huán)境及設(shè)備

試驗(yàn)在江蘇省農(nóng)業(yè)科學(xué)院梨園進(jìn)行,由于處于堆肥時(shí)期故選取可進(jìn)行試驗(yàn)的3行梨樹(shù),梨樹(shù)行間距為 6 m 左右,梨樹(shù)和棚架立柱交叉分布,間距為1.5 m左右,梨樹(shù)單行長(zhǎng)25 m左右,試驗(yàn)環(huán)境如圖8所示。

圖8 梨園試驗(yàn)環(huán)境

試驗(yàn)采用Velodyne公司的VLP-16型激光雷達(dá)進(jìn)行果園環(huán)境信息采集,數(shù)據(jù)的處理與分析使用Dongtintech公司的DTB-3049-H310型工控機(jī),處理器為i7-9700,16 GB內(nèi)存,500 GB固態(tài)硬盤,無(wú)顯卡。

2.2 地面分割及里程計(jì)測(cè)程試驗(yàn)

首先進(jìn)行地面分割的試驗(yàn),試驗(yàn)結(jié)果如圖9所示?？梢钥吹皆糝-GPF方法錯(cuò)誤地將果樹(shù)樹(shù)干、棚架的豎直立柱部分和遠(yuǎn)處的豎直墻體識(shí)別為地面點(diǎn),而改進(jìn)R-GPF方法在避免以上問(wèn)題的同時(shí)獲得了更好的空曠地面分割效果,故改進(jìn)R-GPF方法擁有更好的地面分割效果。

圖9 梨園環(huán)境下地面分割結(jié)果

再在梨園中進(jìn)行前端里程計(jì)試驗(yàn),基于原始R-GPF方法的激光雷達(dá)里程計(jì)和基于改進(jìn)R-GPF方法的激光雷達(dá)里程計(jì)的輸出結(jié)果與實(shí)際軌跡的對(duì)比如圖10所示,進(jìn)行調(diào)頭和彎曲行進(jìn)的目的是檢測(cè)里程計(jì)在場(chǎng)景變化較大的情況下的魯棒性。針對(duì)實(shí)際軌跡的獲取,由于果園上方存在金屬棚架導(dǎo)致GPS信號(hào)接收較差,影響實(shí)際定位效果,故通過(guò)直接測(cè)量獲取實(shí)際軌跡:沿激光雷達(dá)前進(jìn)方向每隔50 cm左右選取一測(cè)量點(diǎn),該測(cè)量點(diǎn)即為激光雷達(dá)實(shí)際位置,設(shè)定初始測(cè)量點(diǎn)為全局坐標(biāo)系原點(diǎn),通過(guò)卷尺測(cè)量初始測(cè)量點(diǎn)與下一測(cè)量點(diǎn)的相對(duì)坐標(biāo)即可獲得下一測(cè)量點(diǎn)的全局坐標(biāo),按照該方法遍歷剩余測(cè)量點(diǎn)來(lái)獲得所有測(cè)量點(diǎn)的全局坐標(biāo)便可得到實(shí)際軌跡;再分析該實(shí)際軌跡的精度,每4個(gè)測(cè)量點(diǎn)選取一點(diǎn)直接測(cè)量該點(diǎn)在全局坐標(biāo)系下的具體坐標(biāo),并計(jì)算該坐標(biāo)與之前遍歷所獲得坐標(biāo)的偏差,所有所選測(cè)量點(diǎn)在x和y方向上的偏差不超過(guò)7 cm,故可通過(guò)以上方法獲取實(shí)際軌跡。相比于原始R-GPF方法的激光雷達(dá)里程計(jì)的平均輸出頻率10.40 Hz,改進(jìn)R-GPF方法的平均輸出頻率可達(dá)到15.58 Hz,滿足前端里程計(jì)的輸出頻率要求。

圖10 梨園環(huán)境下里程計(jì)輸出結(jié)果

再通過(guò)計(jì)算兩種里程計(jì)的輸出軌跡和實(shí)際軌跡的橫向偏差來(lái)評(píng)價(jià)其測(cè)程性能,如圖11所示,設(shè)定里程計(jì)輸出軌跡在實(shí)際軌跡左側(cè)時(shí)橫向偏差為正值,在右側(cè)時(shí)為負(fù)值。計(jì)算橫向偏差的相關(guān)數(shù)據(jù),原始R-GPF方法所對(duì)應(yīng)的最大橫向偏差接近50 cm,橫向偏差均值為22.5 cm,標(biāo)準(zhǔn)差為24.7 cm;改進(jìn)R-GPF方法所對(duì)應(yīng)的最大橫向偏差小于25 cm,橫向偏差均值為12.7 cm,標(biāo)準(zhǔn)差為13.4 cm?？梢悦黠@看出,本文提出的改進(jìn)激光雷達(dá)里程計(jì)在整個(gè)過(guò)程中都保持著較小且較為穩(wěn)定的橫向偏差,而原始激光雷達(dá)里程計(jì)的橫向偏差變化幅度大且一直處于較高位。綜合分析基于改進(jìn)R-GPF方法的激光雷達(dá)里程計(jì)在更高頻率的輸出情況下仍能獲得更好的測(cè)程效果。

圖11 梨園環(huán)境下里程計(jì)橫向偏差

2.3 地圖構(gòu)建試驗(yàn)及BAT啟發(fā)式自適應(yīng)重采樣方法有效性分析

由于原始Gmapping算法前端里程計(jì)采用的ICP方法累計(jì)誤差大且果園環(huán)境復(fù)雜、場(chǎng)景大,所以原始Gmapping算法的前端里程計(jì)表現(xiàn)效果很差,故最終建圖效果對(duì)比在基于原始R-GPF的Gmapping算法和基于改進(jìn)Gmapping算法兩者中進(jìn)行,最終兩種算法的建圖效果如圖12所示。由于梨園兩側(cè)地頭分別為墻體和較寬的空曠道路,中間部分為3行交叉分布的梨樹(shù)和棚架立柱,故圖12最左側(cè)為構(gòu)建的墻體,最右側(cè)無(wú)構(gòu)建內(nèi)容,中間部分為構(gòu)建的梨樹(shù)和棚架立柱。

圖12 梨園環(huán)境下建圖效果

先進(jìn)行地圖構(gòu)建偏差的計(jì)算和對(duì)比,從每行的最右側(cè)果樹(shù)或棚架立柱開(kāi)始分別向左計(jì)算列間距離偏差和向下計(jì)算行間距離偏差,最終共遍歷51棵果樹(shù)和棚架立柱,建圖偏差對(duì)比如圖13所示?？梢钥吹皆嫉貓D構(gòu)建算法所得到的梨樹(shù)列間距離偏差在40 cm范圍內(nèi),行間距離偏差均值為18.2 cm,標(biāo)準(zhǔn)差為9.3 cm。相比于原始地圖構(gòu)建算法波動(dòng)較大的為列間距離偏差,改進(jìn)地圖構(gòu)建算法的列間距離偏差始終保持在20 cm范圍內(nèi),表現(xiàn)較好;同時(shí)在行間距離偏差方面改進(jìn)地圖構(gòu)建算法表現(xiàn)更好,行間距離偏差均值為10.3 cm,標(biāo)準(zhǔn)差為6.3 cm;可以看到相比于原始地圖構(gòu)建算法,改進(jìn)地圖構(gòu)建算法在列間距離偏差、行間距離偏差和行間距離偏差的標(biāo)準(zhǔn)差3個(gè)方面分別減小了50%、43.41%和32.26%;一般情況下地圖更新步驟會(huì)進(jìn)一步增加地圖構(gòu)建的誤差,在此前提下改進(jìn)地圖構(gòu)建算法的行間距離偏差的各項(xiàng)數(shù)據(jù)相比于里程計(jì)橫向偏差的各項(xiàng)數(shù)據(jù)仍有進(jìn)一步減小,這也可以從側(cè)面反映出后端BAT啟發(fā)式自適應(yīng)重采樣方法有效實(shí)現(xiàn)了對(duì)初始定位信息的進(jìn)一步優(yōu)化。

圖13 梨園環(huán)境下建圖偏差對(duì)比

再結(jié)合圖12分析整體建圖效果,當(dāng)作業(yè)平臺(tái)從第2、3行間沿逆時(shí)針?lè)较蛘{(diào)頭來(lái)到第1、2行間時(shí),場(chǎng)景變化較大,而原始地圖構(gòu)建算法地面分割魯棒性較差導(dǎo)致前端里程計(jì)輸出誤差較大,且其后端優(yōu)化部分對(duì)定位情況的再優(yōu)化效果不明顯導(dǎo)致左側(cè)墻壁出現(xiàn)重疊和偏移,最終導(dǎo)致第1行出現(xiàn)較大偏移,第2行右側(cè)出現(xiàn)重疊,建圖效果較差;改進(jìn)地圖構(gòu)建算法憑借精確和穩(wěn)健的前端激光雷達(dá)里程計(jì)和后端優(yōu)化重采樣算法對(duì)定位的再優(yōu)化獲得較好的建圖效果,可以滿足后續(xù)重定位、導(dǎo)航等作業(yè)的精度要求。

3 結(jié)論

(1)為了獲得高精度果園二維環(huán)境地圖,首先通過(guò)改進(jìn)R-GPF方法實(shí)現(xiàn)準(zhǔn)確和快速的地面分割,進(jìn)而獲得高精度高頻率的前端激光雷達(dá)里程計(jì)信息,再在后端優(yōu)化部分提出了一種BAT啟發(fā)式自適應(yīng)重采樣算法對(duì)激光雷達(dá)里程計(jì)信息進(jìn)行合理程度的再優(yōu)化獲得更優(yōu)的定位信息,最終獲得精準(zhǔn)的果園二維環(huán)境地圖。

(2)在梨園中進(jìn)行前端里程計(jì)試驗(yàn),基于改進(jìn)R-GPF方法的激光雷達(dá)里程計(jì)的平均輸出頻率可達(dá)到15.58 Hz,其對(duì)應(yīng)的最大橫向偏差小于25 cm,橫向偏差均值為12.7 cm,標(biāo)準(zhǔn)差為13.4 cm,比基于原始R-GPF方法的激光雷達(dá)里程計(jì)表現(xiàn)更好,并且滿足前端里程計(jì)的輸出頻率和精度要求。進(jìn)行梨園環(huán)境地圖構(gòu)建,改進(jìn)地圖構(gòu)建算法表現(xiàn)更好:列間距離偏差方面表現(xiàn)良好,始終保持在20 cm范圍內(nèi),在行間距離偏差方面表現(xiàn)更優(yōu)秀,行間距離偏差均值為10.3 cm,標(biāo)準(zhǔn)差為6.3 cm,建圖精度可以滿足后續(xù)重定位、導(dǎo)航等作業(yè)的精度要求,同時(shí)行間距離偏差側(cè)面反映出后端BAT啟發(fā)式自適應(yīng)重采樣方法的有效性。