孫冬雪， 楊宏韜， 劉克平

(長春工業(yè)大學(xué) 電氣與電子工程學(xué)院， 吉林 長春 130012)

自適應(yīng)卡爾曼濾波圖像雅克比估計(jì)

孫冬雪， 楊宏韜， 劉克平*

(長春工業(yè)大學(xué) 電氣與電子工程學(xué)院， 吉林 長春 130012)

基于自適應(yīng)卡爾曼濾波實(shí)現(xiàn)了圖像雅克比矩陣的遞歸估計(jì)，以及過程噪聲和測量噪聲的在線估計(jì)和調(diào)節(jié)。補(bǔ)償了圖像采集、傳輸和處理帶來的時(shí)延，進(jìn)行了仿真驗(yàn)證。

圖像雅克比矩陣； 自適應(yīng)卡爾曼濾波； 時(shí)延補(bǔ)償

0 引 言

圖像雅克比矩陣估計(jì)是視覺伺服領(lǐng)域最重要的研究內(nèi)容之一。在機(jī)器人系統(tǒng)中，圖像空間與機(jī)器人工作空間的非線性微分變化關(guān)系就是利用圖像雅克比矩陣表示的，其關(guān)系如下[1]：

由于機(jī)器人關(guān)節(jié)位置是隨時(shí)間變化的，由此可得圖像雅克比矩陣是時(shí)變的，因此需要對(duì)其在線計(jì)算或估計(jì)。

傳統(tǒng)方法一般利用Broyden估計(jì)器對(duì)圖像雅克比矩陣進(jìn)行估計(jì)[2-4]。其基本原理是利用機(jī)器人每一步的運(yùn)動(dòng)變化求取相應(yīng)的圖像特征誤差變化，從而估計(jì)各個(gè)時(shí)刻的圖像雅克比矩陣的值。此類方法從最初的靜態(tài)Broyden更新法改進(jìn)為動(dòng)態(tài)Broyden更新法，在一定范圍內(nèi)提高了機(jī)器人跟蹤的穩(wěn)定性和收斂速度；后來發(fā)展為帶遺忘因子動(dòng)態(tài)Broyden更新法，此方法在圖像雅克比矩陣的估計(jì)過程中，利用遺忘因子減少了舊數(shù)據(jù)的權(quán)重，從而減小了舊數(shù)據(jù)對(duì)系統(tǒng)穩(wěn)定性的影響。上述方法不僅都需要精確的機(jī)器人及攝像機(jī)參數(shù)，而且也沒有考慮噪聲和時(shí)延對(duì)系統(tǒng)的影響，因此，當(dāng)相機(jī)到達(dá)期望位置后，不可避免地存在較為明顯的震蕩現(xiàn)象。

為了克服Broyden方法存在的問題，近年來,對(duì)圖像雅克比矩陣在線估計(jì)方法的研究也吸引了國內(nèi)外許多學(xué)者的關(guān)注。文獻(xiàn)[5]使用遞推最小二乘法，在系統(tǒng)存在噪聲時(shí)對(duì)圖像雅克比矩陣進(jìn)行在線估計(jì)；文獻(xiàn)[6]通過跟蹤一個(gè)點(diǎn)在圖像特征空間的運(yùn)動(dòng)，用最小二乘法估計(jì)當(dāng)前時(shí)刻的圖像雅克比矩陣;文獻(xiàn)[7]在沒有考慮系統(tǒng)時(shí)延和噪聲的情況下，利用目標(biāo)點(diǎn)期望位置和實(shí)際位置的差值定義方差最小化函數(shù)，并由此估計(jì)圖像雅克比矩陣；文獻(xiàn)[8]則控制機(jī)器人在工作空間中產(chǎn)生幾次微小的運(yùn)動(dòng)，對(duì)于每次微小運(yùn)動(dòng)在圖像特征空間都會(huì)有相應(yīng)的變化，通過兩者之間的比值得到局部圖像雅克比矩陣的動(dòng)態(tài)估計(jì)值；文獻(xiàn)[9]將圖像雅克比矩陣作為系統(tǒng)的狀態(tài)觀測量，然后利用Kalman-Bucy濾波算法對(duì)其進(jìn)行估計(jì)，這種研究方法在近年來也得到了廣泛的應(yīng)用，并取得了較好的成果。

然而，上述研究方法都是假設(shè)系統(tǒng)沒有時(shí)延且已知過程噪聲協(xié)方差陣Q和測量噪聲協(xié)方差陣R等模型參數(shù)。但是在機(jī)器人視覺伺服控制中，難以獲取精確的過程噪聲協(xié)方差陣Q和測量噪聲協(xié)方差陣R等模型參數(shù)，特別是當(dāng)系統(tǒng)還同時(shí)存在由圖像采集、傳輸和處理過程帶來的時(shí)延情況下，采集的濾波參數(shù)不僅難以保證系統(tǒng)穩(wěn)定最優(yōu)的估計(jì)結(jié)果，而且可能導(dǎo)致系統(tǒng)發(fā)散[10]。

針對(duì)文獻(xiàn)[5-9]的不足，文中提出了一種自適應(yīng)卡爾曼濾波補(bǔ)償算法。該方法不但能夠?qū)V波參數(shù)進(jìn)行在線估計(jì)和調(diào)節(jié)，還能利用自適應(yīng)卡爾曼濾波基本方程，通過圖像雅克比矩陣的視覺伺服系統(tǒng)狀態(tài)模型估計(jì)當(dāng)前時(shí)刻的圖像雅克比矩陣，進(jìn)而預(yù)測目標(biāo)特征。最后通過仿真驗(yàn)證了機(jī)器人跟蹤過程中的誤差可以利用文中所提方法得到有效的改善，從而更進(jìn)一步表明了該方法適用于解決圖像雅克比矩陣的估計(jì)問題。

1 圖像雅克比矩陣

圖像雅克比矩陣是視覺伺服的一個(gè)核心概念，需要用它將視覺空間反饋信息映射到機(jī)械手控制空間中對(duì)機(jī)械手運(yùn)動(dòng)實(shí)施控制，使其到達(dá)期望狀態(tài)。因此，圖像雅克比矩陣的求取是視覺伺服研究的一個(gè)重要內(nèi)容。

機(jī)器人在運(yùn)動(dòng)過程中，末端執(zhí)行器與目標(biāo)對(duì)象的相對(duì)位置一定會(huì)發(fā)生變化。雖然這種變化與攝像機(jī)安裝位置或目標(biāo)對(duì)象的運(yùn)動(dòng)無關(guān)，但是能通過圖像特征變化表現(xiàn)出來。所以，兩者之間必然存在著一一對(duì)應(yīng)的映射關(guān)系。如果想從圖像特征的變化得到機(jī)器人的動(dòng)作規(guī)律，就要明確這種映射關(guān)系。因此，在基于圖像的視覺伺服控制系統(tǒng)中，圖像雅克比矩陣常被用來表示這種映射關(guān)系。

式(1)是目前使用最多的圖像雅克比矩陣模型。在基于圖像的視覺伺服控制系統(tǒng)中，因?yàn)閿z像機(jī)的位置變化是由機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)引起的，所以式(1)的變化關(guān)系可以轉(zhuǎn)移到機(jī)器人的關(guān)節(jié)空間上，此時(shí)，式(1)的映射關(guān)系可變?yōu)椋?/p>

J2=J1Jr也稱為圖像雅克比矩陣。單從定義上看，它表示了圖像空間與機(jī)器人關(guān)節(jié)空間的聯(lián)系。雖然式(1)與式(3)定義圖像雅克比矩陣表達(dá)形式不同，但是機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)規(guī)律的計(jì)算都可以從圖像特征變化求取圖像雅克比矩陣的逆得到。

2 自適應(yīng)卡爾曼濾波補(bǔ)償算法

2.1 自適應(yīng)卡爾曼濾波算法

主要有兩種噪聲干擾存在于未知的機(jī)器人視覺伺服系統(tǒng)的圖像雅克比矩陣估計(jì)中：

1)過程噪聲η(k)是由系統(tǒng)的成像模型變化導(dǎo)致的；

2)測量噪聲v(k)是由系統(tǒng)的圖像采集與處理導(dǎo)致的。

自適應(yīng)卡爾曼濾波的基本思想是針對(duì)未知或不確定的系統(tǒng)噪聲統(tǒng)計(jì)特性(Q,R)問題，利用觀測數(shù)據(jù)進(jìn)行濾波，與此同時(shí)，對(duì)其進(jìn)行在線修正。從而減小濾波誤差，防止濾波器發(fā)散[11]。將式(1)離散化為:

為實(shí)現(xiàn)對(duì)J(q)矩陣各個(gè)元素的估計(jì)，定義mn×1維的狀態(tài)向量為[12]：

與此同時(shí)，將式(5)定義為系統(tǒng)狀態(tài)，而系統(tǒng)輸出定義為圖像特征變化的差值，定義如下：

得到如下狀態(tài)方程：

式中：η(k)、v(k)----分別為過程噪聲序列和圖像觀測噪聲序列，其統(tǒng)計(jì)特性如下：

據(jù)此可以設(shè)計(jì)出自適應(yīng)卡爾曼濾波的遞推算法：

式中：Pk----狀態(tài)估計(jì)誤差方差陣。

2.2 自適應(yīng)卡爾曼濾波補(bǔ)償器設(shè)計(jì)

受系統(tǒng)時(shí)延的影響，在實(shí)際控制過程中,當(dāng)前時(shí)刻特征點(diǎn)的狀態(tài)不能直接得到。假設(shè)系統(tǒng)采樣周期為T，由圖像采集、傳輸和處理等過程引起的時(shí)延為hT。從而只能得到從初始0時(shí)刻到k-h時(shí)刻的機(jī)械手末端在圖像空間的位置fi(i=0,1，…,k-h)[13-14]。根據(jù)式(7)可以得到系統(tǒng)的狀態(tài)方程和觀測方程，從k-h+1時(shí)刻到k時(shí)刻的圖像雅克比矩陣可以用自適應(yīng)卡爾曼濾波估計(jì)出來。

在考慮時(shí)延情況時(shí),視覺伺服系統(tǒng)的狀態(tài)方程和觀測方程為：

加時(shí)延補(bǔ)償后的自適應(yīng)卡爾曼濾波算法如下：

將視覺伺服系統(tǒng)用式(13)和式(14)的自適應(yīng)卡爾曼濾波補(bǔ)償算法進(jìn)行h次預(yù)測，從k-h+1時(shí)刻到k時(shí)刻的圖像雅克比矩陣就能估計(jì)出來。

3 仿真實(shí)驗(yàn)研究

通過一個(gè)執(zhí)行器末端帶有攝像機(jī)的機(jī)器人系統(tǒng)來驗(yàn)證所給出的基于自適應(yīng)卡爾曼濾波補(bǔ)償算法估計(jì)圖像雅克比矩陣的有效性，該系統(tǒng)完成由攝像機(jī)引導(dǎo)機(jī)器人沿環(huán)形軌跡運(yùn)動(dòng)的任務(wù)。

設(shè)被跟蹤目標(biāo)的運(yùn)動(dòng)方程為:

假設(shè)機(jī)器人起始關(guān)節(jié)角位于(0°,90°)處，采樣周期為50 ms，粗略估計(jì)J(q)的初值為

自適應(yīng)卡爾曼濾波的初始參數(shù)取值如下：過程噪聲是均值為0.1，方差為0.5的高斯白噪聲，測量噪聲均值的初值為r=[0.01;0.01]，初始方差陣Rv=0.5diag{1,1}，遺忘因子b=0.65。狀態(tài)估計(jì)誤差方差陣P的初值可取P(0)=105I4。假定系統(tǒng)時(shí)延τ是兩個(gè)采樣周期。

采用卡爾曼濾波方法的圖像雅克比矩陣估計(jì)實(shí)驗(yàn)結(jié)果如圖1～圖3所示。

圖1 軌跡跟蹤曲線(Kalman)

圖2 跟蹤誤差曲線(Kalman)

圖3 關(guān)節(jié)角運(yùn)動(dòng)軌跡(Kalman)

采用自適應(yīng)卡爾曼濾波補(bǔ)償算法的圖像雅克比矩陣估計(jì)的實(shí)驗(yàn)結(jié)果如圖4～圖6所示。

由圖1和圖4可以看出，機(jī)械手在跟蹤初期能夠快速接近目標(biāo)，并且對(duì)橢圓軌跡進(jìn)行比較精確的跟蹤。從圖2和圖5中可以看出，兩種方法在k=8左右迅速接近目標(biāo)軌跡；由圖2可以看出,在k=20左右系統(tǒng)達(dá)到穩(wěn)定狀態(tài)；在k=70～120之間系統(tǒng)出現(xiàn)震蕩；當(dāng)k>120時(shí)，系統(tǒng)才逐漸歸于穩(wěn)定。采用自適應(yīng)卡爾曼濾波補(bǔ)償算法比卡爾曼濾波算法在跟蹤過程中的軌跡曲線更平滑；而且跟蹤軌跡在k=30左右?guī)缀鯖]有誤差，當(dāng)k>100時(shí)，系統(tǒng)無明顯震蕩，體現(xiàn)了較好的跟蹤性能。

圖4 軌跡跟蹤曲線(加時(shí)延與自適應(yīng))

圖5 跟蹤誤差曲線(加時(shí)延與自適應(yīng))

圖6 關(guān)節(jié)角運(yùn)動(dòng)軌跡(加時(shí)延與自適應(yīng))

跟蹤誤差及時(shí)間對(duì)比見表1。

表1 跟蹤誤差及時(shí)間對(duì)比表

利用表1中的數(shù)據(jù)可以對(duì)比兩種估計(jì)圖像雅克比矩陣的方法，從而得出利用自適應(yīng)卡爾曼濾波補(bǔ)償算法估計(jì)圖像雅克比矩陣的機(jī)器人在X軸和Y軸的跟蹤誤差減小。因此，采用自適應(yīng)卡爾曼濾波補(bǔ)償算法估計(jì)圖像雅克比矩陣的方法更具有實(shí)用性。

4 結(jié) 語

提出了基于自適應(yīng)卡爾曼濾波補(bǔ)償算法的圖像雅克比矩陣估計(jì)方法。在噪聲和時(shí)延系統(tǒng)中，該方法可以有效地估計(jì)圖像雅克比矩陣，并應(yīng)用于機(jī)器人視覺伺服控制中。仿真實(shí)驗(yàn)證明了所給出的自適應(yīng)卡爾曼濾波補(bǔ)償算法可以準(zhǔn)確地估計(jì)圖像雅克比矩陣，并且降低了跟蹤過程中X、Y軸方向上的跟蹤誤差，減小了目標(biāo)跟蹤過程中的震蕩，從而提高了機(jī)器人系統(tǒng)的跟蹤精度。

[1] 高振東,蘇劍波.帶有時(shí)延補(bǔ)償?shù)膱D像雅克比矩陣估計(jì)方法[J].控制理論與應(yīng)用,2009,26(1):23-27.

[2] Shademan A, Farahmand A M, Ja Gersand M. Robust jacobian estimation for uncalibrated visual servoing[C]//IEEE International Conference on Robotics and Automation. [S.l.]: IEEE,2010:5564-5569.

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

[4] Piepmeier J A, Mcmurray G V, Lipkin H. A dynamic jacobian estimation method for uncalibrated visual servoing[C]//Ieee/asme International Conference on Advanced Intelligent Mechatronics. Proceedings. [S.l.]: IEEE,1999:944-949.

[5] Piepmeier J A, Mcmurray G V, Lipkin H. Uncalibrated dynamic visual servoing [J]. Robotics & Automation IEEE Transactions on,2004,20(1):143-147.

[6] Yoshimi B H, Allen P K. Alignment using an uncalibrated camera system [J]. IEEE Transactions on Robotics & Automation,1995,11(4):516-521.

[7] 項(xiàng)龍江,司秉玉,薛定宇,等.模型無關(guān)的無定標(biāo)視覺伺服控制[J].機(jī)器人,2003,25(5):424-427.

[8] 郭振民,陳善本,吳林.一種基于圖象的無標(biāo)定視覺伺服方法的研究[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2002,34(3):294-296.

[9] Qian J, Su J. Online estimation of image Jacobian matrix by Kalman-Bucy filter for uncalibrated stereo vision feedback[C]// IEEE International Conference on Robotics and Automation, 2002. Proceedings. ICRA. [S.l.]: IEEE Xplore,2002:562-567.

[10] 劉文芳,邴志剛,盧勝利,等.帶時(shí)延補(bǔ)償?shù)膱D像雅克比矩陣在線估計(jì)方法[J].計(jì)算機(jī)工程與應(yīng)用,2010,46(21):181-184.

[11] 辛菁,白蕾,劉丁.基于自適應(yīng)kalman濾波的機(jī)器人6DOF無標(biāo)定視覺定位[J].系統(tǒng)仿真學(xué)報(bào),2014,26(3):586-591.

[12] 徐鵬，楊宏韜，李巖，等.基于H∞粒子濾波算法的視覺伺服雅可比矩陣在線辨識(shí)[J].廣西大學(xué)學(xué)報(bào):自然科學(xué)版，2017，42(3):1051-1057.

[13] 王新梅,魏武,劉瑋,等.魯棒卡爾曼濾波下的圖像雅可比矩陣帶時(shí)延補(bǔ)償?shù)墓烙?jì)[J].控制理論與應(yīng)用,2015,32(8):1052-1057.

[14] 張顯偉，吳忠偉，張鳳海，等.移動(dòng)機(jī)器人模糊滑模軌跡跟蹤控制[J].長春工業(yè)大學(xué)學(xué)報(bào)：自然科學(xué)版，2013,34(4)：373-378.

ImagejacobianmatrixestimationbasedonadaptiveKalmanfilter

SUN Dongxue, YANG Hongtao, LIU Keping*

(School of Electrical & Electronic Engineering, Changchun University of Technology, Changchun 130012， China)

An adaptive Kalman filter is applied to realize the recursive estimation of the image Jacobian matrix. The process and measurement noise are estimated and adjusted on-line compensate the delay brought by image acquisition, transmission and processing. Simulation results indicate the method is feasible.

image jacobian matrix; adaptive Kalman filter; time-delay compensation.

2017-07-22

吉林省科技廳基金資助項(xiàng)目(201505190009JH)； 吉林省教育廳基金資助項(xiàng)目(2016338)

孫冬雪(1991-)，女，漢族，吉林松原人，長春工業(yè)大學(xué)碩士研究生，主要從事機(jī)器人視覺伺服方向研究,E-mail:sundongxue1121@163.com. *通訊作者：劉克平(1971-)，男，漢族，吉林長春人，長春工業(yè)大學(xué)教授，博士生導(dǎo)師，主要從事復(fù)雜系統(tǒng)建模、優(yōu)化與控制等方向研究,E-mail:liukeping@ccut.edu.cn.

10.15923/j.cnki.cn22-1382/t.2017.5.07

TP 241

A

1674-1374(2017)05-0447-06