視覺伺服機械臂手機抓取最佳位姿檢測①

田軍委,閆明濤,丁良華,張 震,張磊蒙,郝陽波

1(西安工業(yè)大學(xué) 機電工程學(xué)院,西安 710021)

2(內(nèi)蒙古北方重工業(yè)集團有限公司,包頭 014033)

機器人在對目標物體進行自動抓取之前,首先應(yīng)確定被抓物體的位姿,即確定合適的抓取位置及抓取姿態(tài),而機器視覺檢測是最常用的目標位姿檢測方法.典型的機器視覺位姿提取方法主要包括單目視覺和雙目視覺[1]兩大類.單目視覺操作方便,處理數(shù)據(jù)信息速度快,但缺乏深度信息,難以完成測量深度的工作;雙目視覺利用空間同一點在兩個攝像機上成像點的匹配,可以進行深度測量,并可以對簡單環(huán)境中的形狀簡單的物體定位定姿,它的缺點是計算過程涉及大量的圖像匹配和目標識別算法,只能針對具有簡單幾何關(guān)系的目標使用,無法在復(fù)雜背景下應(yīng)用.本文研究是基于二維平面坐標系下進行位姿檢測,因此使用單目視覺更為簡單方便.

單目視覺采集完圖像,圖像需預(yù)處理,在圖像匹配研究成果中,賈迪等[2]對從局部不變特征點、直線、區(qū)域匹配3 個方面對圖像匹配方法予以綜述,其中局部不變特征點是圖像匹配中發(fā)展最早也比較成熟的圖像方法,以SIFT 的改進算法SURF 不變特征點在圖像匹配中更為常見,圖像匹配效果相對較好;林志東等[3]對特定條件下SIFT與SURF 圖像匹配算法的性能進行了對比實驗,特征點提取方面,SURF 算法運算速度更快,而SIFT 算法能提取出更多的特征點;特征匹配方面,SIFT 算法在圖像存在光照差異時具有更大的優(yōu)勢,SURF 算法在處理有視角差異的圖像時穩(wěn)定性更好.在位姿測量中,徐昌源等[4]提出了改進最小二乘法的雙目視覺位姿測量技術(shù)研究與測試的方法,該測量方法雖效果佳,但僅適用于雙目視覺位姿測量的數(shù)學(xué)模型中;谷鳳偉等[1]設(shè)計出一種簡易的單目視覺位姿測量方法,但需要將一種精確的靶標檢測算法與單目視覺測量方法相結(jié)合,才能實現(xiàn)機器人位姿的測量,由于該精確的靶標也需要圖像處理和檢測,在計算過程中難免會造成誤差積累.在視覺伺服過程中,徐德[5]對單目視覺在視覺伺服研究中進行了綜述,分析了不同視覺伺服方法的特點及典型應(yīng)用,認為混合視覺伺服(基于位置的視覺伺服和基于圖像的視覺伺服)在未來有很大的發(fā)展前景,但目前,基于圖像的視覺伺服仍是當(dāng)前研究的熱點;陳梅等[6]針對無標定視覺伺服系統(tǒng)eye-in-hand 型結(jié)構(gòu),提出一種以圖像特征為反饋的模糊控制策略,對平面內(nèi)運動物體進行跟蹤,該eye-inhand 型結(jié)構(gòu)為視覺伺服奠定了一定的基礎(chǔ),伺服效果相對較好,但其反饋的模糊控制系統(tǒng)相對比較復(fù)雜.

針對以上文獻中的圖像處理算法與視覺伺服問題,本文提出一種相機在機械臂上eye-in-hand 型視覺伺服抓取手機策略,選取SURF 不變特征點匹配算法進行目標檢測,以及對二維坐標系下目標圖像進行最佳位姿解算,為伺服機械臂抓取手機提供準確性.

1 視覺伺服手機抓取策略

針對特殊環(huán)境手機拾取問題,本文采用一種基于視覺伺服的機械臂方案,即采用相機搜索手機,然后引導(dǎo)機械臂進行抓取.機械臂與相機位置一般有兩種關(guān)系:一種相機在機械臂上eye-in-hand,這種可操作性高并且可以獲取較高的圖像數(shù)據(jù);一種是相機在機械臂外eye-to-hand,其優(yōu)點是相機獲取的視野廣、靈活性強,但機械臂易受相機的位置影響.本文采用相機在機械臂上eye-in-hand,經(jīng)過相機與機械臂末端執(zhí)行器的標定之后可以獲得固定的變換關(guān)系,O1O2O3O4分別表示機械臂坐標系、機械手抓坐標系、相機坐標系、手機坐標系的原點.該方案原理如圖1所示.

圖1 機械臂抓取手機系統(tǒng)框圖

用機械臂抓取手機方法步驟為:第1 步:利用標定好的相機對掉進溝縫中的手機進行采樣;第2 步:將采樣到的圖像進行圖像預(yù)處理,并進行目標檢測;第3 步:將處理好的圖像,即采集到手機的目標狀態(tài),進行手機位姿檢測;第4 步:將檢測到手機的最佳位姿進行解算,并伺服控制機械臂對手機進行抓取.在抓取過程中,抓取的準確性、安全性、適應(yīng)性主要依賴于手機目標檢測和位姿解算,使機械臂及手抓按最佳方案進行抓取.

2 位姿檢測方法

位姿檢測是伺服機械臂抓取手機的關(guān)鍵,位姿檢測前對相機標定是采集圖像的必要條件,是減小誤差,提高相機匹配速度的重要步驟.位姿檢測方法包括SURF不變特征點匹配進行目標檢測,以及二維坐標系下的最佳位姿解算.

2.1 相機成像原理及相機標定

相機成像模型主要由世界坐標系Ow-XwYwZw,相機坐標系Oc-XcYcZc、像素坐標系Op-u v、圖像坐標系Oi-XiYi構(gòu)成,P是相機坐標系中的一點,其坐標是(Xc,Yc,Zc),P′是在圖像中的成像點,在圖像坐標系的坐標是(x,y),在像素坐標系是(u,v).成像原理模型如圖2所示.

圖2 攝像機成像模型

相機標定是為了確定相機坐標系、圖像坐標系、像素坐標系與真實坐標系之間的關(guān)系,具體而言即由相機的成像原理創(chuàng)建數(shù)學(xué)模型,借助已知特征的像素坐標和對應(yīng)的世界坐標推導(dǎo)相機的模型參數(shù)[7].通過張正友對相機的內(nèi)外參數(shù)進行標定.張正友標定流程,如圖3所示.

圖3 張正友標記流程

利用相機標定結(jié)果和單目攝像頭成像原理完成相機成像過程的坐標系變換.確定物體相對于相機坐標系的位姿,進而確定相對于機械臂的位姿,相機標定結(jié)果可以求出相機的內(nèi)外參數(shù)矩陣,相機的內(nèi)參數(shù)矩陣是唯一,而外參數(shù)矩陣不唯一[1].內(nèi)參數(shù)矩陣對位姿解算在視覺伺服機械臂具有重要的意義.

2.2 SURF 不變特征點匹配算法

SURF 算法[3]在提取特征點時,主要包含以下6 個步驟.

步驟1.建立積分圖像.通過簡單的加減法去計算任一區(qū)域的像素之和.如圖4所示,矩形ABCD內(nèi)所有像素的灰度值之和計算公式為:

圖4 利用積分圖快速計算

步驟2.建立Hessian 矩陣.一個像素點(x,y),設(shè)函數(shù)f(x,y)的Hessian 矩陣定義為:

可以看出H矩陣由函數(shù)f的二階偏導(dǎo)數(shù)構(gòu)成,每一個像素點都可以解算出一個H矩陣.式中,Lxx(x,σ)為圖像f與尺度為σ 的高斯二階導(dǎo)數(shù)的卷積其定義如下:

對于Lxy(x,σ)和Lyy(x,σ),定義相似.

步驟3.建立尺度空間.SUFR 特征極值點的檢測算法和提取是基于尺度空間理論.通過改變使用高斯濾波器的尺度,不是改變圖像本身來構(gòu)成對不同尺度的響應(yīng).

步驟4.提取特征點.利用非極值去除掉確定特征點,在海森行列式圖像的每個像素點與3 維鄰域的26 個點進行大小比較,設(shè)定一個閾值,用3 維線性插值法得到亞像素級的特征點后,去掉特征值小于閾值的點,得出更穩(wěn)定的點.

步驟5.選取特征點的方向.以特征點為中心,檢測興趣點確定尺度s,統(tǒng)計半徑扇形內(nèi)所有點的Haar 小波響應(yīng),并給靠近特征點的像素賦予較大的權(quán)重.統(tǒng)計該區(qū)域所有Harr 小波響應(yīng)之和,形成矢量,即扇形的方向.遍歷所有的扇形區(qū)域,選擇最長矢量方向作為該特征點的方向.

步驟6.構(gòu)造SURF 的特征點描述子.以特征點為中心,構(gòu)造出以20s為邊長的正方形窗口,s為特征點所在的尺度.將其分為16 個4×4 的子區(qū)域,每個子區(qū)域統(tǒng)計包含有25 個像素元的水平x和垂直y的Haar小波特征之和分別為和.統(tǒng)計出每個子區(qū)域的4 維描述子得出特征向量長度為64 維的特征描述子.

2.3 最佳位姿檢測

2.3.1 算法原理

手機的最佳位姿檢測包括SURF 不變特征點匹配的目標檢測和其位姿解算.對機械臂上相機標定完,解出相機的內(nèi)外參數(shù)矩陣,對SURF 不變特征點匹配檢測到手機圖像在二維坐標系下進行最佳位姿檢測與解算,解算原理過程如圖5所示.

圖5 位姿解算原理圖

原理過程如下:在圖像坐標系Oi-XiYi中,設(shè)手機的質(zhì)心坐標為O4(sx,sy),設(shè)機械臂末端手抓中心O2(zx,zy)與相機光軸中心Oi所標定的偏差距離e,標定機械手抓的手指姿態(tài)為PQ,與相機的成像的投影關(guān)系如圖5(a)所示,其中Op-u v是像素坐標系.

伺服機械臂從圖5(a)移動至圖5(b)狀態(tài),使其手抓的中心O2與手機的質(zhì)心O4重合,如圖5(b)所示.手機姿態(tài)的y軸與圖像坐標系Oi-XiYi的Xi軸夾角是α,手機不動,驅(qū)動機械手抓旋轉(zhuǎn) α角度,使其手指PQ與手機姿態(tài)的y軸在投影上重合,即機械手抓旋轉(zhuǎn) α角度后手指PQ的新姿態(tài)為P1Q1,抓取點為A(x1,y1)和B(x2,y2)兩點.旋轉(zhuǎn)角度 α和手指的新姿態(tài)P1Q1為伺服機械臂抓取手機提供了準確性.

2.3.2 算法設(shè)計

位姿解算是將圖像在像素坐標系Op-u v中位置關(guān)系轉(zhuǎn)化為圖像的物理坐標Oi-XiYi,其目的是在圖像的物理坐標系中可以進行幾何關(guān)系的計算,轉(zhuǎn)化公式為矩陣形式(4)[1]:

其中,(u,v)是在水平方向上和垂直方向的像素,設(shè)Oi(u0,v0)是圖像的坐標系中心的像素,dx和dy是每個像素在x軸和y軸的方向上的實際物理尺寸,由相機的標定可以知u0,v0,dx,dy.SURF 不變特征點算法匹配到的手機矩形框CDEF 中利用圖像算法可求出手機的質(zhì)心O4(us,vs)和CDEF 的像素坐標,根據(jù)幾何關(guān)系進而可以求出手機的抓持點A(u1,v1)和B(u2,v2)的像素坐標,其匹配到的手機矩形框,如圖6所示.

圖6 匹配到手機矩形框

將手機的抓持點A(u1,v1)和B(u2,v2) 的像素坐標帶入式(4)推導(dǎo)可得A和B的物理坐標,如式(5)和式(6):

所求出A(x1,y1)和B(x2,y2)的圖像物理坐標,可以解出夾角 α即最佳位姿角,理論上伺服給機械手抓的旋轉(zhuǎn)角度也應(yīng)為α,但由于機械臂的系統(tǒng)誤差使得旋轉(zhuǎn)角度小于或大于 α,故取旋轉(zhuǎn)角度為β.取機械手抓相對手機順時針旋轉(zhuǎn)為正,逆時針旋轉(zhuǎn)為負,利用在坐標系中由兩點之間的幾何關(guān)系可求其斜率,可得式(7):

由式(5)～式(7)可得式(8):

求出式(8)中的角度 α,取值范圍[0,90°],若為正則機械手抓順時針旋轉(zhuǎn),負則逆時針旋轉(zhuǎn).α是手機在二維平面的位姿角也是機械手抓旋轉(zhuǎn)的夾持角,其夾持點為A和B兩點.由式(8)可知,dx,dy是相機內(nèi)參數(shù),故 α只與夾持點A和B兩點的像素點的差值有關(guān),簡化了圖像中復(fù)雜的計算,從而提高視覺伺服效果.

從算法的設(shè)計原理和求解結(jié)果來看,相比傳統(tǒng)的位姿檢測算法,在計算上有較大的改進.如最早由Fischler等于1981年提出典型PNP 問題[8],是其根據(jù)目標點的圖像坐標和相機成像模型解算目標相對于相機的位姿問題,而在其平面PNP 問題的求解過程中會用到非線性求解算法,該算法雖法精度較高,但一般需要迭代,計算量較大,且存在穩(wěn)定性問題.而在后期的研究深入過程中如典型的算法有Oberkampf[9]提出的POSIT 算法;Schweighofer[10]提出的平面目標位姿估計算法(SP);Lepetit 等[11]提出的EPnP 算法,Lu 等[12]提出的正交迭代算法等.這些典型算法在位姿求解過程中都屬于非線性求解,因此會占用圖像解算過程中的資源問題,而本算法的提出可以良好的避免復(fù)雜的迭代問題,減少占用資源問題.

2.4 算法實現(xiàn)步驟

具體過程如下:

(1)讀取照片;

(2)提取SURF 特征點;

(3)根據(jù)特征點生成圖像的特征向量;

(4)初步建立一個匹配對(含野值);

(5)預(yù)測仿射變化,去除不滿足變化的野值;

(6)獲取目標物的多邊形框;

(7)二維坐標系下其最佳位姿解算;

(8)伺服控制機械臂抓取手機.

3 實驗及分析

相機標定和目標檢測實驗選用Matlab 2018b 版軟件,以及Matlab 硬件支持包usbwebcams,操作系統(tǒng)為64 位Win 10 系統(tǒng).位姿解算與伺服機械手抓抓取實驗選用Rethink 雙臂機器人的右臂,其操作系統(tǒng)為64 位Linux 系統(tǒng)Ubuntu 16.04,進行抓取驗證.

Rethink 雙臂機器人的右臂進行位姿檢測和手機抓取實驗之前需要先完成以下4 點:(1) 右臂上相機的內(nèi)外參數(shù)的標定;(2) 手抓中心與相機中心的位置校正;(3) 將雙臂機器人的UI 顯示界面窗口大小標定為640×480 像素;(4) 右臂末端初始位置的標定,使其在沿水平面方向移動搜索[13–16].

3.1 相機標定實驗

機械臂上的攝像頭最大分辨率為1280×800 像素的攝像頭,標定板為10×7 的棋盤格,各格邊長28 mm×28 mm,其采集20 張像素尺寸為480×640 圖片,導(dǎo)入到Matlab 工具箱中進行攝像機標定[17,18].

Matlab 工具箱標定實驗中,需對相機標定結(jié)果進行評估,可以通過相機的投影誤差分析、以攝像機為中心和以標定板為中心的三維外部參數(shù)可視化進行分析,標定過程具體如圖7所示.

圖7 Matlab 工具箱標定實驗

在Matlab 工具箱標定實驗后,在其命令行中輸入cameraParams.IntrinsicMatrix,可以求出相機內(nèi)參數(shù)矩陣,輸入RadialDistortion和TangentialDistortion 可以求出相機的徑向畸變和切向畸變系數(shù)[19,20],具體相機的物理參數(shù)如表1所示.

表1 相機標定結(jié)果

3.2 SURF 特征點目標檢測與位姿解算實驗

為了驗證提出算法對伺服機械臂抓取手機的有效性,完成位姿檢測實驗,該實驗包括SURF 不變特征點的目標檢測實驗和位姿解算實驗.同時為使實驗更具說服力,筆者完成單一背景下和復(fù)雜背景(沒有遮擋)下的兩組位姿檢測對比實驗,觀察復(fù)雜背景下是否會影響位姿檢測的算法實驗效果.分別取五組單一背景和五組復(fù)雜背景下的位姿檢測實驗的實驗結(jié)果進行分析對比及評價,實驗結(jié)果分析表,如表2所示.單一背景下位姿檢測實驗過程和復(fù)雜背景下位姿檢測實驗過程,如圖8和圖9所示.

圖8 單一背景下位姿檢測實驗

圖9 復(fù)雜背景下位姿檢測實驗

表2 單一背景下與復(fù)雜背景下的位姿檢測結(jié)果分析

實驗表明,單一背景中的SURF 不變特征點提取的特征點數(shù)相對較多,但易出現(xiàn)交叉點;復(fù)雜背景中的提取的特征點數(shù)雖較少,但出現(xiàn)的交叉點也較少;二者在特征點匹配的正確率、匹配時間上相當(dāng),在匹配效果上單一背景的總體稍好于復(fù)雜背景,但從整體的實驗結(jié)果而言,復(fù)雜背景下該算法可以滿足檢測到手機.無論是在單一背景下還是在復(fù)雜背景下,其SURF 不變特征點匹配實驗的特征點匹配速度和效果都相對準確穩(wěn)定,并且可以匹配到手機在o-xy平面上的任意偏轉(zhuǎn)角度,即手機在二維平面上的位姿,利用圖像算法和提出的位姿解算算法可以求解出兩個理論的機械手指夾持點A和B,進而得到位姿角 α.

通過手機的抓取實驗來驗證解算出的位姿角 α的準確性,其中抓取實驗是以手抓為參考對象,取順、逆時針手機狀態(tài)各6 張,其像素尺寸為640×480 像素,是其在復(fù)雜背景下的二維坐標系的位姿狀態(tài).由相機標定實驗和SURF 特征點匹配實驗可求出相機的內(nèi)參數(shù)u0,v0,dx,dy及機械手抓抓取手機的兩個夾持點A和B,代入式(8)解出最佳位姿角 α,即機械臂末端手抓應(yīng)旋轉(zhuǎn)角度 β.解算出的位姿角 α和機械臂末端手抓旋轉(zhuǎn)的角度 β存在一定的誤差,為驗證該誤差是否會影響手機抓取,列出位姿角與手抓旋轉(zhuǎn)角度誤差分析表,如表3所示.

在表3中,位姿角 α與輸出給機械手抓旋轉(zhuǎn)角度β存在一定的誤差,分析出主要誤差來源是雙臂機器人機械臂的末端手抓中舵機旋轉(zhuǎn)的精度,該舵機的旋轉(zhuǎn)精度只能精確到0.1 位,屬于系統(tǒng)的絕對誤差,其不可避免;經(jīng)過實驗抓取發(fā)現(xiàn)其相對誤差來源于光線、手機表面的紋理等.實驗抓取過程中發(fā)現(xiàn),誤差小于1～2°,不影響手機的抓取.為避免機械臂在抓取中出現(xiàn)碰撞,筆者先在PC 端仿真抓取手機模擬,最后將編譯的代碼下載到雙臂機器系統(tǒng)中.選取的12 組手機抓取實驗,良好的驗證了提出算法的有效性,為視覺抓取提供了準確性.位姿解算與抓取手機實驗,如圖10所示.

表3 位姿角與手抓旋轉(zhuǎn)角度誤差分析表

圖10 位姿解算與手機抓取實驗

4 結(jié)論與展望

本文提出一種二維平面坐標系下利用單目視覺系統(tǒng)測量手機的最佳位姿檢測算法,該算法將單目視覺與伺服機械臂相結(jié)合,通過相機標定,圖像采集、圖像預(yù)處理等過程,實現(xiàn)SURF 不變特征點匹配到目標物體,完成目標檢測.通過攝像機成像原理,計算出從像素坐標系到圖像坐標系的位置變換關(guān)系,利用兩點間求斜率的幾何關(guān)系進而求出目標物體的最佳位姿角,即伺服機械手抓應(yīng)旋轉(zhuǎn)的角度,本算法簡單易行,可有效避免雙目視覺位姿測量過程中圖像匹配資源占用問題.實驗驗證了該算法的有效性和準確性,可以完成手機最佳位姿檢測及機械手抓持點的確定,滿足視覺伺服機械臂抓取要求.同時,該測量算法可以應(yīng)用到工業(yè)零件定位、碼垛機器人搬運等領(lǐng)域,具有廣闊的應(yīng)用前景.