矮化密植果園多臂采摘機器人任務(wù)規(guī)劃

李 濤，邱 權(quán)※，趙春江，謝 豐,3

（1. 北京市農(nóng)林科學(xué)院北京農(nóng)業(yè)智能裝備技術(shù)研究中心，北京 100097；2. 北京市農(nóng)林科學(xué)院北京農(nóng)業(yè)信息技術(shù)研究中心，北京 100097；3. 江蘇大學(xué)農(nóng)業(yè)工程學(xué)院，鎮(zhèn)江 212000）

0 引言

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

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

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

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

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

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

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

1 多臂協(xié)同采摘機器人作業(yè)任務(wù)規(guī)劃

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

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

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

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

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

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

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

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

為保證本文任務(wù)規(guī)劃方案的合理性，提出如下前提和假設(shè)：

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

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

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

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

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

2.1 問題描述

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

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

為統(tǒng)一術(shù)語，將果實記作城市，機械臂記作旅行商。假設(shè)作業(yè)區(qū)域內(nèi)有36顆果實，按圖3分布，4個采摘臂協(xié)同作業(yè)，則各集合元素示例如表1所示。

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

2.2 求解方法

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

2.2.1 AOMTSP-GA染色體編碼

AOMTSP-GA的染色體是AOMTSP解的表達形式，不同的染色體唯一確定各臂對多目標的采摘訪問序列[24]。選取恰當(dāng)?shù)娜旧w編碼方案是AOMTSP-GA的關(guān)鍵。

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

2.2.2 選擇算子

選擇算子將優(yōu)化的個體直接傳到下一代或?qū)⑴鋵徊娈a(chǎn)生的新個體遺傳到下一代。常用的選擇策略包括基于適應(yīng)度比例（輪賭盤策略）、基于排名、基于局部競爭機制（錦標賽策略）、最佳個體保存等策略[25]。其中最佳個體保存策略收斂性較好，輪賭盤策略可避免局部最優(yōu)[26]。

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

2.2.3 交叉算子

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

2.2.4 變異算子

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

2.2.5 異步規(guī)則

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

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

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

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

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

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

2.2.6 適應(yīng)度計算

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

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

3 基于AOMTSP-GA的多臂采摘協(xié)同任務(wù)規(guī)劃試驗

3.1 雙臂協(xié)同采摘機器人軟硬件系統(tǒng)集成

本試驗基于圖4所示的多臂采摘機器人硬件平臺進行，包括：1）2組直角坐標機械臂。每組機械臂具有3個平移自由度和1個旋轉(zhuǎn)自由度，重復(fù)定位精度為±0.02～0.05 mm，有效行程為X軸650 mm、Y軸650 mm、Z軸450 mm，R軸±30°，有效載荷10 kg，運行速度50 mm/s，其中各關(guān)節(jié)電機采用直流伺服電機驅(qū)動，基于CANOpen總線協(xié)議控制，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，采用紅外雙目立體感知技術(shù)，最高分辨率為1280像素×720像素，深度測量范圍為0.4～6 m，全局快門，數(shù)據(jù)接口為USB3.0 Type-C；4）嵌入式計算機為NVIDIA Jetson TX2，NVIDIA PascalTM架構(gòu)，圖形處理器單元為NVIDIA MaxwellTM，有256個CUDA內(nèi)核，中央處理器單元為Quad ARM?Cortex-A系列A57。

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

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

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

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

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

為便于采摘的任務(wù)規(guī)劃，對目標果實識別與定位結(jié)果根據(jù)本文任務(wù)分區(qū)方法（圖3）作進一步劃分，得到如圖6c和圖7c所示的作業(yè)區(qū)域。

3.3 多臂協(xié)同采摘機器人任務(wù)規(guī)劃仿真試驗

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

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

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

表2 多臂協(xié)同規(guī)劃算法參數(shù)取值設(shè)置 Table 2 Configuration of parameters in the multiple manipulator cooperatively planning algorithm

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

3.4 雙臂協(xié)同采摘機器人多目標采摘試驗

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

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

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

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

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

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

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

4 結(jié)論

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

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

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