郭丙華,李中華
(1肇慶學(xué)院電子信息工程系,廣東 肇慶 526061;2.中山大學(xué)信息科學(xué)與技術(shù)學(xué)院,廣東 廣州 510006)
移動(dòng)機(jī)器人定位是移動(dòng)機(jī)器人研究領(lǐng)域的一項(xiàng)非常重要內(nèi)容,移動(dòng)機(jī)器人為完成某些任務(wù),常常首先考慮的是自身的定位問題。針對(duì)這個(gè)問題,研究人員提出了許多定位算法,基于路標(biāo)定位是其常用的一種方法,其采用的路標(biāo)有兩種,一種是在特定的位置人工放置特殊的路標(biāo),通過(guò)傳感器對(duì)路標(biāo)的識(shí)別來(lái)實(shí)現(xiàn)移動(dòng)機(jī)器人的位姿估計(jì)。如文[1]的編碼路標(biāo),文[2]人為放置的不同寬度的圓環(huán)路標(biāo)。由于需要在特殊的位置放置特別的路標(biāo),因而使得移動(dòng)機(jī)器人的應(yīng)用受到限制。另一種是采用傳感器提取環(huán)境中的某些特定標(biāo)記作為路標(biāo),如文[3]在室內(nèi)環(huán)境中通過(guò)視覺傳感器提取天花板上規(guī)則的平行線以及其相交的角點(diǎn)作為移動(dòng)機(jī)器人定位的路標(biāo)。文[4]利用視覺傳感器提取環(huán)境中的SIFT(Scale Invariant Feature Transform)特征點(diǎn)作為移動(dòng)機(jī)器人的定位路標(biāo)點(diǎn)。由于SIFT特征點(diǎn)的復(fù)雜性以及數(shù)量太多,其計(jì)算的復(fù)雜性隨之增大。文[5]提取環(huán)境中的SURF和U-SURF特征點(diǎn)并將其應(yīng)用于移動(dòng)機(jī)器人的長(zhǎng)時(shí)間室外環(huán)境定位,其實(shí)驗(yàn)結(jié)果證明采用SURF和U-SURF特征點(diǎn)的定位速度要優(yōu)于SIFT特征點(diǎn)。文[6]則利用環(huán)境中的角點(diǎn),采用RANSAC算法進(jìn)行路徑估計(jì),由于角點(diǎn)簡(jiǎn)單,其定位算法的速度要快于采用SIFT和SURF特征點(diǎn)。
根據(jù)三角測(cè)量原理,三個(gè)路標(biāo)點(diǎn)可以確定移動(dòng)機(jī)器人在環(huán)境中的位姿[7],路標(biāo)點(diǎn)越多,采用定位算法估計(jì)的精度也越高,但算法計(jì)算的復(fù)雜度也越大。文[8]利用圖論模型(Graph-theoretic Formulation)提出了一種用于移動(dòng)機(jī)器人定位的特征路標(biāo)點(diǎn)數(shù)量最優(yōu)選擇算法,顯著降低了用于定位所需的路標(biāo)數(shù)量,但運(yùn)動(dòng)前需要傳感器預(yù)先離線獲取環(huán)境數(shù)據(jù)。
移動(dòng)機(jī)器人的工作環(huán)境不可避免存在各種障礙物,障礙物與地平面的交界輪廓邊緣存在穩(wěn)定的局部不變特征,而且這些特征只占整個(gè)環(huán)境中特征數(shù)量的小部分,以這些特征點(diǎn)作為移動(dòng)機(jī)器人定位的路標(biāo)點(diǎn),其計(jì)算的復(fù)雜度將大幅度降低。本文采用立體視覺方法,結(jié)合Harris-Laplace算子、快速均值漂移方法構(gòu)建了一個(gè)環(huán)境中障礙物與地平面交界輪廓的特征角點(diǎn)提取算法,即障礙物地平角點(diǎn)提取算法提取地平角點(diǎn),并以這些角點(diǎn)為路標(biāo)點(diǎn)結(jié)合自適應(yīng)粒子濾波方法(APF),實(shí)現(xiàn)移動(dòng)機(jī)器人的位置估計(jì)。由于自適應(yīng)粒子濾波方法的高效和魯棒特性,以及路標(biāo)點(diǎn)(障礙物地平角點(diǎn))大幅度的減少,因而可以使移動(dòng)機(jī)器人在其運(yùn)動(dòng)的環(huán)境中實(shí)現(xiàn)快速、準(zhǔn)確的自主定位。
采用三角測(cè)量原理,立體視覺可以重建場(chǎng)景的3D信息。若以移動(dòng)機(jī)器人的中心為坐標(biāo)中心,根據(jù)攝像機(jī)的配置和投影關(guān)系[9],當(dāng)不考慮攝像頭的畸變時(shí),可以得到圖像平面坐標(biāo)(u,v)和場(chǎng)景3D坐標(biāo)(xw,yw,zw)的關(guān)系式:
(1)
其中(u0,v0)為圖像平面的中心點(diǎn)坐標(biāo),B和h分別為左右攝像機(jī)的間距和安裝高度,m0和n0為攝像機(jī)在移動(dòng)機(jī)器人坐標(biāo)系的水平安裝位置,f為攝像機(jī)的焦距,D為左右攝像機(jī)圖像對(duì)的視差值,ux和uy為CCD單位像數(shù)長(zhǎng)度。
通過(guò)上述方程(1)可以找到移動(dòng)機(jī)器人空間點(diǎn)在圖像中的對(duì)應(yīng)點(diǎn)。由于移動(dòng)機(jī)器人工作的環(huán)境不可避免存在各種障礙物,為了利用方程(1)快速分離出障礙物地平角點(diǎn),作如下約束:①移動(dòng)機(jī)器人移動(dòng)的平面為水平面;②場(chǎng)景中的障礙物定義為高出水平面的物體。這些約束對(duì)移動(dòng)機(jī)器人工作于室內(nèi)的環(huán)境來(lái)說(shuō)是合理的。針對(duì)移動(dòng)機(jī)器人工作的環(huán)境,提取的角點(diǎn)可以分為位于地平面上的角點(diǎn)PG和除此以外位于障礙物上的角點(diǎn)PO,考慮角點(diǎn)提取誤差,我們對(duì)提取的角點(diǎn)Pi(xi,yi,zi)如果滿足下式,則確定為地平面上角點(diǎn),即
Pi(xi,yi,zi|zi<δ)?PG
(2)
其中δ為給定的閥值。在方程(1)中,yw為環(huán)境的深度坐標(biāo),各角點(diǎn)距離攝像頭的深度yi可由下式得到
yi=yw-n0
(3)
角點(diǎn)是環(huán)境中存在的一類重要的局部不變特征點(diǎn)[10],其表現(xiàn)在目標(biāo)輪廓線上曲率的局部極大,具有旋轉(zhuǎn)、平移、縮放不變的性質(zhì),且計(jì)算量小,因而被廣泛應(yīng)用于圖像配準(zhǔn)與匹配、目標(biāo)識(shí)別等方面,在此我們將其作為環(huán)境的路標(biāo)點(diǎn)用于移動(dòng)機(jī)器人的位姿估計(jì)。角點(diǎn)的提取常用Harris算子[10],Harris算子具有平移和旋轉(zhuǎn)不變性,且對(duì)光照不敏感,但不具有尺度不變性,由于移動(dòng)機(jī)器人在移動(dòng)過(guò)程中,障礙物相對(duì)移動(dòng)機(jī)器人,其尺度可能發(fā)生變化,我們擬采用Harris-Laplace算子[11]提取角點(diǎn)。在移動(dòng)機(jī)器人整個(gè)工作環(huán)境中存在大量的角點(diǎn),為了降低因大量的角點(diǎn)帶來(lái)的復(fù)雜計(jì)算,我們僅僅提取障礙物地平角點(diǎn)作為定位的路標(biāo)點(diǎn)。
為了提取障礙物地平角點(diǎn),我們擬借助于三維的立體視覺重構(gòu)。由于采用Harris-Laplace算子提取的環(huán)境角點(diǎn)位于場(chǎng)景中的不同的位置,為了獲得所需的障礙物地平角點(diǎn),需要去掉多余的角點(diǎn)。在此我們采用均值漂移方法對(duì)角點(diǎn)進(jìn)行類聚,具體利用文[12]提供的快速均值漂移分割算法得到各個(gè)區(qū)域,由角點(diǎn)所處的區(qū)域進(jìn)行角點(diǎn)類聚,并對(duì)類聚的角點(diǎn)采用最小二乘方法進(jìn)行平面擬合。假定擬合的平面方程為:
zk=akx+bky+ck
(4)
式中k為擬合的第k個(gè)平面,ak、bk和ck分別為平面系數(shù)。若地平面的角點(diǎn)為Pi(xi,yi,0),則障礙物地平角點(diǎn)也滿足方程(4)。但由于存在各種誤差,我們采用下式確定障礙物地平角點(diǎn),即Pi到方程(4)的距離d滿足
(5)
式中δ為給定的閥值,可由具體的實(shí)驗(yàn)確定。
障礙物地平角點(diǎn)的提取算法具體如下:
1) 采用Harris-Laplace算子提取場(chǎng)景角點(diǎn)。
2) 利用立體視覺匹配得到視差,并對(duì)角點(diǎn)進(jìn)行3D重構(gòu),根據(jù)角點(diǎn)位置由方程(2)得到地平面的角點(diǎn)和其它角點(diǎn)。
3) 采用快速均值漂移分割方法對(duì)環(huán)境區(qū)域分割,并對(duì)分割后同一區(qū)域的角點(diǎn)進(jìn)行聚類,再對(duì)類聚的角點(diǎn)進(jìn)行平面擬合,得到平面方程(4)。
4) 對(duì)地平面角點(diǎn)利用方程(5)去掉多余的角點(diǎn)。
由上述算法得到的障礙物地平角點(diǎn)將用于下述的粒子濾波定位。
粒子濾波(PF)是以貝葉斯濾波算法為基礎(chǔ)[13],其采用隨機(jī)采樣方式并以樣本的估計(jì)來(lái)逼近后驗(yàn)概率密度,在魯棒性和定位精度上具有較大的優(yōu)勢(shì)。
假定移動(dòng)機(jī)器人的位姿狀態(tài)χ可以用下式表示
χ={x,y,θ}
(6)
其中x,y為移動(dòng)機(jī)器人的位置,θ為方向,并且狀態(tài)χ符合Markov假設(shè)。假定移動(dòng)機(jī)器人在k時(shí)刻信度為Bel(χk),則有
Bel(χk)=p(χk|z0~k)
(7)
z0~k表示0~k時(shí)刻的觀測(cè)結(jié)果,p(χk|z0~k)狀態(tài)預(yù)測(cè)估計(jì)的后驗(yàn)概率。根據(jù)貝葉斯規(guī)則,可得
(8)
其中η=1/p(z0~k),p(χk|χk-1)為狀態(tài)轉(zhuǎn)移概率密度,可以通過(guò)移動(dòng)機(jī)器人的運(yùn)動(dòng)模型得到,p(zk|χk)為觀測(cè)測(cè)量概率密度,可從立體視覺觀測(cè)到的信息模型求得。對(duì)于方程(8),粒子濾波方法采用一組加權(quán)的粒子集來(lái)表示位姿空間的概率分布,即:
(9)
粒子濾波方法通過(guò)運(yùn)動(dòng)模型和觀測(cè)模型更新粒子分布,假定移動(dòng)機(jī)器人采用兩后輪獨(dú)立驅(qū)動(dòng),兩后輪的角位移用旋轉(zhuǎn)編碼器測(cè)定,則其運(yùn)動(dòng)模型可用下式表示
(10)
其中mk-1,nk-1分別為在(k-1)時(shí)刻左、右輪的旋轉(zhuǎn)編碼器所測(cè)量出的脈沖個(gè)數(shù),Mp為輪子轉(zhuǎn)動(dòng)一周的脈沖個(gè)數(shù),R、T分別為輪子半徑和兩輪子之間的寬度。取χk={xk,yk,θk},設(shè)w(k-1)為隨機(jī)噪聲,則上式可表示為
χk=f(χk-1)+w(k-1)
(11)
移動(dòng)機(jī)器人可以根據(jù)χk-1以及其分布按上述方程產(chǎn)生新的粒子分布,實(shí)現(xiàn)p(χk|χk-1)的預(yù)測(cè)過(guò)程。
粒子濾波進(jìn)入校正階段,需要利用觀測(cè)模型來(lái)更新粒子的權(quán)重,通過(guò)提取較大權(quán)重的粒子得到真實(shí)的移動(dòng)機(jī)器人位姿密度。觀測(cè)模型的建立來(lái)源于前述障礙物地平角點(diǎn)提取算法得到的角點(diǎn)深度信息和位置信息。
障礙物地平角點(diǎn)的深度表現(xiàn)為特征角點(diǎn)距離移動(dòng)機(jī)器人的遠(yuǎn)近。通過(guò)立體視覺方法,采用左右圖像對(duì)匹配可以得到其視差,由方程(1)可以求得各個(gè)障礙物地平角點(diǎn)的深度信息。假定觀測(cè)模型其概率符合高斯分布,則有
(12)
假定移動(dòng)機(jī)器人在k時(shí)刻的中心坐標(biāo)為(xc,yc),其觀測(cè)到的障礙物地平角點(diǎn)的位置由方程(1)轉(zhuǎn)換到移動(dòng)機(jī)器人的坐標(biāo)系后為(xk,yk),期望的距離為
(13)
移動(dòng)機(jī)器人在k時(shí)刻觀測(cè)概率密度可由下式得到
(14)
其中N為提取到的障礙物地平角點(diǎn)的數(shù)量。
由于粒子濾波方法的高效和魯棒特性,針對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)模型和觀測(cè)模型,我們采用粒子濾波算法來(lái)實(shí)現(xiàn)移動(dòng)機(jī)器人的定位。為了有效降低計(jì)算的開銷,我們采用Dieter Fox提出的自適應(yīng)粒子濾波算法[14]來(lái)實(shí)現(xiàn)移動(dòng)機(jī)器人定位。該方法采用后驗(yàn)概率分布與粒子集概率逼近之間的誤差的K-L距離動(dòng)態(tài)地調(diào)整粒子數(shù)目,當(dāng)概率密度集中在狀態(tài)空間的較小部分時(shí)采用較小的粒子數(shù),否則采用較大的粒子數(shù),具體細(xì)節(jié)參見文獻(xiàn)[14],在其算法中插入由運(yùn)動(dòng)模型和觀測(cè)模型的數(shù)據(jù)更新。
移動(dòng)機(jī)器人移動(dòng)的地平面為水平面,圖1為實(shí)驗(yàn)中的移動(dòng)機(jī)器人和實(shí)驗(yàn)環(huán)境,移動(dòng)機(jī)器人上安裝的左右攝像頭標(biāo)定的內(nèi)外參數(shù)矩陣如下:
左右攝像機(jī)的內(nèi)參數(shù)分別為AL和AR,即
兩攝像頭之間的位置關(guān)系(旋轉(zhuǎn)矩陣R和T平移矩陣)為:
為了測(cè)試和驗(yàn)證本文提出的算法的性能,我們進(jìn)行了一系列的實(shí)驗(yàn),主要包括室內(nèi)環(huán)境的定位實(shí)驗(yàn)和性能對(duì)比實(shí)驗(yàn)。實(shí)驗(yàn)中移動(dòng)機(jī)器人的運(yùn)動(dòng)速度v≤0.5 m/s,方程(2)和方程(3)中取δ=10。整個(gè)算法是在Ti的DSP處理器(DM642)上運(yùn)行,其中移植了部分OpenCv中函數(shù)庫(kù)。
實(shí)驗(yàn)的室內(nèi)環(huán)境見圖1所示,圖2~圖6為實(shí)驗(yàn)測(cè)試的結(jié)果。圖2為校正后的左右立體圖像對(duì),圖3為通過(guò)立體圖像對(duì)匹配算法得到的視差圖,為了能夠快速得到其視差圖,我們采用了OpenCV[15]函數(shù)庫(kù)中提供的塊匹配函數(shù) cvFindStereoCorrespondenceBM函數(shù)獲得其視差,并利用cvReprojectImageTo3D函數(shù)重建特征角點(diǎn)的3D位置,從圖中可以看到還有少量匹配不正確的位置,由于最終只需重建特征角點(diǎn)的3D位置,因而可以忽略。
圖1 實(shí)驗(yàn)用的移動(dòng)機(jī)器人和實(shí)驗(yàn)環(huán)境Fig.1 Mobile robot and experiment environment
圖2 經(jīng)過(guò)校正后的左右攝像機(jī)圖像對(duì)Fig.2 Rectified stereo image pairs
圖3 匹配后得到的視差圖Fig.3 Disparity map
圖4、5、6為采用上述的障礙物地平角點(diǎn)提取算法得到角點(diǎn),最終得到的障礙物地平角點(diǎn)將作為路標(biāo)用于移動(dòng)機(jī)器人定位。圖4為采用Harris-Laplace算子提取的所有環(huán)境角點(diǎn)。圖5為采用均值漂移分割得到各個(gè)區(qū)域并對(duì)區(qū)域的角點(diǎn)進(jìn)行平面方程擬合結(jié)果。圖6中的“+”標(biāo)記為最終得到的特征角點(diǎn),比較圖4和圖6可以看到,用于定位的角點(diǎn)數(shù)量已大大減少。
圖4 提取的Harris角點(diǎn)Fig.4 Extracted harris corner
圖5 角點(diǎn)所在的平面Fig.5 Harris corner plane
圖6 障礙物地平角點(diǎn)圖Fig.6 Obstacle ground corner
圖7為移動(dòng)機(jī)器人在實(shí)驗(yàn)環(huán)境中的定位軌跡圖,其中藍(lán)線為移動(dòng)機(jī)器人移動(dòng)的軌跡,紅線為采用粒子濾波估計(jì)的軌跡。從圖中可以看出,在拐彎處實(shí)際軌跡與估計(jì)軌跡誤差比較大,這可能是移動(dòng)機(jī)器人在拐彎時(shí)容易發(fā)生滑動(dòng)。由于立體視覺計(jì)算出角點(diǎn)的深度其誤差隨著距離的增大而變大,為提高估計(jì)的精度,對(duì)深度距離超過(guò)4 m的角點(diǎn)不予考慮。
圖7 定位的環(huán)境和軌跡圖Fig.7 Trajectory of the mobile robot
為了測(cè)試算法的性能,我們采用了環(huán)境中的SIFT、SURF、角點(diǎn)(Harris)和地平角點(diǎn)等特征點(diǎn)進(jìn)行了實(shí)驗(yàn),在移動(dòng)機(jī)器人運(yùn)動(dòng)的路徑上我們選取了4個(gè)位置進(jìn)行性能對(duì)比,見圖7中A、B、C和D標(biāo)識(shí),其中A位置近似直線運(yùn)動(dòng),B位置大拐彎運(yùn)動(dòng),C位置面對(duì)多個(gè)障礙物,D位置小拐彎運(yùn)動(dòng),對(duì)比的性能包括算法運(yùn)行的速度,定位的精度以及與文[6]所述算法的對(duì)比實(shí)驗(yàn)。
圖8為移動(dòng)機(jī)器人在上述4個(gè)位置提取的4種特征點(diǎn)的數(shù)量,其中地平角點(diǎn)的數(shù)量相對(duì)其它3種要少得多,圖中超過(guò)4 m以外的特征點(diǎn)均已排除掉。
圖8 4種特征點(diǎn)提取數(shù)量對(duì)比圖Fig.8 4 Kinds of feature points extraction quantity
圖9是在圖7中的A、B、C和D 等4個(gè)位置分別提取SIFT、SURF、角點(diǎn)(Harris)和地平角點(diǎn)(O-G-Corner) 4種特征點(diǎn)用于本文的定位算法所需的時(shí)間圖,圖中帶“▽”的連線為文[6]中利用環(huán)境角點(diǎn)采用RANSAC運(yùn)動(dòng)估計(jì)算法在上述4個(gè)位置定位所需的時(shí)間。從圖中看出在算法速度上采用地平角點(diǎn)的定位算法要快于采用SIFT、SURF和角點(diǎn)特征點(diǎn),與文[6]采用的算法相比較,完成定位所需的時(shí)間更少,在圖9中,在A位置的定位所需的時(shí)間較大,這主要是由于粒子濾波定位初期,需要更多的粒子,因而完成定位需要更多的時(shí)間。
圖9 算法運(yùn)行時(shí)間對(duì)比圖Fig.9 Run-time comparison
圖10為本文算法在4個(gè)位置的定位精度,與文[6]中采用RANSAC運(yùn)動(dòng)估計(jì)算法一樣均能達(dá)到較高的定位精度。
圖10 算法定位精度圖Fig.10 Localization accuracy comparison
角點(diǎn)是一類重要的局部不變特征點(diǎn),而環(huán)境中不可避免存在障礙物,特別是在室內(nèi)環(huán)境。與環(huán)境中存在的所有特征角點(diǎn)相比較,障礙物地平角點(diǎn)只占環(huán)境中角點(diǎn)數(shù)量的一小部分,以其為路標(biāo)點(diǎn)可以實(shí)現(xiàn)移動(dòng)機(jī)器人高效快速的定位。本文采用立體視覺方法,結(jié)合Harris-Laplace算子和快速均值漂移方法提出了一種障礙物地平角點(diǎn)提取算法,并利用自適應(yīng)粒子濾波方法融合障礙物地平角點(diǎn)的深度和位置信息,實(shí)現(xiàn)了移動(dòng)機(jī)器人高效快速和較為精確的定位,實(shí)驗(yàn)的結(jié)果證明了上述方法是行之有效的。
參考文獻(xiàn):
[1] 王景川, 陳衛(wèi)東, 張栩. 一種魯棒的室外移動(dòng)機(jī)器人路標(biāo)定位系統(tǒng)[J], 上海交通大學(xué)學(xué)報(bào), 2010, 44(9):1181-1186.
[2] TWEDDLE B. Computer vision based navigation for spacecraft proximity operations[D]. Cambridge, USA: Massachusetts Institute of Technology, 2010.
[3] XU De, HAN Liwei, TAN Min, et al. Ceiling-based visual positioning for an indoor mobile robot with monocular
vision[J]. IEEE Transactions on Industrial Electronics, 2009, 56(5):1617-1628.
[4] CAMPOS F M, CORREIA L, CALADO J M F. Mobile robot global localization with non-quantized SIFT features[C]//The 15th International Conference on Advanced Robotics, Tallinn, Estonia : IEEE, 2011: 582-587.
[5] VALGREN C, LILIENTHAL A J. SIFT, SURF & seasons: appearance-based long-term localization in outdoor environments[J]. Robotics and Autonomous Systems, 2010, 58(2):149-156.
[7] BETKE M, GURVITS L. Mobile robot localization using landmarks[J]. IEEE Transactions on Robotics and Automation, 1997, 13(2):251-263.
[8] SALA P L, SIM R, SHOKOUFANDEH A, et al. Landmark selection for vision-based navigation[J]. IEEE Transactions on Robotics, 2006, 22(2):334-349.
[9] TSAI R. A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses[J]. IEEE J. Robotics and Automation. 1987, 3(4): 323-344.
[10] 熊會(huì)元, 宗志堅(jiān), 余志, 等. 基于凸包的棋盤格角點(diǎn)自動(dòng)識(shí)別與定位方法[J]. 中山大學(xué)學(xué)報(bào):自然科學(xué)版, 2009, 48(1):1-5.
[11] MIKOLAJCZYK K, SCHMID C. Scale & affine invariant interest point detectors[J]. International Journal of Computer Vision, 2004, 60( 1) :63-86.
[12] ARTHUR D,VASSILVITSKII S. K-means++: The advantages of careful seeding[C]//Proc. ACM-SIAM Symp. on Discrete Algorithms, New Orleans, Louisiana : IEEE, 2007: 1027-1035.
[13] THRUN S, BEETZ M, BENNEWITZ M, et al. Robust monte carlo localization for mobile robots[J]. Artificial Intelligence, 2000, 128(1/2): 99-141.
[14] FOX D. Adapting the sample size in particle filters through KLD-sampling[J]. International Journal of Robotics Research (IJRR), 2003, 22(12): 985-1004.
[15] BRADSKI G, KAEBLER A. Learning open CV[M]. 于仕琪, 劉瑞禎, 譯. 北京: 清華大學(xué)出版社, 2011.