果園環(huán)境下移動(dòng)采摘機(jī)器人導(dǎo)航路徑優(yōu)化

胡廣銳，孔微雨，齊 闖，張 碩，卜令昕，周建國，陳 軍

（西北農(nóng)林科技大學(xué)機(jī)械與電子工程學(xué)院，楊凌 712100）

0 引言

隨著智慧農(nóng)業(yè)的不斷發(fā)展，智能化機(jī)器人技術(shù)被廣泛應(yīng)用于果園除草、剪枝、套袋、收獲等作業(yè)環(huán)節(jié)，有效地減輕了果農(nóng)的勞動(dòng)強(qiáng)度，提高了作業(yè)效率與質(zhì)量[1-4]。機(jī)器人在果園等非結(jié)構(gòu)化的環(huán)境下行間行駛的安全性是研究重點(diǎn)，合理、可靠、安全的導(dǎo)航路徑規(guī)劃方法是保證機(jī)器人安全行駛的重要手段之一[5-6]。

導(dǎo)航路徑規(guī)劃根據(jù)獲取環(huán)境信息的完整性分為局部路徑規(guī)劃與全局路徑規(guī)劃[1,7-8]。全局路徑規(guī)劃算法需要環(huán)境的整體信息，如田塊形狀、大小等[9]。局部路徑規(guī)劃只需要機(jī)器人附近的環(huán)境信息就能完成路徑規(guī)劃[10]。因此，完成局部路徑規(guī)劃可分為兩步，一是環(huán)境感知，使用傳感器獲取機(jī)器人周圍的環(huán)境信息；二是規(guī)劃路徑，根據(jù)傳感器獲得的信息，使用優(yōu)化算法，規(guī)劃出一條合理、安全的行駛路徑。

局部路徑規(guī)劃注重路徑的安全性與實(shí)時(shí)性，其典型算法包括A*算法[11-12]、人工勢(shì)場(chǎng)法[13-14]、粒子群算法[15-16]等。近年來，隨著傳感器技術(shù)的進(jìn)步，學(xué)者們基于機(jī)器視覺[17-20]或激光雷達(dá)[21-24]進(jìn)行了農(nóng)業(yè)機(jī)器人路徑規(guī)劃的相關(guān)研究。李秀智等[25]通過相機(jī)獲取田間作物行信息，基于隨機(jī)采樣一致性（Random Sample Consensus, RANSAC）算法剔除離群點(diǎn)，使用最小二乘法（Least Squares Method, LSM）提取導(dǎo)航路徑，路徑檢測(cè)率可達(dá)93.8%。陳子文等[26]基于自動(dòng)霍夫（Hough）變換累加閾值法提高了作物行提取的魯棒性與適用性。激光雷達(dá)傳感器能精準(zhǔn)地獲取環(huán)境的距離信息，且具有很強(qiáng)的抗干擾能力[27]。趙騰等[28-29]結(jié)合視覺與激光雷達(dá)的優(yōu)點(diǎn)，利用Hough變換法規(guī)劃導(dǎo)航路徑。劉沛等[30]使用激光掃描儀獲取果樹位置，采用LSM規(guī)劃農(nóng)機(jī)導(dǎo)航路徑。艾長(zhǎng)勝等[31]使用二維激光雷達(dá)獲取葡萄園壟行點(diǎn)云，提出了基于支持向量機(jī)（Support Vector Machine, SVM）的葡萄園路徑規(guī)劃算法。劉路等[32]采用16線激光雷達(dá)獲取玉米三維點(diǎn)云信息，引入置信區(qū)間結(jié)合K-means算法實(shí)現(xiàn)對(duì)玉米植株聚類，最后使用徑向基函數(shù)（Radial Basis Function, RBF）路徑規(guī)劃法，使規(guī)劃路徑盡可能避開植株主干?，F(xiàn)有研究多以檢測(cè)單株或作物行為主，規(guī)劃路徑為直線，雖便于機(jī)器人控制，但不利于躲避行間果樹冠層與行人等障礙物。陳日強(qiáng)等[33]雖通過無人機(jī)機(jī)載激光雷達(dá)成功提取了果樹冠層輪廓，但未將冠層信息用于機(jī)器人導(dǎo)航路徑規(guī)劃中。

綜上，機(jī)器人在果園作業(yè)時(shí)，果樹不規(guī)則的較大冠層與行人等障礙物是阻礙機(jī)器人行駛的隱患之一。傳統(tǒng)果園規(guī)劃路徑多為直線，不利于躲避果樹冠層等障礙物。本研究以紡錘形蘋果園為研究對(duì)象，提出改進(jìn)人工勢(shì)場(chǎng)法的機(jī)器人行間導(dǎo)航路徑優(yōu)化方法，主要步驟包括點(diǎn)云預(yù)處理、壟行線提取和初始路徑優(yōu)化。通過移動(dòng)采摘機(jī)器人搭載的固態(tài)激光雷達(dá)獲取果園三維點(diǎn)云數(shù)據(jù)，采用點(diǎn)云預(yù)處理去除果園地面點(diǎn)云，提取果園壟行與果樹冠層點(diǎn)云；分別采用RANSAC、LSM和Hough變換3種方法提取壟行線與初始路徑；將果樹冠層與行人視為障礙物，以初始路徑為基礎(chǔ)，結(jié)合改進(jìn)人工勢(shì)場(chǎng)法，優(yōu)化初始路徑，最終規(guī)劃出安全可靠的導(dǎo)航路徑，為移動(dòng)采摘機(jī)器人果園行間自主導(dǎo)航提供技術(shù)參考。

1 材料與方法

1.1 果園移動(dòng)采摘機(jī)器人系統(tǒng)組成

移動(dòng)采摘機(jī)器人由機(jī)器人底盤、激光雷達(dá)（RS-LiDAR-M1，速騰聚創(chuàng)，深圳市速騰聚創(chuàng)科技有限公司，中國）、主控制器（Jetson TX2，英偉達(dá)，英偉達(dá)有限公司，美國）、機(jī)械臂采摘系統(tǒng)組成（圖1a）。為便于果園移動(dòng)采摘機(jī)器人具有良好的通行能力，設(shè)計(jì)機(jī)器人底盤后輪由步進(jìn)電機(jī)驅(qū)動(dòng)，輪距為0.85 m，軸距為0.85 m，前輪采用阿克曼轉(zhuǎn)向機(jī)構(gòu)。激光雷達(dá)局部坐標(biāo)系以激光雷達(dá)的幾何中心為原點(diǎn)O，機(jī)器人前進(jìn)方向?yàn)閄軸正方向，垂直X軸向左為Y軸正方向，Z軸由右手定則確定（圖1b）。激光雷達(dá)安裝在機(jī)器人底盤前端居中位置，距地面高度約為0.5 m。激光雷達(dá)采用了微機(jī)電系統(tǒng)（Micro-Electro-Mechanical System, MEMS）固態(tài)激光雷達(dá)技術(shù)，MEMS振鏡快速震動(dòng)的同時(shí)發(fā)射高頻率激光束對(duì)外界環(huán)境進(jìn)行持續(xù)性的掃描，當(dāng)激光束遇到障礙物，反射光束被雷達(dá)接收，經(jīng)過時(shí)間飛行法[34]提供三維點(diǎn)云數(shù)據(jù)。激光雷達(dá)具體參數(shù)如表1所示。

表1 激光雷達(dá)參數(shù) Table 1 Parameters of LiDAR

本研究使用主控制器Jetson TX2裝載 Ubuntu16.04 LTS操作系統(tǒng)，并基于機(jī)器人操作系統(tǒng)（Robot Operating System, ROS）與點(diǎn)云庫（Point Cloud Library, PCL）進(jìn)行開發(fā)，CPU集群為雙核denver2處理器和四核ARM Cortex-A57組成，最高頻率為2.0 GHz，8 GB運(yùn)行內(nèi)存。

移動(dòng)采摘機(jī)器人電控系統(tǒng)工作原理如圖2所示。在果園環(huán)境中全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System, GNSS）信號(hào)被果樹冠層遮擋嚴(yán)重，差分定位效果差，因此移動(dòng)采摘機(jī)器人行間導(dǎo)航使用激光雷達(dá)采集環(huán)境三維點(diǎn)云信息，三維點(diǎn)云數(shù)據(jù)由以太網(wǎng)口傳輸至主控制器進(jìn)行濾波、去除地面、果樹冠層信息獲取、提取壟行線、優(yōu)化初始路徑等處理。使用Arduino控制器作為下位機(jī)，通過串口接收主控制器控制指令，控制前輪轉(zhuǎn)角與后輪轉(zhuǎn)速，避免移動(dòng)采摘機(jī)器人與果樹冠層等障礙物碰撞，保障移動(dòng)采摘機(jī)器人在果園行間導(dǎo)航安全行駛。

1.2 果園三維點(diǎn)云數(shù)據(jù)獲取

本研究于2020年12月11日在陜西省楊凌匯承果業(yè)公司果園（34°18′N，108°1′E）進(jìn)行了果園三維點(diǎn)云獲取試驗(yàn)（圖3）。蘋果樹樹齡為10年，樹形為紡錘形，行距約為4.2 m，株距約為1.6 m（圖3a）。果園三維點(diǎn)云分布不均勻，近端點(diǎn)云密集，遠(yuǎn)端點(diǎn)云逐漸稀疏（圖3b）。本研究提取Y軸上-4.0～4.0 m，X軸上1.0～6.0 m和Z軸上-0.5～4.0 m范圍內(nèi)的三維點(diǎn)云數(shù)據(jù)作為感興趣區(qū)域（Region of Interest, ROI）。因果園地面不平整且雜草多，導(dǎo)致果園三維點(diǎn)云雜點(diǎn)較多，所以需濾除地面點(diǎn)云，減少地面點(diǎn)云對(duì)壟行線與初始路徑提取的干擾。

1.3 果園三維點(diǎn)云預(yù)處理

激光雷達(dá)采集的原始果園三維點(diǎn)云每一幀約為7.8萬個(gè)點(diǎn)，數(shù)據(jù)量大，為減少計(jì)算量、去除噪點(diǎn)與離群點(diǎn)需對(duì)原始果園三維點(diǎn)云進(jìn)行預(yù)處理。使用直通濾波算法[32]提取ROI點(diǎn)云；體素下采樣濾波算法是使用立方體的形心來表示立方體內(nèi)的所有點(diǎn)，保留三維點(diǎn)云數(shù)據(jù)的結(jié)構(gòu)特征，并大大降低三維點(diǎn)云數(shù)量，因此本研究采用邊長(zhǎng)為0.1 m的立方體對(duì)ROI點(diǎn)云進(jìn)行了體素下采樣；體素下采樣后果園三維點(diǎn)云中仍會(huì)有許多噪點(diǎn)與離群點(diǎn)，故采用統(tǒng)計(jì)濾波去除噪點(diǎn)與離群點(diǎn)；為降低地面點(diǎn)云對(duì)果樹冠層輪廓提取的干擾，采用文獻(xiàn)[35]提出的地面平面擬合（Ground Plane Fitting, GPF）算法分割地面和非地面點(diǎn)云。

1.4 壟行線與初始路徑提取

果園三維點(diǎn)云經(jīng)預(yù)處理后，能較好分辨出果園壟行與果樹冠層點(diǎn)云，使用高度閾值將果園三維點(diǎn)云分割成壟行點(diǎn)云和冠層點(diǎn)云，根據(jù)果園壟高與經(jīng)驗(yàn)將高度閾值設(shè)置為0.1 m。壟行點(diǎn)云具有明顯的行特征，將壟行點(diǎn)云投影至XY平面，使用LSM、Hough變換和RANSAC方法分別提取壟行線與初始路徑，比較提取效果。將冠層點(diǎn)云投影至XY平面上，提取冠層點(diǎn)云邊界輪廓特征，根據(jù)冠層邊界輪廓優(yōu)化導(dǎo)航路徑，減低移動(dòng)采摘機(jī)器人在作業(yè)中與果樹冠層碰撞的幾率。

1.4.1 最小二乘法（LSM）

最小二乘法（Least Squares Method, LSM）是一種被普遍使用的數(shù)學(xué)優(yōu)化技術(shù)，通過最小化誤差平方和來尋找點(diǎn)云數(shù)據(jù)的最佳函數(shù)參數(shù)[30]。采用LSM擬合移動(dòng)采摘機(jī)器人行駛方向兩側(cè)壟行點(diǎn)云提取壟行線。當(dāng)點(diǎn)云坐標(biāo)滿足F(W)取最小值時(shí)，W即為擬合的壟行線方程參數(shù)矩陣如式（1）所示：

式中W=[kd]T為壟行線的參數(shù)矩陣；k為壟行線的斜率；d為壟行線截距，m；X′n×2=[XI]為點(diǎn)云X軸坐標(biāo)值矩陣Xn×1=[x1x2…xn]T與單位矩陣I組成的矩陣；Yn×1=[y1y2…yn]T為點(diǎn)云Y坐標(biāo)值組成的矩陣。對(duì)式（1）求導(dǎo)，當(dāng)X′TX′為正定矩陣時(shí)，壟行線方程參數(shù)矩陣W如式（2）所示：

1.4.2 霍夫（Hough）變換法

霍夫（Hough）變換是利用點(diǎn)與線的對(duì)偶特性，多用于圖像中的導(dǎo)航線提取，即將直角坐標(biāo)系下的直線轉(zhuǎn)化到Hough空間中進(jìn)行處理[26]。本研究將Hough變換應(yīng)用于壟行點(diǎn)云提取壟行線中，Hough空間下的直線方程如式（3）所示：

式中ρ為直線到原點(diǎn)的距離，m；θ為直線經(jīng)過的垂線與X軸之間的夾角，(°)；a、b為點(diǎn)云橫、縱坐標(biāo)值，m。

本研究將兩側(cè)的果樹行點(diǎn)云數(shù)據(jù)的直線檢測(cè)問題轉(zhuǎn)換到Hough空間中，在Hough空間尋找交點(diǎn)值（ρ，θ）完成直線檢測(cè)任務(wù)。在Hough空間中曲線交點(diǎn)很多，存在干擾點(diǎn)。為對(duì)干擾點(diǎn)進(jìn)行過濾，將ρ與θ離散化，形成一個(gè)個(gè)小區(qū)域，點(diǎn)數(shù)最多的2個(gè)區(qū)域的平均值作為最優(yōu)參數(shù)。

1.4.3 隨機(jī)采樣一致性（RANSAC）方法

本研究使用隨機(jī)采樣一致性（Random Sample Consensus, RANSAC）方法提取兩側(cè)壟行線，通過隨機(jī)采樣從兩側(cè)壟行點(diǎn)云中各選取一個(gè)子集建立直線模型，并計(jì)算該直線模型的內(nèi)點(diǎn)數(shù)檢驗(yàn)直線模型的正確性，不斷迭代來獲取最優(yōu)的直線模型，該直線模型即為提取的壟行線。提取壟行線流程如圖4所示。

迭代閾值KRANSAC為擬合的關(guān)鍵參數(shù)，若KRANSAC值設(shè)置過大則耗時(shí)過長(zhǎng)，設(shè)置過小則擬合效果不佳，根據(jù)文獻(xiàn)[25]中的選取方法，KRANSAC值的選取依據(jù)如式（4）所示：

式中α為迭代過程中選取的所有點(diǎn)均為內(nèi)點(diǎn)的概率，%；ω為從數(shù)據(jù)中選擇一個(gè)點(diǎn)為內(nèi)點(diǎn)的概率，%；N為數(shù)據(jù)總點(diǎn)數(shù)。

初始路徑為左右2條壟行線的平均值，初始路徑方程如式（5）所示：

式中ko=(kl+kr)/2為初始路徑的斜率；bo=(bl+br)/2為初始路徑的截距，m；其中，kl與kr分別為左、右兩側(cè)壟行線的斜率；bl與br分別為左、右兩側(cè)壟行線的截距，m。

1.5 壟行點(diǎn)云噪聲處理

本研究從實(shí)時(shí)性與抗噪能力兩個(gè)方面，分別對(duì)利用LSM、Hough變換和RANSAC 3種方法所提取初始路徑的效果進(jìn)行了評(píng)價(jià)，得到實(shí)時(shí)性佳，抗噪能力強(qiáng)的初始路徑提取方法。實(shí)時(shí)性是以提取初始路徑的處理時(shí)間為評(píng)價(jià)指標(biāo)，抗噪能力是初始路徑提取方法對(duì)兩種噪聲的抵抗能力。本研究在壟行點(diǎn)云中分別加入了高斯噪聲和人為噪聲，其中高斯噪聲是取消統(tǒng)計(jì)濾波處理，并對(duì)壟行點(diǎn)云進(jìn)行均值為0，方差為0.1的高斯噪聲處理；人為噪聲則是在壟行點(diǎn)云的行間中加入干擾點(diǎn)，將干擾點(diǎn)視為一種人為噪聲。

1.6 初始路徑優(yōu)化方法

本研究基于點(diǎn)云法向量方法從冠層點(diǎn)云中提取了邊界輪廓數(shù)據(jù)，首先對(duì)投影至XY平面上的冠層點(diǎn)云及其鄰近點(diǎn)做直線擬合，選取冠層點(diǎn)云中一點(diǎn)P，其鄰近點(diǎn)云集Pk={P0,P1…Pk-1}，設(shè)定直線u·x+v·y=c（u2+v2=1），使用最小二乘法擬合直線，向量q(u,v)即為點(diǎn)P的法向量。其次，根據(jù)文獻(xiàn)[36]可采用鄰近點(diǎn)最大夾角大于設(shè)定閾值的方法提取冠層輪廓點(diǎn)云，將冠層輪廓點(diǎn)云記為點(diǎn)集U={d0,d1…dn-1}。最后，通過式（6）判斷2條壟行線和冠層輪廓數(shù)據(jù)點(diǎn)的位置關(guān)系，去除壟行線外側(cè)的冠層數(shù)據(jù)，將2條壟行線內(nèi)側(cè)冠層數(shù)據(jù)分為左側(cè)點(diǎn)集UL與右側(cè)點(diǎn)集UR。

式中dli與dri分別為冠層輪廓點(diǎn)di(xi,yi)距左、右側(cè)壟行線的距離，m；i為點(diǎn)集索引號(hào)，i=0,1,2,…,n-1；n為冠層輪廓點(diǎn)云個(gè)數(shù)。

人工勢(shì)場(chǎng)法通過構(gòu)建虛擬斥力與引力勢(shì)場(chǎng)，計(jì)算勢(shì)場(chǎng)梯度下降方向規(guī)劃機(jī)器人的行駛路徑，但傳統(tǒng)人工勢(shì)場(chǎng)法易陷入局部最優(yōu)解或震蕩[13]。為避免初始路徑的優(yōu)化過程陷入局部最優(yōu)或震蕩，本研究?jī)H建立斥力勢(shì)場(chǎng)，兩側(cè)果樹冠層輪廓點(diǎn)云勢(shì)場(chǎng)相互作用，但方向相反，勢(shì)能可相互抵消。斥力勢(shì)場(chǎng)函數(shù)E如式（7）所示：

式中E為點(diǎn)云po(xo,yo)在點(diǎn)pj(xj,yj)處產(chǎn)生的勢(shì)能，J；λ、μ均為勢(shì)能系數(shù)，λ越大勢(shì)能越大，μ越大勢(shì)能變化越平緩，經(jīng)前期預(yù)試驗(yàn)取λ=1.5，μ=4.0。

左側(cè)冠層點(diǎn)集左側(cè)點(diǎn)集UL與右側(cè)點(diǎn)集UR在點(diǎn)pj(xj,yj)處的綜合勢(shì)能Ei(pi)如式（8）所示：

式中pr、pl分別為左側(cè)點(diǎn)集UL與右側(cè)點(diǎn)集UR中的點(diǎn)；nr、nl分別為左側(cè)點(diǎn)集UL與右側(cè)點(diǎn)集UR中的點(diǎn)的個(gè)數(shù)。

在初始路徑長(zhǎng)度一定的前提下，離散步長(zhǎng)S越小，離散路徑點(diǎn)就越多，優(yōu)化路徑就越平滑，但耗時(shí)也就越長(zhǎng)。本研究假設(shè)機(jī)器人的初始位置是相對(duì)安全的，因此將機(jī)器人每次規(guī)劃路徑的初始位置的勢(shì)能值作為勢(shì)能閾值Ey。迭代閾值KOptimal與優(yōu)化步長(zhǎng)So需滿足的約束條件如式（9）所示：

式中D2為果樹行距，m；KOptimal為迭代閾值；So為優(yōu)化步長(zhǎng)，m。

在預(yù)試驗(yàn)中發(fā)現(xiàn)，優(yōu)化步長(zhǎng)So太大會(huì)導(dǎo)致路徑規(guī)劃錯(cuò)誤，迭代閾值KOptimal過大會(huì)導(dǎo)致優(yōu)化時(shí)間過長(zhǎng)。因此本研究設(shè)置優(yōu)化步長(zhǎng)So為0.005 m，迭代次數(shù)KOptimal為420，離散步長(zhǎng)S為0.25 m。

初始路徑躲避果樹較大冠層與行人等障礙物的能力較弱，因此本研究將冠層與行人視為障礙物，采用改進(jìn)人工勢(shì)場(chǎng)法對(duì)初始路徑進(jìn)行優(yōu)化處理，以躲避行間果樹冠層與行人等障礙物。以S為步長(zhǎng)將初始路徑離散化為離散點(diǎn)，根據(jù)改進(jìn)人工勢(shì)場(chǎng)依次優(yōu)化每一個(gè)離散點(diǎn)，最后使用二次B樣條曲線擬合優(yōu)化后的離散點(diǎn)得到優(yōu)化路徑。初始路徑的優(yōu)化具體流程如圖5所示。

2 結(jié)果與分析

2.1 果園三維點(diǎn)云預(yù)處理結(jié)果

為探究三維點(diǎn)云預(yù)處理方法對(duì)移動(dòng)采摘機(jī)器人不同運(yùn)動(dòng)狀態(tài)的適應(yīng)性，移動(dòng)采摘機(jī)器人在靜止?fàn)顟B(tài)下和0.6 m/s前進(jìn)速度下分別采集果園三維點(diǎn)云數(shù)據(jù)。從2種機(jī)器人狀態(tài)采集的果園三維點(diǎn)云中分別選取了150幀點(diǎn)云進(jìn)行處理，預(yù)處理后的點(diǎn)云個(gè)數(shù)與濾波算法的處理時(shí)間如表2所示。由表2可知，機(jī)器人在靜、動(dòng)2種狀態(tài)下采集的三維點(diǎn)云經(jīng)預(yù)處理后的平均點(diǎn)云數(shù)分別為3 308和3 202，預(yù)處理的總平均處理時(shí)間分別約為0.172 s和0.170 s，點(diǎn)云個(gè)數(shù)與總平均處理時(shí)間都沒有明顯變化，表明本研究選擇的點(diǎn)云預(yù)處理方法對(duì)機(jī)器人不同運(yùn)動(dòng)狀態(tài)具有一定的適用性。通過對(duì)比分析直通濾波、下采樣濾波和統(tǒng)計(jì)濾波的處理時(shí)間，其中統(tǒng)計(jì)濾波的時(shí)間最長(zhǎng)，約為132.774×10-3s，約占總預(yù)處理時(shí)間的76%，不利于算法的實(shí)時(shí)性。

表2 果園點(diǎn)云預(yù)處理結(jié)果 Table 2 Pretreatment results of orchard point cloud

果園三維點(diǎn)云經(jīng)預(yù)處理后的可視化結(jié)果如圖6所示。直通濾波后提取了ROI點(diǎn)云（圖6a），點(diǎn)云個(gè)數(shù)約為5.8萬個(gè)，下降了約25%；體素化下采樣后點(diǎn)云個(gè)數(shù)約為5 987，保留了原始果園三維點(diǎn)云的結(jié)構(gòu)特征，點(diǎn)云數(shù)下降了約90%的（圖6b），極大的降低了點(diǎn)云數(shù)；統(tǒng)計(jì)濾波后點(diǎn)云數(shù)約為5 571個(gè)，去除約7%的離群點(diǎn)（圖6c），地面平面擬合后點(diǎn)云個(gè)數(shù)約為3 308，去除了約41%的地面點(diǎn)云，成功保留了果園壟行與果樹冠層點(diǎn)云信息（圖6d）。

2.2 壟行線與初始路徑提取結(jié)果

本研究對(duì)果園點(diǎn)云中分別加入高斯噪聲和人為噪聲，使用RANSAC、LSM和Hough變換3種方法提取壟行線和初始路徑的可視化結(jié)果如圖7所示。由圖7可知，3種方法均可處理壟行點(diǎn)云數(shù)據(jù)。由圖7a可知，RANSAC在未加入噪聲、加入高斯噪聲和加入人為噪聲下提取壟行線結(jié)果（以左側(cè)壟行線斜率為例）分別為-0.049，-0.040，-0.047，提取結(jié)果未發(fā)生明顯變化，表明RANSAC對(duì)高斯噪聲和人為噪聲均具有一定的抵抗能力。同理，由圖7c可知，Hough變換對(duì)高斯噪聲和人為噪聲也具有一定的抵抗能力。由圖7b可知，加入人為噪聲后，LSM提取的左側(cè)壟行線斜率為-0.232，明顯偏離壟行點(diǎn)云，提取效果下降。LSM考慮了整體點(diǎn)云到提取直線距離最短，當(dāng)具有較多偏離壟行點(diǎn)云的數(shù)據(jù)時(shí)，會(huì)導(dǎo)致提取的壟行線出現(xiàn)偏差。

為量化分析3種方法提取初始路徑的效果，將3種提取方法與3種點(diǎn)云噪聲處理方式作為因素，提取初始路徑的截距（bo，m）、斜率（ko）和處理時(shí)間作為響應(yīng)，從移動(dòng)采摘機(jī)器人在靜態(tài)下采集果園三維點(diǎn)云數(shù)據(jù)中選取100幀果園三維點(diǎn)云提取初始路徑，將響應(yīng)值分為未加入噪聲組、高斯噪聲組和人為噪聲組，導(dǎo)入數(shù)據(jù)統(tǒng)計(jì)與分析軟件Minitab 19中進(jìn)行組間方差分析，結(jié)果如表3所示。對(duì)表3中未加入噪聲組分析可知，RANSAC提取初始路徑截距（bo=-0.034 m）、斜率（ko=0.006）與LSM的提取結(jié)果（bo=-0.026 m，ko=-0.001）沒有顯著性差異，而與Hough變換提取的結(jié)果（bo=-0.212 m，ko=0.073）具有顯著性差異。對(duì)高斯噪聲組分析可知，3種方法對(duì)加入高斯噪聲后的提取結(jié)果均沒有顯著性差異，表明3種方法均能很好的抵抗高斯噪聲的影響。對(duì)比分析未加入噪聲組、高斯噪聲組和人為噪聲組可知，RANSAC與Hough變換提取初始路徑截距與斜率均無顯著性差異，但加入人為噪聲后LSM提取初始路徑的斜率（ko= -0.046）與未加入噪聲的結(jié)果（ko=-0.001）具有顯著性差異，表明了RANSAC與Hough變換的抗噪能力要優(yōu)于LSM。

對(duì)3種方法的處理時(shí)間分析可知，RANSAC在未加入噪聲組、高斯噪聲組合人為噪聲組的處理時(shí)間均不具有顯著性差異，LSM在未加入噪聲組的處理時(shí)間（1.222×10-3s）與另外兩組均具有顯著性差異（P<0.05），Hough變換在未加入噪聲組的處理時(shí)間（54.552×10-3s）與另外兩組均具有顯著性差異（P<0.05），表明RANSAC的處理時(shí)間相較于另2中方法受噪聲影響小。3種方法在每組內(nèi)的處理時(shí)間均具有顯著性差異，其中RANSAC的運(yùn)行時(shí)間要明顯低于LSM與Hough變換，約為0.147×10-3s，標(biāo)準(zhǔn)差為0.014×10-3s。Hough變換法運(yùn)行時(shí)間最長(zhǎng)，約為54.552×10-3s，標(biāo)準(zhǔn)差為13.121×10-3s。RANSAC可通過選取合理的迭代次數(shù)獲得良好的實(shí)時(shí)性。因此三種方法的實(shí)時(shí)性強(qiáng)弱依次為RANSAC、LSM和Hough變換。綜合考慮到優(yōu)化路徑的實(shí)時(shí)性與抗噪能力，本研究采用改進(jìn)的人工勢(shì)場(chǎng)法優(yōu)化的初始路徑由RANSAC提取。

2.3 初始路徑優(yōu)化結(jié)果

本研究改進(jìn)了人工勢(shì)場(chǎng)法，并對(duì)RANSAC提取的初始路徑進(jìn)行優(yōu)化，使優(yōu)化后的路徑盡可能避開果樹冠層等障礙物。為探究本研究所提方法的適用性，選取了行人作為另一種障礙物，讓試驗(yàn)人員站立果園行間模擬行人障礙物，運(yùn)用改進(jìn)人工勢(shì)場(chǎng)法建立了冠層障礙物與行人障礙物點(diǎn)云的人工勢(shì)場(chǎng)，并將三維人工勢(shì)場(chǎng)圖投影到XY平面內(nèi)顯示等勢(shì)線（圖8）。由圖8可知，距冠層與行人障礙物越近勢(shì)能絕對(duì)值越大，距冠層與行人障礙物越遠(yuǎn)勢(shì)能絕對(duì)值越小。兩側(cè)冠層障礙物輪廓點(diǎn)云產(chǎn)生的勢(shì)能范圍為-30.17～30.01 J（圖8a）。兩側(cè)冠層與行人障礙物輪廓點(diǎn)云產(chǎn)生的勢(shì)能范圍為-32.80～29.96 J（圖8b）。在XY平面內(nèi)行間均具有一條0等勢(shì)線，0等勢(shì)線具有遠(yuǎn)離兩側(cè)高勢(shì)能區(qū)域的特點(diǎn)。

本研究采用冠層與行人障礙物點(diǎn)云距導(dǎo)航路徑的最短距離（D，m）來評(píng)價(jià)路徑的安全性。冠層與行人障礙物點(diǎn)云距導(dǎo)航路徑的最短距離越大表明該路徑越安全。運(yùn)用改進(jìn)人工勢(shì)場(chǎng)對(duì)初始路徑進(jìn)行優(yōu)化，優(yōu)化結(jié)果如圖9所示。由圖9a可知，僅有冠層障礙物時(shí)，優(yōu)化后的路徑具有避開果樹冠層的趨勢(shì)。改進(jìn)人工勢(shì)場(chǎng)法優(yōu)化后的路徑將冠層障礙物點(diǎn)云距初始路徑的最短距離由0.705 m提高至0.907 m，提高了28.65%。試驗(yàn)人員站立在機(jī)器人前方約2.5 m處模擬行人障礙物，導(dǎo)航路徑優(yōu)化結(jié)果如圖9b所示。改進(jìn)人工勢(shì)場(chǎng)法優(yōu)化后的路徑將行人障礙物點(diǎn)云距初始路徑的最短距離由0.156 m提高至0.863 m，表明優(yōu)化后的路徑具有繞開行人障礙物的能力，提高了導(dǎo)航路徑的安全性。

從采集的果園點(diǎn)云中根據(jù)有無行人障礙物各選取3幀作為樣本，使用RANSAC提取的初始路徑、傳統(tǒng)人工勢(shì)場(chǎng)法優(yōu)化后的路徑和改進(jìn)人工勢(shì)場(chǎng)法優(yōu)化后的路徑作為導(dǎo)航路徑，計(jì)算冠層與行人障礙物點(diǎn)云距導(dǎo)航路徑的最短距離，結(jié)果如表4所示。由表4中僅有冠層障礙物的第3幀可知，障礙物點(diǎn)云距RANSAC提取的路徑與傳統(tǒng)人工勢(shì)場(chǎng)法優(yōu)化路徑的最短距離分別為0.705、0.894 m，傳統(tǒng)人工勢(shì)場(chǎng)法將此最短距離提高了26.81%；由第二幀可知，障礙物點(diǎn)云距RANSAC提取的路徑與改進(jìn)人工勢(shì)場(chǎng)法優(yōu)化路徑的最短距離分別為0.501、0.856 m，改進(jìn)人工勢(shì)場(chǎng)法將此最短距離提高了70.86%，而傳統(tǒng)人工勢(shì)場(chǎng)法陷入了震蕩；有行人障礙物時(shí)，改進(jìn)人工勢(shì)場(chǎng)法優(yōu)化后的路徑更具優(yōu)勢(shì)，行人障礙物點(diǎn)云距初始路徑的最短距離由0.324 m提高至0.778 m。優(yōu)化后的路徑比RANSAC提取的初始路徑距點(diǎn)云的最短距離更大，表明了經(jīng)優(yōu)化后的路徑更安全。傳統(tǒng)人工勢(shì)場(chǎng)法優(yōu)化后的路徑同樣比RANSAC提取的初始路徑更安全，但傳統(tǒng)人工勢(shì)場(chǎng)法易陷入震蕩，無法繼續(xù)求解。使用改進(jìn)人工勢(shì)場(chǎng)法的機(jī)器人行間導(dǎo)航路徑優(yōu)化方法對(duì)100幀果園點(diǎn)云數(shù)據(jù)進(jìn)行優(yōu)化，平均處理時(shí)間為0.059 s，標(biāo)準(zhǔn)差為0.007 s，表明該方法能滿足實(shí)時(shí)性要求。

表4 障礙物點(diǎn)云距3種導(dǎo)航路徑的最短距離 Table 4 Shortest distance between three navigation paths and obstacle point cloud

3 結(jié) 論

本研究在果園壟行線與果樹冠層點(diǎn)云的提取方法研究基礎(chǔ)上，提出了一種改進(jìn)人工勢(shì)場(chǎng)法的機(jī)器人行間導(dǎo)航路徑優(yōu)化方法，為避免機(jī)器人與果樹冠層等障礙物碰撞提供了一種解決方法，提高了移動(dòng)采摘機(jī)器人行間行駛的安全性。

1）從實(shí)時(shí)性與抗噪能力2個(gè)方面評(píng)價(jià)了最小二乘法（Least Squares Method, LSM）、霍夫（Hough）變換和隨機(jī)采樣一致性（Random Sample Consensus, RANSAC）3種方法提取壟行線與初始路徑的性能。在本研究的試驗(yàn)條件下對(duì)處理時(shí)間、初始路徑截距與斜率進(jìn)行了組間方差分析，試驗(yàn)結(jié)果表明RANSAC運(yùn)行時(shí)間最優(yōu)，約為0.147×10-3s、標(biāo)準(zhǔn)差為0.014×10-3s。RANSAC與Hough變換的抗噪能力要優(yōu)于LSM。

2）通過舍棄引力勢(shì)場(chǎng)，改進(jìn)了人工勢(shì)場(chǎng)法，將果樹冠層與行人均視為障礙物，建立了機(jī)器人行駛兩側(cè)果樹冠層與行人障礙物輪廓點(diǎn)云勢(shì)場(chǎng)，對(duì)初始路徑進(jìn)行了優(yōu)化, 避免了傳統(tǒng)人工勢(shì)場(chǎng)法容易陷入震蕩的問題。優(yōu)化結(jié)果表明，改進(jìn)后的導(dǎo)航路徑不僅能躲避果樹冠層，而且能夠繞開行人障礙物。改進(jìn)人工勢(shì)場(chǎng)法優(yōu)化后的路徑可將障礙物點(diǎn)云距導(dǎo)航路徑的最短距離由0.156 m提高至0.863 m，且優(yōu)化處理平均耗時(shí)0.059 s，標(biāo)準(zhǔn)差為0.007 s，有效保障了規(guī)劃路徑的安全性與實(shí)時(shí)性。