專利名稱：一種基于顏色路標的移動機器人視覺導航方法
技術領域：
本發明涉及智能控制領域，具體涉及移動機器人的視覺導航方法。
背景技術：
自從20世紀60年代初，第一臺工業機器人發明以來，機器人的發展已 有半個多世紀，機器人的應用越來越廣泛，幾乎滲透到所有的領域。機器人
大致經過三代的演變第一代是可編程的示教再現型機器人；第二代是具有 一定感覺功能和自適應能力的離線編程機器人；第三代機器人是智能機器人。 機器人正在向智能化和視覺化的趨勢發展，智能化和視覺化的機器系統將從 事目前傳統機器系統和人類難以勝任的惡劣環境下的一些操作。
移動機器人是機器人學中的一個重要分支，是一類能夠通過傳感器感知 環境和自身狀態，實現在非結構環境下，動態決策與規劃、行為控制與執行 等多項功能于一體的高智能化機器系統。它與機器人學、通訊技術、計算機
視覺、多傳感器信息融合、智能控制以及多智能體(Multi-Agent)、機械學等 學科密切相關，體現了信息科學和人工智能技術的最新成果，具有重要的軍 用及民用價值，是現代機器人學中一個重要而且相當活躍的研究領域。
眾所周知，人們從外界環境獲取的信息中，80%來自于視覺，其他來自于 觸覺、聽覺、嗅覺等感覺器官。當人們的眼睛從自己周圍的環境獲取大量信 息，并傳入大腦后，由大腦根據知識或經驗，對信息進行加工、推理等處理
工作，最后識別、理解周圍環境包括環境內的對象物、物體間的相對位置、 形狀、大小、顏色及紋理；從而使人能與周圍環境進行有效的信息交互，完 成各種復雜的任務與行動。同樣，移動機器人感知外部環境也需要"眼睛"， 那就是機器視覺。現在，視覺系統己成為移動機器人重要的組成部分之一。 移動機器人視覺可以定義為這樣一個過程利用視覺傳感器獲取三維景物的 二維圖像，通過視覺處理算法對一幅或多幅圖像進行處理、分析和解釋，得到有關景物的符號描述，并為特定的任務提供有用的信息，用于引導機器人
的動作。
在移動機器人的研究課題中，對移動機器人的視覺導航進行研究是一個 重要方面。視覺信息是人類感知環境世界的主要途徑，也是移動機器人視覺 導航的關鍵技術。如果機器人能夠能像人一樣通過視頻處理而具有從外部環 境獲取信息的能力，這樣就不僅大大提高了它們環境適應和生存能力，更重 要的是使人機交互更加方便自然。因此這方面的研究工作具有重要的理論意 義和潛在的應用價值。
世界上第一個自治機器人Shakey(Nilsson 1984)，就使用了基于單色圖 像的障礙物檢測方法。Shakey是在無質感的地面上行走，障礙物可以通過對 單色輸入圖像進行邊緣檢測來檢測。但是Shakey能行走的環境需要人工專門 設置。障礙物的表面不能有反射并且表面的顏色都是均勻統一的。光源也需 要特別放置，使得墻和地面不能出現反射和陰影。Becker等使用綁縛在屋頂 的簡單路標來實現機器人的導航。St印hen Se和David Lowe提出了利用視覺 路標進行移動機器人全局定位和地圖生成算法。該算法在沒有任何修改的環 境中利用尺度不變特征變換(Scale Invariant Feature Transform, SIFT) 的來提取環境圖像特征作為自然路標，實現了機器人的定位和地圖構建。
但現有技術針對移動機器人視覺導航中，存在自然路標的魯棒性不高， 圖像處理時間復雜度高等缺點。
發明內容
本發明針對移動機器人視覺導航中，自然路標的魯棒性不高，圖像處理 時間復雜度高等缺點，提出了基于顏色路標的移動機器人視覺導航的方法。
本發明實現基于顏色路標的移動機器人視覺導航方法所采用的技術方案， 具體包括如下步驟
設計帶有兩個不同顏色環的圓柱體作為機器人視覺導航的路標，對不同的路標加以編號并存儲；機器人根據不同路標的色帶組合依次尋找路標，算 法模塊根據公式丄=~^^/實時計算移動機器人到路標的距離L;移動機器人
在開始運動時以初始行進的方向為x軸建立機器人坐標系，里程計等慣性器
件實時檢測機器人的當前坐標(X， Y)和旋轉角度TH，調用公式 U + Zcos77/ (2) Pr = r + isin77/ (3) 確定路標坐標(PX， PY);
機器人行進過程中，獲取每一時刻路標坐標0^,^J，以及機器人本身的
坐標和旋轉角度(^，;^A)，機器人與路標的距離《；采用粒子群優化算法
(ParticleSwarm Optimization, PSO)確定最優坐標值。最優坐標值應滿足距 離適應度函數與角度適應度函數最小。
尋找路標并識別的過程中，首先在HSI(HSI:Hue， Saturation, Intensity, 色調，飽和度，亮度)顏色空間下，以H為主閾值，I為輔助閾值的閾值分割法和 基于S分量邊緣點數量統計的分割法相結合，分割路標上的兩色塊，并得到它 們的面積、圖像坐標等信息。通過兩個顏色塊的面積大小相近小于某一閾值 進行初步的路標判斷，采用水平方向的兩色塊的中心坐標之差小于某一閾值 作為路標識別的進一步判斷，根據兩色塊的顏色并與存儲空間中已經編號的 路標比對最終完成路標的識別。
本發明的有益效果本發明是一種基于顏色路標的移動機器人視覺導航 的方法，與常規的移動機器人視覺導航方法相比，具有人工路標簡單，圖像 處理復雜度低。機器人對路標的識別魯棒性高，并通過數學建模，完成了移 動機器人的視覺重定位。完成了機器人學習環境和利用環境認識自我的結合。
圖1機器人視覺導航流程圖 圖2人工路標示意3路標成像模型示意圖 圖4機器人視覺重定位示意圖
具體實施例方式
本發明設計了一種基于顏色路標的移動機器人視覺導航方法，移動機器人
在開始運動時以初始行進的方向為X軸建立默認的坐標系，這個坐標系稱作 機器人坐標系.，也稱為局部坐標系，機器人中的里程計等慣性器件獲取得到 每時刻移動機器人的在局部坐標系中的位置坐標，當然也可以通過坐標旋轉 平移得到移動機器人的全局坐標。里程計在短時間內的坐標是較為準確的， 但隨著時間的增加由于移動機器人車輪的打滑，路面的不平整等因素會使坐 標的誤差累積不斷加大，移動機器人的坐標就會出現與實際情況偏差變大。 此時，移動機器人利用設置的路標，通過視覺獲取并計算路標的最優坐標值 來求得機器人的準確坐標進行全局定位。
如圖1所示為機器人視覺導航示意流程圖，具體包括如下步驟
首先設計帶有不同顏色環的圓柱體作為機器人視覺導航的路標，對不同 的路標加以編號并存儲；
然后機器人根據不同路標的色帶組合依次尋找路標，實時計算移動機器 人到路標的距離，并引入粒子群算法獲得路標的最優坐標。
最后根據里程計誤差，判斷是否引入機器人視覺重定位模塊來重置機器 人位姿，以實現持續安全的導航。
在目標檢測和識別中，顏色作為物體表面的一個特征提供目標的位置或 姿態等重要信息。常用的彩色圖像目標檢測和識別方法大多基于固定的顏色 閾值，這種方法簡單，處理速度快，但由于空間光線變化的影響，故在目標 識別和檢測時的魯棒性不盡人意。對于移動機器人準確快速實現路標的識別 要求來說，重點在魯棒性、實時性以及符合要求的精度要求。在視覺圖像的 處理程序中， 一些簡單的信息通過顏色的閾值分割邊界聯接而得到，如不同的顏色通道(Channel)、相同顏色下一定數目的顏色區域也即色塊(Blob)、 色塊的周邊坐標信息(Top， Bottom、 Left、 Right)、色塊的面積大小(Area)、 色塊中心坐標'(Center of Gravity of the blob, XCG YCG)等。為保證路標 設定的魯棒性，每個路標使用兩種與環境差異較大的顏色Channell和 Channel2，使兩色帶寬度相同，兩色帶間距為一色帶寬度，并對不同的路標 加以編號，進行存儲。通過色環的圖像信息，實現機器人對路標的識別和對 準。引入改進的灰度相關匹配法，優化了兩色環中心垂直距離值。通過建立 機器人坐標系和路標的成像模型，得到了路標的全局坐標，并最終引入粒子 群算法對其優化。建立數學模型，完成了基于路標的機器人視覺重定位。
具體采用如下方法來設計路標，設計帶有兩個不同顏色環色帶的圓柱體 作為機器人視覺導航的路標，兩色環的顏色選取與周圍環境差異較大的顏色， 例如紅色、綠色等，色環帶的寬度和兩色環帶間距為一固定值。色環帶的兩 兩組合可以方便的區分不同的路標，路標設計成圓柱體可以保證機器人在各 個角度都能獲得無差別的路標圖像，從而確保了機器人對路標識別的魯棒性。 對不同的路標加以編號并存儲。
在HSI顏色空間下，以H為主閾值，I為輔助閾值的閾值分割法和基于S分 量邊緣點數量統計的分割法相結合，實現對彩色路標的分割。對路標圖像分 割的目的是要分割出柱子上的兩色塊，并得到它們的面積、圖像坐標等圖像 信息，以用于路標識別。根據我們路標設計的特點和環境的復雜性，我們選 取以區域和邊緣相結合的彩色圖像分割方法。具體做法是在HSI顏色空間中以 H分量為主閾值，S分量為輔助閾值對彩色圖像進行閾值圖像分割；對I分量圖 像首先用Canny算子進行邊緣檢測，然后用基于邊緣點的數量統計分割出色環 區域，將兩幅分割圖像合成，完成路標圖像中對色塊的最終分割。
通過分割的色塊采取以下相應的措施進行路標的識別 1)通過兩個顏色塊的面積大小相近小于某一閾值進行初步的路標判斷，這樣在路標不完全進入攝像頭視野時也不影響路標的識別，如根據公式
|Areal-Area2|/( Areal+Area2)〈2呢進行初步判定，其中，Areal禾B Area2分 別為亮色塊面積；
2) 采甩水平方向的兩色塊的中心坐標之差(|Xra- X^l)小于某一閾值 Threshold作為路標識別的進一步判斷，防止視野中相同顏色其他色塊的干 擾，如根據公式IXe(;「 X』〈Threshold進一步判斷；
3) 根據兩色塊的顏色并與存儲空間中已經編號的路標比對最終完成路標的識
另lj
機器人可根據不同路標的色帶組合依次尋找人工路標的柱子，找到后對 準路標，并向其駛近直到機器人到路標的距離接近設定值。.
假設圖2是路標通過攝像頭在成像平面的投影，兩色塊的中心坐標為Xce，
攝像頭中兩色塊垂直方向中心距離為AYo;。由圖3的路標成像模型可知，在移 動機器人焦距f，路標的兩色帶中心距離W，以及攝像頭中兩色塊垂直方向中
心距離AYa;已知的情況下，算法模塊調用公式(1)得到移動機器人到路標的
距離L:
移動機器人在開始運動時即以初始行進的方向為X軸建立默認的坐標系， 這個坐標系稱為機器人坐標系。設置在機器人中的里程計等慣性器件實時檢 測機器人的當前坐標(X， Y)和旋轉角度TH。根據距離L、當前坐標(X， Y) 和旋轉角度TH根據以下公式可以得到已知路標的坐標(PX， PY): U +丄cosm (2) Pl^F + ZsinTTf (3)
機器人行進過程中，通過上述計算分別得到每一時刻路標的坐標 (;^，;^)，由此確定機器人本身的坐標和旋轉角度(^A,A)，機器人與路標的
距離《。機器人從開始駛近路標至停止會得到路標的一系列坐標值，從這一系列 坐標中如何求出最恰當的是研究的關鍵。 一般的方法是求這一系列坐標值的
算術平均，作為路標的最優坐標值。本發明采用PS0 (Particle Swarm Optimizer,粒子群最優值)算法確定最恰當的坐標值。
PS0算法。PSO模擬鳥群的捕食行為，建立速度-位置(v-x)搜索模型，把 優化問題空間轉化為隨機粒子空間，通過反復迭代尋找最優解。每一個備選 解(機器人坐標和旋轉角度)作為一個粒子，粒子的優劣程度由適應度函數 F(x)決定。每一個粒子都由一個速度決定其更新的方向和大小，粒子們追隨 當前最優粒子通過迭代在解空間中搜索。每一次迭代，粒子通過跟蹤兩個極 值更新自己的速度和位置粒子本身所找到的最優解pBest和整個種群目前找 到的最優解gBest。定義種群中存在n個粒子，每個粒子m維，其速度和位置的 更新方法為<formula>formula see original document page 10</formula>其中， x,JA:)分別表示粒子/(/ = 1,2,...,")第^>/ = l,2,...,w)維分量在k時刻的速 度和位置；/^W表示粒子i第d維分量到k時刻為止搜索到的最優位置；&("表 示種群中所有粒子第d維分量至ljk時刻為止搜索到的最優位置；n,6為(0 1)之 間的隨機數；q,6為加速常數，表示每個粒子受pBest和gBest位置吸引的加速 項的權重， 一般取q-q-2; w為慣性權重，較大則算法具有較強的全局搜索 能力，較小則算法傾向于局部搜索， 一般是將隨迭代次數線性減小，即<formula>formula see original document page 10</formula>
利用PSO算法確定路標的最優坐標值。應用PSO算法解決優化問題的兩個 重要步驟是問題解的編碼和適應度函數的選擇。路標的最優坐標值(戸,^)應滿足與機器人每時刻的距離和或角度和最接近己測量值，即位置坐標的距離 適應度函數與旋轉角度適應度函數最小，
2fe^1)2 "K/2^)2 -《](7)
或 —tan%] (8)
念=1 /7X — &
最小，其中，n為總的粒子數，k表示不同的時刻。
以下以(7)式的距離適應度函數為例作為PSO算法的適應度函數確定路標 的最優坐標值。具體包括如下步驟
St印l:根據設定值，依次尋找路標，并對準。
St印2:駛近路標，記錄每時刻路標的坐標0^,i^)，機器人本身的坐標和
旋轉角度(&,A為)，機器人與路標的距離《。
St印3:將St印2中獲取的上述信息作為粒子群優化算法的粒子。粒子的位置 即為(M,肌)，速度是位置的10%。粒子是2維的。從速度分量中分別找到每維
的最大值vx^和w^，作為邊界約束值。
St印4:算法模塊調用公式(4)更新粒子的各維速度，并進行邊界約束(即 超過[-v,^J的值以邊界值代替)。算法模塊調用公式(5)更新粒子的各
維位置。
St印5:算法模塊調用公式(7)求每一個粒子的距離適應度函數，更新&(fc)
和g,(/t)。
St印6:返回St印4進行迭代，直到達到最大迭代次數/^_或滿足精度要
求(收斂)。
下面以一具體實例，說明隨著里程計誤差的增大，機器人根據已獲得的
路標坐標來實現自身的視覺重定位過程。如圖4所示，移動機器人從起始點A (xl，yl)開始尋找路標^(X1,Y1)，當它找到后，對準并駛向^，直到距離^到指定的長度。接著機器人尋找目標g( X2，Y2)，并重復以上過程。如果機 器人的行駛路徑是由A (Xl，yi)到B (x2，y2)再到C (x3，y3)。由里程計測量 得到線段BC的長度。由圖像處理可以得到機器人從B和C到相應坐標的距離。 這時機器人在C點的坐標可由公式(9)得到
X3 = X2 -丄2 cos(/ + or)
r3 =72—L2sin(/ + ") ②
<9C = " + or
其中"和A為中間變量，具體含義如圖4所示。算法模塊調用公式(10)
"=cos -^-^-^
K 一 T "=tan-1 J2 ^
計算得到 /——- (10)
z = V(《_z2)2+(《一r2)2 其中D為B點到C點的距離，L為路標S到路標A的距離。
權利要求
1、一種基于顏色路標的移動機器人視覺導航方法，其特征在于，設計帶有兩個不同顏色環的圓柱體作為機器人視覺導航的路標，對不同的路標加以編號并存儲；機器人根據不同路標的顏色環組合依次尋找并識別路標，實時計算移動機器人到路標的距離L；以機器人初始行進方向為X軸建立坐標系，里程計實時檢測機器人的當前坐標和旋轉角度(X，Y，TH)，并計算行進過程中每時刻路標坐標(pxk，pyk)，由此確定每時刻機器人坐標和旋轉角度(xk，yk，thk)；根據每時刻路標坐標、機器人坐標和旋轉角度(xk，yk，thk)，機器人與路標的距離dk；算法模塊采用PSO算法確定最優坐標值。
2、 根據權利要求l所述的移動機器人視覺導航方法，其特征在于，算 法模塊根據公式丄-I。/實時計算移動機器人到路標的距離L。
3、 根據權利要求l所述的移動機器人視覺導航方法，其特征在于，所 述PSO算法具體包括，把每時刻路標坐標、機器人坐標和旋轉角度作為一個粒 子，每一個粒子都由一個速度決定其更新的方向和大小，粒子們追隨當前最 優粒子通過反復迭代尋找最優解，最優坐標值滿足位置坐標的距離適應度函 數與旋轉角度適應度函數最小。
4、 根據權利要求l所述的移動機器人視覺導航方法，其特征在于，機 器人的當前坐標和旋轉角度(X， Y， TH)、路標坐標(PX， PY)滿足關系p;r = z+^cosr/f 禾口/T = ;r+^sin77/。
5、 根據權利要求i所述的移動機器人視覺導航方法，其特征在于，尋找路標及識別的步驟進一步包括，在HSI顏色空間下，采用以H為主閾值，I為輔助閾值的閾值分割法和基于s分量邊緣點數量統計的分割法相結合，分割路標上的兩色塊，并獲取它們的面積、圖像坐標。
6、 根據權利要求5所述的移動機器人視覺導航方法，其特征在于，分割路標上的兩色塊后，通過兩個顏色塊的面積大小相近小于某一閾值進行初 步的路標判斷，采用水平方向的兩色塊的中心坐標之差小于某一閾值進一步 判斷，根據兩色塊的顏色并與存儲空間中已經編號的路標比^f最終完成路標 的識別。
全文摘要
本發明請求保護一種基于顏色路標的移動機器人視覺導航方法，涉及智能控制領域。本發明設計了帶有兩個不同顏色環的圓柱體作為機器人視覺導航的路標，在HSI(HSIHue，Saturation，Intensity，色調，飽和度，亮度)顏色空間下，以H為主閾值，I為輔助閾值的閾值分割法和基于S分量邊緣點數量統計的分割法相結合，有效地實現了彩色路標的分割；通過色環的圖像信息，實現了機器人對路標的識別和對準技術。引入改進的灰度相關匹配法，優化了兩色環中心垂直距離值。通過建立機器人坐標系和路標的成像模型，得到了路標的全局坐標，并最終引入粒子群算法對其優化。建立數學模型，完成了基于路標的機器人視覺重定位。
文檔編號G01C21/26GK101619984SQ200910104460
公開日2010年1月6日 申請日期2009年7月28日 優先權日2009年7月28日
發明者毅 張, 張百勝, 玲 李, 巍 王, 元 羅, 胡章芳 申請人:重慶郵電大學