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

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

（1. 華南理工大學機械與汽車工程學院，廣州 510641；2. 廣東省現代農業裝備研究所，廣州 510630）

0 引言

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

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

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

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

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

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

1 最優采摘空間

1.1 環境建模

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

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

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

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

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

1.2 篩除無效子空間

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

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

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

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

1.3 選擇最優采摘空間

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

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

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

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

2 運動路徑規劃

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

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

通過機械臂的運動學公式獲得感應點的空間位置pr和執行點的雅可比矩陣Job。

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

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

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

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

其中位置任務ep與姿態任務eo為

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

機械臂執行點根據感應點與障礙物的實時狀態產生自避障運動作為子任務的第二個分任務：

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

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

2）當ep逐漸縮小時，λ增大，機械臂末端靠近目標，此時機械臂逐漸從“避障優先”的運動策略轉換為“姿態優先”的運動策略，兼顧機械臂的避障運動和末端最優姿態的調整；

3）當ep→0時，λ→1+h，機械臂末端已接近目標區域，此時機械臂完全切換為“姿態優先”的運動策略，調整末端的姿態，完成采摘任務。

3 采摘路徑規劃試驗

3.1 采收試驗平臺的搭建

番茄機器人采收系統如圖5所示。

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

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

3.2 運動過程分析

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

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

采摘運動過程中，機械臂各關節角度隨時間變化如圖9所示，機械臂的各關節角度變化連續，沒有發生突變，運行平穩。

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

3.3 試驗驗證

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

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

表1 采摘試驗結果 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*算法在工作空間內隨機采樣結點，其結點數在2 000個左右，而OPS算法提前為機械臂規劃合理的采摘空間，拓展結點數目大幅降低，采摘成功率為99.81%。

4 結 論

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

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