李榮明，蘆利斌，陳鼎新

（第二炮兵工程學(xué)院 602教研室，陜西 西安 710025）

室內(nèi)機(jī)器人的運(yùn)動(dòng)軌跡和位置控制，對(duì)室內(nèi)空間測(cè)量方法與技術(shù)提出了需求?，F(xiàn)在常用的室內(nèi)空間測(cè)量手段有超聲波測(cè)量、紅外測(cè)量、射頻（RF）測(cè)量和視覺測(cè)量等，其中視覺測(cè)量因?yàn)橐曈X圖像包含了目標(biāo)的豐富信息且視覺系統(tǒng)價(jià)格低廉、使用方便，引起研究人員的極大關(guān)注[1]。日本東京大學(xué)利用實(shí)時(shí)雙目立體視覺和機(jī)器人整體姿態(tài)信息集成，開發(fā)了一種仿真機(jī)器人動(dòng)態(tài)行走導(dǎo)航系統(tǒng)[2]。文獻(xiàn)[3]中研究了一種利用雙目立體視覺系統(tǒng)定位手術(shù)器械的方法，為外科手術(shù)提供了一定的輔助作用。

筆者研究的室內(nèi)空間測(cè)量系統(tǒng)，主要用于實(shí)時(shí)測(cè)量室內(nèi)小型飛行或地面移動(dòng)機(jī)器人的空間位置。測(cè)量精度要求：≤20 mm；實(shí)時(shí)性要求：≥10 Hz。通過3個(gè)低成本的USB攝像頭對(duì)靜止或運(yùn)動(dòng)目標(biāo)（其上放置一紅色指示燈）的圖像拍攝，來實(shí)時(shí)確定室內(nèi)目標(biāo)的空間精確位置信息，并可將這些信息實(shí)時(shí)傳遞給目標(biāo)控制系統(tǒng)進(jìn)行輔助控制。

1 工作原理

室內(nèi)空間測(cè)量利用雙目立體視覺原理[4]，即采用兩個(gè)攝像頭在針孔成像模型下，如果攝像頭光心和像點(diǎn)已知，就可以確定這兩點(diǎn)組成的唯一射線，物點(diǎn)必然在此射線上。由兩個(gè)攝像頭進(jìn)行交匯就有兩條這樣的射線，射線相交于物點(diǎn)即確定出空間物點(diǎn)的位置。如圖1中，空間點(diǎn)P是O1P1與O2P2兩條射線的交點(diǎn)。該系統(tǒng)采用3個(gè)攝像頭兩兩交匯得到空間點(diǎn)的3個(gè)位置坐標(biāo)，求其均值作為目標(biāo)的位置信息，來提高系統(tǒng)測(cè)量的精度。

圖1 用雙攝像頭觀測(cè)空間物點(diǎn)Fig.1 Observing space object point with dual cameras

2 系統(tǒng)總體設(shè)計(jì)

2.1 硬件設(shè)計(jì)

硬件部分主要由3臺(tái)USB攝像頭及連接延長線，1臺(tái)數(shù)據(jù)處理計(jì)算機(jī)和1個(gè)紅色指示燈組成。3臺(tái)攝像頭利用支架布置在房間3個(gè)角落，使之能最大范圍地觀測(cè)到測(cè)量空間全部，并通過USB接口連接在計(jì)算機(jī)上（如圖2所示）。室內(nèi)實(shí)驗(yàn)空間面積大約在30 m2，測(cè)量范圍設(shè)計(jì)為3 m×3 m×1.5 m。

圖2 硬件組成與連接Fig.2 Hardware components and connections

攝像頭選用藍(lán)色妖姬S8攝像頭，靜態(tài)分辨率可達(dá)1 280×960，最高幀速率為 150幀/秒，USB2.0接口。使用的圖像采集頻率為30 Hz，圖像分辨率為320×240。

計(jì)算機(jī)上安裝VC++6.0、OpenCV1.0和DirectX9.0b SDK等軟件，用于數(shù)據(jù)處理和軟件開發(fā)等。

紅色指示燈放置在目標(biāo)物體上，便于攝像頭捕捉和圖像識(shí)別。

2.2 軟件設(shè)計(jì)

軟件部分分為3大模塊：攝像頭標(biāo)定模塊、圖像處理模塊和空間位置計(jì)算模塊，數(shù)據(jù)流程如圖3所示。

圖3 軟件系統(tǒng)模塊及數(shù)據(jù)流程Fig.3 Software system module and data process

1）攝像頭標(biāo)定模塊 通過從不同角度對(duì)標(biāo)定棋盤格的拍攝與角點(diǎn)識(shí)別，先后計(jì)算出每臺(tái)攝像頭的內(nèi)參數(shù)與外參數(shù)。

2）圖像處理模塊 對(duì)攝像頭實(shí)時(shí)采集的圖像進(jìn)行畸變矯正、平滑濾波、閾值分割、形態(tài)學(xué)處理和特征角點(diǎn)檢測(cè)等，提取出目標(biāo)點(diǎn)坐標(biāo)。

3）空間位置計(jì)算模塊 輸入目標(biāo)點(diǎn)在二維圖像上的坐標(biāo)和攝像頭參數(shù)，利用空間兩條射線相交確定一點(diǎn)的原理，實(shí)時(shí)計(jì)算目標(biāo)物體的位置信息。

3 功能模塊算法設(shè)計(jì)與實(shí)現(xiàn)

OpenCV是一個(gè)開源的計(jì)算機(jī)視覺庫[5]，由一系列C函數(shù)和少量C++類構(gòu)成，可實(shí)現(xiàn)圖像處理和計(jì)算機(jī)視覺方面的很多通用算法。它支持多種平臺(tái)，移植性強(qiáng)、執(zhí)行效率高，可以很好地應(yīng)用于工程實(shí)際當(dāng)中。本文借助OpenCV庫函數(shù)來實(shí)現(xiàn)主要功能模塊的算法。

3.1 攝像頭標(biāo)定模塊

攝像頭標(biāo)定是視覺測(cè)量的必要步驟，通過標(biāo)定可得到二維圖像坐標(biāo)與三維空間坐標(biāo)之間的關(guān)系。目前普遍使用的是張氏平面標(biāo)定法[6]，它無需知道模板的運(yùn)動(dòng)參數(shù)，避免了其他標(biāo)定法設(shè)備要求高，操作繁瑣等缺點(diǎn)，同時(shí)又能保證標(biāo)定的精確度和參數(shù)的魯棒性。

張氏標(biāo)定法假設(shè)模板位于世界坐標(biāo)系ZW=0的平面，然后從不同角度拍攝3幅以上(含3幅)模板圖像，根據(jù)模板上的點(diǎn)和其圖像對(duì)應(yīng)點(diǎn)的映射關(guān)系線性求解出攝像頭的內(nèi)外參數(shù)，再根據(jù)相應(yīng)算法得到畸變系數(shù)，最后利用LM算法優(yōu)化參數(shù)。攝像頭標(biāo)定流程如圖4所示。

圖4 攝像頭標(biāo)定流程Fig.4 Camera calibration process

利用OpenCV庫函數(shù)實(shí)現(xiàn)攝像頭標(biāo)定算法如下：

1）讀入或拍攝標(biāo)定圖像，使用cvFindChessboardCorners函數(shù)進(jìn)行棋盤格角點(diǎn)檢測(cè)，確定角點(diǎn)的位置。如果所有角點(diǎn)都被檢測(cè)到，它們都被以鏈表形式存儲(chǔ)起來并輸出，函數(shù)返回非零值，否則函數(shù)返回零。

2）當(dāng)提取到了所有的角點(diǎn)后，使用cvFindCornerSubPix函數(shù)對(duì)這些角點(diǎn)進(jìn)行細(xì)化，得到角點(diǎn)亞像素級(jí)的坐標(biāo)值；再使用cvDrawChessboardCorners函數(shù)將檢測(cè)到的角點(diǎn)在圖像中顯示出來。

3）使用cvCalibrateCamera2函數(shù)，輸入角點(diǎn)的空間三維坐標(biāo)值以及二維圖像坐標(biāo)值，可以快速計(jì)算出攝像頭內(nèi)參，包含以像素為單位的焦距、圖像主點(diǎn)坐標(biāo)和畸變系數(shù)等。

4）再通過外參標(biāo)定圖像的角點(diǎn)檢測(cè)，輸入已計(jì)算出的內(nèi)參數(shù)和畸變系數(shù)，使用cvFindExtrinsicCameraParams2函數(shù)計(jì)算攝像頭的外參數(shù)。

3.2 圖像處理模塊

圖像處理模塊主要是完成實(shí)時(shí)拍攝的圖像中目標(biāo)點(diǎn)的坐標(biāo)獲取，在室內(nèi)空間，受周圍環(huán)境特別是光照影響，要快速準(zhǔn)確地提取出目標(biāo)是件困難的工作。為此，在目標(biāo)物體上放置一紅色指示燈，通過亮度對(duì)比濾除復(fù)雜的背景環(huán)境，使圖像處理變得簡單有效。圖像處理的具體流程如圖5所示。

圖5 圖像處理流程Fig.5 Flow chart of image processing

主要過程的實(shí)現(xiàn)代碼如下：

3.3 空間位置計(jì)算模塊

由標(biāo)定模塊得到的內(nèi)外參數(shù)計(jì)算出攝像頭C1和C2的投影矩陣M1與M2，圖像處理模塊得到目標(biāo)點(diǎn)P在兩幅成像平面中的像點(diǎn)為 P1（u1，v1）和 P2（u2，v2），設(shè)點(diǎn) P 的世界坐標(biāo)為（X，Y，Z），利用針孔成像模型得：

mkij分別為 Mk的第 i行第 j列元素。 在式（1）和式（2）中消去Zc1或Zc2，從而得到關(guān)于X，Y，Z的4個(gè)線性方程：

由解析幾何知，式（3）或式（4）的幾何意義是過O1P1或O2P2的直線。由于空間點(diǎn)P是O1P1與O2P2交點(diǎn)，它必然同時(shí)滿足式（3）與式（4）。 因此，可以將式（3）與式（4）聯(lián)立求出 P點(diǎn)的坐標(biāo)（X，Y，Z）。 事實(shí)上，式（3）與式（4）為包含（X，Y，Z）3個(gè)變量的4個(gè)線性方程，只需其中3個(gè)就可解出（X，Y，Z），已假設(shè)了直線O1P1與O2P2一定相交，或者說，4個(gè)方程必定有解且解是唯一的。由于實(shí)驗(yàn)測(cè)量數(shù)據(jù)總是有噪聲的，用最小二乘法求出（X，Y，Z）。 將公式（3）和（4）寫成矩陣形式，得到最小二乘結(jié)果如下式：

上述算法通過 CvMat*Crossingpoint（CvMat*a_homography，CvMat*b_homography，CvPoint2D32f a_pt，CvPoint2D32f b_pt）函數(shù)給予實(shí)現(xiàn)，指針a_homography和b_homography分別指向兩個(gè)攝像頭的投影矩陣，a_pt和b_pt為目標(biāo)點(diǎn)在兩成像平面上的像素坐標(biāo)，函數(shù)返回交點(diǎn)的坐標(biāo)向量即目標(biāo)點(diǎn)的空間坐標(biāo)。

4 實(shí)驗(yàn)驗(yàn)證分析

對(duì)該系統(tǒng)的實(shí)驗(yàn)主要從兩個(gè)方面來進(jìn)行的，一方面是檢驗(yàn)系統(tǒng)測(cè)量的實(shí)時(shí)性，另一方面是檢驗(yàn)其精確性。

1）驗(yàn)證系統(tǒng)測(cè)量的實(shí)時(shí)性

先調(diào)用QueryPerformanceFrequency（）函數(shù)獲得機(jī)器內(nèi)部定時(shí)器的時(shí)鐘頻率為2 520 690 000 Hz，然后采集圖像并測(cè)量計(jì)算 1 000次，再調(diào)用 QueryPerformanceCounter（）函數(shù)返回前后的時(shí)間計(jì)數(shù)值，利用兩次獲得的計(jì)數(shù)之差及時(shí)鐘頻率，計(jì)算出此測(cè)量系統(tǒng)的采集頻率為：1 000/（11 923 425 995 955-11 758 675 015 563）/2 520 690 000=15.3 Hz， 完全能夠滿足室內(nèi)低速運(yùn)動(dòng)機(jī)器人的測(cè)量要求。

2）驗(yàn)證系統(tǒng)測(cè)量的精確度

①靜態(tài)測(cè)量驗(yàn)證。在室內(nèi)坐標(biāo)系中，利用地板線和卷尺測(cè)量多組目標(biāo)點(diǎn)的空間位置，并與系統(tǒng)測(cè)量結(jié)果進(jìn)行比較（如表1所示），測(cè)量誤差均在15 mm范圍內(nèi)。圖6給出目標(biāo)點(diǎn)在（500，500，115）位置時(shí)靜態(tài)測(cè)量的界面截圖。

表1 靜態(tài)測(cè)量結(jié)果與實(shí)際坐標(biāo)值比較Tab.1 Compared static measurement result with true position

圖6 目標(biāo)在（500，500，115）位置測(cè)量界面Fig.6 Measurement interface of target in（500，500，115）position

②動(dòng)態(tài)測(cè)量驗(yàn)證。由于在室內(nèi)沒有其他可靠的定位系統(tǒng)作為參考，這里僅利用遙控小汽車在地面從點(diǎn)（-1 000，-1000，115）開始，沿1.5 m×1.5 m的正方形低速行駛，汽車高115 mm。在測(cè)量的原始數(shù)據(jù)輸出前進(jìn)行均值濾波，剔除野值，使得數(shù)據(jù)輸出更加平滑可靠。系統(tǒng)記錄下實(shí)時(shí)測(cè)量的位置信息，利用Matlab繪出運(yùn)動(dòng)軌跡三維坐標(biāo)圖如圖7所示，不難看出繪出的運(yùn)動(dòng)軌跡成正方形，且始末點(diǎn)坐標(biāo)與實(shí)際一致。

圖7 目標(biāo)運(yùn)動(dòng)軌跡圖Fig.7 Track figure of target movement

3）結(jié)果分析

系統(tǒng)測(cè)量的結(jié)果誤差在15 mm范圍內(nèi)，基本滿足室內(nèi)空間位置測(cè)量的需要。分析引起誤差的原因有：

①攝像頭標(biāo)定過程引入的誤差，比如標(biāo)定模板制作粗糙，粘貼的平板無法做到完全水平；另外在外參數(shù)標(biāo)定中，標(biāo)定模板距離攝像頭較遠(yuǎn)，成像小，以及受攝像頭固有噪聲和環(huán)境噪聲影響，使得棋盤格角點(diǎn)提取有一定誤差；

②攝像頭動(dòng)態(tài)采集圖像的分辨率太低，僅為320×240，影響了圖像的質(zhì)量，圖像處理中目標(biāo)提取的一點(diǎn)點(diǎn)偏離就會(huì)給計(jì)算結(jié)果帶來很大誤差；

③室內(nèi)地面反光給目標(biāo)特征提取帶來很大干擾，特別是當(dāng)目標(biāo)進(jìn)入反光區(qū)中，就很難準(zhǔn)確捕捉到目標(biāo)點(diǎn)，使得測(cè)量數(shù)據(jù)很不可靠。

5 結(jié) 論

本文設(shè)計(jì)實(shí)現(xiàn)了一種基于計(jì)算機(jī)視覺的室內(nèi)空間測(cè)量系統(tǒng)，通過固定在室內(nèi)的3個(gè)USB攝像頭實(shí)時(shí)捕獲目標(biāo)圖像，能夠比較精確地測(cè)量室內(nèi)靜止或低速運(yùn)動(dòng)目標(biāo)的空間位置。整個(gè)系統(tǒng)僅需一臺(tái)普通計(jì)算機(jī)和3個(gè)USB攝像頭及10 m延長線，攝像頭連同USB延長線的價(jià)格在200元左右，成本低廉、搭設(shè)簡易，且系統(tǒng)移植性強(qiáng)、適用廣泛，可在多種室內(nèi)環(huán)境進(jìn)行目標(biāo)測(cè)量，測(cè)量結(jié)果精確、實(shí)時(shí)性強(qiáng)，完全可以滿足室內(nèi)移動(dòng)機(jī)器人的測(cè)量要求。

[1]蔡征宇，沈俊杰，楊昀.基于工業(yè)CCD攝像機(jī)的機(jī)器人視覺定位系統(tǒng)研究[J].機(jī)電一體化，2010（3）：22-23.

CAI Zheng-yu，SHEN Jun-jie，YANG Yun.The robot vision system using CCD camera[J].Mechatronics，2010（3）：22-23.

[2]Okada K，Inaba M，Inoue H.Integration of real-time binocular stereo vision and whole body information for dynamic walking navigation of humanoid robot[C]//IEEE Conference on Multisensor Fusion and Intergration for Intelligent Systems.Tokyo：[s.n.]，2003：131-136.

[3]劉晶晶.基于雙目立體視覺的三維定位技術(shù)研究[D].武漢：華中科技大學(xué)，2007：9-14.

[4]黨樂.基于雙目立體視覺的三維重建方法研究[D].西安：長安大學(xué)，2009：3-5.

[5]Bradski G，Kaebler A.學(xué)習(xí) OpenCV（中文版）[M].于仕琪，劉瑞幀，譯.北京：清華大學(xué)出版社，2009.

[6]ZHANG Zheng-you.Flexible camera calibration by viewing a plane from unknown orientations[C]//Proceedings of the 7th International Conference on Computer Vision.Liege，Belgium：Elsevier Science Publishers，1999：666-674.