基于視覺里程計和自然信標融合的移動機器人定位算法

王 志，朱世強，李月華，張學群

（浙江大學流體動力與機電系統國家重點實驗室，杭州 310027）

基于視覺里程計和自然信標融合的移動機器人定位算法

王 志，朱世強※，李月華，張學群

（浙江大學流體動力與機電系統國家重點實驗室，杭州 310027）

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

機器視覺；機器人；圖像處理；視覺定位；自然信標

王 志，朱世強，李月華，張學群. 基于視覺里程計和自然信標融合的移動機器人定位算法[J]. 農業工程學報，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 引 言

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

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

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

1 機器人定位系統

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

圖1 試驗系統Fig.1 Experimental system

圖2 定位系統流程圖Fig.2 Flow diagram of proposed localization method

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

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

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

2.1 圖像處理與特征檢測

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

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

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

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

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

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

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

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

由對應元素相等可得到：

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

式中px0,py0和ud0,vd0分別為參考定位圖像的特征點世界坐標和圖像坐標。然后將世界坐標系相對于攝像機坐標系的位置轉化為攝像機坐標系相對于世界坐標系的位置

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

3 基于自然信標視覺定位

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

3.1 自然信標庫建立

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

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

式中det表示矩陣行列式，Tr為矩陣直跡，k為常數，C為相關矩陣，且

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

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

3.2 自然信標識別與定位

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

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

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

假設誤差δq服從均值為 0 的正態分布，則根據式（16）可得到：

式中，ΔR表示Δq對應的旋轉矩陣。采用改進的羅德里格參數變換可得到：

式中，w表示動態噪聲，no表示觀測噪聲。由于Xk,Yk是方向誤差空間的矢量，兩者可以視為不相關，因此動態噪聲w的協方差矩陣Q和觀測噪聲no的協方差矩陣W均為對角矩陣。為了更好地估計旋轉角度，本文結合慣導系統的旋轉角度進行計算。利用視覺里程計計算Q，慣導系統計算W。

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

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

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

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

4 驗結果與分析

4.1 自然信標檢測試驗

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

圖4 不同時刻周圍自然信標檢測結果Fig.4 Detection results of natural landmarks in different times

圖5 天花板自然信標檢測結果圖Fig.5 Detection results of ceiling natural landmarks

4.2 總體定位試驗

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

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

圖6 機器人運動軌跡圖Fig.6 Moving trajectory of robot

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

4.3 任意位置定位試驗

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

表1 隨機位置定位結果Table 1 Localization results in random points mm

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

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

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

4.4 算法實時性分析

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

5 結論與討論

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

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

[1]吳功平，曹珩，皮淵，等. 高壓多分裂輸電線路自主巡檢機器人及其應用[J]. 武漢大學學報：工學版，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]. 儀器儀表學報，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]. 農業機械學報，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]韓明瑞，周波，錢堃，等. 基于激光雷達的室外移動機器人三維定位和建圖[J]. 華中科技大學學報：自然科學版，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]. 農業機械學報，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]. 自動化學報，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]馮國虎，吳文啟，曹聚亮，等. 單目視覺里程計/慣性組合導航算法[J]. 中國慣性技術學報，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]. 光學學報，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]. 光學學報，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]侯俊捷，魏新國，孫軍華. 基于同心圓合成圖像匹配的雙目視覺標定[J]. 光學學報，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]. 機器人，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]. 兵工學報，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]. 農業工程學報，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]. 農業工程學報，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]. 機器人，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]潘良晨，陳衛東. 室內移動機器人的視覺定位方法研究[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]. 北京理工大學學報，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]王景川，陳衛東，曹其新. 基于全景視覺與里程計的移動機器人自定位方法研究[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]. 系統仿真學報，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]朱奇光，王佳，張朋珍，等. 基于高斯矩改進SURF算法的移動機器人定位研究[J]. 儀器儀表學報，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]. 北京理工大學學報，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]趙欽君，趙東標，韋虎. Harris-SIFT算法及其在雙目立體視覺中的應用[J]. 電子科技大學學報，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

國家自然科學基金創新群體基金（51521064）；杭州市創新鏈產業鏈重大科技創新項目（20132111A04）；杭州市重大科技創新項目（20142013A56）

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

Email：11325067@zju.edu.cn

※通信作者：朱世強，男，浙江義烏人，教授，博士，浙江省機器人產業技術聯盟理事長，中國自動化學會機器人專委會委員，主要從事機器人技術研究。杭州 浙江大學機械工程學院，310027。Email：sqzhu@zju.edu.cn。