一種基于時(shí)間窗的多機(jī)器人路徑規(guī)劃方法
【專利說明】
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及機(jī)器人設(shè)計(jì)領(lǐng)域，尤其涉及一種基于時(shí)間窗的多機(jī)器人路徑規(guī)劃方法。
【【背景技術(shù)】】
[0002]目前，隨著新技術(shù)的快速發(fā)展，機(jī)器人的應(yīng)用也越來越廣泛，并且同一個(gè)系統(tǒng)中的機(jī)器人數(shù)量也在不斷增加，多機(jī)器人調(diào)度問題日趨重要。由于多個(gè)機(jī)器人在同一個(gè)系統(tǒng)內(nèi)執(zhí)行任務(wù)，它們有可能在行走路徑上產(chǎn)生沖突。沖突有三種基本類型，即路口沖突、趕超沖突和相向沖突。其中路口沖突是指兩臺(tái)機(jī)器人同時(shí)到達(dá)路口，趕超沖突是指兩臺(tái)機(jī)器人同向行駛，后面機(jī)器人速度快，會(huì)在行駛過程中產(chǎn)生追尾事件，相向沖突是指兩臺(tái)機(jī)器人相向行駛的碰撞。目前解決沖突的策略主要有等待策略、更改路徑、速度調(diào)節(jié)等。其中更改路徑策略就涉及對(duì)機(jī)器人的路徑規(guī)劃。
[0003]路徑規(guī)劃指在具有一定障礙物的環(huán)境中(此環(huán)境可以通過機(jī)器人與環(huán)境的交互感知)，給定一個(gè)起始點(diǎn)和目標(biāo)點(diǎn)，參照一定的準(zhǔn)則，如最短路徑因素、最少花費(fèi)時(shí)間、最少拐彎次數(shù)，最少花費(fèi)費(fèi)用等等，在所處環(huán)境中搜索出一條最優(yōu)的無碰撞路徑。
[0004]而多機(jī)器人系統(tǒng)路徑規(guī)劃是建立在單個(gè)機(jī)器人基礎(chǔ)之上，在保證單個(gè)機(jī)器人完成從起始點(diǎn)到終點(diǎn)的路徑規(guī)劃，同時(shí)避免機(jī)器人和周圍環(huán)境障礙物以及其它機(jī)器人發(fā)生沖突碰撞，完成多機(jī)器人之間的協(xié)調(diào)，避免碰撞和死鎖等情況發(fā)生，使得多臺(tái)機(jī)器人能共同順利的完成指定任務(wù)。
[0005]目前，對(duì)于多機(jī)器人系統(tǒng)路徑規(guī)劃已經(jīng)提出了一些算法，典型的算法通過將單臺(tái)機(jī)器人的算法和時(shí)間窗原理相結(jié)合來規(guī)劃路徑。例如將Dijkstra算法和時(shí)間窗原理相結(jié)合，順序規(guī)劃各個(gè)機(jī)器人的路徑，在已規(guī)劃機(jī)器人路徑的基礎(chǔ)上，運(yùn)用基于Dijkstra的算法繼續(xù)規(guī)劃下一臺(tái)機(jī)器人，實(shí)現(xiàn)機(jī)器人的無碰撞路徑規(guī)劃。
[0006]但是，現(xiàn)有算法很多是基于精確的時(shí)間窗，或應(yīng)用于特定的場景來進(jìn)行，通用性較差;對(duì)沖突的判斷比較繁瑣，并且沖突的解決方案多需要等待，停車次數(shù)多，用戶體驗(yàn)較差。
【
【發(fā)明內(nèi)容】
】
[0007]基于現(xiàn)有技術(shù)的上述問題，本發(fā)明提出了一種基于時(shí)間窗的多機(jī)器人路徑規(guī)劃方法。該方法按照機(jī)器人優(yōu)先級(jí)順序，依次進(jìn)行路徑規(guī)劃，對(duì)于待規(guī)劃機(jī)器人，搜索所有可行路徑作為搜索路徑集，從中找到一條能在已有空閑時(shí)間窗內(nèi)執(zhí)行完的可行路徑作為其路徑，最后進(jìn)行機(jī)器人死鎖判斷。
[0008]其中，死鎖判斷通過對(duì)每一個(gè)非正常執(zhí)行任務(wù)狀態(tài)的機(jī)器人，根據(jù)前方的阻塞/停止機(jī)器人的位置和方向，增加一條有向邊，如果最后圖中出現(xiàn)環(huán)，則為死鎖。
[0009]其中，對(duì)于新加入的系統(tǒng)的機(jī)器人，執(zhí)行預(yù)先規(guī)劃方法，計(jì)算各條可行路徑的執(zhí)行時(shí)間，將執(zhí)行時(shí)間最短的可行路徑作為新機(jī)器人的路徑。
[0010]本發(fā)明采用的技術(shù)方案為:[0011 ] 一種基于時(shí)間窗的多機(jī)器人路徑規(guī)劃方法，包括如下步驟:
[0012]I)在多機(jī)器人的系統(tǒng)地圖中設(shè)定節(jié)點(diǎn)和短路徑，并保存；
[0013]2)初始化，清除節(jié)點(diǎn)和路徑的時(shí)間窗，清除所有機(jī)器人的已規(guī)劃標(biāo)記、阻塞標(biāo)記和循環(huán)標(biāo)記；
[0014]3)獲取各個(gè)機(jī)器人的速度、位置和狀態(tài)信息，所述狀態(tài)包括待規(guī)劃狀態(tài)和無需規(guī)劃狀態(tài);使用預(yù)定路徑搜索算法在預(yù)設(shè)條件下為各個(gè)機(jī)器人搜索若干路徑，搜索到的路徑作為相應(yīng)機(jī)器人的可選路徑；
[0015]4)清除所有機(jī)器人的時(shí)間窗、已規(guī)劃標(biāo)記和循環(huán)標(biāo)記；
[0016]5)根據(jù)所有具有無需規(guī)劃狀態(tài)的機(jī)器人和具有阻塞標(biāo)記的機(jī)器人的位置和狀態(tài)更新時(shí)間窗；
[0017]6)選擇一個(gè)無阻塞標(biāo)記且無已規(guī)劃標(biāo)記的待規(guī)劃狀態(tài)的機(jī)器人，計(jì)算該機(jī)器人的每一條可選路徑的空閑時(shí)間窗，包括該可選路徑上各個(gè)節(jié)點(diǎn)、短路徑的空閑時(shí)間窗；
[0018]7)判斷步驟6所選擇的機(jī)器人是否具有可選路徑能在對(duì)應(yīng)的空閑時(shí)間窗內(nèi)完成任務(wù)。如果有，則選擇其中耗時(shí)最短的可選路徑作為該機(jī)器人的行進(jìn)路徑，并對(duì)該機(jī)器人打上已規(guī)劃標(biāo)記，同時(shí)更新所有的點(diǎn)時(shí)間窗和邊時(shí)間窗，如果沒有，則放寬預(yù)設(shè)條件，使用所述預(yù)定路徑搜索算法為該機(jī)器人搜索其他路徑，如果沒能搜索到新的可選路徑，則對(duì)該機(jī)器人打上阻塞標(biāo)記和循環(huán)標(biāo)記，如果搜索到新的可選路徑，則將新路徑加入該機(jī)器人的可選路徑并返回步驟6;
[0019]8)判斷所有無阻塞標(biāo)記的待規(guī)劃狀態(tài)的機(jī)器人中，是否還有未被打上已規(guī)劃標(biāo)記的機(jī)器人，如果有則返回步驟6，如果沒有則繼續(xù)執(zhí)行以下步驟；
[0020]9)判斷是否還有打上循環(huán)標(biāo)記的機(jī)器人，如果有則返回步驟4，如果沒有則繼續(xù)執(zhí)行以下步驟；
[0021]10)檢測(cè)目前系統(tǒng)中是否存在機(jī)器人死鎖，如果有，則報(bào)警。
[0022]優(yōu)選的，每隔預(yù)設(shè)時(shí)間重復(fù)執(zhí)行步驟2至步驟10。
[0023]優(yōu)選的，所述預(yù)設(shè)時(shí)間為100ms。
[0024]優(yōu)選的，所述待規(guī)劃狀態(tài)包括任務(wù)待分配狀態(tài)、任務(wù)運(yùn)行狀態(tài)和任務(wù)阻塞狀態(tài);所述無需規(guī)劃狀態(tài)包括機(jī)器人故障狀態(tài)和任務(wù)結(jié)束狀態(tài)。
[0025]優(yōu)選的，其中步驟5更新時(shí)間窗的方法為對(duì)無需規(guī)劃狀態(tài)機(jī)器人和具有阻塞標(biāo)記機(jī)器人所在的短路徑或節(jié)點(diǎn)進(jìn)行封鎖，以表明該短路徑或節(jié)點(diǎn)被占用。
[0026]優(yōu)選的，其中步驟6中按照機(jī)器人優(yōu)先級(jí)從高到低選擇。
[0027]優(yōu)選的，其中步驟3和7中所述的預(yù)定路徑搜索算法為Di jkstra算法、A*算法或者BFS算法。
[0028]優(yōu)選的，其中步驟3中所述的預(yù)設(shè)條件為預(yù)先設(shè)定的路徑搜索的上限時(shí)間或者搜索到的路徑的上限數(shù)量，若路徑搜索的時(shí)間達(dá)到上限時(shí)間或者搜索到的路徑數(shù)量達(dá)到上限數(shù)量，則結(jié)束本次搜索。
[0029]優(yōu)選的，其中步驟7中所述的放寬預(yù)設(shè)條件是指增加路徑搜索的上限時(shí)間，或者增加搜索到的路徑的上限數(shù)量。
[0030]優(yōu)選的，所述步驟10中檢測(cè)機(jī)器人死鎖的具體步驟如下:
[0031]10.1)統(tǒng)計(jì)所有具有阻塞標(biāo)記的機(jī)器人的關(guān)鍵路徑和/或關(guān)鍵點(diǎn);所述關(guān)鍵路徑是指機(jī)器人所有可選路徑中都經(jīng)過的路徑，所述關(guān)鍵點(diǎn)是指機(jī)器人所有可選路徑中都經(jīng)過的節(jié)點(diǎn)；
[0032]10.2)對(duì)于每一個(gè)具有關(guān)鍵路徑和/或關(guān)鍵點(diǎn)的機(jī)器人，判斷在其關(guān)鍵路徑和/或關(guān)鍵點(diǎn)上是否有其他具有阻塞標(biāo)記的機(jī)器人，如果有，則建立一有向邊，該有向邊從該具有關(guān)鍵路徑和/或關(guān)鍵點(diǎn)的機(jī)器人指向該其他具有阻塞標(biāo)記的機(jī)器人；
[0033]10.3)判斷所有有向邊是否有形成環(huán)，如果有，則為死鎖;如果沒有，則沒有死鎖。
[0034]優(yōu)選的，對(duì)于死鎖中無解的機(jī)器人，記錄其每條可行路徑上的沖突短路徑和沖突節(jié)點(diǎn)，并記錄沖突依賴關(guān)系，對(duì)被依賴最多的機(jī)器人給出預(yù)警。
[0035]本發(fā)明的有益效果為:
[0036]I)可以預(yù)先估算整條路的時(shí)間窗，如果機(jī)器人可以按照預(yù)定的時(shí)間窗完成任務(wù)，則中途不會(huì)出現(xiàn)死鎖情況；
[0037]2)和遺傳算法，蟻群算法，粒子群算法這些需要自學(xué)習(xí)/隨機(jī)量的調(diào)度算法等比較，時(shí)間窗算法不需要多次迭代，只是在有限的路徑集上計(jì)算可能性，相對(duì)計(jì)算量比較小，而且不會(huì)陷入局部最小，更適合真實(shí)的調(diào)度場景應(yīng)用。
[0038]3)可更合理并且更容易的檢測(cè)沖突。
[0039]4)對(duì)存在沖突的機(jī)器人進(jìn)行死鎖檢測(cè)，并給出預(yù)警。。
【【附圖說明】】
[0040]此處所說明的附圖是用來提供對(duì)本發(fā)明的進(jìn)一步理解，構(gòu)成本申請(qǐng)的一部分，但并不構(gòu)成對(duì)本發(fā)明的不當(dāng)限定，在附圖中:
[0041 ]圖1是本發(fā)明的動(dòng)態(tài)規(guī)劃方法的流程圖。
【【具體實(shí)施方式】】
[0042]下面將結(jié)合附圖以及具體實(shí)施例來詳細(xì)說明本發(fā)明，其中的示意性實(shí)施例以及說明僅用來解釋本發(fā)明，但并不作為對(duì)本發(fā)明的限定。
[0043]首先說明本發(fā)明使用的一些術(shù)語:
[0044]時(shí)間窗:時(shí)間窗指的是機(jī)器人占用某個(gè)資源的時(shí)間段，即機(jī)器人從該時(shí)間段的起始時(shí)間到終止時(shí)間占用該資源。當(dāng)不同機(jī)器人占用資源的時(shí)間窗沒有重疊時(shí)，則表明沒有發(fā)生碰撞和沖