基于仿真機(jī)器人的雙目視覺(jué)定位*

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

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

1 引言

計(jì)算機(jī)視覺(jué)是實(shí)現(xiàn)機(jī)器人人工智能的關(guān)鍵技術(shù)之一。很多視覺(jué)系統(tǒng)中，需要獲得目標(biāo)物體的位置信息，目標(biāo)定位是計(jì)算機(jī)視覺(jué)的一個(gè)重要功能。借助圖像信息的目標(biāo)定位中很重要的一個(gè)實(shí)現(xiàn)方法是模仿人眼的視覺(jué)功能的雙目視覺(jué)定位。

Marr教授首先提出了一種雙目視覺(jué)計(jì)算方法，奠定了雙目立體視覺(jué)發(fā)展的理論基礎(chǔ)。雙目視覺(jué)直接模擬人類(lèi)雙眼處理景物的方式，可靠簡(jiǎn)便，在許多領(lǐng)域極具應(yīng)用價(jià)值。日本研制了一種自適應(yīng)雙目視覺(jué)伺服系統(tǒng)，利用雙目立體視覺(jué)原理，實(shí)現(xiàn)對(duì)運(yùn)動(dòng)方式未知目標(biāo)的自適應(yīng)跟蹤；日本東京大學(xué)將實(shí)時(shí)雙目立體視覺(jué)和機(jī)器人整體姿態(tài)信息集成，開(kāi)發(fā)了仿真機(jī)器人動(dòng)態(tài)導(dǎo)航系統(tǒng)[1]。哈爾濱工業(yè)大學(xué)采用異構(gòu)雙目活動(dòng)視覺(jué)系統(tǒng)實(shí)現(xiàn)了全自主足球機(jī)器人導(dǎo)航?；鹦恰?63”計(jì)劃課題《人體三維尺寸的非接觸測(cè)量》，采用雙視點(diǎn)投影光柵三維測(cè)量原理，由雙攝像機(jī)獲取圖像對(duì)，通過(guò)計(jì)算機(jī)進(jìn)行圖像數(shù)據(jù)處理，不僅可獲取服裝設(shè)計(jì)所需的特征尺寸，還可根據(jù)需要獲取人體圖像上任意一點(diǎn)的三維坐標(biāo)[1]。

筆者在已搭建的實(shí)驗(yàn)平臺(tái)上，實(shí)現(xiàn)了雙目視覺(jué)的定位功能，該機(jī)器人平臺(tái)的定位功能需求通過(guò)獲取圖像信息和機(jī)器人姿態(tài)信息，定位圖像中各個(gè)匹配目標(biāo)點(diǎn)的坐標(biāo)，使原有系統(tǒng)功能得到提升。

2 機(jī)器人仿真實(shí)驗(yàn)平臺(tái)

2.1 硬件系統(tǒng)

本系統(tǒng)的實(shí)驗(yàn)平臺(tái)采用導(dǎo)軌機(jī)器人平臺(tái)體制構(gòu)建一個(gè)模擬仿真環(huán)境，通過(guò)搭建一個(gè)二維機(jī)器人導(dǎo)軌架結(jié)構(gòu)，在獲取運(yùn)動(dòng)信息和云臺(tái)姿態(tài)的情況下，對(duì)機(jī)器人仿真環(huán)境下的目標(biāo)進(jìn)行檢測(cè)、識(shí)別、跟蹤和定位。該平臺(tái)機(jī)器人硬件部分如圖1所示。

其中，2個(gè)機(jī)械臂可在X方向上移動(dòng)，在機(jī)械臂上分別有可在Y方向上移動(dòng)的導(dǎo)軌機(jī)器人。攝像機(jī)搭載在2個(gè)Y軸導(dǎo)軌懸掛的高精度數(shù)控云臺(tái)吊艙上，伺服電機(jī)驅(qū)動(dòng)吊艙沿橫向Y導(dǎo)軌運(yùn)動(dòng)，提供了攝像機(jī)的Y方向運(yùn)動(dòng)自由度。高精度數(shù)控云臺(tái)能實(shí)現(xiàn)攝像機(jī)繞豎直向上的Z軸轉(zhuǎn)動(dòng)和在Z-Y平面、Z-X平面俯仰的精確運(yùn)動(dòng)，以仿真攝像機(jī)的各種拍攝姿態(tài)。機(jī)器人運(yùn)動(dòng)信息和云臺(tái)運(yùn)動(dòng)信息均可以通過(guò)采集卡傳輸?shù)接?jì)算機(jī)控制系統(tǒng)，實(shí)現(xiàn)對(duì)機(jī)器人和云臺(tái)的精確控制。

2.2 軟件系統(tǒng)

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

3 基本原理

3.1 參數(shù)定位法

基于仿真平臺(tái)的基本定位算法主要采用的是參數(shù)定位法和雙目視覺(jué)定位法。參數(shù)定位法的基本原理是通過(guò)獲取云臺(tái)姿態(tài)信息參數(shù)，然后解算出攝像機(jī)視軸與理想地球球面的交點(diǎn)作為定位結(jié)果，參數(shù)定位法原理如圖3所示。

其中，坐標(biāo)系D，E，F(xiàn)表示通過(guò)解算求得的攝像機(jī)鏡頭坐標(biāo)系，它的視場(chǎng)中某向量與地球表面的交點(diǎn)B即為定位結(jié)果。此參數(shù)定位法的優(yōu)點(diǎn)是算法實(shí)現(xiàn)簡(jiǎn)單快速，但它是基于理想的地球橢球模型，如果需要更高精度的定位，需要有詳細(xì)的地理表面信息庫(kù)作為支持。本實(shí)驗(yàn)平臺(tái)有2個(gè)導(dǎo)軌機(jī)器人，采用相似的定位算法，數(shù)據(jù)融合時(shí)，按它們距目標(biāo)距離的權(quán)重作融合，并沒(méi)有更充分地利用圖像信息和2架云臺(tái)之間的關(guān)系信息。

為了更充分地利用2個(gè)導(dǎo)軌移動(dòng)機(jī)器人的圖像信息，只要云臺(tái)不處于某些特定的姿態(tài)，如兩攝像頭的視軸正好平行（實(shí)際中只在極少情況下出現(xiàn)這種姿態(tài)），可以將兩攝像頭的圖像信息作雙目視覺(jué)定位，充分利用多機(jī)通信協(xié)同定位，更好地利用圖像信息，也很少需要地理信息庫(kù)等其他條件的支持。在實(shí)際系統(tǒng)應(yīng)用中，可以使用參數(shù)定位法與雙目視覺(jué)法分別求解，為定位結(jié)果提供更多的參考基礎(chǔ)。

3.2 單目視覺(jué)原理

單目視覺(jué)的基本原理如圖4所示。

由攝像機(jī)針孔成像模型可以得到相似關(guān)系

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

由空間中的坐標(biāo)點(diǎn)映射為圖像中坐標(biāo)點(diǎn)的公式為

式（3）為式（2）的展開(kāi)形式，矩陣A為攝像機(jī)的內(nèi)參數(shù)矩陣，fx和fy分別表示徑向和軸向焦距，cx和cy表示主點(diǎn)（Principle Point）的坐標(biāo)；矩陣R為攝像機(jī)鏡頭坐標(biāo)系在世界坐標(biāo)系旋轉(zhuǎn)矩陣，T為攝像機(jī)鏡頭坐標(biāo)系在世界坐標(biāo)系中的平移向量；W表示P點(diǎn)的世界坐標(biāo)，在展開(kāi)形式中以齊次坐標(biāo)表示；R和T形成的矩陣稱(chēng)為外參數(shù)矩陣，內(nèi)參數(shù)矩陣與外參數(shù)矩陣的乘積定義為矩陣M。若同時(shí)有兩個(gè)攝像機(jī)的多個(gè)對(duì)應(yīng)點(diǎn)，即可形成雙目視覺(jué)的情況，如圖5所示。

若該世界坐標(biāo)系中的同一點(diǎn)P在左右兩攝像機(jī)中分別形成點(diǎn)P1和P2，則有映射方程

將式（4）、（5）分別展開(kāi)并聯(lián)立方程

解此方程組即可求得世界坐標(biāo)系下的坐標(biāo)值。

4 算法實(shí)現(xiàn)

整個(gè)算法實(shí)現(xiàn)的流程如圖6所示。

圖6 算法實(shí)現(xiàn)流程

4.1 攝像機(jī)標(biāo)定

求解攝像機(jī)鏡頭內(nèi)參數(shù)與外參數(shù)的過(guò)程稱(chēng)為攝像機(jī)標(biāo)定。內(nèi)參數(shù)是定位過(guò)程中求解必要的參數(shù)，在攝像機(jī)鏡頭焦距等內(nèi)參數(shù)情況改變時(shí)必須重新進(jìn)行，由于仿真平臺(tái)使用的是定焦鏡頭，所以每次調(diào)焦后只需進(jìn)行一次標(biāo)定即可。標(biāo)定算法采用的是基于張正友標(biāo)定算法[2]實(shí)現(xiàn)的Calibration Toolbox[3-4]。為了便于角點(diǎn)檢測(cè)，選取若干幅棋盤(pán)格平面圖進(jìn)行標(biāo)定，如圖7所示。

由于機(jī)器人攝像機(jī)是定焦鏡頭，內(nèi)參數(shù)在鏡頭不變焦的情況下是不變的，所以由此標(biāo)定算法求出的內(nèi)參數(shù)將在機(jī)器人系統(tǒng)中作為初始配置參數(shù)。

由此標(biāo)定算法求出的內(nèi)參數(shù)與外參數(shù)均作為輸入?yún)?shù)完成一次仿真驗(yàn)證，用以確定雙目定位算法的正確性，并且為機(jī)器人平臺(tái)調(diào)試提供參考真值。

4.2 Matlab仿真驗(yàn)證

由Calibration Toolbox解算出的內(nèi)參數(shù)與外參數(shù)進(jìn)行仿真實(shí)驗(yàn)。在多個(gè)視圖中分別手工依次標(biāo)定多個(gè)目標(biāo)點(diǎn)，本次實(shí)驗(yàn)取的4個(gè)點(diǎn)是圖7中棋盤(pán)格外圈的4個(gè)內(nèi)角點(diǎn)，然后將這16幅圖片依次作為左右視角圖片，進(jìn)行雙目視覺(jué)定位解算，結(jié)果如圖8所示。

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

4.3 仿真機(jī)器人平臺(tái)實(shí)現(xiàn)及試驗(yàn)結(jié)果

已經(jīng)實(shí)現(xiàn)的軟件可以完成目標(biāo)的跟蹤、識(shí)別和基于參數(shù)法的定位功能。為了有效利用數(shù)據(jù)，實(shí)現(xiàn)多機(jī)通信的協(xié)同定位，本系統(tǒng)添加了雙目視覺(jué)定位模塊。該模塊主要包括雙目視覺(jué)算法子模塊與系統(tǒng)接口子模塊2個(gè)部分。

定位算法子模塊完成求解方程組以解算目標(biāo)坐標(biāo)值的功能；接口子模塊負(fù)責(zé)與其余模塊通信，獲取姿態(tài)參數(shù)，將姿態(tài)參數(shù)轉(zhuǎn)化成外參數(shù)矩陣，與初始配置的內(nèi)參數(shù)矩陣一起作為定位的輸入?yún)?shù)。

在VC++開(kāi)發(fā)環(huán)境下編寫(xiě)的程序嵌入系統(tǒng)，完成該雙目定位功能，試驗(yàn)結(jié)果如表1所示。

表1 定位坐標(biāo)與真值

定位結(jié)果誤差如圖9所示。

由上述定位結(jié)果誤差分析可以看出，定位結(jié)果的解算精確度與定位點(diǎn)的位置有關(guān)。由于邊角點(diǎn)離圖像中心主點(diǎn)比較遠(yuǎn)，畸變相對(duì)于處于圖像中心點(diǎn)的原點(diǎn)位置大，定位結(jié)果誤差較大，而且定位結(jié)果誤差主要來(lái)自Z軸方向的空間點(diǎn)計(jì)算誤差，由于Z軸方向的數(shù)值完全由攝像機(jī)鏡頭的焦距解算得到，圖像模糊畸變對(duì)于結(jié)果影響較大，若已知目標(biāo)的水平高度，僅計(jì)算在X-Y平面內(nèi)的偏離直線(xiàn)長(zhǎng)度，屏蔽Z軸的誤差，誤差均在10 mm以?xún)?nèi)，定位結(jié)果的準(zhǔn)確性明顯提升。

5 小結(jié)

介紹了雙目視覺(jué)的基本原理，基于此原理開(kāi)發(fā)出在仿真機(jī)器人平臺(tái)上的雙目視覺(jué)定位功能，使基于參數(shù)法定位的原機(jī)器人系統(tǒng)的定位功能得到了提升。由定位結(jié)果可以看出，定位結(jié)果準(zhǔn)確，并且有較高的精度。

[1]林琳.機(jī)器人雙目視覺(jué)定位技術(shù)研究[D].西安：西安電子科技大學(xué),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.