基于相機(jī)角度自適應(yīng)的視覺伺服

唐明君，孔慶生

（復(fù)旦大學(xué) 電子工程系，上海200433）

0 引 言

視覺伺服是一種通過采集真實(shí)物體的圖像，利用圖像反饋的信息來控制或調(diào)整機(jī)器人的方法。一般來說，視覺伺服控制主要可以分為3類：基于圖像的 （imagebased）［1，2］，基于位置的 （position－based）［3，4］和混合類型的（combined）［5，6］。但是以上3種類型的視覺伺服都離不開大量的圖像處理，因此圖像處理的速度成為限制視覺伺服系統(tǒng)實(shí)時(shí)性的最大瓶頸［7］。同時(shí)，裝置傳統(tǒng)相機(jī)的機(jī)器人由于視野域有限，可能丟失目標(biāo)物體的特征點(diǎn)。為解決機(jī)器人裝置傳統(tǒng)相機(jī)角度固定造成的視野域有限的問題，參考文獻(xiàn) ［8，9］提出了采用全景相機(jī)的視覺伺服方法，然而視野的擴(kuò)大也帶來圖像處理時(shí)計(jì)算量的成倍增加［10］。

以往大多數(shù)的視覺伺服控制主要集中在相機(jī)的路徑軌跡設(shè)計(jì)，而忽視了相機(jī)角度的旋轉(zhuǎn)因素。本文提出了基于給定軌跡的相機(jī)角度自適應(yīng)調(diào)整視覺伺服算法。該算法通過建立相機(jī)坐標(biāo)系和世界坐標(biāo)系之間的映射關(guān)系，計(jì)算機(jī)器人運(yùn)動軌跡上相近兩點(diǎn)相機(jī)方向的角度差，自適應(yīng)的控制相機(jī)旋轉(zhuǎn)。因?yàn)榧尤肓讼鄼C(jī)的自適應(yīng)旋轉(zhuǎn)，能夠控制相機(jī)拍攝目標(biāo)物體的特征點(diǎn)，使目標(biāo)物體始終出現(xiàn)在相機(jī)的視野域中。

1 幾何模型

視覺伺服類似于人類通過眼睛和大腦來獲取和處理圖像信息，然后控制身體運(yùn)動。隨著數(shù)字信號理論的發(fā)展和計(jì)算機(jī)硬件后摩爾時(shí)代的到來，成倍增長的計(jì)算能力和成本的降低使得通過計(jì)算機(jī)對視覺信號進(jìn)行實(shí)時(shí)處理成為可能，這也推動了基于圖像的視覺伺服的發(fā)展。采用全景相機(jī)的視覺伺服方法，雖然解決了機(jī)器人搭載固定傳統(tǒng)相機(jī)造成的視野域有限問題，但隨著視野的擴(kuò)大，相應(yīng)也帶來了圖像處理時(shí)計(jì)算量的增長。如圖1所示，假設(shè)圖像的清晰度一致，傳統(tǒng)相機(jī)的視野角度為α，則采用全景相機(jī)的圖像處理復(fù)雜度大致為傳統(tǒng)相機(jī)的360／α（圖像的高度相同，但寬度不同），但它們的有效觀測域幾乎相同。而基于圖像和混合控制的視覺伺服計(jì)算量主要集中于圖像處理模塊，因此，減小圖像的輸入規(guī)模就能有效的提高視覺伺服控制的響應(yīng)速度。本文采用可自由控制相機(jī)轉(zhuǎn)動的角度自適應(yīng)算法，可以顯著降低計(jì)算復(fù)雜度，提高機(jī)器人移動控制的實(shí)時(shí)性，而且有限視野域的自由轉(zhuǎn)動模型更接近人類處理視覺信息的過程。

圖1 兩種不同相機(jī)的視野域與計(jì)算量對比

類似于人類雙眼的雙目視覺伺服采用有角度差的兩幅對特征點(diǎn)的圖像作為輸入信號，使用視覺差原理重建三維環(huán)境和目標(biāo)物體的位置和幾何信息。這兩幅圖像可以通過兩個(gè)不同角度的攝像機(jī)或者單個(gè)相機(jī)在不同位置對同一特征點(diǎn)拍攝得到。本文采用角度可調(diào)的單目相機(jī)，假定相機(jī)的旋轉(zhuǎn)和機(jī)器人的平移獨(dú)立控制，不相互關(guān)聯(lián)的。

不失一般性，假定每個(gè)被觀測的物體有4個(gè)共面但不共線的特征點(diǎn)Oi（i＝1，2，3，4）［11］。如圖2所示，假設(shè)機(jī)器人初始位置在c，以c為坐標(biāo)原點(diǎn)建立直角坐標(biāo)系，則特征點(diǎn)Oi在歐氏空間c中的坐標(biāo)為ai＝ （aix，aiy，aiz）T；在短暫的時(shí)間Δt后，機(jī)器人位置在c′，以c′為坐標(biāo)原點(diǎn)建立直角坐標(biāo)系，則特征點(diǎn)Oi在歐氏空間c′中的坐標(biāo)為a′i＝ （a′ix，a′iy，a′iz）T。Δt的時(shí)間足夠短，可以假定特征點(diǎn)Oi的位置信息不變。我們可以通過Oi在兩坐標(biāo)系中的坐標(biāo)的關(guān)系來計(jì)算坐標(biāo)系的變換，包括平移和旋轉(zhuǎn)。

圖2 不同相機(jī)坐標(biāo)系的坐標(biāo)變換

圖2可以看出直角坐標(biāo)系c′可以由直角坐標(biāo)系c經(jīng)過平移T和旋轉(zhuǎn)R得出。平移T使得c′的坐標(biāo)原點(diǎn)和c的坐標(biāo)原點(diǎn)重合，旋轉(zhuǎn)使得兩坐標(biāo)系的xyz軸重合?？蓪懽鞣匠?（1）

其中R∈R3×3是一個(gè)3×3的矩陣，T∈R3是一個(gè)三維向量。

ai是Oi在以相機(jī)位置為坐標(biāo)原點(diǎn)建立的世界坐標(biāo)系中的坐標(biāo)，是真實(shí)的三維空間的距離，ei是Oi在相機(jī)坐標(biāo)系中的坐標(biāo)，是圖像測得的距離，可以通過相機(jī)采集的圖像計(jì)算得出，也就是視覺伺服中控制模塊的輸入信息。真實(shí)距離ai和圖像像素距離ei之間存在一個(gè)全局的可逆對應(yīng)關(guān)系［12］：

其中M∈R3×3是常數(shù)的，相機(jī)固有的，上三角可逆矩陣

在方程 （3）中，u0，v0∈R 是主點(diǎn) （principal point）（例如圖象中心的像素點(diǎn)）在圖象空間中的坐標(biāo)。α，β∈R分別表示相機(jī)的像素和焦距長度?！蔙表示相機(jī)軸線之間的夾角。總之，M是一個(gè)與相機(jī)參數(shù)相關(guān)的常數(shù)矩陣，相機(jī)校準(zhǔn)后，M即為已知條件。由于M可逆，所以ai可以由ei計(jì)算得到

由方程 （1）（2）（3）可得

設(shè)E ＝ ［e1，e2，e3，e4］T，E＇＝ ［e′1，e′2，e′3，e′4］T，其中E，E′∈R4×3由 （5）可得

由于 （ei，e′i）可以由相機(jī)采集，視為已知，而按照假設(shè)機(jī)器人的路徑已經(jīng)規(guī)劃好，即T已知，求R，這是一個(gè)超定方程。Oi（i＝1，2，3，4）是4個(gè)共面但不共線的特征點(diǎn)，由三點(diǎn)決定一個(gè)平面得出O4可以用O1，O2，O3線性表示，即矩陣E的秩為3，所以方程 （6）一定有解或最小二乘解。但由于測量誤差，方程 （6）可能沒有精確解。采用最小二乘解作為方程的近似解

2 控制模型設(shè)計(jì)

2.1 旋轉(zhuǎn)矩陣的極坐標(biāo)表示

目前的機(jī)器人視覺伺服中，機(jī)器人靠內(nèi)置的發(fā)動機(jī)控制相機(jī)的轉(zhuǎn)動，而發(fā)動機(jī)的輸入?yún)?shù)是相機(jī)需要旋轉(zhuǎn)的角速度 （時(shí)間Δt內(nèi)變化的角度），故計(jì)算出旋轉(zhuǎn)矩陣R后使用極坐標(biāo)表示，便于計(jì)算相機(jī)旋轉(zhuǎn)的角速度。

不妨設(shè)相機(jī)的初始方向與x軸單位向量n的方向一致，經(jīng)過旋轉(zhuǎn)相機(jī)的目標(biāo)方向op的方向一致。由極坐標(biāo)的知識可知，三維歐幾里德空間R3中的任意點(diǎn)都可以由三維坐標(biāo)（θ1，θ2，r）表示出。其中θ1∈ ［－π，π），θ2∈ ［－），r∈［0，＋∞）。旋轉(zhuǎn)矩陣R作用于向量n的效果等同于將向量n先進(jìn)行水平旋轉(zhuǎn)θ1度，再進(jìn)行垂直旋轉(zhuǎn)θ2度。

由于旋轉(zhuǎn)矩陣R只包含角度信息，故傳統(tǒng)極坐標(biāo)中的極半徑r可以忽略，如方程 （8）

其中θ1∈ ［－π，π），θ2∈ ［－）。

如圖3所示，可以計(jì)算出旋轉(zhuǎn)的角度θ1和θ2，其中op是與x軸同向的單位向量（1，0，0）T經(jīng)過旋轉(zhuǎn)矩陣R的作用后得到的，即op＝（1，0，0）TR。p′是點(diǎn)p在xoy平面的投影，θ1是op′和x軸的夾角，即θ1＝∠p′ox。θ2是op′和op的夾角，即θ2＝ ∠p′op。

圖3 R的極坐標(biāo)表示

考慮平面機(jī)器人始終在同一平面運(yùn)動，可以忽略垂直角度θ2的調(diào)整，只需要對角度θ1的進(jìn)行自適應(yīng)調(diào)整?？杀硎救绶匠?/p>

其中θ0∈ ［－π），是個(gè)常數(shù)，通常等于0。

因此R和θ1存在一一對應(yīng)的關(guān)系。θ1可以由R表示為方程 （10）

這個(gè)簡化使得平面機(jī)器人的相機(jī)只需要在一個(gè)維度上進(jìn)行旋轉(zhuǎn)，極大地減小了機(jī)器人控制發(fā)動機(jī)的難度，同時(shí)也提高了旋轉(zhuǎn)精度，降低了計(jì)算的復(fù)雜度。根據(jù)上述R的極坐標(biāo)表示法，可以設(shè)計(jì)角度自適應(yīng)的機(jī)器人視覺伺服模型。

2.2 開環(huán)誤差模型

在上述機(jī)器人控制相機(jī)旋轉(zhuǎn)的模型基礎(chǔ)上，可以建立相應(yīng)的相機(jī)角度自動調(diào)整的開環(huán)誤差視覺伺服模型。開環(huán)系統(tǒng)因?yàn)椴恍枰獦?gòu)建輸出到輸入的反饋回路，使得系統(tǒng)的設(shè)計(jì)更加簡單，計(jì)算的復(fù)雜度也會降低，符合機(jī)器人視覺伺服對實(shí)時(shí)性的要求。但系統(tǒng)的穩(wěn)定性也可能隨之減小，需要仔細(xì)地論證，本文將在之后討論該系統(tǒng)的穩(wěn)定性。

圖4 開環(huán)系統(tǒng)及穩(wěn)定性分析

如圖4所示，假定ω∈R3是初始位置c的相機(jī)的方向向量，ω′∈R3是機(jī)器人移動后的位置期望c′的相機(jī)方向向量，角速度可定義為方程 （11）

式中：Δt——機(jī)器人從位置c移動到c′的時(shí)間，不妨設(shè)Δt為機(jī)器人調(diào)整發(fā)動機(jī)的單位時(shí)間。設(shè)ω＝ ［x0，y0，z0］T，其中＝1，則可以計(jì)算出ω′＝ωR。歸一化后記作ω′ ＝ ［x′0，y′0，z′0］T，其中x＝1。由此可以計(jì)算出在Δt時(shí)間內(nèi)需要轉(zhuǎn)過的角度

方程 （12）中，x0和y0是初始位置的方向信息，為已知量。計(jì)算x′0和y′0需要用到的矩陣R可以由方程 （7）計(jì)算得出。因此，可以計(jì)算出機(jī)器人需要旋轉(zhuǎn)的相機(jī)角度Δθ1。

2.3 穩(wěn)定性

在開環(huán)誤差系統(tǒng)中，如果誤差無界或者累積誤差不收斂，則系統(tǒng)的穩(wěn)定性很差，嚴(yán)重時(shí)，可能給系統(tǒng)帶來災(zāi)難性的崩潰。本文提出的開環(huán)誤差系統(tǒng)誤差有界且不累積，所以穩(wěn)定性較好。

如圖4所示，期望的旋轉(zhuǎn)后的方向向量ω′和旋轉(zhuǎn)后實(shí)際的方向向量Rω存在一個(gè)誤差值，這是由測量誤差造成的，可以表示為方程 （13）

根據(jù)測量誤差有界和最小二乘法可以得出珘ω也是有界的。由方程 （12）可知，相應(yīng)的轉(zhuǎn)化成旋轉(zhuǎn)角度誤差珓θ也是有界的。系統(tǒng)根據(jù)實(shí)時(shí)圖像計(jì)算旋轉(zhuǎn)角度，誤差并不累積。所以累積誤差可記為珓θΣ＝Σ珓θ＝珓θn，珓θn為最后一次調(diào)整的誤差。當(dāng)t→∞時(shí)，珓θ（t）＜θM。θM由相機(jī)的參數(shù)決定的測量誤差等決定。

3 實(shí)驗(yàn)結(jié)果

在Matlab中進(jìn)行機(jī)器人視覺伺服控制算法的仿真。試驗(yàn)中使用的校準(zhǔn)相機(jī)參數(shù)矩陣值為

圖5為目標(biāo)物物體在機(jī)器人相機(jī)圖像空間中的運(yùn)動軌跡圖。o為開始位置，＊為結(jié)束位置。圖6為機(jī)器人相機(jī)方向向量表示為極坐標(biāo)后θ1隨時(shí)間變化的曲線圖像，初始角度設(shè)為0°。圖7為期望調(diào)整角度與實(shí)際調(diào)整角度的誤差珓θ隨時(shí)間變化曲線圖。從圖像上可以看出珓θ＜5°，且不隨時(shí)間累積。

4 結(jié)束語

本文采用機(jī)器人搭載的校準(zhǔn)相機(jī)采集的圖像作為輸入，提出了相機(jī)角度自適應(yīng)調(diào)整的視覺伺服算法，能夠有效地解決移動機(jī)器人在視覺伺服控制過程中的視野域問題。由于采用了自適應(yīng)的方法來調(diào)整傳統(tǒng)相機(jī)的方向，而不同于全景相機(jī)增大輸入圖像的規(guī)模，因此既能保證目標(biāo)物體始終處于相機(jī)的有效視野域，又不會增加圖像處理的計(jì)算量，提高了視覺伺服控制系統(tǒng)的實(shí)時(shí)性。同時(shí)，由于系統(tǒng)誤差不累積，使得系統(tǒng)能有較好的穩(wěn)定性。

：

［1］Lippiello V，Sieiliano B Position.Based visual servoing in industrial multirobot cells using a hybrid camera configuration ［J］.IEEE Trans Robot，2007，23 （1）：73－86.

［2］Heitzmann T，Doignon C，Albitar C，et al.Position based visual servoing using a coded structured light sensor ［C］／／IEEE Conf Robotic and Sensors Environments，2008：126－131.

［3］Chaumette F，Hutchinson S，Visual servo control I.Basic approaches ［M］.IEEE Robot Autom，2006，13 （4）：82－90.

［4］Mariottini G L，Oriolo G，Prattiehizzo D.Image based visual servoing for nonholonomic mobile robots using eplpolar geometry ［J］.IEEE Trans Robot，2007，23 （1）：87－100.

［5］HU Guoqiang，Nicholas Gans，Norman Fitz Coy，et al.Adaptive homography－based visual servo tracking control via a quaternion formulation ［J］.IEEE Trans Controls Systems Technology，2010，18 （1）：128－135.

［6］Heitzmann T，Doignon C，Albitar C，et al.Position based visual servoing using a coded structured light sensor ［C］／／Robotic and Sensors Environments，2008：126－131.

［7］Novotny P M，Stoll J A，Dupont P E，et al.Real time visual servoing of a robot using three dimensional ultrasound ［C］／／IEEE Conf Robotics and Automation，2007：2655－2660.

［8］Hadj Abdelkader H，Mezouar Y，Martinet P，et al.Catadioptric visual servoing from 3Dstraight lines ［J］.IEEE Trans Robotics，2008：652－665.

［9］YANG Shaoping，CHEN Xiong，KONG Qingsheng.Imagebased visual servoing for mobile robots with central panoramic camera ［J］.Computer Engineering and Design，2010，31（19）：4261－4264 （in Chinese）. ［楊少平，陳雄，孔慶生.采用全景相機(jī)的移動機(jī)器人視覺伺服 ［J］.計(jì)算機(jī)工程與設(shè)計(jì)，2010，31 （19）：4261－4264.］

［10］Tahri O，Araujo H.Non central catadioptric cameras visual servoing for mobile robots using a radial camera model ［C］／／Intelligent Robots and Systems，IEEE／RSJ International Conf，2012：1683－1688.

［11］Chaumette F，Hutchinson S.Visual servo control II.Advanced approaches［Tutorial］［C］／／Robotics ＆ Automation Magazine，IEEE，2007：109－118.

［12］Mariottini G L，Oriolo G，Prattichizzo D.Image based visual servoing for nonholonomic mobile robots using epipolar geometry ［J］.IEEE Trans Robotics，2007 （23）：87－100.