基于歐氏聚類的三維激光點(diǎn)云田間障礙物檢測(cè)方法

尚業(yè)華 張光強(qiáng) 孟志軍 王 昊 蘇春華 宋正河

(1.中國(guó)農(nóng)業(yè)大學(xué)工學(xué)院， 北京 100083； 2.北京市農(nóng)林科學(xué)院智能裝備技術(shù)研究中心， 北京 100097；3.農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)機(jī)械化技術(shù)開(kāi)發(fā)推廣總站， 北京 100122)

0 引言

近幾年國(guó)內(nèi)對(duì)農(nóng)機(jī)自動(dòng)駕駛技術(shù)的需求不斷增長(zhǎng)，而已應(yīng)用的農(nóng)機(jī)自動(dòng)駕駛技術(shù)主要是輔助駕駛和特定地塊基于路徑規(guī)劃的自動(dòng)駕駛。要實(shí)現(xiàn)全場(chǎng)景的無(wú)人駕駛，田間障礙物檢測(cè)是首要的環(huán)境感知技術(shù)[1]。目前主要的障礙物檢測(cè)有基于視覺(jué)的環(huán)境感知方案、基于超聲波與毫米波雷達(dá)的感知方案和基于激光雷達(dá)等的感知方案[2-3]。其中基于視覺(jué)的環(huán)境感知方案由于受環(huán)境光影響較大且在夜晚不能保證感知精度，單獨(dú)用于自動(dòng)駕駛中的田間識(shí)別與避障仍有一定的局限性,需融合其他傳感器[4]。基于三維激光雷達(dá)對(duì)農(nóng)田環(huán)境的障礙物檢測(cè)研究較少，目前農(nóng)機(jī)領(lǐng)域研究較多的為二維激光雷達(dá)，主要應(yīng)用在障礙物的距離判別，但因輸出信息為二維點(diǎn)，不能實(shí)時(shí)檢測(cè)障礙物的尺寸、形狀等信息[5-8]。因此探索采用三維激光雷達(dá)對(duì)農(nóng)田環(huán)境障礙物進(jìn)行檢測(cè)具有重要的科研與應(yīng)用意義。

在對(duì)農(nóng)田障礙物檢測(cè)方面，國(guó)內(nèi)外已有相關(guān)研究報(bào)道[9-20]。目前對(duì)農(nóng)田環(huán)境障礙物檢測(cè)的方法主要有基于單目、雙目和多視角幾何進(jìn)行圖像特征與深度學(xué)習(xí)的方法。其中基于單目視覺(jué)的障礙物檢測(cè)主要有基于特征和基于學(xué)習(xí)的方法，基于雙目和多視角幾何的檢測(cè)對(duì)環(huán)境進(jìn)行三維重建以獲取深度信息；另有二維激光雷達(dá)與視覺(jué)結(jié)合或采用Kinect獲取圖像和深度信息進(jìn)行障礙物的檢測(cè)方法。但此類以視覺(jué)為主導(dǎo)的障礙物檢測(cè)方案受光照情況影響較大，在夜間作業(yè)時(shí)檢測(cè)效果大幅下降。

三維激光雷達(dá)因?yàn)樘綔y(cè)距離較遠(yuǎn)且精度高、受環(huán)境光變化的影響小等而成為道路車輛自動(dòng)駕駛方案的重要傳感器之一，其在道路車輛自動(dòng)駕駛中主要負(fù)責(zé)檢測(cè)道路車輛、行人等障礙物[21-25]，但在農(nóng)業(yè)領(lǐng)域的研究主要在作物表型方面[26]，在農(nóng)機(jī)自動(dòng)駕駛領(lǐng)域應(yīng)用較少[27]。近兩年隨著農(nóng)機(jī)自動(dòng)駕駛功能需求的增加和三維激光雷達(dá)成本的下降，基于三維激光雷達(dá)在農(nóng)機(jī)自動(dòng)駕駛方面的研究逐漸增多。文獻(xiàn)[28]以拖拉機(jī)為載體設(shè)計(jì)了基于激光雷達(dá)的農(nóng)田環(huán)境點(diǎn)云采集系統(tǒng)。文獻(xiàn)[29]通過(guò)安裝2個(gè)二維激光雷達(dá)在不同的位置獲得不同的掃描信息并進(jìn)行融合來(lái)進(jìn)行障礙物的檢測(cè)。文獻(xiàn)[30]采用基于柵格的方法識(shí)別三維點(diǎn)云中的障礙物信息，但對(duì)尺寸不一、形態(tài)各異的田間障礙物并不具有適應(yīng)性。目前三維激光雷達(dá)在農(nóng)田環(huán)境障礙物檢測(cè)方面的研究還處于初始階段，但由于其不受光照條件影響，且三維激光雷達(dá)相較于圖像方法或二維激光雷達(dá)有更豐富的深度特征信息，為農(nóng)田環(huán)境障礙物檢測(cè)的準(zhǔn)確性和可靠性提供了更多的信息支持。

針對(duì)目前農(nóng)田環(huán)境障礙物檢測(cè)的問(wèn)題，本文提出一種基于三維激光點(diǎn)云聚類的田間障礙物檢測(cè)方法。本文擬對(duì)三維激光雷達(dá)掃描的包含農(nóng)田障礙物的點(diǎn)云進(jìn)行濾波、地面分割和聚類，針對(duì)農(nóng)田地面特性對(duì)聚類后的障礙物點(diǎn)云進(jìn)行濾波，對(duì)聚類得到的障礙物點(diǎn)云添加長(zhǎng)方體框來(lái)標(biāo)識(shí)，通過(guò)對(duì)多幀田間障礙物點(diǎn)云進(jìn)行檢測(cè)來(lái)驗(yàn)證方法的有效性。

1 材料與方法

本研究使用的雷達(dá)為速騰聚創(chuàng)公司的RS-LiDAR-32型三維激光雷達(dá)。其激光發(fā)射線束為32線，垂直視場(chǎng)角-25°～15°，垂直角分辨率非線性分布，水平視場(chǎng)角360°，水平角分辨率0.1°，測(cè)距范圍0.4～120 m(目標(biāo)反射率20%)，最大測(cè)距200 m，測(cè)距精度±3 cm，輸出點(diǎn)云約每秒60萬(wàn)點(diǎn)。

在進(jìn)行點(diǎn)云處理時(shí)使用Ubuntu 16.04系統(tǒng)平臺(tái)，配合雷達(dá)本身的ROS驅(qū)動(dòng)，采用點(diǎn)云庫(kù)PCL 1.8.0的部分庫(kù)函數(shù)，用C++語(yǔ)言進(jìn)行處理。在對(duì)激光點(diǎn)云進(jìn)行田間障礙物檢測(cè)時(shí)主要有濾波、地面分割和聚類幾個(gè)環(huán)節(jié)。其分別使用體素柵格濾波下采樣、感興趣區(qū)域分割、基于RANSAC算法的地面點(diǎn)云檢測(cè)與分割、基于K維樹(shù)(K-d tree)最近鄰搜索的歐氏聚類等方法檢測(cè)出障礙物點(diǎn)云，并采用PCL點(diǎn)云庫(kù)函數(shù)進(jìn)行點(diǎn)云的渲染，用長(zhǎng)方體框標(biāo)識(shí)出。具體的點(diǎn)云處理流程如圖1所示。

圖1 點(diǎn)云障礙物檢測(cè)流程圖Fig.1 Point cloud obstacle detection flowchart

1.1 體素柵格下采樣

由于三維激光雷達(dá)的點(diǎn)云數(shù)據(jù)量較大，在對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行處理時(shí)需先進(jìn)行濾波，對(duì)點(diǎn)云進(jìn)行體素柵格下采樣，即在不損失點(diǎn)云重要特征信息的情況下對(duì)點(diǎn)云進(jìn)行減量，從而加速后續(xù)處理計(jì)算。

體素(Voxel)概念上類似于二維圖像像素，表示在三維空間中一個(gè)微小的立方體。體素柵格為微小的空間三維立方體的集合，其將三維點(diǎn)云進(jìn)行空間柵格劃分，即一個(gè)點(diǎn)一定屬于某一個(gè)體素，一個(gè)體素可能包含零至多個(gè)點(diǎn)，如圖2所示。

圖2 體素柵格示意圖Fig.2 Voxel grid diagram

本文采用的體素柵格下采樣方法首先為輸入的點(diǎn)云數(shù)據(jù)創(chuàng)建一個(gè)三維體素柵格，然后在每個(gè)體素(即三維立方體)內(nèi)，用體素內(nèi)所有點(diǎn)的質(zhì)心來(lái)近似顯示體素中其他點(diǎn)。這樣經(jīng)過(guò)體素下采樣濾波后，減少了點(diǎn)云數(shù)量，提高了后續(xù)算法處理速度，同時(shí)由于采用體素內(nèi)所有點(diǎn)的質(zhì)心點(diǎn)表示，相比于用體素中心來(lái)逼近的方法能更好地保留點(diǎn)云的特征。

1.2 感興趣區(qū)域點(diǎn)云分割

由于三維激光雷達(dá)掃描的范圍較大，而農(nóng)機(jī)行駛過(guò)程中一般只對(duì)前方行駛區(qū)域的障礙物感興趣。所以對(duì)下采樣濾波后的點(diǎn)云框選出一個(gè)感興趣區(qū)域，刪掉區(qū)域外的點(diǎn)云，對(duì)區(qū)域內(nèi)的點(diǎn)云進(jìn)行后續(xù)處理，減少處理時(shí)間，增加精度。

選定的感興趣區(qū)域是在三維空間中框選出一個(gè)三維立方體，此立方體為包圍雷達(dá)發(fā)射中心的一個(gè)三維區(qū)域。在選取感興趣區(qū)域時(shí)立方體的長(zhǎng)邊為將要探及障礙物的距離范圍，綜合考慮農(nóng)田常用動(dòng)力機(jī)械的車速、剎車距離以及車身四周的預(yù)留空間，長(zhǎng)邊的長(zhǎng)度為農(nóng)機(jī)前方2倍的剎車距離和農(nóng)機(jī)后方2個(gè)車身的長(zhǎng)度之和。感興趣區(qū)域的寬度為以農(nóng)機(jī)為中心，左右兩側(cè)共計(jì)2個(gè)農(nóng)機(jī)作業(yè)幅寬的寬度。感興趣區(qū)域的高度為探及障礙物的高度范圍，以激光雷達(dá)所在高度為零點(diǎn)，包括上下2部分范圍。同時(shí)由于雷達(dá)在水平視場(chǎng)角360°掃描，感興趣區(qū)域內(nèi)包含農(nóng)機(jī)本身的點(diǎn)云，其在后續(xù)聚類過(guò)程中會(huì)造成誤聚類，故在感興趣區(qū)域內(nèi)將農(nóng)機(jī)本身的點(diǎn)云去除，為方便劃分，該區(qū)域選取為農(nóng)機(jī)外接矩形的尺寸，如圖3所示。

圖3 感興趣區(qū)域劃分示意圖Fig.3 Region of interest segment diagram

1.3 基于RANSAC算法的地面分割

經(jīng)過(guò)濾波處理的點(diǎn)云在進(jìn)行障礙物檢測(cè)前需先將地面的點(diǎn)云去除，因此要基于點(diǎn)云進(jìn)行地面的檢測(cè)。由于農(nóng)田地面并不像機(jī)動(dòng)車道路一樣平坦，其土質(zhì)及顆粒尺寸變異較大，在農(nóng)田地面點(diǎn)云附近會(huì)有一些噪點(diǎn)。根據(jù)高度信息采用一般的直通濾波不能很好地將地面去除，最小二乘擬合是從整體誤差最小的角度考慮，容易因離群點(diǎn)使得擬合面與真實(shí)面的誤差較大，因此本研究應(yīng)用隨機(jī)采樣一致性(Random sample consensus, RANSAC)算法進(jìn)行地面檢測(cè)。

RANSAC算法是一種通過(guò)對(duì)觀測(cè)數(shù)據(jù)進(jìn)行隨機(jī)采樣來(lái)估計(jì)模型參數(shù)的學(xué)習(xí)算法。其假設(shè)給定數(shù)據(jù)集同時(shí)包含模型內(nèi)點(diǎn)和模型外點(diǎn)，在初始時(shí)隨機(jī)選取一個(gè)原始數(shù)據(jù)的子集，并假定其為待擬合模型，然后針對(duì)該擬合模型測(cè)試所有其他數(shù)據(jù)，當(dāng)數(shù)據(jù)滿足誤差閾值時(shí)即認(rèn)為其為該模型共識(shí)集的一部分，隨后將隨機(jī)選取點(diǎn)假定為待擬合模型得到共識(shí)集，進(jìn)行一定次數(shù)的迭代后判斷共識(shí)集內(nèi)點(diǎn)數(shù)最多的模型即為待擬合模型結(jié)果。

將RANSAC算法應(yīng)用在三維點(diǎn)云的地面檢測(cè)中，將檢測(cè)出的地面與待聚類的障礙物點(diǎn)云區(qū)分，防止后續(xù)聚類過(guò)程中誤將地面點(diǎn)云檢測(cè)為障礙物。在算法設(shè)計(jì)時(shí)首先設(shè)定算法的迭代次數(shù)為I，距離誤差閾值為ΔT1，點(diǎn)云的總點(diǎn)數(shù)為N，在初始時(shí)隨機(jī)選取3個(gè)點(diǎn)構(gòu)成待擬合地面，設(shè)3個(gè)點(diǎn)的三維坐標(biāo)為(X1，Y1，Z1)、(X2，Y2，Z2)、(X3，Y3，Z3)，則擬合的平面模型為

Ax+By+Cz+D=0

(1)

其中

A=(Y2-Y1)(Z3-Z1)-(Z2-Z1)(Y3-Y1)
B=(Z2-Z1)(X3-X1)-(X2-X1)(Z3-Z1)
C=(X2-X1)(Y3-Y1)-(Y2-Y1)(X3-X1)
D=-(AX1+BY1+CZ1)

則空間任一點(diǎn)(X0，Y0，Z0)到該平面的距離L為

(2)

當(dāng)某點(diǎn)與此次假設(shè)平面的距離L≤ΔT1時(shí)，則此點(diǎn)為模型內(nèi)點(diǎn)。依次遍歷除初始采樣的3點(diǎn)之外的其他N-3個(gè)點(diǎn)，記錄此模型的內(nèi)點(diǎn)數(shù)目。

再隨機(jī)采樣3個(gè)點(diǎn)構(gòu)造一個(gè)平面模型，按照同樣的方法得到該模型的內(nèi)點(diǎn)數(shù)目。按照此隨機(jī)采樣的方法迭代I次。隨著迭代次數(shù)的增加產(chǎn)生合理結(jié)果的概率亦隨之增加。最終基于每個(gè)模型的內(nèi)點(diǎn)數(shù)目表決，選取內(nèi)點(diǎn)數(shù)目最多的地面模型即為最佳擬合結(jié)果。

在應(yīng)用該算法進(jìn)行地面擬合的設(shè)計(jì)階段，為驗(yàn)證其有效性，在Matlab中進(jìn)行了測(cè)試，對(duì)隨機(jī)生成的高斯分布的三維點(diǎn)云進(jìn)行平面擬合，如圖4所示。經(jīng)試驗(yàn)該算法可以很好地對(duì)有離群值的三維點(diǎn)云進(jìn)行地面擬合。

圖4 三維點(diǎn)云RANSAC算法平面擬合示意圖Fig.4 3D point cloud RANSAC test diagram

1.4 基于K-d tree最近鄰搜索的歐氏聚類

經(jīng)過(guò)上述濾波和田間地面點(diǎn)云分割后，其余點(diǎn)云即為田間障礙物點(diǎn)云，將此點(diǎn)云進(jìn)行基于K-d tree最近鄰搜索的歐氏聚類處理檢測(cè)出障礙物。

1.4.1K-d tree的三維應(yīng)用

K-d tree是K維空間中劃分點(diǎn)的一種數(shù)據(jù)結(jié)構(gòu)[31]，其常用于多維空間中關(guān)鍵數(shù)據(jù)的搜索。該數(shù)據(jù)結(jié)構(gòu)將空間劃分以便提高搜索效率，其每個(gè)節(jié)點(diǎn)所在超平面將K維空間劃分為2部分。圖5為K-d tree在三維空間中的劃分示意圖。圖中的紅框垂直平面為第1次分割，將空間切割為2個(gè)子單元格，然后將每個(gè)子空間由綠色水平平面分割為2個(gè)子單元格。最后，將4個(gè)單元格分別由4個(gè)藍(lán)色垂直平面分成2個(gè)子單元格。

圖5 K-d tree 三維示例Fig.5 K-d tree three-dimensional diagram

K-d tree形似二叉搜索樹(shù)，本研究將其應(yīng)用在三維點(diǎn)云處理中，故K-d tree中的K=3，在確定每個(gè)節(jié)點(diǎn)的關(guān)鍵值時(shí)依照下面的原則：在樹(shù)的第n層(n對(duì)3取余的數(shù)值序號(hào)是該層中節(jié)點(diǎn)關(guān)鍵值的序號(hào))，在每個(gè)節(jié)點(diǎn)以該關(guān)鍵值建立一個(gè)超平面，且超平面垂直于該關(guān)鍵值的軸，該超平面把空間分為2部分，左側(cè)的點(diǎn)由該節(jié)點(diǎn)的左子樹(shù)表示，右側(cè)的點(diǎn)由右側(cè)的子樹(shù)表示。在對(duì)每幀點(diǎn)云數(shù)據(jù)建樹(shù)過(guò)程中根節(jié)點(diǎn)不同，根節(jié)點(diǎn)為每幀數(shù)據(jù)的第1個(gè)點(diǎn)，當(dāng)給定根節(jié)點(diǎn)后其余點(diǎn)即可按此規(guī)則掛載建立K-d tree。為便于理解該方法，對(duì)三維K-d tree舉例說(shuō)明，如圖6所示。

華北多特高壓交直流強(qiáng)耦合大受端電網(wǎng)系統(tǒng)保護(hù)方案設(shè)計(jì)//羅亞洲,陳得治,李軼群,王青,張劍云,訾鵬,等//(22):11

圖6 K-d tree 示例Fig.6 A K-d tree example

圖6中紅色數(shù)字為該層節(jié)點(diǎn)的關(guān)鍵值，藍(lán)色數(shù)字為與上一層關(guān)鍵值比較的數(shù)值。由圖6可見(jiàn)，在根節(jié)點(diǎn)(3，5，6)取第1維3為該層關(guān)鍵值，(2，1，8)中的2小于3，故在其左子葉，(4，5，6)中的4大于3，故在其右子葉；在第2層取第2維為該層的關(guān)鍵值，下一層的(2，0，5)中0小于(2，1，8)中的1，故在下一層的左子葉，(2，3，7)中的3大于(2，1，8)中的1，故在下一層的右子葉。后續(xù)依此類推，首先確定該層節(jié)點(diǎn)的關(guān)鍵值，若數(shù)據(jù)點(diǎn)同一維度的數(shù)值小于該節(jié)點(diǎn)的關(guān)鍵值則在該節(jié)點(diǎn)的左子樹(shù)，反之則在右子樹(shù)。其在空間的劃分如圖7所示。

圖7 K-d tree示例在三維空間的劃分Fig.7 Schematic of K-d tree example in threedimensional space

1.4.2K-d tree 最近鄰搜索的歐氏聚類

最近鄰搜索算法旨在找到最接近給定輸入點(diǎn)的點(diǎn)。在應(yīng)用K-d tree進(jìn)行三維空間點(diǎn)的搜索時(shí)，從根節(jié)點(diǎn)開(kāi)始，算法以遞歸的方式向下沿著樹(shù)進(jìn)行搜索，一旦到達(dá)葉子節(jié)點(diǎn)，檢查該節(jié)點(diǎn)到給定點(diǎn)的歐氏距離，如果小于設(shè)定的最近鄰距離閾值則認(rèn)為其為最近鄰點(diǎn)[32-33]。該算法在搜索時(shí)檢查在分割平面的另一側(cè)是否有任何點(diǎn)比當(dāng)前最佳點(diǎn)更靠近搜索點(diǎn)，將分割超平面與以當(dāng)前最近距離為半徑的搜索點(diǎn)構(gòu)成的超球面相比較，如果超球面越過(guò)該平面，則該平面的另一側(cè)可能會(huì)有更近的點(diǎn)，則從當(dāng)前節(jié)點(diǎn)向下移動(dòng)樹(shù)的另一分支以尋找更近的點(diǎn)；如果超球面不與分割平面相交，則算法將繼續(xù)沿樹(shù)搜索，并消除該節(jié)點(diǎn)另一側(cè)的整個(gè)分支[34-35]。由于K-d tree在樹(shù)的每一層上將域的范圍劃分為一半，因此若對(duì)有m個(gè)節(jié)點(diǎn)的K-d tree應(yīng)用最近鄰搜索，時(shí)間復(fù)雜度由原來(lái)的2m降到lbm。

通過(guò)使用樹(shù)的屬性可以快速消除搜索空間的大部分，更快完成此搜索過(guò)程。本研究應(yīng)用基于K-d tree的最近鄰搜索，對(duì)輸入的三維點(diǎn)云建立K-d tree，在該點(diǎn)云樹(shù)上搜索給定目標(biāo)點(diǎn)的最近鄰點(diǎn)。

從根節(jié)點(diǎn)開(kāi)始按照樹(shù)的結(jié)構(gòu)依次遍歷樹(shù)的每個(gè)節(jié)點(diǎn)，設(shè)定兩點(diǎn)間距離閾值為ΔT，Δ為樹(shù)的該層節(jié)點(diǎn)關(guān)鍵值與待檢測(cè)點(diǎn)值的差，考慮到點(diǎn)云坐標(biāo)有負(fù)值的情況，且為了避免漏搜索最近鄰點(diǎn)，當(dāng)Δ>-ΔT時(shí)則進(jìn)行下一層左子樹(shù)的搜索，當(dāng)樹(shù)的該層節(jié)點(diǎn)關(guān)鍵值Δ<ΔT時(shí)則進(jìn)行下一層右子樹(shù)的搜索。假設(shè)待搜索的節(jié)點(diǎn)坐標(biāo)為(Xnode，Ynode，Znode)，目標(biāo)點(diǎn)的坐標(biāo)為(Xtarget，Ytarget，Ztarget)，令

Δx=Xnode-Xtarget

(3)

Δy=Ynode-Ytarget

(4)

Δz=Znode-Ztarget

(5)

若|Δx|≤ΔT、|Δy|≤ΔT、|Δz|≤ΔT3個(gè)條件同時(shí)滿足，則計(jì)算待檢測(cè)點(diǎn)與目標(biāo)點(diǎn)的歐氏距離D，計(jì)算公式為

(6)

若D≤ΔT，則認(rèn)為此節(jié)點(diǎn)為目標(biāo)點(diǎn)的最近鄰點(diǎn)。按照此方法對(duì)所有點(diǎn)進(jìn)行搜索，直到滿足距離誤差閾值的所有最近鄰點(diǎn)搜索完畢。將搜索到的該目標(biāo)點(diǎn)的最近鄰點(diǎn)均記錄在一個(gè)集合中，當(dāng)所得最近鄰點(diǎn)集合的元素?cái)?shù)量滿足設(shè)定的障礙物類點(diǎn)簇的數(shù)量范圍時(shí)，則識(shí)別此點(diǎn)云集合為一個(gè)障礙物點(diǎn)簇。因在聚類進(jìn)行最近鄰搜索時(shí)計(jì)算的距離為歐幾里得距離，故稱此過(guò)程為歐氏聚類。

依次對(duì)所有預(yù)處理后的點(diǎn)云進(jìn)行聚類，如果待處理點(diǎn)已屬于某一類，則進(jìn)行剩余其他點(diǎn)的處理，防止重復(fù)聚類。當(dāng)聚類完畢時(shí)得到多個(gè)點(diǎn)云集合，即為多個(gè)障礙物點(diǎn)簇。

1.5 聚類后類簇濾波

由于農(nóng)田環(huán)境的復(fù)雜性和目前三維激光雷達(dá)的不穩(wěn)定性，容易有一些小土塊和不平的地面被誤檢測(cè)為障礙物，因此對(duì)聚類后的點(diǎn)云類簇在最后一步再進(jìn)行濾波。本研究中共進(jìn)行2種濾波，即針對(duì)類簇點(diǎn)數(shù)量的濾波和外接長(zhǎng)方體尺寸的濾波。

針對(duì)類簇點(diǎn)數(shù)量的濾波即判斷聚類后每個(gè)類簇內(nèi)點(diǎn)的數(shù)量，設(shè)定一個(gè)待檢測(cè)障礙物的最小數(shù)量和最大數(shù)量，將不在此范圍內(nèi)的聚類認(rèn)為是誤檢測(cè)的類，只保留在此范圍內(nèi)的聚類。外接長(zhǎng)方體尺寸的濾波，以下簡(jiǎn)稱體濾波，即判斷聚類后每個(gè)類簇的最小外接長(zhǎng)方體尺寸并按尺寸將一部分非障礙物過(guò)濾掉。給聚類后的每個(gè)類添加一個(gè)最小外接長(zhǎng)方體，計(jì)算每個(gè)聚類的外接長(zhǎng)方體的體積，將體積過(guò)小或體積過(guò)大的類刪掉，剩下的即認(rèn)為是聚類的障礙物。體濾波閾值的選取可依據(jù)待檢測(cè)障礙物的外接長(zhǎng)方體體積確定。

為測(cè)試體濾波對(duì)障礙物檢測(cè)的效果，進(jìn)行了對(duì)照試驗(yàn)。試驗(yàn)時(shí)拖拉機(jī)與橫穿農(nóng)田的行人距離不變，考慮到農(nóng)機(jī)的剎車距離特性，此處選為10 m。按照上述步驟完成聚類后，對(duì)障礙物點(diǎn)云對(duì)照進(jìn)行處理，一組經(jīng)過(guò)體濾波，另外一組不經(jīng)過(guò)體濾波。結(jié)果如圖8所示。

圖8 體濾波對(duì)檢測(cè)的影響Fig.8 Influence of volume filtering on detection

圖8為體濾波處理后的點(diǎn)云鳥(niǎo)瞰圖，其中圖8a中間處的紅色長(zhǎng)方體框?yàn)闄z測(cè)到的行人，左上角處的紅色長(zhǎng)方體框?yàn)檎`檢測(cè)的障礙物地面小突起；圖8b中間處的紅色長(zhǎng)方體框?yàn)闄z測(cè)到的行人，左上角處的綠色長(zhǎng)方體框?yàn)榻?jīng)過(guò)體濾波過(guò)濾掉的點(diǎn)云聚類。由圖8可以看出體濾波能夠?qū)r(nóng)田地面的誤檢測(cè)起到很好的過(guò)濾作用。

2 試驗(yàn)與結(jié)果分析

為驗(yàn)證算法的有效性、評(píng)估算法的檢測(cè)誤差，本研究使用RS-LiDAR-32型三維激光雷達(dá)于2020年7月在北京市小湯山國(guó)家精準(zhǔn)農(nóng)業(yè)示范基地開(kāi)展田間試驗(yàn)。試驗(yàn)時(shí)將激光雷達(dá)安裝在迪爾904型拖拉機(jī)上，安裝位置為拖拉機(jī)前配重塊上方，安裝高度為95 cm，安裝方位角為雷達(dá)自身坐標(biāo)系的x軸正方向?qū)?zhǔn)拖拉機(jī)的正前方。安裝方法見(jiàn)圖9。

圖9 試驗(yàn)安裝圖Fig.9 Test installation

試驗(yàn)時(shí)將拖拉機(jī)前部激光雷達(dá)對(duì)著一片玉米地，拖拉機(jī)前部安裝的激光雷達(dá)距離玉米地30 m，玉米株高約0.5 m，試驗(yàn)場(chǎng)景如圖10所示。

圖10 試驗(yàn)場(chǎng)景Fig.10 Test environment

試驗(yàn)采集的三維點(diǎn)云在ROS下記錄為bag包數(shù)據(jù)，在處理時(shí)將bag包分幀轉(zhuǎn)換為pcd格式的點(diǎn)云，將pcd格式的點(diǎn)云按照上述算法進(jìn)行處理，最后將檢出的障礙物點(diǎn)云用以最小點(diǎn)和最大點(diǎn)為對(duì)角點(diǎn)的長(zhǎng)方體框標(biāo)注出來(lái)。

試驗(yàn)時(shí)針對(duì)農(nóng)田環(huán)境的特性和雷達(dá)的點(diǎn)云密度特性設(shè)定了算法的參數(shù)。在體素柵格下采樣時(shí)設(shè)定柵格邊長(zhǎng)為0.15 m。劃定感興趣區(qū)域時(shí)，為盡可能將車身安全距離范圍內(nèi)障礙物均檢測(cè)到，設(shè)定長(zhǎng)度為60 m，其中雷達(dá)前方50 m，后方10 m；寬度為20 m，以雷達(dá)為中心左右兩側(cè)各10 m；高度為7.5 m，由于雷達(dá)本身安裝在拖拉機(jī)上且考慮到對(duì)地面的檢測(cè)，故感興趣區(qū)域中包括雷達(dá)下方1.5 m，雷達(dá)上方6 m。在劃定立方體感興趣區(qū)域時(shí)以雷達(dá)坐標(biāo)為原點(diǎn)，在三維空間中以立方體的對(duì)角點(diǎn)確定立方體，其中最小點(diǎn)坐標(biāo)為(-10，-10，-1.5) m、最大點(diǎn)坐標(biāo)為(50，10，6) m；為避免裝載雷達(dá)的拖拉機(jī)自身反射點(diǎn)云干擾聚類，將車身的外接立方體從感興趣區(qū)域中去除，用同樣的方法設(shè)定立方體的對(duì)角點(diǎn)最小點(diǎn)坐標(biāo)為(-5，-1.5，-1.5) m、最大點(diǎn)坐標(biāo)為(0.4，1.5，1.5) m，濾波時(shí)將此部分區(qū)域的點(diǎn)云濾除。在進(jìn)行基于RANSAC算法的地面檢測(cè)時(shí)設(shè)定迭代次數(shù)為60，距離誤差閾值為0.3 m；在進(jìn)行基于K-d tree的歐氏聚類時(shí)設(shè)定距離誤差閾值為0.6 m。對(duì)聚類后類簇的濾波時(shí)選用單個(gè)障礙物類簇的最小點(diǎn)云數(shù)為10，最大點(diǎn)云數(shù)為240，在進(jìn)行體濾波時(shí)為平衡障礙物漏檢率和地面誤檢率，在濾波閾值的選擇上取的相對(duì)折中，其中最小閾值為0.06，最大閾值為1.2。

主要設(shè)計(jì)了2個(gè)試驗(yàn)：田間常見(jiàn)障礙物的檢測(cè)試驗(yàn)和障礙物距離對(duì)檢測(cè)正確率的影響試驗(yàn)。

2.1 田間常見(jiàn)障礙物檢測(cè)試驗(yàn)

為驗(yàn)證該算法的田間障礙物檢測(cè)效果，對(duì)田間常見(jiàn)障礙物應(yīng)用該算法進(jìn)行了試驗(yàn)。障礙物包括：自然放置在田間農(nóng)機(jī)具、田間草堆和田埂等低矮障礙物；地頭矮房等多障礙物；田間道路兩側(cè)樹(shù)木等有重疊的障礙物。檢測(cè)結(jié)果如圖11所示。在處理后的點(diǎn)云圖中將檢測(cè)出的障礙物用外接長(zhǎng)方體框標(biāo)注出來(lái)。由圖11可以看出該算法可以有效檢測(cè)田間農(nóng)機(jī)具、草堆和田埂等低矮障礙物；同時(shí)可以檢測(cè)地頭矮房等多個(gè)障礙物；對(duì)道路兩側(cè)樹(shù)木等多個(gè)重疊障礙物滿足算法設(shè)定的參數(shù)條件也可以同時(shí)檢測(cè)并用長(zhǎng)方體框標(biāo)注出來(lái)。試驗(yàn)結(jié)果表明該算法均可以檢測(cè)田間低矮障礙物、多個(gè)障礙物和障礙物間有重疊的場(chǎng)景。

圖11 田間常見(jiàn)障礙物的檢測(cè)結(jié)果Fig.11 Detection result of common obstacles in field

2.2 距離對(duì)障礙物檢測(cè)的影響試驗(yàn)

為測(cè)試距離對(duì)障礙物檢測(cè)的影響，對(duì)不同距離的障礙物進(jìn)行了試驗(yàn)?？紤]到田間運(yùn)動(dòng)的行人是影響農(nóng)機(jī)行駛安全的重要因素之一，試驗(yàn)時(shí)以行人作為待檢測(cè)障礙物橫穿拖拉機(jī)前方，待檢測(cè)的人身高為1.75 m，分別與雷達(dá)距離5、10、15、20、25、30 m進(jìn)行點(diǎn)云的掃描。采用上述算法和參數(shù)對(duì)采集的不同距離的點(diǎn)云進(jìn)行處理。處理前后點(diǎn)云如圖12所示。

圖12 對(duì)田間不同距離行人的檢測(cè)Fig.12 Detection of pedestrians at different distances in field

圖12中每圖的左側(cè)為采集的原始點(diǎn)云，右側(cè)為處理后的點(diǎn)云，對(duì)檢出的行人和玉米地用紅色長(zhǎng)方體框標(biāo)出。由圖12可以看出，原始點(diǎn)云的密度較大， 且在雷達(dá)周邊有拖拉機(jī)反射的點(diǎn)云，經(jīng)過(guò)濾波和感興趣區(qū)域分割后點(diǎn)云數(shù)量減少，但掃描的障礙物特征并未損失且加速了計(jì)算過(guò)程。每幀點(diǎn)云的體素柵格濾波、感興趣區(qū)域切分和基于RANSAC的地面分割總平均用時(shí)500 ms，基于K-d tree的聚類平均用時(shí)1 ms。

試驗(yàn)共采集有效點(diǎn)云1 937幀，平均單幀點(diǎn)云約115 200個(gè)點(diǎn)，對(duì)掃描的單幀點(diǎn)云按照前述方法進(jìn)行處理并統(tǒng)計(jì)，結(jié)果如表1所示。

表1列出了不同距離的采集點(diǎn)云幀數(shù)、地面誤檢率和行人檢出率。其中地面誤檢率是指由于農(nóng)田土地突起不平整的干擾在聚類過(guò)程中因滿足聚類條件而被檢測(cè)出的概率。行人檢出率是指經(jīng)過(guò)算法處理能夠檢測(cè)出田間行人的概率。經(jīng)試驗(yàn)，該算法參數(shù)的平均地面誤檢率為4.76%，對(duì)田間平均行人檢出率為96.11%。

表1 不同距離下田間行人的檢測(cè)準(zhǔn)確率Tab.1 Detection accuracy of pedestrians in the field at different distances

經(jīng)分析可發(fā)現(xiàn)在距離5～20 m內(nèi)，行人檢出率較高，均在96%以上，剩余未檢出的原因?yàn)樵谧詈筮M(jìn)行體濾波處理時(shí)由于激光雷達(dá)本身不穩(wěn)定造成的個(gè)別幀的點(diǎn)數(shù)較少，外接矩形較小而被過(guò)濾掉從而造成漏檢。在距離25～30 m時(shí)，由于距離相對(duì)較遠(yuǎn)，根據(jù)三維激光雷達(dá)向外發(fā)散的特性，遠(yuǎn)處的點(diǎn)較近處的點(diǎn)稀疏，在濾波作用下檢出率稍有下降；且因靠近田埂，故地面誤檢率稍有上升。

在農(nóng)田環(huán)境中，由于農(nóng)機(jī)駕駛速度一般較低，其剎車距離也較小。假設(shè)車速為30 km/h，摩擦因數(shù)取0.3，則剎車距離為11.76 m。故在近距離內(nèi)的障礙物檢出率可以很大程度上為農(nóng)機(jī)行駛提供可靠的數(shù)據(jù)。后續(xù)可以在算法上進(jìn)行更深入研究，融合多種傳感器感知，并在控制策略上進(jìn)行設(shè)計(jì)來(lái)最大程度地保證安全。

3 結(jié)論

(1)以三維激光雷達(dá)為傳感器，提出一種基于歐氏聚類的三維激光點(diǎn)云田間障礙物檢測(cè)方法。該方法首先劃定點(diǎn)云中的三維感興趣區(qū)域并應(yīng)用體素下采樣方法對(duì)點(diǎn)云進(jìn)行濾波以加快點(diǎn)云處理的速度，然后采用RANSAC算法對(duì)點(diǎn)云中的農(nóng)田地面和地上物體進(jìn)行區(qū)分，最后對(duì)地面上點(diǎn)云采用基于K-d tree的歐氏聚類，對(duì)聚類后的障礙物點(diǎn)云添加長(zhǎng)方體框來(lái)標(biāo)識(shí)。

(2)實(shí)現(xiàn)了以Ubuntu 16.04為系統(tǒng)平臺(tái)，以C++為編程語(yǔ)言的三維點(diǎn)云歐氏聚類算法。實(shí)現(xiàn)了對(duì)點(diǎn)云的濾波、地面分割和障礙物聚類的檢測(cè)流程，且對(duì)算法中最后一步濾波的影響進(jìn)行了對(duì)比試驗(yàn)，試驗(yàn)表明體濾波可以顯著降低地面的誤檢率。

(3)對(duì)該算法進(jìn)行了田間試驗(yàn)，實(shí)現(xiàn)了農(nóng)田中農(nóng)機(jī)具、草堆、田埂、矮房和田間道路兩側(cè)樹(shù)木的檢測(cè)。

(4)對(duì)在農(nóng)田中橫穿的行人進(jìn)行了多種距離的采樣和檢測(cè)。試驗(yàn)結(jié)果表明應(yīng)用該算法對(duì)30 m內(nèi)田間行人平均檢出率為96.11%。