周毛等 張 棟 周 濤 陳 霞
(青島理工大學(xué)信息與控制工程學(xué)院 青島 266520)
隨著科技的不斷進(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í)識別與采摘成功率較高。
機(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ù)。
逆運(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。
圖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í)間變化的笛卡兒位置
對基于視覺的采摘機(jī)械臂來說,視覺是非常重要的部分。只有在視覺的引導(dǎo)下,才能獲得目標(biāo)的正確信息,從而控制機(jī)械臂完成指定的任務(wù)工作。采摘機(jī)器人的視覺系統(tǒng)在標(biāo)定的基礎(chǔ)上,識別和定位目標(biāo)對象。
雙目測距原理[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 雙目測距原理圖
(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)。
為實(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)番茄摘下。
為驗(yàn)證采摘機(jī)械臂控制、目標(biāo)識別定位的準(zhǔn)確性與有效性,在室內(nèi)模擬場景下進(jìn)行實(shí)驗(yàn)。
采摘機(jī)器人由6自由度機(jī)械臂構(gòu)成,底部由輪子組成。采用萊娜機(jī)器視覺公司的HNY-CV-002作為雙目視覺傳感器,通過提取目標(biāo)物體的顏色信息進(jìn)行RGB空間的分割,最終得到目標(biāo)物體的三維坐標(biāo)。采摘機(jī)器人的主控制器是Raspberry Pi 3B。
實(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ī)器人采摘流程圖
首先用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í)識別與采摘成功率較高。