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

孫冬雪， 楊宏韜， 劉克平

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

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

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

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

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

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

0 引 言

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

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

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

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

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

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

1 圖像雅克比矩陣

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 仿真實驗研究

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

設(shè)被跟蹤目標(biāo)的運動方程為:

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

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

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

圖1 軌跡跟蹤曲線(Kalman)

圖2 跟蹤誤差曲線(Kalman)

圖3 關(guān)節(jié)角運動軌跡(Kalman)

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

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

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

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

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

跟蹤誤差及時間對比見表1。

表1 跟蹤誤差及時間對比表

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

4 結(jié) 語

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

[1] 高振東,蘇劍波.帶有時延補償?shù)膱D像雅克比矩陣估計方法[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] 曾祥進,黃心漢,王敏.基于Broyden在線圖像雅可比矩陣辨識的視覺伺服[J].華中科技大學(xué)學(xué)報：自然科學(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] 項龍江,司秉玉,薛定宇,等.模型無關(guān)的無定標(biāo)視覺伺服控制[J].機器人,2003,25(5):424-427.

[8] 郭振民,陳善本,吳林.一種基于圖象的無標(biāo)定視覺伺服方法的研究[J].哈爾濱工業(yè)大學(xué)學(xué)報,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ù)膱D像雅克比矩陣在線估計方法[J].計算機工程與應(yīng)用,2010,46(21):181-184.

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

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

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

[14] 張顯偉，吳忠偉，張鳳海，等.移動機器人模糊滑模軌跡跟蹤控制[J].長春工業(yè)大學(xué)學(xué)報：自然科學(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

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

孫冬雪(1991-)，女，漢族，吉林松原人，長春工業(yè)大學(xué)碩士研究生，主要從事機器人視覺伺服方向研究,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