基于改進Gmapping算法的果園二維環境地圖精準構建

薛金林 王培曉 周 俊 程 峰

(南京農業大學工學院, 南京 210031)

0 引言

果園作業機器人的定位與導航技術研究主要集中于基于SLAM算法的果園環境地圖構建研究,果園環境地圖能否實現精準構建直接影響后續定位與導航作業的效果。

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

本文對Gmapping算法進行改進,前端里程計以LeGO-LOAM測程框架為底層,針對果園實際環境對R-GPF方法的探測區模型和地面點估計方法進行相應改進,提高果園內地面分割效果,進而獲得高精高頻的激光雷達里程計。后端優化部分采用一種受蝙蝠尋找獵物行為啟發的自適應重采樣方法實現對初始定位信息的有效和合理程度的優化,以提高最終的定位精度,從而獲得精準的果園環境地圖。

1 精準果園環境地圖構建方法

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

圖1 精準地圖構建算法框架

1.1 精準地圖構建算法整體流程

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

(1)

再進行權重歸一化處理

(2)

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

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

1.2 基于改進R-GPF地面分割方法的激光雷達里程計

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

圖2 激光雷達里程計框架

1.2.1地面分割問題定義

(3)

1.2.2探測區模型

LeGO-LOAM使用激光雷達位于不同水平分辨率上同一線上任意兩點之間的角度差來提取地面,它往往對某些噪聲很敏感,如崎嶇不平的地面,草坪或灌木叢也會阻礙地面分割,因為它們的形狀梯度是任意的。鑒于基于線的地面分割方法易受干擾,因此提出了魯棒的基于多平面的R-GPF地面分割方法,該方法以激光雷達為坐標原點建立二維圓形探測網格,再根據點云的三維坐標進行空間上點云的多平面分布。

R-GPF方法的探測網格M沿徑向和周向進行規則劃分,最終形成環區和扇區,其中Nr和Nθ分別是環數和扇區的數目,這樣M最終將分為徑向距離為Rmax/Nr,對應軸向角為2π/Nθ的Nr×Nθ個網格,其中Rmax為最大邊界,探測網格M劃分結果如圖3a所示。

圖3 探測區模型示意圖

然而在語義KITTI數據集上對所有序列的各幀點云數據進行測量的實驗數據[14]表明,有90%以上的地面點位于激光雷達原點附近20 m范圍內。因此,R-GPF方法的探測網格M的劃分方式面臨以下兩個問題:①當點云距離坐標原點過遠時,對應網格內的點云過于稀疏,因此難以擬合出地平面。②由于靠近坐標原點的網格太小并且原點附近點云過于密集,這種情況有時會導致地平面的法向向量估計失敗[15]。

(4)

其中

θk=arctan2(yk,xk)

(5)

ΔRm=Rmax,m-Rmin,m

(6)

Rmax,m=Rmin,m+1

(7)

進行參數的合理設定,取{Nr,1,Nr,3}={4,6},{Nθ,1,Nθ,3}={30,40}。同時,Rmax,3=Rmax,Rmin,1=Rmin,Rmax=80.0 m,且全局最小邊界Rmin取1.0 m來去除掉移動平臺對點云數據的影響。

同時Z2中區的網格尺寸根據實時點云密度進行自適應調整,其中整數Nr,2和Nθ,2變化范圍分別為[6,8]和[36,60]。改進的探測區模型如圖3b所示。

與R-GPF方法的探測網格劃分方式相比,改進的探測區模型中,Z1內區和Z3外區的網格更大,可以有效地解決外側的稀疏性問題,同時可以實現對原點周圍地平面法向量的穩健估計,加以Z2中區的自適應調整策略,最終有助于提高地面分割的效果;此外,探測區模型網格的數量減少到576～840個,有助于里程計的高頻輸出。

1.2.3地面點似然估計

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

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

(8)

同時假設每個網格的對應空間都相互獨立。

(9)

σ(u3,n)=

(10)

式中z——單位豎直向量

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

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

(2)高度:經水平度評價函數評估后,一些FPs如較為豎直的果樹樹干和灌木側面已經被剔除,但灌木等物體的平緩頂端未被過濾,故針對此類FPs提出高度評價函數為

(11)

因此,最終估計的地面點可以直接表示為

(12)

1.2.4改進R-GPF地面分割方法的有效性驗證

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

圖4 在語義KITTI數據集00序列第5幀附近的地面分割結果

比較基于原始R-GPF地面分割方法的激光雷達里程計和基于改進R-GPF地面分割方法的激光雷達里程計的測程效果,直觀比較如圖5所示,具體數據如圖6和表1所示。可以明顯看出,本文提出的改進激光雷達里程計顯示出更好的性能,在整個過程中都保持較小且穩定的絕對誤差產出,同時可以得出,精確的地面分割可以有效提高激光雷達里程計的性能。

表1 絕對誤差對應數據

圖5 在語義KITTI數據集00序列上的測程結果與實際軌跡的絕對誤差

圖6 絕對誤差對應數據圖

1.3 BAT啟發式自適應重采樣方法

Gmapping算法由于其后端較為粗暴的重采樣方式和重采樣過程中對粒子收斂程度的較少干預分別導致其粒子的多樣性表述能力和定位準確性難以適應果園等大場景下的環境地圖構建。受蝙蝠尋找獵物行為啟發本文提出了一種重采樣方法來解決這兩個問題從而實現在果園等大場景下環境地圖的精準構建。

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

圖7 BAT啟發式自適應重采樣流程圖

1.3.1重采樣粒子集的自適應生成

執行完后端優化的權重歸一化處理步驟后,對整個粒子群進行權重差異性評估,判斷當前是否需要執行重采樣步驟,粒子群權重差異性指標Neff的計算公式為

(13)

如果權重差異性指標計算值大于設定重采樣閾值,說明差異性很小,不需要執行重采樣步驟;反之則表明差異性較大,易造成定位不準,需執行自適應重采樣步驟。

相比于權重較大的粒子,權重較小的粒子并不能很好地反映真實的后驗分布,并且由于部分高權重粒子已經接近于真實狀態,并不需要過度優化。故為提高整個粒子群的定位準確性并保證算法的實時性,該改進算法的重采樣步驟僅作用于權重較小的部分粒子。

根據粒子權重將整個粒子群進行降序排序,篩選權重最高的粒子作為最佳粒子,其次設定重采樣粒子集自適應生成閾值為粒子群權重差異性指標計算值向下舍入得到的最近整數。將集合中粒子次序大于自適應生成閾值的粒子放入新生成的粒子集中。該方法可以根據粒子群權重差異性程度來自適應調整重采樣粒子集的大小來獲得更合理的重采樣粒子覆蓋范圍。

1.3.2迭代時間的自適應調整

由重采樣粒子集的自適應生成過程可知需要重新采樣的粒子已經接近于真實狀態,所以重采樣不需要過長時間的迭代優化,過長時間的迭代優化會導致粒子的退化[22],故應根據重采樣前作業平臺預估位姿粒子與最佳粒子之間的實際差異程度指標dt來自適應調整迭代時間

(14)

zt——全局最佳粒子對應的測量值

同時結合重采樣粒子集自適應生成閾值,提出了一種能夠在重采樣步驟中自適應調整粒子優化迭代時間的策略

(15)

式中T——迭代時間Tmin——迭代時間下限

Tmax——迭代時間上限

l——自適應生成閾值

基于以上分析,BAT啟發式自適應重采樣方法可以在保證粒子多樣性的前提下獲得更好的粒子群定位效果,即獲得更準確的最終定位信息來實現果園等大場景地圖的精準構建。

2 果園環境地圖構建試驗

2.1 試驗環境及設備

試驗在江蘇省農業科學院梨園進行,由于處于堆肥時期故選取可進行試驗的3行梨樹,梨樹行間距為 6 m 左右,梨樹和棚架立柱交叉分布,間距為1.5 m左右,梨樹單行長25 m左右,試驗環境如圖8所示。

圖8 梨園試驗環境

試驗采用Velodyne公司的VLP-16型激光雷達進行果園環境信息采集,數據的處理與分析使用Dongtintech公司的DTB-3049-H310型工控機,處理器為i7-9700,16 GB內存,500 GB固態硬盤,無顯卡。

2.2 地面分割及里程計測程試驗

首先進行地面分割的試驗,試驗結果如圖9所示。可以看到原始R-GPF方法錯誤地將果樹樹干、棚架的豎直立柱部分和遠處的豎直墻體識別為地面點,而改進R-GPF方法在避免以上問題的同時獲得了更好的空曠地面分割效果,故改進R-GPF方法擁有更好的地面分割效果。

圖9 梨園環境下地面分割結果

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

圖10 梨園環境下里程計輸出結果

再通過計算兩種里程計的輸出軌跡和實際軌跡的橫向偏差來評價其測程性能,如圖11所示,設定里程計輸出軌跡在實際軌跡左側時橫向偏差為正值,在右側時為負值。計算橫向偏差的相關數據,原始R-GPF方法所對應的最大橫向偏差接近50 cm,橫向偏差均值為22.5 cm,標準差為24.7 cm;改進R-GPF方法所對應的最大橫向偏差小于25 cm,橫向偏差均值為12.7 cm,標準差為13.4 cm。可以明顯看出,本文提出的改進激光雷達里程計在整個過程中都保持著較小且較為穩定的橫向偏差,而原始激光雷達里程計的橫向偏差變化幅度大且一直處于較高位。綜合分析基于改進R-GPF方法的激光雷達里程計在更高頻率的輸出情況下仍能獲得更好的測程效果。

圖11 梨園環境下里程計橫向偏差

2.3 地圖構建試驗及BAT啟發式自適應重采樣方法有效性分析

由于原始Gmapping算法前端里程計采用的ICP方法累計誤差大且果園環境復雜、場景大,所以原始Gmapping算法的前端里程計表現效果很差,故最終建圖效果對比在基于原始R-GPF的Gmapping算法和基于改進Gmapping算法兩者中進行,最終兩種算法的建圖效果如圖12所示。由于梨園兩側地頭分別為墻體和較寬的空曠道路,中間部分為3行交叉分布的梨樹和棚架立柱,故圖12最左側為構建的墻體,最右側無構建內容,中間部分為構建的梨樹和棚架立柱。

圖12 梨園環境下建圖效果

先進行地圖構建偏差的計算和對比,從每行的最右側果樹或棚架立柱開始分別向左計算列間距離偏差和向下計算行間距離偏差,最終共遍歷51棵果樹和棚架立柱,建圖偏差對比如圖13所示。可以看到原始地圖構建算法所得到的梨樹列間距離偏差在40 cm范圍內,行間距離偏差均值為18.2 cm,標準差為9.3 cm。相比于原始地圖構建算法波動較大的為列間距離偏差,改進地圖構建算法的列間距離偏差始終保持在20 cm范圍內,表現較好;同時在行間距離偏差方面改進地圖構建算法表現更好,行間距離偏差均值為10.3 cm,標準差為6.3 cm;可以看到相比于原始地圖構建算法,改進地圖構建算法在列間距離偏差、行間距離偏差和行間距離偏差的標準差3個方面分別減小了50%、43.41%和32.26%;一般情況下地圖更新步驟會進一步增加地圖構建的誤差,在此前提下改進地圖構建算法的行間距離偏差的各項數據相比于里程計橫向偏差的各項數據仍有進一步減小,這也可以從側面反映出后端BAT啟發式自適應重采樣方法有效實現了對初始定位信息的進一步優化。

圖13 梨園環境下建圖偏差對比

再結合圖12分析整體建圖效果,當作業平臺從第2、3行間沿逆時針方向調頭來到第1、2行間時,場景變化較大,而原始地圖構建算法地面分割魯棒性較差導致前端里程計輸出誤差較大,且其后端優化部分對定位情況的再優化效果不明顯導致左側墻壁出現重疊和偏移,最終導致第1行出現較大偏移,第2行右側出現重疊,建圖效果較差;改進地圖構建算法憑借精確和穩健的前端激光雷達里程計和后端優化重采樣算法對定位的再優化獲得較好的建圖效果,可以滿足后續重定位、導航等作業的精度要求。

3 結論

(1)為了獲得高精度果園二維環境地圖,首先通過改進R-GPF方法實現準確和快速的地面分割,進而獲得高精度高頻率的前端激光雷達里程計信息,再在后端優化部分提出了一種BAT啟發式自適應重采樣算法對激光雷達里程計信息進行合理程度的再優化獲得更優的定位信息,最終獲得精準的果園二維環境地圖。

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