機(jī)器人無標(biāo)定手眼協(xié)調(diào)系統(tǒng)視覺跟蹤

秦偉洋, 岳曉峰, 吳煥新, 張鵬飛

(長春工業(yè)大學(xué) 機(jī)電工程學(xué)院， 吉林 長春 130012)

0 引 言

手眼協(xié)調(diào)控制是機(jī)器人系統(tǒng)重要的控制手段。相比傳統(tǒng)的在標(biāo)定條件下使用的手眼協(xié)調(diào)系統(tǒng)，無標(biāo)定視覺手眼協(xié)調(diào)系統(tǒng)具有更高的靈活性與適應(yīng)性，是機(jī)器人伺服控制系統(tǒng)未來重要的發(fā)展方向和研究熱點(diǎn)[1]。Herve[2]闡述了圖像信息從圖像空間映射變換到機(jī)器人任務(wù)空間的可操作性。Cooperstock J等[3]在手眼關(guān)系無標(biāo)定的情況下，利用神經(jīng)網(wǎng)絡(luò)方法實(shí)現(xiàn)機(jī)器人的控制，進(jìn)行了機(jī)器人的靜態(tài)定位和抓取目標(biāo)物體的實(shí)驗(yàn)。神經(jīng)網(wǎng)絡(luò)方法具有強(qiáng)大的函數(shù)擬合能力，能夠比較準(zhǔn)確地?cái)M合機(jī)器人任務(wù)空間和圖像空間之間的非線性關(guān)系，根據(jù)系統(tǒng)的動(dòng)態(tài)特性建立控制率[4-5]，神經(jīng)網(wǎng)絡(luò)的缺點(diǎn)是必須進(jìn)行大量的樣本學(xué)習(xí)，相對(duì)來說，較為繁瑣和復(fù)雜、計(jì)算量較大。蘇劍波等利用ADRC方法來設(shè)計(jì)控制器[6-7]，其原理是將機(jī)器人手眼協(xié)調(diào)系統(tǒng)近似地看成一個(gè)簡(jiǎn)單的線性系統(tǒng)，將對(duì)該系統(tǒng)有影響的各種因素歸為外部干擾，對(duì)其進(jìn)行在線補(bǔ)償，以此來實(shí)現(xiàn)系統(tǒng)的動(dòng)態(tài)反饋。但這種方法由于系統(tǒng)響應(yīng)初期誤差較大，而且參數(shù)的選取大多是靠累積的經(jīng)驗(yàn)，運(yùn)動(dòng)目標(biāo)跟蹤結(jié)果并不理想。Sutanto等通過研究實(shí)時(shí)估計(jì)圖像雅可比矩陣來實(shí)現(xiàn)無標(biāo)定手眼協(xié)調(diào)的方法[8-9]。

文中研究基于雅可比矩陣的無標(biāo)定手眼協(xié)調(diào)系統(tǒng)視覺跟蹤問題，提出一種改進(jìn)的Kalman濾波算法對(duì)雅可比矩陣進(jìn)行在線估計(jì)，并推導(dǎo)出相應(yīng)的雅可比矩陣在線估計(jì)算法，在Matlab平臺(tái)上進(jìn)行仿真，驗(yàn)證了文中雅可比矩陣在線估計(jì)算法的可行性。

1 雅可比矩陣

假設(shè)f∈Rm為圖像特征參數(shù)向量，r∈Rn為機(jī)械手在機(jī)器人工作空間中的位姿向量，m為圖像特征空間維數(shù)，n為任務(wù)空間維數(shù)，則圖像雅可比矩陣可以描述從機(jī)器人工作空間到圖像特征空間的變換，其表達(dá)式如下：

(1)

其中

(2)

Ji(r)----圖像雅可比矩陣，Ji(r)∈Rm×n。

此模型首先由Weiss[10]提出,它反映了機(jī)器人工作空間和圖像特征空間的映射變換關(guān)系，又被稱為特征敏感度矩陣[11-13]。

(3)

Jr(θ)----機(jī)器人雅可比矩陣,Jr(θ)∈Rn×p。

(4)

(5)

令

J=Ji(r)·Jr(θ)

則可以寫為：

(6)

雅可比矩陣不是定常矩陣[14]，隨著機(jī)器人的位姿改變，機(jī)器人運(yùn)動(dòng)空間和圖像特征空間會(huì)發(fā)生相對(duì)變化，雅可比矩陣也會(huì)隨之變化，因此雅可比矩陣具有時(shí)變的特點(diǎn)。所以，對(duì)雅可比矩陣進(jìn)行實(shí)時(shí)估計(jì)是非常重要的。

2 圖像雅可比矩陣的在線辨識(shí)

構(gòu)造一個(gè)系統(tǒng)，將系統(tǒng)的狀態(tài)設(shè)為待估計(jì)的雅可比矩陣參數(shù)，那么對(duì)系統(tǒng)的觀測(cè)即可得到雅可比矩陣的估計(jì)值。根據(jù)圖像雅可比矩陣的表達(dá)式，將其離散化，則有：

f(k+1)≈f(k)+J(r(k))·Δr(k)

(7)

設(shè)系統(tǒng)的觀測(cè)矢量x是一個(gè)mn×1維矢量，定義如下：

(8)

令圖像雅可比矩陣的觀測(cè)向量x(k)為系統(tǒng)狀態(tài)，由機(jī)械手運(yùn)動(dòng)所引起的圖像特征變化為系統(tǒng)輸出，即y(k)=f(k+1)-f(k),得到如下狀態(tài)方程：

(9)

式中:η(k)，ν(k)----分別為狀態(tài)噪聲和圖像觀察噪聲，假定為高斯白噪聲。

根據(jù)系統(tǒng)特點(diǎn)和工程實(shí)際應(yīng)用，建立簡(jiǎn)化的自適應(yīng)Kalman濾波算法，將基于極大似然估計(jì)準(zhǔn)則的R和Q矩陣的估計(jì)和調(diào)整問題轉(zhuǎn)變成一個(gè)確定新息協(xié)方差矩陣Pv,以及對(duì)應(yīng)的自適應(yīng)調(diào)整參數(shù)αk的問題，其遞推方程為：

狀態(tài)預(yù)測(cè)：

(10)

觀測(cè)噪聲協(xié)方差陣：

(11)

系統(tǒng)噪聲協(xié)方差陣：

(12)

增益矩陣：

(13)

(14)

估計(jì)均方誤差：

Pk=[I-KkHk]Pk,k-1

(15)

新息協(xié)方差矩陣：

(16)

N----平滑窗口的寬度。

狀態(tài)估計(jì)方程：

(17)

(18)

3 視覺控制器設(shè)計(jì)

(19)

式中:c1,c2----分別為PI控制器的比例與積分系數(shù)。

成像系統(tǒng)中，從世界坐標(biāo)系變換到圖像像素坐標(biāo)系過程如下：

1)從世界坐標(biāo)系到相機(jī)坐標(biāo)系

(20)

其中,R為3×3的旋轉(zhuǎn)矩陣，T為3×1的平移矩陣：

2)從相機(jī)坐標(biāo)系變換到圖像物理坐標(biāo)系

(21)

式中:f----攝像機(jī)焦距。

3)從圖像物理坐標(biāo)系變換到圖像像素坐標(biāo)系

(22)

式中:fu,fv----分別為圖像平面上X和Y軸方向上單位距離內(nèi)的像素?cái)?shù);

u0,v0----分別為圖像物理坐標(biāo)系的中心坐標(biāo)。

過程1)中的R，T一般被稱為攝像機(jī)的外部參數(shù)。2)中的方程一般被稱為攝像機(jī)的針孔模型。3)中的fu、fv、u0、v0以及2)中的f則是攝像機(jī)的內(nèi)部參數(shù)。

4 仿 真

在Matlab平臺(tái)上對(duì)手眼協(xié)調(diào)系統(tǒng)運(yùn)動(dòng)目標(biāo)跟蹤過程進(jìn)行仿真，任務(wù)定義為：機(jī)器人末端執(zhí)行器作二維平面運(yùn)動(dòng)，跟蹤在工作平面上運(yùn)動(dòng)的點(diǎn)目標(biāo)。使機(jī)器人末端執(zhí)行期保持始終在目標(biāo)點(diǎn)正上方。仿真參數(shù)設(shè)置如下：目標(biāo)運(yùn)動(dòng)平面為機(jī)器人基坐標(biāo)系的xoy平面，手抓運(yùn)動(dòng)平面高于xoy平面60 mm。手上攝像機(jī)相對(duì)于機(jī)器人末端坐標(biāo)系設(shè)定為

(a) 目標(biāo)與機(jī)器人運(yùn)動(dòng)軌跡

(b) 誤差變化曲線

利用Kalman濾波算法對(duì)圖像雅可比矩陣進(jìn)行估計(jì)，對(duì)其進(jìn)行相同條件的仿真，平面的仿真跟蹤軌跡和跟蹤誤差變化如圖2所示。

(a) 目標(biāo)與機(jī)器人運(yùn)動(dòng)軌跡

(b) 誤差變化曲線

從圖1(a)和圖2(a)可以看出，比較利用Kalman濾波算法和文中提出的算法對(duì)圖像雅可比矩陣進(jìn)行在線辨識(shí)，文中提出的算法魯棒性更好，機(jī)器人跟蹤運(yùn)動(dòng)目標(biāo)的準(zhǔn)確率更高；從圖1(b)和圖2(b)可以看出，利用改進(jìn)的Kalman濾波算法，跟蹤誤差較小且收斂速度較快。

5 結(jié) 語

分析了工業(yè)機(jī)器人手眼協(xié)調(diào)系統(tǒng)的原理，研究了世界坐標(biāo)系、相機(jī)坐標(biāo)系以及圖像坐標(biāo)系之間的變換關(guān)系，分析了圖像雅可比矩陣的定義以及在線辨識(shí)算法，并提出了一種改進(jìn)的Kalman濾波算法對(duì)其進(jìn)行在線辨識(shí)，實(shí)現(xiàn)了機(jī)器人工作空間內(nèi)跟蹤運(yùn)動(dòng)目標(biāo)，仿真結(jié)果驗(yàn)證了文中算法的有效性。

參考文獻(xiàn)：

[1] Papanikolopoulos N P, Nelson B J, Khosla P K. Six degree-of-freedom hand/eye visual tracking with uncertain parameters[J]. Robotics & Automation IEEE Transactions on,1994,11(5):725-732.

[2] Herve J Y, Sharma R, Cucka P. Toward robust vision-based control: hand/eye coordination without calibration[C]// IEEE International Symposium on Intelligent Control. [S.l.]: IEEE,1991:457-462.

[3] Cooperstock J R, Milios E E. Self-supervised learning for docking and target reaching [J]. Robotics & Autonomous Systems,1993,11(3/4):243-260.

[4] 潘且魯,蘇劍波,席裕庚.基于神經(jīng)網(wǎng)絡(luò)的機(jī)器人手眼無標(biāo)定平面視覺跟蹤[J].自動(dòng)化學(xué)報(bào),2001,27(2):194-199.

[5] 謝冬梅,曲道奎,徐方.基于神經(jīng)網(wǎng)絡(luò)的機(jī)器人視覺伺服控制[J].微計(jì)算機(jī)信息,2006,22(4):4-6.

[6] 馬紅雨,蘇劍波.基于耦合ADRC原理的機(jī)器人無標(biāo)定手眼協(xié)調(diào)[J].機(jī)器人,2003,25(1):39-43.

[7] 劉曉麗.基于優(yōu)化ADRC的無標(biāo)定機(jī)器人手眼協(xié)調(diào)[D].西安:西安理工大學(xué),2006.

[8] Sutanto H, Sharma R, Varma V. Image based autodocking without calibration[C]//IEEE International Conference on Robotics and Automation. [s.l.]: Proceedings. IEEE,1997,2:974-979.

[9] 錢江,蘇劍波.基于圖像雅可比矩陣的無標(biāo)定二維運(yùn)動(dòng)跟蹤[J].模式識(shí)別與人工智能,2003,16(3):257-262.

[10] Weiss L E, Sanderson A C, Neuman C P. Dynamic sensor-based control of robots with visual feedback [J]. IEEE Journal on Robotics & Automation,1987,7(5):404-417.

[11] Espiau B, Chaumette F, Rives P. A new approach to visual servoing in robotics[J]. IEEE Transactions on Robotics & Automation,1992,8(3):313-326.

[12] 錢江,蘇劍波.圖像雅可比矩陣的在線Kalman濾波估計(jì)[J].控制與決策,2003,18(1):77-80.

[13] 曾祥進(jìn),黃心漢,王敏.基于Broyden在線圖像雅可比矩陣辨識(shí)的視覺伺服[J].華中科技大學(xué)學(xué)報(bào):自然科學(xué)版,2008,36(9):17-20.

[14] 徐波，王金生，徐洲石.智能巡檢機(jī)器人調(diào)度與路線規(guī)劃能力[J].長春工業(yè)大學(xué)學(xué)報(bào),2016,37(3):269-272.