基于視覺里程計和自然信標(biāo)融合的移動機(jī)器人定位算法

王 志，朱世強(qiáng)，李月華，張學(xué)群

（浙江大學(xué)流體動力與機(jī)電系統(tǒng)國家重點實驗室，杭州 310027）

基于視覺里程計和自然信標(biāo)融合的移動機(jī)器人定位算法

王 志，朱世強(qiáng)※，李月華，張學(xué)群

（浙江大學(xué)流體動力與機(jī)電系統(tǒng)國家重點實驗室，杭州 310027）

針對未知室內(nèi)環(huán)境下移動機(jī)器人的定位問題，提出了一種基于單目視覺和自然信標(biāo)融合的移動機(jī)器人定位算法。該方法采用普通攝像頭和全景攝像頭結(jié)合的方式進(jìn)行定位。采用普通攝像頭提取天花板特征，進(jìn)行視覺里程計定位。全景普通攝像頭提取周邊環(huán)境及天花板信息，采用Harris-SIFT構(gòu)建自然信標(biāo)庫，并且進(jìn)行自然信標(biāo)檢測與識別。在兩者的基礎(chǔ)上，移動機(jī)器人進(jìn)行實時定位。利用自然信標(biāo)定位時，提出方向濾波器結(jié)合視覺里程計和慣導(dǎo)系統(tǒng)估計旋轉(zhuǎn)角度，利用自然信標(biāo)求取定位特征點，兩者結(jié)合進(jìn)行定位。該方法有效地結(jié)合了相對定位的快速性和絕對定位的準(zhǔn)確性。在室內(nèi)環(huán)境進(jìn)行試驗，其中自然信標(biāo)定位在x，y方向的平均誤差分別為38.66和31.54 mm。相比視覺里程計而言，結(jié)合自然信標(biāo)定位的平均誤差分別減小了32.53%和68.68%。融合視覺里程計的自然信標(biāo)定位運(yùn)算速度相比僅靠自然信標(biāo)定位而言提高了約6倍左右，同時與視覺里程計定位處于同一數(shù)量級。試驗結(jié)果表明相對于視覺里程計而言，所提算法大大提高了定位精度，同時不影響算法的實時性。

機(jī)器視覺；機(jī)器人；圖像處理；視覺定位；自然信標(biāo)

王 志，朱世強(qiáng)，李月華，張學(xué)群. 基于視覺里程計和自然信標(biāo)融合的移動機(jī)器人定位算法[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(10)：70－77. doi：10.11975/j.issn.1002-6819.2017.10.009 http://www.tcsae.org

Wang Zhi, Zhu Shiqiang, Li Yuehua, Zhang Xuequn. Mobile robot localization algorithm by combining visual odometry with natural landmarks[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(10):70－77. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2017.10.009 http://www.tcsae.org

0 引 言

定位技術(shù)是移動機(jī)器人領(lǐng)域研究的熱點和難點之一。傳統(tǒng)的定位方式主要有：電磁引導(dǎo)[1]、超聲定位[2]、激光定位[3-6]、無線電定位[7]、慣導(dǎo)定位[8]以及 GPS定位等。這些定位方式大多只能用于簡單固定的環(huán)境，擴(kuò)展性較差，需對環(huán)境進(jìn)行相應(yīng)改造。近年來，由于視覺定位的便捷性，不需對環(huán)境進(jìn)行改造，在移動機(jī)器人領(lǐng)域得到了快速發(fā)展[9]。視覺定位通過獲取環(huán)境信息，檢測環(huán)境中的特征如點[10]、線[11]、特定區(qū)域[12]等獲取相機(jī)的位姿估計。視覺定位技術(shù)可分為絕對定位和相對定位。絕對定位主要是在環(huán)境中人為設(shè)置已知位置的信標(biāo)，根據(jù)信標(biāo)定位機(jī)器人[13]。由于信標(biāo)位置已知，因此該定位方式簡單可靠，但是適用面窄。一旦信標(biāo)被遮擋或損壞，定位過程將無法進(jìn)行。一般情況下，信標(biāo)主要設(shè)置在機(jī)器人周邊，但也有信標(biāo)設(shè)置在天花板上[14]。相對定位是指機(jī)器人通過對比前一時刻與當(dāng)前時刻環(huán)境中特征對應(yīng)關(guān)系獲得機(jī)器人當(dāng)前變化量，然后結(jié)合上一時刻位姿得到當(dāng)前位姿。該定位方式靈活，可用于機(jī)器人的實時定位與導(dǎo)航[15]。最常用的視覺相對定位技術(shù)主要是視覺里程計技術(shù)[16]。視覺里程計可分為單目視覺里程計、雙目視覺里程計以及全方位視覺里程計[17]。但是視覺相對定位存在累計誤差，不適合長距離定位。

基于視覺的定位需要快速、準(zhǔn)確地提取環(huán)境特征。楊國國等[18]卷積神經(jīng)網(wǎng)絡(luò)來識別圖像中目標(biāo)特征進(jìn)行害蟲定位與識別。賀付亮等[19]利用視覺圖像分割提取環(huán)境特征來識別桑葚。在室內(nèi)環(huán)境下，可以很容易地從地面、周邊環(huán)境以及天花板上提取。由于天花板具有不容易遮擋，變動小以及受光照影響小等特點，研究者多選用天花板作為視覺定位的參照物。韓立偉等[20]提出了一種基于直線和單特征點的視覺推算定位方法，主要提取天花板圖像中 2條垂直直線交點作為特征點，以及上下幀圖像間特征直線的夾角作為方位角，從而進(jìn)行定位。但該方法在每一時刻間都有累計誤差，造成定位結(jié)果不準(zhǔn)確。潘良晨等[21]等提出了一種基于特征跟蹤的定位方法。該方法仍然以天花板作為參照物，采集到特征點后根據(jù)卡爾曼濾波方法估算機(jī)器人當(dāng)前位姿。但是該方法本質(zhì)上仍是相對定位，無法得到準(zhǔn)確的絕對位置。Chen等[22]提出了一種基于天花板的視覺 SLAM，該算法主要包括天花板特征提取，特征定位和視覺里程計三部分，同時考慮到幾何約束。但是該算法運(yùn)算復(fù)雜度高，不適用于實時定位。此外由于室內(nèi)環(huán)境變動不大，因此也可作為定位參照物。馬建光等[23]提出了一種基于全向攝像機(jī)的定位方法。該方法主要全景攝像頭獲取全景圖像，然后進(jìn)行圖像壓縮，最后根據(jù)投影圖像的特征點和特征向量進(jìn)行定位。由于全景圖像畸變大，故定位精度不高[24]。

本文提出了一種基于視覺和自然信標(biāo)融合的移動機(jī)器人定位方法，該方法結(jié)合環(huán)境特征與天花板特征進(jìn)行定位。首先機(jī)器人在環(huán)境中選取特定點作為自然信標(biāo)，存儲其天花板特征以及環(huán)境特征。在定位過程中結(jié)合視覺里程計和自然信標(biāo)進(jìn)行定位。

1 機(jī)器人定位系統(tǒng)

本論文采用的機(jī)器人平臺為TurtleBot平臺，試驗系統(tǒng)如圖1所示。其視覺定位系統(tǒng)由PC機(jī)、普通彩色攝像機(jī)以及全景攝像機(jī)組成。2個攝像機(jī)光軸均垂直向上，垂直于天花板。普通攝像機(jī)采集天花板圖像，檢測得到天花板特征點與特征直線位置信息。全景攝像頭采集周邊環(huán)境圖像，通過環(huán)境圖像判斷機(jī)器人方位，然后結(jié)合天花板特征信息定位。整個定位系統(tǒng)流程圖如圖2所示。

圖1 試驗系統(tǒng)Fig.1 Experimental system

圖2 定位系統(tǒng)流程圖Fig.2 Flow diagram of proposed localization method

2 基于天花板的視覺里程計定位

由于天花板被遮擋的可能性較小，因此可以提取穩(wěn)定的特征。辦公室內(nèi)的天花板一般有矩形扣板和矩形日光燈，具有兩組直線，組內(nèi)直線平行，組間直線垂直，可以方便地提取特征直線。初始化時選定離圖像中心最近的直線交點作為特征點，特征直線的方向作為初始方向。在假定機(jī)器人不會大距離打滑的情況下根據(jù)下一幀圖像特征點位置和特征直線方向角進(jìn)行定位。其工作流程圖如圖3所示。

圖3 視覺里程計定位軟件流程圖Fig.3 Software flowchart of visual odometry

2.1 圖像處理與特征檢測

天花板之間有縫隙，形成了相鄰的矩形。但是由于光線噪聲等影響，會造成邊緣模糊，造成特征提取不準(zhǔn)確。因此需對圖像進(jìn)行濾波處理。本文選用改進(jìn)的導(dǎo)向濾波器進(jìn)行濾波[25-26]。導(dǎo)向濾波器具有較好的邊緣保持特性[27]。由于其運(yùn)算復(fù)雜度與支持窗口大小無關(guān)，因此具有較好的實時性。濾波函數(shù)可表示為：

式中G為參考圖像；I為輸入圖像，則濾波器輸出?Z可視為參考圖像G在窗口?ζ1(p)下的線性變換；“·”代表兩矩陣對應(yīng)元素相乘；μG,ζ1(p),μI,ζ1(p)和μG·I,ζ1(p)分別代表G,I和G·I的平均值。定義為

式中q是以點p為中心窗口的鄰域點；Z(p)是點p處濾波器的輸出值；Ni為鄰域點數(shù)目； 為常數(shù)。

經(jīng)圖像濾波處理后對圖像進(jìn)行特征直線檢測。本文采用hough算法檢測特征直線，但是由于hough算子比較耗時，因此本文只對定位參考圖像做全局檢索，得到特征直線和特征點信息。其他圖像均只在上一幀圖像特征點附近做局部檢索。加快了檢測速度，提高了算法實時性。

2.2 基于天花板視覺里程計定位模型

假定地面平整，天花板無遮擋物，同時地面與天花板平行。相機(jī)經(jīng)標(biāo)定后得到的焦距內(nèi)參數(shù)為kx,ky。假定天花板高度為h，則攝像機(jī)每一個像素所代表的世界坐標(biāo)系中的距離的比例系數(shù)為kl=h/kx。則相機(jī)坐標(biāo)系與世界坐標(biāo)系的對應(yīng)關(guān)系為式（3）。

式中udi,vdi為第i幀圖像特征點相對于光軸中心的坐標(biāo)；xwi,ywi為特征點在世界坐標(biāo)系下的位置；pxi,pyi為世界坐標(biāo)系相對于攝像機(jī)坐標(biāo)系的位置；φi為第i幀圖像攝像機(jī)的方位角。由式（3）可得到式（4）和式（5）。

式中ud,i+1,vd,i+1為第(i+1)幀圖像特征點相對于光軸中心的坐標(biāo)；px,i+1,py,i+1為第(i+1)幀圖像世界坐標(biāo)系相對于攝像機(jī)坐標(biāo)系的位置；φi+1為第(i+1)幀圖像攝像機(jī)的方位角。Δφi為第i幀與(i+1)幀圖像間角度變化量；Δpxi與Δpyi為第i幀與(i+1)幀圖像間位置變化量。

由對應(yīng)元素相等可得到：

在之前的研究中[10-11]，都是直接計算上下幀圖像之間的相對位置，然后逐步累積。而本文則采取當(dāng)前時刻圖像與初始時刻圖像進(jìn)行定位，只要特征點在視野范圍內(nèi)，參考圖像即為初始圖像，本文算法無需累計運(yùn)算，這樣可大大減小累計誤差，提高定位精度。因此機(jī)器人位置可表示為

式中px0,py0和ud0,vd0分別為參考定位圖像的特征點世界坐標(biāo)和圖像坐標(biāo)。然后將世界坐標(biāo)系相對于攝像機(jī)坐標(biāo)系的位置轉(zhuǎn)化為攝像機(jī)坐標(biāo)系相對于世界坐標(biāo)系的位置

由于攝像機(jī)始終垂直于天花板，所以各點縱坐標(biāo)已知且固定，只需計算平面坐標(biāo)，減小了計算難度。

3 基于自然信標(biāo)視覺定位

由于視覺里程計的累計特性，在長時間運(yùn)行時不可避免地將出現(xiàn)累計誤差。特別是機(jī)器人在一個閉合的軌跡上運(yùn)行時，誤差并不會隨機(jī)器人回到原點而清零，而是繼續(xù)累計。因此每隔一段距離需用自然信標(biāo)對機(jī)器人進(jìn)行絕對位置校正，這樣既不會影響定位的實時性，還能提高定位精度，去除累計誤差。

3.1 自然信標(biāo)庫建立

為了融合自然信標(biāo)進(jìn)行定位，在定位開始前需要首先建立自然信標(biāo)庫，然后基于自然信標(biāo)進(jìn)行定位。建立自然信標(biāo)的關(guān)鍵在于特征的提取與存儲以及自然信標(biāo)點的選取問題。目前常用的特征提取算法有：SIFT算子[28-29]、SURF算子[30]、FAST算子[31]以及Harris算子[32]等。其中SIFT算法是目前最成功的局部特征提取算法，其具有很好的尺度、旋轉(zhuǎn)以及光照不變性[28]。但是由于SIFI算子計算復(fù)雜，難以應(yīng)用到實時定位中。

在本文中，為了減小計算復(fù)雜度，選用 Harris算子來提取環(huán)境特征。Harris算子R具有計算簡單，對圖像旋轉(zhuǎn)及灰度變化具有魯棒性等特點。其值為：

式中det表示矩陣行列式，Tr為矩陣直跡，k為常數(shù)，C為相關(guān)矩陣，且

式中Iu(x),Iv(x),Iuv(x)分別為圖像在點x的灰度值在u和v方向的偏導(dǎo)和二階混合偏導(dǎo)。當(dāng)某點的R值大于設(shè)定值時，該點為特征點。

本文中的自然信標(biāo)融合天花板與周邊環(huán)境，因此需要對天花板圖像和環(huán)境全景圖像分別進(jìn)行特征提取和存儲。在建立自然信標(biāo)庫的時候，自然信標(biāo)點的選取也影響著機(jī)器人的定位精度。自然信標(biāo)數(shù)量太多，會造成運(yùn)算速度的下降，不利于實時定位；但是如果數(shù)量太少，則會積累較多的累計誤差。在本文中，選取原點作為第一個自然信標(biāo)，然后將環(huán)境圖像與自然信標(biāo)庫中的圖像進(jìn)行特征檢測與匹配，當(dāng)匹配特征點數(shù)目小于一定閾值T時，表明該點與遠(yuǎn)離自然信標(biāo)，可選取該點為新的自然信標(biāo)點，添加到自然信標(biāo)庫中，且賦予特定的ID編號。本文中取T=15。

3.2 自然信標(biāo)識別與定位

機(jī)器人借助自然信標(biāo)進(jìn)行定位，需準(zhǔn)確識別到自然信標(biāo)以及其ID。識別到非信標(biāo)點或者錯誤的信標(biāo)ID會造成很大的定位誤差，且難以校正。建立自然信標(biāo)庫后，與自然信標(biāo)卡中圖像逐一比對。若采用傳統(tǒng)的SIFT特征檢測與匹配算法會耗費(fèi)大量的時間。為了提高實時性，本文選用Harris-SIFT算法進(jìn)行自然信標(biāo)檢測與識別。選用Harris角點檢測方法替代SIFT算子檢測特征點，然后為每個特征點選擇主方向，生成特征向量描述子，最后根據(jù)特征向量間的歐氏距離作為匹配判據(jù)，找出匹配特征點。

機(jī)器人方位的判定對自身定位起著非常重要的作用。本文提出方向濾波器估計機(jī)器人旋轉(zhuǎn)角度。本文中的方向濾波器選取方向誤差作為狀態(tài)矢量，采用間接卡爾曼濾波的方法對機(jī)器人旋轉(zhuǎn)角度進(jìn)行估計，由于數(shù)據(jù)融合僅僅發(fā)生在誤差空間，近似線性空間，適合采用卡爾曼濾波器。假設(shè)當(dāng)前圖像I到自然信標(biāo)圖像N的旋轉(zhuǎn)變換用四元數(shù)表示，的估計值用表示，兩者的誤差用δq表示，則有：

?表示四元數(shù)相乘。令Δq表示視覺里程計計算得到的上下幀之間的旋轉(zhuǎn)變換，則有：

假設(shè)誤差δq服從均值為 0 的正態(tài)分布，則根據(jù)式（16）可得到：

式中，ΔR表示Δq對應(yīng)的旋轉(zhuǎn)矩陣。采用改進(jìn)的羅德里格參數(shù)變換可得到：

式中，w表示動態(tài)噪聲，no表示觀測噪聲。由于Xk,Yk是方向誤差空間的矢量，兩者可以視為不相關(guān)，因此動態(tài)噪聲w的協(xié)方差矩陣Q和觀測噪聲no的協(xié)方差矩陣W均為對角矩陣。為了更好地估計旋轉(zhuǎn)角度，本文結(jié)合慣導(dǎo)系統(tǒng)的旋轉(zhuǎn)角度進(jìn)行計算。利用視覺里程計計算Q，慣導(dǎo)系統(tǒng)計算W。

方向濾波器最終的輸出為：

利用Harris-SIFT算法檢測對當(dāng)前時刻天花板圖像與自然信標(biāo)天花板圖像進(jìn)行匹配，求得特征點位置。但是由于天花板特征非常相似，容易出現(xiàn)誤匹配，因此在匹配完成之后，仍采用上述聚類分析的方法，得出最佳匹配點。具體過程如下：1）輸入：包含n個匹配點的旋轉(zhuǎn)角度和簇的數(shù)目ki；2）輸出：ki個簇，使平方誤差準(zhǔn)則最小。具體步驟：1）任意選擇ki個對象作為初始的簇中心；2）根據(jù)簇中對象的平均值，將每個對象(重新)賦予最類似的簇；3）更新簇的平均值，即計算每個簇中對象的平均值；4）直到平均值不再發(fā)生變化。

根據(jù)對象數(shù)量最多的簇選取定位特征點。本文中取ki=3。然后利用3.2節(jié)中的方法進(jìn)行定位。即：

式中，px,refpy,ref和ud,refvd,ref和分別為自然信標(biāo)圖像的特征點世界坐標(biāo)和圖像坐標(biāo)。Δφi,ref為機(jī)器人相對于自然信標(biāo)的轉(zhuǎn)動角度。

4 驗結(jié)果與分析

4.1 自然信標(biāo)檢測試驗

自然信標(biāo)包含環(huán)境自然信標(biāo)和天花板自然信標(biāo)，在移動機(jī)器人行進(jìn)過程中，需對自然信標(biāo)進(jìn)行檢測，檢測結(jié)果如圖4和圖5所示。圖5中白色圓圈表示定位特征點。從圖4和圖5中可以看出，可根據(jù)匹配特征點的數(shù)量來判定自然信標(biāo)是否匹配成功。若匹配成功，便可根據(jù)自然信標(biāo)進(jìn)行定位。

圖4 不同時刻周圍自然信標(biāo)檢測結(jié)果Fig.4 Detection results of natural landmarks in different times

圖5 天花板自然信標(biāo)檢測結(jié)果圖Fig.5 Detection results of ceiling natural landmarks

4.2 總體定位試驗

在圖 1 所示移動機(jī)器人上，對本文提出的基于視覺和自然信標(biāo)融合的定位算法進(jìn)行了驗證，該平臺是一個機(jī)器人開源平臺，供用戶進(jìn)行二次開發(fā)。試驗在實際室內(nèi)環(huán)境中進(jìn)行?？刂茩C(jī)器人在一個長為6.3 m，寬為3.78 m的矩形軌跡上運(yùn)動。分別采用視覺里程計以及視覺里程計結(jié)合自然信標(biāo) 2種方式進(jìn)行定位，以機(jī)器人起始位置作為坐標(biāo)原點，以天花板縫隙方向分別作為x軸和y軸建立世界坐標(biāo)系。機(jī)器人沿逆時針運(yùn)動，最后回到原點。試驗共記錄了 264組試驗數(shù)據(jù)。為便于顯示，按一定間隔選取了77組數(shù)據(jù)進(jìn)行顯示，整體定位結(jié)果如圖6所示。

從圖 6中可以看出，視覺里程計定位軌跡與真實軌跡大致吻合，但是中間出現(xiàn)較大定位誤差，且回到原點后，累計誤差無法消除，會隨著機(jī)器人的運(yùn)動繼續(xù)增大。但是，融合自然信標(biāo)后的自然定位與真實位置基本吻合，在運(yùn)動過程中不會出現(xiàn)較大的定位誤差。而且由于自然信標(biāo)的存在，當(dāng)機(jī)器人回到原點后，可以根據(jù)自然信標(biāo)判斷已回到原點，這樣可以消除累計誤差，所以融合自然信標(biāo)定位的運(yùn)動軌跡可以實現(xiàn)閉環(huán)。視覺里程計定位和融合自然信標(biāo)定位的定位誤差曲線見圖7。從圖7中可以看出視覺里程計定位x軸的平均誤差為57.3 mm，y軸的平均誤差為100.69 mm，而融合自然信標(biāo)定位的x軸的平均誤差為38.66 mm，y軸的平均誤差為31.54 mm。相比視覺里程計而言，結(jié)合視覺里程計和自然信標(biāo)定位算法的x軸的平均誤差減小了32.53%，y軸的平均誤差減小了68.68%，因此融合自然信標(biāo)的視覺定位技術(shù)可以大大提高定位精度，同時不會影響算法的實時性。

圖6 機(jī)器人運(yùn)動軌跡圖Fig.6 Moving trajectory of robot

圖7 定位誤差圖Fig.7 Error map of localization

4.3 任意位置定位試驗

為了驗證本文所提的融合自然信標(biāo)定位的絕對定位特性以及其穩(wěn)定性。本文還做了任意點起步定位試驗。即機(jī)器人不是從原點出發(fā)，而是從任意位置出發(fā)。如果采用視覺里程計定位，機(jī)器人的位置又從原點開始定位，無法進(jìn)行絕對定位。而采用本文算法，則可以對任意位置進(jìn)行定位，且沒有累積誤差。本文隨機(jī)選取了 4個點作為測試點，然后進(jìn)行定位。4個點定位結(jié)果如表1所示。

表1 隨機(jī)位置定位結(jié)果Table 1 Localization results in random points mm

然后分別從位置2順時針運(yùn)動，位置3逆時針運(yùn)動，所得到的定位數(shù)據(jù)分別如表 2所示。為了方便顯示，只選用了其中部分結(jié)果顯示。其中位置2總共測試了64組測試點，其平均誤差為：x軸為36.3 mm，y軸為39.1 mm。位置 3總共測試了43組測試點，其平均誤差為：x軸為28.5 mm，y軸為42.2 mm。

表2 從任意位置起步定位結(jié)果Table 2 Localization results starting from random positions mm

從表 2中可以看出，本文所提的基于自然信標(biāo)定位的方法打破了傳統(tǒng)視覺里程計定位不能精確絕對定位的限制。當(dāng)自然信標(biāo)庫建立完成后，機(jī)器人可以在任意位置進(jìn)行絕對定位，提高了機(jī)器人的實用性。

4.4 算法實時性分析

在實時性方面，由于視覺里程計定位為相對定位，而且定位過程中采用局部搜索的方式選取特征點，實時性較好。在試驗中，視覺里程計的平均運(yùn)行時間是53 ms，自然信標(biāo)定位是 405 ms，而本文提出的融算法時間為69 ms。因此，融合視覺里程計的自然信標(biāo)定位算法的運(yùn)算速度相比僅靠自然信標(biāo)定位提高了約 6倍，同時其運(yùn)行時間與視覺里程計相當(dāng)，其精度卻大幅提高。因此，本文算法可以用于實時定位。

5 結(jié)論與討論

本文敘述了一種基于視覺和自然信標(biāo)融合的移動機(jī)器人定位方法。該方法將相對定位和絕對定位進(jìn)行了很好地結(jié)合。選用自然信標(biāo)結(jié)合視覺里程計進(jìn)行定位。在環(huán)境未知的辦公室環(huán)境下進(jìn)行多組試驗，試驗結(jié)果表明該方法在閉合路徑下可以實現(xiàn)閉環(huán)，無累計誤差。其平均誤差均在 50 mm以內(nèi)。其中x軸的平均誤差僅為38.66 mm，y軸的平均誤差僅為31.54 mm。該算法還能實現(xiàn)任意位置起步絕對定位，且平均誤差也在50 mm以內(nèi)。實時性方面，本文算法的運(yùn)算速度相比僅靠自然信標(biāo)定位提高了約 6倍，而且與視覺里程計相當(dāng)，可以應(yīng)用于機(jī)器人實時定位。

目前本文工作主要集中應(yīng)用于有方形塊的天花板的室內(nèi)環(huán)境。下一步工作中，主要研究怎樣快速建立自然信標(biāo)庫以及降低自然信標(biāo)庫誤識別與誤匹配，提高定位精度。同時擴(kuò)展到普通環(huán)境，能夠適用于其他類型的天花板及室內(nèi)環(huán)境，增強(qiáng)算法通用性。

[1]吳功平，曹珩，皮淵，等. 高壓多分裂輸電線路自主巡檢機(jī)器人及其應(yīng)用[J]. 武漢大學(xué)學(xué)報：工學(xué)版，2012，45(1)：96－102.Wu Gongping, Cao Yan, Pi Yuan, et al. An autonomous inspection robot for high voltage bundled transmission line and its application[J]. Engineering Journal of Wuhan University, 2012, 45(1): 96－102. (in Chinese with English abstract)

[2]苑晶，黃亞樓，孫鳳池. 面向移動機(jī)器人自定位的無線網(wǎng)絡(luò)構(gòu)造算法及實現(xiàn)[J]. 儀器儀表學(xué)報，2011，32(1)：99－106.Yuan Jing, Huang Yalou, Sun Fengchi. Construction algorithm and its implementation for wireless network in self-localization of mobile robot[J]. Chinese Journal of Scientific Instrument,2011, 32(1): 99－106. (in Chinese with English abstract)

[3]錢曉明，張浩，王曉勇，等. 基于激光掃描匹配的移動機(jī)器人相對定位技術(shù)研究[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2016，47(3)：14－21.Qian Xiaoming, Zhang Hao, Wang Xiaoyong, et al. Relative localization technology for mobile robot based on laser scan matching[J]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(3): 14－21. (in Chinese with English abstract)

[4]韓明瑞，周波，錢堃，等. 基于激光雷達(dá)的室外移動機(jī)器人三維定位和建圖[J]. 華中科技大學(xué)學(xué)報：自然科學(xué)版，2015(增刊 1)：315－318.Han Mingrui, Zhou Bo, Qian Kun, et al. 3D localization and mapping of outdoor mobile robots using a LIDAR[J]. Journal of Huazhong University of Science and Technology: Nature Science Edition, 2015(Supp.1): 315－318. (in Chinese with English abstract)

[5]劉洞波，劉國榮，胡慧，等. 基于激光測距的溫室移動機(jī)器人全局定位方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2010，41(5)：158－163.Liu Dongbo, Liu Guorong, Hu Hui, et al. Method of mobile robot global localization based on laser range finder in greenhouse[J]. Transactions of the Chinese Society for Agricultural Machinery, 2010, 41(5): 158－163. (in Chinese with English abstract)

[6]曾慧，吳福朝，胡占義. 基于平面激光測量的移動機(jī)器人自定位方法[J]. 自動化學(xué)報，2007，33(2)：138－144.Zeng Hui, Wu Fuchao, Hu Zhanyi. Robot self- localization based on planar laser measurement[J]. Acta Automatica Sinica, 2007, 33(2): 138－144. (in Chinese with English abstract)

[7]Mikhaylov K, Tikanm?ki A, Pet?j?j?rvi J, et al, On the selection of protocol and parameters for UWB-based wireless indoors localization[C]//International Symposium on Medical Information and Communication Technology (ISMICT).Worcester, MA, IEEE, 2016:1－5.

[8]馮國虎，吳文啟，曹聚亮，等. 單目視覺里程計/慣性組合導(dǎo)航算法[J]. 中國慣性技術(shù)學(xué)報，2011，19(3)：302－306.

[9]Ben-Afia A, Deambrogio L, Salos D, et al. Review and classification of vision-based localization techniques in unknown environments[J]. Iet Radar Sonar Navigation, 2014,8(9): 1059－1072.

[10]晁志超，伏思華，姜廣文，等. 單目攝像機(jī)-激光測距傳感器位姿測量系統(tǒng)[J]. 光學(xué)學(xué)報，2011，31(3)：0312001.Chao Zhichao, Fu Sihua, Jiang Guangwen, et al. Mono camera and laser range finding sensor position-pose measurement system[J]. Acta Optica Sinica, 2011, 31(3):0312001. (in Chinese with English abstract)

[11]趙振慶，葉東， 陳剛，等. 垂直直線特征的雙目視覺位姿測量方法[J]. 光學(xué)學(xué)報，2014，34(10)：1015003.Zhao Zhenqing, Ye Dong, Chen Gang, et al. Binocular vision method of measuring pose based on perpendicular lines[J].Acta Optica Sinica, 2014, 34(10): 1015003. (in Chinese with English abstract)

[12]侯俊捷，魏新國，孫軍華. 基于同心圓合成圖像匹配的雙目視覺標(biāo)定[J]. 光學(xué)學(xué)報，2012，32(3)：0315003.Hou Junjie, Wei Xinguo, Sun Junhua. Calibration method for binocular vision based on matching synthetic images of concentric circles[J]. Acta Optica Sinica, 2012, 32(3):0315003. (in Chinese with English abstract)

[13]厲茂海，洪炳镕，羅榮華，等. 基于單目視覺的移動機(jī)器人全局定位[J]. 機(jī)器人，2007，29(20)：140－144.Li Maohai, Hong Bingrong, Luo Ronghua, et al. Monocularvision-based mobile robot global localization[J]. Robot, 2007,29(20): 140－144. (in Chinese with English abstract)

[14]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－1625.

[15]Yu Jinxia，Cai Zixing，Duan Zhuohua, et a1. Design of dead reckoning system for mobile robot[J]. Journal of Central South University of Technology, 2006, 13(5): 542－547.

[16]Kitt B, Geiger A, Lategahn H. Visual odometry based on stereo image sequences with RANSAC-based outlier rejection scheme[J]. IEEE Intelligent Vehicles Symposium, 2010, 43(6):486－492.

[17]姜燕華，熊光明，姜巖，等. 智能車輛視覺里程計算法研究進(jìn)展[J]. 兵工學(xué)報，2012，33(2)：215－220.Jiang Yanhua, Xiong Guangming, Jiang Yan, et al. A review of visual odometry for intelligent vehicles[J]. Acta Armamentarii,2012, 33(2): 215－220. (in Chinese with English abstract)

[18]楊國國，鮑一丹，劉子毅. 基于圖像顯著性分析與卷積神經(jīng)網(wǎng)絡(luò)的茶園害蟲定位與識別[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(6)：156－162.Yang Guoguo, Bao Yidan, Liu Ziyi. Localization and recognition of pests in tea plantation based on image saliency analysis and convolutional neural network[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(6): 156－162. (in Chinese with English abstract)

[19]賀付亮，郭永彩，高潮，等. 基于視覺顯著性和脈沖耦合神經(jīng)網(wǎng)絡(luò)的成熟桑葚圖像分割[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(6)：148－155.He Fuliang, Guo Yongcai, Gao Chao, et al. Image segmentation of ripe mulberries based on visual saliency and pulse coupled neural network[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2017, 33(6): 148－155. (in Chinese with English abstract)

[20]韓立偉，徐德，基于直線和單特征點的移動機(jī)器人視覺推算定位法[J]. 機(jī)器人，2008，30(1)：79－90.Han Liwei, Xu De. A visual dead reckoning positioning method based on lines and single feature point for mobile robot[J]. Robot, 2008, 30(1): 79－90. (in Chinese with English abstract)

[21]潘良晨，陳衛(wèi)東. 室內(nèi)移動機(jī)器人的視覺定位方法研究[J].機(jī)器人，2006，28(5)：504－509.Pan Liangchen, Chen Weidong. Vision-based localization of indoor mobile robot[J]. Robot, 2006, 28(5): 504－509. (in Chinese with English abstract)

[22]Chen Pengjin, Gu Zhaopeng, Zhang Guodong, et al. Ceiling vision localization with feature pairs for home service robots[C]//2014 IEEE International Conference on Robotics and Biomimetics (ROBIO), Bali, IEEE, 2014: 2274－2279.

[23]馬建光，賈云得. 一種基于全向攝像機(jī)的移動機(jī)器人定位方法[J]. 北京理工大學(xué)學(xué)報，2003，23(3)：317－321.Ma Jianguang, Jia Yunde. An omnidirectional camera based localization for a mobile robot[J]. Transactions of Beijing Institute of Technology, 2003, 23(3): 317－321. (in Chinese with English abstract)

[24]王景川，陳衛(wèi)東，曹其新. 基于全景視覺與里程計的移動機(jī)器人自定位方法研究[J]. 機(jī)器人，2005，27(1)：41－45.Wang Jingchuan, Chen Weidong, Cao Qixin. Omni-vision and odometer based self-localization for mobile robot[J].Robot, 2005, 27(1): 41－45. (in Chinese with English abstract)

[25]Li Zhengguo, Zheng Jinghong, Zhu Zijian, et al. Weighted guided image filtering[J]. IEEE Transactions on Image Processing, 2015, 24(1): 120－129.

[26]Kou Fei, Chen Weihai, Wen Changyun, et al. Gradient domain guided image filtering[J]. IEEE Transactions on Image Processing. 2015, 24(11): 4528－4539.

[27]Zhu Shiqiang，Wang Zhi, Zhang Xuequn, et al. Edgepreserving guided filtering based cost aggregation for stereo matching[J]. Journal of Visual Communication and Image Represention, 2016, 39: 107－119.

[28]劉立，萬亞平，劉朝暉，等. 基于 SIFT匹配算法的移動機(jī)器人單目視覺定位研究[J]. 系統(tǒng)仿真學(xué)報，2012，24(9)：1823－1830.Liu Li, Wan Yaping, Liu Zhaohui, et al. Object localization for mobile robots using single-eye based on SIFT[J]. Journal of System Simulation, 2012, 24(9): 1823－1830. (in Chinese with English abstract)

[29]An Xuecheng, Hong Wei ，Xia Hua, Research on binocular vision absolute localization method for indoor robots based on natural landmarks[C]//Chinese Automation Congress(CAC), Wuhan, IEEE, 2015: 604－609.

[30]朱奇光，王佳，張朋珍，等. 基于高斯矩改進(jìn)SURF算法的移動機(jī)器人定位研究[J]. 儀器儀表學(xué)報，2015，36(11)：2451－2457.Zhu Qiguang, Wang Jia, Zhang Pengzhen, et al. Research on mobile robot localization based on gaussian moment improved SURF algorithm[J]. Chinese Journal of Scientific Instrument, 2015, 36(11): 2451－2457. (in Chinese with English abstract)

[31]王蒙，戴亞平. 基于FAST角點檢測的局部魯棒特征[J]. 北京理工大學(xué)學(xué)報，2013，33(10)：1045－1050.Wang Meng, Dai Yaping. Local robust feature based on FAST corner detection[J]. Transactions of Beijing Institute of Technology, 2013, 33(10): 1045－1050. (in Chinese with English abstract)

[32]趙欽君，趙東標(biāo)，韋虎. Harris-SIFT算法及其在雙目立體視覺中的應(yīng)用[J]. 電子科技大學(xué)學(xué)報，2010，39(4)：546－550.Zhao Qinjun, Zhao Dongbiao, Wei Hu. Harris-SIFT algorithm and its application in binocular stereo vision [J]. Journal of University of Electronic Science and Technology of China,2010, 39(4): 546－550. (in Chinese with English abstract)

Mobile robot localization algorithm by combining visual odometry with natural landmarks

Wang Zhi, Zhu Shiqiang※, Li Yuehua, Zhang Xuequn
(State Key Laboratory of Fluid Power and Mechatronic Systems, Zhejiang University, Hangzhou,310027,China)

Localization is crucial for the mobile robot system. Various localization methods have been proposed, such as light detection and ranging (LIDAR), ultrasonic method, WIFI, ultra wideband (UWB), vision odometry, and so on. The vision-based localization method has been intensively researched recently. Many researchers focus on the visual odometry,such as the monocular odometry and the stereo odometry. But the most obvious weakness of the visual odometry is the cumulative error, which will increase as the increasing of the displacement. To solve this problem, the artificial landmarks are adopted for absolute localization. But it needs previous setting and calibration. According to these observations, a mobile robot localization algorithm based on monocular vision and natural landmarks was proposed. The ordinary camera and omnidirectional camera are adopted in this method. The ordinary camera is used to extract the features from the ceiling for visual odometry, and the omnidirectional camera is used to extract the features from the environment and ceiling; then a natural landmark base is built using the Harris-SIFT (Scale-invariant feature transform) method. If there are not natural landmarks, the localization is implemented according to the change of the feature point between 2 neighboring images. If there exist natural landmarks, they will be used to localize the robot. During the visual odometry process, the feature point is extracted from the ceiling image and then tracked the feature point through the whole trajectory. The rotation angle and translation matrix can be determined by the coordinates of the feature point in the image and the world coordinate system. If the feature point is far away from the image center, it should be reselected so that it is close to the image center. The local search method for feature point is adopted to accelerate the localization. The search of feature point is only implemented near the feature point in the last frame. If there exist natural landmarks, the orientation filter is used to estimate the orientation angles. In this filter, the indirect Kalman filter is adopted, which uses the orientation error space as the state of the filter.Moreover, the orientation filter fuses the data of visual odometry and the inertial measurement unit to calculate the orientation angles. Then, the Harris-SIFT algorithm is used to determine the feature point for localization. To fully remove the outliers, the cluster analysis is implemented to cluster the matching points. And then the optimal matching is used to determine the feature point. The localization is implemented considering the orientation angles and the feature point obtained above. This algorithm has both of the advantages of the real-time performance of relative localization and the accuracy of absolute localization. The indoor experiment was implemented. The average errors of the proposed method inxaxis andyaxis were 38.66 and 31.54 mm,respectively. Compared with the visual odometry, the errors were decreased by 32.53% and 68.68%, respectively. As to the computational efficiency, the runtime of the localization based on only natural landmarks was almost 6 times higher than that of the proposed method. Thus, the proposed method can achieve real-time performance. The indoor experimental results demonstrate that compared with the visual odometry, the proposed method is much more accurate, and has a better extendibility.

computer vision; robots; image processing; visual localization; natural landmarks

10.11975/j.issn.1002-6819.2017.10.009

TP391.4

A

1002-6819(2017)-10-0070-08

2016-11-24

2017-04-15

國家自然科學(xué)基金創(chuàng)新群體基金（51521064）；杭州市創(chuàng)新鏈產(chǎn)業(yè)鏈重大科技創(chuàng)新項目（20132111A04）；杭州市重大科技創(chuàng)新項目（20142013A56）

王 志，男，博士生，主要從事雙目立體視覺方面的研究。杭州浙江大學(xué)流體動力與機(jī)電系統(tǒng)國家重點實驗室，310027。

Email：11325067@zju.edu.cn

※通信作者：朱世強(qiáng)，男，浙江義烏人，教授，博士，浙江省機(jī)器人產(chǎn)業(yè)技術(shù)聯(lián)盟理事長，中國自動化學(xué)會機(jī)器人專委會委員，主要從事機(jī)器人技術(shù)研究。杭州 浙江大學(xué)機(jī)械工程學(xué)院，310027。Email：sqzhu@zju.edu.cn。