基于單應(yīng)性矩陣分解的視覺控制器設(shè)計(jì)及仿真

宗曉萍，李悅霞，劉偉東

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

基于單應(yīng)性矩陣分解的視覺控制器設(shè)計(jì)及仿真

宗曉萍，李悅霞，劉偉東

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

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

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

圖1 基于圖像的機(jī)器人視覺伺服結(jié)構(gòu)Fig.1 Structure of robot visual servo based on image

1 成像平面間的單應(yīng)性關(guān)系

當(dāng)所觀察的目標(biāo)特征點(diǎn)都在同一個(gè)平面上時(shí)，攝像機(jī)在不同時(shí)刻對(duì)該平面所成圖像間的關(guān)系就是直射變換，即單應(yīng)性（Homography）［5］.2個(gè)成像平面間的幾何關(guān)系如圖2所示，設(shè)空間一平面π，M為該平面上一點(diǎn)，n為平面π的單位法向量，F(xiàn)1，F(xiàn)2分別是攝像機(jī)處于當(dāng)前和期望位置時(shí)的攝像機(jī)坐標(biāo)系，π1，π2是相應(yīng)的成像平面.設(shè)點(diǎn)M在F1中的坐標(biāo)為χ＝［XCYCZC］，在成像平面π1中的齊次坐標(biāo)為m＝［xy1］.當(dāng)攝像機(jī)到達(dá)期望位置時(shí)，點(diǎn)M的攝像機(jī)坐標(biāo)、成像坐標(biāo)分別用χ＊，m＊表示.

圖2 2個(gè)成像平面間的幾何關(guān)系Fig.2 Geometric relationship between the two retinals

攝像機(jī)采用針孔模型，對(duì)焦距進(jìn)行歸一化，則攝像機(jī)坐標(biāo)與成像坐標(biāo)間的關(guān)系為

此時(shí)，設(shè)矩陣A是點(diǎn)M在2個(gè)成像平面上射影點(diǎn)間的單應(yīng)性矩陣，則

2 視覺控制器的設(shè)計(jì)

本文的視覺控制器正是利用成像平面間的單應(yīng)性關(guān)系，通過分解單應(yīng)性矩陣得到機(jī)器人的運(yùn)動(dòng)參數(shù)，建立起圖像特征點(diǎn)與機(jī)器人位姿之間的關(guān)系，從而完成視覺伺服任務(wù).

2.1 求解單應(yīng)性矩陣

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

對(duì)（3）式進(jìn)行整理，可得

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

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

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

2.2 分解單應(yīng)性矩陣

設(shè)d為平面π到當(dāng)前攝像機(jī)坐標(biāo)系原點(diǎn)處的距離，則平面π的方程為

設(shè)R為3×3的旋轉(zhuǎn)矩陣，t為3維的平移向量，它們描述2個(gè)攝像機(jī)坐標(biāo)系間的關(guān)系，則

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

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

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

由文獻(xiàn)［5］對(duì)單應(yīng)性矩陣進(jìn)行分解所得的2組解差別很大，事實(shí)上，只有其中正確的一組解才能使攝像機(jī)由當(dāng)前位置順利地到達(dá)期望位置［7］.

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

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

2.4 提取RPY角

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

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

3 仿真實(shí)驗(yàn)

3.1 構(gòu)建仿真模型

以Puma560機(jī)器人為例，利用Matlab中的機(jī)器人工具箱和機(jī)器視覺工具箱，在Simulink環(huán)境下創(chuàng)建sub－system來構(gòu)建視覺伺服系統(tǒng)仿真模型.基于單應(yīng)性矩陣分解的視覺伺服仿真結(jié)構(gòu)如圖3所示.

圖3 基于單應(yīng)性矩陣分解的視覺伺服系統(tǒng)仿真結(jié)構(gòu)Fig.3 Simulation structure of the visual servo system based on homography matrix decomposition

系統(tǒng)仿真模型由以下3個(gè)部分組成：

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

2）攝像機(jī)模型實(shí)現(xiàn)三維場(chǎng)景到二維場(chǎng)景的轉(zhuǎn)換.Camera模塊中uv＝camera（p，C，T），其中p為目標(biāo)特征點(diǎn)的3維世界坐標(biāo)，C為攝像機(jī)的內(nèi)參矩陣，T為攝像機(jī)坐標(biāo)系相對(duì)于世界坐標(biāo)系的變換矩陣，模塊輸出uv是目標(biāo)特征點(diǎn)在當(dāng)前攝像機(jī)成像平面內(nèi)的圖像像素坐標(biāo).

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

3.2 仿真實(shí)驗(yàn)驗(yàn)證

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

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

圖4 目標(biāo)特征點(diǎn)在圖像平面的移動(dòng)軌跡Fig.4 Movement trajectory of the target feature points in the image plane

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

4 結(jié)論

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

［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］ 宗曉萍.智能機(jī)器人視覺伺服系統(tǒng)研究［D］.保定：河北大學(xué)，2007.

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

［7］ 班建安.機(jī)器人混合視覺伺服控制研究［D］.西安：西安理工大學(xué)，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ī)器人視覺伺服系統(tǒng)仿真［J］.機(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.

（責(zé)任編輯：孟素蘭）

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

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

視覺伺服控制；單應(yīng)性矩陣；圖像插值；Puma560

10.3969／j.issn.1000－1565.2013.03.014

TP24

A

1000－1565（2013）03－0300－05

2012－12－20

國(guó)家自然科學(xué)基金資助項(xiàng)目（面上項(xiàng)目）（11271106）

宗曉萍（1964－），女，河北蔚縣人，河北大學(xué)教授，博士，主要從事混合動(dòng)態(tài)系統(tǒng)及智能控制方面的研究.

E－mail：zxp＠hbu.edu.cn