自適應卡爾曼濾波圖像雅克比估計

孫冬雪， 楊宏韜， 劉克平

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

自適應卡爾曼濾波圖像雅克比估計

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

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

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

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

0 引 言

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

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

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

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

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

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

1 圖像雅克比矩陣

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

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

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

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

2 自適應卡爾曼濾波補償算法

2.1 自適應卡爾曼濾波算法

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

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

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

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

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

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

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

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

據此可以設計出自適應卡爾曼濾波的遞推算法：

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

2.2 自適應卡爾曼濾波補償器設計

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

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

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

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

3 仿真實驗研究

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

設被跟蹤目標的運動方程為:

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

自適應卡爾曼濾波的初始參數取值如下：過程噪聲是均值為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)時延τ是兩個采樣周期。

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

圖1 軌跡跟蹤曲線(Kalman)

圖2 跟蹤誤差曲線(Kalman)

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

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

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

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

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

圖6 關節(jié)角運動軌跡(加時延與自適應)

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

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

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

4 結 語

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

[1] 高振東,蘇劍波.帶有時延補償的圖像雅克比矩陣估計方法[J].控制理論與應用,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].華中科技大學學報：自然科學版,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] 項龍江,司秉玉,薛定宇,等.模型無關的無定標視覺伺服控制[J].機器人,2003,25(5):424-427.

[8] 郭振民,陳善本,吳林.一種基于圖象的無標定視覺伺服方法的研究[J].哈爾濱工業(yè)大學學報,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] 劉文芳,邴志剛,盧勝利,等.帶時延補償的圖像雅克比矩陣在線估計方法[J].計算機工程與應用,2010,46(21):181-184.

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

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

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

[14] 張顯偉，吳忠偉，張鳳海，等.移動機器人模糊滑模軌跡跟蹤控制[J].長春工業(yè)大學學報：自然科學版，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è)大學碩士研究生，主要從事機器人視覺伺服方向研究,E-mail:sundongxue1121@163.com. *通訊作者：劉克平(1971-)，男，漢族，吉林長春人，長春工業(yè)大學教授，博士生導師，主要從事復雜系統(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