雙目視覺伺服的4-DOF機(jī)械手臂運(yùn)動(dòng)控制

張新良，冷正明，趙運(yùn)基，付陳琳

(河南理工大學(xué)電氣工程與自動(dòng)化學(xué)院，河南焦作 454000)

0 引言

多自由度機(jī)械臂和工業(yè)機(jī)器人越來越多地應(yīng)用于生產(chǎn)制造、機(jī)械加工、零件分揀及貨物搬運(yùn)等領(lǐng)域，這類機(jī)器人通過精確的示教作業(yè)，能完成復(fù)雜的點(diǎn)到點(diǎn)的動(dòng)作，極大提高了生產(chǎn)加工效率[1]。然而傳統(tǒng)的示教方法在生產(chǎn)環(huán)境和被控對象變化時(shí)，容易引起目標(biāo)位置和姿態(tài)偏差，進(jìn)而導(dǎo)致產(chǎn)品加工不合格或機(jī)器人任務(wù)執(zhí)行失敗[2]。為了保證機(jī)器人對生產(chǎn)環(huán)境的變化具有適應(yīng)性，有必要引入目標(biāo)識(shí)別和定位技術(shù)實(shí)時(shí)獲取目標(biāo)位置和姿態(tài)的變化，自動(dòng)調(diào)整執(zhí)行機(jī)構(gòu)位姿，減少生產(chǎn)準(zhǔn)備時(shí)間。基于機(jī)器視覺的目標(biāo)識(shí)別和定位是其中一種重要的方法，其基于采集的含有目標(biāo)物體的場景圖像，結(jié)合攝像機(jī)的標(biāo)定參數(shù)，利用空間幾何映射關(guān)系，計(jì)算得到目標(biāo)物體幾何尺寸、形貌和位置等參數(shù)，具有非接觸、動(dòng)態(tài)響應(yīng)快、效率高等特點(diǎn)，在場景建模[3]、目標(biāo)識(shí)別[4]以及生產(chǎn)線上工件尺寸的在線測量[5]等有廣泛的應(yīng)用。相比于單目視覺技術(shù)，雙目視覺不存在盲區(qū)，擁有更豐富的深度信息，可以解決大范圍空間尺寸三坐標(biāo)的現(xiàn)場測量問題。因此，基于雙目視覺技術(shù)的機(jī)械手臂目標(biāo)定位系統(tǒng)，在擺放、裝配、采摘等作業(yè)中具有更廣泛的應(yīng)用價(jià)值[6-7]。本文以矩形目標(biāo)物體為例，結(jié)合四自由度(4-DOF)的機(jī)械手臂執(zhí)行機(jī)構(gòu)，研究了基于雙目立體視覺技術(shù)的目標(biāo)物體識(shí)別、空間位姿測量以及運(yùn)動(dòng)定位控制等關(guān)鍵技術(shù)，結(jié)合機(jī)械手臂的動(dòng)力學(xué)模型分析，給出了控制系統(tǒng)的實(shí)現(xiàn)方法。

1 4-DOF機(jī)械手臂控制系統(tǒng)

4-DOF機(jī)械手臂控制系統(tǒng)整體架構(gòu)如圖1所示。整個(gè)系統(tǒng)由視覺系統(tǒng)和機(jī)械手臂控制系統(tǒng)2部分組成，視覺系統(tǒng)以PC機(jī)為控制核心，采集和處理目標(biāo)圖像。機(jī)械手臂控制系統(tǒng)以DSP為核心，控制機(jī)械手臂抓取目標(biāo)物體放置于目標(biāo)位置。本文設(shè)計(jì)的機(jī)械手臂控制任務(wù)是抓取矩形類物體放置于矩形區(qū)域中。

圖1 機(jī)器視覺輔助定位4-DOF機(jī)械手臂實(shí)驗(yàn)平臺(tái)

控制系統(tǒng)的任務(wù)執(zhí)行采用基于Eye-to-Hand雙目視覺的位置控制方案，利用雙目攝像機(jī)采集目標(biāo)圖像信息，在PC機(jī)上進(jìn)行圖像處理，完成目標(biāo)的識(shí)別與空間位姿測量。然后將目標(biāo)的位姿信息發(fā)送至DSP控制器，轉(zhuǎn)化為機(jī)械手臂各自由度上的控制指令，驅(qū)動(dòng)電機(jī)轉(zhuǎn)動(dòng)相應(yīng)角度，使得末端夾持器以特定的姿態(tài)到達(dá)目標(biāo)物體的位置，控制夾持器實(shí)現(xiàn)對目標(biāo)物體的抓取與搬運(yùn)，同時(shí)在機(jī)械手臂上安裝傳感器形成位置閉環(huán)，保證機(jī)械手臂的控制精度。

2 矩形目標(biāo)識(shí)別與空間位姿測量

根據(jù)控制任務(wù)的要求，對于任意放置的矩形目標(biāo)和放置區(qū)域的位置，4-DOF機(jī)械手臂需要按照矩形的長邊方向抓取目標(biāo)物體，按與放置區(qū)域長邊一致的方向放置目標(biāo)物體。因此，基于雙目視覺的目標(biāo)識(shí)別和位姿測量過程即是提取目標(biāo)(目標(biāo)物體和目標(biāo)區(qū)域)的輪廓特征參數(shù)和矢量參數(shù)，利用圖像匹配算法識(shí)別對象，結(jié)合雙目視覺相機(jī)的標(biāo)定參數(shù)，確定目標(biāo)相對于機(jī)械手臂參考系的空間坐標(biāo)。

2.1 矩形目標(biāo)輪廓提取與匹配

為了保證檢測出單像素邊緣的圖像輪廓，本文通過掏空二值化圖像內(nèi)部像素點(diǎn)，利用鏈碼跟蹤的方法提取和存儲(chǔ)輪廓邊緣信息[8]。首先對目標(biāo)圖像進(jìn)行灰度化、濾波和圖像閾值分割，得到二值圖像。遍歷圖像中的每一個(gè)像素點(diǎn)，如果當(dāng)前像素點(diǎn)灰度值為255，且周圍相鄰8個(gè)像素點(diǎn)灰度值也為255，則該點(diǎn)灰度值置0；如果當(dāng)前像素點(diǎn)灰度值為0，該點(diǎn)灰度值仍置0；除上述2種情況的其他像素點(diǎn)灰度值設(shè)置為255。通過鏈碼的方向進(jìn)行輪廓跟蹤，利用輪廓起始點(diǎn)坐標(biāo)和相應(yīng)的方向鏈碼值連接輪廓邊界，不同的輪廓通過結(jié)束標(biāo)志區(qū)分，從而得到由鏈碼表示的輪廓序列，保存圖像中目標(biāo)的輪廓信息。如圖2所示的2個(gè)矩形輪廓，可以看出，采用的輪廓提取方法能夠準(zhǔn)確地提取目標(biāo)的輪廓，較好地保留了目標(biāo)的特征信息。

(a)目標(biāo)物體和目標(biāo)區(qū)域輪廓

(b)原始圖像的輪廓信息

進(jìn)一步，通過比較2個(gè)矩形輪廓的周長以及輪廓面積，便可識(shí)別矩形目標(biāo)物體和目標(biāo)區(qū)域，即周長/面積小的矩形輪廓為待抓取的目標(biāo)物體，大矩形輪廓為待放置區(qū)域。

2.2 矩形目標(biāo)矢量參數(shù)提取

矩形目標(biāo)物體和目標(biāo)區(qū)域的矢量信息決定了機(jī)械手臂末端夾持器抓取和放置動(dòng)作的方向。本文利用目標(biāo)的質(zhì)心坐標(biāo)和方向來計(jì)算目標(biāo)的空間三維坐標(biāo)和方向，得到圖像中目標(biāo)的矢量參數(shù)。

圖像中第k個(gè)目標(biāo)的質(zhì)心坐標(biāo)定義為

(1)

式中：m00為圖像中目標(biāo)的零階圖像矩；m10和m01為一階圖像矩。

(i+j)階圖像矩定義為

(2)

式中：i、j為非負(fù)整數(shù)；f(r,c)為圖像中(r,c)處的灰度值，即當(dāng)位于目標(biāo)上時(shí)值為1，否則為0。

圖像中第k個(gè)目標(biāo)的方向角度θ定義為穿過該目標(biāo)的長軸與x軸的夾角，即

(3)

式中：C11(k)、C20(k)和C02(k)分別為第k個(gè)目標(biāo)的圖像中心矩，定義為

(4)

分別將目標(biāo)物體和目標(biāo)位置輪廓坐標(biāo)值帶入式(1)和式(2)，即可求得其質(zhì)心坐標(biāo)和方向，如圖3所示。

圖3 矩形目標(biāo)的質(zhì)心和方向矢量參數(shù)

2.3 矩形目標(biāo)空間位姿測量

采用立體匹配方法求取雙目視覺相機(jī)采集的圖像對中目標(biāo)的視差，結(jié)合標(biāo)定的雙目視覺相機(jī)內(nèi)外參數(shù)和三角測距原理，計(jì)算得到目標(biāo)的空間三維坐標(biāo)，實(shí)現(xiàn)目標(biāo)的定位。立體匹配方法的原理是首先利用質(zhì)心作為匹配點(diǎn)得到目標(biāo)視差的估計(jì)值，對該值加減一定的視差余量來設(shè)定搜索范圍。然后利用區(qū)域匹配，在右目圖像中搜索與左目圖像中目標(biāo)質(zhì)心相似度最高的匹配點(diǎn)從而得到視差[9]。

灰度圖局部窗口之間灰度值的相似度使用去均值的歸一化互相關(guān)(ZNCC)系數(shù)r表示，即,

(5)

則視差搜索范圍表示為

r(x,y)=max({r(x,y,p)|(pc-dd

(6)

式中：pc為質(zhì)心匹配時(shí)得到的目標(biāo)視差值；ud、dd為對應(yīng)視差值的上偏移量和下偏移量。

結(jié)合雙目成像系統(tǒng)中的三角測距原理，以左目相機(jī)的光心作為坐標(biāo)系的原點(diǎn)，計(jì)算得到目標(biāo)在空間中的三維坐標(biāo)，即

(7)

式中：Tx為雙目視覺相機(jī)的基線長度，mm；(xl,yl)和(cx,cy)分別為左目相機(jī)圖像坐標(biāo)系的形心坐標(biāo)和校正后的主點(diǎn)坐標(biāo);f為左目相機(jī)焦距，mm；xr為右目相機(jī)在圖像坐標(biāo)系的橫坐標(biāo)。

3 4-DOF機(jī)械手臂運(yùn)動(dòng)控制

基于目標(biāo)的三維位置信息，對機(jī)械手臂進(jìn)行運(yùn)動(dòng)學(xué)分析獲得機(jī)械手臂各關(guān)節(jié)的變量，將期望位置坐標(biāo)轉(zhuǎn)換為各自由度上的旋轉(zhuǎn)或伸縮位移，控制機(jī)械手臂的末端夾持器進(jìn)行抓取和放置操作，完成控制任務(wù)。

4-DOF機(jī)械手臂如圖4(a)所示，各自由度上的運(yùn)動(dòng)分別為：腰部機(jī)械臂的水平旋轉(zhuǎn)運(yùn)動(dòng)；大臂沿水平導(dǎo)軌的直線伸縮運(yùn)動(dòng)；小臂垂直腰部平面的旋轉(zhuǎn)運(yùn)動(dòng)；小臂末端的機(jī)械夾持器拾取。機(jī)械臂由5個(gè)關(guān)節(jié)組成，其中關(guān)節(jié)3和關(guān)節(jié)4共用一個(gè)步進(jìn)電機(jī)，關(guān)節(jié)1、關(guān)節(jié)3、關(guān)節(jié)4和關(guān)節(jié)5為旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)2為移動(dòng)關(guān)節(jié)。選取機(jī)械手臂的基座中心作為空間坐標(biāo)系的原點(diǎn)，對機(jī)械手臂進(jìn)行坐標(biāo)系建模，如圖4(b)所示。其中X0-Z0表示機(jī)械臂基座的坐標(biāo)系(基坐標(biāo)系)，即關(guān)節(jié)1的坐標(biāo)系，依次建立關(guān)節(jié)2～4的坐標(biāo)系，X5-Z5表示機(jī)械臂末端夾持器的坐標(biāo)系，也是夾持器的實(shí)際位姿。

(a)4-DOF機(jī)械手臂結(jié)構(gòu)

(b)4-DOF機(jī)械手臂關(guān)節(jié)坐標(biāo)系圖4 4-DOF機(jī)械手臂結(jié)構(gòu)及關(guān)節(jié)坐標(biāo)系

利用D-H法對機(jī)械臂進(jìn)行分析建模[10]，根據(jù)機(jī)械手臂的結(jié)構(gòu)參數(shù)，建立其運(yùn)動(dòng)學(xué)模型如下：

x=[18.7+23.4cos(88.723 99-α)+λ]sinβ

(8)

y=8.7+23.4sin(88.723 99-α)

(9)

z=[18.7+23.4cos(88.723 99-α)+λ]cosβ

(10)

式中：(x,y,z)為機(jī)械臂夾持器的空間坐標(biāo)；α、β和γ分別為小臂旋轉(zhuǎn)角度、腰部旋轉(zhuǎn)角度和大臂伸縮距離。

通過該正運(yùn)動(dòng)學(xué)模型和機(jī)械手臂各個(gè)關(guān)節(jié)的角度，計(jì)算可得機(jī)械手臂末端夾持器在機(jī)械手臂坐標(biāo)系中的位置。反之，通過該運(yùn)動(dòng)學(xué)模型的逆模型可求解給定期望位置條件下各個(gè)關(guān)節(jié)的旋轉(zhuǎn)角度和移動(dòng)位移，完成4-DOF機(jī)械手臂運(yùn)動(dòng)路徑的規(guī)劃。

由式(8)～式(10)，4-DOF機(jī)械手臂逆運(yùn)動(dòng)學(xué)模型描述為：

(11)

(12)

(13)

4 實(shí)驗(yàn)驗(yàn)證

本文采用USB3.0通信協(xié)議雙目視覺相機(jī)采集彩色未壓縮的雙目720P圖像，速率31幀/s。雙目視覺相機(jī)的基線距離為6 cm，分辨率為1 280×480，工作距離為48 cm，鏡頭焦距為4.3 mm。分別使用張正友平面標(biāo)定法[11]和Brown算法[12]標(biāo)定雙目視覺相機(jī)的內(nèi)參數(shù)矩陣、外參數(shù)矩陣和畸變參數(shù)向量，標(biāo)定值如表1所示。

表1 雙目視覺相機(jī)標(biāo)定參數(shù)

4-DOF機(jī)械手臂DSP控制系統(tǒng)實(shí)驗(yàn)平臺(tái)如文獻(xiàn)[13]所述，關(guān)節(jié)的旋轉(zhuǎn)或伸縮動(dòng)作通過各自由度上的步進(jìn)電機(jī)驅(qū)動(dòng)，旋轉(zhuǎn)角度和伸縮距離與電機(jī)脈沖個(gè)數(shù)nx、ny、nd的關(guān)系為：

α=0.002 25nx+30

(14)

β=0.002 25ny

(15)

λ=0.003 175nd

(16)

末端夾持器初始坐標(biāo)為(0,30.85,28.7)cm，即各自由度上的初始位移α=30°，β=0°，γ=0 cm。圖5為機(jī)械手臂和雙目視覺相機(jī)的工作空間中任意放置的矩形目標(biāo)物體和目標(biāo)區(qū)域，4-DOF機(jī)械手臂末端夾持器按照矩形長邊方向抓取目標(biāo)物體并按與目標(biāo)區(qū)域長邊對齊的方式放置的工作過程。其中的22組目標(biāo)位置測試實(shí)驗(yàn)數(shù)據(jù)如表2所示。

圖5 機(jī)械手臂目標(biāo)抓取與放置過程

編號(hào)目標(biāo)物體坐標(biāo)和方向視覺定位坐標(biāo)和方向夾持器坐標(biāo)和方向X/mmY/mmθ/(°)X/mmY/mmθ/(°)X/mmY/mmθ/(°)150-604548.7-59.94547.7-60.644.82-100-30120-100.4-29.2121.8-101.2-30.5122.33-50-30110-50.2-29.9111.0-48.9-30.9122.440-30800.1-30.480.51.9-29.881.3550-306050.3-30.459.750.3-27.561.66100-302099.6-32.421.8101.5-49.922.57-1000150-99.3-0.1149.6-99.22.2151.48-500120-48.4-0.9121.6-48.51.0122.1900900.00.090.00.20.592.5105006050.3-0.859.951.22.059.51110003098.2-2.429.797.21.631.512-10030160-100.131.1160.0-101.4-28.062.113-503080-49.429.979.4-49.930.881.214030600.828.759.949.628.659.51550304050.327.941.450.131.440.716100302099.827.421.899.832.221.517-10090110-100.192.7110.6-100.493.580.618-509070-48.790.269.0-48.391.971.619090500.888.650.51.391.550.52050903049.587.831.051.491.330.8211009020101.888.620.0101.390.619.9220120400.8119.741.51.1121.041.5

目標(biāo)物體大小為52.0 mm×14.0 mm，目標(biāo)區(qū)域大小為114.0 mm×53.0 mm，實(shí)驗(yàn)過程目標(biāo)物體高度已知，故數(shù)據(jù)中的坐標(biāo)只包含X、Y軸坐標(biāo)。每次實(shí)驗(yàn)記錄3類數(shù)據(jù)，即矩形目標(biāo)物體實(shí)際的坐標(biāo)和方向、視覺系統(tǒng)的定位坐標(biāo)和方向及機(jī)械手臂末端夾持器到達(dá)目標(biāo)物體時(shí)的實(shí)際坐標(biāo)和方向。從表2可以看出，視覺系統(tǒng)的定位誤差在3 mm以內(nèi)，方向誤差小于2°；機(jī)械手臂夾持器的抓取誤差小于4 mm，方向誤差最大值小于3°，具有較高的定位精度和控制精度，達(dá)到了機(jī)械臂控制系統(tǒng)的設(shè)計(jì)要求。

5 結(jié)論

本文針對生產(chǎn)線上工作的工業(yè)機(jī)器人生產(chǎn)柔性差，無法適應(yīng)工作環(huán)境變化的情況，以4-DOF機(jī)械手臂為研究對象，設(shè)計(jì)了機(jī)器視覺輔助定位的運(yùn)動(dòng)控制系統(tǒng)，利用視覺系統(tǒng)定位矩形目標(biāo)，實(shí)現(xiàn)目標(biāo)的抓取與放置操作。設(shè)計(jì)的機(jī)械臂控制系統(tǒng)工作穩(wěn)定，具有較高的定位和控制精度，對其他的工業(yè)機(jī)器人目標(biāo)定位與抓取作業(yè)研究有一定借鑒意義。