方寶富1, 2，潘啟樹1，洪炳镕1，鐘秋波1，王偉光1

?

一種基于全維視覺與前向單目視覺的目標(biāo)定位算法

方寶富，潘啟樹，洪炳镕，鐘秋波，王偉光

（1. 哈爾濱工業(yè)大學(xué)計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，黑龍江哈爾濱150001；2. 合肥工業(yè)大學(xué)計(jì)算機(jī)與信息學(xué)院，安徽合肥230009）

在機(jī)器人足球中，機(jī)器人周圍的環(huán)境信息被顏色特殊化。在對基于顏色閾值分割的分析基礎(chǔ)上，針對不同光線的情況下提出了改進(jìn)的顏色閾值分割法，實(shí)現(xiàn)了全維視覺和前向單目視覺對場上不同顏色目標(biāo)的識別。在此基礎(chǔ)上實(shí)現(xiàn)了前項(xiàng)單目視覺和全維視覺的目標(biāo)定位，利用Kalman濾波算法，實(shí)現(xiàn)二者的信息融合，從而實(shí)現(xiàn)更準(zhǔn)確的目標(biāo)定位信息。實(shí)驗(yàn)結(jié)果表明基于此算法可以彌補(bǔ)中遠(yuǎn)距離的目標(biāo)信息的準(zhǔn)確度，證明本算法是可行且有效的。

數(shù)字圖像處理；卡爾曼濾波；定位；顏色閾值

在機(jī)器人足球、機(jī)器人追捕、機(jī)器人編隊(duì)等機(jī)器人應(yīng)用系統(tǒng)中，關(guān)鍵技術(shù)之一就是對環(huán)境信息的識別。環(huán)境識別的主要任務(wù)是快速準(zhǔn)確對環(huán)境中動態(tài)及靜態(tài)目標(biāo)實(shí)現(xiàn)定位，一般都是通過機(jī)器人配備的傳感器如攝像機(jī)、激光測距儀、紅外測距儀里程計(jì)等獲得數(shù)據(jù)然后分析而得。本文是基于攝像機(jī)實(shí)時(shí)采集圖像數(shù)據(jù)、圖像處理和識別場地中的目標(biāo)，然后現(xiàn)實(shí)機(jī)器人足球中的目標(biāo)定位。

對于基于視覺系統(tǒng)獲得信息的機(jī)器人來說，目標(biāo)識別是建立在圖像分割的基礎(chǔ)上的，圖像分割從可以看成是將圖片的像素根據(jù)圖像的特征，按照一定的原則方法進(jìn)行分類的過程，分類所依據(jù)的特性：灰度、顏色、多譜特性、空間特性和紋理。目前圖像分割算法一般按照圖片像素的不連續(xù)性和相關(guān)性，可以分為兩類：一類是基于像素的亮度或值的不連續(xù)變化來分割圖像，如基于點(diǎn)、線和邊緣的檢測；另一類是根據(jù)應(yīng)用規(guī)則來分割圖像的相關(guān)區(qū)域，如基于閾值選擇、區(qū)域生長和聚類的檢測方法。

在機(jī)器人足球場地內(nèi)，各個(gè)不同目標(biāo)用不同的顏色加以區(qū)分，本文利用目標(biāo)的顏色信息，以顏色特征作為圖像分割的依據(jù)，提出基于全維視覺和前項(xiàng)單目視覺的目標(biāo)定位算法。顏色空間也稱為顏色模型，如RGB、YCrCb、HSV、HIS、CMY等，它們之間可以相互轉(zhuǎn)換。最常用到的是RGB顏色空間，但是RGB顏色模型存在各分量高相關(guān)性的缺點(diǎn)，即在相同顏色在不同的光照條件下，得到的RGB顏色分量變化很大，即受光照影響較大，而YUV顏色空間則較好，本文則采用YUV顏色空間。

1 基于顏色閾值的目標(biāo)識別

1.1 圖像預(yù)處理

視頻采集的RGB圖像是采用的RGB24格式，即RGB每個(gè)分量都是由8位數(shù)據(jù)位來表示的，這樣每個(gè)分量都有256個(gè)級別，圖像數(shù)據(jù)轉(zhuǎn)換成YUV顏色空間之后，每個(gè)分量仍然是256個(gè)級別，即每個(gè)分量由8位數(shù)據(jù)位來表示，所以將轉(zhuǎn)換后的YUV顏色空間數(shù)據(jù)也像原RGB圖像的數(shù)據(jù)那樣保存，大小一樣，因此可以將圖像數(shù)據(jù)進(jìn)行替換，圖像的其它信息如圖像大小、尺寸、格式等數(shù)據(jù)還保留，以供后續(xù)程序進(jìn)行目標(biāo)識別時(shí)方便使用。

圖像預(yù)處理的主要工作是RGB顏色空間圖像數(shù)據(jù)轉(zhuǎn)換為YUV顏色空間，這個(gè)轉(zhuǎn)換為線性變換，如公式(1)所示

（1）

1.2 基于顏色閾值的目標(biāo)識別

文獻(xiàn)[6]中顏色查詢表內(nèi)存儲的是目標(biāo)顏色的YUV分量范圍，如圖1所示，形成了一個(gè)閾值長方體，若一個(gè)像素值在這個(gè)閾值長方體內(nèi)，則說明該像素很可能就是目標(biāo)像素點(diǎn)，利用顏色查詢表便能夠快速的判斷一個(gè)YUV顏色空間的像素值是否屬于目標(biāo)的顏色空間域。但該算法存在一個(gè)潛在的問題，由于顏色查詢表存儲的是YUV分量范圍，于是在光線較暗的情況下對目標(biāo)顏色進(jìn)行標(biāo)定時(shí)，可能會出現(xiàn)兩個(gè)目標(biāo)的YUV分量范圍會有交叉部分，如圖2(a)所示，當(dāng)交叉的部分較小時(shí)，對目標(biāo)的識別沒有太大影響，但當(dāng)交叉的部分較大時(shí)，對目標(biāo)的識別有很大的影響；若發(fā)生這種情況只能再次標(biāo)定目標(biāo)，并減小標(biāo)定范圍，但減小標(biāo)定范圍時(shí)，可能會發(fā)生目標(biāo)識別不完整、不準(zhǔn)確，這是一個(gè)矛盾。于是本文提出了改進(jìn)的顏色查詢表，即讓顏色查詢表內(nèi)存儲目標(biāo)顏色的YUV離散分量范圍，轉(zhuǎn)化為顏色空間投影如圖2(b)所示，即一個(gè)目標(biāo)顏色對應(yīng)的是一個(gè)或多個(gè)閾值長方體。若在進(jìn)行目標(biāo)顏色標(biāo)定的時(shí)候，發(fā)生上述問題時(shí)，即會在識別窗口顯示出錯(cuò)誤識別的顏色。

圖1 閾值長方體投影I

(a)?????????????(b)

2 基于全維視覺和前向單目視覺的目標(biāo)定位

基于顏色的目標(biāo)識別要實(shí)現(xiàn)多個(gè)目標(biāo)的同時(shí)識別，該算法由3個(gè)環(huán)節(jié)組成：① 基于目標(biāo)顏色的聚類，即實(shí)現(xiàn)圖像分割；② 目標(biāo)區(qū)域提取與生長；③ 多目標(biāo)的識別。在識別目標(biāo)時(shí)候采用了2種不同的視覺圖像，在機(jī)器人頭頂上的全維視覺圖像和在機(jī)器人前方的前向視覺圖像。本文分別闡述了如何實(shí)現(xiàn)全維和前向的定位方法，最后采用Kalman濾波實(shí)現(xiàn)二者的信息融合，從而得到更加準(zhǔn)確的目標(biāo)位置。

2.1 全維視覺系統(tǒng)的目標(biāo)定位

2.1.1 角度定位

當(dāng)攝像機(jī)的中心對準(zhǔn)全維反射鏡的軸心時(shí)，根據(jù)全維視覺系統(tǒng)反射鏡的對稱結(jié)構(gòu)，光線直線傳播及小孔成像原理可知，景物的形變主要發(fā)生在反射鏡的緯度上，而在經(jīng)度上的形變很小，所以目標(biāo)的方向定位比較準(zhǔn)確。因此，可以利用反正切函數(shù)求出目標(biāo)的角度。

公式（2）中，(，)和 (x，y)分別是目標(biāo)質(zhì)心和全維視覺中心相對于以圖片左下角為原點(diǎn)的笛卡爾坐標(biāo)系中的坐標(biāo)。是機(jī)器人極坐標(biāo)系中的極角，機(jī)器人坐標(biāo)系如圖3所示。

(a) 源圖像坐標(biāo)系?????????(b) 機(jī)器人坐標(biāo)系

圖3 坐標(biāo)變換示意圖

2.1.2 距離定位

利用攝像機(jī)參數(shù)，全維反射鏡的曲面方程，以及全維攝像機(jī)的高度等數(shù)據(jù)，推導(dǎo)出了可以用于全維視覺定位的公式。但受全維視覺系統(tǒng)的加工精度、攝像機(jī)畸變以及不易維護(hù)等因素影響，在實(shí)際使用的時(shí)候采用插值的方法進(jìn)行全維視覺系統(tǒng)的目標(biāo)定位，并且通過實(shí)驗(yàn)比較了雙曲線插值與線性插值的性能，在絕對誤差和相對誤差比較接近的情況下，最終選用了方法簡單的線性插值，公式如下公式3所示

其中 (x,y)和 (x,y)分別是目標(biāo)質(zhì)心和全維視覺中心相對于以圖片左下角為原點(diǎn)的笛卡爾坐標(biāo)系中的坐標(biāo)，是第次插值時(shí)的機(jī)器人極坐標(biāo)系中的像素距離，d是第次插值時(shí)的實(shí)際距離。

2.2 前向單目視覺的目標(biāo)定位

前向單目視覺的目標(biāo)定位就是通過前向單目視覺的目標(biāo)識別后獲得的二維圖像數(shù)據(jù)轉(zhuǎn)化為實(shí)際環(huán)境的三維數(shù)據(jù)信息的過程，通過攝像機(jī)的內(nèi)外參數(shù)、俯仰角和高度等信息，利用立體幾何的知識可以推導(dǎo)出轉(zhuǎn)換公式。

如圖4所示，攝像機(jī)在地面上的投影點(diǎn)為原點(diǎn)，建立一個(gè)機(jī)器人直角坐標(biāo)系，以機(jī)器人右側(cè)為軸正方向，其正前方為軸正方向；成像平面是以中心為原點(diǎn)的直角坐標(biāo)系。是攝像機(jī)的俯視角，是攝像機(jī)到水平面的距離，∠是攝像機(jī)的水平視角，∠是攝像機(jī)的垂直視角。單目攝像機(jī)目標(biāo)定位就是要把成像平面上的點(diǎn)(,)，變換到機(jī)器人坐標(biāo)系的坐標(biāo)(,)。利用立體幾何知識，得到目標(biāo)定位的如公式(4)

圖4 單目攝像機(jī)定位模型

,為每單個(gè)像素的水平和垂直視角：

但這種轉(zhuǎn)換方法需要一種約束，即假定所有的目標(biāo)點(diǎn)都在水平面上，當(dāng)目標(biāo)在水平面上時(shí)，定位的絕對誤差隨目標(biāo)與機(jī)器人的距離的增加而增大，這是由于單位像素內(nèi)的信息量相對增大，失真較多造成的；當(dāng)物體是具有一定高度的物體時(shí)，如球等，則在距離機(jī)器人越近，相對誤差就越大，這是由于這種方法將所有的目標(biāo)都投影到地平面上，對具有一定高度的物體定位時(shí)，同樣將物體的高度信息也投影到了地平面上，這樣就導(dǎo)致了誤差增大。解決這種問題的方法是，不使用目標(biāo)識別后的質(zhì)心數(shù)據(jù)進(jìn)行目標(biāo)定位，而是采用目標(biāo)的最低點(diǎn)數(shù)據(jù)進(jìn)行定位，即將目標(biāo)約束在了地平面上了，這種方法適用于對立柱、球門等側(cè)表面直接與地面相接的物體定位。但當(dāng)物體是球，而且是直徑比較大的時(shí)候，就需要進(jìn)行修正，本文提出利用目標(biāo)的先驗(yàn)知識修正誤差的方法。

如圖5所示，把球投影到￠平面上，利用先驗(yàn)知識球的半徑，以及角度∠''，求出｜''｜，即在軸方向上的修正值；再在立體圖形中，再利用立體幾何知識求出｜｜，即在軸方向上的修正值。修正后的定位公式如公式(5)

其中是攝像機(jī)的俯視角，是攝像機(jī)到水平面的距離，、為每單個(gè)像素的水平和垂直視角：, (,)是成像平面的坐標(biāo)，f(,)和f(,)分別是修正后的球在和方向上的定位數(shù)據(jù)。

圖5 單目攝像機(jī)修正定位模型

但是，在實(shí)驗(yàn)室燈光穩(wěn)定的環(huán)境下，前向攝像機(jī)對球的識別后的質(zhì)心，總體上隨距離的增加而降低，這是由于當(dāng)球離機(jī)器近時(shí)，球的上表面不是很亮，而下部暗，識別的球的質(zhì)心偏高；當(dāng)球離機(jī)器遠(yuǎn)時(shí)，球的上表面變得光亮，識別的球的質(zhì)心偏低，所以本文針對這種情況對式(5)中的f(,)進(jìn)行了線性修正，如式(6)

2.3 基于信息融合的目標(biāo)定位

由于在全自主足球機(jī)器人上安裝了全維視覺和前向單目視覺兩種視覺，而且兩種視覺的視野有重合的區(qū)域，如圖6所示，因此在對目標(biāo)定位時(shí)，需要采用一定的方法來使用視覺系統(tǒng)對目標(biāo)的定位數(shù)據(jù)。本文提出一種基于數(shù)據(jù)融合的目標(biāo)定位算法，采用卡爾曼濾波器算法，對全維視覺與前向單目視覺的定位信息進(jìn)行融合。

圖6 視覺系統(tǒng)視野

卡爾曼濾波器是根據(jù)系統(tǒng)的狀態(tài)方程和測量方程，得出系統(tǒng)狀態(tài)的最優(yōu)估計(jì)的數(shù)學(xué)方法，是對系統(tǒng)噪聲、測量誤差的不確定性描述，它是以最優(yōu)的方式融合傳感器數(shù)據(jù)與系統(tǒng)狀態(tài)。在卡爾曼濾波理論中，被考查的系統(tǒng)被假定為線性系統(tǒng)且具有高斯白噪聲，該方法還可以將多個(gè)傳感器的測量值優(yōu)化的合并成對系統(tǒng)狀態(tài)的估計(jì)。

假設(shè)系統(tǒng)有來自兩個(gè)傳感器的對目標(biāo)的測量值和，方差分別為和，因此可以利用這兩個(gè)測量值分別得到對目標(biāo)的估計(jì)

用加權(quán)最小平方方法得到

（8）

其中是的權(quán)重，對求導(dǎo)，并使其等于零，可以得到最小的誤差估計(jì)

（10）

取w=1/σ狀，代入式(10)得到

按卡爾曼濾波的狀態(tài)方程形式可以寫出

（12）

由于在全自主足球機(jī)器人的比賽中，自身與球的運(yùn)動都可以看作是隨機(jī)運(yùn)動，很難求出置信度高的目標(biāo)的位置的測量值的方差，但是可以根據(jù)實(shí)驗(yàn)求出測量噪聲的方差，因此兩個(gè)傳感器的噪聲可以利用公式(12)融合。目標(biāo)位置的真實(shí)值、測量值和噪聲之間的關(guān)系如下

-＝

其中為測量值，為真實(shí)值，為噪聲，將其代入公式(12)得到

Z-=₁-+K((₂-) - (₁-))=

₁-+K(₂-₁) （14）

Z=₁+K(₂-₁) （15）

公式(15)即為本文中的數(shù)據(jù)融合公式，其中都是根據(jù)全維視覺與單目視覺的定位實(shí)驗(yàn)數(shù)據(jù)算出的方差，由于₁和₂都是由傳感器獲得的測量值，且Z不作為系統(tǒng)的狀態(tài)值成為下次濾波的輸入，所以卡爾曼濾波增益在融合過程中保持不變，這樣簡化算法，計(jì)算速度快，是一種穩(wěn)態(tài)卡爾曼濾波。

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

3.1 角度測量實(shí)驗(yàn)

角度定位實(shí)驗(yàn)是利用HIT－III型機(jī)器的全維視覺進(jìn)行定位，選擇的時(shí)間是晚上，室內(nèi)環(huán)境為4盞日光燈，避免日光變化對目標(biāo)識別產(chǎn)生影響。角度測量實(shí)驗(yàn)如圖7所示。實(shí)驗(yàn)中，球做為目標(biāo)，在機(jī)器人周圍隨機(jī)移動，共測量25組實(shí)驗(yàn)數(shù)據(jù)，試驗(yàn)結(jié)果如圖8所示。

圖7 實(shí)驗(yàn)過程說明

圖8 角度實(shí)驗(yàn)結(jié)果

3.2 距離定位實(shí)驗(yàn)

距離定位實(shí)驗(yàn)是利用HIT－III型機(jī)器的全維視覺和前向單目視覺共同進(jìn)行的定位。實(shí)驗(yàn)中，球做為目標(biāo)，在機(jī)器人前方由近及遠(yuǎn)運(yùn)動，如圖9所示，共測量31組實(shí)驗(yàn)數(shù)據(jù)，圖10是前向視覺識別結(jié)果和實(shí)際距離關(guān)系圖，圖11是全維視覺識別結(jié)果和實(shí)際距離關(guān)系圖，圖12是數(shù)據(jù)融合以后目標(biāo)距離與實(shí)際距離關(guān)系圖。

圖9 實(shí)驗(yàn)過程說明

圖10 前向視覺識別結(jié)果和實(shí)際距離關(guān)系圖

圖11 全維視覺識別結(jié)果和實(shí)際距離關(guān)系圖

圖12 數(shù)據(jù)融合以后目標(biāo)距離與實(shí)際距離關(guān)系圖

3.3 結(jié)果分析

由角度定位實(shí)驗(yàn)表明，全維視覺的角度定位準(zhǔn)確度很高，最大誤差不超過4°，且誤差隨球與機(jī)器人的距離的增加而減小，這是由于，當(dāng)球離機(jī)器較近時(shí)，球的面積較大，識別的球的質(zhì)心跳動較大，所以誤差會較大；當(dāng)球離機(jī)器較遠(yuǎn)時(shí)，球的面積較小，識別的球的質(zhì)心跳動較小，所以誤差會較小。通常當(dāng)距離大于1.5米時(shí)，誤差就會減小下來。

由距離定位實(shí)驗(yàn)表明，當(dāng)球與機(jī)器人的距離小于2米時(shí)，全維與前向視覺的定位精度都比較高，且全維的精度要高于前向的。但當(dāng)距離進(jìn)一步增加時(shí)，全維和前向的誤差都隨之增大，全維的定位整體上大于真實(shí)距離，而前向的定位整體上小于真實(shí)距離，于是利用穩(wěn)態(tài)卡爾曼濾波進(jìn)行數(shù)據(jù)整合后，距離大于2米的定位數(shù)據(jù)驅(qū)于真實(shí)值。

4 結(jié) 論

本文提出了一種基于彩色圖像的目標(biāo)定位算法，主要解決機(jī)器人足球中準(zhǔn)確定位目標(biāo)問題；通過分別實(shí)現(xiàn)基于前向視覺和全維視覺的圖像分割、目標(biāo)的識別，繼而實(shí)現(xiàn)在前向和全維圖像下的目標(biāo)定位，最后采用Kalman濾波方法實(shí)現(xiàn)二者之間的融合，試驗(yàn)結(jié)果表明該方法是有效的，能夠?qū)崿F(xiàn)目標(biāo)的準(zhǔn)確定位。

[1] Wang S, Siskind J M. Image segmentation with ratio cut [J]. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 2003, 25(6):675-690.

[2] 王書朋, 姬紅兵. 自適應(yīng)尺度目標(biāo)跟蹤算法[J]. 計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào), 2008, 20(11): 1466-1470.

[3] 呂漢明, 王 揚(yáng), 王天軍. 基于混合策略的區(qū)域增長三角網(wǎng)格重構(gòu)算法[J]. 計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào), 2006, 18(9): 1426-1431.

[4] Tkalcic M, Tasic J F. Colour spaces: perceptual, historical and applicational background [C]// EUROCON 2003, Computer as a Tool. The IEEE Region 8.Turku, Finncon, 2003: 304-308.

[5] 伍 翼. 基于機(jī)器視覺的移動機(jī)器人控制研究[D]. 武漢: 華中科技大學(xué), 2005.

[6] 譚 琪. 基于全維視覺的目標(biāo)識別與定位[D]. 哈爾濱: 哈爾濱工業(yè)大學(xué), 2007.

[7] [美]Siegwart R, Nourbakhsh I R著. 自主移動機(jī)器人導(dǎo)論[M]. 李人厚譯. 西安: 西安交通大學(xué)出版社, 2006. 235-251.

An Object Localization Algorithm Based on Omni-direction Vision and Front Monocular Vision

FANG Bao-fu, PAN Qi-shu, HONG Bing-rong, ZHONG Qiu-bo, WANG Wei-guang

( 1. School of Computer Science and Technology, Harbin Institute of Technology, Harbin Heilongjiang 150001, China;2. School of Computer and Information, Hefei University of Technology, Hefei Anhui 230009, China )

In the robot soccer system, the environmental information around the robot is characterized in colors. On the analysis of the color threshold segmentation, an improved algorithm practicable in different color environments is proposed to realize the identification of objects in different colors in the field through omni-direction vision and front monocular vision, by the use of which, the robot’s object location through the two visions can be realized and then a more accurate object location can be achieved by combining the information from the two visions through Kalman filter algorithm. Experimental results prove the feasibility and validity of this algorithm which improves the accuracy of medium-and-long-distance object location.

digital image processing; Kalman filter; localization; color threshold

TP 391

A

1003-0158(2011)03-0006-07

2009-12-18

上海市機(jī)械自動化及機(jī)器人重點(diǎn)實(shí)驗(yàn)室開放課題（Z0802）；國家自然科學(xué)基金資助項(xiàng)目（60705015）；安徽省自然科學(xué)基金資助項(xiàng)目（070412064）

方寶富（1978-），男，安徽樅陽人，講師，博士研究生，主要研究方向?yàn)闄C(jī)器人視覺及多機(jī)器人協(xié)作與競爭。