專利名稱：基于單個(gè)路標(biāo)的視覺(jué)定位與導(dǎo)航方法
基于單個(gè)路標(biāo)的視覺(jué)定位與導(dǎo)航方法技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人視覺(jué)定位與導(dǎo)航技術(shù)領(lǐng)域。背景技術(shù)：
機(jī)器人工作過(guò)程中經(jīng)常需要確定自身位置(定位)并進(jìn)行導(dǎo)航，因此 定位和導(dǎo)航一直是機(jī)器人領(lǐng)域中的一個(gè)重要研究方向，具有很好的應(yīng)用前景。常用的定位方法有利用超聲或紅外傳感器等感知機(jī)器人與周圍物體的相對(duì)位置，從而 對(duì)機(jī)器人進(jìn)行定位，這種方法可以較準(zhǔn)確的判斷出機(jī)器人與周圍物體的相對(duì)位置，但是要 想對(duì)機(jī)器人進(jìn)行絕對(duì)定位卻很困難，因此主要用于導(dǎo)航。另一種方法是利用視覺(jué)獲取環(huán)境 信息與數(shù)據(jù)庫(kù)中的數(shù)據(jù)進(jìn)行匹配從而對(duì)機(jī)器人進(jìn)行定位，該方法可以較準(zhǔn)確的獲取機(jī)器人 的絕對(duì)位置，但是定位算法比較復(fù)雜。常用的導(dǎo)航方法有利用導(dǎo)軌進(jìn)行導(dǎo)航，利用超聲或紅外傳感器等進(jìn)行導(dǎo)航和利用視覺(jué) 進(jìn)行導(dǎo)航。其中利用導(dǎo)軌進(jìn)行導(dǎo)航精度很高，實(shí)時(shí)性好，但是導(dǎo)軌鋪設(shè)困難，導(dǎo)航路徑固 定；利用超聲或紅外傳感器等進(jìn)行導(dǎo)航精度較高，但信息量小，不利于機(jī)器人的綜合運(yùn)用； 利用視覺(jué)進(jìn)行導(dǎo)航由于信息量大，設(shè)備簡(jiǎn)單，使用方便，受到了廣大研究者的青睞。目前，視覺(jué)定位與導(dǎo)航中使用最多的是利用路標(biāo)進(jìn)行定位與導(dǎo)航。路標(biāo)選取可分為兩 大類 一是利用自然信息作為路標(biāo)，二是利用人造路標(biāo)。利用自然信息作為路標(biāo)可以提取 同一場(chǎng)景中多種自然信息，定位與導(dǎo)航精度高，但該方法計(jì)算復(fù)雜，并且需要事先建模， 系統(tǒng)實(shí)時(shí)性不高；利用人造路標(biāo)進(jìn)行定位與導(dǎo)航算法相對(duì)簡(jiǎn)單，系統(tǒng)實(shí)時(shí)性較高，但是抗 干擾能力差，可靠性不高。
發(fā)明內(nèi)容本發(fā)明目的是解決現(xiàn)有機(jī)器人定位與導(dǎo)航方法實(shí)時(shí)性不夠高、抗干擾 能力差、路標(biāo)信息量小、路徑不易改變等問(wèn)題，提供一種簡(jiǎn)單、經(jīng)濟(jì)，路徑可隨意改變且 實(shí)時(shí)性更高的機(jī)器人視覺(jué)定位與導(dǎo)航方法，以及適合該方法的路標(biāo)。該方法根據(jù)路標(biāo)和機(jī)器人在圖像坐標(biāo)系的位姿關(guān)系計(jì)算機(jī)器人相對(duì)于路標(biāo)的相對(duì)位 姿，依據(jù)路標(biāo)坐標(biāo)系和世界坐標(biāo)系的關(guān)系計(jì)算機(jī)器人在世界坐標(biāo)系的絕對(duì)位姿，即對(duì)機(jī)器 人進(jìn)行定位。根據(jù)路標(biāo)、目標(biāo)點(diǎn)和機(jī)器人在圖像中的相互關(guān)系對(duì)機(jī)器人進(jìn)行視覺(jué)導(dǎo)航。 1、本發(fā)明提供的基于單個(gè)路標(biāo)的機(jī)器人視覺(jué)定位方法，包括如下步驟-第一、路標(biāo)的設(shè)計(jì)該路標(biāo)為表盤式組合結(jié)構(gòu)，路標(biāo)主體是半徑為R的圓盤，圓盤圓心為路標(biāo)中心，圓 盤上設(shè)有用于機(jī)器人定位的第一輔助部分和指示下一路標(biāo)所在方向的第二輔助部分；路標(biāo) 的主體采用黃色，第一輔助部分采用藍(lán)色，第二輔助部分采用紫色；第二、路標(biāo)鋪設(shè)5首先確定機(jī)器人的移動(dòng)路徑，然后將路標(biāo)間隔鋪設(shè)在移動(dòng)路徑上方，鋪設(shè)時(shí)，使當(dāng) 前路標(biāo)的中心點(diǎn)及其第二輔助部分的中心點(diǎn)和下一路標(biāo)的中心點(diǎn)及其第一輔助部分的中 心點(diǎn)位于同一條直線上；第三、路標(biāo)的識(shí)別與定位令機(jī)器人起始時(shí)位于第一個(gè)路標(biāo)下方，由機(jī)器人對(duì)路標(biāo)進(jìn)行圖像采集，并對(duì)圖像進(jìn) 行顏色識(shí)別，利用模板匹配確定在圖像坐標(biāo)系中路標(biāo)的中心坐標(biāo)(x，力，利用統(tǒng)計(jì)平均坐 標(biāo)確定在圖像坐標(biāo)系中路標(biāo)第一輔助部分的中心坐標(biāo)Oq，^)和第二輔助部分的中心坐標(biāo)(x2，:v2);第四、機(jī)器人的定位1) 、機(jī)器人的相對(duì)定位令MOiN為路標(biāo)坐標(biāo)系，M'CVN'為路標(biāo)坐標(biāo)系在圖像中的投影，則機(jī)器人在圖像中 相對(duì)于路標(biāo)坐標(biāo)系的坐標(biāo)為Aw = —/ x sin(0 — )8) △w = —/ x cos(0 — J0))3 = arctan"1-0 = arctan^5——^其中，/為圖像中路標(biāo)距圖像中心的距離；)8為圖像中路標(biāo)坐標(biāo)系N'軸與機(jī)器人前進(jìn)方向的夾角，6>為圖像中心和路標(biāo)中心連線與機(jī)器人前進(jìn)方向的夾角，(;c。,j^)代表機(jī)器人在圖像中的坐標(biāo)；2) 、機(jī)器人的絕對(duì)定位假設(shè)路標(biāo)在世界坐標(biāo)系XwOwYw中的坐標(biāo)為(X, 7)，則機(jī)器人在世界坐標(biāo)系中的絕對(duì)坐標(biāo)(;r，r)為X' = JT +丄sin(a -義) F= 7 —丄cos(a -義)其中^=//^; A=0-j8;a為世界坐標(biāo)系與路標(biāo)坐標(biāo)系的夾角，若路標(biāo)坐標(biāo)系相 對(duì)于世界坐標(biāo)系發(fā)生逆時(shí)針旋轉(zhuǎn)則a為正，反之為負(fù)，A:為圖像坐標(biāo)系與世界坐標(biāo)系的比 例系數(shù)。其中，圖像坐標(biāo)系與世界坐標(biāo)系的比例系數(shù)it的計(jì)算方法如下 統(tǒng)計(jì)與路標(biāo)同高度的長(zhǎng)度為L(zhǎng)cm的線段在圖像中的像素個(gè)數(shù)"，則比例系數(shù)為上述第三步路標(biāo)的識(shí)別與定位中對(duì)圖像進(jìn)行顏色識(shí)別的處理區(qū)域?yàn)檫呴L(zhǎng)4W ~ 6;U 的方形區(qū)域，路標(biāo)主體半徑R的取值范圍一般為10~30cm 。路標(biāo)的識(shí)別與定位具體方法是1) 、設(shè)定黃色的顏色閾值為123<^/<143， 32<H<52，紫色的閾值范圍為 174<[/<194， 322 <//< 342 ，藍(lán)色的閾值范圍為176 < C7 < 196 ， 188 <//< 212 ; 其中C7為YUV顏色空間的色調(diào)分量U,i/為HSI顏色空間的色調(diào)分量H;2) 、計(jì)算處理區(qū)域的起止坐標(biāo)(;c-;7iU ,:v-/^及)，(;c + ;^及，少+ ;^i )， 2^/7^3;3) 、利用設(shè)置的顏色閾值對(duì)處理區(qū)域內(nèi)的每一像素點(diǎn)進(jìn)行顏色識(shí)別，并分別將黃色、 紫色、藍(lán)色和其他顏色的像素點(diǎn)灰度值設(shè)置為255、 150、 100和0，然后進(jìn)行腐蝕濾波和 膨脹濾波去除干擾點(diǎn)；4) 、統(tǒng)計(jì)灰度值非0的像素點(diǎn)的平均坐標(biāo)(;r,，，x，)，采用邊長(zhǎng)為1.5W 的方形為模 板，在(;c,，，:^^)附近進(jìn)行模板匹配精確定位當(dāng)前路標(biāo)中心在圖像坐標(biāo)系的坐標(biāo)(;c,力；5) 、分別統(tǒng)計(jì)以(:c,力為中心，2M為邊長(zhǎng)的方形區(qū)域內(nèi)灰度值為150和100的像素 點(diǎn)的平均坐標(biāo)，即為當(dāng)前路標(biāo)第一輔助部分的中心在圖像坐標(biāo)系的坐標(biāo)(x,，乂)和第二輔助 部分的中心在圖像坐標(biāo)系的坐標(biāo)(^,力)。本發(fā)明同時(shí)提供了一種基于以上所述機(jī)器人視覺(jué)定位方法的機(jī)器人視覺(jué)導(dǎo)航方法，該 方法在以上定位的基礎(chǔ)上還包括如下步驟 第五、導(dǎo)航方法1) 、固定路徑時(shí)的導(dǎo)航方法令機(jī)器人按權(quán)利要求1中第二步鋪設(shè)的路標(biāo)沿固定路徑移動(dòng)導(dǎo)航，則首先依第四步中的方法計(jì)算出6(，機(jī)器人車體偏離角 / = 6>，如果—r<y<r，機(jī)器人前行，如果7>『， 機(jī)器人需順時(shí)針旋轉(zhuǎn)，如果y〈-r，機(jī)器人需逆時(shí)針旋轉(zhuǎn)，其中r為預(yù)先設(shè)定的是否對(duì)機(jī)器人進(jìn)行糾偏的閾值；2) 、按編程設(shè)定路徑時(shí)的導(dǎo)航方法如果路標(biāo)不變，導(dǎo)航路徑依導(dǎo)航目標(biāo)點(diǎn)編程設(shè)定，其中的導(dǎo)航目標(biāo)點(diǎn)可以是路徑中的某個(gè)路標(biāo)，也可以是路標(biāo)以外的某個(gè)點(diǎn)，則機(jī)器人車體偏離角y的計(jì)算公式為y。 一y其中、'=jc。 +^Ad， AJ為當(dāng)前目標(biāo)點(diǎn)到前一目標(biāo)點(diǎn)和當(dāng)前路標(biāo)連線的垂直距離并規(guī)定目標(biāo)點(diǎn)在連線右側(cè)時(shí)zw為正，目標(biāo)點(diǎn)在連線左側(cè)時(shí)zw為負(fù)，如果-r<y<r，機(jī)器 人前行，如果y〉r，機(jī)器人需順時(shí)針旋轉(zhuǎn)，如果7<-r，機(jī)器人需逆時(shí)針旋轉(zhuǎn)，r為預(yù)先設(shè)定的是否對(duì)機(jī)器人進(jìn)行糾偏的閎值； 第六、預(yù)測(cè)路標(biāo)位置如果^〈r/，則認(rèn)為當(dāng)前路標(biāo)有效，其在下一幀圖像中的坐標(biāo)仍預(yù)測(cè)為(x，力； 如果^>7/，則應(yīng)尋找新的路標(biāo)，預(yù)測(cè)新路標(biāo)在下一幀圖像中的坐標(biāo)(x'，乂)為7其中r/為是否尋找下一路標(biāo)的閾值，D為兩路標(biāo)在世界坐標(biāo)系的距離。r/取值范圍 一般為(Cv。-20"~>;。)，機(jī)器人延遲越大，r/越小，機(jī)器人延遲越小，r/應(yīng)越接近圖 像中心縱坐標(biāo)^ 。本發(fā)明的優(yōu)點(diǎn)和積極效果1、本發(fā)明方法利用單個(gè)路標(biāo)即可實(shí)現(xiàn)機(jī)器人的定位和導(dǎo)航，算法簡(jiǎn)單，并且避免了 多個(gè)路標(biāo)進(jìn)行定位時(shí)的匹配失誤，提高了實(shí)時(shí)性和可靠性。2、本發(fā)明所設(shè)計(jì)的路標(biāo)含有 力h信息，即使機(jī)器人發(fā)生很大偏轉(zhuǎn)仍能找到正確的方向，有很強(qiáng)的抗干擾能力。3、本 發(fā)明方法涉及的導(dǎo)航路徑即可以為固定路徑，也可以編程設(shè)定，不僅提高了導(dǎo)航路徑的靈 活性，也提高了路標(biāo)鋪設(shè)的靈活性。4、本發(fā)明所設(shè)計(jì)的路標(biāo)可以采用紙張、塑料等材質(zhì) 制成，非常經(jīng)濟(jì)，并且該路標(biāo)結(jié)構(gòu)簡(jiǎn)單，鋪設(shè)方便。
圖l(a)為路標(biāo)外形示意圖、圖l(b)為路標(biāo)鋪設(shè)方式示意圖；其中，1為路標(biāo)第一輔助 部分，2為路標(biāo)第二輔助部分，3為路標(biāo)主體。圖2(a)為機(jī)器人相對(duì)定位示意圖，圖2(b)為機(jī)器人絕對(duì)定位示意圖；圖中，XOY， MOiN， XwOwYw分別為圖像坐標(biāo)系、路標(biāo)坐標(biāo)系和世界坐標(biāo)系；M'O!'N'為路標(biāo)坐標(biāo)系 在圖像中的投影；Oo為圖像中心點(diǎn)(即機(jī)器人在圖像中的投影)，OoQ為機(jī)器人當(dāng)前的前 進(jìn)方向。圖3 (a)為世界坐標(biāo)系及其中的路標(biāo)和目標(biāo)點(diǎn)示意圖；3 (b)為某一時(shí)刻的一幀圖像示意 圖，其中a為前一目標(biāo)A在圖像中的投影，b為當(dāng)前路標(biāo)B在圖像中的投影，e為當(dāng)前目 標(biāo)點(diǎn)E在圖像中的投影。圖4為預(yù)測(cè)下一路標(biāo)位置示意圖。圖5為路標(biāo)鋪設(shè)實(shí)例圖。圖6(a)為機(jī)器人定位實(shí)例圖，圖6(b)為圖6(a)的局部放大圖，其中的十字交點(diǎn)為找 到的路標(biāo)及其兩個(gè)輔助部分的中心。圖7為導(dǎo)航實(shí)例圖，圖中，4為當(dāng)前路標(biāo)，5為當(dāng)前目標(biāo)點(diǎn)所在位置。圖8 (a)為預(yù)測(cè)下一路標(biāo)位置實(shí)例圖，圖8(b)為圖8(a)的局部放大圖，其中的白色十字交點(diǎn)為預(yù)測(cè)的新路標(biāo)的中心。 圖9為算法流程圖。
具體實(shí)施方式
步驟l:路標(biāo)設(shè)計(jì)
如圖l(a)所示，本發(fā)明所設(shè)計(jì)的路標(biāo)可選用紙張、塑料等材質(zhì)制成，結(jié)構(gòu)為表盤式組 合結(jié)構(gòu)。路標(biāo)主體3是半徑為R的圓盤，圓盤圓心為路標(biāo)中心，R的取值范圍一般為 10 30cm，相機(jī)分辨率越高，R的取值可以越?。宦窐?biāo)輔助部分形狀設(shè)計(jì)為梯型結(jié)構(gòu)，其 寬度為R，輔助部分內(nèi)側(cè)距離圓心為R/2，其中路標(biāo)的第二輔助部分能夠以圓盤的圓心為 中心正負(fù)旋轉(zhuǎn)卯度。
路標(biāo)的主體采用黃色，第一輔助部分采用藍(lán)色，第二輔助部分采用紫色。
步驟2:路標(biāo)鋪設(shè)
首先確定機(jī)器人的移動(dòng)路徑，然后將路標(biāo)間隔鋪設(shè)在移動(dòng)路徑上方(盡量讓所有路禾示
在一個(gè)水平面內(nèi)，以提高定位精度)，如圖l(b)所示；鋪設(shè)時(shí)，使當(dāng)前路標(biāo)的中心(;c,力及 其第二輔助部分的中心(^，^)和下一路標(biāo)的中心(Jc',3/)及其第一輔助部分的中心 (x、,A')在一條直線上，如圖4所示。
步驟3:初始化
1、 (jc，力代表當(dāng)前路標(biāo)中心在圖像中的坐標(biāo)，(;c。，y。)代表機(jī)器人在圖像中的坐標(biāo)， 本實(shí)施例中機(jī)器人在圖像中的坐標(biāo)為圖像中心坐標(biāo)(568,426)，將機(jī)器人放到第一個(gè)路標(biāo)下 方，并設(shè)定起始時(shí)當(dāng)前路標(biāo)中心在圖像中的坐標(biāo)為圖像中心坐標(biāo)(x。,y。)；
2、 設(shè)定黃色的顏色閾值為123<￡/<143， 32</f<52,紫色的閾值范圍為 174<t/<194， 322 <//< 342 ，藍(lán)色的閾值范圍為176 < " < 196 ， 188 </f < 212 ;
其中[/為YUV顏色空間的色調(diào)分量U，H為HSI顏色空間的色調(diào)分量H。
3、 統(tǒng)計(jì)與路標(biāo)同高度的100cm長(zhǎng)度的線段在圖像中的像素個(gè)數(shù)n，比例系數(shù)
)t = ~^~，本實(shí)施例中A:-5.1; 100c附
4、 根據(jù)攝像機(jī)的分辨率設(shè)定顏色識(shí)別的處理區(qū)域?yàn)檫呴L(zhǎng)4W 6W 的方形區(qū)域，本 實(shí)施例中選用的邊長(zhǎng)為4M;
5、 依據(jù)機(jī)器人的性能設(shè)定尋找下一路標(biāo)的閾值^，其取值范圍一般為
(Cy。 - 20"~ j。)，機(jī)器人延遲越大，r/越小，機(jī)器人延遲越小，r/應(yīng)越接近圖像中心
縱坐標(biāo)y。；本實(shí)施例中？7" = _y。 - = 426 - 10 x 5.1 = 375 ;
6、 設(shè)定機(jī)器人的糾偏閾值r為l(T，即當(dāng)機(jī)器人的偏離角度在(-l(T 1(T)之間時(shí)，
9機(jī)器人仍然正常前行，當(dāng)超過(guò)這個(gè)范圍之后，對(duì)機(jī)器人進(jìn)行糾偏。
步驟4:定位方法
1、 路標(biāo)在圖像中的識(shí)別定位
1) 計(jì)算處理區(qū)域的起止坐標(biāo)(;c - 2W ， y - 2M) ， (x + 2欣，y + 2M)
2) 利用設(shè)置的顏色閾值對(duì)處理區(qū)域內(nèi)的每一像素點(diǎn)進(jìn)行顏色識(shí)別，并分別將黃色、 紫色、藍(lán)色和其他顏色的像素點(diǎn)灰度值設(shè)置為255、 150、 100和0，然后進(jìn)行腐 蝕濾波和膨脹濾波去除干擾點(diǎn)；
3) 統(tǒng)計(jì)灰度值非0的像素點(diǎn)的平均坐標(biāo)(、，j，)，采用邊長(zhǎng)為1.5M的方形為模
板，在(JC,，，^，)附近進(jìn)行模板匹配精確定位當(dāng)前路標(biāo)中心在圖像坐標(biāo)系的坐
標(biāo)"，力；
4) 分別統(tǒng)計(jì)以(jc,力為中心，2M為邊長(zhǎng)的方形區(qū)域內(nèi)灰度值為150和100的像素 點(diǎn)的平均坐標(biāo)，即為當(dāng)前路標(biāo)第一輔助部分的中心在圖像坐標(biāo)系的坐標(biāo)(^,A) 和第二輔助部分的中心在圖像坐標(biāo)系的坐標(biāo)(^,h)，如圖6所示。
2、 機(jī)器人的相對(duì)定位
如圖2(a)所示，/為圖像中路標(biāo)距圖像中心的距離；j8為圖像中路標(biāo)坐標(biāo)系N'軸與機(jī) 器人前進(jìn)方向的夾角，0為圖像中心和路標(biāo)中心連線與機(jī)器人前進(jìn)方向的夾角，則圖像中 機(jī)器人在路標(biāo)坐標(biāo)系的相對(duì)位置為
△m = —/ x sin(0 — )8) Aw = —/ x cos(0 — j3)
其中
)3 = arctan^——^
-少
0 = arctan Xo _ x y0 — _y
3、 機(jī)器人的絕對(duì)定位
如圖2(b)所示，假設(shè)路標(biāo)在世界坐標(biāo)系的坐標(biāo)為(z,;r)，則機(jī)器人在世界坐標(biāo)系的絕 對(duì)坐標(biāo)(JT，:r)為
X +丄sin(a —義)
r= :r - icos(a —義)
其中丄-〃A:;義=6>-)3;a為世界坐標(biāo)系與路標(biāo)坐標(biāo)系的夾角，若路標(biāo)坐標(biāo)系相 對(duì)于世界坐標(biāo)系發(fā)生逆時(shí)針旋轉(zhuǎn)則a為正，反之為負(fù)；
步驟5:導(dǎo)航方法
1、固定路徑時(shí)的導(dǎo)航方法
10如圖3所示，如果按路標(biāo)所示的固定路徑八一>8^>(:^>0進(jìn)行導(dǎo)航，則按步驟4中
2的方法計(jì)算出0，車體偏離角y-0，如果-r〈y〈r(r為i(r)，機(jī)器人前行；如果y >r，
機(jī)器人需順時(shí)針旋轉(zhuǎn)。如果y〈-r，機(jī)器人需逆時(shí)針旋轉(zhuǎn)。其中r為是否對(duì)機(jī)器人進(jìn)行糾
偏的閾值。
2、編程設(shè)定路徑時(shí)的導(dǎo)航方法
如圖3所示，如果路標(biāo)不變，導(dǎo)航路徑設(shè)定為A—〉E—〉F—〉D，(其中A、 E、 F、 D 為路徑中的不同目標(biāo)點(diǎn))則y的計(jì)算公式為
<formula>formula see original document page 11</formula>
其中jc。^;c。 +AAJ， AJ為當(dāng)前目標(biāo)點(diǎn)到前一目標(biāo)點(diǎn)與當(dāng)前路標(biāo)連線的垂直距離(規(guī) 定目標(biāo)點(diǎn)在連線右側(cè)時(shí)AJ為正，目標(biāo)點(diǎn)在連線左側(cè)時(shí)Ac/為負(fù)，如圖3所示A《為正， 為負(fù))。圖3(a)為世界坐標(biāo)系及其中的路標(biāo)和目標(biāo)點(diǎn)；3(b)為某一時(shí)刻的一幀圖像，其中a 為前一目標(biāo)A在圖像中的投影，b為當(dāng)前路標(biāo)B在圖像中的投影，e為當(dāng)前目標(biāo)點(diǎn)E在圖 像中的投影。
如果—r<y<r (r為i(T)，機(jī)器人前行；如果y〉r，機(jī)器人需順時(shí)針旋轉(zhuǎn)。如果 y<-r，機(jī)器人需逆時(shí)針旋轉(zhuǎn)。
步驟6:預(yù)測(cè)路標(biāo)位置
如果:v〈r/，則認(rèn)為當(dāng)前路標(biāo)有效，其在下一幀圖像中的坐標(biāo)仍預(yù)測(cè)為(x,力。 如果7>7/，則應(yīng)尋找新的路標(biāo)，如圖4所示，預(yù)測(cè)新路標(biāo)在下一幀圖像中的坐標(biāo) (x'，力為
<formula>formula see original document page 11</formula>
其中7X"為是否尋找下一路標(biāo)的閾值，"為兩路標(biāo)在世界坐標(biāo)系的距離。
應(yīng)用實(shí)例1:
路標(biāo)鋪設(shè)實(shí)例如圖5所示，根據(jù)機(jī)器人的行走路徑將路標(biāo)水平間隔鋪設(shè)在路徑上方 的房頂上，其中各相鄰路標(biāo)之間的鋪設(shè)方法參見(jiàn)步驟2。.
應(yīng)用例2:
機(jī)器人定位實(shí)例如圖6所示，通過(guò)模板匹配和統(tǒng)計(jì)平均坐標(biāo)可分別得到在圖像坐標(biāo) 系中路標(biāo)的中心坐標(biāo)為(501，260),輔助部分1的中心坐標(biāo)為(447，310)，已知圖像中心坐標(biāo)為(568， 426)， A: = 5.1， a = 0°, X = 0， 7 = 140 。則機(jī)器人在圖像中相對(duì)于路標(biāo)坐標(biāo) 系的位置為
Aw = —V(501 — 568)2 + (260 — 426)2 x sin(arctan 568 — 501 — arctan 447 — 501) -167 、 426-260 310-260
568 — 501 447 — 501、
An = —V(501 — 568)2 + (260 — 426)' x cos(arctan--arctan-) —64
V、 J 、 乂、 426-260 310 -260'
機(jī)器人在世界坐標(biāo)系的絕對(duì)坐標(biāo)為
JT = 0 + ^(501 — 568)2 + (260 — 426)2 x sin[O _ (arctan 568 —501 — arctan 447 — 501)] / 5.1 —"cwj ^ ,、 /l、 426-260 310-260
r = 140 — V(501 — 568)2 + (260 — 426)2 x cos[O — (arctan 568 — 5Q1 — arctan 447 — 5%] / 5.1 ^ 、 ,lv 426 — 260 310 — 260
實(shí)際測(cè)量的-34cw，5'= 129cw ，誤差小于2cw 應(yīng)用例3:
機(jī)器人導(dǎo)航實(shí)例如圖7所示，圖像上方的路標(biāo)4為當(dāng)前路標(biāo)，并且已知
jc = 399，少=101， x0 = 568jo = 426。
1、 若機(jī)器人按路標(biāo)指示的固定路徑行走，則
fl + 568 — 399 ，7;。
y = 0 = arctan-= 27.5
7 426 - 101
27.5° >10° ，小車需順時(shí)針旋轉(zhuǎn)才能到達(dá)當(dāng)前路標(biāo)4。
2、 若編程設(shè)定行走路線，設(shè)當(dāng)前目標(biāo)點(diǎn)5在當(dāng)前路標(biāo)(路標(biāo)4)左側(cè)，iAJ = -60， 已知& = 5.1,貝U
V= id +娠=568 - 5.1 x 60 = 262
+ 262 - 399 荊。
y = arctan-= —22.9
426 -101
因?yàn)?22.9° <-10'，小車需逆時(shí)針旋轉(zhuǎn)才能到達(dá)當(dāng)前目標(biāo)點(diǎn)5。 應(yīng)用例4:
預(yù)測(cè)路標(biāo)位置實(shí)例如圖8所示，已知在圖像坐標(biāo)系中路標(biāo)中心坐標(biāo)為(396,378)，輔
助部分2的中心坐標(biāo)為(458，342)，尋找下一路標(biāo)的閾值r/為375， 6 = 5.1， " = 140。
因?yàn)閊>7/,需尋找新路標(biāo)，新路標(biāo)在下一幀圖像中的坐標(biāo)計(jì)算如下
458 — 396_ ^M。
伊=arccos~p w 30.14
V(458-396)2 +(342-378)2
jc'=396 + 5.1x140xcos30.1° "013 盧378 —5.1xl40xsin30.1。 《19
1權(quán)利要求
1、一種基于單個(gè)路標(biāo)的機(jī)器人視覺(jué)定位方法，其特征在于該定位方法包括如下步驟第一、路標(biāo)的設(shè)計(jì)該路標(biāo)為表盤式組合結(jié)構(gòu)，路標(biāo)主體是半徑為R的圓盤，圓盤圓心為路標(biāo)中心，圓盤上設(shè)有用于機(jī)器人定位的第一輔助部分和指示下一路標(biāo)所在方向的第二輔助部分；路標(biāo)的主體采用黃色，第一輔助部分采用藍(lán)色，第二輔助部分采用紫色；第二、路標(biāo)鋪設(shè)首先確定機(jī)器人的移動(dòng)路徑，然后將路標(biāo)間隔鋪設(shè)在移動(dòng)路徑上方，鋪設(shè)時(shí)，使當(dāng)前路標(biāo)的中心點(diǎn)及其第二輔助部分的中心點(diǎn)和下一路標(biāo)的中心點(diǎn)及其第一輔助部分的中心點(diǎn)位于同一條直線上；第三、路標(biāo)的識(shí)別與定位令機(jī)器人起始時(shí)位于第一個(gè)路標(biāo)下方，由機(jī)器人對(duì)路標(biāo)進(jìn)行圖像采集，并對(duì)圖像進(jìn)行顏色識(shí)別，利用模板匹配確定在圖像坐標(biāo)系中路標(biāo)的中心坐標(biāo)(x，y)，利用統(tǒng)計(jì)平均坐標(biāo)確定在圖像坐標(biāo)系中路標(biāo)第一輔助部分的中心坐標(biāo)(x1，y1)和第二輔助部分的中心坐標(biāo)(x2，y2)；第四、機(jī)器人的定位1)、機(jī)器人的相對(duì)定位令MO1N為路標(biāo)坐標(biāo)系，M’O1’N’為路標(biāo)坐標(biāo)系在圖像中的投影，則機(jī)器人在圖像中相對(duì)于路標(biāo)坐標(biāo)系的坐標(biāo)為Δm＝-l×sin(θ-β)Δn＝-l×cos(θ-β)<maths id="math0001" num="0001" ><math><![CDATA[ <mrow><mi>&beta;</mi><mo>=</mo><mi>arctan</mi><mfrac> <mrow><msub> <mi>x</mi> <mn>1</mn></msub><mo>-</mo><mi>x</mi> </mrow> <mrow><msub> <mi>y</mi> <mn>1</mn></msub><mo>-</mo><mi>y</mi> </mrow></mfrac> </mrow>]]></math></maths><maths id="math0002" num="0002" ><math><![CDATA[ <mrow><mi>&theta;</mi><mo>=</mo><mi>arctan</mi><mfrac> <mrow><msub> <mi>x</mi> <mn>0</mn></msub><mo>-</mo><mi>x</mi> </mrow> <mrow><msub> <mi>y</mi> <mn>0</mn></msub><mo>-</mo><mi>y</mi> </mrow></mfrac> </mrow>]]></math></maths>其中，l為圖像中路標(biāo)距圖像中心的距離；β為圖像中路標(biāo)坐標(biāo)系N’軸與機(jī)器人前進(jìn)方向的夾角，θ為圖像中心和路標(biāo)中心連線與機(jī)器人前進(jìn)方向的夾角，(x0，y0)代表機(jī)器人在圖像中的坐標(biāo)；2)、機(jī)器人的絕對(duì)定位假設(shè)路標(biāo)在世界坐標(biāo)系XwOwYw中的坐標(biāo)為(X，Y)，則機(jī)器人在世界坐標(biāo)系中的絕對(duì)坐標(biāo)(X′，Y′)為X′＝X+Lsin(α-λ)Y′＝Y(jié)-Lcos(α-λ)其中L＝l/k；λ＝θ-β；α為世界坐標(biāo)系與路標(biāo)坐標(biāo)系的夾角，若路標(biāo)坐標(biāo)系相對(duì)于世界坐標(biāo)系發(fā)生逆時(shí)針旋轉(zhuǎn)則α為正，反之為負(fù)，k為圖像坐標(biāo)系與世界坐標(biāo)系的比例系數(shù)。
2、根據(jù)權(quán)利要求i所述的方法，其特征在于圖像坐標(biāo)系與世界坐標(biāo)系的比例系數(shù);t的 計(jì)算方法如下-統(tǒng)計(jì)與路標(biāo)同高度的長(zhǎng)度為L(zhǎng)cm的線段在圖像中的像素個(gè)數(shù)w ，則比例系數(shù)為
3、 根據(jù)權(quán)利要求1或2所述的方法，其特征在于第三步路標(biāo)的識(shí)別與定位中對(duì)圖像 進(jìn)行顏色識(shí)別的處理區(qū)域?yàn)檫呴L(zhǎng)4M ~ 6矢及的方形區(qū)域，路標(biāo)主體半徑R的取值范圍一般 為10~30cm 。
4、 根據(jù)權(quán)利要求3所述的方法，其特征在于第三步路標(biāo)的識(shí)別與定位方法是1) 、設(shè)定黃色的顏色閾值為123<^/< 143, 32 <//<52，紫色的閾值范圍為 174<C/<194， 322 <"< 342，藍(lán)色的閾值范圍為176 < f/< 196 ， 188 <//< 212 ; 其中C7為YUV顏色空間的色調(diào)分量U，//為HSI顏色空間的色調(diào)分量H;2) 、計(jì)算處理區(qū)域的起止坐標(biāo)(;c-/^及，y-piU )， (jc + /wW ，_y + /7)U )， 2《p《3;3) 、利用設(shè)置的顏色閾值對(duì)處理區(qū)域內(nèi)的每一像素點(diǎn)進(jìn)行顏色識(shí)別，并分別將黃色、 紫色、藍(lán)色和其他顏色的像素點(diǎn)灰度值設(shè)置為255、 150、 100和0，然后進(jìn)行腐蝕濾波和 膨脹濾波去除干擾點(diǎn)；4) 、統(tǒng)計(jì)灰度值非0的像素點(diǎn)的平均坐標(biāo)(jc，，y,，)，采用邊長(zhǎng)為1.5M的方形為模板，在Oc一j一)附近進(jìn)行模板匹配精確定位當(dāng)前路標(biāo)中心在圖像坐標(biāo)系的坐標(biāo)(x,力；5) 、分別統(tǒng)計(jì)以(:c,力為中心，2M為邊長(zhǎng)的方形區(qū)域內(nèi)灰度值為150和100的像素點(diǎn)的平均坐標(biāo)，即為當(dāng)前路標(biāo)第一輔助部分的中心在圖像坐標(biāo)系的坐標(biāo)(JCp;O和第二輔助 部分的中心在圖像坐標(biāo)系的坐標(biāo)(^,h)。
5、 一種基于權(quán)利要求1所述機(jī)器人視覺(jué)定位方法的機(jī)器人視覺(jué)導(dǎo)航方法，其特征在 于該方法還包括如下步驟第五、導(dǎo)航方法1) 、固定路徑時(shí)的導(dǎo)航方法令機(jī)器人按權(quán)利要求1中第二步鋪設(shè)的路標(biāo)沿固定路徑移動(dòng)導(dǎo)航，則首先依第四步中的方法計(jì)算出e，機(jī)器人偏離角7 = 0，如果-r<y<r，機(jī)器人前行，如果7>7\機(jī)器 人需順時(shí)針旋轉(zhuǎn)，如果7<-r，機(jī)器人需逆時(shí)針旋轉(zhuǎn)，其中r為預(yù)先設(shè)定的是否對(duì)機(jī)器人進(jìn)行糾偏的閾值；2) 、按編程設(shè)定路徑時(shí)的導(dǎo)航方法如果路標(biāo)不變，導(dǎo)航路徑依導(dǎo)航目標(biāo)點(diǎn)編程設(shè)定，其中的導(dǎo)航目標(biāo)點(diǎn)可以是路徑中的某個(gè)路標(biāo)，也可以是路標(biāo)以外的某個(gè)點(diǎn)，則機(jī)器人偏離角7的計(jì)算公式為y = arctan~51-其中x。'-;c。 +itAJ， Arf為當(dāng)前目標(biāo)點(diǎn)到前一目標(biāo)點(diǎn)和當(dāng)前路標(biāo)連線的垂直距離并規(guī)定目標(biāo)點(diǎn)在連線右側(cè)時(shí)AJ為正，目標(biāo)點(diǎn)在連線左側(cè)時(shí)A^為負(fù)，如果-r<y<r,機(jī)器 人前行，如果y〉r,機(jī)器人需順時(shí)針旋轉(zhuǎn)，如果y〈-r，機(jī)器人需逆時(shí)針旋轉(zhuǎn)，r為預(yù)先設(shè)定的是否對(duì)機(jī)器人進(jìn)行糾偏的閾值；第六、預(yù)測(cè)路標(biāo)位置如果^<77"，則認(rèn)為當(dāng)前路標(biāo)有效，其在下一幀圖像中的坐標(biāo)仍預(yù)測(cè)為(jc，力； 如果;;>7/，則應(yīng)尋找新的路標(biāo)，預(yù)測(cè)新路標(biāo)在下一幀圖像中的坐標(biāo)(x'，/)為<formula>formula see original document page 4</formula>其中r/為是否尋找下一路標(biāo)的閾值，D為兩路標(biāo)在世界坐標(biāo)系的距離。
6、根據(jù)權(quán)利要求5所述的方法，其特征在于是否尋找下一路標(biāo)的閾值r/取值范圍一般為(Cy。-20A:)~:v。)，機(jī)器人延遲越大，r/越小，機(jī)器人延遲越小，T/應(yīng)越接近圖像中心縱坐標(biāo)少。。
全文摘要
基于單個(gè)路標(biāo)的視覺(jué)定位與導(dǎo)航方法。解決現(xiàn)有定位與導(dǎo)航方法實(shí)時(shí)性不高、抗干擾能力差、路徑不易改變等問(wèn)題。本發(fā)明提供的路標(biāo)包括圓盤式路標(biāo)主體和圓盤上用于對(duì)機(jī)器人進(jìn)行定位的第一輔助部分和指示下一路標(biāo)所在方向的第二輔助部分。該方法利用單個(gè)路標(biāo)即可實(shí)現(xiàn)機(jī)器人的定位與導(dǎo)航，包括將路標(biāo)間隔鋪設(shè)在機(jī)器人上方；通過(guò)鏡頭獲取場(chǎng)景圖像并對(duì)路標(biāo)及其第一、第二輔助部分進(jìn)行識(shí)別定位，然后利用圖像坐標(biāo)系、路標(biāo)坐標(biāo)系和世界坐標(biāo)系的關(guān)系對(duì)機(jī)器人進(jìn)行定位以及導(dǎo)航。基于該路標(biāo)的定位與導(dǎo)航算法有效的縮小了處理區(qū)域，實(shí)時(shí)性好，抗干擾能力強(qiáng)，路徑可編程設(shè)定。
文檔編號(hào)G01C11/00GK101660908SQ20091007040
公開(kāi)日2010年3月3日 申請(qǐng)日期2009年9月11日 優(yōu)先權(quán)日2009年9月11日
發(fā)明者曹作良, 聃 王, 賈云偉 申請(qǐng)人:天津理工大學(xué)