基于雙目視覺的采摘機器人機械臂控制?

周毛等 張 棟 周 濤 陳 霞

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

1 引言

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

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

2 采摘機械臂的數(shù)學模型

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

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

表1 D-H參數(shù)

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

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

3 采摘機械臂運動學逆解

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

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

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

求得 θ2；

求得 θ3；

求得 θ4；

求得 θ5和 θ6。

4 采摘機械臂關(guān)節(jié)空間運動

圖2 采摘機械臂的三維模型

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

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

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

5 目標的識別與定位

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

5.1 雙目測距的原理

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

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

圖5 雙目測距原理圖

5.2 目標識別

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

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

5.3 目標定位

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

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

6 實驗

為驗證采摘機械臂控制、目標識別定位的準確性與有效性，在室內(nèi)模擬場景下進行實驗。

6.1 采摘機器人硬件介紹

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

6.2 實驗過程

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

圖6 采摘機器人采摘流程圖

7 結(jié)語

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