周毛等 張 棟 周 濤 陳 霞

（青島理工大學(xué)信息與控制工程學(xué)院 青島 266520）

1 引言

隨著科技的不斷進(jìn)步與發(fā)展，采摘機(jī)器人將成為代替人力作業(yè)，提高農(nóng)業(yè)產(chǎn)值的關(guān)鍵。不同種類的采摘機(jī)器人的產(chǎn)生，如西紅柿采摘機(jī)器人［1］，蘋果采摘機(jī)器人［2］，葡萄采摘機(jī)器人［3］等，都對農(nóng)業(yè)科技的發(fā)展有著重要意義。在執(zhí)行采摘任務(wù)的過程中，機(jī)械臂的控制是非常重要的，其中機(jī)械臂正逆運(yùn)動(dòng)學(xué)建模與求解和目標(biāo)定位是采摘機(jī)器人機(jī)械臂控制的關(guān)鍵問題。同時(shí)，采摘機(jī)器人通常還面臨一些非結(jié)構(gòu)化的和復(fù)雜的外部環(huán)境，為使采摘機(jī)械臂具有從未知環(huán)境中獲取信息的能力，需要給采摘機(jī)械臂配備外部傳感器，雙目視覺傳感器作為采摘機(jī)器人的“眼睛”，可以用來感知環(huán)境以及實(shí)現(xiàn)農(nóng)業(yè)目標(biāo)果實(shí)的識別和獲取目標(biāo)物體的三維坐標(biāo)信息，為采摘機(jī)械臂的控制帶來方便。能否準(zhǔn)確、快速識別出采摘目標(biāo)，直接影響著采摘效率。因此，農(nóng)業(yè)目標(biāo)果實(shí)的識別也是采摘機(jī)器人的關(guān)鍵技術(shù)之一，吸引著廣大學(xué)者的深入持續(xù)研究［4～5］。

本文首先采用經(jīng)典 D-H［6～8］方法對采摘機(jī)械臂進(jìn)行數(shù)學(xué)建模，在此基礎(chǔ)上進(jìn)行逆運(yùn)動(dòng)學(xué)求解以獲得滿足末端執(zhí)行器位姿的各關(guān)節(jié)角，并采用顏色分割［9～11］的方法對目標(biāo)番茄進(jìn)行識別，根據(jù)識別出的番茄目標(biāo)三維坐標(biāo)信息來控制采摘機(jī)械臂完成采摘任務(wù)。最后通過實(shí)驗(yàn)，表明所設(shè)計(jì)的采摘機(jī)器人果實(shí)識別與采摘成功率較高。

2 采摘機(jī)械臂的數(shù)學(xué)模型

機(jī)械臂數(shù)學(xué)模型的建立是正逆運(yùn)動(dòng)學(xué)的重要部分，本系統(tǒng)采用6自由度機(jī)械臂，通過D-H（dena?vit-hartenberg）法建立采摘機(jī)械臂的數(shù)學(xué)模型。采摘機(jī)械臂參考坐標(biāo)系如圖1所示，根據(jù)D-H法可以得出采摘機(jī)械臂的D-H參數(shù)，如表1所示。

圖1 采摘機(jī)械臂各關(guān)節(jié)參考坐標(biāo)系

表1 D-H參數(shù)

表1中，θ表示繞z軸的旋轉(zhuǎn)角；d表示在z軸上兩條相鄰公垂線之間的距離；α表示兩個(gè)相鄰z軸之間的旋轉(zhuǎn)角；a表示相鄰兩個(gè)關(guān)節(jié)軸線方向的公垂線的長度。相鄰坐標(biāo)系變換矩陣可通過表示四個(gè)運(yùn)動(dòng)的矩陣相乘得到：

機(jī)器人的正運(yùn)動(dòng)學(xué)方程建立如下，由于關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)的串聯(lián)機(jī)械臂，其中關(guān)節(jié)和連桿參數(shù)dj、αj、和 aj均為常數(shù)。

3 采摘機(jī)械臂運(yùn)動(dòng)學(xué)逆解

逆運(yùn)動(dòng)學(xué)問題在機(jī)器人運(yùn)動(dòng)學(xué)研究中占有重要的地位，直接影響著控制的精確性。根據(jù)已知的末端執(zhí)行器的位置和姿態(tài)，求解相應(yīng)的關(guān)節(jié)角。

若要求θ1，可將上式兩邊同時(shí)乘以0T1(θ1)-1，得到：

再由式（1）兩邊矩陣對應(yīng)元素相等，消去θ2，θ3，θ4，θ5，θ6后，求得 θ1。同理由：

求得 θ2；

求得 θ3；

求得 θ4；

求得 θ5和 θ6。

4 采摘機(jī)械臂關(guān)節(jié)空間運(yùn)動(dòng)

圖2 采摘機(jī)械臂的三維模型

根據(jù)采摘機(jī)械臂的D-H參數(shù)，建立采摘機(jī)械臂的模型如圖2所示，并對隨時(shí)間變化的各關(guān)節(jié)角做仿真，關(guān)節(jié)運(yùn)動(dòng)軌跡是平滑的如圖3所示，末端執(zhí)行器在笛卡兒空間運(yùn)動(dòng)軌跡如圖4所示。

圖3 隨時(shí)間變化的關(guān)節(jié)坐標(biāo)

圖4 末端執(zhí)行器隨時(shí)間變化的笛卡兒位置

5 目標(biāo)的識別與定位

對基于視覺的采摘機(jī)械臂來說，視覺是非常重要的部分。只有在視覺的引導(dǎo)下，才能獲得目標(biāo)的正確信息，從而控制機(jī)械臂完成指定的任務(wù)工作。采摘機(jī)器人的視覺系統(tǒng)在標(biāo)定的基礎(chǔ)上，識別和定位目標(biāo)對象。

5.1 雙目測距的原理

雙目測距原理［12～14］如圖5所示，兩個(gè)攝像機(jī)成像平面位于同一平面，光軸平行，基線距離一定，焦距相等，假設(shè)主點(diǎn)和已經(jīng)校準(zhǔn)，在左右圖像上具有相同的像素坐標(biāo)。一般情況下，相平面很少與鏡頭完美重疊，圖像中心點(diǎn)也幾乎不會和像主點(diǎn)重合，引入cx和cy來模擬圖像中心可能的位移。P是空間一點(diǎn)，P世界坐標(biāo)系為(X，Y，Z)，左右圖像上的成像點(diǎn) pl和 pr，對應(yīng)的橫坐標(biāo)為xl和xr，假設(shè)左邊相機(jī)坐標(biāo)系為世界坐標(biāo)O-XYZ，左邊圖像坐標(biāo)系為ol-xlyl，右邊圖像坐標(biāo)系為or-xryr，視差d=xl-xr。由透視變化三角幾何關(guān)系得：

其中 f表示相機(jī)的焦距，T表示兩相機(jī)之間的基線長，焦距 fx實(shí)際上透鏡的物理距離長度與成像儀每個(gè)單元尺寸sx的乘積（sx的單位是像素/毫米，物理焦距的單位是mm，fx的單位是像素）。

圖5 雙目測距原理圖

5.2 目標(biāo)識別

（a）原始圖像；（b）用偽色彩顯示的像素分類（C=4）；（c）在xy色度二維空間中的點(diǎn)集中心；（d）類型c=2的所有像素；（e）用半徑為15的圓形結(jié)構(gòu)化元素進(jìn)行形態(tài)學(xué)開操作后的結(jié)果；（f）用半徑為4的圓形結(jié)構(gòu)化元素進(jìn)行形態(tài)學(xué)閉操作后的結(jié)果

基于顏色的圖像分割是一種直觀有效的方法，首先把每一個(gè)彩色像素轉(zhuǎn)換成xy色度坐標(biāo)，使用K均值算法［15］找出平面上的點(diǎn)集，且每一個(gè)點(diǎn)集對應(yīng)一種可辨識顏色的像素群。K均值算法必須指定要尋找的點(diǎn)集數(shù)量，該場景有4種不同顏色的元素：紅色番茄、有點(diǎn)黃葉子、橘色番茄、灰暗的背景，像素被類聚到4個(gè)色度類型。圖像中的像素具有值 c={0，1，2，3}，它表明相應(yīng)的輸入像素被分配到了哪一類。選擇屬于類型2的像素，它是一個(gè)局部圖像，類型2的所有像素被顯示為白色，對應(yīng)原始圖像中的紅色目標(biāo)。番茄由于鏡像反射都出現(xiàn)了空洞，通過形態(tài)學(xué)操作來提高圖像處理結(jié)果，把圖像像素分為目標(biāo)和非目標(biāo)。

5.3 目標(biāo)定位

為實(shí)現(xiàn)采摘機(jī)械臂對番茄的自主采摘，關(guān)鍵是獲取番茄的三維空間位置，以便為末端執(zhí)行器動(dòng)作提供參數(shù)。本文通過顏色分類的方法識別出番茄目標(biāo)，確定番茄的數(shù)量及每個(gè)番茄對應(yīng)的像素點(diǎn)二維坐標(biāo)。假設(shè)第i個(gè)番茄對應(yīng)像素點(diǎn)構(gòu)成集合si，該番茄的質(zhì)心坐標(biāo)(xi，yi)可由下式求得：

其中：n為集合si中元素的個(gè)數(shù)；(xk，yk)為si中的元素。在得到每個(gè)番茄質(zhì)心二維坐標(biāo)［16］后，由雙目得到深度圖像中對應(yīng)像素點(diǎn)的深度值，利用OpenCV中的reprojectimageTo3D函數(shù)，可以計(jì)算出每個(gè)番茄質(zhì)心的三維坐標(biāo)值。從而采摘機(jī)械臂實(shí)現(xiàn)目標(biāo)定位，控制器控制采摘機(jī)械臂將目標(biāo)番茄摘下。

6 實(shí)驗(yàn)

為驗(yàn)證采摘機(jī)械臂控制、目標(biāo)識別定位的準(zhǔn)確性與有效性，在室內(nèi)模擬場景下進(jìn)行實(shí)驗(yàn)。

6.1 采摘機(jī)器人硬件介紹

采摘機(jī)器人由6自由度機(jī)械臂構(gòu)成，底部由輪子組成。采用萊娜機(jī)器視覺公司的HNY-CV-002作為雙目視覺傳感器，通過提取目標(biāo)物體的顏色信息進(jìn)行RGB空間的分割，最終得到目標(biāo)物體的三維坐標(biāo)。采摘機(jī)器人的主控制器是Raspberry Pi 3B。

6.2 實(shí)驗(yàn)過程

實(shí)驗(yàn)通過分析番茄目標(biāo)物體實(shí)際坐標(biāo)位置與末端執(zhí)行器到達(dá)位置之間的誤差，從而證明機(jī)械臂算法與識別定位的正確性。視覺系統(tǒng)處理圖像速度為30幀/s，每一幀圖像的分辨率為640×360。本實(shí)驗(yàn)用于近距離測量（500mm內(nèi)），實(shí)驗(yàn)得出采摘機(jī)器人計(jì)算得到的位置與目標(biāo)番茄實(shí)際位置之間的平均誤差為（4.8，9.2，5.3）（mm），實(shí)驗(yàn)誤差在允許誤差范圍內(nèi)，采摘機(jī)器人可以完成自主采摘任務(wù)，文中的方法得到驗(yàn)證。采摘機(jī)器人采摘的流程圖如圖6所示。

圖6 采摘機(jī)器人采摘流程圖

7 結(jié)語

首先用D-H方法對采摘機(jī)械臂進(jìn)行建模，給出了6自由度機(jī)械臂逆解算法，并采用顏色分割的方法對目標(biāo)果實(shí)進(jìn)行識別，根據(jù)識別出的番茄目標(biāo)三維坐標(biāo)信息控制機(jī)械臂完成采摘任務(wù)。最后，在室內(nèi)模擬農(nóng)業(yè)現(xiàn)場場景中進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明所設(shè)計(jì)的采摘機(jī)器人對果實(shí)識別與采摘成功率較高。