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

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

0 引言

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

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

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

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

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

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

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

1.1 環(huán)境建模

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

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

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

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

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

1.2 篩除無效子空間

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

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

式中(i=1,2,…,k)，hiy表示第i個(gè)與采摘點(diǎn)同高度障礙物點(diǎn)的y軸坐標(biāo)，hix表示第i個(gè)與采摘點(diǎn)同高度障礙物點(diǎn)的x軸坐標(biāo)，r表示枝條的近似半徑，li表示第i個(gè)采摘點(diǎn)同高度障礙物與采摘點(diǎn)的距離，θi+1表示第i個(gè)子空間的采摘姿態(tài)集合。如圖3所示，以各枝條同高度障礙點(diǎn)Ph為采摘姿態(tài)角度（以y軸負(fù)方向?yàn)閰⒖挤较颍┑膭澐忠罁?jù)，圖中所示為第二個(gè)子空間的采摘姿態(tài)角度范圍[θ2min,θ2max]。

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

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

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

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

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

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

2 運(yùn)動(dòng)路徑規(guī)劃

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.1 采收試驗(yàn)平臺(tái)的搭建

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

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

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

3.2 運(yùn)動(dòng)過程分析

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

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

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

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

3.3 試驗(yàn)驗(yàn)證

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

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

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

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

4 結(jié) 論

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

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