矮化密植果園多臂采摘機器人任務規劃

李 濤，邱 權※，趙春江，謝 豐,3

（1. 北京市農林科學院北京農業智能裝備技術研究中心，北京 100097；2. 北京市農林科學院北京農業信息技術研究中心，北京 100097；3. 江蘇大學農業工程學院，鎮江 212000）

0 引言

中國是世界最大水果生產國和消費國，預計2025年水果行業市場規模將達到27460.1億元左右[1]。然而國內水果生產綜合機械化率與發達國家相比仍較低，僅為28.6%，特別是采摘階段，采摘機械化率不足3%[2]。隨著勞動力成本的上升和人口老齡化問題的加劇，鮮果產業發展面臨瓶頸[3]。

近年來，矮化密植栽培標準果園規模不斷擴大，為果園智能化、機械化作業提供了有利條件，采摘機器人與新型農藝相結合成為水果產業發展新趨勢[4]。目前，果園采摘機器人研究在果實識別定位[5]、路徑規劃[6]、位姿調節[7]、柔性抓取[8]等方面取得了積極進展。但總體而言，仍普遍存在效率低的問題，難以滿足鮮果采摘的實際需求[9]。特別是在矮化密植標準果園規模采摘的需求背景下，提升采摘機器人作業效率迫在眉睫。

針對作業效率問題，Williams等[10]研究了具有4個3自由度（Degree of Freedom，DoF）串聯關節型機械臂的獼猴桃采摘機器人，協同開展采摘作業，并能有效避免各機械臂間碰撞；Fu等[11]研制了具有4個3DoF直角坐標機械臂的獼猴桃協同采摘機器人；西班牙AGROBOT公司的草莓采摘機器人[12]配備了24個機械臂，同時開展作業；以色列蘋果收獲機器人FFRobotics[13]具有4個采摘機械臂，可實現1 s/個的采摘速度。使用多機械臂來提高作業效率，已成為領域內研究者的共識[14]。

不同于單臂采摘機器人，多臂協同采摘的突出挑戰是如何根據果實分布情況對各臂任務進行合理規劃和調度以有效提升工作效率[15]。為此，一些研究者根據目標分布劃分相互獨立的作業區域，并分配不同機械臂進行作業[16-18]。例如：針對簇狀生長特征明顯的作物，研究者通過果實分布聚類情況來劃分不同臂的作業范圍。Williams等[16]提出的奇異果多臂協同采摘機器人，以二維水平歐式距離為判斷標準，對果實進行聚類，并據此為各個機械臂的任務分區；Fu等[11]采用改進K-means的方法對奇異果果簇分布進行聚類。上述研究均假定多臂機器人可到達視野范圍的各個區域，對于其他簇狀生長特征并不明顯的果實，上述方法存在初始聚類中心難以選取的問題[17]。另一類多臂協作方式為直接對多臂作業空間進行分離，使各臂能夠各司其職，工作區域互不交叉[12-13,18]，這種方式有效避免了不同機械臂的沖突問題，可實現多臂并行工作。然而，這類機器人各臂的實際工作范圍受幾何結構或運動學的限制，存在各臂均不可達的作業區域，導致漏采。

隨著采摘規模、機械臂數量和作業范圍的增加，死區、漏采和各臂沖突等問題愈加突出[19]，多臂作業空間解耦與采摘死區間存在著矛盾關系。為解決死區問題，需將各臂的作業區域進行適度重疊，即每個機械臂既有專屬的采摘作業范圍，也有共同的采摘區域，從而實現作業范圍完全覆蓋。

多機械臂采摘不同區域的果實可視為一類旅行商調度問題，但由于重疊區域的存在以及避免多機械臂沖突的需求，傳統旅行商問題[20-21]（Travelling Salesman Problem，TSP）和多旅行商問題[22-23]（Multiple TSP，MTSP）難以適用。為此，本文提出異步重疊訪問域的旅行商問題（Asynchronous Overlapped MTSP，AOMTSP），解決重疊訪問域和多個旅行商同時進入重疊域的沖突。遺傳算法（Genetic Algorithm，GA）是求解TSP及MTSP問題的重要方法，已得到諸多成功應用。

鑒于此，本文基于AOMTSP研究矮化密植果園多臂采摘機器人的協同作業任務規劃問題，參考MTSP建立0-1規劃數學模型，基于GA提出AOMTSP的求解方法。通過面向矮化密植果園的多臂直角坐標機器人系統，研究基于AOMTSP-GA的多臂協作機器人作業任務規劃策略，以避免重疊域內多臂沖突，確保機器人以最短時間遍歷作業范圍內的所有目標果實，提高作業效率。

1 多臂協同采摘機器人作業任務規劃

本文研究對象為基于直角坐標系的多臂采摘機器人，如圖1所示，主要應用于矮化密植果園。該機器人在機械結構上由4個直角坐標機械臂構成，各單元包含X、Y、Z方向共3個直線模組和旋轉關節。末端裝配果梗果實分離機構和果實傳輸波紋管。機器人總體結構參數如圖1所示。

當直角坐標機械臂單元的各直線模組XYZ主軸夾角為90°時，各采摘單元的作業范圍在空間上互相分離，可開展并行作業。但在各機械臂相鄰作業區域的銜接處，受幾何結構的限制存在不可達作業區，如圖2所示，形成采摘死區，導致漏采。

為解決采摘死區問題，需擴展各執行器可達范圍，使各臂作業區域均能覆蓋采摘死區。針對本文的多臂協同采摘機器人，對機器人結構進行如下改進：1）將X軸改為上下貫通的共享導軌，使上下2個機構可共享中間區域，形成重疊采摘區；2）將Z軸和Y軸導軌增加旋轉機構，使其可左右調節，形成一定的夾角，當左右兩側對準中間區域時，可形成重疊采摘。采摘作業時，為減少總行程、提高作業效率，避免各機械臂在同時訪問重疊域時發生沖突，須合理規劃各機械臂采摘任務，并遵從如下規則：

1）果實僅能被1個機械臂采摘，摘取后其他機械臂不必訪問該果實位置；

2）不限制機械臂對專屬采摘區和重疊采摘區內果實的摘取順序，允許各臂競爭重疊區域內的所有可達目標；

3）限制2個及以上機械臂同時進入重疊采摘區；

4）不限制各臂的初始采摘果實目標，各機械臂在果實間行進速度相同，總遍歷時間不包含機械臂摘取果實（即停留在果實目標處）的時間；

5）所有果實目標摘取結束后，遍歷總時長最短。

為保證本文任務規劃方案的合理性，提出如下前提和假設：

1）假設視覺感知系統能夠準確檢測并定位果實的三維空間位置；

2）不考慮被葉片完全遮擋的果實；

3）多機械臂協同作業任務規劃階段不考慮機械臂在不同果實間切換時的路徑差異，認為果實間的切換時長與其歐式空間距離為線性關系，且速度因子不變。

由于深度卷積神經網絡具有出色的表征提取能力和學習能力，結合深度傳感器、多線激光雷達等先進傳感器，目標果實的檢測定位并不困難，但完全或近似被完全遮擋的果實感知問題仍未有有效的解決方案。而矮化密植標準果園的冠層充分窄化，果實分布近似于“果墻”，果實拓撲結構簡單，避障動作易實現。

2 異步重疊訪問域多旅行商問題描述與求解

2.1 問題描述

根據本文任務規劃規則可知：專屬作業區訪問規則約束與傳統旅行商問題（Travelling Salesman Problem，TSP）類似（規則5））；多個機械臂的重疊采摘域分配問題與多旅行商問題（Multiple TSP，MTSP）類似（規則1）、2）和5）），但簡單混合TSP和MTSP難以滿足規則3）、4）約束。鑒于此，本文提出異步重疊訪問域多旅行商問題（Asynchronously Overlapped Multiple Traveling Salesman Problem，AOMTSP），具體描述如下：

設存在N個分布于不同位置的城市，C={1,...,N}表示所有城市的集合，并包含M+S個互不相交的子集，即有其中{Cm,m=1,...,M}表示M個專屬城市集的集合，{CS,s=M+1,...,M+S}表示S個共享城市集的集合?？紤]M個旅行商依次訪問所有城市，要求第m個專屬城市集僅能被第m個旅行商訪問，第s個城市集可被某2個及以上旅行商訪問，并假設各旅行商由某一城市到下一城市時以恒定速度沿直線行走，各旅行商在城市停留時長為Thold，確定M個旅行商完全遍歷N個城市的訪問序列，使所有旅行商遍歷所有城市的總時長最短，且共享城市僅可被游歷1次；相鄰2個旅行商同時訪問其對應的共享城市集合中城市時，排隊等候時間為Tw。

為統一術語，將果實記作城市，機械臂記作旅行商。假設作業區域內有36顆果實，按圖3分布，4個采摘臂協同作業，則各集合元素示例如表1所示。

表1 異步重疊訪問域多旅行商問題示例 Table 1 Examples of Asynchronously Overlapped Multiple Traveling Salesman Problem (AOMTSP)

2.2 求解方法

本文提出基于遺傳算法的AOMTSP求解方法，以下記作AOMTSP-GA。首先確立符合約束條件的編解碼方案；然后遵循經典GA的選擇、交叉和編譯算子對編解碼方案進行種群迭代，最終形成符合預期的訪問序列。

2.2.1 AOMTSP-GA染色體編碼

AOMTSP-GA的染色體是AOMTSP解的表達形式，不同的染色體唯一確定各臂對多目標的采摘訪問序列[24]。選取恰當的染色體編碼方案是AOMTSP-GA的關鍵。

傳統MTSP方案常選用間隔點方法區分不同旅行商的序列段，但由于AOMTSP存在重疊訪問域，傳統MTSP方案無法正確描述。為此，本文提出雙染色體編碼方案：先將一組城市編號的有序數組構造為城市染色體，再將一組旅行商編號的有序數組構造為與城市染色體等長的旅行商染色體，旅行商序號指示城市染色體中對應位置的城市所屬關系集合。在不顛倒各旅行商訪問序列順序的前提下，4個旅行商所訪問城市編號任意穿插，形成城市染色體；根據城市與旅行商的對應關系，依次列出旅行商染色體數組；根據旅行商染色體中的序號，依次讀取城市染色體相應位置上的城市編號，順序存入該旅行商訪問序列數組中，解碼各旅行商訪問次序。

2.2.2 選擇算子

選擇算子將優化的個體直接傳到下一代或將配對交叉產生的新個體遺傳到下一代。常用的選擇策略包括基于適應度比例（輪賭盤策略）、基于排名、基于局部競爭機制（錦標賽策略）、最佳個體保存等策略[25]。其中最佳個體保存策略收斂性較好，輪賭盤策略可避免局部最優[26]。

為提高算法收斂速度，同時提高全局搜索能力，本文采用最佳個體保存與輪盤賭選擇相結合的策略。首先對每代種群所有個體計算適應度并按照適應度排序，保留適應度最佳的個體，直接傳入下一代；剩余個體采用輪盤賭選擇策略產生，并傳入下一代。

2.2.3 交叉算子

交叉算子將2個父代個體的部分結構重組生成新個體[27]，以提高GA 的搜索能力。首先采用部分匹配交叉（Partially Matched Crossover，PMX），交叉2個父代城市染色體，然后利用旅行商染色體檢測交叉后的專屬城市分配，查看是否符合專屬訪問區約束。首先隨機生成2個位串交叉點，定義這2個點之間的區域為1個匹配區域，隨后交換2個父代城市染色體的匹配區域，最后檢測交換后的2個父代染色體匹配區域外元素是否與匹配區域內重復，若重復則逐一替換為匹配區域內對應的元素。

2.2.4 變異算子

變異算子負責對城市染色體和旅行商染色體內的編碼進行變異[28]。變異算子包含若干種不同的變異操作以維持種群的多樣性，防止過早收斂。首先隨機生成2個基因位置分別作為變異段的起止位置，隨后分別采用翻轉、交換、滑動來重排城市基因段；旅行商染色體變異的實質是對共享城市集內城市的旅行商映射進行再分配。

2.2.5 異步規則

為滿足任務規劃規則3），避免不同旅行商同時訪問1個共享城市集，需加入異步規則，增加排隊時長，以還原多機械臂任務規劃過程中2個或2個以上旅行商訪問1個共享區域的等候過程。

異步規則的關鍵在于建立旅行商時間表。首先定義專屬城市集合內各城市間距離如下：

式中dTL，dTR，dBR和dBL為旅行商TL、TR、BR和BL的專屬城市區域距離向量；2,1d為城市1至城市2的距離，共享城市區域距離向量dOC,… ,dOR也可得到。

然后根據城市間距離計算旅行商訪問時間。假設旅行商TL已依次訪問專屬城市1～6，正在訪問共享城市26，從起點開始計時，則當前時刻為

接下來檢查各旅行商訪問相同共享城市集內城市的時刻，查看是否有交叉，若存在交叉則需為旅行商增加排隊等候時間Tw。按照時間先后，規定后訪問共享城市集的旅行商需等待Tw，并更新該旅行商的時間表。重復上述步驟，反復檢查并更新各旅行商時間表，直至各旅行商時間表中均不存在共享城市集內的交叉訪問。

由于排隊時間的增加將直接導致總耗時的增加，因此異步規則中所設定的排隊約束條件是一個懲罰性約束，以引導算法向排隊時間和總耗時更短的方向進化。

2.2.6 適應度計算

AOMTSP優化目標為各旅行商遍歷各城市的時間總和最小。在考慮規則4）、規則3）和優化目標的前提下，設計適應度函數F(x)為

式中c為旅行商行進速度，m/s；f(x)為當前訪問序列下的總路徑長度，m；n為旅行商同時訪問相同共享城市集合的次數。由式（3）可知，適應度越大表明個體的性能越好，AOMTSP的優化目標為適應度函數F(x)的函數值最大[29]。

3 基于AOMTSP-GA的多臂采摘協同任務規劃試驗

3.1 雙臂協同采摘機器人軟硬件系統集成

本試驗基于圖4所示的多臂采摘機器人硬件平臺進行，包括：1）2組直角坐標機械臂。每組機械臂具有3個平移自由度和1個旋轉自由度，重復定位精度為±0.02～0.05 mm，有效行程為X軸650 mm、Y軸650 mm、Z軸450 mm，R軸±30°，有效載荷10 kg，運行速度50 mm/s，其中各關節電機采用直流伺服電機驅動，基于CANOpen總線協議控制，X軸、Y軸電機功率為750 W，Z軸電機功率為400 W，R軸電機功率為100 W；2）支撐框架為80 mm×80 mm鋁制型材，長×寬×高為2 m× 0.6 m×0.46 m；3）深度相機為英特爾Realsense D455，采用紅外雙目立體感知技術，最高分辨率為1280像素×720像素，深度測量范圍為0.4～6 m，全局快門，數據接口為USB3.0 Type-C；4）嵌入式計算機為NVIDIA Jetson TX2，NVIDIA PascalTM架構，圖形處理器單元為NVIDIA MaxwellTM，有256個CUDA內核，中央處理器單元為Quad ARM?Cortex-A系列A57。

嵌入式計算機TX2操作系統為Ubuntu 18.04。本試驗中機器人采用ROS-Melodic進行控制系統開發，機械臂運動規劃使用ROS-Moveit!功能包。圖5為ROS-Moveit!的雙臂協同采摘機器人控制系統示意圖。

為驗證本文AOMTSP-GA任務規劃算法在實際機器人系統中的有效性，基于上述機器人軟硬件平臺，開展協同任務規劃試驗。具體試驗內容包括：1）果實檢測與定位試驗，以驗證視覺檢測與定位系統的有效性；2）根據前期矮化密植果園調研結果，本文機器人平臺作業范圍內果實數量為20～90 顆，平均約為43顆，因此試驗分別選取43（平均數）和90顆果實2種情況，開展任務規劃仿真試驗，檢驗本文AOMTSP-GA的收斂情況及任務規劃性能；3）基于多臂機器人平臺開展實際試驗，驗證本文所提方法在3種不同果實分布情況下的AOMTSP-GA任務規劃效果。

3.2 果實目標檢測與定位試驗

試驗在實驗室和實際果園2種環境中開展，實驗室環境中采摘面長1.8 m、高2.2 m、冠層深0.1 m，如圖6a所示。實際果園位于北京市昌平區天匯園，果樹品種為SH6矮化中間砧型富士蘋果，采摘面高約2.8 m、冠層深0.4 m，如圖7a所示。

多臂協同采摘感知系統硬件平臺采用Realsense D455深度相機進行圖像采集，NVIDIA Jetson TX2作為圖像邊緣計算單元。首先基于YOLOV5深度卷積神經網絡模型開展，通過深度相機RGB圖像對果實進行多目標在線檢測；得到RGB圖像檢測結果后，在對齊的深度圖中提取果實區域的深度信息，并結合相機內參矩陣求取果實目標的三維空間位置，進而得到所有目標果實的三維空間分布情況，識別結果如圖6b和圖7b所示。

為便于采摘的任務規劃，對目標果實識別與定位結果根據本文任務分區方法（圖3）作進一步劃分，得到如圖6c和圖7c所示的作業區域。

3.3 多臂協同采摘機器人任務規劃仿真試驗

采用AOMTSP-GA和隨機遍歷2種算法分別進行2組仿真試驗，具體參數設置如表2所示，目標果實的分布方案如圖8a、圖9a、圖10a所示，試驗結果如表 3所示。

43顆目標果實的規劃試驗結果表明，相較于隨機遍歷算法，AOMTSP-GA算法的采摘任務規劃作業遍歷時長縮短40.97%，遍歷路徑如圖8b所示；算法迭代500次收斂，如圖8c所示。90顆目標果實的試驗中，相較于隨機遍歷算法，2種參數條件下的AOMTSP-GA算法作業遍歷時長分別減少44.53%和54.98%，遍歷路徑分別如圖9b和圖10b所示。如圖9c和圖10c所示，適當增大種群數量和交叉變異比參數，可使優化解更接近于全局最優解，但算法收斂的迭代次數和求解時間也相應增加。

為說明多臂協同采摘相比于單臂的優越性，采用順序遍歷的采摘方案，以單機械臂采摘90顆目標果實作為對比。試驗中設單臂單果采摘單次耗時3 s，末端執行器行進速度1 m/s。結果表明：單臂作業的采摘總耗時約為270 s，遍歷路徑規劃總耗時約為140 s，合計總時長約為410 s；單臂采摘方案總耗時約為多臂AOMTSP-GA算法的4.28倍。

表2 多臂協同規劃算法參數取值設置 Table 2 Configuration of parameters in the multiple manipulator cooperatively planning algorithm

表3 不同果實分布方案下4臂協同采摘機器人任務規劃仿真試驗結果 Tabel 3 Simulation results of task planning of 4-arm cooperative picking robot under different fruit distribution schemes

3.4 雙臂協同采摘機器人多目標采摘試驗

為驗證本文AOMTSP-GA任務規劃算法在實際機器人系統中的有效性，基于圖4所示雙臂協同采摘機器人硬件平臺開展實際采摘任務規劃試驗。

首先按照統一機器人描述格式（Unified Robot Description Format，URDF）編寫機器人模型描述文件，并基于ROS-Moveit!系統[30]配置運動學規劃器，并建立雙臂協同采摘機器人控制系統；然后將AOMTSP-GA算法部署于ROS分布式通訊系統內，初始化規劃器節點，該節點以話題形式訂閱果實多目標三維位置信息，并通過回調函數實時計算左右2個臂的果實采摘遍歷順序，然后以話題形式將目標果實的序列信息發布；配置左右2個機械臂控制節點分別訂閱規劃器節點發布的采摘序列話題，調用Moveit!運動規劃組求解運動學逆解，生成各關節位置、速度、加速度軌跡點，形成左右臂2個運動學組的動態軌跡，驅動雙臂各自抵達當前笛卡爾目標位置；當左右2個機械臂抵達當前果實目標位置后，反饋執行結果至規劃節點，規劃節點更新采摘序列話題，以話題形式發布下一目標果實位置信息，并重復上述步驟，直至完成所有果實的遍歷。

基于ROS-Moveit!開發雙機械臂運動規劃器，使用開源OMPL（Open Motion Planning Library）機器人運動規劃庫。OMPL涵蓋當前最新的機器人運動規劃算法，適用于高維空間和復雜約束下的機器人路徑求解，可避免高維度機械臂規劃中的維度爆炸問題。幾何約束及運動學約束配置如下：1）根據規劃分區和X滑軌尺寸，設置2個機械臂的沿X軸最大活動范圍為0～1 m；2）由于結構限制，將第三連桿所裝配的旋轉滑臺活動范圍限制為60°。完成OMPL規劃器配置后，添加規劃請求適配器組，為軌跡路點配置速度和加速度參數，形成動態運動軌跡。運動學逆解器采用IKFast，該求解器得到的運動學逆解為封閉解，求解速度更快并支持零空間解，是目前求運動學逆解的先進算法。此外，考慮到矮化密植果園園藝特征[31]，試驗中不在工作空間添加障礙物，將柔性枝葉視為可碰撞物體。

按照2個機械臂左右分布，劃分左側專屬采摘區域L、右側專屬采摘區域R和重疊采摘區O，按照由均勻到集中的3種果實分布分3組進行試驗：試驗1果實分布相對稀疏、均勻，L區分布11顆果實，R區分布5顆果實，重疊區域O分布12顆果實；試驗2果實分布較為集中，L、R、O各區果實數量分別為6、6和16；試驗3果實分布最為集中，L、R、O各區數量分別為5、3和20。以順序規劃和隨機遍歷2種方法作為對比。

試驗結果如表4所示，對于試驗1，本文方法相比于順序規劃法和隨機遍歷法，路徑總長度分別減少47.5%和51.16%，遍歷總時長分別減少10.69%和27.18%。當果實為布局2時，果實分布相對集中于重疊采摘區，3種方案的路徑總長度均有所下降，本文方法未發生訪問沖突，而另外的2種方法和均出現不同程度的訪問沖突，增加了遍歷時長，本算法較順序規劃法和隨機遍歷法時長減少了20.45%和23.33%。隨著果實進一步集中，當果實為布局3時，3者遍歷總長度均顯著減少，但訪問沖突情況增加，排隊等候時間對遍歷總時長的影響最為顯著，本文方法的異步訪問優化機制效果明顯，有效降低了沖突次數，縮短了遍歷總時長，相較于順序規劃法和隨機遍歷法，遍歷時長分別減少12.94%和21.69%。

表4 不同果實分布下雙臂協同采摘機器人采摘試驗結果 Table 4 The experimental results of dual-manipulator robotic harvester under different distribution of fruits

綜上，果實分布均勻稀疏情況下，遍歷路徑總長度對總作業時間影響較為明顯，優化多機械臂的采摘遍歷路徑長度可有效提升效率；果實分布較為密集情況下，僅優化遍歷路徑長度對效率的提升不大，訪問沖突引起的排隊等候占主導地位，異步訪問協調機制作用明顯。本文任務規劃方案以遍歷總時間為優化目標，考慮訪問沖突和遍歷路徑長度對總時長的影響，不同的果實分布條件下，均可提升采摘效率。

4 結論

1）本文將矮化密植果園多機械臂機器人采摘任務規劃歸納為異步重疊訪問域多旅行商問題，基于遺傳算法給出了問題的求解方法。仿真試驗結果表明：所提方法求解不同重疊訪問域多旅行商問題時，可分別在500、2000和2500次迭代后收斂至滿意解，具有良好的收斂性。

2）針對多個機械臂訪問重疊區內果實發生沖突的問題，本文方法引入異步規則，對不滿足異步規則的任務規劃方案施加懲罰，以總遍歷時長為優化目標，確保重疊訪問域內多臂異步協同；仿真算例表明，本文方法相比于隨機遍歷方法，在43顆果實條件下遍歷時長可縮短40.97%，在90顆果實條件下分別最多可縮短54.98%；單臂采摘90顆果實的遍歷時長約為本文算法4.28倍。

3）在實驗室環境下，設置了3種不同情況的果實分布，開展2個機械臂的協同采摘試驗，結果表明，本文在3種果實分布情況下，相較于順序規劃法和隨機遍歷法，本文方法的作業遍歷時長分別縮短10.69%和27.18%、20.45%和23.33%以及12.94%和21.69%。