基于仿真機器人的雙目視覺定位*

周子嘯 ，趙曉林 ，胡 峰 ，張 利

（清華大學 a.微納電子系；b.電子工程系，北京 100084）

1 引言

計算機視覺是實現機器人人工智能的關鍵技術之一。很多視覺系統中，需要獲得目標物體的位置信息，目標定位是計算機視覺的一個重要功能。借助圖像信息的目標定位中很重要的一個實現方法是模仿人眼的視覺功能的雙目視覺定位。

Marr教授首先提出了一種雙目視覺計算方法，奠定了雙目立體視覺發展的理論基礎。雙目視覺直接模擬人類雙眼處理景物的方式，可靠簡便，在許多領域極具應用價值。日本研制了一種自適應雙目視覺伺服系統，利用雙目立體視覺原理，實現對運動方式未知目標的自適應跟蹤；日本東京大學將實時雙目立體視覺和機器人整體姿態信息集成，開發了仿真機器人動態導航系統[1]。哈爾濱工業大學采用異構雙目活動視覺系統實現了全自主足球機器人導航。火星“863”計劃課題《人體三維尺寸的非接觸測量》，采用雙視點投影光柵三維測量原理，由雙攝像機獲取圖像對，通過計算機進行圖像數據處理，不僅可獲取服裝設計所需的特征尺寸，還可根據需要獲取人體圖像上任意一點的三維坐標[1]。

筆者在已搭建的實驗平臺上，實現了雙目視覺的定位功能，該機器人平臺的定位功能需求通過獲取圖像信息和機器人姿態信息，定位圖像中各個匹配目標點的坐標，使原有系統功能得到提升。

2 機器人仿真實驗平臺

2.1 硬件系統

本系統的實驗平臺采用導軌機器人平臺體制構建一個模擬仿真環境，通過搭建一個二維機器人導軌架結構，在獲取運動信息和云臺姿態的情況下，對機器人仿真環境下的目標進行檢測、識別、跟蹤和定位。該平臺機器人硬件部分如圖1所示。

其中，2個機械臂可在X方向上移動，在機械臂上分別有可在Y方向上移動的導軌機器人。攝像機搭載在2個Y軸導軌懸掛的高精度數控云臺吊艙上，伺服電機驅動吊艙沿橫向Y導軌運動，提供了攝像機的Y方向運動自由度。高精度數控云臺能實現攝像機繞豎直向上的Z軸轉動和在Z-Y平面、Z-X平面俯仰的精確運動，以仿真攝像機的各種拍攝姿態。機器人運動信息和云臺運動信息均可以通過采集卡傳輸到計算機控制系統，實現對機器人和云臺的精確控制。

2.2 軟件系統

本系統的軟件框架如圖2所示，在該系統中完成雙目定位功能模塊。

3 基本原理

3.1 參數定位法

基于仿真平臺的基本定位算法主要采用的是參數定位法和雙目視覺定位法。參數定位法的基本原理是通過獲取云臺姿態信息參數，然后解算出攝像機視軸與理想地球球面的交點作為定位結果，參數定位法原理如圖3所示。

其中，坐標系D，E，F表示通過解算求得的攝像機鏡頭坐標系，它的視場中某向量與地球表面的交點B即為定位結果。此參數定位法的優點是算法實現簡單快速，但它是基于理想的地球橢球模型，如果需要更高精度的定位，需要有詳細的地理表面信息庫作為支持。本實驗平臺有2個導軌機器人，采用相似的定位算法，數據融合時，按它們距目標距離的權重作融合，并沒有更充分地利用圖像信息和2架云臺之間的關系信息。

為了更充分地利用2個導軌移動機器人的圖像信息，只要云臺不處于某些特定的姿態，如兩攝像頭的視軸正好平行（實際中只在極少情況下出現這種姿態），可以將兩攝像頭的圖像信息作雙目視覺定位，充分利用多機通信協同定位，更好地利用圖像信息，也很少需要地理信息庫等其他條件的支持。在實際系統應用中，可以使用參數定位法與雙目視覺法分別求解，為定位結果提供更多的參考基礎。

3.2 單目視覺原理

單目視覺的基本原理如圖4所示。

由攝像機針孔成像模型可以得到相似關系

式中：（x，y）為圖像中 P 點的坐標，（X，Y，Z）為空間中 P 點的坐標，f為鏡頭的焦距。在已知焦距f的情況下，空間中的點可以與圖像中的點的坐標值相互轉化。

由空間中的坐標點映射為圖像中坐標點的公式為

式（3）為式（2）的展開形式，矩陣A為攝像機的內參數矩陣，fx和fy分別表示徑向和軸向焦距，cx和cy表示主點（Principle Point）的坐標；矩陣R為攝像機鏡頭坐標系在世界坐標系旋轉矩陣，T為攝像機鏡頭坐標系在世界坐標系中的平移向量；W表示P點的世界坐標，在展開形式中以齊次坐標表示；R和T形成的矩陣稱為外參數矩陣，內參數矩陣與外參數矩陣的乘積定義為矩陣M。若同時有兩個攝像機的多個對應點，即可形成雙目視覺的情況，如圖5所示。

若該世界坐標系中的同一點P在左右兩攝像機中分別形成點P1和P2，則有映射方程

將式（4）、（5）分別展開并聯立方程

解此方程組即可求得世界坐標系下的坐標值。

4 算法實現

整個算法實現的流程如圖6所示。

圖6 算法實現流程

4.1 攝像機標定

求解攝像機鏡頭內參數與外參數的過程稱為攝像機標定。內參數是定位過程中求解必要的參數，在攝像機鏡頭焦距等內參數情況改變時必須重新進行，由于仿真平臺使用的是定焦鏡頭，所以每次調焦后只需進行一次標定即可。標定算法采用的是基于張正友標定算法[2]實現的Calibration Toolbox[3-4]。為了便于角點檢測，選取若干幅棋盤格平面圖進行標定，如圖7所示。

由于機器人攝像機是定焦鏡頭，內參數在鏡頭不變焦的情況下是不變的，所以由此標定算法求出的內參數將在機器人系統中作為初始配置參數。

由此標定算法求出的內參數與外參數均作為輸入參數完成一次仿真驗證，用以確定雙目定位算法的正確性，并且為機器人平臺調試提供參考真值。

4.2 Matlab仿真驗證

由Calibration Toolbox解算出的內參數與外參數進行仿真實驗。在多個視圖中分別手工依次標定多個目標點，本次實驗取的4個點是圖7中棋盤格外圈的4個內角點，然后將這16幅圖片依次作為左右視角圖片，進行雙目視覺定位解算，結果如圖8所示。

圖7中每一個小格是0.02 m，取左上角點為原點，則定位所得的結果坐標應為（0，0），（0，0.14），（0，0.1）和（0.14，0.10）4個值。每一個點由16幅圖兩兩定位，即每一個點有120個雙目定位結果，由圖8可以定性地看出結果的正確性，只有極少幾個點的值與定位結果相差較大。

4.3 仿真機器人平臺實現及試驗結果

已經實現的軟件可以完成目標的跟蹤、識別和基于參數法的定位功能。為了有效利用數據，實現多機通信的協同定位，本系統添加了雙目視覺定位模塊。該模塊主要包括雙目視覺算法子模塊與系統接口子模塊2個部分。

定位算法子模塊完成求解方程組以解算目標坐標值的功能；接口子模塊負責與其余模塊通信，獲取姿態參數，將姿態參數轉化成外參數矩陣，與初始配置的內參數矩陣一起作為定位的輸入參數。

在VC++開發環境下編寫的程序嵌入系統，完成該雙目定位功能，試驗結果如表1所示。

表1 定位坐標與真值

定位結果誤差如圖9所示。

由上述定位結果誤差分析可以看出，定位結果的解算精確度與定位點的位置有關。由于邊角點離圖像中心主點比較遠，畸變相對于處于圖像中心點的原點位置大，定位結果誤差較大，而且定位結果誤差主要來自Z軸方向的空間點計算誤差，由于Z軸方向的數值完全由攝像機鏡頭的焦距解算得到，圖像模糊畸變對于結果影響較大，若已知目標的水平高度，僅計算在X-Y平面內的偏離直線長度，屏蔽Z軸的誤差，誤差均在10 mm以內，定位結果的準確性明顯提升。

5 小結

介紹了雙目視覺的基本原理，基于此原理開發出在仿真機器人平臺上的雙目視覺定位功能，使基于參數法定位的原機器人系統的定位功能得到了提升。由定位結果可以看出，定位結果準確，并且有較高的精度。

[1]林琳.機器人雙目視覺定位技術研究[D].西安：西安電子科技大學,2009.

[2]ZHANG Z Y.Flexible camera calibration by viewing a plane from unknown orientations[C]//Proc.of IEEE International Conference on Computer Vision.Corfu， Greece:[s.n.]， 1999： 666-673.

[3]BOUGUET J Y.Camera calibration toolbox for Matlab[EB/OL].[2010-03-01].http：//www.vision.caltech.edu/bouguetj/calib_doc/.

[4]DAVID A F，JEAN P.Computer vision：a modern approach[M].New Jersey：Prentice Hall， 2004.