基于障礙物地平角點的移動機器人定位*

郭丙華，李中華

(1肇慶學院電子信息工程系，廣東 肇慶 526061;2.中山大學信息科學與技術學院，廣東 廣州 510006)

移動機器人定位是移動機器人研究領域的一項非常重要內容，移動機器人為完成某些任務，常常首先考慮的是自身的定位問題。針對這個問題，研究人員提出了許多定位算法，基于路標定位是其常用的一種方法，其采用的路標有兩種，一種是在特定的位置人工放置特殊的路標，通過傳感器對路標的識別來實現移動機器人的位姿估計。如文[1]的編碼路標，文[2]人為放置的不同寬度的圓環路標。由于需要在特殊的位置放置特別的路標，因而使得移動機器人的應用受到限制。另一種是采用傳感器提取環境中的某些特定標記作為路標，如文[3]在室內環境中通過視覺傳感器提取天花板上規則的平行線以及其相交的角點作為移動機器人定位的路標。文[4]利用視覺傳感器提取環境中的SIFT(Scale Invariant Feature Transform)特征點作為移動機器人的定位路標點。由于SIFT特征點的復雜性以及數量太多，其計算的復雜性隨之增大。文[5]提取環境中的SURF和U-SURF特征點并將其應用于移動機器人的長時間室外環境定位，其實驗結果證明采用SURF和U-SURF特征點的定位速度要優于SIFT特征點。文[6]則利用環境中的角點，采用RANSAC算法進行路徑估計，由于角點簡單，其定位算法的速度要快于采用SIFT和SURF特征點。

根據三角測量原理，三個路標點可以確定移動機器人在環境中的位姿[7]，路標點越多，采用定位算法估計的精度也越高，但算法計算的復雜度也越大。文[8]利用圖論模型(Graph-theoretic Formulation)提出了一種用于移動機器人定位的特征路標點數量最優選擇算法，顯著降低了用于定位所需的路標數量，但運動前需要傳感器預先離線獲取環境數據。

移動機器人的工作環境不可避免存在各種障礙物，障礙物與地平面的交界輪廓邊緣存在穩定的局部不變特征，而且這些特征只占整個環境中特征數量的小部分，以這些特征點作為移動機器人定位的路標點，其計算的復雜度將大幅度降低。本文采用立體視覺方法，結合Harris-Laplace算子、快速均值漂移方法構建了一個環境中障礙物與地平面交界輪廓的特征角點提取算法，即障礙物地平角點提取算法提取地平角點，并以這些角點為路標點結合自適應粒子濾波方法(APF)，實現移動機器人的位置估計。由于自適應粒子濾波方法的高效和魯棒特性，以及路標點(障礙物地平角點)大幅度的減少，因而可以使移動機器人在其運動的環境中實現快速、準確的自主定位。

1 場景3D重建模型

采用三角測量原理，立體視覺可以重建場景的3D信息。若以移動機器人的中心為坐標中心，根據攝像機的配置和投影關系[9]，當不考慮攝像頭的畸變時，可以得到圖像平面坐標(u,v)和場景3D坐標(xw,yw,zw)的關系式：

(1)

其中(u0,v0)為圖像平面的中心點坐標，B和h分別為左右攝像機的間距和安裝高度，m0和n0為攝像機在移動機器人坐標系的水平安裝位置，f為攝像機的焦距，D為左右攝像機圖像對的視差值，ux和uy為CCD單位像數長度。

通過上述方程(1)可以找到移動機器人空間點在圖像中的對應點。由于移動機器人工作的環境不可避免存在各種障礙物，為了利用方程(1)快速分離出障礙物地平角點，作如下約束：①移動機器人移動的平面為水平面;②場景中的障礙物定義為高出水平面的物體。這些約束對移動機器人工作于室內的環境來說是合理的。針對移動機器人工作的環境，提取的角點可以分為位于地平面上的角點PG和除此以外位于障礙物上的角點PO，考慮角點提取誤差，我們對提取的角點Pi(xi,yi,zi)如果滿足下式，則確定為地平面上角點，即

Pi(xi,yi,zi|zi<δ)?PG

(2)

其中δ為給定的閥值。在方程(1)中，yw為環境的深度坐標，各角點距離攝像頭的深度yi可由下式得到

yi=yw-n0

(3)

2 障礙物地平角點提取算法

角點是環境中存在的一類重要的局部不變特征點[10]，其表現在目標輪廓線上曲率的局部極大，具有旋轉、平移、縮放不變的性質，且計算量小，因而被廣泛應用于圖像配準與匹配、目標識別等方面，在此我們將其作為環境的路標點用于移動機器人的位姿估計。角點的提取常用Harris算子[10]，Harris算子具有平移和旋轉不變性，且對光照不敏感，但不具有尺度不變性，由于移動機器人在移動過程中，障礙物相對移動機器人，其尺度可能發生變化，我們擬采用Harris-Laplace算子[11]提取角點。在移動機器人整個工作環境中存在大量的角點，為了降低因大量的角點帶來的復雜計算，我們僅僅提取障礙物地平角點作為定位的路標點。

為了提取障礙物地平角點，我們擬借助于三維的立體視覺重構。由于采用Harris-Laplace算子提取的環境角點位于場景中的不同的位置，為了獲得所需的障礙物地平角點，需要去掉多余的角點。在此我們采用均值漂移方法對角點進行類聚，具體利用文[12]提供的快速均值漂移分割算法得到各個區域，由角點所處的區域進行角點類聚，并對類聚的角點采用最小二乘方法進行平面擬合。假定擬合的平面方程為：

zk=akx+bky+ck

(4)

式中k為擬合的第k個平面，ak、bk和ck分別為平面系數。若地平面的角點為Pi(xi,yi,0)，則障礙物地平角點也滿足方程(4)。但由于存在各種誤差，我們采用下式確定障礙物地平角點，即Pi到方程(4)的距離d滿足

(5)

式中δ為給定的閥值，可由具體的實驗確定。

障礙物地平角點的提取算法具體如下：

1) 采用Harris-Laplace算子提取場景角點。

2) 利用立體視覺匹配得到視差，并對角點進行3D重構，根據角點位置由方程(2)得到地平面的角點和其它角點。

3) 采用快速均值漂移分割方法對環境區域分割，并對分割后同一區域的角點進行聚類，再對類聚的角點進行平面擬合，得到平面方程(4)。

4) 對地平面角點利用方程(5)去掉多余的角點。

由上述算法得到的障礙物地平角點將用于下述的粒子濾波定位。

3 粒子濾波

粒子濾波(PF)是以貝葉斯濾波算法為基礎[13]，其采用隨機采樣方式并以樣本的估計來逼近后驗概率密度，在魯棒性和定位精度上具有較大的優勢。

假定移動機器人的位姿狀態χ可以用下式表示

χ={x,y,θ}

(6)

其中x,y為移動機器人的位置，θ為方向，并且狀態χ符合Markov假設。假定移動機器人在k時刻信度為Bel(χk)，則有

Bel(χk)=p(χk|z0～k)

(7)

z0～k表示0～k時刻的觀測結果，p(χk|z0～k)狀態預測估計的后驗概率。根據貝葉斯規則，可得

(8)

其中η=1/p(z0～k)，p(χk|χk-1)為狀態轉移概率密度，可以通過移動機器人的運動模型得到，p(zk|χk)為觀測測量概率密度，可從立體視覺觀測到的信息模型求得。對于方程(8)，粒子濾波方法采用一組加權的粒子集來表示位姿空間的概率分布，即：

(9)

4 運動模型與觀測模型

粒子濾波方法通過運動模型和觀測模型更新粒子分布，假定移動機器人采用兩后輪獨立驅動，兩后輪的角位移用旋轉編碼器測定，則其運動模型可用下式表示

(10)

其中mk-1，nk-1分別為在(k-1)時刻左、右輪的旋轉編碼器所測量出的脈沖個數，Mp為輪子轉動一周的脈沖個數，R、T分別為輪子半徑和兩輪子之間的寬度。取χk={xk,yk,θk}，設w(k-1)為隨機噪聲，則上式可表示為

χk=f(χk-1)+w(k-1)

(11)

移動機器人可以根據χk-1以及其分布按上述方程產生新的粒子分布，實現p(χk|χk-1)的預測過程。

粒子濾波進入校正階段，需要利用觀測模型來更新粒子的權重，通過提取較大權重的粒子得到真實的移動機器人位姿密度。觀測模型的建立來源于前述障礙物地平角點提取算法得到的角點深度信息和位置信息。

障礙物地平角點的深度表現為特征角點距離移動機器人的遠近。通過立體視覺方法，采用左右圖像對匹配可以得到其視差，由方程(1)可以求得各個障礙物地平角點的深度信息。假定觀測模型其概率符合高斯分布，則有

(12)

假定移動機器人在k時刻的中心坐標為(xc,yc)，其觀測到的障礙物地平角點的位置由方程(1)轉換到移動機器人的坐標系后為(xk,yk)，期望的距離為

(13)

移動機器人在k時刻觀測概率密度可由下式得到

(14)

其中N為提取到的障礙物地平角點的數量。

5 應用與實驗

由于粒子濾波方法的高效和魯棒特性，針對移動機器人的運動模型和觀測模型，我們采用粒子濾波算法來實現移動機器人的定位。為了有效降低計算的開銷，我們采用Dieter Fox提出的自適應粒子濾波算法[14]來實現移動機器人定位。該方法采用后驗概率分布與粒子集概率逼近之間的誤差的K-L距離動態地調整粒子數目，當概率密度集中在狀態空間的較小部分時采用較小的粒子數，否則采用較大的粒子數，具體細節參見文獻[14]，在其算法中插入由運動模型和觀測模型的數據更新。

移動機器人移動的地平面為水平面，圖1為實驗中的移動機器人和實驗環境，移動機器人上安裝的左右攝像頭標定的內外參數矩陣如下：

左右攝像機的內參數分別為AL和AR，即

兩攝像頭之間的位置關系(旋轉矩陣R和T平移矩陣)為：

為了測試和驗證本文提出的算法的性能，我們進行了一系列的實驗，主要包括室內環境的定位實驗和性能對比實驗。實驗中移動機器人的運動速度v≤0.5 m/s，方程(2)和方程(3)中取δ=10。整個算法是在Ti的DSP處理器(DM642)上運行，其中移植了部分OpenCv中函數庫。

5.1 室內環境的定位實驗

實驗的室內環境見圖1所示，圖2～圖6為實驗測試的結果。圖2為校正后的左右立體圖像對，圖3為通過立體圖像對匹配算法得到的視差圖，為了能夠快速得到其視差圖，我們采用了OpenCV[15]函數庫中提供的塊匹配函數 cvFindStereoCorrespondenceBM函數獲得其視差，并利用cvReprojectImageTo3D函數重建特征角點的3D位置，從圖中可以看到還有少量匹配不正確的位置，由于最終只需重建特征角點的3D位置，因而可以忽略。

圖1 實驗用的移動機器人和實驗環境Fig.1 Mobile robot and experiment environment

圖2 經過校正后的左右攝像機圖像對Fig.2 Rectified stereo image pairs

圖3 匹配后得到的視差圖Fig.3 Disparity map

圖4、5、6為采用上述的障礙物地平角點提取算法得到角點，最終得到的障礙物地平角點將作為路標用于移動機器人定位。圖4為采用Harris-Laplace算子提取的所有環境角點。圖5為采用均值漂移分割得到各個區域并對區域的角點進行平面方程擬合結果。圖6中的“+”標記為最終得到的特征角點，比較圖4和圖6可以看到，用于定位的角點數量已大大減少。

圖4 提取的Harris角點Fig.4 Extracted harris corner

圖5 角點所在的平面Fig.5 Harris corner plane

圖6 障礙物地平角點圖Fig.6 Obstacle ground corner

圖7為移動機器人在實驗環境中的定位軌跡圖，其中藍線為移動機器人移動的軌跡，紅線為采用粒子濾波估計的軌跡。從圖中可以看出，在拐彎處實際軌跡與估計軌跡誤差比較大，這可能是移動機器人在拐彎時容易發生滑動。由于立體視覺計算出角點的深度其誤差隨著距離的增大而變大，為提高估計的精度，對深度距離超過4 m的角點不予考慮。

圖7 定位的環境和軌跡圖Fig.7 Trajectory of the mobile robot

5.2 性能對比實驗

為了測試算法的性能，我們采用了環境中的SIFT、SURF、角點(Harris)和地平角點等特征點進行了實驗，在移動機器人運動的路徑上我們選取了4個位置進行性能對比，見圖7中A、B、C和D標識，其中A位置近似直線運動，B位置大拐彎運動，C位置面對多個障礙物，D位置小拐彎運動，對比的性能包括算法運行的速度，定位的精度以及與文[6]所述算法的對比實驗。

圖8為移動機器人在上述4個位置提取的4種特征點的數量，其中地平角點的數量相對其它3種要少得多，圖中超過4 m以外的特征點均已排除掉。

圖8 4種特征點提取數量對比圖Fig.8 4 Kinds of feature points extraction quantity

圖9是在圖7中的A、B、C和D 等4個位置分別提取SIFT、SURF、角點(Harris)和地平角點(O-G-Corner) 4種特征點用于本文的定位算法所需的時間圖，圖中帶“▽”的連線為文[6]中利用環境角點采用RANSAC運動估計算法在上述4個位置定位所需的時間。從圖中看出在算法速度上采用地平角點的定位算法要快于采用SIFT、SURF和角點特征點，與文[6]采用的算法相比較，完成定位所需的時間更少，在圖9中，在A位置的定位所需的時間較大，這主要是由于粒子濾波定位初期，需要更多的粒子，因而完成定位需要更多的時間。

圖9 算法運行時間對比圖Fig.9 Run-time comparison

圖10為本文算法在4個位置的定位精度，與文[6]中采用RANSAC運動估計算法一樣均能達到較高的定位精度。

圖10 算法定位精度圖Fig.10 Localization accuracy comparison

6 結 論

角點是一類重要的局部不變特征點，而環境中不可避免存在障礙物，特別是在室內環境。與環境中存在的所有特征角點相比較，障礙物地平角點只占環境中角點數量的一小部分，以其為路標點可以實現移動機器人高效快速的定位。本文采用立體視覺方法，結合Harris-Laplace算子和快速均值漂移方法提出了一種障礙物地平角點提取算法，并利用自適應粒子濾波方法融合障礙物地平角點的深度和位置信息，實現了移動機器人高效快速和較為精確的定位，實驗的結果證明了上述方法是行之有效的。

參考文獻：

[1] 王景川, 陳衛東, 張栩. 一種魯棒的室外移動機器人路標定位系統[J], 上海交通大學學報, 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] 熊會元, 宗志堅, 余志, 等. 基于凸包的棋盤格角點自動識別與定位方法[J]. 中山大學學報:自然科學版, 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]. 于仕琪, 劉瑞禎, 譯. 北京: 清華大學出版社, 2011.