基于卡爾曼濾波器的圖像雅克比在線估計

劉 偉， 于振中 ， 惠 晶

(江南大學(xué) 輕工過程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫214122)

在基于標(biāo)定的視覺伺服中［1-2］，攝像機(jī)內(nèi)外部參數(shù)等系統(tǒng)相關(guān)參數(shù)必須在視覺伺服之前精確標(biāo)定。然而，這些系統(tǒng)參數(shù)的精確標(biāo)定不僅費(fèi)時且很難獲得。無標(biāo)定視覺伺服技術(shù)在只有這些參數(shù)粗略知識甚至未知的情況下也能對機(jī)械手運(yùn)動實(shí)施有效控制［3-4］。

基于圖像雅克比在線估計方法的無標(biāo)定視覺伺服的核心問題是獲得實(shí)時滿足系統(tǒng)控制性能要求的圖像雅克比估計值。圖像雅克比在線估計目的不是辨識出真實(shí)的系統(tǒng)參數(shù)，而是能夠利用估計到的圖像雅克比設(shè)計滿足性能要求的視覺控制器［5-6］。目前，對圖像雅克比的在線估計方法已有大量的研究［7-8］，但是這些估計方法在處理圖像噪聲方面很薄弱。文中提出一種基于卡爾曼濾波器的圖像雅克比在線估計方法，該方法對圖像噪聲有很強(qiáng)的魯棒性且不需要關(guān)于系統(tǒng)參數(shù)的先驗(yàn)知識。

1 圖像雅克比矩陣

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

機(jī)械手手爪的位置在機(jī)械手基坐標(biāo)系中可表示為p = ［p1，p2，…，pn］T，手爪在圖像空間中表示為f = ［f1，f2，…，fm］T。當(dāng)手爪移動時，它在機(jī)器人工作空間的速率˙p 和在圖像空間的速率之間有如下關(guān)系:

其中，J(p)為一個m × n 的圖像雅克比矩陣。式(1)描述了f·與˙p 之間的微分關(guān)系。下面將用卡爾曼濾波器對J(p)進(jìn)行在線估計。

2 卡爾曼估計

卡爾曼濾波器被廣泛運(yùn)用到系統(tǒng)狀態(tài)的觀測中，它對系統(tǒng)噪聲和外部干擾有很強(qiáng)的魯棒性。構(gòu)建一個輔助系統(tǒng)，它的狀態(tài)向量x(k)在時刻k 是由圖像雅克比J(k)的一連串行元素構(gòu)成的增廣向量。

其中

是圖像雅克比J(k)第i 行。式(1)的離散形式為

定義系統(tǒng)輸出為y(k)= f(k +1)－f(k)，輔助系統(tǒng)可以被描述為

其中，η(k)，v(k)為狀態(tài)噪聲和觀測噪聲，在文中它們都被設(shè)為是高斯白噪聲。C(k)為測量矩陣:

對于式(4)描述的輔助系統(tǒng)，用卡爾曼濾波器觀測其狀態(tài)變量簡單易行。在每一步中可由以下迭代公式對圖像雅克比進(jìn)行合理的估計。

式中:Rη和Rv為η(k)和v(k)的協(xié)方差矩陣。

輔助系統(tǒng)的初始狀態(tài)^x(或^J(0))可由以下步驟獲得。讓機(jī)械手在初始位置的附近區(qū)域移動n 次Δp1… Δpn，在圖像空間中觀察對應(yīng)位移Δf1… Δfn。初始雅克比矩陣可由下式獲得:

水利工程中堤壩是其重要的組成部分，其具有種類多，施工工藝較繁瑣的特點(diǎn)，特別是較易形成滲漏，對堤壩的滲漏原因進(jìn)行深入分析，在此同時，結(jié)合堤壩滲漏情況，選擇最為適合的防治加固技術(shù)對其進(jìn)行有效處理，保證堤壩防滲加固技術(shù)的應(yīng)用效果。

其中，^x(0)可由^J(0)帶入式(2)得到。

3 雅克比視覺伺服控制器

在有視覺信息反饋下圖像雅克比可用于控制機(jī)械手移動到期望位置。由文中提出的卡爾曼估計方法獲得圖像雅克比的估計值后，需采用一種控制策略驅(qū)動機(jī)械手從當(dāng)前位置運(yùn)動到期望位置。如果機(jī)械手的當(dāng)前位置和期望位置都用在圖像空間中的特征向量表示，可以定義在k 時刻兩者之間的誤差:

其中，f*(k)為手爪在圖像空間中的期望位置;fg(k)為手爪在圖像空間中的當(dāng)前位置。在視覺跟蹤任務(wù)中，期望位置f*(k)定義為移動目標(biāo)fo(k)的圖像位置。那么控制策略為

式(10)中λ(k)由機(jī)械手的速率約束方程設(shè)定:

其中，Δpmax為機(jī)械手驅(qū)動器可提供的手爪最大移動速度。

整個無標(biāo)定手眼協(xié)調(diào)系統(tǒng)的架構(gòu)如圖1 所示。

圖1 無標(biāo)定手眼協(xié)調(diào)系統(tǒng)Fig.1 Uncalibrated hand-eye coordination system

4 仿真與實(shí)驗(yàn)

通過仿真與實(shí)驗(yàn)驗(yàn)證該圖像雅克比估計方法的性能和該控制方案的有效性。圖2 為實(shí)驗(yàn)測試平臺。該平臺是由三自由度并聯(lián)delta 機(jī)械手和一個相機(jī)組成的手眼協(xié)調(diào)系統(tǒng)。實(shí)驗(yàn)伺服任務(wù)為:驅(qū)動機(jī)械手，讓其手爪跟蹤一個二維隨機(jī)運(yùn)動的物體。機(jī)械手與相機(jī)的關(guān)系和相機(jī)參數(shù)都是未知的。

圖2 實(shí)驗(yàn)測試平臺Fig.2 Experiment testbed

4.1 仿 真

在仿真中手爪跟蹤平面內(nèi)運(yùn)動目標(biāo)的任務(wù)定義為:目標(biāo)在xoy 平面做直線運(yùn)動，并在運(yùn)動過程中改變運(yùn)動方向，機(jī)械手跟蹤目標(biāo)運(yùn)動。仿真參數(shù)設(shè)置如下:相機(jī)的位置在機(jī)械手基坐標(biāo)系中為［200，650，850］，它的光軸朝向基坐標(biāo)系xoy 平面中的點(diǎn)［42，340，0］。相機(jī)的焦距f = 15 mm。手爪最大速度為0.8 m/s，初始位置為(320，420)。目標(biāo)的運(yùn)動速度為0.2 m/s，初始位置為(200，0)。為了使仿真更真實(shí)全面評價所提方法，在水平和垂直方向上對目標(biāo)和手爪的圖像位置疊加了三像素二維高斯噪聲。機(jī)械手的跟蹤軌跡和跟蹤誤差如圖3 和圖4 所示。

圖3 仿真跟蹤軌跡Fig.3 Curve of the simulation tracking

圖4 仿真跟蹤誤差曲線Fig.4 Error curve of the simulation tracking

由圖3 可以看出，機(jī)械手能在跟蹤初期以手爪最大速度0.8 m/s 向目標(biāo)接近，隨后能夠持續(xù)有效的對目標(biāo)進(jìn)行跟蹤。

由圖4 可以看出，跟蹤開始后末端執(zhí)行器與目標(biāo)的誤差呈急劇下降態(tài)勢，在接近目標(biāo)后出現(xiàn)小范圍的震蕩，隨著跟蹤步數(shù)的增加震蕩減弱，最后收斂且誤差小于10 mm。

綜上所述，此種非標(biāo)定方法能夠?qū)崿F(xiàn)機(jī)械手對目標(biāo)的平穩(wěn)跟蹤，從跟蹤路徑上分析系統(tǒng)的響應(yīng)速度還有待提高;從誤差曲線上來看，雖然誤差實(shí)現(xiàn)了較好的收斂，但是誤差震蕩較大，這也有待提高。

與其他圖像雅克比在線估計方法相比，基于卡爾曼濾波器的方法對圖像噪聲有更強(qiáng)的魯棒性。以文中方法和文獻(xiàn)［7］提出的方法做同樣的仿真實(shí)驗(yàn)，所有參數(shù)設(shè)置相同。仿真結(jié)果如圖5 所示。

由圖5 可以看出，由于圖像噪聲的影響手爪對目標(biāo)的跟蹤軌跡抖動較大。對比圖3 與圖5 可知，文中所提方法對圖像噪聲有更強(qiáng)的魯棒性。

圖5 對比軌跡Fig.5 Compared curves

4.2 實(shí) 驗(yàn)

在實(shí)驗(yàn)中，采用如圖2 所示三自由度的并聯(lián)Delta 機(jī)械手。視覺部分為一臺彩色CCD 相機(jī)，圖像窗口為512 × 512 像素。工作平面為200 mm ×200 mm。為了簡化圖像處理和對象識別，末端執(zhí)行器和目標(biāo)都貼上了色塊。目標(biāo)的初始位置為(20，125)，手爪初始位置為(80，140)。圖像系統(tǒng)的采樣頻率為100 ms。目標(biāo)的移動速度大約為0.1 m/s。實(shí)驗(yàn)所得機(jī)械手和目標(biāo)運(yùn)動軌跡如圖6 所示。

由圖6 可以看出，在跟蹤過程中，機(jī)械手始終向目標(biāo)靠近，并且其追蹤隨機(jī)運(yùn)動目標(biāo)的線路也較為合理，能夠較好地完成跟蹤任務(wù)。

圖6 目標(biāo)與機(jī)械手軌跡Fig.6 Tracks of the target and manipulator

5 結(jié) 語

提出基于卡爾曼濾波器的圖像雅克比在線估計算法，該算法對視覺反饋噪聲有很強(qiáng)的魯棒性?；谶@種估計方法，設(shè)計了一實(shí)現(xiàn)二維移動物體跟蹤任務(wù)的無標(biāo)定手眼系統(tǒng)。仿真和實(shí)驗(yàn)都顯示了該方法對于圖像雅克比矩陣在線估計的有效性。下一步工作將改進(jìn)算法將其應(yīng)用到其他無標(biāo)定的手眼協(xié)調(diào)任務(wù)中。

［1］HU G，MacKunis W，Gans N，et al . Homography-based visual servo control with imperfect camera calibration［J］. Automatic Control，2009，54(6):1318-1324.

［2］ZHENG Yuan，PENG Silong.A practical roadside camera calibration method based on least squares optimization［J］.Intelligent Transportation Systems，2014，15(2):831-843.

［3］HAO M，SUN Zengqi.A universal state-space approach to uncalibrated model-free visual servoing［J］. Mechatronics，2012，17(5):833-846.

［4］Goncalves P J S，Mendonca L F，Sousa J M C，et al. Uncalibrated eye-to-hand visual servoing using inverse fuzzy models［J］.Fuzzy Systems，2008，16(2):341-353.

［5］Serra P，Cunha R，Hamel T，et al.Nonlinear image-based visual servo controller for the flare maneuver of fixed-wing aircraft using optical flow［J］.Control Systems Technology，2015，23(2):570-583.

［6］WANG Ying，LANG Haoxiang，De Silva C W，et al.A hybrid visual servo controller for robust grasping by wheeled mobile robots［J］.Mechatronics，2010，15(5):757-769.

［7］JIANG Ping，Bamforth L C A，F(xiàn)ENG Zuren，et al.Indirect iterative learning control for a discrete visual servo without a camerarobot model［J］.Systems，Man，and Cybernetics，Part B:Cybernetics，2007，37(4):863-876.

［8］Chien Chern Cheah，LI Xiang，YAN Xiao，et al.Observer-based optical manipulation of biological cells with robotic tweezers［J］.Robotics，2014，30(1):68-80.