機(jī)器人視覺(jué)Kalman和FIR濾波穩(wěn)像算法設(shè)計(jì)與比較

金英連， 王斌銳, ， 徐 崟

（1. 中國(guó)計(jì)量學(xué)院機(jī)電工程學(xué)院，浙江 杭州 310018；2. 浙江大學(xué)控制科學(xué)與工程系，浙江 杭州 310007）

移動(dòng)機(jī)器人視覺(jué)不可避免的存在干擾，主要表現(xiàn)為圖像抖動(dòng)[1]。數(shù)字穩(wěn)像包括運(yùn)動(dòng)估計(jì)、平滑和補(bǔ)償?shù)萚2-4]。早期的運(yùn)動(dòng)估計(jì)利用塊匹配BMA (Blocks Matching Algorithm)進(jìn)行，但受噪聲影響比較大[5]。改進(jìn)算法有KLT匹配算法[6]。Tien-Ying Kuo等利用圖像的比特平面匹配估計(jì)運(yùn)動(dòng)參數(shù)[7]。匹配搜索有全搜索(FS)、三步法(TSS)、二維對(duì)數(shù)法(TDL)、交叉法(CS)及菱形法(DS)等。為量化匹配程度，準(zhǔn)則有絕對(duì)誤差和(SAD)、最小平均絕對(duì)差 (MAD)、最小均方差誤差 (MSE)、歸一化互相關(guān) (NC)等。鐘平等研究投影法PA(Projection Algorithm) 運(yùn)動(dòng)估計(jì)，利用灰度變化獲得圖像的位移矢量[8]。孫輝等利用光流約束方程求解運(yùn)動(dòng)參數(shù)[9]。早期的穩(wěn)像利用均值濾波，用濾波前后對(duì)應(yīng)幀的參數(shù)差作為補(bǔ)償參數(shù)。趙文華等將運(yùn)動(dòng)參數(shù)代入FIR濾波器中，通過(guò)與截?cái)嗪瘮?shù)乘積進(jìn)行濾波[10]。Kalman濾波首先預(yù)估圖像運(yùn)動(dòng)，然后根據(jù)誤差協(xié)方差對(duì)觀測(cè)到的值進(jìn)行調(diào)整[11]。目前Kalman和FIR是濾波器的研究熱點(diǎn)[12-14]。在移動(dòng)機(jī)器人對(duì)接作業(yè)過(guò)程中，機(jī)器人需要依據(jù)對(duì)接點(diǎn)目標(biāo)，在水平和垂直方向上進(jìn)行位置調(diào)整。其間因路面不平、機(jī)械間隙、摩擦和柔性等原因會(huì)引入無(wú)意運(yùn)動(dòng)，表現(xiàn)為視頻相鄰幀圖像間會(huì)有明顯的平移抖動(dòng)。從已有文獻(xiàn)可見(jiàn)，穩(wěn)像算法較多，但通過(guò)對(duì)不同算法的比較研究，設(shè)計(jì)適合移動(dòng)機(jī)器人對(duì)接作業(yè)中視覺(jué)穩(wěn)像應(yīng)用背景的可見(jiàn)文獻(xiàn)較少。

1 基于濾波的數(shù)字穩(wěn)像算法

1.1 圖像運(yùn)動(dòng)建模

本文結(jié)合機(jī)器人對(duì)接作業(yè)視頻特點(diǎn)，選取仿射變換矩陣。第n-1幀圖像In1-和第n幀圖像In中對(duì)應(yīng)像素的坐標(biāo)qn1-和nq滿(mǎn)足仿射變換關(guān)系：

1.2 特征點(diǎn)提取與匹配

定義特征窗口是由相鄰像素點(diǎn)的集合組成，窗口的中心點(diǎn)即特征點(diǎn)。KLT特征提取根據(jù)特征窗口梯度矩陣的特征值。特征窗口的梯度矩陣為：

式中，w代表特征窗口，gx、gy分別為圖像在橫向和縱向的梯度。G的特征值表征特征窗口的紋理特征。

移動(dòng)機(jī)器人對(duì)接作業(yè)中，隨著兩機(jī)器人不斷接近，視頻中特征窗口內(nèi)所有像素點(diǎn)的運(yùn)動(dòng)一致，匹配的過(guò)程即搜索過(guò)程。本文采用菱形搜索（DS）算法，使用兩種大小的模板。首先使用步長(zhǎng)大的模板，進(jìn)行粗定位，避免搜索陷于局部最小；再用小模板準(zhǔn)確定位。

特征點(diǎn)匹配采用SAD匹配準(zhǔn)則。定義In(x′,y′)為圖像In中(x′,y′)點(diǎn)的像素值， (x′,y′)點(diǎn)和圖像In-1中(x,y)點(diǎn)相匹配，則匹配度為：

式中，w′是與w相對(duì)應(yīng)的特征窗口。SAD最小的點(diǎn)即為具有最高匹配度的點(diǎn)。

根據(jù)式（2），可建立圖像間的運(yùn)動(dòng)方程組。當(dāng)匹配點(diǎn)的對(duì)數(shù)K>3時(shí)，方程組超定，需用最小二乘法通過(guò)最小化誤差的平方和來(lái)求解。

1.3 運(yùn)動(dòng)參數(shù)濾波與補(bǔ)償

1）遞歸Kalman濾波

圖像運(yùn)動(dòng)參數(shù)在k時(shí)刻的離散過(guò)程可描述為：

式中，狀態(tài)變量（仿射變換矩陣參數(shù)及其變化速度）x∈Rn；M為狀態(tài)變量的增益矩陣；B為控制變量u的增益矩陣；f為過(guò)程噪聲（抖動(dòng)）N(0,Q)，Q為方差。

定義Kalman濾波器的觀測(cè)方程為：

式中，H為觀測(cè)矩陣，v為觀測(cè)噪聲N(0,R)，R為方差。

Kalman濾波分為預(yù)測(cè)和測(cè)量更新兩步。k時(shí)刻的預(yù)測(cè)方程設(shè)計(jì)為：

k時(shí)刻的測(cè)量更新方程設(shè)計(jì)為：

式中，Kk為Kalman濾波器的增益。

2）FIR濾波

本文設(shè)計(jì)的嚴(yán)格線性相位FIR濾波器為：

式中，L為濾波器的階數(shù)；h(i)為濾波器的系數(shù)。

本文采用窗函數(shù)設(shè)計(jì)FIR濾波器系數(shù)：

補(bǔ)償變換即使用濾波后的有意運(yùn)動(dòng)參數(shù)變換矩陣，將原視頻序列轉(zhuǎn)變?yōu)榉€(wěn)定后視頻序列。

由于視頻抖動(dòng)情況復(fù)雜，容易受到外界環(huán)境干擾，理論上判定Kalman和FIR哪種更適合視頻去抖困難。本文采用實(shí)驗(yàn)和仿真來(lái)比較兩種方法。

2 運(yùn)動(dòng)參數(shù)Kalman濾波仿真

MATLAB仿真中，Kalman濾波器初始運(yùn)動(dòng)參數(shù)為[1 0 0 1 0 0]T，狀態(tài)方差為0.00001，觀測(cè)方差為0.3。機(jī)器人帶動(dòng)攝像頭在x和y方向上做正余弦曲線有意運(yùn)動(dòng)。對(duì)采集到的視頻進(jìn)行特征提取、匹配和模型參數(shù)求解。而后進(jìn)行濾仿真，得到圖像橫向x和縱向y方向上的運(yùn)動(dòng)參數(shù)濾波前后曲線如圖1。

圖1可見(jiàn)，濾波前，x、y方向上的運(yùn)動(dòng)曲線不光滑，存在抖動(dòng)。經(jīng)過(guò)Kalman濾波后，運(yùn)動(dòng)曲線得到了改善，抖動(dòng)基本上被去除，Kalman濾波是收斂的。由于Kalman濾波器的初始值和協(xié)方差初值問(wèn)題，圖1 (a)中濾波后的曲線在初始幀誤差比較大。但隨著不斷有新的數(shù)據(jù)加入，協(xié)方差和估計(jì)值會(huì)不斷的改善。濾波結(jié)果的數(shù)據(jù)對(duì)比如表1所示。

圖1 運(yùn)動(dòng)參數(shù)濾波前后對(duì)比

表1 平移抖動(dòng)濾波前后結(jié)果對(duì)比

Kalman濾波后的偏差、均方差、誤差平均值都比濾波前小。

3 不同濾波算法的比較研究

對(duì)同一視頻，本文分別用Kalman濾波和FIR濾波進(jìn)行穩(wěn)像，曲線如圖2所示。

圖2所示，兩種濾波方法都使原始曲線得到平滑，與有意運(yùn)動(dòng)趨勢(shì)保持一致。進(jìn)一步分析，F(xiàn)IR濾波相比于Kalman濾波，與有意運(yùn)動(dòng)偏差稍大。由式（12）可知，F(xiàn)IR低通濾波后的當(dāng)前值是通過(guò)前序值與截止函數(shù)積的求和得到，前序值存在的誤差會(huì)一直被累積。

Kalman與FIR濾波結(jié)果對(duì)比如表2所示。

圖2 Kalman濾波和FIR濾波結(jié)果對(duì)比

表2 平移對(duì)接視頻Kalman與FIR濾波結(jié)果對(duì)比

Kalman濾波的偏差、均方差都比FIR濾波小。

4 對(duì)接作業(yè)視頻穩(wěn)像實(shí)驗(yàn)

針對(duì)UP-VoyagerII機(jī)器人1（配置USB攝像頭）與Dr. Robot機(jī)器人2對(duì)接作業(yè)，提取了其中一段視頻進(jìn)行實(shí)驗(yàn)。視頻采集頻率20幀/s，實(shí)驗(yàn)時(shí)間5s。穩(wěn)像中使用的圖像為8位灰度圖。雙機(jī)器人移動(dòng)作業(yè)中，機(jī)器人1移動(dòng)速度為0.11m/s，機(jī)器人2移動(dòng)速度為0.07m/s，機(jī)器人2直線前進(jìn)。

本文自主建立的實(shí)驗(yàn)環(huán)境和利用VC++和OpenCV開(kāi)發(fā)的穩(wěn)像軟件如圖3所示。

圖3 實(shí)驗(yàn)環(huán)境及軟件界面

圖4 是視頻中第64-74幀序列中偶數(shù)幀圖像穩(wěn)像前后的對(duì)比圖。

圖4 視頻序列穩(wěn)像前后對(duì)比

原視頻中第64幀在y方向上存在-3個(gè)像素的抖動(dòng)；第68幀在x、y方向上存在1和4個(gè)像素的抖動(dòng)；第74幀在x、y方向上存在-4和-2個(gè)像素的抖動(dòng)。濾波后，第64幀在y方向上存在-1個(gè)像素的抖動(dòng)；第68幀在x、y方向上存在0.1和0.6個(gè)像素的抖動(dòng)；第74幀在x、y方向上存在-0.2和-0.1個(gè)像素的抖動(dòng)。 可見(jiàn)穩(wěn)像后，抖動(dòng)被抑制在1個(gè)像素以下。測(cè)得穩(wěn)像系統(tǒng)執(zhí)行時(shí)間為23.7629ms，小于視頻采樣時(shí)間，滿(mǎn)足實(shí)時(shí)性的要求。

5 結(jié) 論

從理論建模、仿真分析、對(duì)比研究和實(shí)物實(shí)驗(yàn)可見(jiàn)：

1）濾波器初始值的選擇對(duì)視頻初始幀運(yùn)動(dòng)參數(shù)穩(wěn)像影響較大，所以機(jī)器人對(duì)接作業(yè)應(yīng)避免在視頻初始階段進(jìn)行；

2) Kalman與FIR濾波均適用于平移抖動(dòng)的去抖處理，但Kalman優(yōu)于FIR；

3）本文設(shè)計(jì)的基于KLT特征、SAD匹配、DS搜索和Kalman濾波的機(jī)器人視覺(jué)穩(wěn)像算法，對(duì)于處理機(jī)器人視頻的平移抖動(dòng)效果明顯。

下一步將研究濾波參數(shù)的優(yōu)化設(shè)計(jì)。

[1]陳茹雯, 黃 仁, 張志勝, 等. 基于數(shù)學(xué)模型的視覺(jué)測(cè)量系統(tǒng)圖像畸變校正方法[J]. 機(jī)械工程學(xué)報(bào),2009, 45(7): 243-248.

[2]Corsini G, Diani M, Masini A. Video sequence stabilization for real-time remote sensing applications [C]//Geoscience and Remote Sensing Symposium: IEEE conference, 2006: 980-983.

[3]Liu Lingqiao, Fu Zhizhong, Qian Wei. A new motion estimation method for video stabilization [J].Computer Science and Computational Technology,2008, 2(1): 440-444.

[4]Tico M, Alenius S, Vehvilainen M. Method of motion estimation for image stabilization [J]. Acoustics,Speech and Signal Processing, 2006, 2(2): 277-280.

[5]Essannnouni F, Salam A, Aboutajdine D. A new fast full search block matching algorithm using frequency domain [J]. Signal Processing and Its Applications,2005, 8(2): 559-562.

[6]王云麗, 張 鑫, 高 超, 等. 航拍視頻拼圖中基于特征匹配的全局運(yùn)動(dòng)估計(jì)方法[J]. 航空學(xué)報(bào), 2008,29(5): 1218-1220.

[7]Kuo Tienying, Wang Chunghsin. Fast local motion estimation and robust global motion decision for digital image stabilization [J]. International Conference on Intelligent Hiding and Multimedia Signal Processing, 2008, 232(10): 441-444.

[8]鐘 平, 于前洋, 金 光. 動(dòng)態(tài)圖像序列間運(yùn)動(dòng)補(bǔ)償方法探討[J]. 光學(xué)技術(shù), 2007, 29(7): 442-443.

[9]馮 波, 趙春暉, 楊 濤, 等. 基于光流特征與序列比對(duì)的實(shí)時(shí)行為識(shí)別[J]. 計(jì)算機(jī)應(yīng)用研究, 2007,24(3): 194-198.

[10]趙文華, 姚天翔, 葉秀清, 等. RANSAC算法在視頻去抖中的應(yīng)用[J]. 電路與系統(tǒng), 2005, 10(4):91-94.

[11]Lee Kai Ki, Wong Kin Hong, Chang Ming Yuen, et al.Extended Kalman filtering approach to stereo video stabilization [C]//Tampa: 19th International Conference on Pattern Recognition, 2008: 1-4.

[12]Battiato S, Puglisi G, Bruna A. A robust video stabilization system by adaptive motion vectors filtering [C]//Multimedia and Expo. Hannover: IEEE conference, 2008: 373-376.

[13]邢開(kāi)亮, 尹義蓉, 黃永華, 等. 提高網(wǎng)絡(luò)授時(shí)精度的Kalman濾波方法[J]. 儀器儀表學(xué)報(bào), 2011, 32(4):949-954.

[14]吳賽成, 秦石喬, 王省書(shū), 等. 應(yīng)用Kalman濾波器提高機(jī)抖激光陀螺姿態(tài)測(cè)量系統(tǒng)瞬時(shí)精度的方法[J].儀器儀表學(xué)報(bào), 2011, 32(1): 201-205.