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

?

一種基于全維視覺與前向單目視覺的目標定位算法

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

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

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

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

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

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

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

1 基于顏色閾值的目標識別

1.1 圖像預處理

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

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

（1）

1.2 基于顏色閾值的目標識別

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

圖1 閾值長方體投影I

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

2 基于全維視覺和前向單目視覺的目標定位

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

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

2.1.1 角度定位

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

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

(a) 源圖像坐標系?????????(b) 機器人坐標系

圖3 坐標變換示意圖

2.1.2 距離定位

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

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

2.2 前向單目視覺的目標定位

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

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

圖4 單目攝像機定位模型

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

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

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

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

圖5 單目攝像機修正定位模型

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

2.3 基于信息融合的目標定位

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

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

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

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

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

（8）

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

（10）

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

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

（12）

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

-＝

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

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

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

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

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

3 實驗結(jié)果及分析

3.1 角度測量實驗

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

圖7 實驗過程說明

圖8 角度實驗結(jié)果

3.2 距離定位實驗

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

圖9 實驗過程說明

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

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

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

3.3 結(jié)果分析

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

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

4 結(jié) 論

本文提出了一種基于彩色圖像的目標定位算法，主要解決機器人足球中準確定位目標問題；通過分別實現(xiàn)基于前向視覺和全維視覺的圖像分割、目標的識別，繼而實現(xiàn)在前向和全維圖像下的目標定位，最后采用Kalman濾波方法實現(xiàn)二者之間的融合，試驗結(jié)果表明該方法是有效的，能夠?qū)崿F(xià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)尺度目標跟蹤算法[J]. 計算機輔助設(shè)計與圖形學學報, 2008, 20(11): 1466-1470.

[3] 呂漢明, 王 揚, 王天軍. 基于混合策略的區(qū)域增長三角網(wǎng)格重構(gòu)算法[J]. 計算機輔助設(shè)計與圖形學學報, 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] 伍 翼. 基于機器視覺的移動機器人控制研究[D]. 武漢: 華中科技大學, 2005.

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

[7] [美]Siegwart R, Nourbakhsh I R著. 自主移動機器人導論[M]. 李人厚譯. 西安: 西安交通大學出版社, 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

上海市機械自動化及機器人重點實驗室開放課題（Z0802）；國家自然科學基金資助項目（60705015）；安徽省自然科學基金資助項目（070412064）

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