基于單應性矩陣分解的視覺控制器設計及仿真

宗曉萍，李悅霞，劉偉東

（河北大學電子信息工程學院，河北保定 071002）

基于單應性矩陣分解的視覺控制器設計及仿真

宗曉萍，李悅霞，劉偉東

（河北大學電子信息工程學院，河北保定 071002）

機器人視覺伺服是指利用視覺傳感器得到的圖像信息，構造機器人的位置閉環反饋，從而實現機器人的定位控制或者軌跡跟蹤.根據視覺反饋信息的類型，機器人視覺伺服分為基于位置的視覺伺服控制和基于圖像的視覺伺服控制［1］.隨著圖像雅克比矩陣的引入，基于圖像的視覺伺服控制方法（IBVS）在20世紀80年代中期興起.該方法計算高效，并且對空間模型和攝像機標定誤差具有魯棒性.繼而研究者提出了各種各樣對圖像雅克比矩陣進行線性化的方法［2］，但這些研究僅限于機器人末端執行器或目標做微小運動的情況.同時，Chaumette等提出了若干不能應用傳統局部線性化方法來解決的視覺伺服控制問題［3］.隨后Ezio Malis等提出了基于單應性矩陣分解的視覺伺服控制方法［4］，許多學者就單應性矩陣的分解算法進行研究［5］.

本文視覺伺服系統采用雙環動態look－and－move的控制方式［1］，基于圖像的機器人視覺伺服系統結構如圖1所示，攝像機采用單目眼在手的安裝方式（eye－in－hand），內環為關節伺服控制，外環為視覺控制.視覺控制器采用應用廣泛的Faugera和Lustman的單應性矩陣分解方法，但該方法最終會得到2組不同的解［5］，而只有1組解能夠使得機器人完成視覺伺服任務.在此，采用中間圖像插值方法，根據攝像機位姿間的關系對分解的結果進行篩選，從而得到正確的解，進而提取RPY角得到機器人的控制量，并通過實驗驗證了該方法的有效性.

圖1 基于圖像的機器人視覺伺服結構Fig.1 Structure of robot visual servo based on image

1 成像平面間的單應性關系

當所觀察的目標特征點都在同一個平面上時，攝像機在不同時刻對該平面所成圖像間的關系就是直射變換，即單應性（Homography）［5］.2個成像平面間的幾何關系如圖2所示，設空間一平面π，M為該平面上一點，n為平面π的單位法向量，F1，F2分別是攝像機處于當前和期望位置時的攝像機坐標系，π1，π2是相應的成像平面.設點M在F1中的坐標為χ＝［XCYCZC］，在成像平面π1中的齊次坐標為m＝［xy1］.當攝像機到達期望位置時，點M的攝像機坐標、成像坐標分別用χ＊，m＊表示.

圖2 2個成像平面間的幾何關系Fig.2 Geometric relationship between the two retinals

攝像機采用針孔模型，對焦距進行歸一化，則攝像機坐標與成像坐標間的關系為

此時，設矩陣A是點M在2個成像平面上射影點間的單應性矩陣，則

2 視覺控制器的設計

本文的視覺控制器正是利用成像平面間的單應性關系，通過分解單應性矩陣得到機器人的運動參數，建立起圖像特征點與機器人位姿之間的關系，從而完成視覺伺服任務.

2.1 求解單應性矩陣

至少需要4對相互獨立的射影點才能唯一確定一個直射變換.由于成像坐標采用第3個元素為1的齊次坐標，因而，令（2）式中A的元素a33＝1，以第1對射影匹配點為例［6］，代入（2）式，可得

對（3）式進行整理，可得

對于2個成像平面中的每對匹配點，都能得到一組如（4）式所示的方程.4對獨立的射影點就能得到含有8個未知量aij（i＝1，2，3，j＝1，2，3，除a33以外）的線性方程組，如（5）式所示.

如果問一問和今天的教育相匹配的詞語是“快樂”還是“沉重”，是“成長”還是“升學”，我想大多數人都會選擇后者。是的，正是現行的教育，讓無數個家庭、家長和孩子面臨沉重的壓力：擇校壓力、升學壓力、考試壓力、分數壓力……并因此焦慮愁苦、憂心如焚。正是這樣的教育，讓社會上充斥著各種追名逐利的辦學機構，培優班門庭若市，小考、中考、高考輔導班擠破腦袋……正是這樣的教育，讓學校里滿是學生肩膀上輕不下來的書包，教師臉上揮之不去的愁容，校長心里落不下地的石頭……顯然，這樣的教育，只會讓無數人指責和詬病，呼吁和請求，不能讓人滿意。

通過求解線性方程組（5）式，就能求得單應性矩陣A.

2.2 分解單應性矩陣

設d為平面π到當前攝像機坐標系原點處的距離，則平面π的方程為

設R為3×3的旋轉矩陣，t為3維的平移向量，它們描述2個攝像機坐標系間的關系，則

由（6）式和（7）式，得

由（1）式知，χ和χ＊分別是m和m＊的射影坐標，從而可得

根據文獻［5］，分解單應性矩陣A，從而得到攝像機運動和特征點所在平面的參數，即R，t，n和d.一般情況下，方程（9）可得到8組解，然后通過攝像機可見性進行約束，篩選得到2組可以物理實現的解［5］.

由文獻［5］對單應性矩陣進行分解所得的2組解差別很大，事實上，只有其中正確的一組解才能使攝像機由當前位置順利地到達期望位置［7］.

采用中間圖像插值的方法對分解結果進行進一步地篩選，即：給出當攝像機處于當前位置與期望位置間的一個中間位置時對特征點所成的圖像，設π3是攝像機在中間位置時的成像平面，點M在該平面的成像坐標為m′＝［x′y′1］.同時設中間攝像機坐標系相對于期望攝像機坐標系的旋轉矩陣為DRM，當前攝像機坐標系相對于中間攝像機坐標系和期望攝像機坐標系的旋轉矩陣分別為MRC和DRC.由于攝像機的期望位置是一定的，而所插入的中間位置由自己選定，因而DRM為已知量.這時利用m和m′，m和m＊能夠計算出2個相應的單應性矩陣，并且按照文獻［5］對其進行分解，得到MRC和DRC各2組解，它們需滿足攝像機位置之間的關系

通過排列組合的形式能夠找出滿足（10）式的唯一解，以完成視覺伺服任務.

2.4 提取RPY角

選出正確的解后，需要從旋轉矩陣R中提取出滾動角φr（roll）、俯仰角φp（pitch）和偏航角φy（yaw）.點M由當前攝像機坐標到期望攝像機坐標的3×3旋轉矩陣為R，設以RPY角表示的旋轉矩陣為Q＝Rot（x，φr）Rot（y，φr）Rot（z，φy），如果只考慮旋轉運動，可知R＝Q，則

控制器的輸出部分：以單應性矩陣分解結果中的t作為攝像機由當前位置到達期望位置的平移速度，即T＝［TxTyTz］＝t，使用R中提取的RPY角作為角速度［8］，即Ω＝［ωxωyωz］＝［φrφpφy］，因而可得到視覺控制器的控制輸出為y＝k［TxTyTzωxωyωz］，k為增益系數.

3 仿真實驗

3.1 構建仿真模型

以Puma560機器人為例，利用Matlab中的機器人工具箱和機器視覺工具箱，在Simulink環境下創建sub－system來構建視覺伺服系統仿真模型.基于單應性矩陣分解的視覺伺服仿真結構如圖3所示.

圖3 基于單應性矩陣分解的視覺伺服系統仿真結構Fig.3 Simulation structure of the visual servo system based on homography matrix decomposition

系統仿真模型由以下3個部分組成：

1）機器人部分由jacobn模塊、ijacob模塊、相乘（Product）模塊、積分（Integrator）模塊和fkine模塊組成［9］.jacobn模塊中J＝jacobn（robot，q）可以求得機器人雅克比矩陣，其中robot選用6自由度的關節式機器人Puma560，q為機器人的關節角向量.ijacob模塊完成矩陣求逆運算.相乘模塊實現q·＝J－1p·，p為機器人的位姿，從而得到關節角的變化量q·.然后通過T＝fkine（robot，q）求得機器人總的變換陣.

2）攝像機模型實現三維場景到二維場景的轉換.Camera模塊中uv＝camera（p，C，T），其中p為目標特征點的3維世界坐標，C為攝像機的內參矩陣，T為攝像機坐標系相對于世界坐標系的變換矩陣，模塊輸出uv是目標特征點在當前攝像機成像平面內的圖像像素坐標.

3）視覺控制器由單應性控制器和比例環節構成.單應性控制器采用子系統封裝的形式，首先根據攝像機針孔模型完成特征點像素坐標到成像坐標的轉換，然后，應用該文中所提出的方法求解并分解單應性矩陣，應用中間圖像插值篩選出正確的一組解，分別輸出平移向量t和旋轉矩陣R，最后根據公式（11）從旋轉矩陣R中提取RPY角.

3.2 仿真實驗驗證

本實驗以長方體為跟蹤目標，選取長方體上表面的4個角點作為特征點進行定位.這4個特征點的世界坐標分別為［2，0.3，0.2］m，［2，0.3，－0.2］m，［2，－0.3，－0.2］m，［2，－0.3，0.2］m.攝像機配置如下：焦距為8mm，物理感光陣列為6.4mm×6.4mm，像素為512×512，其成像平面的原點在圖像平面的坐標為［256，256］pixel.目標特征點的期望像素坐標為［256，406］pixel，［456，406］pixel，［456，106］pixel，［256，106］pixel.

目標特征點在圖像平面的移動軌跡如圖4所示，其中‘＊’表示特征點的起始位置，‘＋’表示特征點的期望位置，‘o’表示特征點實際到達的位置，可以看出采用基于單應性矩陣分解的視覺控制器能夠使目標特征點成像于期望位置.圖5表明目標特征點的像素誤差逐漸趨近于零，說明采用該視覺控制器能夠順利地完成機器人的視覺伺服任務.

圖4 目標特征點在圖像平面的移動軌跡Fig.4 Movement trajectory of the target feature points in the image plane

圖5 目標特征點的圖像像素誤差的均方根曲線Fig.5 Root mean square curve of image pixel error for the target feature points

4 結論

設計了一種基于單應性矩陣分解的視覺伺服控制器，充分利用了攝像機在不同時刻對目標物體所成圖像間的單應性關系，通過分解單應性矩陣，進而得到機器人的運動參數.該方法避免了圖像深度信息的求取，而且它能夠滿足機器人的一般運動，因而在一定程度上拓寬了機器人的應用范圍.最后，仿真實驗表明，應用該方法設計視覺控制器，目標特征點的圖像像素誤差趨近于零，機器人能夠快速準確地到達期望位置.

［1］ HUTCHINSON S，HAGER G D，CORKE P I.A tutorial on visual servo control［J］.IEEE Trans on Robotics and Automation，1996，12（5）：651－670.

［2］ PAPANIKOLOPOULOS N P，KHOSLA P K，KANADE T.Visual tracking of a moving target by a camera mounted on a robot：a combination of vision and control［J］.IEEE Trans on Robotics and Automation，1993，9（1）：14－35.

［3］ CHAUMETTE F.Potential problems of stability and convergence in image－based and position－based visual servoing［C］／／KRIEGMAN D，HAGER G，MORSE S，et al.The confluence of vision and control，Lecture Notes in Cont and lnfo Sci，Berlin：Springer－Verlag，1998：66－78.

［4］ MALIS E，CHAUMETTE F，BOUDET S.2－1／2Dvisual servoing［J］.IEEE Trans on Robotics and Automation，1999，15（2）：238－250.

［5］ FAUGERAS O，LUSTMAN F.Motion and structure from motion in apiecewise planar environment［J］.International Journal of Pattern Recognition and Artificial Intelligence，1988，2（3）：485－508.

［6］ 宗曉萍.智能機器人視覺伺服系統研究［D］.保定：河北大學，2007.

ZONG Xiaoping.Study of the visual servoing system on intelligent robot［D］.Baoding：Heibei University，2007.

［7］ 班建安.機器人混合視覺伺服控制研究［D］.西安：西安理工大學，2009.

BAN Jianan.Research on robot hybrid visual servoing control［D］.Xi'an：Xi'an University of Technology，2009.

［8］ GANS N，HUTCHINSON S.A switching approach to visual servo control［J］.Proceedings of the 2002IEEE International Symposium on Intelligent Control，2002，2002：770－776.

［9］ 宗曉萍，淮小利，王培光.基于圖像的PUMA560機器人視覺伺服系統仿真［J］.機床與液壓，2007，35（10）：161－164.

ZONG Xiaoping，HUAI Xiaoli，WANG Peiguang.Simulation of image－based visual servoing system for PUMA560robot［J］.Machine Tool ＆Hydraulics，2007，35（10）：161－164.

（責任編輯：孟素蘭）

Design and simulation of visual controller based on homography matrix decomposition

ZONG Xiaoping，Ll Yuexia，LlU Weidong
（College of Electronic and Informational Engineering，Hebei University，Baoding 071002，China）

In order to avoid solving the depth information in the traditional visual servo control method based on image－jacobian matrix，and overcome the constraint involved only small motions of robot end－effector，a visual controller based on homography matrix decomposition was designed in this paper.According to the decomposition of homography matrix between the two retinas when a camera forms the image of aplane at different time，and the screening method of intermediate image interpolation，the visual controller parameters were determined.Finally，the system simulation for Puma560was implemented in the Matlab／Simulink，and the experimental results verified the validity of the method.

visual servo control；homography matrix；image interpolation；Puma560

為避免傳統的基于圖像雅克比矩陣視覺伺服控制方法中深度信息的求取，克服機械手末端僅限于做微小運動的約束，設計了基于單應性矩陣分解的視覺控制器.根據對攝像機在不同時刻對同一平面所成圖像間的單應性矩陣的分解，并通過中間圖像插值篩選方法，確定其視覺控制器參數.最后，以Puma560機器人為例在Matlab／Simulink環境下進行系統仿真，其結果驗證了該方法的有效性.

視覺伺服控制；單應性矩陣；圖像插值；Puma560

10.3969／j.issn.1000－1565.2013.03.014

TP24

A

1000－1565（2013）03－0300－05

2012－12－20

國家自然科學基金資助項目（面上項目）（11271106）

宗曉萍（1964－），女，河北蔚縣人，河北大學教授，博士，主要從事混合動態系統及智能控制方面的研究.

E－mail：zxp＠hbu.edu.cn