專利名稱：一種基于可行通道的機器人目標追蹤方法
技術領域：
本發明涉及移動機器人技術領域，特別是一種基于可行通道的機器人 目標追蹤方法。
背景技術：
從世界上第一臺機器人誕生以來，機器人技術得到了迅速的發展。機 器人的應用范圍也已經從工業制造領域擴展到軍事、核工業、航空航天、 服務業、醫療、人類日常生活等多個領域。其中具有廣闊應用前景和軍事 價值的移動機器人受到了普遍關注。
目標追蹤是移動機器人的一個典型研究內容，在軍事、反恐、安保等 任務中具有重要的應用前景。在獲取目標的位置后，需要解決的問題是如
何躲避周圍的障礙并保持對目標的追蹤。目前有一些避障、奔向目標的方 法，諸如人工勢場法以及改進、基于行為的方法、智能控制方法的應用等， 取得了一些研究進展，但是考慮到機器人的實際尺寸、實際環境的不確定 性、復雜性等因素，機器人避障追蹤目標仍需要深入的研究。
發明內容
本發明的目的是提供一種基于可行通道的機器人目標追蹤方法，使機器人通過自身的視覺傳感器、超聲傳感器、紅外傳感器和碼盤提供的信息 實現對目標的無碰追蹤，達到令人滿意的效果。
為實現上述目的，本發明的技術解決方案是
一種基于可行通道的機器人目標追蹤方法，其特征在于，包括步驟
A) 在目標上設置色標筒；
B) 機器人用攝像機搜尋目標上的色標筒以確定目標，綜合視覺測量與
碼盤信息，得到目標在機器人坐標系下的估計位置(p:，《)；
C) 將超聲和紅外傳感器信息統一到機器人坐標系下，結合估計的目標 位置，產生機器人的決策空間，同時獲取障礙物分布點集
D) 建立機器人通道集/^'(f = O,l,...,尺,-1);
E) 結合決策空間中的目標信息和障礙物分布信息，選取出合適的機器人 通道；
F) 根據決策空間中的紅外傳感器信息以及機器人通道選擇情況，確定機 器人的轉角^與速度大小v，實現對機器人的控制。
所述的一種基于可行通道的機器人目標追蹤方法，其所述目標上的色 標筒為中空的圓柱形筒，由至少兩種顏色上下組合而成，色標筒的中心與 目標的中心保持一致。
所述的一種基于可行通道的機器人目標追蹤方法，其所述機器人坐標 系為以機器人中心為極點，當前運動方向為極軸的極坐標系。
所述的一種基于可行通道的機器人目標追蹤方法，其所述障礙物分布 點集的數目^為4的倍數。所述的一種基于可行通道的機器人目標追蹤方法，其所述機器人通道
為一有向矩形，且機器人中心坐落在該矩形上。
所述的一種基于可行通道的機器人目標追蹤方法，其所述機器人通道 集的數目/^為4的倍數，且7^為尺j的倍數。
本發明只根據自身感知信息做出決策，可以實現實時的控制，為移動 機器人在安全防御、安保等方面的應用提供技術支持。
圖1是本發明的一種基于可行通道的機器人目標追蹤方法的控制框圖； 圖2是障礙物分布(Xrf=16)示意圖3是機器人對靜態目標的追蹤軌跡；
圖4是機器人追蹤動態目標的軌跡。
具體實施例方式
本發明提供了一種基于可行通道的機器人目標追蹤方法，在機器人通 過視覺和碼盤信息估算出目標在自身坐標系中的位置后，將超聲傳感器和 紅外傳感器信息統一到機器人坐標系下，結合估計的目標位置，產生機器
人的決策空間，同時獲取障礙物分布點集；建立機器人通道集，結合決策 空間中的目標信息和障礙物分布信息，選取出合適的機器人通道，進而綜 合決策空間中的紅外傳感器信息，確定出機器人的轉角與速度大小，實現 對機器人的控制，使得機器人在避開障礙物的同時追蹤目標。控制框圖如 圖1所示，其中，(",《)為目標在機器人坐標系下的估計位置，《(^: = 0,1，...,7^-l)為障礙物分布點集，F尸》-0,1,...,/^-l)為機器人通 道集合，0為機器人轉角，v為機器人速度大小。
1.目標位置估計
建立以機器人中心為極點，當前方向為極軸的機器人坐標系S。跟隨 機器人有四個攝像機&(/)(/ = 1,2,3,4)，從機器人中心到這四個攝像機光心
的連線方向分別與各自的光軸方向相一致，且分別與機器人當前運動方向
右轉工方向、當前運動方向、當前運動方向左轉工方向、當前運動方向的 2 2
反方向相一致。
目標上配備一個色標筒，色標筒為中空的圓柱形筒，由至少兩種顏色 上下纟早合而成，色標筒的中心與目標的中心保持一致。機器人通過對色標 筒的視覺識別完成對目標的識別，結合視覺標定，進而獲取估計的相對信 息。令(p:,《)為目標在機器人坐標系Z中的估計位置。
定義(",力是圖像坐標系上一點，(x，》z)是其在世界坐標系上的坐標，
有
<formula>formula see original document page 7</formula>
其中#是攝像機的內參數矩陣，由攝像機標定得到。
記("pVl)和(m2,v2)分別為色標筒頂端和底端的點，(A，乂，A)和 (A,h，z》是它們所對應的世界坐標系的坐標，",="2， z,=z2， 乂是 色標筒的實際高度，則"估算如下<formula>formula see original document page 8</formula>
式中"，由#得到，v2-v,是色標筒在圖像中的像素高度差，《是攝像機與
機器人中心之間的距離。
記( ，v》是色標筒的中心，~，《分別為圖像的寬度和視野的寬度， 則《估算如下
《=
，
arc tan( A tan(《/ 2)) + (/ — 2). - (/ = 1， 2， 3)2w.
2
置tan(~^. tan(《/ 2)) + ;r (/ = 4 a wr < 0)
arc tan(j tan(《/ 2》—丌(>'=4 a wr》0)
如果機器人無法發現目標，此時，視覺測量失效，可以采用碼盤信息
估計目標位置。如果機器人在7;時間內仍無法發現，它不得不搜索。記
(p二,《一)為前一采樣時刻的目標坐標，A,A分別為上一決策周期左輪和 右輪行使的距離。從而，機器人轉角 和機器人中心行使的距離《估算
<formula>formula see original document page 8</formula>
其中，《為左輪和右輪之間的距離。
目標在機器人坐標系S中的坐標可如下估計:
P"7 = J +《2 - 2C0S(d - A)
2.決策空間在實際的機器人系統中，不同傳感器分布于機器人的不同位置。為此， 有必要將傳感信息統一到機器人坐標系S下，得到機器人的決策空間，便 于機器人進行基于傳感的決策。基于超聲傳感器信息，將機器人周圍的環 境等分成K,等份，^/為4的倍數。機器人周圍障礙物分布可描述為點集 《^ = 0,1,.，"^^-1)， 《用描述，定義集合
;r，"為決
;={《("，《)(A = o，i,...，A:rf — i)}，見圖2，其中，《=
策空間中與《相對應的超聲傳感器信息。
3.機器人通道
考慮到機器人具有一定的物理尺寸，機器人通道定義為一有向矩形， 寬度為2>;,，長度為/>,，且機器人中心坐落在該矩形上。
設定機器人共有^,=^個通道可以選擇，i^為4的倍數，《d為尺
/
的C^倍，C^〉0為一自然數。這些通道組成通道集/^"^ = 0,1,...,&-1)，
2;r
每個通道對應一個方向;r + :/ ，可用集合《表示為
《=
2;r
F《& =-兀+ 二W = 0,1,...,《y ， 1) }> ，與機器人當前運動方向相對應
尺/
的通道為前向通道F/f"2 。定義障礙物分布點集f的子集
4 5 4
定義《描述點《與通道/^'的關系，當該點在通道內時，《=1。于
是有<formula>formula see original document page 10</formula>
當Q,中所有的點都不在F《中，意味著通道F《是安全的，機器人可
以沿著通道/^'的方向運動，記^=0。
<formula>formula see original document page 10</formula>
4.選取合適的機器人通道
<formula>formula see original document page 10</formula>
為了實現機器人對目標的有效追蹤，需要從所有安全的通道中選取出 合適的一個。選擇標準是機器人盡量朝著目標方向運動，定義合適的機器 人通道為Fif 。
定義機器人運動方向到目標方向的角度為《，《=-《+2&;r ， 、=0(" = 0),6>0g=—《。其中，《e[-7T,;r)， 6^e(-oo,Qo)。當《在-;r或者 ;r周圍跳變時，、的值發生改變。《從負到正，、加1;《從正到負，&減1。
首先計算出目標所在的通道wf ， r =
2;r
.3
若(一7T <《:< 一;r / 2) a O / 2 < 6>j < 7T)， 貝lj有、=、+1 ; 若 (;r/2〈(9么<;r)a(-;r<《<-;r/2)，貝'J ^=、-1。
(1)如果《S0，判斷當前方向的相應通道是否可行，即F/一是否為 0 。若F，/2=0 ，搜尋/ = 0,1,...直至U (尺,/2 —Omodi^^r ，使得 F，/2_')m°", =1，于是，"-(i^/^-Z + OmodX^如果Ff盧—')m。^始終
為o， p二r。若/^//2 = 1，搜尋/ = 0，1,...，直至^(/://2 + 0010(1^:/ = 7，使得
'=0，于是p二(尺,/2 + 0mod^;如果/^//2+''— 始終為1，表明沒有發現合適的通道。
(2)如果《<0，判斷當前方向的相應通道是否可行，即F/一是否為
0 。若/^,/2 = 0 ，搜尋/ = 0，1,...直至lj (^y/^ + ^modi^^r ，使得 《V2+,)m。"， = i ，于是p = / 2 +,—丄)mod《,，如果《V2+')加^始終為o，
p = r。若尸，/2 = 1，搜尋z'=o,u.直到(尺"2 —/)mod^:r ，使得
/^〃2—')m。W =0'于是/7:(i^/2 —/)m0dX/;如果7^盧—')m。",始終為1，
表明沒有合適的通道。
5.機器人決策
一旦機器人獲取尸/T，機器人計算出轉角^二+ 如果機器人
《/
當前運動方向所對應的紅外傳感器探測到障礙，機器人速度大小v設為0， 否則，v = V(/。當機器人無法發現合適的通道時，v取0。
本發明所提供的一種基于可行通道的機器人目標追蹤方法應用于中科院 自動化研究所研制的智能機器人A頂上，該直徑48cm，由雙輪差動驅動， 安裝有4個攝像機，16路超聲傳感器，16路紅外傳感器。每個攝像機視 野約60° 。當機器人和目標之間的距離小于60c奶時，認為追蹤成功。 《rf =160 ， C# = 4 ， = 40 , D。 = 100cw ， Z>w = 60cm ， = 5 ，
《=0.27m，《=5cm， ^=20cm"。采用本發明所提供的目標追蹤方法， 效果令人滿意，圖3和圖4分別給出了目標靜止和目標運動時，機器人的
無碰追蹤軌跡。
權利要求
1.一種基于可行通道的機器人目標追蹤方法，其特征在于，包括步驟A)在目標上設置色標筒；B)機器人用攝像機搜尋目標上的色標筒以確定目標，綜合視覺測量與碼盤信息，得到目標在機器人坐標系下的估計位置(ρnT，θnT)；C)將超聲和紅外傳感器信息統一到機器人坐標系下，結合估計的目標位置，產生機器人的決策空間，同時獲取障礙物分布點集Pnk(k＝0，1，...，Kd-1)；D)建立機器人通道集FPvt(t＝0，1，...，Kf-1)；E)結合決策空間中的目標信息和障礙物分布信息，選取出合適的機器人通道；F)根據決策空間中的紅外傳感器信息以及機器人通道選擇情況，確定機器人的轉角θ與速度大小v，實現對機器人的控制。
2. 如權利要求1所述的一種基于可行通道的機器人目標追蹤方法，其 特征在于，所述目標上的色標筒為中空的圓柱形筒，由至少兩種顏色上下 組合而成，色標筒的中心與目標的中心保持一致。
3. 如權利要求1所述的一種基于可行通道的機器人目標追蹤方法，其 特征在于，所述機器人坐標系為以機器人中心為極點，當前運動方向為極 軸的極坐標系。
4. 如權利要求1所述的一種基于可行通道的機器人目標追蹤方法，其 特征在于，所述障礙物分布點集的數目尺,為4的倍數。
5. 如權利要求1所述的一種基于可行通道的機器人目標追蹤方法，其特征在于，所述機器人通道為一有向矩形，且機器人中心坐落在該矩形上。
6.如權利要求1所述的一種基于可行通道的機器人目標追蹤方法，其 特征在于，所述機器人通道集的數目尺，為4的倍數，且^為^的倍數。
全文摘要
本發明一種基于可行通道的機器人目標追蹤方法，涉及機器人技術，機器人用視覺與碼盤信息得到目標在機器人坐標系下的位置估計(ρ<sub>n</sub><sup>T</sup>，θ<sub>n</sub><sup>T</sup>)，并將超聲和紅外傳感器信息統一到該坐標系下，產生決策空間，同時獲取障礙物分布點集P<sub>n</sub><sup>k</sup>(k＝0，1，…，K<sub>d</sub>-1)；建立機器人通道集FP<sub>v</sub><sup>t</sup>(t＝0，1，…，K<sub>f</sub>-1)，結合決策空間中的目標信息和障礙物分布信息，選取合適的機器人通道，進而綜合決策空間中的紅外傳感器信息，確定機器人的轉角θ與速度大小v，實現對機器人的控制，使機器人在避開障礙物的同時追蹤目標。本發明方法使機器人通過自身視覺、超聲、紅外傳感器和碼盤提供的信息實現對目標的無碰追蹤。該方法只根據自身感知信息做出決策，可以實現實時的控制，為移動機器人在安全防御、安保等方面的應用提供技術支持。
文檔編號G05D1/12GK101667037SQ20081011958
公開日2010年3月10日 申請日期2008年9月3日 優先權日2008年9月3日
發明者侯增廣, 超 周, 張文文, 曹志強, 旭 王, 瑗 袁, 民 譚 申請人:中國科學院自動化研究所