基于視覺的機器人采摘實驗平臺設計與實現(xiàn)

杜文龍， 徐雪峰

(淮安信息職業(yè)技術學院， 江蘇 淮安 223003)

0 引 言

機器人通常采用視覺伺服控制，自主地抓取和操縱各種感興趣的物體[1-2]?，F(xiàn)代視覺伺服控制方案可以分為基于圖像的、基于位置的和先進的方法。這些視覺伺服方案往往需要推導一個機器人—目標交互矩陣之間的圖像特征和機器人運動學。然而，推導相互作用矩陣并不是一個簡單的問題，特別是對于一個未標定的視覺伺服系統(tǒng)。此外，交互矩陣與每個圖像特征點相對于攝像機幀的深度值有關，這在單目視覺系統(tǒng)中很難測量。因此，許多先進的視覺伺服方案都需要使用一個估計過程來估計當前相機目標配置的每個視覺特征或交互矩陣的深度值。這一要求對視覺伺服控制系統(tǒng)的視覺和估計設計提出了更高的要求。本文提出了一種視覺伺服系統(tǒng)，將基于圖像和位置的視覺伺服系統(tǒng)集成到一個可變結構框架中，無需使用交互矩陣、模糊建?；蚰：?guī)則庫。

1 機器人采摘系統(tǒng)

1.1 采摘機器人概述

近年來，由于出生率下降和農民老齡化的影響，農業(yè)勞動力短缺問題日益嚴重。為解決這一問題，可利用采摘機器人，進行快速的農產品采摘并保持高品質。采摘機器人在水果識別、切割系統(tǒng)、移動系統(tǒng)等方面存在一定的困難。利用攝像機對水果進行圖像處理，是一種新的水果識別方法[3-5]。為此，研究了農產品采摘機器人系統(tǒng)。本文針對采摘機器人應用常用的圖像處理技術進行了開發(fā)和可行性研究，構建了采摘物及其位置的識別系統(tǒng)、切莖攝像定位系統(tǒng)和使用這些系統(tǒng)的采摘機器人。

1.2 采摘機器人結構分層

機器人采摘系統(tǒng)采用分核控制的方式，如圖1所示，將獨立的功能模塊綁定到單獨的CPU上，對于每個核可以單獨設置最大的負荷、時基以及可能的周期時間；各獨立模塊間通過共享數(shù)據(jù)、系統(tǒng)文件及網(wǎng)絡通訊等方式進行數(shù)據(jù)交互，實現(xiàn)功能的完全解耦。IV型控制器采用分核控制的方式，將獨立的功能模塊綁定到單獨的CPU上，對于實時內核環(huán)境使用的每個核，可以單獨設置最大的負荷、時基以及可能的周期時間；各獨立模塊間通過共享數(shù)據(jù)、系統(tǒng)文件、網(wǎng)絡通訊等方式進行數(shù)據(jù)交互，實現(xiàn)功能的完全解耦。這里，對其中各分層的設計功能解析可闡釋如下。

(1)RSP層：將原有獨立的導航算法和手臂控制算法統(tǒng)一整理到通用算法庫，并結合系統(tǒng)管理和總線功能，向上提供機器人系統(tǒng)所需的基本功能。

(2)通用平臺層：在RSP層的基礎上，調用不同的算法和總線協(xié)議接口，重新構建不同方向的機器人平臺。

(3)信息數(shù)據(jù)交互層：負責跟手臂平臺的示教盒、移動平臺的PAD和車隊管理系統(tǒng)進行數(shù)據(jù)通信。車隊管理系統(tǒng)負責整個復合機器人的：車隊編隊、地圖編輯和手臂任務觸發(fā)。

圖1 軟件系統(tǒng)結構

整個機器人控制軟件系統(tǒng)包括機器人控制器軟件和上位機調度軟件。研究可得各部分的設計闡述如下。

(1)操作系統(tǒng)：采用風河實時操作系統(tǒng)。

(2)基礎函數(shù)庫：為機器人系統(tǒng)提供基礎功能函數(shù)。包括：

① 機器人服務平臺，將機器人系統(tǒng)與操作系統(tǒng)隔離，并實現(xiàn)基本的總線協(xié)議棧和數(shù)據(jù)工具；

② 機器人算法庫，實現(xiàn)機器人手臂、底盤相關算法功能。

(3)機器人基礎系統(tǒng)：整個機器人系統(tǒng)由3個單獨的進程構成。包括：

① 機械臂平臺進程，用于控制復合機器人的機械臂，實現(xiàn)模型正反解、軌跡規(guī)劃及位置插補等功能；

② 移動平臺進程，用于控制復合機器人的底盤，實現(xiàn)底盤在二維激光傳感器下的地圖創(chuàng)建、實時定位、軌跡規(guī)劃、運動控制及安全保護等，對于不同的移動底盤，可以開啟不同的移動平臺進程；

③ 協(xié)調進程，用于控制整個復合機器人系統(tǒng)，包括系統(tǒng)的上下電、手臂底盤的任務協(xié)調等。

2 機器人運動控制

對串聯(lián)機器人逆運動學問題的研究已有幾十年的歷史。因其在機械手的控制中是必需的。求解逆運動學在計算上是可拓的，在機械手的實時控制中通常需要很長時間。圖2為斯坦福機械手的關節(jié)軸和尺度分配﹐就既能幫助渡過運動不確定位置﹐又能增加最大啟動牽引力。機械手執(zhí)行的任務是在直角坐標系中進行的，而執(zhí)行機構是在關節(jié)空間中進行的。笛卡爾空間包括方向矩陣和位置向量。然而，關節(jié)空間由關節(jié)角表示。將機械手末端執(zhí)行器的位置和姿態(tài)由笛卡爾空間轉換為關節(jié)空間，稱為逆運動學問題。解析地推導逆運動學解有2種解法，即幾何解法和代數(shù)解法。這里采用幾何方法。

圖2 機械手的關節(jié)軸和尺度分配

2.1 運動學正解

機器人正運動學是為了解決末端位置的求解問題，已知關節(jié)軸的轉角，通過運動學正解即可求解末端姿態(tài)和位置。矩陣T為2個坐標系相互之間的對應關系，矩陣繞各個軸的相對關系如下：

(1)

根據(jù)機械構型，求得各連桿變換矩陣如下：

(2)

(3)

其中，di為連桿位置偏距。推導過程：c23與s23分別為θ2與θ3之和的c(θ2+θ3)余弦與s(θ2+θ3)正弦。即：

(4)

最后，得到6個連桿坐標變換矩陣的乘積：

(5)

其中，rii為旋轉關系元素；pi為空間位置。

研究可得，位置末端的數(shù)學公式可寫為如下形式：

(6)

研究可得，工具末端的數(shù)學公式可寫為如下形式：

(7)

2.2 運動學反解

(8)

(9)

(10)

(1)解析第一軸。利用上面公式，計算求得第一關節(jié)的角度θ1。運算時用到如下數(shù)學公式：

0?sin(φ-θ1)=

(11)

(2)解析第三軸。計算求得第三關節(jié)的角度θ3。運算時需用到如下數(shù)學公式：

(12)

將上式分別平方后相加，得：

d2d3cos(θ23-θ2)+d2d4sin(θ23-θ2)=

[(c1px+s1py-d1)2+pz2-d22-d32-d42]/2

sin(φ+θ3)=[(c1px+s1py-d1)2+pz2-

d22-d32-d42]/(2ρ)=k;

(13)

(3)解析第二軸。計算求得第二、三關節(jié)的角度之和θ23，進而求得第二關節(jié)角度θ2。運算時需用到如下數(shù)學公式：

(14)

令式兩邊元素(1,4)和元素(2,4)相等得到：

(15)

θ23=atan2(s23,c23)?θ2=θ23-θ3.

分析可知，θ2有4個值。

(4)解析第四軸。計算求得第四關節(jié)的角度θ4。運算時需用到如下數(shù)學公式：

(16)

當關節(jié)5不處于零位，由元素(1,3)和元素(3,3)以求得關節(jié)4的值:

(17)

θ4=atan2(s1r13-c1r23,c1c23r13+s1c23r23+s23r33),

(18)

θ5=0時，操作臂處于奇異位形，此時關節(jié)軸4和關節(jié)軸6成一條直線，機器人末端連桿的運動只有一種，在這種情況下，所有結果都是θ4和θ6的和或差，這種情況可以通過檢查θ4的反正切的2個變量是否趨近于零來判斷。如果都趨近于零，則θ4可以任意選取，之后選取θ6時，可以參照θ4進行選取。

(5)解析第六軸。計算求得第六關節(jié)的角度θ6?？梢郧蟮藐P節(jié)6的值。運算時需用到如下數(shù)學公式：

(19)

θ6=atan2(c1s23r12+s1s23r22-c23r32,-c1s23r11-s1s23r21+c23r31),

(20)

(6)解析第五軸。計算求得第五關節(jié)的角度θ5?？梢郧蟮藐P節(jié)5的值。運算時需用到如下數(shù)學公式：

(21)

(22)

(23)

θ5=atan2(s5,c5),

(24)

由于操作臂腕關節(jié)“翻轉“可以得到另外4個解，即：

(25)

綜上研究匯總后，可得整體涉及的數(shù)學公式詳見如下：

(26)

3 機器視覺伺服

3.1 視覺伺服系統(tǒng)

本次研究中，圖像處理系統(tǒng)由兩臺彩色CCD攝像機、一塊采集板和圖像處理應用程序組成。圖像處理系統(tǒng)配置的相機是RF系統(tǒng)的高成像質量CCD相機，規(guī)格為68萬像素1/4英寸CCD, 450電視線分辨率。圖像采集板為Leutron PicPort，圖像處理應用為MVTec HALCON。將兩臺攝像機并聯(lián)放置，用立體視覺法檢測待采摘物的位置。相機的定位系統(tǒng)如圖3所示，在該定位系統(tǒng)中，攝像機可以在水平方向、垂直方向和深度方向上跟隨待采摘物進行定位。定位系統(tǒng)各尺寸均有滑軌、正時帶和直流電機。所有電機均由DSP控制器控制。圖像處理系統(tǒng)將位置信息提供給控制器。定位系統(tǒng)根據(jù)這些信息控制攝像機的位置。

圖3 采摘機器人視覺機構框圖

3.2 圖像處理

本次研究是開發(fā)一種能夠識別待采摘物的位置的采摘機器人，可以切下甜椒莖，作為農民將采摘物放入容器中。該拾取機器人所需的主要功能如圖4所示。故而，這既是一種識別技術，也是一種采摘技術，一種移動技術。為了實現(xiàn)這些功能，采摘機器人具有平行立體視覺的圖像處理系統(tǒng)、通過視覺反饋控制跟蹤被識別甜椒的定位系統(tǒng)和切割裝置。摘選機器人大約有1 000 mm寬，550 mm深，1 400 mm高。當車輪不動時，工作范圍水平方向230 mm，垂直方向180 mm，深度方向180 mm。在此范圍內，采摘裝置固定在安裝2個攝像頭的機架上。啟動后，可向前移動80 mm，并用修枝機將被識別的甜椒的莖部剪掉。

圖4 圖像處理系統(tǒng)

工件識別只解決了搬運哪個工件的問題，至于要搬的工件在哪，還需要對工件進行準確的定位。如本文開篇所述，工件定位包括工件的位置定位和工件的姿態(tài)定位，位置定位是依據(jù)中心定位實現(xiàn)的，姿態(tài)定位是通過確定工件長軸的朝向實現(xiàn)的。

在圖像經平滑、二值化、多目標分塊處理一系列過程后，各工件就被分割成獨立的區(qū)域，利用圖像的幾何不變矩特征求取區(qū)域的質心，對于二值圖像來說，該區(qū)域的質心也就是工件的中心。

圖像的幾何不變矩特征主要表現(xiàn)了圖像區(qū)域的幾何特征，又稱為幾何矩，由于其具有旋轉、平移、尺度等特性的不變特征，所以又稱其為不變矩。具體地對于區(qū)域大小為M×N的數(shù)字圖像f(x,y)，其(p+q)階矩定義為：

(27)

其中，f(x,y)相當于一個像素的質量；Mpq為不同p、q值下的圖像的矩。零階矩M00是區(qū)域密度的總和，即：

(28)

一階矩M10為圖像對y軸的慣性矩；M01為圖像對x軸的慣性矩。

(29)

在采摘機器人目標抓取時，計算的中心在圖像中的坐標代入攝像機標定的關系矩陣，可得工件表面中心在機器人坐標系中的位置，即可獲取抓取的位置中心。

4 系統(tǒng)實驗

4.1 抓取實驗的設置

本文采摘針對的是蔬菜采摘，不存在遮擋問題，同時要求機器人抓取和搬運速度快，時間復雜度小。結合上述分析，本文采用基于位置的視覺控制方式的單目Eye-to-Hand方案，即將單目攝像機固定在操作臺上方，通過對工件圖像處理進行工件識別，并確定工件相對于機器人坐標系的位置，從而生成控制信號，完成機器人對蔬菜采摘搬運操作。工件識別與定位是機器人采摘工件的前提和基礎，其識別的結果正確與否和定位的精度直接影響到機器人操作結果的準確性。蔬菜采摘識別是依賴于機器視覺的圖像匹配過程，是通過比較工件圖像與模板圖像來實現(xiàn)的。研究得到的抓取訓練耗時的實驗結果見表1。

表1 抓取訓練耗時

4.2 機械臂控制實驗

為了驗證機器人抓緊過程準確率，共進行了200次的實驗過程，結果見表2。

表2 采摘任務的結果

從測試結果看，視覺識別的準確率為94.5%，從結果看基本達到要求。

5 結束語

本文視覺伺服系統(tǒng)控制器利用基于圖像的視覺伺服來抓取附近感興趣的對象，對攝像機標定誤差和圖像噪聲具有較強的魯棒性。其次對采摘機器人進行了運動學正反關系的建模，同時進行了機器人抓手控制。再次，對采摘機器人的圖像處理部分進行了詳細的分析，提出了一種系統(tǒng)量化的不確定性，對期望的末端執(zhí)行器速度的定位誤差也做了研究測試。該控制器利用基于圖像的視覺伺服來抓取附近感興趣的對象，對攝像機標定誤差和圖像噪聲具有較強的魯棒性。這種基于反應的方法簡化了視覺伺服系統(tǒng)的設計，提高了可靠性和適用性。最后將所提到的方法應用到智能采摘系統(tǒng)的實驗中，證明了運動學和圖像處理算法的有效性，驗證了該視覺伺服控制方法在實際場景中對采摘物識別效率。