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

【發明內容】

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