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

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

0 引言

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

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

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

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

1.1 棚架模型

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

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

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

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

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

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

1.3 工作原理

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

圖3 平臺(tái)工作原理圖Fig.3 Working principle chart of platform

2 最優(yōu)采摘空間設(shè)計(jì)與運(yùn)動(dòng)學(xué)分析

2.1 機(jī)械手工作空間等效模型

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

(1)

(2)

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

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

2.2 設(shè)計(jì)方案

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

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

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

2.3 設(shè)計(jì)變量

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

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

(3)

式中E——采摘效率

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

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

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

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

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

tplan——雙臂運(yùn)動(dòng)規(guī)劃時(shí)間,s

texecute——雙臂運(yùn)動(dòng)執(zhí)行時(shí)間,s

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

Va=VL+VR-VC

(4)

式中VL——左球臺(tái)體積,m3

VR——右球臺(tái)體積,m3

VC——相交體積,m3

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

(5)

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

Va=VL(2-KV)

(6)

由球臺(tái)體積公式計(jì)算得

(7)

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

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

h——棚架厚度,mm

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

(8)

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

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

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

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

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

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

(9)

其中

(10)

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

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

(11)

2.4.2約束條件

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

(12)

為保證圖7b中雙臂工作空間球體相交點(diǎn)Q高于棚架空間頂面高度，進(jìn)而確保在任意果實(shí)分布高度下不存在果實(shí)漏采的情況，需要約束采用自適應(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算法具有一定的隨機(jī)性，每次運(yùn)行結(jié)果不盡相同，算法運(yùn)行若干次，選取Pareto解集中參數(shù)Lb最大的非支配解作為雙臂并行采摘平臺(tái)空間布局的最優(yōu)向量X，通過(guò)對(duì)最優(yōu)向量進(jìn)行圓整化處理，得到最終結(jié)果為X=(870 mm,1 020 mm,450 mm)。

(13)

2.4.3MOPSO算法求解

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

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

2.5 雙臂運(yùn)動(dòng)學(xué)分析

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

2.5.1開鏈運(yùn)動(dòng)模型

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

圖8 雙臂開鏈運(yùn)動(dòng)模型Fig.8 Open chain motion model with dual robotic arms

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

(14)

其中

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

(Rx,Ry,Rz)——?dú)W拉姿態(tài)角,rad

(15)

(16)

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

(17)

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

(18)

(19)

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

(20)

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

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

(21)

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

(22)

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

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

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

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

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

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

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

圖10 雙臂運(yùn)動(dòng)控制模型Fig.10 Motion control model of dual manipulators

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

圖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)過(guò)變換可得

(26)

(27)

其中

(28)

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

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

3.3 試驗(yàn)方法

3.3.1果實(shí)定位誤差試驗(yàn)

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

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

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

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

3.3.2雙臂并行采摘試驗(yàn)

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

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

3.4 結(jié)果與分析

3.4.1果實(shí)定位誤差試驗(yàn)

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

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

3.4.2雙臂并行采摘試驗(yàn)

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

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

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

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

4 結(jié)論

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

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

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

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