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

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

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

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

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

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

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

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

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

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

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

1）果實(shí)僅能被1個(gè)機(jī)械臂采摘，摘取后其他機(jī)械臂不必訪問該果實(shí)位置；

2）不限制機(jī)械臂對(duì)專屬采摘區(qū)和重疊采摘區(qū)內(nèi)果實(shí)的摘取順序，允許各臂競(jìng)爭(zhēng)重疊區(qū)域內(nèi)的所有可達(dá)目標(biāo)；

3）限制2個(gè)及以上機(jī)械臂同時(shí)進(jìn)入重疊采摘區(qū)；

4）不限制各臂的初始采摘果實(shí)目標(biāo)，各機(jī)械臂在果實(shí)間行進(jìn)速度相同，總遍歷時(shí)間不包含機(jī)械臂摘取果實(shí)（即停留在果實(shí)目標(biāo)處）的時(shí)間；

5）所有果實(shí)目標(biāo)摘取結(jié)束后，遍歷總時(shí)長(zhǎng)最短。

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

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

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

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

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

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

2.1 問題描述

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

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

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

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

2.2 求解方法

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

2.2.1 AOMTSP-GA染色體編碼

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

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

2.2.2 選擇算子

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

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

2.2.3 交叉算子

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

2.2.4 變異算子

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

2.2.5 異步規(guī)則

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

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

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

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

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

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

2.2.6 適應(yīng)度計(jì)算

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

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

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

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

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

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

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

3.2 果實(shí)目標(biāo)檢測(cè)與定位試驗(yàn)

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

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

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

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

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

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

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

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

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

3.4 雙臂協(xié)同采摘機(jī)器人多目標(biāo)采摘試驗(yàn)

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

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

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

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

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

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

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

4 結(jié)論

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

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

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