基于密度自適應的RANSAC非結構化環(huán)境下果園機器人導航

褚福春，宮金良，張彥斐

(1 山東理工大學 機械工程學院, 山東 淄博 255000； 2 山東理工大學農(nóng)業(yè)工程與食品科學學院, 山東 淄博 255000)

我國作為水果生產(chǎn)大國，水果種植面積與產(chǎn)量均為世界第一。但是果樹種植過程中存在著自動化水平低、管理不規(guī)范和采摘效率低等問題。針對這一系列問題，加大對果園智能裝備的發(fā)展力度，尤其是加大對丘陵山區(qū)果園管理裝備的改造已迫在眉睫。提高果園生產(chǎn)作業(yè)的自動化水平，不僅可以提高水果產(chǎn)量，還能夠減少人工勞動力，降低生產(chǎn)成本，對我國許多水果種植地區(qū)具有比較重要的現(xiàn)實意義，因此研究出一款應用于果園的自主作業(yè)機器人勢在必行[1-5]。

智慧農(nóng)業(yè)是農(nóng)業(yè)信息化發(fā)展從數(shù)字化到網(wǎng)絡化再到智能化的高級階段,對農(nóng)業(yè)發(fā)展具有里程碑意義,已成為世界現(xiàn)代農(nóng)業(yè)發(fā)展的趨勢。為進一步提高生產(chǎn)效率和水果品質(zhì),農(nóng)業(yè)機械裝備的智能化是未來發(fā)展的必然趨勢。隨著智慧農(nóng)業(yè)的不斷發(fā)展，智能化機器人技術被廣泛應用于果園除草、剪枝、采摘等作業(yè)環(huán)節(jié)，有效地減輕了果農(nóng)的勞動強度，提高了作業(yè)效率與質(zhì)量[6-9]。機器人在果園等非結構化的環(huán)境下行間行駛的安全性是研究重點，合理、可靠、安全的導航路徑規(guī)劃方法是保證機器人安全行駛的重要手段之一[10-11]。傳統(tǒng)的GPS導航方案，在果園中由于受到果樹遮擋，衛(wèi)星信號弱，無法進行高精度定位[12]。激光雷達作為一種新興的導航定位傳感器，具有精度高、掃描速度快和抗干擾能力強等特點，在果園環(huán)境感知中得到了越來越廣泛的應用[13-15]。

本文提出一種果園機器人導航技術，基于三維激光雷達和慣性測量單元(Inertial measurement unit，IMU)等傳感器，采集果園三維點云數(shù)據(jù)，利用改進的隨機采樣一致性 (Random sample consensus，RANSAC)算法，擬合推算得到導航線。根據(jù)三維點云信息，提出換行策略，實現(xiàn)換行作業(yè)?；诠麍@機器人的差速轉(zhuǎn)向結構和純追蹤模型進行軌跡跟蹤，實現(xiàn)果園機器人在非結構化環(huán)境下的自主導航。

1 果園導航方法

首先使用16線激光雷達采集果園點云數(shù)據(jù)，在經(jīng)過點云濾波處理后，降低了點云密度。然后進行地面點云的分割，去除地面點云后通過改進的RANSAC擬合出果樹行直線，根據(jù)平行直線之間的關系，進一步推算得到導航線，結合橫向偏差、航向偏差以及激光雷達IMU融合定位數(shù)據(jù)，通過純跟蹤控制算法調(diào)整機器人位姿。在行駛到行末觸發(fā)換行判定程序，確定機器人是否需要換行，完成換行后繼續(xù)執(zhí)行果樹行內(nèi)導航程序。如此，便實現(xiàn)果園機器人的自主導航，主要導航技術框架如圖1所示。

圖1 果園機器人導航技術框架Fig.1 Framework of orchard robot navigation technology

1.1 果樹行內(nèi)直線行駛

果樹行直線提取的方法通常是在獲取當前幀的果樹點云后，將該幀點云投影到世界坐標系中，然后將該幀點云分割成左右兩樹行的點云集，再從左右樹行點云集中擬合出兩側果樹行的直線，常用的獲取方法有RANSAC算法和最小二乘法(Least squares method，LSM)。

LSM算法是一種被普遍使用的數(shù)學優(yōu)化方法，該方法通過最小誤差平方和來求解點云數(shù)據(jù)中的最佳函數(shù)參數(shù)[16]。采用LSM算法擬合果樹行直線，當點云坐標滿足f(Q)取到最小值，即：

式中，Q=[k b]T為果樹行直線的參數(shù)矩陣，k為果樹行直線的斜率，b為果樹行直線的截距；X′為點云X軸坐標值組成的矩陣，Y′為點云Y軸坐標值組成的矩陣。

1981年有學者提出RANSAC算法，最初用于解決定位的問題，用迭代法求解含有局部點的觀測數(shù)據(jù)的數(shù)學模型參數(shù)[17-18]。RANSAC算法通過對觀測值隨機抽樣估算參數(shù)，該算法的簡要步驟為：

1)隨機從數(shù)據(jù)集中選取n個樣本數(shù)據(jù)，建立參數(shù)模型M；

2)計算數(shù)據(jù)集中所有數(shù)據(jù)與參數(shù)模型M的誤差，若誤差小于閾值，加入內(nèi)點集；

3)如果當前內(nèi)點集元素個數(shù)大于最優(yōu)內(nèi)點集，則更新最優(yōu)內(nèi)點集，同時更新迭代次數(shù)h，h的表達式為：

式中，η為置信度，一般取0.995，λ為內(nèi)點的比例，m為計算模型所需要的最少樣本數(shù)；

4)如果迭代次數(shù)大于h，則退出，否則迭代次數(shù)加1，并重復上述步驟。

RANSAC算法較LSM算法具有較大的優(yōu)勢，尤其是在行距較大的果園中導航精度更高，魯棒性更好。本文基于傳統(tǒng)RANSAC算法，結合果園機器人的實際應用場景，提出一種基于點云密度自適應的RANSAC算法，即使存在部分點云缺失的情況，也能較好地擬合出果樹行直線。傳統(tǒng)的RANSAC算法固定閾值，在擬合點云較為稀疏的地方可能會出現(xiàn)較大偏差。改進后的RANSAC算法具體步驟為：

1)將當前幀的左右兩側果樹行點云ξi進行聚類分割，并去除地面和雜草點云的干擾，得到左右兩側點云集合ξli和ξri；

2)一般果園都是按照平行直線種植，本文設定左、右2條平行直線分別為Ll和Lr：

式中，kl、kr為果樹行直線斜率，bl，br為果樹行直線截距；

3)分別從左右兩側點云集合ξli和ξri中隨機選取2點，確定構成果樹行直線的參數(shù)；

4)根據(jù)點云密度設定不同初始閾值Ri，從而分割出內(nèi)外點，選取出內(nèi)點集合，將最大內(nèi)點集合的閾值設為最佳閾值Rbest，將其他內(nèi)點集合的閾值更新為Rbest，進而確定與直線Li的幾何距離不大于Rbest的點云集合ξ(Li)，即為L的最大一致集合。設定內(nèi)點(xi,yi)，擬合直線Ax+By+C=0，則有:

5)重復n次隨機選擇，得到直線L1，L2，…，Ln和相應的一致集合ξ(L1)，ξ(L2)，…，ξ(Ln)；

6)確定最大一致集合，將最大一致集合中的點擬合成一條直線即為最佳直線L，通過ξli和ξri獲取的最佳直線分別為Ll和Lr，即獲取到最佳斜率kl和kr，最佳截距bl和br；

7)在獲取到左右兩側果樹行最佳直線后，根據(jù)平行直線關系，進一步提取出導航線Lc，果園機器人通過純跟蹤算法沿導航線行駛，完成果樹行內(nèi)的自主導航。其中，Lc的計算公式為：

由于Ll、Lr和Lc為三條平行直線，故k=kl=kr。

1.2 行末換行策略

一般果園的樹行兩端的特征很明顯，即行首和行末都不存在果樹，因此存在一定的轉(zhuǎn)向空間。當激光雷達檢測到果園機器人兩側大片點云數(shù)據(jù)缺失的時候，可以判定機器人已到達換行區(qū)域，工控機下發(fā)換行指令，給定機器人底盤特定的速度指令，保證每次換行轉(zhuǎn)彎動作的準確性，默認左轉(zhuǎn)，可根據(jù)果園實際情況進行調(diào)整。完成換行動作后，繼續(xù)向前行駛一段距離dts，確保其完全進入到下一行，然后繼續(xù)執(zhí)行樹行跟蹤的指令，具體過程如圖2。

圖2 果園機器人換行策略Fig.2 Line breaking strategy of orchard robot

設定轉(zhuǎn)彎半徑為Rt，換行時的路徑為Sth，則

2 運動控制

2.1 基本運動模型

本試驗采用的是差速轉(zhuǎn)向機器人底盤，同側兩車輪通過鏈傳動共用一個電機來驅(qū)動，故同側前后兩輪輪速大小相等?？芍苯咏邮諄碜怨た貦C發(fā)送的線速度v和角速度 ω(主要是指x軸方向上的線速度和z軸方向上的角速度)，實現(xiàn)機器人的各種運動。圖3中XOY為建立的大地坐標系，v為機器人瞬時線速度，機器人正前方為正；vr為右側輪速度；vl為左側輪速度；ω為瞬時角速度，逆時針方向為正；θ為姿態(tài)角；￡為輪距；p為機器人中心位置；Ll、Lr為左、右兩側果樹行；機器人的位姿可用矢量P=[x,y,θ]T表示。其中：

圖3 運動模型Fig.3 Motion model

機器人主要有3種運動狀態(tài)：

1)vl≠vr，方向相同且均不為0時，機器人做圓周運動，此時

2)vl=-vr時 ，機器人原地轉(zhuǎn)動，此時v=0；

3)vr=vl時，機器人直線行駛，此時 ω =0。

在驅(qū)動輪與地面接觸運動為純滾動的情況下，機器人的運動學模型可以表示為：

2.2 融合定位

IMU以100 Hz的頻率輸出加速度和角速度數(shù)據(jù),激光雷達以10 Hz的頻率輸出點云數(shù)據(jù)。每收到一幀點云數(shù)據(jù),系統(tǒng)首先擴增一個代表當前激光雷達位姿初始估計的狀態(tài)向量,然后使用優(yōu)化后的點云觀測模型對特征進行提取和跟蹤;每收到一幀IMU數(shù)據(jù),根據(jù)運動模型對IMU狀態(tài)進行估計,同時更新系統(tǒng)狀態(tài)的協(xié)方差矩陣;最終通過擴展卡爾曼濾波算法融合IMU與激光雷達的觀測數(shù)據(jù)更新位姿狀態(tài),以100 Hz的頻率輸出結果。相鄰測量時刻間的狀態(tài)量定義如下：

式中：w表示世界坐標系；u表示IMU坐標系；i表示測量時刻；S為系統(tǒng)狀態(tài)量；Sut為t時刻IMU狀態(tài)量，包括相對于世界坐標系的平移、速度和旋轉(zhuǎn)，分別以p、v和四元數(shù)q表示；ua和ug分別為加速度計和陀螺儀的零偏。

本文提出的以激光雷達為主，融合IMU信息的定位方案，可實現(xiàn)果園機器人的厘米級定位，對非結構化環(huán)境下的導航十分重要。

2.3 軌跡跟蹤

果園機器人通過純跟蹤算法結合傳感器數(shù)據(jù)確定前視距離，最終將機器人位姿信息上傳至工控機，通過對比當前幀與前一幀位姿變化，確定橫向偏差和航向偏差是否超過設定閾值，工控機下發(fā)速度指令給單片機，驅(qū)動左右兩側電機，保證機器人能夠按照預定導航線軌跡行駛，完成樹行跟蹤的目標。

純追蹤算法是一種模擬人類駕駛習慣的幾何追蹤模型，根據(jù)車輛的行駛速度和路徑信息設定軌跡上的一個路徑點，計算車輛當前位置距離預設目標點所需轉(zhuǎn)過的角度，控制車輛向路徑點行駛[19]。具體跟蹤方法是：在目標軌跡K上選擇一個點B作為跟蹤點，則機器人從當前位置到達點B的軌跡可以視為一段半徑為R的圓弧軌跡，對點B跟蹤的關鍵在于對圓弧軌跡半徑R的求解，圖4中 α為航向偏差，f為前視距離，d為橫向偏差。綜合各參數(shù)，可計算得到軌跡半徑R。

圖4 果園機器人軌跡跟蹤示意圖Fig.4 Orchard robot trajectory tracking diagram

具體計算步驟為：

在求得軌跡半徑R之后、線速度v給定的情況下可以求解出角速度 ω，計算公式為:

2.4 路徑跟蹤仿真結果分析

純追蹤算法中前視距離的大小將會直接影響機器人的軌跡跟蹤效果，前視距離較大時，機器人將會以較小的曲率向路徑行駛，但系統(tǒng)響應較慢，可能會產(chǎn)生“抄近路”的問題，影響控制精度；當前視距離較小時，機器人將會以較大曲率向路徑行駛，機器人會因此頻繁轉(zhuǎn)向，產(chǎn)生較大的振蕩，降低機器人的穩(wěn)定性。因此，前視距離的大小對導航十分重要。

通過Python3.5 IDLE進行路徑跟蹤仿真，結合果園機器人差速運動模型和純跟蹤算法，初始位姿設置為 (0，-1，0)，前視距離分別設置為 2、3、4 m，在0.8 m/s速度下進行仿真，結果如圖5所示。

圖5 不同前視距離(f )下路徑跟蹤仿真結果Fig.5 Path tracking results at different forward-viewing distances(f)

從仿真結果中可以看出，前視距離為2 m時，機器人在曲率較大的路徑處的實際運動軌跡較為曲折，實際運動軌跡和目標軌跡出現(xiàn)明顯偏差；前視距離為4 m時，機器人雖然會以平滑的運動軌跡行駛，但也出現(xiàn)了實際軌跡與目標軌跡存在較大偏差的問題；只有當前視距離為3 m時，機器人能夠較好地追蹤目標軌跡，故設定機器人前視距離為 3 m。

3 試驗驗證及結果

3.1 硬件系統(tǒng)組成

圖6a為果園機器人整體框架，由機器人底盤、工控機、Velodyne 16線激光雷達和9軸IMU組成。機器人長、寬和高分別為60、45和25 cm，整體采用差速轉(zhuǎn)向的結構設計，使其在果園中具有良好的通行能力。機器人左右兩側各安裝一個驅(qū)動電機，通過鏈傳動的方式帶動四輪轉(zhuǎn)動。圖6b為果園機器人實物圖，除導航所必須的硬件結構，還配備了高清攝像頭，可以實時獲取果園真實畫面。為方便各感器融合，定義果園機器人坐標系，以IMU中心為坐標原點，機器人正前方為x軸方向，平行于車軸且指向左側的方向為y軸正方向，過坐標原點垂直指向正上方為z軸正方向。

圖6 果園機器人基本組成Fig.6 Basic composition of orchard robot

3.2 試驗方案

為了驗證果園機器人自主導航系統(tǒng)的實際效果，在山東理工大學蘭玉彬教授團隊與淄博市沂源縣山東中以現(xiàn)代智慧農(nóng)業(yè)有限公司共建的智慧化無人果園試驗基地進行了試驗。該果園果樹行距約為 4 m，果樹高約 3.5 m，行距約 4 m。圖7a為果園真實環(huán)境，可以看到果樹枝葉茂盛，對GPS信號遮擋十分嚴重。

圖7 點云預處理Fig.7 Point cloud pretreatment

通過三維激光雷達獲取點云數(shù)據(jù)(圖7b)后，首先進行預處理降低點云密度，借助濾波算法去除地面和雜草的干擾，采用平面模型擬合當前的地面。由于現(xiàn)實的地面并不是一個“完美的”平面，尤其是在果園這種環(huán)境中，地面非常不平整，要很好地完成地面分割，就必須要處理存在一定坡度變化的地面情況,即不能將這種坡度的變化視為非地面，不能因為坡度的存在而引入噪聲。一種簡單的處理方法就是沿著x方向(車頭的方向)將空間分割成若干個子平面，然后對每個子平面使用地面平面擬合算法從而得到能夠處理陡坡的地面分割方法。對于給定的點云集合設為P，最終將會被分割為地面點云集Pg和非地面點云集合Png，如圖7c所示。

分割出地面點云后，利用濾波算法將其過濾掉，處理后的結果如圖8a。通過聚類算法將果樹行點云簇進行聚類，最終會形成一個將點云包裹的三維Bounding Box，其主要使用了一種基本的數(shù)據(jù)結構KD樹，它是在一個歐幾里德空間中組織點的基本數(shù)據(jù)結構，本質(zhì)上是一個每個節(jié)點都為k對應維點的二叉樹，聚類后的效果如圖8b。

基于傳統(tǒng)的RANSAC算法，提出一種密度自適應的改進方法，根據(jù)不同點云集設置不同初始閾值，篩選出最大內(nèi)點集合，更新并確定最佳閾值，經(jīng)過多次迭代，更高效且更準確地擬合出果樹行直線，根據(jù)平行直線關系，推算導航線(圖8c)。由圖8d可以看到，即使在部分點云缺失和存在離群果樹點云的情況下，該算法還是能較好地推算出導航線。

圖8 擬合導航線Fig.8 Fitting navigation lines

通過多次試驗，統(tǒng)計果園機器人實際行駛軌跡與導航線之間的橫向偏差和航向偏差，驗證本方案的可行性。對比LSM算法、傳統(tǒng)RANSAC算法和改進后的RANSAC算法分別在0.8和1.0 m/s的不同速度下產(chǎn)生的橫向偏差和航向偏差，每隔10 m進行一次記錄，進行5次記錄，并取平均值。

3.3 試驗結果

綜合多次試驗結果，對比LSM算法、傳統(tǒng)RANSAC算法和改進RANSAC算法，當機器人以0.8 m/s的速度行駛，改進后的RANSAC算法產(chǎn)生的橫向偏差的最大值和平均值分別為0.10 m和0.084 m，航向偏差的最大值和平均值分別為1.5°和1.34°，相較LSM算法和傳統(tǒng)RANSAC算法均為最小，具體結果見表1。但當機器人以1.0 m/s的速度行駛時，各項偏差均明顯增大，具體結果見表2。

表1 速度0.8 m/s下3種算法產(chǎn)生的偏差結果Table 1 The deviation results generated by the three algorithms under the speed of 0.8 m/s

表2 速度1.0 m/s下3種算法產(chǎn)生的偏差結果Table 2 The deviation results generated by the three algorithms under the speed of 1.0 m/s

4 結論

果園機器人通過激光雷達采集點云數(shù)據(jù)，經(jīng)過點云降噪、分割地面與非地面點云和果樹行點云聚類等一系列處理后，依靠改進的RANSAC算法擬合果樹行最佳直線，根據(jù)平行直線關系，推算得到導航線。通過純跟蹤算法對果園機器人進行運動控制，實現(xiàn)果園機器人的自主導航?；诒疚奶岢龅男心Q行策略，實現(xiàn)機器人換行作業(yè)。通過咨詢果園管理專業(yè)人員，了解到果園機器人在行距為4 m的果園進行巡檢、除草等作業(yè)時，橫向偏差不得大于0.15 m、航向偏差不得大于2°。本研究試驗結果顯示：當機器人以0.8 m/s的速度在果園行駛作業(yè)時，改進后的RANSAC算法產(chǎn)生的橫向偏差不超過0.10 m，航向偏差不超過1.5°。因此，本文提出的導航方法基本滿足生產(chǎn)要求，能夠極大地提高果園管理的自動化水平，具有比較重要的現(xiàn)實意義。

未來將在保證定位導航精度的前提下，穩(wěn)步提升果園機器人的行駛速度，提高作業(yè)效率。融合視覺算法，提高果園機器人的環(huán)境感知能力，實現(xiàn)對雜草和障礙物的識別，進而提高控制精度，使機器人在非標準化果園也能夠正常行駛。