金玉成 高 楊 劉繼展 胡春華 周 堯 李萍萍

(1.江蘇大學(xué)現(xiàn)代農(nóng)業(yè)裝備與技術(shù)教育部重點實驗室， 鎮(zhèn)江 212013；2.南京林業(yè)大學(xué)信息科學(xué)技術(shù)學(xué)院， 南京 210037； 3.南京林業(yè)大學(xué)生物與環(huán)境學(xué)院， 南京 210037)

0 引言

“手-眼”協(xié)調(diào)是果蔬采摘機器人的主體和完成自主采摘作業(yè)的關(guān)鍵[1-4]，而植株冠層環(huán)境的高度復(fù)雜性、果實目標(biāo)的隨機性與差異性，對機器人手-眼系統(tǒng)“感知-動作”的智慧性提出了極大挑戰(zhàn)[5]?！癊ye-to-Hand”結(jié)構(gòu)首先在采摘機器人領(lǐng)域得到了廣泛研究并有力推動了采摘機器人技術(shù)的進(jìn)步，但是外置相機遠(yuǎn)景探測造成圖像內(nèi)冗余信息過多、目標(biāo)果實搜尋過程復(fù)雜，手眼及果葉相互遮擋問題突出，識別可靠性與視覺定位誤差較大[6-7]；“先看后動”的靜態(tài)割裂方式缺乏對農(nóng)業(yè)非結(jié)構(gòu)化復(fù)雜環(huán)境和動態(tài)變化的適應(yīng)能力[8-13]；同時其依賴精確手-眼坐標(biāo)系轉(zhuǎn)換、機器人運動學(xué)建模和逆像問題求解，造成機械手誤差累積而影響定位精度[2-3,14]。與之相比，“Eye-in-Hand”的“看并動”手眼協(xié)調(diào)模式中末端對目標(biāo)的定位可實現(xiàn)持續(xù)的視覺反饋和修正，達(dá)到實時視覺伺服控制[4,10, 14-16]，因而在采摘機器人領(lǐng)域逐漸得到重視。日本、美國、比利時等國面向柑橘[17]、茄子[18-19]、番茄[20]、蘋果[21]等開發(fā)的采摘機器人系統(tǒng)，都采用了掌心或腕部安裝相機的“Eye-in-Hand”配置方案。針對“Eye-in-Hand”的手眼協(xié)調(diào)控制問題，VAN HENTEN 等[7, 22]提出通過掌上相機的位置變換獲得兩幅圖像，進(jìn)而3維重構(gòu)獲得果實的空間位置信息；BARTH 等[14]通過掌上相機以20 Hz持續(xù)檢測果實并計算機械手位姿，驅(qū)動末端以速度0.03 m/s逼近目標(biāo)果實，但試驗僅實現(xiàn)了二維圖像像素位置逼近，而無法實現(xiàn)深度(距離)定位，一個視覺伺服控制周期達(dá)到了45 s。瓦根寧大學(xué)的甜椒收獲機器采用CCD相機在手模式，實驗室檢測與采摘成功率達(dá)97%和79%，但機械采放果路徑過長，在運動規(guī)劃和動作執(zhí)行上耗費過多時間，單次作業(yè)時間高達(dá)106 s[23-24]。MEHTA等[25]研究發(fā)現(xiàn)末端的視覺伺服定位精度約為15 mm，僅能滿足大中型果實的采摘定位需要，同時路徑中枝干的存在會影響伺服控制性能甚至?xí)斐赡┒藞?zhí)行器損壞，而測量噪聲對基于高增益的反饋控制效果影響較大。

為實現(xiàn)復(fù)雜實際條件下的采摘機器人高效作業(yè)，基于RGB-D傳感器進(jìn)行“Eye-in-Hand”的手眼協(xié)調(diào)控制已成為客觀需要和研究共識[26-29]。CHEN等[30]開發(fā)了仿人型雙臂式番茄采摘機器人，在頭部和腕部分別安裝Xtion和PrimeSense 傳感器，首先由外置Xtion獲取并引導(dǎo)手臂到達(dá)果枝的近位，再由手上Carmine完成更細(xì)的果實識別定位，但僅通過室內(nèi)的簡化試驗證實了其可行性，試驗精度不足且未完全實現(xiàn)完全自主的手眼協(xié)調(diào)控制。澳大利亞昆士蘭科技大學(xué)LEHNERT 等[16,31]研究的甜椒采摘機器人，需通過手上RealSense與作物行垂直進(jìn)行水平和豎直移動掃描進(jìn)而重構(gòu)建立3維點云模型，然后由RGB-HSV轉(zhuǎn)換的顏色特征識別出紅色甜椒，用深度信息給出植株的位姿估計并引導(dǎo)機械臂抓取和分離果實，試驗發(fā)現(xiàn)手眼協(xié)調(diào)完成采摘的耗時達(dá)到35～40 s，效率偏低[31-33]；SORIA 等[34]構(gòu)建了基于RealSense SR300傳感器的手-眼系統(tǒng)，首先定位在果枝前拍照確定機械手與蘋果植株的相對角度，進(jìn)而控制機械手沿球面移動進(jìn)行多視角的點云獲取，將多視角點云圖像合成獲得真實的相對姿態(tài)，但尚未進(jìn)行手眼協(xié)調(diào)的控制研究。

為此，本文在RealSense深度傳感器的景深160～1 200 mm范圍和160 mm近景的果實可靠識別[35-36]基礎(chǔ)上，結(jié)合升降式采摘機器人結(jié)構(gòu)設(shè)計與新型手眼配置模式和“遠(yuǎn)近景”組合的手眼協(xié)調(diào)控制，進(jìn)行RealSense深度伺服的采摘機器人設(shè)計與遠(yuǎn)近景運動規(guī)劃，并通過試驗進(jìn)行驗證。

1 采摘機器人結(jié)構(gòu)

RealSense深度伺服小型升降式采摘機器人結(jié)構(gòu)示意圖如圖1所示，由自主移動底盤、剪叉升降機構(gòu)、抽拉式果箱、3回轉(zhuǎn)機械臂、夾剪式末端執(zhí)行器和腕部RealSense SR300深度傳感器構(gòu)成。

(1)設(shè)計了由平置直流電機驅(qū)動的多級剪叉升降機構(gòu)，并設(shè)計了彈性輔助啟停裝置以改善啟動性能并實現(xiàn)下降的緩沖，從而有效解決狹小空間通行的底盤小型化與豎直作業(yè)的較大升降需要之間的矛盾。

(2)在2關(guān)節(jié)機械臂基礎(chǔ)上，設(shè)計了剪叉升降平臺上的基座水平伺服回轉(zhuǎn)機構(gòu)，由大小臂、水平回轉(zhuǎn)與豎直升降組合成為PRRR 4自由度機械臂，并將抽拉式果箱安裝于升降平臺上，在特定高度區(qū)域僅由機械臂的3回轉(zhuǎn)動作規(guī)劃即可完成采摘和空中就近放果，大大縮短了周期作業(yè)的路徑長度、運動規(guī)劃復(fù)雜性和耗時；滿足規(guī)范化種植下的采摘要求。

(3)針對該少自由度、短路徑的“采-放”周期作業(yè)，通過機械臂腕部安裝RealSense深度傳感器構(gòu)成“眼在手上”系統(tǒng)，利用其景深160～1 200 mm和較大視場范圍(豎直55°×水平71°)的1 280×720深度探測特點和在手RealSense深度伺服控制，從而保障遠(yuǎn)近景組合的手-眼協(xié)調(diào)精準(zhǔn)定位。

2 手-眼協(xié)調(diào)的姿態(tài)與坐標(biāo)

2.1 采放果工作空間與姿態(tài)分析

該采摘機器人通過豎直升降與機械臂的3自由度回轉(zhuǎn)實現(xiàn)大范圍采摘作業(yè)，而僅需機械臂的3自由度回轉(zhuǎn)實現(xiàn)對空中果箱的放果。升降與回轉(zhuǎn)的末端工作空間與姿態(tài)應(yīng)滿足矮化密植或吊蔓栽培的果實采摘需要，同時通過機械臂的關(guān)節(jié)回轉(zhuǎn)滿足將果實安全送入空中果箱的需要。

2.1.1工作空間分析

根據(jù)蒙特卡羅法對機械臂的工作空間進(jìn)行求解，調(diào)用Matlab機器人工具箱的Rand函數(shù)與Plot函數(shù)，畫出機械臂的工作空間點云圖。

回轉(zhuǎn)機械臂在升降機構(gòu)的基礎(chǔ)上，可對豎直范圍374～2 613 mm分布的果實進(jìn)行采摘，底盤距植株400 mm時的水平方向最大采摘深度為753 mm，從而滿足矮化密植果園冠幅在1.5 m內(nèi)的雙側(cè)采摘。

2.1.2姿態(tài)分析

結(jié)合機器人的工作空間與果樹的尺寸，對采摘機器人采放果姿態(tài)進(jìn)行分析，如圖2所示。

由圖2可知，根據(jù)果實位置和末端可達(dá)性，機器人采果呈現(xiàn)俯摘、平摘、仰摘3種不同姿態(tài)，同時根據(jù)空中抽拉式果箱與機械臂的相對安裝關(guān)系，放果時機械臂末端放果能到達(dá)的姿態(tài)為小臂與水平角度范圍70°～110°。

2.2 手-眼協(xié)調(diào)的坐標(biāo)變換

對“眼在手上”模式，果實定位需獲得果實相對于RealSense的位置，進(jìn)而根據(jù)RealSense與機械臂的相對安裝位置及其運動學(xué)方程，獲得果實相對于機器人坐標(biāo)系的位置坐標(biāo)。

假設(shè)由RealSense SR300獲得的果實位置坐標(biāo)為S0=(x0,y0,z0)，則目標(biāo)果實相對于機器人坐標(biāo)系的位置S1為

S1=S0M

(1)

其中

(2)

式中M——果實相機坐標(biāo)與機器人坐標(biāo)變換矩陣

z0——多級剪叉升降機構(gòu)豎直直線位移，mm

θ1——水平旋轉(zhuǎn)關(guān)節(jié)角位移，rad

θ2——機械臂大臂轉(zhuǎn)動角位移，rad

θ3——小臂轉(zhuǎn)動角位移，rad

L1～L6——機械臂各桿長及RealSense與機械臂的相對安裝尺寸

L1～L6依次為168、8、313、-100、285、125 mm。

3 基于RealSense遠(yuǎn)近景組合的機械臂運動規(guī)劃

3.1 基于RealSense伺服的遠(yuǎn)近景手眼協(xié)調(diào)策略

與CCD被動視覺相比，主動深度視覺干擾信息少、受光線影響較小，但是消費級RGB-D相機受分辨率和精度影響，只進(jìn)行一次遠(yuǎn)景識別定位不能獲得果實個體清晰的深度圖像且出現(xiàn)較大定位誤差。使用RealSense SR300作為視覺傳感器，可在景深160～1 200 mm范圍內(nèi)獲得遠(yuǎn)景的冠層深度與坐標(biāo)信息，同時可實現(xiàn)160 mm超近景和較小視場內(nèi)有限數(shù)量果葉的“大而清”圖像獲取和可靠識別定位[35-36]?；赗ealSense將遠(yuǎn)景的冠層粗探測、子區(qū)域劃分定位和近景的果實精確識別定位有效結(jié)合，實現(xiàn)對機械臂的由粗及細(xì)引導(dǎo)和精確定位，并通過路徑關(guān)鍵點的分段路徑規(guī)劃和組合以提高“眼在手上”的圖像伺服控制實時性和穩(wěn)定性。

基于RealSense伺服的遠(yuǎn)近景手眼協(xié)調(diào)動作流程如圖3所示。OABCD為初始采摘路徑；DBCD為周期作業(yè)路徑。

(1)機械臂腕部由初始位置O運動到遠(yuǎn)景位A，由RealSense SR300自遠(yuǎn)景位A獲得較大樹冠區(qū)域的深度圖和對應(yīng)坐標(biāo)信息；根據(jù)RealSense的豎直和水平視角范圍(55°×71°)和目標(biāo)距離，可獲得不同距離的視場范圍，進(jìn)而根據(jù)RealSense SR300的最佳近景視場范圍將遠(yuǎn)景深度區(qū)域劃分為若干近景子區(qū)域并完成子區(qū)域的定位(圖4)。

(2)機械臂依次根據(jù)某一近景子區(qū)域的坐標(biāo)，由遠(yuǎn)景位A向該子區(qū)域近景位B快速逼近，并在近景位完成對該子區(qū)域內(nèi)果實的近景精確識別定位。

(3)根據(jù)近景位的果實識別定位結(jié)果，由機械臂將末端執(zhí)行器向果實C送進(jìn)并完成對果實的摘取。

(4)末端由果實位置C運動到果箱內(nèi)的放果位D，完成放果作業(yè)后返回該子區(qū)域的近景位B，對這一子區(qū)域的剩余果實進(jìn)行依次采摘，在采摘點C完成采摘后送至果箱放果位D。

(5)將該子區(qū)域內(nèi)的果實采摘結(jié)束后，依上述流程對下一子區(qū)域繼續(xù)進(jìn)行采摘放果作業(yè)，直至果實采摘完成后，采摘機器人移動至下一遠(yuǎn)景位進(jìn)行下一輪循環(huán)采摘作業(yè)。

在該策略中，RealSense分別在遠(yuǎn)景點和近景點兩次反饋“粗、細(xì)”不同深度信息，引導(dǎo)機械臂由遠(yuǎn)及近迫近目標(biāo)并完成采摘。該方法基于有限關(guān)鍵點的深度視覺伺服實現(xiàn)高精度的機械臂引導(dǎo)和連續(xù)作業(yè)，運算量少，可靠性和穩(wěn)定性得到有效保證，從而為實現(xiàn)快速高成功率的機器人采摘作業(yè)提供保證。

3.2 手眼協(xié)調(diào)的關(guān)鍵路徑點

如圖5所示，RealSense視覺傳感器安裝在機械臂的腕部，在遠(yuǎn)近景協(xié)調(diào)策略下分別在遠(yuǎn)景點與近景點反饋目標(biāo)深度信息，引導(dǎo)機械臂運動至目標(biāo)果實采摘。根據(jù)機械臂的作業(yè)范圍，進(jìn)行RealSense遠(yuǎn)近景最佳探測范圍試驗，可知RealSense SR300的最佳遠(yuǎn)景探測范圍為500～700 mm，最佳近景識別距離為200 mm。關(guān)鍵位的設(shè)定對機械臂的精確定位非常關(guān)鍵，在每個關(guān)鍵位的手-眼任務(wù)如下：

(1)遠(yuǎn)景位：機械臂距離冠層500～700 mm處以水平姿態(tài)掃描冠層，順利獲得一定高度遠(yuǎn)景的完整冠層深度數(shù)據(jù)，以進(jìn)行子區(qū)域劃分。700 mm處探測區(qū)域為1 008 mm×729 mm，500 mm處探測區(qū)域為720 mm×520 mm。

(2)近景位：機械臂在確定子區(qū)域后，運動至目標(biāo)子區(qū)域的中心點，即為距離子區(qū)域200 mm處的最佳近景位，仍然以水平姿態(tài)作為理想姿態(tài)，視覺傳感器探測區(qū)域為288 mm×208 mm。

(3)采摘位：機械臂根據(jù)目標(biāo)果實的坐標(biāo)，經(jīng)過姿態(tài)規(guī)劃到達(dá)采摘位，以水平或俯或仰的姿態(tài)進(jìn)行采摘。

(4)放果位：在雙側(cè)果箱的中心點，采摘完成后，機械臂移送果實至放果位。

3.3 采放果機械臂作業(yè)軌跡分段規(guī)劃

3.3.1軌跡規(guī)劃方法

為實現(xiàn)對機械臂的控制，需要在規(guī)劃之前給定初始與目標(biāo)點的機械臂末端的位姿。將機器人在笛卡爾空間的位姿換算為關(guān)節(jié)角，用函數(shù)對關(guān)節(jié)角擬合進(jìn)行軌跡描述。關(guān)節(jié)空間下常用的軌跡插值方法有三次多項式和五次多項式，三次多項式函數(shù)只對軌跡上的位置與速度進(jìn)行約束，為保證機械臂運動的平滑性，還需對關(guān)節(jié)起停處的加速度進(jìn)行約束，即使用五次多項式進(jìn)行軌跡插值。五次多項式插值函數(shù)式為

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(3)

對式(3)進(jìn)行一階與二階求導(dǎo)，可得到機械臂關(guān)節(jié)角速度時間函數(shù)以及加速度時間函數(shù)。通過起終點直接選取的各路徑點的約束條件可確定線性方程組，從而求解方程各系數(shù)。

在設(shè)定各關(guān)鍵點并確定直線軌跡后，需在各段軌跡上添加一定的插值點，并對這些插值點進(jìn)行五次多項式的插值計算，得出插值點的位姿坐標(biāo)隨時間的變化率，即通過插值點描述起點到終點過程末端位姿隨時間的變化關(guān)系。

3.3.2分段軌跡規(guī)劃

機械臂作業(yè)軌跡可分為采果與放果兩段軌跡，考慮到作業(yè)效率與控制難度，從初始位到遠(yuǎn)景位，以及采果完成后移送至果箱的這兩段路徑都采用直線軌跡作為本文最短路徑規(guī)劃方案。

從遠(yuǎn)景位到近景范圍、再到果實采摘位的過程，可以理解為機械臂從遠(yuǎn)景位逼近果實的作業(yè)路徑。在完成果實采摘后，機械臂再從采摘位將果實移送至果箱，進(jìn)而回復(fù)至遠(yuǎn)景位。由采摘位再結(jié)合遠(yuǎn)景位的位姿進(jìn)行逆解，在關(guān)節(jié)空間內(nèi)進(jìn)行軌跡規(guī)劃后生成關(guān)節(jié)角、角速度和加速度，使機械臂到達(dá)給定位姿(圖6)。

3.3.3軌跡銜接

從初始位置到遠(yuǎn)景位，機械臂末端速度在路徑兩端都為0，從遠(yuǎn)景點到近景點過程為快速逼近過程，速度不為0，而采果位到放果位這段軌跡的兩端速度都為0。根據(jù)采摘作業(yè)流程與各關(guān)鍵點可知，需對初始位到遠(yuǎn)景位、遠(yuǎn)景位到采果位以及采果位到放果位進(jìn)行三段直線軌跡規(guī)劃。首先需要將笛卡爾空間內(nèi)的各關(guān)鍵點坐標(biāo)轉(zhuǎn)換成關(guān)節(jié)空間下各轉(zhuǎn)動關(guān)節(jié)的角度和直動關(guān)節(jié)(升降)的位置，如表1所示。

表1 關(guān)鍵點坐標(biāo)與關(guān)節(jié)參數(shù)示例

為保證各擬合點直線軌跡的準(zhǔn)確性，每15 mm選取一個插值點，通過逆解得到各插補點對應(yīng)的機械臂各關(guān)節(jié)參數(shù)。對各段直線軌跡進(jìn)行Matlab編程仿真，得到P0到P1、P1到P2以及P2到P3的各段空間直線插補軌跡。

4 樣機與試驗驗證

4.1 樣機設(shè)計

小型升降式采摘機器人樣機如圖7所示，該樣機主要技術(shù)參數(shù)與性能指標(biāo)如表2所示。

表2 樣機參數(shù)

4.2 驗證試驗

4.2.1試驗材料

如圖8所示，為方便采摘機器人精度試驗的檢測，將定制的兩塊1 m×1 m、間隔尺寸為2 mm的透明刻度板通過鋁型材搭建成直角坐標(biāo)系的坐標(biāo)檢測裝置，并在機械臂腕部安裝激光筆。

調(diào)整并確定機械臂的初始位置，RealSense相機深度方向作為Z軸，測量機械臂初始位置在高度方向即X方向為800 mm，腰部旋轉(zhuǎn)關(guān)節(jié)中心線為Y方向，在網(wǎng)格刻度板上記錄下相對于XOY平面為(800 mm，0，0)的參考點(圖9b)。之后每次測量末端坐標(biāo)的X、Y坐標(biāo)都通過坐標(biāo)點相對于該參考點的讀數(shù)，而Z方向可通過游標(biāo)卡尺測量，從而得出采摘機器人末端的實際坐標(biāo)。

4.2.2試驗方法

(1)采摘機器人底盤固定不動，調(diào)整機械臂至初始位置，在Z方向的透明刻度板上固定若干塑料柑橘果實，將果實中心點作為目標(biāo)點，并根據(jù)刻度記錄果實的實際坐標(biāo)。

(2)應(yīng)用基于RealSense伺服的遠(yuǎn)近景手眼協(xié)調(diào)策略和采放果機械臂作業(yè)軌跡分段規(guī)劃，執(zhí)行不同關(guān)鍵位的和各關(guān)鍵位之間的手-眼任務(wù)，完成單周期的果實采-放。

(3)調(diào)整鋁型材框架與機器人基坐標(biāo)系在Z方向的距離，并在不同的Z1、Z2、…、Zn平面改變果實的位置，對不同位置果實自主執(zhí)行手眼協(xié)調(diào)的果實采-放動作。

(4)根據(jù)激光筆在刻度板上的Mark點讀取每次末端在XOY面的投影坐標(biāo)(圖9c)，由游標(biāo)卡尺測量末端與刻度板的距離(圖9d)，結(jié)合刻度板在機器人基坐標(biāo)系中Z方向的距離，得到末端在Z軸的坐標(biāo)值。同時運行過程中用DV攝像機攝像，用視頻編輯軟件截取機械臂運動的視頻片段，得到其運行時間。

4.2.3試驗結(jié)果分析

采摘機器人由遠(yuǎn)及近手眼協(xié)調(diào)動作的過程如圖10所示。末端實際到達(dá)的坐標(biāo)點測量值與相應(yīng)耗時如表3所示。

由表3可知，在RealSense信息反饋的遠(yuǎn)近景協(xié)調(diào)策略下，機器人在X、Y、Z方向的平均誤差分別為3.51、2.79、3.35 mm。采摘作業(yè)耗時隨果實距離的增加而增加，平均耗時為19.24 s，其中機械臂從初始位到采果動作的平均耗時為12.04 s，中間識別與運算的平均耗時為3.82 s，而放果動作平均耗時為7.2 s，機械臂動作耗時占整個環(huán)節(jié)的80.2%。該手眼協(xié)調(diào)動作精度基本滿足采摘要求，達(dá)到了較好的采摘精度及效率的綜合效果。但是，其精度、效率仍然有較大的提升可能。本自主開發(fā)的機器人各關(guān)節(jié)存在一定誤差，特別是3回轉(zhuǎn)機械臂各關(guān)節(jié)之間存在一定間隙，經(jīng)測試靜止?fàn)顟B(tài)下機械臂的最大間隙為(2.8，2.2，2.5) mm，大約占實際定位誤差的80%，通過優(yōu)化可以顯著提高機器人手眼協(xié)調(diào)動作精度。試驗采取間隔15 mm插值計算機械臂路徑，機械臂實際到達(dá)精度會隨著插值點間隔的減小而提高，同時機器人計算能力的提升將使識別定位、插值計算時間進(jìn)一步減小。

表3 末端位置坐標(biāo)測量結(jié)果

5 結(jié)論

(1)設(shè)計了在手RealSense深度伺服的小型升降式采摘機器人，建立了手-眼協(xié)調(diào)的姿態(tài)與坐標(biāo)關(guān)系。

(2)采用眼在手上的手眼協(xié)調(diào)方式，對采摘機器人提出了基于在手RealSense深度伺服的由遠(yuǎn)及近手眼協(xié)調(diào)策略，并完成了基于深度視覺和遠(yuǎn)近景協(xié)調(diào)策略的作業(yè)流程及運動規(guī)劃。

(3)進(jìn)行了手眼協(xié)調(diào)采摘試驗，結(jié)果表明末端在X、Y、Z方向平均定位精度為3.51、2.79、3.35 mm，平均耗時為19.24 s，其中機械臂從初始位開始采果的平均耗時為12.04 s，中間識別與運算的平均耗時為3.82 s，放果動作平均耗時為7.2 s，機械臂動作耗時占整個環(huán)節(jié)的80.2%。

(4)該機器人結(jié)構(gòu)和在手RealSense深度伺服的手眼協(xié)調(diào)策略可滿足采摘作業(yè)需求，同時關(guān)節(jié)精度的提高和路徑插值計算的優(yōu)化可進(jìn)一步提升手眼協(xié)調(diào)的精度和效率。