針對(duì)移動(dòng)機(jī)器人的局部視覺定位問題進(jìn)行了研究。首先通過移動(dòng)機(jī)器人視覺定位與目標(biāo)跟蹤系統(tǒng)求出目標(biāo)質(zhì)心特征點(diǎn)的位置時(shí)間序列，然后在分析二次成像法獲取目標(biāo)深度信息的缺陷的基礎(chǔ)上，提出了一種獲取目標(biāo)的空間位置和運(yùn)動(dòng)信息的方法。該方法利用序列圖像和推廣卡爾曼濾波，目標(biāo)獲取采用了HIS模型。在移動(dòng)機(jī)器人滿足一定機(jī)動(dòng)的條件下，較精確地得到了目標(biāo)的空間位置和運(yùn)動(dòng)信息。仿真結(jié)果驗(yàn)證了該方法的有效性和可行性。

　　運(yùn)動(dòng)視覺研究的是如何從變化場(chǎng)景的一系列不同時(shí)刻的圖像中提取出有關(guān)場(chǎng)景中的目標(biāo)的形狀、位置和運(yùn)動(dòng)信息，將之應(yīng)用于移動(dòng)機(jī)器人的導(dǎo)航與定位。首先要估計(jì)出目標(biāo)的空間位置和運(yùn)動(dòng)信息，從而為移動(dòng)機(jī)器人車體的導(dǎo)航與定位提供關(guān)鍵前提。

　　視覺信息的獲取主要是通過單視覺方式和多視覺方式。單視覺方式結(jié)構(gòu)簡(jiǎn)單，避免了視覺數(shù)據(jù)融合，易于實(shí)現(xiàn)實(shí)時(shí)監(jiān)測(cè)。如果利用目標(biāo)物體的幾何形狀模型，在目標(biāo)上取3個(gè)以上的特征點(diǎn)也能夠獲取目標(biāo)的位置等信息。此方法須保證該組特征點(diǎn)在不同坐標(biāo)系下的位置關(guān)系一致，而對(duì)于一般的雙目視覺系統(tǒng)，坐標(biāo)的計(jì)算誤差往往會(huì)破壞這種關(guān)系。

　　采用在機(jī)器人上安裝車載攝像機(jī)這種局部視覺定位方式，本文對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)視覺定位方法進(jìn)行了研究。該方法的實(shí)現(xiàn)分為兩部分：首先采用移動(dòng)機(jī)器人視覺系統(tǒng)求出目標(biāo)質(zhì)心特征點(diǎn)的位置時(shí)間序列，從而將對(duì)被跟蹤目標(biāo)的跟蹤轉(zhuǎn)化為對(duì)其質(zhì)心的跟蹤；然后通過推廣卡爾曼濾波方法估計(jì)目標(biāo)的空間位置和運(yùn)動(dòng)參數(shù)。

　　1.目標(biāo)成像的幾何模型

　　移動(dòng)機(jī)器人視覺系統(tǒng)的坐標(biāo)關(guān)系如圖1所示。

　　其中O-XYZ為世界坐標(biāo)系；Oc-XcYcZc為攝像機(jī)坐標(biāo)系。其中Oc為攝像機(jī)的光心，X軸、Y軸分別與Xc軸、Yc軸和圖像的x，y軸平行，Zc為攝像機(jī)的光軸，它與圖像平面垂直。光軸與圖像平面的交點(diǎn)O1為圖像坐標(biāo)系的原點(diǎn)。OcO1為攝像機(jī)的焦距f.

　　圖1 移動(dòng)機(jī)器人視覺系統(tǒng)的坐標(biāo)關(guān)系

　　不考慮透鏡畸變，則由透視投影成像模型為：

　　式中，Z′=［u，v］T為目標(biāo)特征點(diǎn)P在圖像坐標(biāo)系的二維坐標(biāo)值；（X，Y，Z）為P點(diǎn)在世界坐標(biāo)系的坐標(biāo)；（Xc0，Yc0，Zc0）為攝像機(jī)的光心在世界坐標(biāo)系的坐標(biāo)；dx，dy為攝像機(jī)的每一個(gè)像素分別在x軸與y軸方向采樣的量化因子；u0，v0分別為攝像機(jī)的圖像中心O1在x軸與y軸方向采樣時(shí)的位置偏移量。通過式（1）即可實(shí)現(xiàn)點(diǎn)P位置在圖像坐標(biāo)系和世界坐標(biāo)系的變換。

　　2.圖像目標(biāo)識(shí)別與定位跟蹤

　　2.1目標(biāo)獲取

　　目標(biāo)的獲取即在攝像機(jī)采集的圖像中搜索是否有特定目標(biāo)，并提取目標(biāo)區(qū)域，給出目標(biāo)在圖像中的位置特征點(diǎn)。

　　由于機(jī)器人控制實(shí)時(shí)性的需要，過于耗時(shí)的復(fù)雜算法是不適用的，因此以顏色信息為目標(biāo)特征實(shí)現(xiàn)目標(biāo)的獲取。本文采用了HSI模型，3個(gè)分量中，I是受光照影響較大的分量。所以，在用顏色特征識(shí)別目標(biāo)時(shí)，減少亮度特征I的權(quán)值，主要以H和S作為判定的主要特征，從而可以提高顏色特征識(shí)別的魯棒性。

　　考慮到連通性，本文利用捕獲圖像的像素及其八連通區(qū)域的平均HS特征向量與目標(biāo)像素的HS特征向量差的模是否滿足一定的閾值條件來(lái)判別像素的相似性；同時(shí)采用中心連接區(qū)域增長(zhǎng)法進(jìn)行區(qū)域增長(zhǎng)從而確定目標(biāo)區(qū)域。圖2給出了目標(biāo)區(qū)域分割的算法流程。

　　圖2 目標(biāo)區(qū)域分割算法流程

　　實(shí)現(xiàn)目標(biāo)區(qū)域提取后，由于目標(biāo)有一定的大小和形狀，為了對(duì)目標(biāo)定位，必須在圖像中選取目標(biāo)上對(duì)應(yīng)的點(diǎn)的圖像位置。由于目標(biāo)的質(zhì)心點(diǎn)具有不隨平移、旋轉(zhuǎn)與比例的改變而變化的特點(diǎn)，故選取目標(biāo)的質(zhì)心點(diǎn)作為目標(biāo)點(diǎn)。

　　質(zhì)心坐標(biāo)計(jì)算公式如下：

　　式中：

　　為質(zhì)心坐標(biāo)；n為目標(biāo)區(qū)域占據(jù)的像素個(gè)數(shù)，且n≥2;（xi，yi）為第i個(gè)像素的坐標(biāo)；p（xi，yi）為第i個(gè)像素的灰度值。

　　2.2目標(biāo)跟蹤

　　運(yùn)動(dòng)目標(biāo)的跟蹤是確定同一物體在不同幀中位置的過程，當(dāng)運(yùn)動(dòng)目標(biāo)被正確檢測(cè)出來(lái)時(shí)，它就對(duì)相鄰幀中檢測(cè)出的目標(biāo)進(jìn)行匹配。匹配過程如下：

　　2.2.1目標(biāo)質(zhì)心位置預(yù)測(cè)

　　目標(biāo)位置預(yù)測(cè)是依據(jù)最小二平方預(yù)測(cè)原理由目標(biāo)質(zhì)心在本幀以及相鄰的連續(xù)前幾幀的位置值，直接預(yù)測(cè)出目標(biāo)質(zhì)心在下一幀的位置值。在等間隔觀測(cè)條件下，可用式（4）的簡(jiǎn)便預(yù)測(cè)：

　　2.2.2搜索聚類的種子點(diǎn)

　　在搜索與上一幀圖像對(duì)應(yīng)質(zhì)心點(diǎn)匹配的點(diǎn)時(shí)，采用基于子塊的模式匹配方法。子模塊是由待匹配的點(diǎn)與周圍8個(gè)鄰點(diǎn)組成。由于這種方法充分考慮了特征點(diǎn)的統(tǒng)計(jì)特性，識(shí)別率大大提高。

　　首先從預(yù)測(cè)質(zhì)心點(diǎn)開始，在100×100像素的動(dòng)態(tài)窗口（以預(yù)測(cè)質(zhì)心點(diǎn)為中心）內(nèi)，按照逆時(shí)針?biāo)阉髦車?鄰域象素的趨勢(shì)進(jìn)行環(huán)狀搜索，并分別計(jì)算由每個(gè)搜索象素決定的子塊與上一幀的目標(biāo)質(zhì)心點(diǎn)T決定的子塊的HS特征值之差的平方和。

　　其中P［i］［j］（i，j=0，1，2）表示由點(diǎn)P決定的子塊中的各個(gè)像素；T［i］［j］（i，j=0，1，2）表示由上一幀的質(zhì)心點(diǎn)決定的子塊中的各個(gè)像素。

　　最后，判定某個(gè)點(diǎn)P是否與上一幀的特征點(diǎn)T匹配的標(biāo)準(zhǔn)為：P須同時(shí)滿足式（7，8）。

　　其中P.H表示待匹配點(diǎn)P的H特征值；meanH表示目標(biāo)區(qū)域的平均H特征值；滿足式（8）能夠保證匹配點(diǎn)在目標(biāo)區(qū)域內(nèi)。

　　2.2.3聚類色塊區(qū)域

　　其目的是找出色塊區(qū)域，色塊區(qū)域的質(zhì)心點(diǎn)即為特征跟蹤結(jié)果。在步驟（2）中已經(jīng)找到了聚類的起始點(diǎn)，由于H反映圖像的色彩特性，所以根據(jù)匹配點(diǎn)的H特征值是否在由色塊的平均H特征值確定的某個(gè)范圍內(nèi)來(lái)聚類色塊區(qū)域，即滿足式（8）。這樣既可保證識(shí)別精度，又減少了圖像信息計(jì)算量。

　　3.二次成像法

　　設(shè)Zc1，Zc2分別表示在t1，t2時(shí)刻目標(biāo)與成像系統(tǒng)的距離（深度值）;d1′，d2′分別表示t1，t2時(shí)刻目標(biāo)在圖像平面的幾何特征值，為便于表示，d1′，d2′可以是目標(biāo)的像的外接圓直徑或者外接矩形的邊長(zhǎng)，則有：

　　式（9）表明：根據(jù)同一目標(biāo)、同一攝像機(jī)所攝物體的圖像幾何特征的變化，可以計(jì)算出它們?cè)诳臻g深度方向運(yùn)動(dòng)時(shí)距離所發(fā)生的變化，這就是二次成像法的原理。

　　分析式（9）可知，二次成像法能夠確定目標(biāo)在攝像機(jī)坐標(biāo)系中的位置，但該方法在攝相機(jī)兩次成像的位置變化不大的情況下誤差會(huì)比較大，而且不能得到目標(biāo)的運(yùn)動(dòng)信息。為此本文提出了利用序列圖像和推廣卡爾曼濾波來(lái)估計(jì)目標(biāo)的空間位置和運(yùn)動(dòng)信息的方法。

　　4.目標(biāo)的空間位置和運(yùn)動(dòng)參數(shù)估計(jì)

　　由于圖像序列前后兩幀的時(shí)間間隔T很小，本文用二階微分方程來(lái)描述P點(diǎn)的運(yùn)動(dòng)軌跡。定義狀態(tài)矢量：

　　則可以定義狀態(tài)方程為：

　　其中：

　　V（k）為模型噪聲，假設(shè)V（k）為零均值的高斯白噪聲，其方差陣為Q（k）=cov（V）。

　　將式（1）離散化得

　　其中n（k）為測(cè)量噪聲。假設(shè)n（k）為零均值的高斯白噪聲，其方差陣為R（k）=cov（n）。

　　則式（10，11）組成系統(tǒng)的離散狀態(tài)方程和測(cè)量方程，當(dāng)該系統(tǒng)滿足可觀測(cè)性條件時(shí)，就可以應(yīng)用推廣卡爾曼濾波對(duì)目標(biāo)的空間位置和運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)。其中r（t），v（t）分別為目標(biāo)相對(duì)于車體的位置和速度，下標(biāo)t代表目標(biāo)，i代表成像系統(tǒng)，a（t）為任意的標(biāo)量。

　　5.實(shí)驗(yàn)結(jié)果

　　利用微軟提供的VFW視頻處理開發(fā)軟件包，由CCD攝像機(jī)和相應(yīng)的視頻采集卡獲取移動(dòng)機(jī)器人前的場(chǎng)景圖像數(shù)據(jù)，在Delphi6下開發(fā)了移動(dòng)機(jī)器人視覺定位與目標(biāo)跟蹤系統(tǒng)的完整程序。本算法在CPU主頻為500MHz，內(nèi)存為 256MB環(huán)境下，對(duì)幀速率為25幀？s，圖像分辨率為320×240的共180幀視頻圖像進(jìn)行了實(shí)驗(yàn)，最終實(shí)現(xiàn)了對(duì)運(yùn)動(dòng)目標(biāo)快速、穩(wěn)定的跟蹤。圖3給出了部分幀圖像的目標(biāo)定位與跟蹤結(jié)果。

　　圖3 目標(biāo)定位與跟蹤結(jié)果。

　　為了驗(yàn)證本文提出的對(duì)目標(biāo)的空間位置和運(yùn)動(dòng)參數(shù)估計(jì)算法的有效性，利用獲取的目標(biāo)質(zhì)心點(diǎn)的位置時(shí)間序列對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)進(jìn)行了跟蹤仿真實(shí)驗(yàn)。

　　由于仿真的相似性，本文只給出了推廣卡爾曼濾波在OZ方向的仿真結(jié)果，如圖4所示。其中圖4（a，b）分別是觀測(cè)噪聲方差為3個(gè)像素時(shí)目標(biāo)在Z軸方向的位置p和運(yùn)動(dòng)速度v的估計(jì)誤差曲線（150次MonteCarlo運(yùn)行）。其中目標(biāo)的起始位置為（115，1，10）m，速度為（110，115，215）m/s，加速度為（0125，011，015）m/s2;攝相機(jī)運(yùn)動(dòng)為實(shí)際中容易實(shí)現(xiàn)的且滿足機(jī)動(dòng)的條件，其初始位置為（010，015，010）m，初始速度為（015，0175，110）m/s，運(yùn)動(dòng)加速度為（0125，0105，015）m/s2.

　　圖4 推廣卡爾曼濾波Z方向（深度）的仿真結(jié)果

　　由仿真結(jié)果可見，隨著機(jī)器人車體的不斷機(jī)動(dòng)和濾波次數(shù)的增加，目標(biāo)位置的估計(jì)值在20幀左右就可收斂到理論真值，而且抖動(dòng)很小，可滿足系統(tǒng)快速定位與跟蹤要求。

　　6.結(jié)束語(yǔ)

　　本文對(duì)移動(dòng)機(jī)器人的局部視覺定位方法進(jìn)行了深入研究。二次成像法要求攝像機(jī)第二次成像時(shí)的位置要有較大變化，從而導(dǎo)致利用序列圖像所獲取的目標(biāo)位置信息誤差較大。與之相比本文提出的定位方法可更精確地得到目標(biāo)的空間位置和運(yùn)動(dòng)參數(shù)。這為移動(dòng)機(jī)器人的路徑規(guī)劃、伺服跟蹤等提供了更可靠的依據(jù)。