基于最優(yōu)空間的獼猴桃雙臂并行采摘平臺設(shè)計與試驗

崔永杰 馬 利 何 智 朱玉桃 王寅初 李 凱,3

(1.西北農(nóng)林科技大學(xué)機械與電子工程學(xué)院， 陜西楊凌 712100；2.農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)物聯(lián)網(wǎng)重點實驗室， 陜西楊凌 712100；3.陜西省農(nóng)業(yè)信息感知與智能服務(wù)重點實驗室， 陜西楊凌 712100)

0 引言

中國獼猴桃種植面積及產(chǎn)量均位居世界第一，遠超獼猴桃出口國新西蘭和意大利，同時陜西省作為中國獼猴桃的核心產(chǎn)區(qū)之一，獼猴桃產(chǎn)業(yè)規(guī)模持續(xù)增加[1-2]。在獼猴桃果園全年精細化管理作業(yè)的過程中，尤其是秋季果實采收，主要是人工采摘，作業(yè)強度大、勞動力成本高[3-4]。國內(nèi)外研究人員針對主流消費果蔬廣泛研發(fā)采摘機器人[5-9]，但目前果蔬采摘機器人在生產(chǎn)效率和投入產(chǎn)出比方面仍不如人工作業(yè)，單臂采摘機器人的作業(yè)能力存在局限性，不能滿足農(nóng)業(yè)生產(chǎn)需求[10-11]。多臂并行采摘是解決生產(chǎn)效率低的一種有效方法。因此，需要設(shè)計一種用于獼猴桃采摘的多臂并行采摘平臺，該平臺能夠在棚架式栽培的獼猴桃環(huán)境下實現(xiàn)多臂并行采摘。

近些年，面向勞動密集型的果蔬采摘任務(wù)，國內(nèi)外研發(fā)多機械臂并行采摘機器人。文獻[12]設(shè)計了獼猴桃三自由度四臂并行采摘機器人，4個機械臂根據(jù)平行四連桿機構(gòu)的物理尺寸間隔布置，同時對機械臂關(guān)節(jié)轉(zhuǎn)角范圍進行了限制。文獻[13]對比分析了機械臂的工作空間特性對獼猴桃采摘效率的影響，并結(jié)合試驗說明在多臂并行采摘時，直角機械臂比關(guān)節(jié)型機械臂更有效率，但末端執(zhí)行器在豎直向上抓取呈自然向下生長的獼猴桃果實的過程中，直角機械臂Z軸對平臺的平整度有較高要求。文獻[14]研發(fā)了多臂并行收獲蘋果方案，布置12個機械臂在兩側(cè)同時對兩行蘋果進行采摘。文獻[15]研制了雙臂蘋果采摘機器人，兩個機械臂分別由一個多關(guān)節(jié)采摘機械臂和另一個負責(zé)收集的管道機械臂組成。文獻[16]闡述了一種在非結(jié)構(gòu)化果園環(huán)境中使用雙臂協(xié)同采摘蘋果的方法，其中第1個機械臂作為搜索臂用于識別蘋果，第2個機械臂作為采摘臂進行收獲。文獻[17]闡述了一種雙臂茄子收獲機器人，該機器人布置兩個多關(guān)節(jié)機械臂，可以實現(xiàn)同時收獲兩個茄子。文獻[18]開發(fā)了一種雙臂番茄收獲機器人，該機器人可實現(xiàn)雙臂協(xié)作采摘番茄。然而，國內(nèi)外目前關(guān)于獼猴桃采摘應(yīng)用多關(guān)節(jié)雙臂并行作業(yè)的研究較少。同時，球體狀的多關(guān)節(jié)機械臂工作空間難以與長方體狀的棚架空間相適應(yīng)，特別是在雙臂并行采摘作業(yè)時，雙臂間存在較大面積的果實漏采區(qū)域，底座固定的機械臂需要借助移動底盤反復(fù)切換位置才可以覆蓋漏采區(qū)域，該過程會造成整機作業(yè)效率大幅損失。國內(nèi)外研究人員通過研究多臂任務(wù)規(guī)劃方法來提升采摘機器人的作業(yè)效率[19-20]，本文從優(yōu)化設(shè)計多臂并行采摘空間的角度來降低作業(yè)效率的損失。

為解決雙臂間因采摘區(qū)域不連續(xù)造成的果實漏采問題，本文采用理論與試驗相結(jié)合的方法對雙臂并行采摘平臺進行設(shè)計。以采摘面積最大和公共區(qū)域占比最小為指標(biāo)建立目標(biāo)優(yōu)化函數(shù)，并利用粒子群算法求解出最優(yōu)采摘空間，在此基礎(chǔ)上，建立雙臂開鏈模型的位置層約束關(guān)系，最后設(shè)計雙臂并行采摘平臺樣機并試驗驗證采摘空間的適應(yīng)性及整機作業(yè)性能，以期為后續(xù)研究獼猴桃雙臂并行采摘任務(wù)規(guī)劃方法提供平臺支撐。

1 總體結(jié)構(gòu)與工作原理

1.1 棚架模型

以位于陜西省寶雞市眉縣獼猴桃實驗站棚架式栽培的獼猴桃果園作為研究對象，獼猴桃果園場景特征如下：①果園棚架平均行寬4 m，平均株間距3 m，棚架平均高度1.8 m。②樹干間設(shè)有支撐鋼管，鋼管頂部用鋼絲固定伸展枝條，使果實呈自然下垂?fàn)顟B(tài)生長。③果實分布在距離地面高度為1.5～1.8 m的空間范圍內(nèi)。④果園行間地面堅實平整可視作平面。

基于以上獼猴桃果園環(huán)境特征，建立棚架空間等效模型，如圖1所示，該模型將作為雙臂并行采摘空間設(shè)計的依據(jù)。

圖1 棚架空間等效模型Fig.1 Equivalent model of scaffolding space1.獼猴桃 2.棚架空間 3.果樹 4.果園地面

1.2 整機結(jié)構(gòu)

基于多關(guān)節(jié)機械臂搭建雙臂并行采摘平臺，如圖2所示。該機器人平臺由視覺系統(tǒng)、2個采摘機械臂、末端執(zhí)行器和移動平臺組成。視覺系統(tǒng)采用Intel RealSense D435i型雙目深度相機作為視覺傳感器，通過眼在手上的布置方式以自下而上的方向進行獼猴桃果實目標(biāo)識別與定位[21]。采摘機械臂為UR5多關(guān)節(jié)機械臂，該機械臂以高靈活性、安全性及輕便性的特點可以有效進行雙臂并行作業(yè)。末端執(zhí)行器由兩個3D打印的仿形夾爪、光電傳感器和氣動部件組成，夾爪內(nèi)側(cè)曲面設(shè)計適應(yīng)獼猴桃果實形狀，以此方式減少采摘過程中的果實損傷。末端和相機總質(zhì)量為3.5 kg，果柄與果實分離力為3～10 N[6]，確定采摘機械臂最大工作負載為4.5 kg，滿足UR5機械臂對于有效載荷低于5 kg的要求。移動平臺由履帶式導(dǎo)航平臺牽引運動至采摘區(qū)域，兩個機械臂搭建在移動平臺上執(zhí)行果實采摘任務(wù)。

圖2 機器人總體結(jié)構(gòu)圖Fig.2 General structure diagram of robot1.深度相機 2.光電傳感器 3.右機械臂 4.計算機 5.控制單元 6.末端執(zhí)行器 7.左機械臂 8.果實筐 9.移動平臺

1.3 工作原理

平臺工作原理如圖3所示。①間歇式移動平臺經(jīng)導(dǎo)航平臺牽引運動至采摘區(qū)域。②2個深度相機進行果實圖像采集，實時將圖像信息傳輸給機器人控制單元。③控制單元基于深度學(xué)習(xí)模型開展果實目標(biāo)檢測，通過檢測彩色圖像果實區(qū)域并提取對應(yīng)的深度信息，結(jié)合相機內(nèi)參矩陣求取果實目標(biāo)相對于相機坐標(biāo)系的空間坐標(biāo)。④控制單元結(jié)合已標(biāo)定的相機外參矩陣完成果實位置信息的外參轉(zhuǎn)換，得到果實目標(biāo)相對于各自機械臂基座坐標(biāo)系的空間位置。⑤計算機以空間距離最短為原則制定果實采摘序列。⑥計算機控制雙臂進行避障運動規(guī)劃，機械臂接收指令運動至棚架空間底面對應(yīng)的待采摘目標(biāo)果實下方位置。⑦末端執(zhí)行器豎直向上運動至目標(biāo)位置，通過光電傳感器信號控制末端手指抓取果實。⑧機械臂法蘭關(guān)節(jié)旋轉(zhuǎn)完成果實與果柄分離操作。⑨視覺系統(tǒng)更新反饋果實位置信息，雙臂進行連續(xù)采摘作業(yè)，完成當(dāng)前區(qū)域果實采摘任務(wù)。⑩移動平臺以固定步距前進至下一個采摘區(qū)域。

圖3 平臺工作原理圖Fig.3 Working principle chart of platform

2 最優(yōu)采摘空間設(shè)計與運動學(xué)分析

2.1 機械手工作空間等效模型

表1 UR5機械臂D-H參數(shù)Tab.1 D-H parameters of UR5 robot arm

(1)

(2)

通過利用Matlab機器人工具箱Robotics Toolbox建立采摘機械手運動學(xué)模型，如圖4a所示。采用蒙特卡洛法進行機械手工作空間分析[23]，隨機選取30 000組關(guān)節(jié)空間向量，在每個軸的運動范圍內(nèi)進行大量隨機采樣，通過正向運動學(xué)函數(shù)得到末端執(zhí)行器工具中心點在空間中可到達位置，如圖4b所示。結(jié)合機械手可達空間分布情況，將機械手工作空間等效為球心位于機械臂基座坐標(biāo)系原點，半徑R0=0.90 m的空間球體，機械手工作空間等效模型如圖4c所示。

圖4 機械手工作空間等效模型Fig.4 Equivalent model of manipulator workspace

2.2 設(shè)計方案

雙臂在移動平臺上的安裝方式主要包括平行安裝、側(cè)向安裝和傾斜安裝。傾斜和側(cè)向安裝方式中機械臂基座固定螺栓存在較大的彎曲應(yīng)力，雙臂運動過程中平臺承載能力和作業(yè)穩(wěn)定性受到影響，且雙臂公共空間會隨著安裝角的增加逐漸縮減，直至側(cè)向安裝方式在棚架空間范圍內(nèi)沒有公共空間[24]，造成大面積的果實漏采問題。相比之下，雙臂平行安裝具有寬裕的公共空間和作業(yè)穩(wěn)定性，適合當(dāng)前機器人作業(yè)場景。

面向獼猴桃果園整行果實采摘任務(wù)，本文采用多臂并行采摘方案，如圖5所示，該方案在理想情況下可以確保機械手對棚架空間范圍內(nèi)分布的獼猴桃實現(xiàn)采摘全覆蓋，進而避免因移動平臺在棚架空間橫向切換采摘位置導(dǎo)致作業(yè)效率大幅損失的問題[25]。但任意雙臂橫向靜態(tài)采摘區(qū)域存在不連續(xù)現(xiàn)象，導(dǎo)致果實漏采問題，如圖中白色圓圈標(biāo)記的果實。同時，移動平臺沿棚架空間縱向間歇式前進采摘時，漏采問題在雙臂縱向動態(tài)采摘區(qū)域中表現(xiàn)更為嚴重。因此，雙臂之間的空間布局和間歇式前進步距Ystep對于果實漏采問題和整機作業(yè)效率存在重要影響。為確保雙臂并行采摘空間在橫向和縱向的連續(xù)性，本文采用理論與試驗相結(jié)合的方法對雙臂并行采摘平臺進行設(shè)計。

圖5 多臂并行采摘方案示意圖Fig.5 Schematic of parallel harvesting with multiple robotic arms

2.3 設(shè)計變量

圖6為獼猴桃棚架空間下方雙臂并行采摘示意圖。結(jié)合前文已建立的棚架空間和機械手工作空間等效模型，可得多關(guān)節(jié)雙臂并行采摘效率為

圖6 雙臂并行采摘示意圖Fig.6 Schematic of parallel harvesting with dual robotic arms

(3)

式中E——采摘效率

Nfruits——區(qū)域果實總數(shù)量

ttotal——區(qū)域果實收獲總時間,s

ρ——每立方米果實數(shù)量

Va——雙臂靜態(tài)采摘空間體積,m3

k——目標(biāo)識別定位次數(shù)

tplan——雙臂運動規(guī)劃時間,s

texecute——雙臂運動執(zhí)行時間,s

由于果實分布高度存在差異，采摘機器人需要多次識別定位完成區(qū)域果實采摘任務(wù)。雙臂靜態(tài)采摘空間體積Va為

Va=VL+VR-VC

(4)

式中VL——左球臺體積,m3

VR——右球臺體積,m3

VC——相交體積,m3

由于VC?VL，VL=VR，定義相交體積VC與左球臺體積VL之比為

(5)

聯(lián)立式(4)、(5)可得

Va=VL(2-KV)

(6)

由球臺體積公式計算得

(7)

式中Sw——棚架空間底面與機械手工作空間等效模型相截面,m2

Su——棚架空間頂面與機械手工作空間等效模型相截面,m2

h——棚架厚度,mm

聯(lián)立式(3)、(6)、(7)可以得到

(8)

根據(jù)式(8)分析得到，影響雙臂并行采摘效率的結(jié)構(gòu)因素主要是h、Su、Sw和KV。其中，棚架厚度h受果實自然分布高度影響，變量Su和Sw受圖中雙臂安裝高度Hb影響，變量KV受圖中雙臂安裝相對位置Lb影響，若變量KV=0，則雙臂采摘效率最大，但為避免區(qū)域內(nèi)果實漏采問題，需要雙臂采摘空間有公共區(qū)域，即KV≠0。

本文將雙臂安裝高度Hb、雙臂安裝相對位置Lb和截面Su的半徑長度Ru(間歇式前進步距Ystep)作為設(shè)計變量。

2.4 基于MOPSO多目標(biāo)優(yōu)化設(shè)計

2.4.1目標(biāo)函數(shù)建立

本文將雙臂并行采摘空間參數(shù)設(shè)計的目標(biāo)體系設(shè)置為2個子目標(biāo)：采摘面積最大和公共區(qū)域占比最小，最大化的采摘面積可以保障雙臂作業(yè)效率，最小化的公共區(qū)域可以保障雙臂作業(yè)安全性。獼猴桃雙臂并行采摘空間參數(shù)設(shè)計模型如圖7所示。如圖7a所示，為保證雙臂作業(yè)效率，需要在機械臂工作空間中尋求最大范圍適應(yīng)棚架空間的矩形區(qū)域，同時為避免果實漏采，矩形區(qū)域定義為過兩圓交點p1和p2的矩形CDFE。定義矩形面積最大作為第1個目標(biāo)函數(shù)，即

圖7 采摘空間參數(shù)設(shè)計模型Fig.7 Parametric model design of harvesting space

(9)

其中

(10)

式中Rw——最低工作截面半徑,mm

為保證雙臂作業(yè)安全性，需要設(shè)計最優(yōu)的公共區(qū)域，本文定義公共區(qū)域為過點p1、q1、p2、q2，兩半軸長度為a和b的橢圓區(qū)域。定義公共區(qū)域占比最小為第2個目標(biāo)函數(shù)，即

(11)

2.4.2約束條件

結(jié)合獼猴桃棚架空間和機械手工作空間等效模型，確定參數(shù)設(shè)計范圍為

(12)

為保證圖7b中雙臂工作空間球體相交點Q高于棚架空間頂面高度，進而確保在任意果實分布高度下不存在果實漏采的情況，需要約束采用自適應(yīng)網(wǎng)格搜索的多目標(biāo)粒子群優(yōu)化算法(Multi-objective particle swarm optimization, MOPSO)求解該模型[26]，算法部署在Matlab R2020b環(huán)境，主要參數(shù)設(shè)置如下：種群數(shù)目100，最大迭代次數(shù)500，外部檔案規(guī)模100，加速常數(shù)c1=c2=2，慣性權(quán)重w=0.4，自適應(yīng)網(wǎng)格法每一維目標(biāo)函數(shù)網(wǎng)格數(shù)20。MOPSO算法具有一定的隨機性，每次運行結(jié)果不盡相同，算法運行若干次，選取Pareto解集中參數(shù)Lb最大的非支配解作為雙臂并行采摘平臺空間布局的最優(yōu)向量X，通過對最優(yōu)向量進行圓整化處理，得到最終結(jié)果為X=(870 mm,1 020 mm,450 mm)。

(13)

2.4.3MOPSO算法求解

結(jié)合最優(yōu)向量計算得到雙臂并行采摘空間具體參數(shù)如表2所示。其中，雙臂靜態(tài)采摘空間體積Va為0.681 m3，公共區(qū)域占比KV為0.217，移動平臺縱向間歇式前進步距Ystep為450 mm，雙臂最大覆蓋棚架空間底面橫向長度2 390 mm，最大覆蓋棚架空間頂面橫向長度1 770 mm，該方案可以有效解決雙臂間采摘區(qū)域不連續(xù)問題。

表2 雙臂并行采摘空間參數(shù)Tab.2 Harvesting space parameters of dual robotic arms in parallel

2.5 雙臂運動學(xué)分析

獼猴桃雙臂并行采摘機器人的協(xié)調(diào)運動屬于松協(xié)調(diào)范疇，左臂和右臂分別針對不同目標(biāo)果實進行采摘，形成一個開鏈結(jié)構(gòu)[22]。本文結(jié)合雙臂空間布局特點和獼猴桃分布位姿，建立了開鏈運動模型和基于位置約束的避障運動策略。通過建立位置層約束關(guān)系，使雙臂完成并行采摘獼猴桃任務(wù)。

2.5.1開鏈運動模型

如圖8所示，以雙臂各自獨立采摘兩個目標(biāo)果實為例，推導(dǎo)雙臂開鏈結(jié)構(gòu)的運動學(xué)關(guān)系。設(shè)定平臺全局坐標(biāo)系為{W}，左臂和右臂的基座坐標(biāo)系分別為{BL}和{BR}，左臂和右臂的末端執(zhí)行器坐標(biāo)系分別為{EL}和{ER}。由于獼猴桃的果軸近似處于豎直方向，果實底部形狀為圓形，因此雙臂末端執(zhí)行器保持初始位姿進行果實抓取，初始位姿為

圖8 雙臂開鏈運動模型Fig.8 Open chain motion model with dual robotic arms

E0=(xk,yk,zk,Rx,Ry,Rz)

(14)

其中

式中 (xk,yk,zk)——果實定位坐標(biāo),mm

(Rx,Ry,Rz)——歐拉姿態(tài)角,rad

(15)

(16)

齊次變換矩陣T中包含位置向量P和姿態(tài)矩陣R，其展開形式可表示為

(17)

將式(17)中的展開形式代入式(15)、(16)中可以得到

(18)

(19)

將式(18)、(19)展開并化簡，可以計算得到雙臂末端執(zhí)行器坐標(biāo)系相對于平臺全局坐標(biāo)系的位置向量和姿態(tài)矩陣為

(20)

2.5.2位置約束關(guān)系

基于最優(yōu)向量X布局的雙臂開鏈結(jié)構(gòu)，存在特殊的位置關(guān)系，即機械臂前兩個連桿的活動范圍半徑(機械臂連桿2的長度)小于雙臂安裝相對位置的一半，具體表示為

(21)

因此，雙臂在前兩個連桿之間不存在干擾問題。由于機械臂位置由前3個連桿所包含的關(guān)節(jié)確定，因此，雙臂間存在碰撞風(fēng)險的連桿和關(guān)節(jié)為連桿3及其所屬的關(guān)節(jié)3和關(guān)節(jié)4。

(22)

由于獼猴桃果實底部形狀為圓形，末端執(zhí)行器夾爪為兩指對置類型，因此，抓取角Rz屬于可調(diào)節(jié)參數(shù)，且該參數(shù)取值范圍對應(yīng)左臂和右臂分別為[0,π]和[-π,0]。同時，由于該機械臂構(gòu)型滿足Piper準(zhǔn)則，逆運動學(xué)具有封閉形式解[27]。因此，通過調(diào)整抓取角，利用位置約束關(guān)系完成逆解篩選任務(wù)，從而選擇出滿足位置約束關(guān)系的安全位姿Ea。

表3 基于位置約束的抓取角仿真結(jié)果Tab.3 Simulation results of grasp angle based on position constraints

3 試驗與結(jié)果分析

為驗證基于最優(yōu)向量布置的雙臂能否全覆蓋采摘棚架空間中的果實點，考量采摘空間的適應(yīng)性及整機作業(yè)性能，本文設(shè)計了獼猴桃多關(guān)節(jié)雙臂并行采摘平臺，搭建了與最優(yōu)采摘空間尺寸相對應(yīng)的室內(nèi)棚架環(huán)境，基于該平臺進行了果實定位誤差試驗和雙臂并行采摘試驗。

3.1 雙臂并行采摘平臺軟硬件系統(tǒng)集成

本研究后期試驗階段，受獼猴桃果實收獲季節(jié)的影響，基于實驗室獼猴桃棚架環(huán)境開展試驗。圖9為雙臂并行采摘硬件平臺，主要包括：①實驗室棚架模型，長×寬×高為2 000 mm×1 500 mm×1 800 mm。②多關(guān)節(jié)采摘機械臂，由6個旋轉(zhuǎn)關(guān)節(jié)組成，重復(fù)精度±0.1 mm，有效載荷為5 kg。③移動平臺長×寬×高為1 500 mm×1 000 mm×1 020 mm。④深度相機為英特爾 RealSense D435i型，最高分辨率為1 280像素×720像素，深度測量范圍為0.4～6 m，數(shù)據(jù)接口為USB3.0 Type-C。⑤兩個嵌入式計算機為NVIDIA Jetson Nano，圖像計算單元為NVIDIA MaxwellTM架構(gòu)，具有128個NVIDIA CUDAR核心，中央處理器單元為四核ARM Cortex-A57 MPCore處理器，操作系統(tǒng)為Ubuntu 18.04。

基于機器人操作系統(tǒng)(Robot operation system，ROS)完成雙臂并行采摘平臺控制系統(tǒng)開發(fā)[28]。雙臂運動控制模型如圖10所示，多關(guān)節(jié)UR5機械臂模型采用統(tǒng)一描述格式(Unified robot description format, URDF)構(gòu)建，左右機械臂基座坐標(biāo)系基于同一世界坐標(biāo)系建立，機械手工具中心點建立在末端執(zhí)行器夾爪中心，利用ROS-MoveIt!工具[29]完成對兩個采摘機械手運動學(xué)模型配置，將機器人碰撞矩陣加載到集成工具包中。

圖10 雙臂運動控制模型Fig.10 Motion control model of dual manipulators

圖11為基于ROS-MoveIt!的雙臂并行采摘控制系統(tǒng)示意圖。通過部署多機分布式通訊系統(tǒng)完成兩個采摘機械臂集成控制，圖像處理單元將果實三維坐標(biāo)信息以話題形式發(fā)布，兩個采摘機械臂控制節(jié)點分別訂閱目標(biāo)果實空間位置話題，機械臂結(jié)合當(dāng)前位姿和目標(biāo)位姿利用開源運動規(guī)劃庫(Open motion planning library, OMPL)中隨機擴展樹算法和碰撞檢測庫(Flexible collision library, FCL)進行無碰撞路徑規(guī)劃。通過調(diào)用ROS-MoveIt!運動規(guī)劃組逆解器IKFast求解運動學(xué)逆解，形成采摘機械臂運動學(xué)組的動態(tài)軌跡，驅(qū)動雙臂抵達各自目標(biāo)果實位置，機械臂執(zhí)行運動的最大加速度和最大速度系數(shù)均設(shè)置為0.2，機械手完成當(dāng)前目標(biāo)果實采摘任務(wù)后，圖像處理節(jié)點更新果實位置信息，直至完成所有果實采摘。

圖11 雙臂并行采摘控制系統(tǒng)示意圖Fig.11 Schematic of control system for parallel harvesting with dual manipulators

3.2 雙臂手眼標(biāo)定

(23)

(24)

其中

R3×3——旋轉(zhuǎn)變換矩陣

T3×1——平移變換矩陣

O1×3——零矩陣

(25)

式(25)經(jīng)過變換可得

(26)

(27)

其中

(28)

圖12a、12b為左臂相機手眼標(biāo)定過程，右臂固定標(biāo)定板并保持當(dāng)前位形，左臂移動采集標(biāo)定板不同位姿，聯(lián)立方程計算左相機外參矩陣。圖12c、12d為右臂相機手眼標(biāo)定過程，左臂固定標(biāo)定板并保持當(dāng)前位形，右臂移動采集標(biāo)定板不同位姿，聯(lián)立方程計算右相機外參矩陣。

圖12 手眼標(biāo)定過程Fig.12 Hand-eye calibration process

3.3 試驗方法

3.3.1果實定位誤差試驗

基于YOLO v4深度學(xué)習(xí)模型開展獼猴桃果實目標(biāo)識別任務(wù)[21,30]，獼猴桃模型的果萼特征采用直徑為1 cm的黑色貼紙等效，識別結(jié)果如圖13所示。由于實驗室環(huán)境下照明燈的光照強度較室外自然光照低，棚架無明顯曝光區(qū)域，果實目標(biāo)識別成功率高。

圖13 獼猴桃果實目標(biāo)識別試驗Fig.13 Experiment of kiwifruit target detection

為保證末端執(zhí)行器有效抓取果實，本試驗對實驗室環(huán)境下果實定位誤差進行分析。獼猴桃果實坐標(biāo)測量方法如圖14所示，GLM40型激光測距儀(博世，精度±2 mm)安裝在機械臂基座平面，安裝平面放置水平儀，確保激光測距儀水平安裝，具體坐標(biāo)測量方法如下：首先移動激光測距儀使激光光束照射到目標(biāo)果實果萼位置，然后控制機械臂末端執(zhí)行器保持如式(14)所示的目標(biāo)姿態(tài)一定，然后拖動機械臂使激光光束穿過夾爪中心，最后在機械臂控制箱讀取果實水平坐標(biāo)(X*,Y*)，在激光測距儀界面讀取果實深度坐標(biāo)Z*。獼猴桃果實識別坐標(biāo)獲取方法：首先以ROS話題形式訂閱果實識別標(biāo)記框中心點的像素坐標(biāo)并完成對應(yīng)深度的映射，然后結(jié)合相機內(nèi)參和外參轉(zhuǎn)換矩陣計算得到果實目標(biāo)相對于機械臂基座坐標(biāo)系的識別坐標(biāo)(X′,Y′,Z′)。

圖14 獼猴桃果實坐標(biāo)測量方法示意圖Fig.14 Schematic of method for measuring kiwifruit coordinates

3.3.2雙臂并行采摘試驗

本試驗采用雙臂分區(qū)分層作業(yè)方式。如圖15所示，結(jié)合雙臂空間布局特點，將采摘空間橫向劃分為左臂空間L、右臂空間R和共享空間M。結(jié)合棚架厚度特點，將棚架厚度間隔100 mm劃分為一層，分別為底層Ⅰ、中間層Ⅱ、頂層Ⅲ。結(jié)合移動平臺最大間歇式前進步距特點，將棚架空間縱向劃分為區(qū)域1和區(qū)域2。本試驗進行2次3組采摘試驗，第1次3組采摘試驗面向區(qū)域1中的果實，第2次3組采摘試驗面向區(qū)域2中的果實：移動平臺保持固定，棚架模型縱向前進450 mm，區(qū)域1變?yōu)橐淹瓿勺鳂I(yè)的區(qū)域F1，區(qū)域2變?yōu)楫?dāng)前采摘任務(wù)區(qū)域。具體3組試驗設(shè)定如下：試驗1果實分布在棚架空間底層，區(qū)域LⅠ、RⅠ、MⅠ果實數(shù)量分別為10、10、15個；試驗2果實分布在棚架空間中間層，區(qū)域LⅡ、RⅡ、MⅡ果實數(shù)量分別為7、7、10個；試驗3果實分布在棚架空間頂層，區(qū)域LⅢ、RⅢ、MⅢ果實數(shù)量分別為5、5、7個。統(tǒng)計試驗過程中未成功遍歷果實數(shù)量和未成功采摘果實數(shù)量以及雙臂沖突次數(shù)，利用秒表計時統(tǒng)計采摘時間。本試驗以果實位置遍歷成功率、果實采摘成功率和平均單果采摘時間作為評價指標(biāo)，這3項評價指標(biāo)分別定義為采摘空間中成功遍歷果實數(shù)量、成功采摘果實數(shù)量以及果實采摘總時間與區(qū)域果實總數(shù)量的比值。

圖15 雙臂并行采摘試驗方法示意圖Fig.15 Schematic of experiment method for parallel harvesting with dual manipulators

3.4 結(jié)果與分析

3.4.1果實定位誤差試驗

試驗針對左臂進行了10組果實定位誤差測量，結(jié)果如表4所示。其中，果實平均水平定位誤差為5.0 mm，果實平均深度定位誤差為8.3 mm。末端執(zhí)行器兩個夾爪間隔65 mm，獼猴桃果實大徑平均值為52.16 mm[31]，末端執(zhí)行器夾爪兩側(cè)有效行程余量6.42 mm。果實水平定位誤差可通過末端執(zhí)行器有效行程余量容錯解決，深度定位誤差通過對末端執(zhí)行器工具中心點坐標(biāo)進行深度補償解決。果實定位誤差滿足采摘機械手定位精度的要求，末端執(zhí)行器能夠有效抓取目標(biāo)果實。

表4 果實定位誤差結(jié)果Tab.4 Fruits positioning error results

3.4.2雙臂并行采摘試驗

雙臂在共享空間采摘過程如圖16所示。首先左臂進入采摘空間并到達目標(biāo)果實下方位置，右臂位于初始位姿(圖16a)；隨后右機械臂運動規(guī)劃進入采摘空間接近待采摘目標(biāo)果實，左臂豎直向上抓取并旋轉(zhuǎn)采摘果實(圖16b)；接下來左臂運動規(guī)劃接近下一目標(biāo)果實，右臂豎直向上抓取并旋轉(zhuǎn)采摘果實(圖16c)。該采摘過程考慮在其中一個機械臂大范圍運動接近目標(biāo)果實的作業(yè)過程中，另一個機械臂處于局部小范圍運動作業(yè)狀態(tài)，以此方式保證雙臂在共享空間作業(yè)時運動規(guī)劃的高效性和安全性。

圖16 雙臂共享空間采摘過程Fig.16 Harvesting process with dual manipulators in public areas

雙臂并行采摘試驗結(jié)果如表5所示。雙臂在兩側(cè)獨立采摘空間果實位置遍歷成功率分別為90.91%和93.18%，果實采摘成功率分別為86.36%和81.82%，平均單果采摘時間分別為4.80、4.61 s。雙臂在共享采摘空間果實位置遍歷成功率92.19%，果實采摘成功率為78.13%，平均單果采摘時間為8.17 s。綜合計算得到，雙臂并行采摘平均果實位置遍歷成功率為92.09%，平均果實采摘成功率為82.10%，平均單果采摘時間為5.86 s。其中，雙臂在兩側(cè)獨立采摘空間作業(yè)時不存在運動沖突問題，雙臂在共享空間中作業(yè)時共存在8次運動沖突的情況，通過位姿調(diào)整使機械臂繼續(xù)選擇滿足位置約束關(guān)系的安全位姿重新完成運動規(guī)劃。未成功遍歷的果實主要是位于機械臂基座坐標(biāo)系原點附近的果實，由于機械臂目標(biāo)構(gòu)型奇異造成運動規(guī)劃失敗。果實采摘失敗的主要原因有：果實被樹葉遮擋未成功檢測；果實定位誤差造成末端未能抓取果實；果實分布在機械臂奇異點位置；特別在共享空間內(nèi)，雙臂交叉采摘造成難以產(chǎn)生無碰撞運動路徑；采摘過程中棚架振動造成雙臂間相互干擾果實抓取。試驗結(jié)果驗證了雙臂能夠全覆蓋遍歷除奇異點外的所要求的目標(biāo)果實點，解決了因作業(yè)區(qū)域不連續(xù)造成的果實漏采問題。

表5 雙臂并行采摘試驗結(jié)果Tab.5 Experiment results of parallel harvesting with dual manipulators

4 結(jié)論

(1)以采摘面積最大和公共區(qū)域占比最小為指標(biāo)建立目標(biāo)函數(shù)，利用粒子群算法求解出最優(yōu)雙臂安裝相對位置870 mm、最優(yōu)雙臂安裝高度1 020 mm和移動平臺最大間歇式前進步距450 mm，解決了雙臂間采摘區(qū)域不連續(xù)的問題。

(2)雙臂在共享空間中作業(yè)時通過局部調(diào)整抓取角使雙臂滿足位置層約束關(guān)系，保證中間區(qū)域多數(shù)果實的安全采摘任務(wù)。

(3)果實定位誤差試驗結(jié)果表明，果實平均水平定位誤差為5.0 mm，平均深度定位誤差為8.3 mm，基本滿足獼猴桃采摘作業(yè)要求。

(4)雙臂并行采摘試驗結(jié)果表明，平均果實位置遍歷成功率92.09%，驗證了采摘空間的覆蓋能力，平均果實采摘成功率82.10%，平均單果采摘時間5.86 s，雙臂僅在共享空間存在較低的碰撞風(fēng)險，驗證了雙臂并行采摘平臺的作業(yè)可行性。