基于相機角度自適應的視覺伺服

唐明君，孔慶生

（復旦大學 電子工程系，上海200433）

0 引 言

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

以往大多數的視覺伺服控制主要集中在相機的路徑軌跡設計，而忽視了相機角度的旋轉因素。本文提出了基于給定軌跡的相機角度自適應調整視覺伺服算法。該算法通過建立相機坐標系和世界坐標系之間的映射關系，計算機器人運動軌跡上相近兩點相機方向的角度差，自適應的控制相機旋轉。因為加入了相機的自適應旋轉，能夠控制相機拍攝目標物體的特征點，使目標物體始終出現在相機的視野域中。

1 幾何模型

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

圖1 兩種不同相機的視野域與計算量對比

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

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

圖2 不同相機坐標系的坐標變換

圖2可以看出直角坐標系c′可以由直角坐標系c經過平移T和旋轉R得出。平移T使得c′的坐標原點和c的坐標原點重合，旋轉使得兩坐標系的xyz軸重合。可寫作方程 （1）

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

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

其中M∈R3×3是常數的，相機固有的，上三角可逆矩陣

在方程 （3）中，u0，v0∈R 是主點 （principal point）（例如圖象中心的像素點）在圖象空間中的坐標。α，β∈R分別表示相機的像素和焦距長度。∈R表示相機軸線之間的夾角。總之，M是一個與相機參數相關的常數矩陣，相機校準后，M即為已知條件。由于M可逆，所以ai可以由ei計算得到

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

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

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

2 控制模型設計

2.1 旋轉矩陣的極坐標表示

目前的機器人視覺伺服中，機器人靠內置的發動機控制相機的轉動，而發動機的輸入參數是相機需要旋轉的角速度 （時間Δt內變化的角度），故計算出旋轉矩陣R后使用極坐標表示，便于計算相機旋轉的角速度。

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

由于旋轉矩陣R只包含角度信息，故傳統極坐標中的極半徑r可以忽略，如方程 （8）

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

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

圖3 R的極坐標表示

考慮平面機器人始終在同一平面運動，可以忽略垂直角度θ2的調整，只需要對角度θ1的進行自適應調整。可表示如方程

其中θ0∈ ［－π），是個常數，通常等于0。

因此R和θ1存在一一對應的關系。θ1可以由R表示為方程 （10）

這個簡化使得平面機器人的相機只需要在一個維度上進行旋轉，極大地減小了機器人控制發動機的難度，同時也提高了旋轉精度，降低了計算的復雜度。根據上述R的極坐標表示法，可以設計角度自適應的機器人視覺伺服模型。

2.2 開環誤差模型

在上述機器人控制相機旋轉的模型基礎上，可以建立相應的相機角度自動調整的開環誤差視覺伺服模型。開環系統因為不需要構建輸出到輸入的反饋回路，使得系統的設計更加簡單，計算的復雜度也會降低，符合機器人視覺伺服對實時性的要求。但系統的穩定性也可能隨之減小，需要仔細地論證，本文將在之后討論該系統的穩定性。

圖4 開環系統及穩定性分析

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

式中：Δt——機器人從位置c移動到c′的時間，不妨設Δt為機器人調整發動機的單位時間。設ω＝ ［x0，y0，z0］T，其中＝1，則可以計算出ω′＝ωR。歸一化后記作ω′ ＝ ［x′0，y′0，z′0］T，其中x＝1。由此可以計算出在Δt時間內需要轉過的角度

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

2.3 穩定性

在開環誤差系統中，如果誤差無界或者累積誤差不收斂，則系統的穩定性很差，嚴重時，可能給系統帶來災難性的崩潰。本文提出的開環誤差系統誤差有界且不累積，所以穩定性較好。

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

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

3 實驗結果

在Matlab中進行機器人視覺伺服控制算法的仿真。試驗中使用的校準相機參數矩陣值為

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

4 結束語

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

：

［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］.計算機工程與設計，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.