一種機(jī)器人導(dǎo)航路徑規(guī)劃方法
【專利摘要】本發(fā)明涉及一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，包括：設(shè)定一個(gè)起始路標(biāo)，以該起始路標(biāo)為基礎(chǔ)將所述機(jī)器人所處場景內(nèi)設(shè)置的所有路標(biāo)進(jìn)行層級劃分，并且將相鄰層級直屬關(guān)系路標(biāo)之間的位置關(guān)系進(jìn)行保存，作為路標(biāo)索引；輸入機(jī)器人當(dāng)前路標(biāo)與目標(biāo)路標(biāo)；確定當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的層級；搜索當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的共同關(guān)系路標(biāo)；確定所述路徑為當(dāng)前路標(biāo)到共同關(guān)系路標(biāo)與共同關(guān)系路標(biāo)到目標(biāo)路標(biāo)的路徑，路徑規(guī)劃完成。本技術(shù)方案的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，采用多叉樹結(jié)構(gòu)將路標(biāo)劃分層級，將路標(biāo)層級與相鄰層級直屬關(guān)系的路標(biāo)之間的位置關(guān)系保存為路標(biāo)索引，使得路徑規(guī)劃中算法簡單、效率高，規(guī)劃的路徑唯一，很適合在嵌入式設(shè)備上運(yùn)行。
【專利說明】
一種機(jī)器人導(dǎo)航路徑規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于機(jī)器人視覺導(dǎo)航領(lǐng)域，特別涉及一種機(jī)器人導(dǎo)航路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 為了實(shí)現(xiàn)機(jī)器人視覺導(dǎo)航，需要設(shè)置路標(biāo)和導(dǎo)航模塊，以及利用路標(biāo)與導(dǎo)航模塊 創(chuàng)建機(jī)器人視覺導(dǎo)航地圖。所述路標(biāo)可以設(shè)置在室內(nèi)的天花板上;所述導(dǎo)航模塊為了拍攝 與識別所述路標(biāo)，一般包括相機(jī)、圖像處理單元與計(jì)算單元，所述相機(jī)設(shè)置在機(jī)器人頭部頂 端，所述圖像處理單元、計(jì)算單元設(shè)置在機(jī)器人內(nèi)部;所述地圖創(chuàng)建一般是指機(jī)器人利用所 述相機(jī)拍攝路標(biāo)，并且利用導(dǎo)航模塊內(nèi)的圖像處理單元、計(jì)算單元等進(jìn)行路標(biāo)識別、標(biāo)定、 計(jì)算與確定各路標(biāo)之間的位置關(guān)系。
[0003] 在創(chuàng)建了機(jī)器人視覺導(dǎo)航地圖的基礎(chǔ)上，導(dǎo)航過程中根據(jù)導(dǎo)航指令，還需要進(jìn)行 機(jī)器人導(dǎo)航路徑的規(guī)劃。
[0004] 現(xiàn)有的機(jī)器人導(dǎo)航路徑的規(guī)劃方法一般采用智能算法、可視圖法、自由空間法、人 工勢場法等，這些方法各有優(yōu)缺點(diǎn)，也常常結(jié)合起來使用，但無一例外，計(jì)算過程都很復(fù)雜， 對于某些導(dǎo)航路徑規(guī)劃簡單的機(jī)器人來說，效率低，不便于嵌入式設(shè)備使用。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于解決現(xiàn)有技術(shù)中的機(jī)器人導(dǎo)航路徑規(guī)劃方法計(jì)算復(fù)雜、效率 低、不便于嵌入式設(shè)備使用的問題。
[0006] 為實(shí)現(xiàn)上述目的，一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，包括：
[0007] 步驟100:設(shè)定一個(gè)起始路標(biāo)，以該起始路標(biāo)為基礎(chǔ)將所述機(jī)器人所處場景內(nèi)設(shè)置 的所有路標(biāo)進(jìn)行層級劃分，并且將相鄰層級直屬關(guān)系路標(biāo)之間的位置關(guān)系進(jìn)行保存，作為 路標(biāo)索引；
[0008] 步驟200:輸入所述機(jī)器人的當(dāng)前路標(biāo)與目標(biāo)路標(biāo)；
[0009] 步驟300:搜索所述當(dāng)前路標(biāo)與所述目標(biāo)路標(biāo)的共同關(guān)系路標(biāo)；
[0010] 步驟400:確定所述路徑為當(dāng)前路標(biāo)經(jīng)所述共同關(guān)系路標(biāo)到目標(biāo)路標(biāo)的路徑，路徑 規(guī)劃完成。
[0011] 進(jìn)一步地，所述步驟100包括：
[0012] 步驟110:設(shè)置路標(biāo)的數(shù)量，并設(shè)定一個(gè)起始路標(biāo)，所述起始路標(biāo)為第一層級路標(biāo)；
[0013] 步驟120:所述機(jī)器人從起始路標(biāo)開始識別路標(biāo)，對路標(biāo)進(jìn)行相機(jī)標(biāo)定，并對每個(gè) 路標(biāo)進(jìn)行層級劃分。
[0014] 步驟130:計(jì)算所有相鄰層級的具有直屬關(guān)系的兩個(gè)路標(biāo)坐標(biāo)系的轉(zhuǎn)換關(guān)系矩陣。
[0015] 進(jìn)一步地，所述步驟100中以路標(biāo)為節(jié)點(diǎn)，采用多叉樹結(jié)構(gòu)對路標(biāo)進(jìn)行層級劃分， 所述起始路標(biāo)為第一層級路標(biāo)。
[0016] 進(jìn)一步地，所述機(jī)器人從已劃分層級的路標(biāo)出發(fā)，能夠同時(shí)識別到所述已劃分層 級的路標(biāo)與其他未劃分層級路標(biāo)時(shí)，將所述其他未劃分層級路標(biāo)的層級數(shù)劃分為比所述已 劃分層級的路標(biāo)大一個(gè)層級。
[0017] 進(jìn)一步地，所述直屬關(guān)系是指，如果兩個(gè)路標(biāo)之間的層級相差一個(gè)層級，且層級較 大的路標(biāo)的層級是根據(jù)層級較小的路標(biāo)的層級而直接確定的，則這兩個(gè)路標(biāo)具有直屬關(guān) 系。
[0018] 進(jìn)一步地，所述相機(jī)標(biāo)定的時(shí)刻是機(jī)器人能夠穩(wěn)定并且準(zhǔn)確的反映出識別的路標(biāo) 的時(shí)刻。
[0019] 進(jìn)一步地，所述步驟130中，設(shè)0ΧΥ、0'X'Y'是兩個(gè)相鄰路標(biāo)的坐標(biāo)系，0'在0ΧΥ中的 坐標(biāo)為(tx，ty)，由X軸到X'軸的角度為Θ，則所述轉(zhuǎn)換關(guān)系矩陣Α是一個(gè)可逆矩陣，即：
[0020]
[0021] 進(jìn)一步地，所述路標(biāo)包括：
[0022] 第一坐標(biāo)標(biāo)記，用于確定路標(biāo)坐標(biāo)系的X軸；
[0023] 第二坐標(biāo)標(biāo)記，用于確定路標(biāo)坐標(biāo)系的Y軸；
[0024] 區(qū)域標(biāo)記，用于確定路標(biāo)所在的空間位置，所述區(qū)域標(biāo)記包括多個(gè)等距離的信息 塊，所述尺寸是所述信息塊之間的距離或者信息塊的長度。
[0025] 進(jìn)一步地，所述機(jī)器人使用所述尺寸將實(shí)際空間與相機(jī)坐標(biāo)系下的尺寸進(jìn)行轉(zhuǎn) 換。
[0026] 進(jìn)一步地，所述步驟300包括：
[0027]步驟310:確定當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的層級；
[0028]步驟320:比較當(dāng)前路標(biāo)層級與路標(biāo)目標(biāo)層級是否相同，若相同，執(zhí)行步驟330,否 則執(zhí)行步驟340;
[0029]步驟330:分別逐層級搜索當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的上層級路標(biāo)，直到搜索到同一個(gè) 路標(biāo)，所述搜索到的同一個(gè)路標(biāo)為共同關(guān)系路標(biāo)，結(jié)束搜索；
[0030]步驟340:比較當(dāng)前路標(biāo)層級與目標(biāo)路標(biāo)層級的大小;若當(dāng)前路標(biāo)層級大于目標(biāo)路 標(biāo)層級，執(zhí)行步驟350，否則執(zhí)行步驟360;
[0031]步驟350:搜索當(dāng)前路標(biāo)的上層級路標(biāo)，直到搜索到與目標(biāo)路標(biāo)同一層級的路標(biāo)， 如果搜索到的該同一層級路標(biāo)是目標(biāo)路標(biāo)，則所述目標(biāo)路標(biāo)為共同關(guān)系路標(biāo)，結(jié)束搜索；否 則逐層搜索該同一層級路標(biāo)和目標(biāo)路標(biāo)的上層級路標(biāo)，直到搜索到同一個(gè)路標(biāo)作為共同關(guān) 系路標(biāo)，結(jié)束搜索；
[0032]步驟360:搜索目標(biāo)路標(biāo)的上層級路標(biāo)，直到搜索到與當(dāng)前路標(biāo)同一層級的路標(biāo)， 如果搜索到的該同一層級路標(biāo)是當(dāng)前路標(biāo)，則所述當(dāng)前路標(biāo)為共同關(guān)系路標(biāo)，結(jié)束搜索；否 則逐層搜索該同一層級路標(biāo)和當(dāng)前路標(biāo)的上層級路標(biāo)，直到搜索到同一個(gè)路標(biāo)作為共同關(guān) 系路標(biāo)，結(jié)束搜索。
[0033] 與現(xiàn)有技術(shù)相比，本技術(shù)方案具有以下優(yōu)點(diǎn)：
[0034] 本技術(shù)方案的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，采用多叉樹結(jié)構(gòu)將路標(biāo)劃分層級， 將路標(biāo)層級與相鄰層級直屬關(guān)系的路標(biāo)之間的位置關(guān)系保存為路標(biāo)索引，使得路徑規(guī)劃中 算法簡單、效率高，規(guī)劃的路徑唯一，很適合在嵌入式設(shè)備上運(yùn)行。
【附圖說明】
[0035] 圖1為本發(fā)明的一種機(jī)器人路徑規(guī)劃方法的流程圖。
[0036] 圖2為本發(fā)明的路標(biāo)多叉樹結(jié)構(gòu)的不意圖。
[0037] 圖3為本發(fā)明搜索當(dāng)前路標(biāo)和目標(biāo)路標(biāo)的共同關(guān)系路標(biāo)的流程圖。
[0038] 圖4-6為本發(fā)明劃分路標(biāo)層級的示意圖。
[0039]圖7為本發(fā)明的路標(biāo)結(jié)構(gòu)示意圖。
[0040](注意：附圖中的所示結(jié)構(gòu)只是為了說明本發(fā)明特征的示意，并非是要依據(jù)附圖所 示結(jié)構(gòu)。）
【具體實(shí)施方式】
[0041] 為使本發(fā)明的上述目的、特征和優(yōu)點(diǎn)能夠更為明顯易懂，下面結(jié)合附圖對本發(fā)明 的具體實(shí)施例進(jìn)行詳細(xì)的說明。
[0042] 本發(fā)明提供了一種機(jī)器人導(dǎo)航路徑規(guī)劃方法。
[0043] 所述的機(jī)器人導(dǎo)航路徑規(guī)劃方法建立在機(jī)器人視覺導(dǎo)航地圖的基礎(chǔ)上。
[0044] 所述的機(jī)器人視覺導(dǎo)航地圖的創(chuàng)建一般是指利用設(shè)置在機(jī)器人導(dǎo)航模塊中的相 機(jī)拍攝機(jī)器人所處場景內(nèi)設(shè)定的路標(biāo)，并且利用設(shè)置在機(jī)器人導(dǎo)航模塊中的圖像處理單 元、計(jì)算單元等進(jìn)行路標(biāo)識別、標(biāo)定、計(jì)算與確定各路標(biāo)之間的位置關(guān)系，并保存在機(jī)器人 中，地圖創(chuàng)建完成。
[0045] 參考圖1，所述的機(jī)器人導(dǎo)航路徑規(guī)劃方法，包括：
[0046] 步驟100,設(shè)定一起始路標(biāo)，以起始路標(biāo)為基礎(chǔ)將機(jī)器人所處場景內(nèi)設(shè)置的所有路 標(biāo)進(jìn)行層級劃分，并且將相鄰層級直屬關(guān)系路標(biāo)之間的位置關(guān)系進(jìn)行保存，作為路標(biāo)索引。
[0047] 所述直屬關(guān)系是指，如果兩個(gè)路標(biāo)之間的層級相差一個(gè)層級，且層級較大的路標(biāo) 的層級是根據(jù)層級較小的路標(biāo)的層級而直接確定的，則這兩個(gè)路標(biāo)具有直屬關(guān)系。
[0048] 參考圖2,假設(shè)所述路標(biāo)的數(shù)量根據(jù)導(dǎo)航需求設(shè)置為7個(gè)，采用多叉樹結(jié)構(gòu)以路標(biāo) 為節(jié)點(diǎn)，將7個(gè)路標(biāo)劃分為三個(gè)層級，起始路標(biāo)E為第一層級路標(biāo)，第四路標(biāo)D、第六路標(biāo)F為 第二層級路標(biāo)，第一路標(biāo)A、第二路標(biāo)B、第三路標(biāo)C、第七路標(biāo)G為第三層級路標(biāo)，并且保存7 個(gè)路標(biāo)的層級與相鄰層級直屬關(guān)系路標(biāo)之間的位置關(guān)系，作為路標(biāo)索引。具體的層級劃分 方法在后文說明。
[0049]所述多叉樹結(jié)構(gòu)常用于計(jì)算機(jī)領(lǐng)域，包括節(jié)點(diǎn)與樹枝;樹枝將節(jié)點(diǎn)連接起來就構(gòu) 成了多叉樹結(jié)構(gòu)。
[0050]本實(shí)施例中，所述節(jié)點(diǎn)為路標(biāo)，所述相鄰層級直屬關(guān)系路標(biāo)之間的位置關(guān)系為樹 枝。
[0051 ]步驟200，輸入機(jī)器人當(dāng)前路標(biāo)與目標(biāo)路標(biāo)。
[0052]所述當(dāng)前路標(biāo)是指與機(jī)器人當(dāng)前所處位置相對應(yīng)的路標(biāo)，所述目標(biāo)路標(biāo)，是指與 機(jī)器人導(dǎo)航目的地位置相對應(yīng)的路標(biāo)。
[0053]參考圖2,假設(shè)機(jī)器人位于第七路標(biāo)G相對應(yīng)的位置，根據(jù)指令要去到第一路標(biāo)A相 對應(yīng)的位置，則，第七路標(biāo)G為當(dāng)前路標(biāo)，第一路標(biāo)A為目標(biāo)路標(biāo)。
[0054]所述當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的輸入，是將路標(biāo)的ID值輸入，每個(gè)路標(biāo)具有不同的ID 值，ID值根據(jù)需要設(shè)定是數(shù)值；
[0055]所述當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的輸入，只要是給每個(gè)路標(biāo)設(shè)定了不同的代碼，代碼可 以是任意形式，能夠輸入機(jī)器人，并且機(jī)器人能夠識別即可。
[0056]步驟300,搜索當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的共同關(guān)系路標(biāo)。
[0057]參考圖3,所述共同關(guān)系路標(biāo)的確定，包括：
[0058]步驟310,確定當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的層級。
[0059] 根據(jù)步驟100中設(shè)置的路標(biāo)索引，能夠知道所有路標(biāo)的層級，因而也可以輕松的確 定當(dāng)前路標(biāo)的層級與目標(biāo)路標(biāo)的層級。
[0060] 步驟320，比較當(dāng)前路標(biāo)層級與路標(biāo)目標(biāo)層級，若相同，執(zhí)行步驟330，若不同執(zhí)行 步驟340。
[0061 ]所述的比較當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的層級有兩種結(jié)果:一是當(dāng)前路標(biāo)與目標(biāo)路標(biāo)層 級相同，二是當(dāng)前路標(biāo)與目標(biāo)路標(biāo)層級不同。
[0062] 所述的當(dāng)前路標(biāo)與目標(biāo)路標(biāo)層級不同也分為兩種情況，一是當(dāng)前路標(biāo)層級大于目 標(biāo)路標(biāo)，二是當(dāng)前路標(biāo)層級小于目標(biāo)路標(biāo)。
[0063] 所述的層級大小是指，若N大于Μ，則第N層級路標(biāo)的層級大于第Μ層級路標(biāo)的層級。 [0064]比如，第三層級路標(biāo)大于第二層級路標(biāo)與第一層級路標(biāo)，第二層級路標(biāo)大于第一 層級路標(biāo)。
[0065]步驟330,分別逐層級搜索當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的上層級路標(biāo)，直到搜索到同一個(gè) 路標(biāo)，所述搜索到的同一個(gè)路標(biāo)為共同關(guān)系路標(biāo)，結(jié)束搜索。
[0066] 所述當(dāng)前路標(biāo)的上層級路標(biāo)，是指與當(dāng)前路標(biāo)具有直屬關(guān)系的層級逐層級減小的 所有路標(biāo);以此類推任意路標(biāo)。
[0067] 所述當(dāng)前路標(biāo)的上層級路標(biāo)包含上一層級路標(biāo)，上一層級路標(biāo)的層級比當(dāng)前路標(biāo) 小一層級，上二層級路標(biāo)的層級比當(dāng)前路標(biāo)小二層，上Ν層級路標(biāo)的層級比當(dāng)前路標(biāo)小Ν層 級；以此類推任意路標(biāo)。
[0068]參考圖2,第三層級的第七路標(biāo)G的上層級路標(biāo)包括第六路標(biāo)D與起始路標(biāo)Ε，具體 的說，第六路標(biāo)D是當(dāng)前路標(biāo)的上一層級路標(biāo)，層級為第二層級比第三層級小一層，起始路 標(biāo)Ε是當(dāng)前路標(biāo)的上二層級路標(biāo)，層級為第一層級比第三層級小二層。
[0069] 參考圖2，假設(shè)第七路標(biāo)G為當(dāng)前路標(biāo)，第一路標(biāo)Α為目標(biāo)路標(biāo)，兩個(gè)路標(biāo)都是第三 層級路標(biāo)，層級相同；分別逐層對第七路標(biāo)G和第一路標(biāo)A做上層級路標(biāo)搜索，搜索到第七路 標(biāo)G的上一層級路標(biāo)為第六路標(biāo)F，第一路標(biāo)A的上一層級路標(biāo)為第四路標(biāo)D，未找到共同關(guān) 系路標(biāo);對第六路標(biāo)F和第四路標(biāo)D繼續(xù)做上一層級路標(biāo)的搜索，第六路標(biāo)F和第四路標(biāo)D的 上一層級路標(biāo)都是起始路標(biāo)E，則起始路標(biāo)E為第六路標(biāo)G和第一路標(biāo)A的共同關(guān)系路標(biāo)，結(jié) 束搜索。
[0070] 步驟340,比較當(dāng)前路標(biāo)的層級與目標(biāo)路標(biāo)的層級的大小，若當(dāng)前路標(biāo)層級大于目 標(biāo)路標(biāo)層級，執(zhí)行步驟350，否則執(zhí)行步驟360。
[0071]步驟350:搜索當(dāng)前路標(biāo)的上層級路標(biāo)，直到搜索到與目標(biāo)路標(biāo)同一層級的路標(biāo)， 如果搜索到的該同一層級路標(biāo)是目標(biāo)路標(biāo)，所述目標(biāo)路標(biāo)確定為共同關(guān)系路標(biāo)，結(jié)束搜索； 否則逐層搜索該同一層級路標(biāo)和目標(biāo)路標(biāo)的上層級路標(biāo)，直到搜索到同一個(gè)路標(biāo)作為共同 關(guān)系路標(biāo)，結(jié)束搜索。
[0072] 在本實(shí)施例中，參考圖2,假設(shè)第七路標(biāo)G為當(dāng)前路標(biāo)，其層級為第三層級，第四路 標(biāo)D為目標(biāo)路標(biāo)，其層級為第二層級，第七路標(biāo)G的層級大于第四路標(biāo)D的層級;逐層搜索層 級大的第七路標(biāo)G的上層級路標(biāo)，直到搜索到與第四路標(biāo)D同層級的路標(biāo)，搜索到了第七路 標(biāo)G的上一層級路標(biāo)第六路標(biāo)F，第六路標(biāo)F與第四路標(biāo)D同級，此時(shí)，分別逐層搜索第六路標(biāo) F與第四路標(biāo)D的上層級路標(biāo)，搜索到了第六路標(biāo)F與第四路標(biāo)D的上一層級路標(biāo)起始路標(biāo)E， 找到了共同關(guān)系路標(biāo)起始路標(biāo)E，結(jié)束搜索。
[0073] 在本實(shí)施例中，參考圖2，假設(shè)第七路標(biāo)G為當(dāng)前路標(biāo)，其層級為第三層級，第六路 標(biāo)F為目標(biāo)路標(biāo)，其層級為第二層級，第七路標(biāo)G的層級大于第六路標(biāo)F的層級;逐層搜索層 級大的第七路標(biāo)G的上層級路標(biāo)，直到搜索到與第六路標(biāo)F同層級的路標(biāo)，搜索到了第七路 標(biāo)G的上一層級路標(biāo)第六路標(biāo)F，此時(shí)，找到了共同關(guān)系路標(biāo)，即目標(biāo)路標(biāo)第六路標(biāo)F，結(jié)束搜 索。
[0074]步驟360:搜索目標(biāo)路標(biāo)的上層級路標(biāo)，直到搜索到與當(dāng)前路標(biāo)同一層級的路標(biāo)， 如果搜索到的該同一層級路標(biāo)是當(dāng)前路標(biāo)，所述當(dāng)前路標(biāo)確定為共同關(guān)系路標(biāo)，結(jié)束搜索； 否則逐層搜索該同一層級路標(biāo)和當(dāng)前路標(biāo)的上層級路標(biāo)，直到搜索到同一個(gè)路標(biāo)作為共同 關(guān)系路標(biāo)，結(jié)束搜索。
[0075] 在本實(shí)施例中，參考圖2，假設(shè)第七路標(biāo)G為目標(biāo)路標(biāo)，其層級為第三層級，第四路 標(biāo)D為當(dāng)前路標(biāo)，其層級為第二層級，第七路標(biāo)G的層級大于第四路標(biāo)D的層級;逐層搜索層 級大的第七路標(biāo)G的上層級路標(biāo)，直到搜索到與第四路標(biāo)D同層級的路標(biāo)，搜索到了第七路 標(biāo)G的上一層級路標(biāo)第六路標(biāo)F，第六路標(biāo)F與第四路標(biāo)D同級，此時(shí)，分別逐層搜索第六路標(biāo) F與第四路標(biāo)D的上層級路標(biāo)，搜索到了第六路標(biāo)F與第四路標(biāo)D的上一層級路標(biāo)起始路標(biāo)E， 找到了共同關(guān)系路標(biāo)起始路標(biāo)E，結(jié)束搜索。
[0076] 在本實(shí)施例中，參考圖2，假設(shè)第七路標(biāo)G為目標(biāo)路標(biāo)，其層級為第三層級，第六路 標(biāo)F為當(dāng)前路標(biāo)，其層級為第二層級，第七路標(biāo)G的層級大于第六路標(biāo)F的層級;逐層搜索層 級大的第七路標(biāo)G的上層級路標(biāo)，直到搜索到與第六路標(biāo)F同層級的路標(biāo)，搜索到了第七路 標(biāo)G的上一層級路標(biāo)第六路標(biāo)F，此時(shí)，找到了共同關(guān)系路標(biāo)，即當(dāng)前路標(biāo)第六路標(biāo)F，結(jié)束搜 索。
[0077] 步驟400,確定所述路徑為當(dāng)前路標(biāo)經(jīng)所述共同關(guān)系路標(biāo)到目標(biāo)路標(biāo)的路徑，路徑 規(guī)劃完成。
[0078] 根據(jù)步驟100中確定的路標(biāo)索引，能夠知道所述相鄰層級直屬關(guān)系路標(biāo)之間的位 置關(guān)系，則可以知道當(dāng)前路標(biāo)到共同關(guān)系路標(biāo)的路徑，也可以知道共同關(guān)系路標(biāo)到目標(biāo)路 標(biāo)的路徑，兩段路徑結(jié)合在一起，路徑規(guī)劃完成。
[0079] 在本實(shí)施例中，參考圖2，假設(shè)第七路標(biāo)G為當(dāng)前路標(biāo)，第一路標(biāo)A為目標(biāo)路標(biāo)，找到 第七路標(biāo)G與第一路標(biāo)A的共同關(guān)系路標(biāo)后，根據(jù)索引中的相鄰層級直屬路標(biāo)之間的位置關(guān) 系，能夠知道當(dāng)前路標(biāo)到共同關(guān)系路標(biāo)的路徑，即第七路標(biāo)G到第六路標(biāo)F與第六路標(biāo)F到起 始路標(biāo)E的路徑，也能夠知道共同關(guān)系路標(biāo)到目標(biāo)路標(biāo)的路徑，即第一路標(biāo)A到第四路標(biāo)D與 第四路標(biāo)D到起始路標(biāo)E的路徑，導(dǎo)航路徑規(guī)劃完成。
[0080] 上述方法如果基于與以路標(biāo)為節(jié)點(diǎn)的多叉樹結(jié)構(gòu)的機(jī)器人視覺導(dǎo)航地圖，會具有 工作量小，效率高的優(yōu)點(diǎn)，因此所述的機(jī)器人路徑規(guī)劃方法建立在所述多叉樹結(jié)構(gòu)的機(jī)器 人視覺導(dǎo)航地圖的基礎(chǔ)上，步驟1〇〇是根據(jù)多叉樹結(jié)構(gòu)的機(jī)器人視覺導(dǎo)航地圖來確定路標(biāo) 索引，包括以下步驟：
[0081 ]步驟1，設(shè)置路標(biāo)的數(shù)量，設(shè)定一起始路標(biāo)。
[0082] 在本實(shí)施例中，參考圖2,所述的機(jī)器人視覺導(dǎo)航地圖的創(chuàng)建，是指以路標(biāo)為節(jié)點(diǎn) 構(gòu)建多叉樹結(jié)構(gòu)，將此多叉樹結(jié)構(gòu)作為機(jī)器人視覺導(dǎo)航地圖并保存。
[0083] 在本實(shí)施例中，參考圖2,所述路標(biāo)的數(shù)量根據(jù)導(dǎo)航需求設(shè)置為7個(gè)，包括起始路標(biāo) E，第二層級一路標(biāo)D，第二層級二路標(biāo)F，第三層級一路標(biāo)A，第三層級二路標(biāo)B，第三層級三 路標(biāo)C，第三層級四路標(biāo)G。
[0084] 在其他實(shí)施例中，所述路標(biāo)的數(shù)量根據(jù)導(dǎo)航的需求可以設(shè)置任意數(shù)量，比如2個(gè)， 比如3個(gè)，比如100個(gè)，只要所述路標(biāo)數(shù)量能夠滿足機(jī)器人視覺導(dǎo)航需求即可。
[0085] 所述起始路標(biāo)可以設(shè)定為所有路標(biāo)中的任意一個(gè)，但一般選取位于所有路標(biāo)所處 空間中央位置的路標(biāo)。
[0086] 因?yàn)樵诙嗖鏄涞貓D構(gòu)建過程中，相機(jī)標(biāo)定與坐標(biāo)轉(zhuǎn)換關(guān)系矩陣計(jì)算都存在不可避 免的誤差，并且誤差是相乘疊加的；假設(shè)起始路標(biāo)E與第二層級一路標(biāo)D的相機(jī)標(biāo)定與坐標(biāo) 轉(zhuǎn)換關(guān)系計(jì)算誤差為A 1，第二層級一路標(biāo)D與第三層級一路標(biāo)A的相機(jī)標(biāo)定與坐標(biāo)轉(zhuǎn)換關(guān) 系計(jì)算誤差為A 2，則起始路標(biāo)E到第三層級一路標(biāo)A間的標(biāo)定與計(jì)算誤差為Δ 〇 = Δ 1* Δ 2， 因而，一般選擇位于所有所述路標(biāo)所處空間的中央位置的路標(biāo)為起始路標(biāo)，這樣，在使用本 發(fā)明所述方法時(shí)，由起始路標(biāo)E從其周圍開始構(gòu)建多叉樹結(jié)構(gòu)地圖，可以使得多叉樹結(jié)構(gòu)地 圖的層數(shù)少，以盡可能的減小誤差。
[0087] 在本實(shí)施例中，所述起始路標(biāo)E位于7個(gè)路標(biāo)所處空間的中央位置。
[0088] 在其他實(shí)施例中，所述起始路標(biāo)可以是所有路標(biāo)中的任意一個(gè)，比如參考本實(shí)施 例，可以是第二層級一路標(biāo)D，或者第二層級二路標(biāo)F，或者第三層級二路標(biāo)B。
[0089] 所述步驟1還包括設(shè)置所述路標(biāo)中的尺寸，使用所述尺寸可以將實(shí)際空間與相機(jī) 坐標(biāo)系下的尺寸進(jìn)行轉(zhuǎn)換。
[0090] 在本實(shí)施例中，所述路標(biāo)采用如圖7中所示的路標(biāo)，所述路標(biāo)包括第一坐標(biāo)標(biāo)記 11，用于確定坐標(biāo)X軸;第二坐標(biāo)標(biāo)記12，用于確定坐標(biāo)Y軸;還包括區(qū)域標(biāo)記100，區(qū)域標(biāo)記 設(shè)置于第一坐標(biāo)標(biāo)記11與第二坐標(biāo)標(biāo)記12確定的坐標(biāo)內(nèi)，用于確定路標(biāo)所在的空間位置； 所述區(qū)域標(biāo)記100內(nèi)包括等距離的信息塊，每個(gè)路標(biāo)包含的所述信息塊的數(shù)量和位置不同， 以區(qū)別路標(biāo)；圖2中所述的路標(biāo)包括信息塊110、信息塊120、信息塊130、信息塊140、信息塊 150、信息塊160、信息塊170、信息塊180和信息塊190,相鄰兩兩信息塊之間的距離為5cm。
[0091] 在其他實(shí)施例中，所述路標(biāo)區(qū)域標(biāo)記100內(nèi)若只包括信息塊110、信息塊130和信息 塊190,則，信息塊110與信息塊130之間的距離為10cm,也就是2個(gè)5cm,信息塊130和信息塊 190之間的距離為10cm，也是2個(gè)5cm，信息塊110和信息塊190之間的距離為在與路標(biāo)坐標(biāo)系 X軸與Y軸分別平行的兩個(gè)方向上均為l〇cm。
[0092] 在其他實(shí)施例中，采用如圖7中所述的路標(biāo)，所述的信息塊110、信息塊120、信息塊 130、信息塊140、信息塊150、信息塊160、信息塊170、信息塊180和信息塊190,相鄰兩兩信息 塊之間的距離為1 〇cm 〇
[0093] 在其他實(shí)施例中，采用如圖7中所述的路標(biāo)，所述的信息塊110、信息塊120、信息塊 130、信息塊140、信息塊150、信息塊160、信息塊170、信息塊180和信息塊190,相鄰兩兩信息 塊之間的距離為8cm 〇
[0094] 在其他實(shí)施例中，采用如圖7中所述的路標(biāo)，所述的區(qū)域標(biāo)記100內(nèi)的所述信息塊 可以不是3行3列，可以是4行4列或其他數(shù)量的行與其他數(shù)量的列。
[0095] 在本實(shí)施例中，根據(jù)路標(biāo)中設(shè)置的尺寸可以將所述實(shí)際空間中的尺寸與相機(jī)坐標(biāo) 系下的尺寸進(jìn)行轉(zhuǎn)換，比如，可以將實(shí)際空間中的寬度與相機(jī)坐標(biāo)系下的寬度進(jìn)行轉(zhuǎn)換，或 者也可以將實(shí)際空間中的距離與相機(jī)坐標(biāo)系下的距離進(jìn)行轉(zhuǎn)換;本實(shí)施例中，所述信息塊 之間的距離為5cm，假設(shè)拍攝所述路標(biāo)后，所述信息塊之間的距離在所述相機(jī)坐標(biāo)系下為 1mm，即可以確定所述相機(jī)坐標(biāo)系下10mm的距離在實(shí)際空間中距離為50cm，或者比如所述信 息塊的長度為lcm，所述信息塊的長度在所述相機(jī)坐標(biāo)系下為0.2mm，即可以確定所述相機(jī) 坐標(biāo)系下l〇mm的長度在實(shí)際空間中長度為50cm。
[0096] 在其他實(shí)施例中，所述路標(biāo)也可以采用諸如條形碼、二維碼等路標(biāo)，只要確定并設(shè) 置好所述路標(biāo)中的尺寸，能夠使用所述尺寸可以將實(shí)際空間與相機(jī)坐標(biāo)系下的尺寸進(jìn)行轉(zhuǎn) 換。
[0097] 步驟2，機(jī)器人從起始路標(biāo)開始識別路標(biāo)，對路標(biāo)進(jìn)行相機(jī)標(biāo)定，并對每個(gè)路標(biāo)進(jìn) 行層級劃分。
[0098] 所述相機(jī)標(biāo)定常用于機(jī)器視覺研究，在本發(fā)明中，是指將所述路標(biāo)的坐標(biāo)系都轉(zhuǎn) 化為所述相機(jī)的坐標(biāo)系，并且得知所述路標(biāo)在所述實(shí)際空間與相機(jī)坐標(biāo)系下的尺寸的轉(zhuǎn)換 數(shù)學(xué)關(guān)系，找出其定量的聯(lián)系。
[0099] 所述路標(biāo)的層級劃分由小到大，確定起始路標(biāo)為第一層級路標(biāo)，然后依次將所有 路標(biāo)進(jìn)行層級劃分。
[0100] 在本實(shí)施例中，所述起始路標(biāo)E為第一層級路標(biāo)，然后以起始路標(biāo)E開始將所有路 標(biāo)進(jìn)行層級劃分。
[0101] 在其他實(shí)施例中，所述路標(biāo)的層級劃分也可以由大到小，比如講起始路標(biāo)E劃分為 的第五層路標(biāo)，或者第N層路標(biāo)，N可以為任意數(shù)，也可以是字母等。
[0102] 所述路標(biāo)的層級大小是指:假設(shè)有第Μ層級路標(biāo)與第N層級路標(biāo)，Μ大于N，則，第Μ層 級路標(biāo)的層級大于第Ν層級路標(biāo)的層級。
[0103] 在本實(shí)施例中，起始路標(biāo)Ε為第一層級路標(biāo)，所述第二層級一路標(biāo)D的層級數(shù)大于 起始路標(biāo)Ε。
[0104] 在其他實(shí)施例中，所述路標(biāo)的層級大小可做其他規(guī)定，比如假設(shè)有第Μ層級路標(biāo)與 第Ν層級路標(biāo)，Μ大于Ν，則，第Ν層級路標(biāo)的層級大于第Μ層級路標(biāo)的層級。
[0105] 所述路標(biāo)的層級數(shù)確定采用如下方法:機(jī)器人從已劃分層級的路標(biāo)出發(fā)，能夠同 時(shí)識別到所述已劃分層級的路標(biāo)與其他未劃分層級路標(biāo)時(shí)，將所述其他未劃分層級路標(biāo)的 層級數(shù)劃分為比所述已劃分層級的路標(biāo)大一個(gè)層級。
[0106] 在本實(shí)施例中，所述機(jī)器人從起始路標(biāo)開始識別路標(biāo)，劃分起始路標(biāo)為第一層級 路標(biāo)，在人工干預(yù)下，機(jī)器人從起始路標(biāo)出發(fā)，在某一位置能夠同時(shí)識別到起始路標(biāo)與其他 路標(biāo)時(shí)，將其他路標(biāo)劃分為第二層級路標(biāo)，并對劃分好層級的路標(biāo)進(jìn)行相機(jī)標(biāo)定;所述第一 層級路標(biāo)分別與所有的所述第二層級路標(biāo)具有直屬關(guān)系。
[0107] 所述路標(biāo)的直屬關(guān)系是指，所述兩個(gè)路標(biāo)之間的層級相差一個(gè)層級，且層級較大 的路標(biāo)的層級是根據(jù)層級較小的路標(biāo)的層級而直接確定的;所述具有直屬關(guān)系的相鄰層級 兩個(gè)路標(biāo)在多叉樹結(jié)構(gòu)中直接相連。
[0108] 然后，機(jī)器人從第二層級路標(biāo)出發(fā)，在任何位置均不能夠同時(shí)識別到起始路標(biāo)與 其他路標(biāo)，而只能在某一位置同時(shí)識別到第二層級路標(biāo)與其他路標(biāo)時(shí)，將其他路標(biāo)劃分為 第三層級路標(biāo)，并對劃分好層級的路標(biāo)進(jìn)行相機(jī)標(biāo)定;采用同樣的方法劃分第四層級與第 四層級以上的路標(biāo)。
[0109] 在本實(shí)施例中，參考圖4,所述機(jī)器人1在人工干預(yù)下，從起始路標(biāo)E開始行走，通過 所述導(dǎo)航模塊中的相機(jī)2進(jìn)行路標(biāo)圖像拍攝與識別，當(dāng)所述導(dǎo)航模塊中的相機(jī)2能夠同時(shí)拍 攝到起始路標(biāo)E與第二層級一路標(biāo)D，并且準(zhǔn)確且穩(wěn)定的反映出拍攝到的是起始路標(biāo)E與第 二層級一路標(biāo)D時(shí)，人工干預(yù)，給機(jī)器人1 一個(gè)信號，機(jī)器人1對起始路標(biāo)E與第二層級一路標(biāo) D均進(jìn)行相機(jī)標(biāo)定，分別計(jì)算起始路標(biāo)E坐標(biāo)系、第二層級一路標(biāo)D坐標(biāo)系和所述相機(jī)坐標(biāo)系 的轉(zhuǎn)換關(guān)系矩陣，并且，將所述第二層級一路標(biāo)D劃分為第二層級路標(biāo)。
[0110]然后，參考圖5,機(jī)器人1在人工干預(yù)下從第二層級一路標(biāo)D出發(fā)沿著第三層級二路 標(biāo)B的方向行走，通過所述導(dǎo)航模塊中的相機(jī)2進(jìn)行路標(biāo)圖像拍攝與識別，在此過程中，當(dāng)機(jī) 器人1在任意位置不能同時(shí)識別到起始路標(biāo)E與第三層級二路標(biāo)B，而在某一位置能夠準(zhǔn)確 且穩(wěn)定的反映出同時(shí)識別到的是第二層級一路標(biāo)D與第三層級二路標(biāo)B時(shí)，人工干預(yù)，給機(jī) 器人1 一個(gè)信號，機(jī)器人1對第二層級一路標(biāo)D與第三層級二路標(biāo)B均進(jìn)行相機(jī)標(biāo)定，分別計(jì) 算第二層級一路標(biāo)D坐標(biāo)系、第三層級二路標(biāo)B坐標(biāo)系和所述相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系矩陣， 并且，將所述第三層級二路標(biāo)B劃分為第三層級路標(biāo)。
[0111] 最后，人工干預(yù)機(jī)器人回到第二層級一路標(biāo)D，沿第三層級一路標(biāo)A方向行走，當(dāng)過 程中機(jī)器人能夠準(zhǔn)確并穩(wěn)定的識別出第二層級一路標(biāo)D與第三層級一路標(biāo)A時(shí)，人工干預(yù)， 給機(jī)器人一個(gè)信號，機(jī)器人對第二層級一路標(biāo)D與第三層級一路標(biāo)A均進(jìn)行相機(jī)標(biāo)定，分別 計(jì)算第二層級一路標(biāo)D坐標(biāo)系、第三層級一路標(biāo)A坐標(biāo)系和所述相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系矩 陣，并且，將所述第三層級一路標(biāo)A劃分為第三層級路標(biāo)。
[0112] 在其他實(shí)施例中，參考圖6,所述機(jī)器人1在人工干預(yù)下，從起始路標(biāo)E開始行走，通 過所述導(dǎo)航模塊中的相機(jī)2進(jìn)行路標(biāo)圖像拍攝與識別，當(dāng)所述導(dǎo)航模塊中的相機(jī)2能夠同時(shí) 拍攝到起始路標(biāo)E、第二層級一路標(biāo)D與第二層級二路標(biāo)F，并且準(zhǔn)確且穩(wěn)定的反映出拍攝到 的是起始路標(biāo)E、第二層級一路標(biāo)D與第二層級二路標(biāo)F時(shí)，人工干預(yù)，給機(jī)器人一個(gè)信號，機(jī) 器人對起始路標(biāo)E、第二層級一路標(biāo)D、與第二層級二路標(biāo)F均進(jìn)行相機(jī)標(biāo)定，分別計(jì)算起始 路標(biāo)E坐標(biāo)系、第二層級一路標(biāo)D坐標(biāo)系、第二層級二路標(biāo)F和所述相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系矩 陣，并且，將所述第二層級一路標(biāo)D、與第二層級二路標(biāo)F劃分為第二層級路標(biāo)。
[0113] 在其他實(shí)施例中，采用同樣的方法劃分第四層級與第四層級以上的路標(biāo)，即機(jī)器 人在任意位置均不能同時(shí)識別到路標(biāo)與起始路標(biāo)、或第一層級路標(biāo)、或第二層級路標(biāo)，而在 某一位置能夠同時(shí)識別到路標(biāo)與第三層級路標(biāo)時(shí)，將路標(biāo)劃分為第四層級路標(biāo);第四層級 以上路標(biāo)的層級劃分也采用同樣的方法。
[0114] 在本實(shí)施例中，參考圖2,采用的是多叉樹結(jié)構(gòu)進(jìn)行地圖的創(chuàng)建，所述起始路標(biāo)E為 第一層級路標(biāo)，1張地圖只有一個(gè)第一層級路標(biāo)，所述第二層級一路標(biāo)D劃分為第二層級路 標(biāo)，層級數(shù)比起始路標(biāo)E多一層，所述第三層級一路標(biāo)A、第三層級二路標(biāo)B劃分為第三層級 路標(biāo)。
[0115] 在其他實(shí)施例中，可以采用其他分級或分類方法對路標(biāo)進(jìn)行劃分，比如，也可以將 起始路標(biāo)E與第二層級一路標(biāo)D均確定為第一層級路標(biāo)，其余路標(biāo)再劃分為第二層級或第二 以上層級路標(biāo)。
[0116] 在其他實(shí)施例中，所述機(jī)器人可以從起始路標(biāo)E開始沿任意方向行走，比如沿第二 層級二路標(biāo)F方向開始行走。
[0117] 在本實(shí)施例中，機(jī)器人進(jìn)行相機(jī)標(biāo)定的時(shí)刻是機(jī)器人能夠穩(wěn)定并且準(zhǔn)確的反映出 識別的路標(biāo)時(shí)，一般設(shè)定為機(jī)器人能夠在持續(xù)在3秒的時(shí)間內(nèi)穩(wěn)定并且準(zhǔn)確的識別路標(biāo);并 且，本實(shí)施例中還要求機(jī)器人處于兩個(gè)路標(biāo)距離的二分之一處。
[0118] 在其他實(shí)施例中，時(shí)間可以根據(jù)需要進(jìn)行調(diào)整為4秒或5秒或6秒及以上，只要滿足 需要且能夠?qū)崿F(xiàn)即可；并且只要滿足此條件即可，不需要機(jī)器人一定要處于兩個(gè)路標(biāo)距離 的二分之一處。
[0119] 在本實(shí)施例中，對所述路標(biāo)進(jìn)行相機(jī)標(biāo)定，主要是為了能夠?qū)⑺雎窐?biāo)的坐標(biāo)系 轉(zhuǎn)化為所述照相機(jī)坐標(biāo)系，每一個(gè)路標(biāo)的坐標(biāo)系都已經(jīng)轉(zhuǎn)換到了所述相機(jī)坐標(biāo)系，便于步 驟3中的兩兩路標(biāo)之間的關(guān)系運(yùn)算，計(jì)算簡單。
[0120] 在本實(shí)施例中，所述相機(jī)標(biāo)定方法采用張正友基于針孔相機(jī)模型特定的平面標(biāo)定 方法。
[0121] 在其他實(shí)施例中，根據(jù)需要可以采用其他相機(jī)標(biāo)定方法，只要能夠?qū)⑺雎窐?biāo)坐 標(biāo)系與所述相機(jī)坐標(biāo)系進(jìn)行轉(zhuǎn)換即可，這樣，每一個(gè)路標(biāo)的坐標(biāo)系都已經(jīng)轉(zhuǎn)換到了所述相 機(jī)坐標(biāo)系，便于步驟3中的兩兩路標(biāo)之間的關(guān)系運(yùn)算，計(jì)算簡單。
[0122] 步驟3,計(jì)算所有相鄰層級的具有直屬關(guān)系的兩個(gè)路標(biāo)坐標(biāo)系的轉(zhuǎn)換關(guān)系矩陣。
[0123] 本實(shí)施例中，計(jì)算的是多叉樹結(jié)構(gòu)中所有具有直屬關(guān)系的相鄰層級的兩個(gè)路標(biāo)坐 標(biāo)系的轉(zhuǎn)換關(guān)系矩陣，比如起始路標(biāo)E與第一層級一路標(biāo)D之間的轉(zhuǎn)換關(guān)系矩陣，比如起始 路標(biāo)E與第二層級二路標(biāo)F之間的轉(zhuǎn)換關(guān)系矩陣。
[0124] 本實(shí)施例中，設(shè)0ΧΥ、0 ' X ' Y '是兩個(gè)相鄰路標(biāo)的坐標(biāo)系，0 '在0ΧΥ中的坐標(biāo)為（tx， ty)，由X軸到X'軸的角度為Θ，坐標(biāo)轉(zhuǎn)換公式是:x = x'cos0-y'sin0+tx，y = x'sin0+y'cos0+ ty〇
[0125] SP，轉(zhuǎn)換矩陣如下所示：
[0126]
[0127] 為了更進(jìn)一步簡化公式，便于坐標(biāo)系之間的轉(zhuǎn)換計(jì)算，引入齊次坐標(biāo)形式，則得到 以下公式：
[0128]
[0129 ]由此可得到相應(yīng)的路標(biāo)坐標(biāo)系之間的可逆轉(zhuǎn)換矩陣A:
[0130]
[0131] 由于相鄰層級的具有直屬關(guān)系的兩個(gè)路標(biāo)的坐標(biāo)系均已轉(zhuǎn)換為相機(jī)坐標(biāo)系，兩個(gè) 路標(biāo)坐標(biāo)系原點(diǎn)在所述相機(jī)坐標(biāo)系下的坐標(biāo)計(jì)算簡單，兩個(gè)路標(biāo)坐標(biāo)系的旋轉(zhuǎn)角Θ也計(jì)算 簡單，因而計(jì)算出轉(zhuǎn)換矩陣中的參數(shù)，則能夠確定相鄰層級的具有直屬關(guān)系的坐標(biāo)轉(zhuǎn)換關(guān) 系，并將轉(zhuǎn)換關(guān)系矩陣保存。
[0132] 所述當(dāng)劃分層級的路標(biāo)數(shù)量與設(shè)置的路標(biāo)數(shù)量相同時(shí)，已完成以上步驟1、步驟2、 步驟3后，所述機(jī)器人視覺導(dǎo)航地圖的創(chuàng)建完成，在本實(shí)施例中，得到的是一個(gè)機(jī)器人存儲 著的，以路標(biāo)為節(jié)點(diǎn)的多叉樹結(jié)構(gòu)，相鄰層級的路標(biāo)坐標(biāo)系的轉(zhuǎn)換關(guān)系矩陣也存在機(jī)器人 中作為多叉樹結(jié)構(gòu)的相鄰層級的具有直屬關(guān)系的兩個(gè)路標(biāo)之間的鏈，這樣，導(dǎo)航時(shí)，機(jī)器人 可以將相鄰層級的具有直屬關(guān)系的路標(biāo)之間的實(shí)際距離計(jì)算出來，以完成導(dǎo)航。
[0133] 所述機(jī)器人導(dǎo)航路徑規(guī)劃方法就基于上述的以多叉樹結(jié)構(gòu)創(chuàng)建的機(jī)器人視覺導(dǎo) 航系統(tǒng)。
[0134] 本技術(shù)方案的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，采用多叉樹結(jié)構(gòu)將路標(biāo)劃分層級， 將路標(biāo)層級與相鄰層級直屬關(guān)系的路標(biāo)之間的位置關(guān)系保存為路標(biāo)索引，使得路徑規(guī)劃中 算法簡單、效率高，規(guī)劃的路徑唯一，很適合在嵌入式設(shè)備上運(yùn)行。
[0135] 以上所述，僅為本發(fā)明的較佳實(shí)施例而已，并非用于限定本發(fā)明的保護(hù)范圍，凡在 本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù) 范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，包括： 步驟100:設(shè)定一個(gè)起始路標(biāo)，W該起始路標(biāo)為基礎(chǔ)將所述機(jī)器人所處場景內(nèi)設(shè)置的所 有路標(biāo)進(jìn)行層級劃分，并且將相鄰層級直屬關(guān)系路標(biāo)之間的位置關(guān)系進(jìn)行保存，作為路標(biāo) 索引； 步驟200:輸入所述機(jī)器人的當(dāng)前路標(biāo)與目標(biāo)路標(biāo)； 步驟300:捜索所述當(dāng)前路標(biāo)與所述目標(biāo)路標(biāo)的共同關(guān)系路標(biāo)； 步驟400:確定所述路徑為當(dāng)前路標(biāo)經(jīng)所述共同關(guān)系路標(biāo)到目標(biāo)路標(biāo)的路徑，路徑規(guī)劃 完成。2. 根據(jù)權(quán)利要求1所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述步驟100包 括： 步驟110:設(shè)置路標(biāo)的數(shù)量，并設(shè)定一個(gè)起始路標(biāo)，所述起始路標(biāo)為第一層級路標(biāo)； 步驟120:所述機(jī)器人從起始路標(biāo)開始識別路標(biāo)，對路標(biāo)進(jìn)行相機(jī)標(biāo)定，并對每個(gè)路標(biāo) 進(jìn)行層級劃分。 步驟130:計(jì)算所有相鄰層級的具有直屬關(guān)系的兩個(gè)路標(biāo)坐標(biāo)系的轉(zhuǎn)換關(guān)系矩陣。3. 根據(jù)權(quán)利要求1所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述步驟100中 W路標(biāo)為節(jié)點(diǎn)，采用多叉樹結(jié)構(gòu)對路標(biāo)進(jìn)行層級劃分，所述起始路標(biāo)為第一層級路標(biāo)。4. 根據(jù)權(quán)利要求1所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述機(jī)器人從已 劃分層級的路標(biāo)出發(fā)，能夠同時(shí)識別到所述已劃分層級的路標(biāo)與其他未劃分層級路標(biāo)時(shí)， 將所述其他未劃分層級路標(biāo)的層級數(shù)劃分為比所述已劃分層級的路標(biāo)大一個(gè)層級。5. 根據(jù)權(quán)利要求1所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述直屬關(guān)系是 指，如果兩個(gè)路標(biāo)之間的層級相差一個(gè)層級，且層級較大的路標(biāo)的層級是根據(jù)層級較小的 路標(biāo)的層級而直接確定的，則運(yùn)兩個(gè)路標(biāo)具有直屬關(guān)系。6. 根據(jù)權(quán)利要求2所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述相機(jī)標(biāo)定的 時(shí)刻是機(jī)器人能夠穩(wěn)定并且準(zhǔn)確的反映出識別的路標(biāo)的時(shí)刻。7. 根據(jù)權(quán)利要求2所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述步驟130中， 設(shè)OXY、0 'X ' Y '是兩個(gè)相鄰路標(biāo)的坐標(biāo)系，0 '在OXY中的坐標(biāo)為(tx，ty)，由X軸到X '軸的角度 為9，則所述轉(zhuǎn)換關(guān)系矩陣A是一個(gè)可逆矩陣，即：8. 根據(jù)權(quán)利要求1所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述路標(biāo)包括： 第一坐標(biāo)標(biāo)記，用于確定路標(biāo)坐標(biāo)系的X軸. 第二坐標(biāo)標(biāo)記，用于確定路標(biāo)坐標(biāo)系的Y軸. 區(qū)域標(biāo)記，用于確定路標(biāo)所在的空間位置，所述區(qū)域標(biāo)記包括多個(gè)等距離的信息塊，所 述尺寸是所述信息塊之間的距離或者信息塊的長度。9. 根據(jù)權(quán)利要求8所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述機(jī)器人使用 所述尺寸將實(shí)際空間與相機(jī)坐標(biāo)系下的尺寸進(jìn)行轉(zhuǎn)換。10. 根據(jù)權(quán)利要求1所述的一種機(jī)器人導(dǎo)航路徑規(guī)劃方法，其特征在于，所述步驟300包 括： 步驟310:確定當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的層級； 步驟320:比較當(dāng)前路標(biāo)層級與路標(biāo)目標(biāo)層級，若相同執(zhí)行步驟330，否則執(zhí)行步驟340; 步驟330:分別逐層級捜索當(dāng)前路標(biāo)與目標(biāo)路標(biāo)的上層級路標(biāo)，直到捜索到同一個(gè)路 標(biāo)，所述捜索到的同一個(gè)路標(biāo)為共同關(guān)系路標(biāo)，結(jié)束捜索； 步驟340:比較當(dāng)前路標(biāo)層級與目標(biāo)路標(biāo)層級的大小;若當(dāng)前路標(biāo)層級大于目標(biāo)路標(biāo)層 級，執(zhí)行步驟350，否則執(zhí)行步驟360; 步驟350:捜索當(dāng)前路標(biāo)的上層級路標(biāo)，直到捜索到與目標(biāo)路標(biāo)同一層級的路標(biāo)，如果 捜索到的該同一層級路標(biāo)是目標(biāo)路標(biāo)，則所述目標(biāo)路標(biāo)為共同關(guān)系路標(biāo)，結(jié)束捜索;否則逐 層捜索該同一層級路標(biāo)和目標(biāo)路標(biāo)的上層級路標(biāo)，直到捜索到同一個(gè)路標(biāo)作為共同關(guān)系路 標(biāo)，結(jié)束捜索； 步驟360:捜索目標(biāo)路標(biāo)的上層級路標(biāo)，直到捜索到與當(dāng)前路標(biāo)同一層級的路標(biāo)，如果 捜索到的該同一層級路標(biāo)是當(dāng)前路標(biāo)，則所述當(dāng)前路標(biāo)為共同關(guān)系路標(biāo)，結(jié)束捜索;否則逐 層捜索該同一層級路標(biāo)和當(dāng)前路標(biāo)的上層級路標(biāo)，直到捜索到同一個(gè)路標(biāo)作為共同關(guān)系路 標(biāo)，結(jié)束捜索。
【文檔編號】G01C21/34GK105974928SQ201610614320
【公開日】2016年9月28日
【申請日】2016年7月29日
【發(fā)明人】喬徽, 張騰飛
【申請人】哈爾濱工大服務(wù)機(jī)器人有限公司