雙目免標(biāo)定視覺(jué)伺服系統(tǒng)

劉楚天，陳曉瀚，胡嘉成

(廣東工業(yè)大學(xué) 機(jī)電工程學(xué)院，廣東 廣州 510006)

0 引言

視覺(jué)傳感器搭配機(jī)械臂進(jìn)行作業(yè)在整個(gè)工業(yè)生產(chǎn)中是很常見(jiàn)的。傳統(tǒng)的基于視覺(jué)傳感器的控制方式多是基于位置的視覺(jué)伺服方式,這種方式一般都需要對(duì)系統(tǒng)進(jìn)行標(biāo)定,需要標(biāo)定的部分包括相機(jī)內(nèi)部參數(shù)、機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)以及相機(jī)與機(jī)器人之間的手眼關(guān)系。通常這種方式要求工作環(huán)境與標(biāo)定環(huán)境一致,但在有毒、高溫、水下等環(huán)境中都沒(méi)有辦法進(jìn)行標(biāo)定操作。標(biāo)定的過(guò)程是非常繁瑣的,對(duì)操作人員有一定要求,且標(biāo)定過(guò)的系統(tǒng)結(jié)構(gòu)發(fā)生了變化其參數(shù)也會(huì)發(fā)生變化,需要重新進(jìn)行標(biāo)定。為了解決這些問(wèn)題,免標(biāo)定視覺(jué)伺服的方式被提出,例如使用神經(jīng)網(wǎng)絡(luò)去擬合手眼關(guān)系、基于卡爾曼濾波估計(jì)圖像雅克比矩陣、基于自抗擾控制器(Auto Disturbance Rejection Controller)的免標(biāo)定視覺(jué)伺服控制等[1]。

所謂的免標(biāo)定視覺(jué)伺服是指在不進(jìn)行相機(jī)以及手眼標(biāo)定的情況下,利用視覺(jué)系統(tǒng)控制機(jī)器人完成目標(biāo)任務(wù)的方法。基于圖像的視覺(jué)伺服常用于免標(biāo)定的視覺(jué)伺服控制中[2,3]。基于圖像的視覺(jué)伺服是需要圖像特征的,通常控制六軸機(jī)器人需要四個(gè)特征點(diǎn),但在某些情況下目標(biāo)可能并沒(méi)有豐富的圖像特征,如軌跡跟蹤[4]。因此,需要一種在少特征的情況下也能進(jìn)行免標(biāo)定視覺(jué)伺服的方法。本文提出了一種雙目免標(biāo)定視覺(jué)伺服系統(tǒng),并從系統(tǒng)的軟硬件設(shè)計(jì)、系統(tǒng)圖像雅克比矩陣估計(jì)算法、控制器算法三方面展開(kāi)論述,最后通過(guò)定位實(shí)驗(yàn)說(shuō)明本系統(tǒng)的可行性和有效性。

1 雙目免標(biāo)定視覺(jué)伺服系統(tǒng)總體結(jié)構(gòu)設(shè)計(jì)

雙目免標(biāo)定視覺(jué)伺服系統(tǒng)主要由三部分組成,分別是機(jī)器人執(zhí)行部分、視覺(jué)傳感部分以及控制器部分。整個(gè)系統(tǒng)的硬件部分包括視覺(jué)傳感器、六軸機(jī)械臂以及上位機(jī)。雙目免標(biāo)定視覺(jué)伺服系統(tǒng)結(jié)構(gòu)框圖如圖1所示。

圖1 雙目免標(biāo)定視覺(jué)伺服系統(tǒng)結(jié)構(gòu)框圖

六軸機(jī)械臂作為系統(tǒng)的執(zhí)行機(jī)構(gòu),實(shí)時(shí)地接收上位機(jī)發(fā)送的控制指令,在接收指令之后其內(nèi)部關(guān)節(jié)速度控制器控制各關(guān)節(jié)達(dá)到指令設(shè)定值。視覺(jué)傳感器利用其自身可以通過(guò)光學(xué)元件感知外界信息的特點(diǎn)為系統(tǒng)提供豐富的信息。上位機(jī)是整個(gè)系統(tǒng)的大腦,系統(tǒng)的圖像雅克比矩陣估計(jì)算法、圖像特征提取算法、機(jī)器人的控制算法都是在其中進(jìn)行的。上位機(jī)與機(jī)器人之間的硬件連結(jié)是由雙絞線(xiàn)完成的,軟件上的通信是由socket實(shí)現(xiàn)的,視覺(jué)傳感器和電腦的信息傳輸是通過(guò)USB數(shù)據(jù)線(xiàn)完成的,各個(gè)算法以及模塊之間的信息交換也都是在上位機(jī)上完成的。

為了使各個(gè)模塊之間相互獨(dú)立,本系統(tǒng)采用多線(xiàn)程的方式,以降低圖像處理時(shí)耗長(zhǎng)對(duì)整個(gè)系統(tǒng)的不利影響。圖像處理模塊實(shí)時(shí)地對(duì)視覺(jué)傳感器獲得的圖片進(jìn)行圖像處理,包括濾波、二值化、邊緣檢測(cè)、目標(biāo)提取等,并將獲得的信息構(gòu)造成圖像的特征向量發(fā)送給另外兩個(gè)模塊。圖像雅克比矩陣估計(jì)模塊會(huì)實(shí)時(shí)地接收?qǐng)D像處理模塊發(fā)送的圖像特征向量信息以及機(jī)器人發(fā)送的關(guān)節(jié)角信息,通過(guò)算法結(jié)合上述的信息估算出系統(tǒng)下一步的圖像雅克比矩陣并發(fā)送給控制模塊。控制模塊實(shí)時(shí)接收上面兩個(gè)模塊發(fā)送的信息,并計(jì)算出機(jī)械臂關(guān)節(jié)的運(yùn)動(dòng)速度,實(shí)時(shí)調(diào)整機(jī)械臂的運(yùn)動(dòng)控制機(jī)器人完成定位任務(wù)。

2 系統(tǒng)主要算法設(shè)計(jì)

2.1 系統(tǒng)圖像雅克比矩陣估計(jì)

(1)

(2)

其中:s(t)和s(d)分別為當(dāng)前圖像特征和期望圖像特征。

在絕大多數(shù)機(jī)器人系統(tǒng)中,控制兩輸入的都是關(guān)節(jié)運(yùn)動(dòng)速度。對(duì)于這樣的系統(tǒng),我們可以建立如下微分映射關(guān)系,即:

(3)

(4)

其中:J=JpJr為系統(tǒng)的圖像雅克比矩陣。

圖像雅克比矩陣實(shí)質(zhì)上是對(duì)非線(xiàn)性的手眼映射關(guān)系在某一時(shí)刻線(xiàn)性逼近,這就需要對(duì)每一時(shí)刻的圖像雅克比矩陣進(jìn)行在線(xiàn)估計(jì)。本文選擇線(xiàn)性離散卡爾曼濾波算法對(duì)系統(tǒng)的圖像雅克比矩陣進(jìn)行實(shí)時(shí)在線(xiàn)估計(jì)。

將系統(tǒng)的圖像雅克比矩陣的各個(gè)元素看成是某一線(xiàn)性離散系統(tǒng)的狀態(tài)變量,定義系統(tǒng)的狀態(tài)變量為:

(5)

根據(jù)卡爾曼濾波算法基本理論和濾波公式,首先對(duì)系統(tǒng)的狀態(tài)進(jìn)行先驗(yàn)估計(jì):

(6)

計(jì)算先驗(yàn)誤差協(xié)方差矩陣:

(7)

獲取卡爾曼增益:

(8)

其中:K(k)為k時(shí)刻的最優(yōu)卡爾曼增益;C(k)為k時(shí)刻的測(cè)量矩陣;Rv為測(cè)量噪聲方差陣,一般是常量,取Rv=0.5I8。

矯正狀態(tài)估計(jì)誤差方差陣:

(9)

獲取系統(tǒng)狀態(tài)的最優(yōu)估計(jì):

(10)

x(0)是初始圖像雅克比矩陣所轉(zhuǎn)換的初始向量。初始圖像雅克比矩陣獲取方法是在機(jī)器人的初始位置任意給定l步線(xiàn)性無(wú)關(guān)的試探運(yùn)動(dòng)(l即為機(jī)器人的自由度,本文為6),得到相鄰兩次的機(jī)器人的關(guān)節(jié)角變化,Δp1,…,Δpl,并在每次運(yùn)動(dòng)時(shí)觀察相應(yīng)的軌跡起始端的像素坐標(biāo)變化Δf1,…,Δfl,從而獲取初始的圖像雅克比矩陣估計(jì)值J(0),即:

(11)

2.2 視覺(jué)伺服控制器

定義系統(tǒng)誤差為圖像誤差e(t)=s(d)-s(t)。在跟蹤過(guò)程中,為了加速系統(tǒng)響應(yīng),引入了比例控制策略:

va=c1J+e(t).

(12)

其中:va為機(jī)器人的關(guān)節(jié)角速度;c1為比例控制系數(shù);J+為J的偽逆矩陣。

通過(guò)李雅普諾夫函數(shù)來(lái)驗(yàn)證系統(tǒng)的穩(wěn)定性[5]:

(13)

考慮到系統(tǒng)的圖像雅克比矩陣模型式(4)和圖像誤差e(t),則式(13)的導(dǎo)數(shù)為:

(14)

由李雅普諾夫穩(wěn)定性理論可知:當(dāng)JJ+正定時(shí),閉環(huán)控制系統(tǒng)逐漸趨于穩(wěn)定;當(dāng)JJ+半正定時(shí),該系統(tǒng)無(wú)法通過(guò)李雅普諾夫函數(shù)判斷是否是穩(wěn)定的。

在實(shí)際的系統(tǒng)中,可以通過(guò)合理地選擇圖像特征以及限制機(jī)械臂的關(guān)節(jié)運(yùn)動(dòng)范圍來(lái)使JJ+保持正定,從而避免系統(tǒng)的不收斂。基于圖像的免標(biāo)定視覺(jué)伺服系統(tǒng)控制原理框圖如圖2所示。

圖2 基于圖像的免標(biāo)定視覺(jué)伺服系統(tǒng)控制原理框圖

3 實(shí)驗(yàn)與結(jié)果

3.1 實(shí)驗(yàn)平臺(tái)與實(shí)驗(yàn)任務(wù)

本文通過(guò)機(jī)械臂末端定位實(shí)驗(yàn)對(duì)所提出的雙目免標(biāo)定視覺(jué)伺服系統(tǒng)的可行性進(jìn)行了驗(yàn)證,搭建的系統(tǒng)實(shí)物圖如圖3所示。

圖3 搭建的系統(tǒng)實(shí)物圖

本系統(tǒng)選擇六自由度協(xié)作型操作臂作為執(zhí)行模塊,其控制器通過(guò)雙絞線(xiàn)與上位機(jī)相連,并實(shí)時(shí)地將機(jī)器人的關(guān)節(jié)角信息發(fā)送給上位機(jī)。將兩臺(tái)工業(yè)相機(jī)安裝在操作臂末端使其固連,可以保證兩臺(tái)相機(jī)與機(jī)器人末端的速度是一致的。這兩臺(tái)相機(jī)實(shí)時(shí)地獲取目標(biāo)點(diǎn)的圖片并通過(guò)USB數(shù)據(jù)線(xiàn)發(fā)送給上位機(jī)。上位機(jī)接收到圖片信息以及其機(jī)器人關(guān)節(jié)角信息后會(huì)調(diào)用圖像處理算法、系統(tǒng)的圖像雅克比矩陣估計(jì)算法、控制算法并將處理后得到的信息發(fā)送給機(jī)器人控制器驅(qū)動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)。本實(shí)驗(yàn)的圖像目標(biāo)是兩個(gè)點(diǎn),用以驗(yàn)證少目標(biāo)點(diǎn)的情況下本系統(tǒng)可以完成機(jī)器人的視覺(jué)免標(biāo)定定位實(shí)驗(yàn)。整個(gè)實(shí)驗(yàn)都是在沒(méi)有進(jìn)行相機(jī)、手眼以及機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)標(biāo)定的情況下進(jìn)行的。

本實(shí)驗(yàn)的目標(biāo)是將機(jī)器人從初始位置移動(dòng)到期望位置,即將圖像中的目標(biāo)點(diǎn)從初始位置移動(dòng)到期望位置。初始的圖像特征向量為:

s=[u11,v11,u12,v12,u21,v21,u22,v22]T=

[450,320,504,312,186,352,281,345]T.

(15)

其中:(u11,v11)為相機(jī)1中第1個(gè)特征點(diǎn)初始位置的像素坐標(biāo);(u12,v12)為相機(jī)1中第2個(gè)特征點(diǎn)初始位置的像素坐標(biāo);(u21,v21)為相機(jī)2中第1個(gè)特征點(diǎn)初始位置的像素坐標(biāo);(u22,v22)為相機(jī)2中第2個(gè)特征點(diǎn)初始位置的像素坐標(biāo)。

期望的圖像特征向量為:

[400,212,451,205,137,160,241,151]T.

(16)

本系統(tǒng)所選用的噪聲值是通過(guò)多次實(shí)驗(yàn)效果確定的,上文已介紹,這里不過(guò)多贅述。

3.2 實(shí)驗(yàn)結(jié)果

經(jīng)過(guò)多次重復(fù)實(shí)驗(yàn),可以證明本系統(tǒng)可以達(dá)到預(yù)計(jì)目標(biāo)。其中一次的實(shí)驗(yàn)初始圖像、期望圖像以及經(jīng)過(guò)實(shí)驗(yàn)后的最終圖像如圖4所示,可以看出實(shí)驗(yàn)后的圖像與期望圖像是很相近的。在定位過(guò)程中各圖像中圖像特征點(diǎn)的圖像誤差變化過(guò)程如圖5所示。由圖5可以看出:每個(gè)點(diǎn)都是從一開(kāi)始就朝著縮小圖像誤差的方向運(yùn)動(dòng),直到最后收斂到0附近。

圖4 實(shí)驗(yàn)前、后的圖像

圖5 兩個(gè)相機(jī)中的兩個(gè)圖像特征點(diǎn)的誤差變化過(guò)程

4 結(jié)語(yǔ)

本文提出了一種雙目免標(biāo)定視覺(jué)伺服系統(tǒng),并介紹了系統(tǒng)實(shí)現(xiàn)的理論原理:本系統(tǒng)使用的是基于圖像的視覺(jué)伺服方法,采用圖像雅克比矩陣的方法代替?zhèn)鹘y(tǒng)的手模型,通過(guò)卡爾曼濾波對(duì)圖像雅克比矩陣進(jìn)行估計(jì)并更新,最后通過(guò)比例控制法控制機(jī)器人到達(dá)目標(biāo)位置。定位實(shí)驗(yàn)驗(yàn)證了本系統(tǒng)的可行性。