基于激光雷達的農業(yè)機器人果園樹干檢測算法

牛潤新 張向陽 王 杰 祝 輝 黃 健 陳正偉

(1.中國科學院合肥物質科學研究院， 合肥 230088； 2.中國科學技術大學， 合肥 230026)

0 引言

隨著我國農業(yè)機械自動化水平的不斷提高，農業(yè)機器人相關領域的研究也越來越受到重視[1-2]。果園生產管理過程中的耕作[3]、修剪[4]、施藥[5-6]、除草[7]、采摘[8-9]等活動都離不開農業(yè)機器人對果園環(huán)境的感知[10]。機器人在果園這種非結構化環(huán)境中作業(yè)，首先要知道果樹的準確位置[11-12]。激光雷達作為一種新興的導航定位傳感器，具有精度高、實時性好、全天候等特點[13-15]，在農業(yè)機器人環(huán)境感知研究中得到了廣泛應用。

文獻[16]將二維激光雷達垂直安裝，通過設定高度閾值排除樹冠點及地面點，并采用隱半馬爾可夫模型進行結果優(yōu)化，最終樹干信息提取準確率達89%，由于激光雷達一次只能提取一棵樹干信息，故檢測范圍較小，效率較低。文獻[17]將二維激光雷達水平安裝，利用果樹行間距固定的特點，通過計算激光雷達數據中的內凹點，將成等差數列的數據點視為樹干點，由于依賴行間距，故對株距不固定或果樹缺失情況下果樹檢測的適應性不佳。文獻[18]利用二維激光雷達，采用K-均值聚類算法，通過斜率變化的方法確定聚類數，從而實現對立木的識別，在3種不同樹干分布下準確率達95.5%，但試驗在室內進行，未考慮雜草及其他障礙物等干擾因素。文獻[19]通過改進的基于密度聚類(Density-based spatial clustering of application with noise, DBSCAN)算法對果園樹干進行檢測，并通過航位推算模型排除地面干擾，但算法復雜、計算量較大，且同樣未考慮雜草對果樹檢測的影響。

針對現有果樹檢測方法存在的計算量大、未考慮雜草等影響、適應性較差等問題，本文提出一種基于自適應密度聚類的樹干檢測算法。首先，將激光雷達獲取的數據點進行數據預處理，排除噪聲點及不可利用的數據點，并根據數據點到激光雷達的距離自適應設定密度閾值，進行初步聚類；然后，利用地面類中的數據點過多以及數據點之間趨于直線的特點，通過最小二乘法進行圓的擬合，并設定半徑閾值，將地面類剔除；最后，利用雜草枝葉類中的數據點之間距離不連續(xù)的特征，將雜草枝葉類剔除，完成對果園中果樹樹干的檢測。

1 研究平臺與方法

1.1 研究平臺介紹

本研究以中國科學院合肥物質科學研究院應用技術研究所自主研制的履帶式農業(yè)機器人為平臺，所用傳感器為德國SICK公司生產的LMS111型單線激光雷達，其掃描分辨率為0.5°/0.25°，掃描頻率為25/50 Hz，掃描角最大為270°，最大檢測距離為20 m，當超出最大檢測距離時，返回距離為0。激光雷達在機器人的正前方水平安裝，掃描平面距離地面約350 mm。激光雷達傳感器經Ethernet網絡接口將采集的數據傳輸至上位機，由上位機進行數據處理，最終結果通過RS-232接口發(fā)送至下位機，下位機也可通過RS-232接口接收遙控器信號，輸出脈寬調制(Pulse width modulation, PWM)信號到電動機驅動器驅動電動機，實現機器人行走，如圖1所示。

圖1 研究平臺系統(tǒng)結構圖Fig.1 System structure diagram

定義機器人的局部坐標系原點與激光雷達位置重合，y軸正方向指向機器人前進的正前方，x軸正方向與機器人前進方向垂直向右。二維激光雷達采集的數據點qi以極坐標(di,θi)的形式表示，為了方便計算，將極坐標轉換為笛卡爾坐標形式(xi,yi,θi)，計算公式為

(1)

式中di——數據點qi到激光雷達的距離

θi——數據點qi和激光雷達的連線與x軸正方向的夾角

在激光雷達返回的數據點中，任意2個數據點qi和qj之間的距離di,j為

(2)

1.2 數據預處理

激光雷達在采集數據過程中，發(fā)射器的光束通常會經過多次反射和折射，為了降低干擾，激光雷達拒收超過一個波長時間段的反射光，但在結果中仍然會包含一些孤立噪聲點[20-21]，這些噪聲點并不是真實測得的數據信息，它們不僅會對激光雷達數據處理造成一定程度的干擾，而且會增加數據處理的時間，因此在數據處理之前應將其濾除。另外隨著測量距離的增加，激光雷達數據點變得稀疏，所能提供可利用的有效信息變少，參考意義不大，在數據處理之前也應將其濾除。

針對以上問題，在獲取激光雷達數據后，選取機器人正前方180°的數據點并進行遍歷，將距離大于10 m的數據點濾除，然后在保留的數據中分別計算當前數據點與相鄰數據點之間的距離，并將與左右相鄰數據點距離都較遠的數據視為無效噪聲點，也將其從原始數據中濾除。如圖2所示，矩形框中的數據點距離激光雷達較遠，無法提供可利用的信息，視為無效點；圓形框標記的數據點與相鄰的數據點均較遠，視為噪聲點。

圖2 數據濾波示意圖Fig.2 Data filtering diagram

1.3 基于距離的自適應密度聚類

常規(guī)密度聚類算法DBSCAN可以有效地尋找到被低密度區(qū)域分割的高密度區(qū)域，在具有噪聲的數據中發(fā)現任意形狀的簇，但需提前確定聚類半徑和鄰域內點的數量2個全局參數，且聚類效果對其設定的全局參數值較為敏感[22-23]。由二維激光雷達的基本原理[24-25]可知，激光雷達數據點的密度隨數據點到激光雷達的距離增加而降低?？紤]到果園中雜草和枝葉干擾的情況，若直接使用DBSCAN 聚類算法，當檢測距離較近時，數據點密度較高，聚類半徑相同的情況下，會存在距離較近且較細的雜草枝葉與距離較遠且直徑較大的樹干相比接收到的數據點數量接近甚至更多的現象，從而出現大量誤判與漏檢。因此，為得到更好的聚類效果，提出一種基于自適應密度聚類算法。

為提取出樹干信息，以樹干為主要目標設定聚類半徑，設果園中最細的果樹樹干直徑為Dmin，則聚類半徑為Dmin/2。激光雷達相鄰數據點之間的理論距離h與激光雷達的角度分辨率φ及數據點到激光雷達的距離d之間的關系為

h=dtanφ

(3)

則理論上，以Dmin/2為聚類半徑的圓內所接收到數據點的個數n為：n=Dmin/h=Dmin/(dtanφ),考慮到樹干的橫截面為圓形，則聚類的數據點數目閾值設為

(4)

當以數據點qi為中心，以Dmin/2為半徑的圓內數據點的數目ni≥Nmin時，則該點為核心點，其周圍的點為邊界點。從激光雷達第1個數據點開始依次遍歷所有數據點，找出所有的核心點。由于激光雷達數據點在一個平面內是按逆時針順序展開，為了加快搜索速度，提高計算效率，尋找核心點時只需要計算當前數據點與其一定范圍內的數據點的距離即可。搜索半徑為Dmin/2，則計算與該點左右相鄰的點的個數為：mi=Dmin/(2h)=Dmin/(2ditanφ)，計算出所有核心點后，隨機選取一個核心點初始化隊列Q，并取出隊列中的數據點q，如果數據點q為核心點，則將以該點為核心的邊界點加入到隊列Q中，依次取出隊列Q中的數據點直到隊列Q為空，此時，所有訪問過的數據點生成一個新的聚類簇。然后隨機取出另一未訪問過的核心點初始化隊列Q繼續(xù)計算并生成新的類簇，直到所有的核心點均被訪問,完成數據的初步聚類。

1.4 地面類的排除

圖3 地面聚類示意圖Fig.3 Ground clustering diagram

由于丘陵果園中存在斜坡，或者由于路面顛簸，激光雷達可能掃描到地面，從而影響果樹檢測的準確率，因此必須排除地面類的干擾。如圖3所示，當激光雷達掃描地面時，有可能將地面聚為一類，但地面類數據點具有連續(xù)、量大且大致呈一條直線的特點，利用此特點，可以將地面類排除。

根據類中第1個數據點qf和最后一個數據點qe的中心計算該類的中心點坐標

(5)

式中Gx——類中心點橫坐標

Gy——類中心點縱坐標

df——數據點qf到激光雷達的距離

de——數據點qe到激光雷達的距離

θf——數據點qf與x方向夾角

θe——數據點qe與x方向夾角

果園中果樹樹干最大直徑為Dmax，則根據類中心到激光雷達的距離dg計算類中數據點的數目最大值為nm=Dmax/h=Dmax/(dgtanφ)?？紤]到果園中地面不平整，激光雷達掃描平面與水平面可能存在夾角，此時樹干接收到的數據點變多，因此設定類內數據點數目的閾值Nm=1.5nm。如果類內數據點數量Nc≥Nm，則認為該類為地面類。如果類內數據點數量Nc

R2=(x-A)2+(y-B)2=
x2-2Ax+A2+y2-2By+B2

(6)

式中R——擬合圓半徑

A——擬合圓圓心橫坐標

B——擬合圓圓心縱坐標

令a=-2A，b=-2B，c=A2+B2-R2，得到圓曲線方程的另一種表達形式為

x2+y2+ax+by+c=0

(7)

根據最小二乘法原理，求目標函數F(a,b,c)的最小值

(8)

將F(a,b,c)對a、b、c求偏導，求得最小二乘解為

(9)

由于地面類數據點更趨向于直線，如果2R≥1.5Dmax，則認為該類為地面類，并將此類從數據中剔除。

1.5 排除雜草及枝葉的干擾

理想的果園環(huán)境中，排除地面背景后的剩余類即為果樹類，但丘陵果園環(huán)境比較復雜，常伴有少量較高的雜草和較低的果樹枝葉。盡管選擇合適的激光雷達安裝高度可減少雜草或枝葉的干擾，但卻不能完全避免，因此須考慮雜草和枝葉干擾的情況。

由于單個枝葉寬度遠小于樹干直徑，相同的聚類半徑內其返回數據點的個數必定小于聚類的閾值，因此在初次聚類的過程中已將其排除。而連續(xù)的幾個枝葉在同一平面的投影長度可能大于樹干的直徑，因此在初次聚類的過程中有可能將其聚為一類。為準確提取出樹干的位置信息，必須將雜草及枝葉類排除。觀察實際雜草及枝葉的形態(tài)可知，盡管其在同一平面的投影連續(xù)，但其縱向枝葉間存在一定間距，反映在數據中則會出現相鄰數據點之間的距離產生突變、不連續(xù)的現象(圖4)，因此可利用此特點排除雜草及枝葉類的干擾。

圖4 雜草枝葉聚類示意圖Fig.4 Schematic of weeds and foliage clustering

在初次聚類結果中依次計算相鄰數據點的實際距離，當存在相鄰2個數據點的距離大于給定的閾值時可判定此類為雜草或枝葉類。由于樹干為柱形且表面凹凸不平，設定類中相鄰數據點的距離閾值為

Ti,i+1=2h=2dmtanφ

(10)

式中dm——數據點qi和qi+1到激光雷達距離的平均值

如果類中存在2個相鄰數據點之間的距離di,i+1≥Ti,i+1，則認為該類為雜草或枝葉類。另外，對于存在個別枝葉間的縱向距均小于閾值Ti,i+1的情況，由于樹干的橫截面呈類圓形，在其表面分布的激光雷達相鄰數據點之間的斜率逆時針方向存在一次由正向負的變化，而由多個雜草或枝葉形成的類中由于枝葉間的交錯，必然存在多次相鄰數據點之間斜率正負的變化。同一類簇中相鄰數據點之間的斜率為

(11)

如果類中相鄰數據點之間的斜率正負變化次數大于等于3次，那么該類同樣被認為是雜草或枝葉類，并將此類從數據中剔除。

2 試驗與結果分析

2.1 試驗設計

為檢驗自適應密度聚類算法對于樹干檢測的效果以及利用特征點對地面和雜草等干擾排除的效果，針對四川省閬中楊氏果業(yè)生產應用示范基地中的檸檬(Citruslimon(L.)Burm.f)園中存在斜坡且樹干較為明顯的特點，以安徽省合肥市大蜀山森林公園及周邊的香樟樹園為試驗場地(由于季節(jié)和實驗條件原因，以此試驗地模擬)，樹木為香樟樹(Cinnamomumcamphora(L.) Presl)，試驗中最大坡度為20°，選擇的樹高3～5 m，枝下高1.2～2 m，株距約3 m，環(huán)境中主要的干擾為少量較高的雜草以及斜坡，雜草主要為野生的草本植物以及少量的野生小灌木。為檢驗該方法對樹干檢測的準確性以及在存在地面及雜草干擾情況下的適應性，試驗場景主要分為4種類型，分別為只存在樹干且無干擾的平整地形場景、存在樹干和斜坡的場景、存在樹干和雜草干擾的場景、存在樹干及斜坡和雜草干擾的場景。

試驗中，設置激光雷達角度分辨率為0.5°，掃描頻率為25 Hz，履帶機器人行駛速度0.2 m/s，試驗平臺及場景如圖5所示。

圖5 試驗平臺及場景Fig.5 Experiment platform and scene1.上位機 2.履帶式農業(yè)機器人 3.激光雷達

2.2 聚類及干擾排除試驗

運用基于距離的自適應密度聚類方法，在只存在樹干且無干擾的平整地形環(huán)境下進行試驗，用以檢驗在無干擾情況下算法對樹干檢測的準確率。試驗中每隔0.5 m取出一次完整數據幀，共選取50幀數據進行聚類分析。

首先進行數據預處理，將噪聲點以及不可利用的無效數據點去除，然后利用基于自適應密度聚類方法進行樹干檢測。聚類結果如圖6所示，其中圓形標記的類為真實場景中存在的樹干(下同)，統(tǒng)計聚類后結果中的樹干數目nc以及真實場景中樹干的數目nc，并計算對果樹的漏檢率與誤檢率。在選取的50幀數據中共存在真實的果樹263棵，出現誤檢的有2棵，誤檢率為0.76%，出現漏檢的有5棵，漏檢率為1.90%，無其他干擾情況下的樹干檢測平均正確率為97.3%。當數據點在較粗的樹干上出現噪聲，在數據預處理時噪聲點被濾除，樹干上的數據點被濾除的噪聲分隔開，因此樹干被聚為兩類，從而出現誤檢的現象。當激光雷達位置靠近樹行的一側且激光雷達正前方與樹行平行時，由于角度原因，接近激光雷達的樹干對遠離激光雷達的樹干進行了部分遮擋，導致被部分遮擋的樹干出現了漏檢，隨著履帶小車的移動，激光雷達與樹干之間的角度不斷變化，漏檢的現象消失。

圖6 無干擾時的聚類結果Fig.6 Clustering results without interference

2.3 地面干擾排除試驗

在存在樹干和斜坡的場景中進行試驗，選取20幀同時具有樹干和地面點的數據幀進行聚類及地面干擾的排除。圖7為聚類后地面干擾排除前的結果，其中橢圓標記的類為真實場景中的地面(下同)，圖8為地面干擾排除后的結果。在選取的20幀數據中共存在真實的果樹76棵，其中出現了3次將地面誤判為果樹的情況，在存在地面干擾情況下的正確率為96.1%，誤判是由于地面中的石塊等突然凸起的物體導致的誤檢。

圖7 只存在地面干擾時的聚類結果Fig.7 Clustering results with only ground interference

圖8 地面干擾排除后的結果Fig.8 Results after ground interference removal

2.4 雜草干擾排除試驗

在存在樹干和雜草干擾的場景中進行試驗，選取20幀同時具有樹干和雜草干擾的數據幀進行聚類及雜草干擾的排除。圖9為聚類后雜草排除前的結果，其中矩形標記的類為真實場景中的雜草(下同)，圖10為雜草排除后的結果(存在一處雜草未被排除的情況)。在選取的20幀數據中共存在真實果樹93棵，其中出現了8次將雜草誤判為果樹的情況，在存在雜草干擾情況下的正確率為91.4%，誤判是由于數據點恰好全部落在最靠近激光雷達的雜草表面以及雜草過密所造成的。

圖9 只存在雜草干擾時的聚類結果Fig.9 Clustering results with only weed interference

圖10 雜草干擾排除后的結果Fig.10 Results after weed interference removal

2.5 綜合試驗

為檢驗在同時存在地面和雜草干擾情況下樹干檢測的準確率，選取10幀同時具有地面和雜草干擾的數據幀進行干擾的排除。圖11為干擾排除前的聚類結果，圖12為干擾排除后的結果。在選取的10幀數據中共存在真實果樹37棵，其中出現了3次將地面或雜草誤判為果樹的情況，在同時存在地面和雜草干擾情況下的正確率為91.9%。

在選取的全部100幀數據中，共存在真實果樹469棵，誤判的次數共21次，綜合平均正確率為95.5%，該算法可用于地面平整度低且存在雜草干擾的丘陵果園中的樹干檢測。

圖11 存在雜草和地面干擾時的聚類結果Fig.11 Clustering results in presence of weeds and ground interference

圖12 地面和雜草干擾排除后的結果Fig.12 Results after ground and weed interference removal

3 結論

(1)主要分析了在存在斜坡及雜草干擾的環(huán)境中，利用二維激光雷達獲取環(huán)境信息、并基于自適應密度聚類的樹干檢測算法。數據預處理中，在排除噪聲點及無法利用的數據點后進行聚類，然后利用地面及雜草數據點的特點排除對樹干檢測的干擾。

(2)試驗表明，在無干擾情況下，對樹干的誤檢率為0.76%、漏檢率為1.90%、平均正確率為97.3%；在只存在地面干擾的情況下，樹干檢測平均正確率為96.1%；在只存在雜草干擾和同時存在地面和雜草干擾的情況下，樹干檢測平均正確率分別為91.4%和91.9%；綜合以上各種情況的樹干檢測平均正確率為95.5%，說明在具有干擾情況下樹干檢測正確率較高，對存在斜坡及雜草干擾的喬化果園環(huán)境適應性較強，可用于丘陵地區(qū)樹干較明顯的喬化果園中的樹干檢測。由于單線激光雷達傳感器單一、信息量較小，對于誤判情況及更細果樹的檢測，后續(xù)研究中可融合視覺進行檢測。