番茄串收機械臂運動規(guī)劃方法與試驗

張 勤，劉豐溥，蔣先平，熊 征，徐 燦

（1. 華南理工大學(xué)機械與汽車工程學(xué)院，廣州 510641；2. 廣東省現(xiàn)代農(nóng)業(yè)裝備研究所，廣州 510630）

0 引言

采摘作業(yè)是番茄生產(chǎn)鏈中最耗時、費力的環(huán)節(jié)，隨著番茄種植面積的擴大，勞動成本的逐年提高，機械臂代替人類的智能采收作業(yè)是未來的發(fā)展方向[1]。機械臂的運動規(guī)劃是智能采收的基礎(chǔ)，也是實現(xiàn)高效、適時、無損采摘的重要保證。番茄串外皮脆弱，形狀復(fù)雜，個體差異大；由于采摘目標周圍果實、枝條、葉子等障礙物的存在，采摘運動規(guī)劃的難度大大增加。為此國內(nèi)外學(xué)者做了大量的研究工作。

在規(guī)劃算法方面，應(yīng)用于車輛導(dǎo)航的路徑規(guī)劃方法如人工勢場法[2]、A*算法[3]等都被用于低自由度機械臂規(guī)劃問題。Zhao 等[4]在傳統(tǒng)蟻群算法的基礎(chǔ)上，加入滑動窗口與遺忘因子，為機械臂規(guī)劃了無碰撞的最優(yōu)路徑；Beom等[5]以能量最小為優(yōu)化目標，通過Dijkstra算法為機械臂規(guī)劃漸進最優(yōu)的運動路徑。但上述算法在規(guī)劃空間和關(guān)節(jié)空間自由度數(shù)增加時，算法的計算時間會呈“指數(shù)級”增長，難以滿足實時規(guī)劃的需要。

針對高維空間多自由度機械臂的運動規(guī)劃問題，基于采樣的方法[6-8]被廣泛地使用。為了使規(guī)劃樹在高維空間下更好地拓展，維持采樣方法概率完備的特性，增強采樣方法漸進最優(yōu)的優(yōu)勢，多種基于采樣的優(yōu)化方法被開發(fā)。Li等[9]在Lazy-PRM算法的基礎(chǔ)上，從給定環(huán)境點云的自由構(gòu)型空間中構(gòu)建機械臂的可行運動路徑；Wei等[10]提出了一種基于梯度下降的RRT*-connect算法，大幅降低無效采樣點的數(shù)量，并且通過剪枝操作優(yōu)化路徑，降低路線成本；Mcmahon等[11]通過連接機械臂連桿和末端執(zhí)行器的可達空間體積，產(chǎn)生更加合理的連接路線圖，作為機械臂的可行構(gòu)型解。深度學(xué)習(xí)與采樣方法的結(jié)合可以使采樣更多地發(fā)生在更為理想和有效的狀態(tài)區(qū)域中，因此多種基于深度學(xué)習(xí)的采樣方法被開發(fā)[12-13]。Knips等[14]設(shè)計了一種神經(jīng)運動學(xué)的方法，經(jīng)過視覺探索、形狀分類和姿態(tài)估計等一系列處理，為機械臂實時規(guī)劃系列動作，實現(xiàn)了雜亂桌面上目標的抓?。籎ia等[15]提出了一種改進的Q-Learning自主學(xué)習(xí)算法，根據(jù)Q表進行自主學(xué)習(xí)，減少運動過程的盲目性，提高運動效率，在許多特定的場合下，強化學(xué)習(xí)在機械臂的路徑規(guī)劃方面有著不錯的效果[16-17]。

除了在已知全部環(huán)境信息、初始構(gòu)型和最終構(gòu)型基礎(chǔ)上的全局規(guī)劃方法，為了應(yīng)對更加復(fù)雜且可能未知的工作環(huán)境，多種局部規(guī)劃方法被開發(fā)。Vahrenkamp等[18]不預(yù)先計算最終的抓取姿態(tài)，而是在規(guī)劃過程中確定合理的抓取姿態(tài)，利用雅可比矩陣的偽逆方法將工作空間中的拓展映射至關(guān)節(jié)空間，作為機械臂關(guān)節(jié)空間規(guī)劃樹的拓展方向；Kimmel等[19]提出一種JIST（Jacobian Informed Search Tree）的方法，通過優(yōu)化機械臂末端執(zhí)行器的位移函數(shù)，在忽略連桿的前提下為執(zhí)行器規(guī)劃一個合理有效的姿態(tài)構(gòu)型，為基于采樣的雅可比規(guī)劃采樣器提供了指導(dǎo)性的任務(wù)空間，從而提高規(guī)劃效率，給出更高質(zhì)量的規(guī)劃結(jié)果。

在番茄采摘機械臂的運動規(guī)劃方面，尹建軍等[20]將關(guān)節(jié)型采摘機械臂的三維避障問題轉(zhuǎn)化為平面R-R的二維避障問題，利用A*算法計算平面R-R的無碰路徑，最后進行其他關(guān)節(jié)角的路徑規(guī)劃；鄒宇星等[21]將采摘環(huán)境與空間障礙物離散為單元集合，通過單元信息與關(guān)節(jié)空間的映射關(guān)系建立機械臂的構(gòu)型空間，再通過PRM算法在構(gòu)型空間中規(guī)劃無碰路徑；陽涵疆等[22]根據(jù)機械臂與障礙物的幾何特征進行模型簡化，并在構(gòu)型空間中通過RRT算法搜尋無碰路徑，引導(dǎo)末端執(zhí)行器到達采摘位置點進行采摘任務(wù)；馬冀桐等[23]將障礙物映射至構(gòu)型空間，并基于此產(chǎn)生RRT-connect算法的引導(dǎo)點，大幅提高隨機樹的拓展速度；鄭嫦娥等[24]針對機械臂采摘規(guī)劃訓(xùn)練效率低的問題，分步進行無障礙、單一障礙、混雜障礙的遷移訓(xùn)練，提升算法性能，提高采摘效率。對于現(xiàn)有的工作環(huán)境中無解的情況，Xiong等[25]根據(jù)視覺信息，聚焦采摘目標附近的環(huán)境區(qū)域，生成推送路徑，有序采摘多個目標果實，并在過程中用機械臂推開障礙物，生成可行空間。

綜合國內(nèi)外的研究現(xiàn)狀可以看出，在運動規(guī)劃的各種算法中，基于采樣的PRM、RRT及其改進算法可以避免其他算法帶來的“指數(shù)爆炸”的高維計算問題，通過對采樣點的碰撞檢測大幅減少計算復(fù)雜度，提高規(guī)劃效率，特別適用于多自由度的機械臂規(guī)劃問題。但番茄的采摘環(huán)境屬于非結(jié)構(gòu)化空間，枝條、藤蔓等障礙物難以用規(guī)則體包絡(luò)的方式精確表達；番茄串體積相對較大，實際采收過程中，機械臂采收運動不僅要考慮如何采摘，還需要考慮剪切后番茄串如何避障、如何從復(fù)雜環(huán)境中提取出來的問題?，F(xiàn)有的番茄采摘運動規(guī)劃主要考慮如何避開障礙到達采摘果梗的位置，很少考慮機械臂末端夾持番茄串后因體積變大導(dǎo)致的果實提取困難的問題。為此，本文提出了基于空間分割的實時運動規(guī)劃算法最優(yōu)采摘空間（Optimal Picking Space，OPS），首先通過聚類分離環(huán)境中存在的枝條，并根據(jù)其空間位置將采摘空間分為多個子空間；然后通過探索子空間的體積是否容納串番茄通過以及子空間是否存在采摘的可行構(gòu)型解，判斷并排除無效子空間，在剩余的有效子空間中通過評價函數(shù)獲取最優(yōu)采摘子空間；將最優(yōu)采摘子空間作為機械臂路徑規(guī)劃的指導(dǎo)空間，并為機械臂的運動加入實時避障因子，指導(dǎo)機械臂實時避開障礙，保證番茄串的無損采收，完成采摘任務(wù)。

1 最優(yōu)采摘空間

1.1 環(huán)境建模

以溫室栽培番茄串采摘為研究對象進行環(huán)境建模。由于在番茄生長的前中后期要進行定期“疏葉”操作，在番茄成熟期，采摘果實區(qū)域附近基本上沒有葉子遮擋，因此影響采摘作業(yè)的障礙物主要為番茄植株的枝條。根據(jù)番茄的生長特點，遮擋串番茄的枝條基本以自上而下的方向垂落至地面。以藤蔓枝條為界，可以將采摘工作空間分割為多個子空間，有些子空間比較狹窄，雖然機械手末端剪刀可以進入，但采摘后，機械手末端攜帶較大體積的番茄串難以通過，這些子空間定義為無效子空間，在這些子空間的探索難以獲得可行解，屬于無效探索。通過幾百張被枝條遮擋的串番茄打標圖片的訓(xùn)練，YOLO-v3模型[26]能以較高的成功率獲取番茄果梗采摘點在RGB圖像中的像素位置，再配合深度圖與相機內(nèi)部、外部的參數(shù)，可以獲得番茄果梗采摘點在機械臂基坐標系下的位置Pd[27]。以此位置為中心，將獲取的點云圖通過直通、半徑與體素濾波后，可獲得番茄果梗采摘點附近的障礙物點云信息，環(huán)境建模流程如圖1所示。

針對番茄采摘環(huán)境中存在的枝條障礙物，利用密度聚類算法（Density-Based Spatial Clustering of Application with Noise, DBSCAN）[28]，將獲取的大量點云信息集合D分為k個點云集合Nc(c=1,2,…,k)，分別表示第c個枝條障礙物的點云信息。為了適應(yīng)枝條“近似垂直向下”的生長特點，修改距離計算函數(shù)dis()，將獲取點云中各點之間在機械臂基坐標系x、y軸上的距離放大，使其更利于枝條的分類，坐標值單位均為m（下文同）：

式中p(i)為點云集合D中的第i個點的位置，p(j)為點云集合D中的第j個點的位置，ε表示判斷點是否同類的閾值，β表示在坐標軸x、y軸的放大系數(shù)。

將多組枝條包含的點云數(shù)據(jù)聚類為k組后，分別提取各組枝條點的px、pz坐標和py、pz坐標，在機械臂基坐標系的z-y平面和z-x平面進行基于最小二乘法的四次多項式函數(shù)擬合，2個擬合曲面的交線可近似表達各組枝條障礙物的三維空間曲線，如式（3）：

式中aij，bij表示第i組枝條的第j個系數(shù)。

1.2 篩除無效子空間

如圖2所示，在獲取障礙物空間曲線的基礎(chǔ)上，取采摘點Pd的高度坐標為輸入，獲取此時各枝條同高度障礙點Ph的三維位置。k組枝條將機械臂的采摘空間劃分為k+1個子空間，子空間的并集構(gòu)成整體空間集W。

番茄串采摘采用6自由度機械臂，由于番茄串自身重力的影響，據(jù)實測統(tǒng)計，剪切點果梗方向基本垂直于地面的番茄串占85%以上，為了提高機器人剪切效率，減少不必要的姿態(tài)轉(zhuǎn)換，本文設(shè)定機械臂末端剪刀方向始終與地面平行，因此有效采摘動作需要5個自由度。機械臂末端剪刀到達采摘點Pd并平行于地面時，每個子空間的采摘姿態(tài)角度范圍是末端剪刀在2個相鄰枝條之間可以轉(zhuǎn)動的角度。每個子空間包含的采摘姿態(tài)角度范圍為

式中(i=1,2,…,k)，hiy表示第i個與采摘點同高度障礙物點的y軸坐標，hix表示第i個與采摘點同高度障礙物點的x軸坐標，r表示枝條的近似半徑，li表示第i個采摘點同高度障礙物與采摘點的距離，θi+1表示第i個子空間的采摘姿態(tài)集合。如圖3所示，以各枝條同高度障礙點Ph為采摘姿態(tài)角度（以y軸負方向為參考方向）的劃分依據(jù)，圖中所示為第二個子空間的采摘姿態(tài)角度范圍[θ2min,θ2max]。

機械臂末端執(zhí)行器伸入子空間后，剪切果梗，將番茄串夾取收回時，因為串番茄的體積較大，所以存在機械臂末端回程體積變大的問題。在選擇最優(yōu)采摘空間時，應(yīng)充分考慮番茄串在回程時的避障問題，避免番茄串因為與枝條接觸與碰撞造成果實損傷。通過計算相鄰枝條障礙物之間是否存在足夠夾爪攜帶串番茄返回的空間體積，篩選出不存在番茄串無碰撞通路的子空間集WC；由于機械臂的工作范圍有限，有些子空間在機械臂的工作空間之外，甚至有些子空間要求的采摘姿態(tài)機械臂無法到達，考慮這些因素，篩選出機械臂無法進入的子空間集WU；篩除WC和WU后的空間集即為機械臂可到達并能完成采摘動作、且番茄串可無碰撞取回的可行子空間集WD。

1.3 選擇最優(yōu)采摘空間

在可行子空間集WD中以5°為間隔遍歷采摘姿態(tài)角度范圍，可以在保證姿態(tài)角度樣本數(shù)量足夠的同時減少計算量，提高運算效率。通過Pieper解法求解并獲取機械臂采摘番茄構(gòu)型集Q。設(shè)定評價函數(shù)s，選擇最優(yōu)的一組構(gòu)型。一方面，盡量縮短機械臂在關(guān)節(jié)空間的運動距離，提高采摘效率；另一方面，考慮機械臂在摘取番茄時的可操作性，使得機械臂盡量遠離奇異構(gòu)型，則有：

式中si為第i個構(gòu)型的評分，qinit為初始構(gòu)型的關(guān)節(jié)向量，qi為第i個構(gòu)型的關(guān)節(jié)向量，為此關(guān)節(jié)向量的可操作度，μ1、μ2為采摘效率與可操作度的權(quán)重參數(shù)，因為可操作度的數(shù)值比關(guān)節(jié)空間距離的倒數(shù)值小一個數(shù)量級，在考慮盡量減小關(guān)節(jié)距離的同時兼顧可操作性，本文取μ1=1，μ2=5。

選取評分最高的采摘構(gòu)型，并求取此時的機械臂采摘位姿為

式中Td表示機械臂的采摘位姿矩陣，fk()表示機械臂末端執(zhí)行器的正運動學(xué)公式，qargMax(si)表示評分最高的機械臂構(gòu)型。除此之外，還為機械臂設(shè)定了初始和放置番茄位姿矩陣Tp，使其以合理的構(gòu)型將番茄串放置于果籃中，番茄果梗的放置位置為PP。

2 運動路徑規(guī)劃

確定機械臂工作的采摘位姿和初始位姿后，需要為機械臂在運動的過程中規(guī)劃一條無碰的較優(yōu)路徑，防止機械臂的連桿、關(guān)節(jié)與枝條和果實發(fā)生碰撞，對植株造成損傷或妨礙機械臂的正常運動。在最優(yōu)采摘空間中加入合理的進出空間路標點，引導(dǎo)機械臂的末端執(zhí)行器前端快速進入最優(yōu)采摘空間。首先考慮末端執(zhí)行器前端的避障問題，設(shè)定末端執(zhí)行器前端與障礙物的安全距離閾值d1（m），路標點保證機械手運動過程中其前端與障礙物的距離d>d1；退出空間路標點需要進一步考慮番茄串的避障問題，保證番茄串在最優(yōu)采摘子空間內(nèi)移動時與最近障礙物之間的距離大于安全閾值d2（m）。

在實際的采摘環(huán)境中，串番茄植株呈瀑布式生長，枝條與果梗剪切點的距離在120 mm以內(nèi)，本文所使用的采摘末端機械爪長度240 mm，故僅有末端執(zhí)行器進入最優(yōu)采摘空間。為了保證采摘空間內(nèi)末端執(zhí)行器運動過程中避開障礙，在末端執(zhí)行器上設(shè)置“感應(yīng)點”，使機械臂可以實時根據(jù)障礙物與感應(yīng)點的相對位置判斷此時的危險程度?？紤]干涉的臨界情況，根據(jù)幾何關(guān)系確定末端執(zhí)行器上感應(yīng)點與其前端的距離d3（m）。為機械臂設(shè)置合理的“執(zhí)行點”，使機械臂在獲得障礙物情況后及時做出有效避障動作，“執(zhí)行點”設(shè)置在機械臂的第六關(guān)節(jié)處，如圖4所示。

通過機械臂的運動學(xué)公式獲得感應(yīng)點的空間位置pr和執(zhí)行點的雅可比矩陣Job。

選定最優(yōu)采摘空間后，機械臂僅需考慮構(gòu)成子空間的至多2個枝條障礙物，通過擬合后的空間曲線獲得曲線中距離機械臂“感應(yīng)點”最近的空間位置pob和此處的空間曲率?ob，并以此求得機械臂“執(zhí)行點”的避障速度向量vob：

式中A為避障速度的方向參數(shù)，用于保證機械臂產(chǎn)生的避障速度方向總是與最近的障礙物相向，A=1表示機械臂產(chǎn)生的避障速度方向與遠離最近障礙物的方向相同，A=-1表示機械臂產(chǎn)生的避障速度方向與遠離最近障礙物的方向相反；dmin表示感應(yīng)點與pob的距離，m；γ1控制避障速度隨dmin變化的快慢；σ1為避障速度開始產(chǎn)生作用的控制閾值；α1為避障速度的峰值，m/s。將避障速度映射至機械臂的關(guān)節(jié)空間：

對于冗余機械臂，假設(shè)其自由度為n，其第i個關(guān)節(jié)角度為qi(i=1,2,…,n)，對應(yīng)關(guān)節(jié)向量為q，完成目標軌跡所需關(guān)節(jié)速度的通解[29]為

式中Jd為目標軌跡對應(yīng)的雅可比矩陣，為Jd的廣義逆矩陣，In為單位矩陣，為不影響機械臂末端沿目標軌跡運動時其他任務(wù)軌跡。因為機械臂末端剪刀方向始終與地面平行，此時采摘番茄串的目標任務(wù)需要限制機械臂5個自由度的運動：

其中位置任務(wù)ep與姿態(tài)任務(wù)eo為

式中pc為機械臂末端的實時位置，nc、oc、ac為機械臂末端的實時姿態(tài)，nd、od、ad為機械臂末端摘蕃茄串的姿態(tài)。將機械臂末端運動至最優(yōu)姿態(tài)作為子任務(wù)的第一個分任務(wù)：

機械臂執(zhí)行點根據(jù)感應(yīng)點與障礙物的實時狀態(tài)產(chǎn)生自避障運動作為子任務(wù)的第二個分任務(wù)：

式中λ為子任務(wù)的權(quán)重系數(shù)，γ2控制權(quán)重系數(shù)隨ep變化的快慢，σ2為權(quán)重系數(shù)開始變化的控制閾值，α2表示權(quán)重系數(shù)未偏置前的峰值，h表示權(quán)重系數(shù)的偏置參數(shù)。為使采摘機器人在距離障礙物較遠時提高運動效率，靠近障礙物時增加避障運動的權(quán)重，靠近目標果實時增加姿態(tài)變化的權(quán)重，機械臂根據(jù)感應(yīng)點與障礙物的距離自主實時切換運動策略：

1）當ep較大時，λ→h，機械臂末端距離目標較遠，此時機械臂采取“避障優(yōu)先”的運動策略，防止運動過程中機械臂本體與枝條的碰撞損壞植株；

2）當ep逐漸縮小時，λ增大，機械臂末端靠近目標，此時機械臂逐漸從“避障優(yōu)先”的運動策略轉(zhuǎn)換為“姿態(tài)優(yōu)先”的運動策略，兼顧機械臂的避障運動和末端最優(yōu)姿態(tài)的調(diào)整；

3）當ep→0時，λ→1+h，機械臂末端已接近目標區(qū)域，此時機械臂完全切換為“姿態(tài)優(yōu)先”的運動策略，調(diào)整末端的姿態(tài)，完成采摘任務(wù)。

3 采摘路徑規(guī)劃試驗

3.1 采收試驗平臺的搭建

番茄機器人采收系統(tǒng)如圖5所示。

番茄串的采摘需要5個自由度，為了完成采摘任務(wù)的同時完成避障子任務(wù)，系統(tǒng)使用AUBOI3六自由度協(xié)作機械臂，最大負載3 kg，安裝串番茄采摘執(zhí)行器后工作半徑800 mm，重復(fù)定位精度±0.02 mm；攝像機為Intel? RealSense? Depth Camera D415攝像機，RGB分辨率1 920×1 080，最小深度距離0.16 m，深度視場角65°×40°×72°，安裝在機械臂基座的側(cè)面，保證采收過程中不被遮擋；系統(tǒng)上位機搭載COREI7處理器、8G內(nèi)存、GTX1050顯卡，操作系統(tǒng)為Ubuntu16.04 LTS。

為了多視角觀測，驗證算法的有效性，隨時復(fù)現(xiàn)和分析試驗數(shù)據(jù)，根據(jù)真實試驗平臺在ROS-kinetic上搭建采收仿真系統(tǒng)[30]，用于同步顯示機械臂的運動情況，并收集反饋數(shù)據(jù)信息。

3.2 運動過程分析

采摘過程中，機械臂經(jīng)歷從初始狀態(tài)、進入采摘空間、采摘果實、夾取果實離開采摘空間、回收果實幾個階段。為了更清楚地描述采收過程，以其中一組為例進行說明。通過RGB-D相機獲取環(huán)境信息后，將獲取的點云數(shù)據(jù)聚類擬合處理，作為采收系統(tǒng)的障礙物環(huán)境。在環(huán)境建模完成后，OPS算法為采收機械臂選定最優(yōu)采摘空間，規(guī)劃一條合理的運動路徑，整個運動過程如圖6a所示。為了更好地體現(xiàn)算法在整個采收過程的有效性，在ROS建立的仿真系統(tǒng)中可多視角記錄機械臂的運動狀態(tài)，為了更清晰表示機械臂的運動，選擇從側(cè)俯視角觀測，如圖6b所示。

OPS算法為機械臂在選定的最優(yōu)采摘空間內(nèi)設(shè)定合理有效的采摘姿態(tài)，然后根據(jù)番茄串與構(gòu)成最優(yōu)采摘空間的枝條障礙物的位置關(guān)系，規(guī)劃漸進最優(yōu)的運行路徑，機械臂末端執(zhí)行器在進入采摘空間后，不斷調(diào)整末端姿態(tài)，在完成采摘的任務(wù)同時進行避障子任務(wù)，防止與枝條障礙物發(fā)生碰撞。圖7表示機械臂躲避障礙采摘番茄的過程，其中圖7a、圖7b、圖7c是相對機械臂的正面視角觀測圖，圖7d、圖7e、圖7f為同一采摘動作側(cè)俯視觀察圖。為了便于說明，采摘空間的2個障礙枝條表示為①②，如圖7a、圖7d所示。機械手末端為了達到剪切目標點，又不與枝條①②發(fā)生碰撞，根據(jù)1.3節(jié)式（5）選擇最優(yōu)姿態(tài)，進行剪切果梗的任務(wù)，此時的采摘姿態(tài)兼顧了采摘效率與可操作度，姿態(tài)調(diào)整過程如圖7b、圖7c、圖7e和圖7f所示，為了不與枝條①碰撞，機械手末端逐漸改變姿態(tài)到達采摘目標點。在剪切并夾取番茄串后，機械臂將攜帶果實從采摘空間中出來，并放置儲存箱。在回程中的避障規(guī)劃如圖8所示，因為枝條障礙物①②之間的下端距離過于狹窄，OPS算法為串番茄返回時設(shè)定了出口點，在此次試驗中體現(xiàn)為“向上抬起”的動作，從而使機械臂夾取串番茄無碰返回。由此可見，算法生成的路徑與人工采摘路徑非常相似，人的采摘技能可以通過該算法得到實現(xiàn)。

采摘運動過程中，機械臂各關(guān)節(jié)角度隨時間變化如圖9所示，機械臂的各關(guān)節(jié)角度變化連續(xù)，沒有發(fā)生突變，運行平穩(wěn)。

由于番茄串的生長特點，在采摘過程中，枝條①、②與采摘機器人的最近距離點主要分布在末端執(zhí)行器上，可在末端執(zhí)行器上設(shè)置多個“位置點”，根據(jù)公式（3）確定，每一時刻末端執(zhí)行器與2個障礙物①、②之間的最短距離。如圖10所示，末端執(zhí)行器與構(gòu)成最優(yōu)采摘空間的2個枝條障礙物之間的最小距離大于0.1 m，達到了避障動作要求，符合安全采摘。

3.3 試驗驗證

為驗證OPS算法的性能，針對溫室栽培番茄串的特點，在串番茄培育基地進行了大量的采摘試驗，如圖11所示。

為驗證OPS算法的效率，進行了與Lazy-PRM*和RRT*-connect算法的對比試驗，同時為機械臂設(shè)置采收方案，并與人工采摘番茄工作相比，試驗結(jié)果如表1所示。

表1 采摘試驗結(jié)果 Table 1 Picking test results

由表1可知，RRT*-connect與Lazy-PRM*算法的單串番茄平均采摘時間分別為18.19和26.75 s，人工采摘時間約為15.50 s，而OPS算法的單串番茄平均采摘時間為12.51 s，采摘效率與RRT*-connect相比提升了31.23%，與Lazy-PRM*相比提升了53.23%，與人工采摘相比提升了19.29%。RRT*-connect與Lazy-PRM*算法在工作空間內(nèi)隨機采樣結(jié)點，其結(jié)點數(shù)在2 000個左右，而OPS算法提前為機械臂規(guī)劃合理的采摘空間，拓展結(jié)點數(shù)目大幅降低，采摘成功率為99.81%。

4 結(jié) 論

本文針對溫室栽培串番茄的特點，基于采摘空間分割，考慮番茄串采摘和果實提取的采收全過程，提出最優(yōu)空間運動規(guī)劃算法。該算法可引導(dǎo)機械臂以漸進最優(yōu)的采收動作完成任務(wù)。與現(xiàn)有的算法相比，由于OPS算法根據(jù)環(huán)境與番茄串的位置信息提前為機械臂規(guī)劃采摘子空間，避免在其他無效子空間內(nèi)探索有效路徑；OPS算法同時具有實時性，通過聚類擬合的環(huán)境建模，OPS算法可實時根據(jù)障礙物與機械臂之間的相對位置調(diào)整機械臂末端姿態(tài)，實現(xiàn)避障。

試驗結(jié)果表明：在串番茄的采摘環(huán)境下，利用6自由度機械臂，基于OPS算法的單串番茄從番茄串識別到收納到存儲籃內(nèi)的平均采收時間為12.51 s，與RRT*-connect算法相比降低了31.23%，與Lazy-PRM*算法相比降低了53.23%，與人工采摘相比降低了19.29%，工作效率大幅提高；OPS算法在最優(yōu)采摘空間設(shè)置有效的路標點，與隨機性拓展結(jié)點的RRT*-connect算法和Lazy-PRM*算法相比，大幅減少了路徑的結(jié)點數(shù)，采摘成功率為99.81%，滿足智能采摘的實時性要求。