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

劉 偉， 于振中 ， 惠 晶

(江南大學 輕工過程先進控制教育部重點實驗室，江蘇 無錫214122)

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

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

1 圖像雅克比矩陣

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

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

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

2 卡爾曼估計

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

其中

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

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

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

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

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

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

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

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

3 雅克比視覺伺服控制器

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

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

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

其中，Δpmax為機械手驅動器可提供的手爪最大移動速度。

整個無標定手眼協調系統的架構如圖1 所示。

圖1 無標定手眼協調系統Fig.1 Uncalibrated hand-eye coordination system

4 仿真與實驗

通過仿真與實驗驗證該圖像雅克比估計方法的性能和該控制方案的有效性。圖2 為實驗測試平臺。該平臺是由三自由度并聯delta 機械手和一個相機組成的手眼協調系統。實驗伺服任務為:驅動機械手，讓其手爪跟蹤一個二維隨機運動的物體。機械手與相機的關系和相機參數都是未知的。

圖2 實驗測試平臺Fig.2 Experiment testbed

4.1 仿 真

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

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

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

由圖3 可以看出，機械手能在跟蹤初期以手爪最大速度0.8 m/s 向目標接近，隨后能夠持續有效的對目標進行跟蹤。

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

綜上所述，此種非標定方法能夠實現機械手對目標的平穩跟蹤，從跟蹤路徑上分析系統的響應速度還有待提高;從誤差曲線上來看，雖然誤差實現了較好的收斂，但是誤差震蕩較大，這也有待提高。

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

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

圖5 對比軌跡Fig.5 Compared curves

4.2 實 驗

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

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

圖6 目標與機械手軌跡Fig.6 Tracks of the target and manipulator

5 結 語

提出基于卡爾曼濾波器的圖像雅克比在線估計算法，該算法對視覺反饋噪聲有很強的魯棒性。基于這種估計方法，設計了一實現二維移動物體跟蹤任務的無標定手眼系統。仿真和實驗都顯示了該方法對于圖像雅克比矩陣在線估計的有效性。下一步工作將改進算法將其應用到其他無標定的手眼協調任務中。

［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，FENG 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.