基于三維時(shí)空地圖和運(yùn)動(dòng)分解的多機(jī)器人路徑規(guī)劃算法

屈立成，呂 嬌，趙 明，王海飛，屈藝華

（長(zhǎng)安大學(xué)信息工程學(xué)院，西安 710064）

（?通信作者郵箱qlc@chd.edu.cn）

0 引言

利用機(jī)器人完成重復(fù)繁重的搬運(yùn)任務(wù)是一個(gè)高效快捷的物品搬運(yùn)方法，如自動(dòng)物流分揀系統(tǒng)、智能倉儲(chǔ)系統(tǒng)、智能泊車系統(tǒng)等。在一個(gè)機(jī)器人系統(tǒng)中，多個(gè)移動(dòng)機(jī)器人同時(shí)協(xié)同完成給定的任務(wù)集可以有效減少系統(tǒng)的總耗時(shí)，但不同的機(jī)器人的運(yùn)動(dòng)路徑可能存在沖突、死鎖等問題，如果不能有效地解決上述問題，多機(jī)器人系統(tǒng)的穩(wěn)定性無法得到保障，不能充分發(fā)揮多機(jī)器人系統(tǒng)的并行運(yùn)行特點(diǎn)，即系統(tǒng)在同一時(shí)刻以最低代價(jià)使得盡可能多的機(jī)器人可以同時(shí)互不影響地獨(dú)立運(yùn)行，可以說多機(jī)器人路徑規(guī)劃已經(jīng)成為當(dāng)前機(jī)器人熱門研究方向之一。

多機(jī)器人路徑規(guī)劃是指每個(gè)機(jī)器人在已知地形圖中根據(jù)給定的目標(biāo)終點(diǎn)及當(dāng)前出發(fā)節(jié)點(diǎn)規(guī)劃出一條無碰撞的最優(yōu)路徑，并且要求系統(tǒng)總代價(jià)最小。多機(jī)器人路徑規(guī)劃是一個(gè)NP-Hard（Non-deterministic Polynomial-Hard）問題［1-2］，當(dāng)前并無很好的解決方案。近些年來不少學(xué)者將人工勢(shì)場(chǎng)［3-6］、蟻群算法［7-9］、A*算法［10-11］、遺傳算法［12-14］、神經(jīng)網(wǎng)絡(luò)［14-15］等理論運(yùn)用于多機(jī)器人路徑規(guī)劃系統(tǒng)中。然而，這些算法大多得到的是次優(yōu)解而不是最優(yōu)解，整個(gè)多機(jī)器人系統(tǒng)的運(yùn)行效率較低。

早期對(duì)多機(jī)器人的研究多采用集中式路徑規(guī)劃策略，其中：王佳溶等［16］提出了基于改進(jìn)的兩階段控制理論的AGV（Automated Guided Vehicle）控制調(diào)度策略，利用遺傳算法生成離線路徑庫，采用速度調(diào)節(jié)對(duì)其進(jìn)行在線動(dòng)態(tài)路徑規(guī)劃，若生成的路徑不滿足條件，則使用帶約束多目標(biāo)遺傳算法在線計(jì)算路徑，增加了AGV 調(diào)度系統(tǒng)的靈活性。Luna等［17］提出了一種高效完整的集中式多機(jī)器人路徑規(guī)劃方法，采用了“推”和“交換”兩種原語，不僅提高了解的質(zhì)量，同時(shí)也減少了多機(jī)器人系統(tǒng)的計(jì)算時(shí)間。不同于離線路徑生成階段和在線路徑規(guī)劃的規(guī)劃策略，夏清松等［18］提出了基于路徑規(guī)劃層和碰撞避免層的多機(jī)器人路徑規(guī)劃策略：在第一層只考慮單一機(jī)器人的路徑規(guī)劃，忽略其他機(jī)器人的影響，其規(guī)劃的路徑可能會(huì)與其他機(jī)器人產(chǎn)生嚴(yán)重碰撞；在第二層采用碰撞避免規(guī)則解決多機(jī)器人的局部碰撞，其避碰規(guī)則復(fù)雜，且需要人為提前制定，無法根據(jù)多機(jī)器人系統(tǒng)當(dāng)前特點(diǎn)改變。

一些研究者另辟蹊徑，采用分布式路徑規(guī)劃模型，將路徑規(guī)劃的計(jì)算任務(wù)放到機(jī)器人自身的處理器上，機(jī)器人之間共享地圖信息，解決了傳統(tǒng)多機(jī)器人系統(tǒng)路徑計(jì)算嚴(yán)重依賴中央路徑規(guī)劃模塊的問題。這一類模型必須要解決的主要問題為機(jī)器人必須實(shí)時(shí)接收和發(fā)送地圖數(shù)據(jù)，這些數(shù)據(jù)量一般較大。為解決上述問題，Matoui 等［19］提出了一種基于鄰域人工勢(shì)場(chǎng)的多機(jī)器人分布式路徑規(guī)劃方法，利用分布式體系結(jié)構(gòu)規(guī)劃移動(dòng)機(jī)器人軌跡，采用改進(jìn)的人工市場(chǎng)方法使多機(jī)器人系統(tǒng)獲得良好的軌跡規(guī)劃，提高了多機(jī)器人系統(tǒng)的魯棒性；結(jié)合分布式路徑規(guī)劃和集中式協(xié)調(diào)思想，曹其新等［20］提出了基于保留分區(qū)的分布式多機(jī)器人路徑規(guī)劃策略，采用分布式規(guī)劃架構(gòu)，機(jī)器人與中央模塊通過保留分區(qū)協(xié)調(diào)路徑碰撞可能情況，每個(gè)機(jī)器人都向中央模塊發(fā)送分區(qū)請(qǐng)求，中央模塊通過計(jì)算后將保留分區(qū)下發(fā)至每個(gè)機(jī)器人，機(jī)器人按照接收的保留分區(qū)信息運(yùn)動(dòng)，這種策略沒有有效考慮多機(jī)器人系統(tǒng)可能產(chǎn)生的不確定因素，系統(tǒng)只能在每個(gè)機(jī)器人都按照接收的保留分區(qū)運(yùn)動(dòng)后，才能繼續(xù)下一步計(jì)算。由于自動(dòng)物流分揀系統(tǒng)以及智能倉儲(chǔ)系統(tǒng)中，任務(wù)的產(chǎn)生是隨機(jī)且連續(xù)的，多機(jī)器人系統(tǒng)必須能夠持續(xù)完成任務(wù)，而對(duì)于集中式規(guī)劃算法側(cè)重的是對(duì)任務(wù)集的一次全局規(guī)劃，不能靈活地處理隨機(jī)連續(xù)產(chǎn)生的任務(wù)。多機(jī)器人系統(tǒng)采用分布式路徑規(guī)劃策略后，單個(gè)機(jī)器人作為計(jì)算的最小單元，系統(tǒng)可以高效地完成給定任務(wù)集合，持續(xù)完成產(chǎn)生的任務(wù)，因此本文采用分布式多機(jī)器人路徑規(guī)劃策略。

本文提出了基于三維時(shí)空地圖和運(yùn)動(dòng)分解的多機(jī)器人路徑規(guī)劃算法。路徑搜索空間在z軸方向只能單向遞增，因?yàn)閦軸代表時(shí)間軸，時(shí)間只能向前，無法倒退，不同于一般意義的三維空間，故這里的三維時(shí)空地圖實(shí)際為帶約束三維時(shí)空。所提算法首先使用柵格法構(gòu)建地圖環(huán)境模型，在其垂直坐標(biāo)軸上拓展為時(shí)間軸，構(gòu)成帶約束三維時(shí)空地圖，其中可以存儲(chǔ)所有機(jī)器人在當(dāng)前時(shí)刻至最大時(shí)間刻度范圍內(nèi)的已規(guī)劃路徑、機(jī)器人在無任務(wù)時(shí)的駐車位置信息，以及地圖中存在的障礙物位置信息。機(jī)器人根據(jù)其接收的任務(wù)，采用帶條件深度優(yōu)先搜索（Depth First Search，DFS）算法優(yōu)先遍歷最有可能是最優(yōu)路徑的路徑集合，可以有效解決隨機(jī)持續(xù)倉儲(chǔ)任務(wù)的問題。仿真實(shí)驗(yàn)結(jié)果表明，本文算法規(guī)劃的路徑為最優(yōu)路徑，且具有無碰撞、魯棒性高、可同時(shí)運(yùn)動(dòng)機(jī)器人數(shù)量多等優(yōu)點(diǎn)。

1 問題描述

多機(jī)器人路徑規(guī)劃是指在一個(gè)地圖環(huán)境已知條件下根據(jù)機(jī)器人集合Robots={r1，r2，…，ri，…，rn}和任務(wù)集合Tasks={t1，t2，…，tj，…，tm}規(guī)劃出系統(tǒng)最優(yōu)無碰撞運(yùn)行路徑集，任務(wù)集合中任務(wù)產(chǎn)生的時(shí)間并無規(guī)律。其中：ri表示第i個(gè)機(jī)器人，tj表示第j個(gè)運(yùn)動(dòng)任務(wù)，tj={start，end}包括任務(wù)的起點(diǎn)start和終點(diǎn)end，n表示機(jī)器人數(shù)量，m表示任務(wù)數(shù)量。

為了更加直觀地表示二維柵格地圖中各個(gè)空間位置的連通關(guān)系，將可用的空間位置抽象為節(jié)點(diǎn)，并用連線表示節(jié)點(diǎn)間的有效路徑，既便于地圖的直觀展示，又便于采用數(shù)學(xué)方法對(duì)其進(jìn)行分析。在傳統(tǒng)的二維柵格地圖中，機(jī)器人只能在二維坐標(biāo)空間上進(jìn)行區(qū)分，無法表現(xiàn)其在時(shí)間維度上的發(fā)展變化。如圖1（a）中所示，兩個(gè)機(jī)器人的運(yùn)動(dòng)路徑在左側(cè)有重疊部分，無法在二維坐標(biāo)空間內(nèi)觀察這兩個(gè)機(jī)器人運(yùn)動(dòng)路徑的沖突情況。因此，在二維柵格地圖垂直平面上另行增加時(shí)間維度，即z軸，將二維平面提升至三維空間，形成了包含所有時(shí)刻機(jī)器人運(yùn)動(dòng)路徑及位置信息的三維時(shí)空柵格地圖，統(tǒng)一了機(jī)器人位置、障礙物和運(yùn)動(dòng)路徑等不同概念的表達(dá)方式，其維度提升示意圖如圖1（b）所示。

圖1 空間維度提升示意圖Fig.1 Schematic diagram of space dimension increasement

圖1（b）中，不同標(biāo)記標(biāo)注的線段表示不同的機(jī)器人將要執(zhí)行的路徑，三維時(shí)空地圖中可以存儲(chǔ)所有機(jī)器人在當(dāng)前時(shí)刻至最大時(shí)間刻度范圍內(nèi)的已規(guī)劃的路徑、機(jī)器人在無任務(wù)時(shí)的駐車位置信息，以及地圖中存在的障礙物位置信息。

在平面z=0中，路徑與平面的交點(diǎn)表示當(dāng)前時(shí)刻各機(jī)器人所處的二維位置，而在平面z=t中，路徑與平面的交點(diǎn)表示后續(xù)t時(shí)刻各機(jī)器人所處的位置?？梢钥吹剑S時(shí)空地圖可以清晰地表示不同機(jī)器人的運(yùn)動(dòng)路徑，并且可以在時(shí)間維度上對(duì)在二維平面中的不同路徑加以區(qū)分。

本文采用分布式和集中式相結(jié)合的混合式的多機(jī)器人路徑規(guī)劃策略，將集中式里的中央控制模塊的大量路徑計(jì)算工作分散于每個(gè)機(jī)器人自身的處理器上，中央模塊僅負(fù)責(zé)相對(duì)簡(jiǎn)單的地圖更新和任務(wù)分配工作。路徑規(guī)劃模型由n個(gè)機(jī)器人模塊、1 個(gè)中央通信模塊以及它們之間的通信鏈路組成，其應(yīng)用概念如圖2所示。

圖2 應(yīng)用概念圖Fig.2 Application concept map

2 多機(jī)器人路徑規(guī)劃模型

可以考慮將單個(gè)機(jī)器人執(zhí)行任務(wù)的總耗時(shí)分解為路徑運(yùn)動(dòng)時(shí)間、原地停留時(shí)間和轉(zhuǎn)向時(shí)間，從而可以將這些不同時(shí)間按照一定的規(guī)律組合，形成路徑集，而多機(jī)器人路徑規(guī)劃問題則轉(zhuǎn)化為在這些集合中尋找最優(yōu)路徑問題。則機(jī)器人ri根據(jù)給定的路徑從起點(diǎn)運(yùn)動(dòng)至終點(diǎn)的時(shí)間表達(dá)式如下：

其中：leni是機(jī)器人ri從起點(diǎn)至終點(diǎn)所行駛的路徑長(zhǎng)度；αi是機(jī)器人ri完成一次原地停留所需的單位長(zhǎng)度；pstopi是機(jī)器人ri行駛過程中只包含原地停留的次數(shù)；βi是機(jī)器人ri完成一次轉(zhuǎn)向所需的單位長(zhǎng)度；pdivi是機(jī)器人ri行駛過程中只包含轉(zhuǎn)向的次數(shù)；γi是機(jī)器人ri完成一次既是原地停留也是轉(zhuǎn)向（以下簡(jiǎn)稱混合動(dòng)作）所需的單位長(zhǎng)度；mixi是機(jī)器人ri行駛過程中混合動(dòng)作的次數(shù)；v是機(jī)器人ri穩(wěn)定運(yùn)動(dòng)速度；D是機(jī)器人ri的轉(zhuǎn)向總次數(shù)；是機(jī)器人ri第j次轉(zhuǎn)向所耗費(fèi)的時(shí)間；S是機(jī)器人ri的原地停止總次數(shù)；是機(jī)器人ri第j次原地停止所耗費(fèi)的時(shí)間；M是機(jī)器人ri的混合動(dòng)作總次數(shù)是機(jī)器人ri第j次混合動(dòng)作所耗費(fèi)的時(shí)間。

機(jī)器人ri從接受任務(wù)tj到完成任務(wù)tj所耗費(fèi)的時(shí)間Totali如式（2）所示。

其中：map是二維柵格地圖垂直加上時(shí)間軸形成的三維時(shí)空?qǐng)D；f(map，ri，tj)是機(jī)器人ri從輸入任務(wù)至其計(jì)算單元計(jì)算出路徑機(jī)器人ri本身所消耗的時(shí)間。由于多機(jī)器路徑規(guī)劃問題本身就是NP-Hard 問題，無法在多項(xiàng)式時(shí)間內(nèi)計(jì)算出最終結(jié)果，只能在多項(xiàng)式時(shí)間內(nèi)驗(yàn)證一個(gè)解的正確性，這也就決定了路徑規(guī)劃的計(jì)算時(shí)間是不精確的，無法準(zhǔn)確計(jì)算出其具體值。

這里f(map，ri，tj)是與在map中ri，tj所形成的矩形框拓展至三維空間內(nèi)的空間復(fù)雜度正相關(guān)的函數(shù)，并不是真正的計(jì)算時(shí)間，而是對(duì)計(jì)算時(shí)間的估計(jì)；wf∈(0，1)是對(duì)計(jì)算時(shí)間估計(jì)的信任權(quán)重。u(map，)表示不確定時(shí)間，ε是對(duì)不確定時(shí)間的調(diào)節(jié)因子，wu∈(0，1)是不確定時(shí)間的信任權(quán)重。不確定時(shí)間表達(dá)式如式（3）所示：

其中：P表示所有路徑的全集；sch表示一個(gè)多機(jī)器人系統(tǒng)的路徑規(guī)劃方案；? 表示所有規(guī)劃方案的全集。在自動(dòng)分揀系統(tǒng)、智能倉儲(chǔ)系統(tǒng)以及其他多機(jī)器人應(yīng)用場(chǎng)景下，任務(wù)的產(chǎn)生具有隨機(jī)性和持續(xù)性等特點(diǎn)，單次批量的集中式規(guī)劃算法并不適合這種場(chǎng)景下的多機(jī)器人路徑規(guī)劃，無法滿足系統(tǒng)對(duì)持續(xù)性任務(wù)的高靈活動(dòng)態(tài)規(guī)劃要求。本文采用柵格法，并使用混合式多機(jī)器人路徑規(guī)劃方法，以達(dá)到每個(gè)任務(wù)的總耗費(fèi)時(shí)間Totalj最小，由于任務(wù)的連續(xù)性、隨機(jī)性和不可預(yù)測(cè)性，只需要滿足單個(gè)任務(wù)的耗時(shí)最小，整個(gè)系統(tǒng)的耗時(shí)必然也是在可預(yù)測(cè)范圍內(nèi)最小的。

3 分布式多機(jī)器人路徑規(guī)劃算法

結(jié)合三維空間地圖和式（2）以及帶條件DFS遍歷算法，本文提出了針對(duì)多機(jī)器人路徑規(guī)劃算法，該算法包括三維空間更新、最優(yōu)路徑選擇、帶條件DFS 遍歷。在實(shí)際地圖環(huán)境下，機(jī)器人數(shù)量和任務(wù)數(shù)量沒有絕對(duì)的對(duì)應(yīng)關(guān)系，一個(gè)任務(wù)的執(zhí)行機(jī)器人是不確定的。在同一時(shí)刻系統(tǒng)不會(huì)產(chǎn)生多于一個(gè)任務(wù)，任務(wù)tj產(chǎn)生后，調(diào)度系統(tǒng)根據(jù)每個(gè)機(jī)器人當(dāng)前時(shí)刻所處位置指派Robots集合中最符合此次任務(wù)的機(jī)器人ri完成該任務(wù)，構(gòu)造一個(gè)臨時(shí)任務(wù)ttemp，其起點(diǎn)和終點(diǎn)分別是機(jī)器人ri的當(dāng)前位置以及任務(wù)tj的起點(diǎn)，選擇最符合的機(jī)器人ri可由式（5）得出。

其中：Timelast表示機(jī)器人完成當(dāng)前任務(wù)的剩余時(shí)間。中央控制器指派機(jī)器人ri完成任務(wù)，機(jī)器人ri必須在中央控制器更新地圖周期之內(nèi)完成路徑計(jì)算，并將路徑信息發(fā)送至中央控制器，中央控制器匯總所有機(jī)器人的實(shí)時(shí)位置及機(jī)器人ri的路徑信息，并將更新后的地圖下發(fā)至每個(gè)機(jī)器人。

3.1 三維空間更新

考慮多機(jī)器人路徑規(guī)劃系統(tǒng)規(guī)模為a×b，時(shí)間軸z軸最大值為C，則三維時(shí)空地圖的存儲(chǔ)空間為a×b×C，實(shí)時(shí)三維地圖坐標(biāo)系統(tǒng)為(x，y，z)。中央控制器按照一定的周期T更新實(shí)時(shí)地圖，該周期即是機(jī)器人移動(dòng)單位長(zhǎng)度所消耗的時(shí)間，調(diào)度系統(tǒng)為機(jī)器人ri指派任務(wù)tj后，必須在T時(shí)間內(nèi)計(jì)算出自己的運(yùn)動(dòng)三維時(shí)空路徑，并將其規(guī)劃的路徑發(fā)送至中央控制模塊，如果未能在規(guī)定時(shí)間內(nèi)計(jì)算完成，則需將該機(jī)器人所執(zhí)行的任務(wù)插入到任務(wù)集合的尾部，等待其他任務(wù)處理完畢，再對(duì)該任務(wù)進(jìn)行處理。

中央控制模塊更新之前需要接收n個(gè)機(jī)器人發(fā)送的實(shí)時(shí)位置信息，若實(shí)際接收到的位置信息數(shù)量n′

不同于傳統(tǒng)二維柵格地圖，只是簡(jiǎn)單地將障礙物所處位置值設(shè)置為1，三維時(shí)空地圖中除了可以表示障礙物之外，還可記錄所有機(jī)器人未來將要執(zhí)行的路徑信息，消除了障礙物和路徑之間的物理性質(zhì)差異，并將其統(tǒng)一于三維時(shí)空地圖中，降低了路徑搜索過程中算法的復(fù)雜度。三維時(shí)空地圖中某一點(diǎn)的值可取{0，1，…，maxR}，其中，障礙物點(diǎn)值為maxR，無障礙物的空白處為0，而其他值的點(diǎn)i?{1，2，…，maxR-1}則表示機(jī)器人ri當(dāng)前或者將來某一時(shí)刻所處的三維空間位置坐標(biāo)，機(jī)器人ri規(guī)劃的路徑上所有節(jié)點(diǎn)值均為i，一次地圖更新示意圖如圖3所示。

圖3 地圖更新示意圖Fig.3 Schematic diagram of map updating

圖3中z軸表示以當(dāng)前時(shí)刻為始的時(shí)間軸，圖3（a）和3（b）分別表示在宏觀時(shí)間為t和t+1的不同三維時(shí)空地圖。圖3中宏觀時(shí)間為t時(shí)刻的平面(x，z)可使用式（6）表示：

其矩陣形式可用式（7）表示：

則三維地圖更新操作可表示為：

每一次地圖更新i按{0，1，…，b-1}順序執(zhí)行式（8），則機(jī)器人路徑地圖更新完畢。考慮障礙物靜態(tài)特點(diǎn)，三維時(shí)空地圖中的障礙物需要覆蓋整個(gè)時(shí)間軸，由于以上操作會(huì)導(dǎo)致三維地圖中planexyc-1=0，planexyc-1表示三維地圖中z=c-1時(shí)的(x，y)平面，即障礙物空缺問題，需要做以下處理：

其中，map(x，y，z)表示三維時(shí)空地圖中位置為(x，y)的值，x、y需要滿足如下條件：x?{0，1，…，a-1}，y?{0，1，…，b-1}，z=c-2，同時(shí)需滿足map(x，y，c-2)?Obstacles，Obstacles表示障礙物集合。

考慮機(jī)器人本身動(dòng)態(tài)特點(diǎn)，機(jī)器人可以看作動(dòng)態(tài)障礙物，在路徑規(guī)劃后應(yīng)該在任務(wù)的終點(diǎn)設(shè)置臨時(shí)障礙物，即機(jī)器人本身。對(duì)于機(jī)器人ri，其步驟如下：

1）判斷機(jī)器人ri未完成的路徑長(zhǎng)度；

2）若大于1，需要消除未完成路徑首部的動(dòng)態(tài)障礙物，即map(x，y，z)=0，其中(x，y，z)滿足式（10）：

Research and Application of Jacket Multi-pile Structure Foundation Installation for Offshore Wind Power Engineering ZHANG Qinghai,LI Shanfeng,WANG Shuwen(126)

3）在未完成的路徑尾部設(shè)置動(dòng)態(tài)障礙物，即(x，y，z)=i，其中(x，y，z)滿足式（11）：

其中.start表示機(jī)器人ri執(zhí)行任務(wù)tj的路徑首部；.end表示機(jī)器人ri執(zhí)行任務(wù)tj的路徑尾部。

3.2 最優(yōu)路徑選擇

最優(yōu)路徑選擇算法用于在盡可能短的時(shí)間內(nèi)優(yōu)先從最有可能是最優(yōu)路徑的路徑集合中找出機(jī)器人執(zhí)行任務(wù)的最優(yōu)路徑，由于多機(jī)器人路徑規(guī)劃是NP-Hard 問題，無法在多項(xiàng)式時(shí)間內(nèi)得到最優(yōu)解，這里的盡可能短時(shí)間是指優(yōu)先遍歷最有可能是最優(yōu)解的路徑所需要的時(shí)間，優(yōu)先遍歷的路徑可以在多項(xiàng)式時(shí)間內(nèi)驗(yàn)證該路徑是否是最優(yōu)解，優(yōu)先遍歷最有可能的路徑是指在給定條件下如路徑長(zhǎng)度、轉(zhuǎn)向次數(shù)、停止次數(shù)，采用帶條件DFS 尋找解路徑，這些條件隨著遍歷路徑的數(shù)量增加而線性遞增。

初始化條件時(shí)路徑長(zhǎng)度可由a、b兩點(diǎn)之間的曼哈頓距離求出，如式（12）：

在給定路徑長(zhǎng)度len、停止次數(shù)stop、轉(zhuǎn)向次數(shù)div條件下，驗(yàn)證機(jī)器人ri的一條路徑是否為最優(yōu)解可以通過將該路徑所需要的真實(shí)時(shí)間和完成任務(wù)tj需要的最短時(shí)間進(jìn)行比較，根據(jù)式（1）只需輸入leni、pstopi、pdivi、mixi等參數(shù)，即可計(jì)算完成任務(wù)tj的最短時(shí)間。其中，混合動(dòng)作mixi包含于全部原地停留次數(shù)stopi中或者全部轉(zhuǎn)向次數(shù)divi中，考慮實(shí)際多機(jī)器人系統(tǒng)，應(yīng)該盡量減少原地停留時(shí)間，從而提高系統(tǒng)的運(yùn)行效率。因?yàn)樵赝Ａ舸螖?shù)與轉(zhuǎn)向次數(shù)相比較少，故可以作mixi=(1-p)×stopi，p表示單一只是原地停止點(diǎn)的數(shù)量占所有停止點(diǎn)的數(shù)量的比例，p值較大這將減少機(jī)器人在運(yùn)動(dòng)過程中的混合動(dòng)作，減少不必要的時(shí)間消耗。將式（1）改寫為式（13），即可將式（1）中的4個(gè)參數(shù)減少到3個(gè)。

其中：stopi是所有的原地停留次數(shù)；divi是所有的轉(zhuǎn)向次數(shù)；Td表示每次轉(zhuǎn)向所消耗的平均時(shí)間；Ts表示每次原地停止的平均時(shí)間間隔。

機(jī)器人ri按照自身計(jì)算出的路徑運(yùn)動(dòng)，其真實(shí)消耗時(shí)間可由路徑終點(diǎn)所處的z值減去起點(diǎn)所處的z值表示：

單個(gè)機(jī)器人路徑規(guī)劃流程如圖4 所示。通過路徑選擇算法，按照轉(zhuǎn)向參數(shù)、停止參數(shù)、路程參數(shù)依次遞增的規(guī)律，構(gòu)造路徑集，在這些路徑集中可以找到一個(gè)最優(yōu)路徑。該算法中各子算法步驟如下。

圖4 路徑選擇算法Fig.4 Path selection algorithm

算法1 轉(zhuǎn)向遍歷算法。

1）循環(huán)遍歷符合參數(shù)的路徑集。

2）如果路徑為空，更新div++，條件使能，終止循環(huán)。

3）如果驗(yàn)證合法路徑是否是最優(yōu)解計(jì)算超時(shí)，更新stop++，條件使能，終止循環(huán)。

4）如果路徑合法，結(jié)合式（2）驗(yàn)證該路徑是否為最優(yōu)解。

5）如果是最優(yōu)解，找到最優(yōu)解程序退出。

6）如果不是最優(yōu)解：

a）如果是首次得到路徑，設(shè)置首次路徑標(biāo)志。

b）如果執(zhí)行路徑耗時(shí)減a、b兩點(diǎn)最短耗時(shí)小于maxd，maxd=執(zhí)行路徑耗時(shí)減a、b兩點(diǎn)最短耗時(shí)；否則，maxs=執(zhí)行路徑耗時(shí)減a、b兩點(diǎn)最短耗時(shí)。

c）以上條件都不滿足，設(shè)置最大時(shí)間標(biāo)志，保存已遍歷路徑中的最優(yōu)路徑，更新div++。

3.2.2 停留遍歷

算法2 停留遍歷算法。

1）保存已遍歷路徑中的最優(yōu)路徑，更新stop++。

2）如果未設(shè)置最大時(shí)間標(biāo)志：

a）如果轉(zhuǎn)向遍歷步驟得到的最優(yōu)路徑時(shí)間小于首次路徑標(biāo)志時(shí)間，最優(yōu)路徑為轉(zhuǎn)向遍歷步驟得到的最優(yōu)路徑，程序退出。

b）否則，最優(yōu)路徑為轉(zhuǎn)向遍歷步驟中設(shè)置的首次路徑，程序退出。

3.2.3 路程遍歷

算法3 路程遍歷算法。

1）保存已遍歷路徑中的最優(yōu)路徑，更新len++。

2）如果未設(shè)置最大時(shí)間標(biāo)志：

a）如果停留遍歷步驟得到的最優(yōu)路徑時(shí)間小于首次路徑標(biāo)志時(shí)間，最優(yōu)路徑為停留遍歷步驟得到的最優(yōu)路徑，程序退出。

b）否則，最優(yōu)路徑為停留遍歷步驟中設(shè)置的首次路徑，程序退出。

3.2.4 路徑確認(rèn)

算法4 路徑確認(rèn)算法。

1）如果轉(zhuǎn)向遍歷步驟得到的最優(yōu)路徑時(shí)間小于首次路徑標(biāo)志時(shí)間，最優(yōu)路徑為路程遍歷步驟得到的最優(yōu)路徑，程序退出。

2）否則，最優(yōu)路徑為轉(zhuǎn)向遍歷步驟得到的最優(yōu)路徑，程序退出。

3.3 帶條件DFS遍歷

給定len、stop、div參數(shù)后使用DFS 搜索路徑，本文DFS 算法優(yōu)先遍歷路徑集中參數(shù)符合len、stop、div的路徑情況，在地圖更新周期內(nèi)找到一條路徑，則將該路徑耗時(shí)與3.2 節(jié)中的最短耗時(shí)比較，確認(rèn)是否為最優(yōu)路徑。

算法采用嵌套堆棧結(jié)構(gòu)保存搜索內(nèi)容，其結(jié)構(gòu)如圖5所示。

圖5 堆棧存儲(chǔ)結(jié)構(gòu)Fig.5 Stack storage structure

使用node 堆棧保存已遍歷過的節(jié)點(diǎn)，每一個(gè)節(jié)點(diǎn)都包含一個(gè)指針堆棧，其中存儲(chǔ)當(dāng)前節(jié)點(diǎn)的自身節(jié)點(diǎn)self、父節(jié)點(diǎn)father、祖節(jié)點(diǎn)grandfather，可以用于判斷當(dāng)前節(jié)點(diǎn)相對(duì)于前兩個(gè)地圖更新周期路徑是否產(chǎn)生了轉(zhuǎn)向或者原地停留動(dòng)作，oristack表示當(dāng)前節(jié)點(diǎn)下一步的遍歷節(jié)點(diǎn)堆棧，可根據(jù)實(shí)際情況設(shè)置各遍歷方向不同的入堆棧順序，規(guī)定east=4，north=3，west=2，south=1，stop=0。根據(jù)當(dāng)前節(jié)點(diǎn)和方向堆棧棧頂元素可以計(jì)算出下一步應(yīng)當(dāng)遍歷的節(jié)點(diǎn)位置，定義矩陣：

定義函數(shù)g(i) ∈{0，1，2，3，4}表示取矩陣C的i行元素，則當(dāng)前節(jié)點(diǎn)(x，y，z)的下一點(diǎn)(x′，y′，z′)可表示為：

其中i為方向堆棧上棧頂元素。

算法5 帶條件的DFS算法。

1）初始化參數(shù)。

2）將起點(diǎn)壓入node堆棧。

3）根據(jù)當(dāng)前方向堆棧棧頂元素計(jì)算node 堆棧頂節(jié)點(diǎn)的下一節(jié)點(diǎn)。

4）計(jì)算下一節(jié)點(diǎn)相對(duì)于前兩個(gè)地圖更新周期的轉(zhuǎn)向數(shù)量、原地停止數(shù)量、距終點(diǎn)距離，如果不符合輸入?yún)?shù)len、stop、div，則將方向堆棧出棧，重新執(zhí)行第3）步，如果方向堆棧為空，node堆棧出棧，跳轉(zhuǎn)至第3）步。

5）如果下一點(diǎn)是終點(diǎn)，找到路徑，程序退出。

6）如果下一點(diǎn)是障礙物，當(dāng)前方向出棧，跳轉(zhuǎn)至第3）步。

7）如果下一點(diǎn)位置已經(jīng)超出地圖范圍，當(dāng)前方向出棧，跳轉(zhuǎn)至第3）步。

8）如果node 堆棧為空，表示未找到滿足當(dāng)前參數(shù)條件的路徑。

通過上述帶條件DFS 算法，實(shí)現(xiàn)了按照路徑選擇算法中給定的參數(shù)求解相應(yīng)的路徑功能，達(dá)到了符合當(dāng)前參數(shù)的最短路徑要求，由于三維時(shí)空地圖中路徑搜索方向是沿著時(shí)間軸單向遞增的，最大遍歷節(jié)點(diǎn)為存儲(chǔ)空間的一半，則帶條件DFS算法的時(shí)間復(fù)雜度為(a×b×C)/2。

4 實(shí)驗(yàn)與結(jié)果分析

4.1 實(shí)驗(yàn)環(huán)境

本文采用的實(shí)驗(yàn)配置為i5-9300H CPU，8 GB 運(yùn)行內(nèi)存，在Windows 10 中使用Visual Studio 2019 編程進(jìn)行仿真實(shí)驗(yàn)，設(shè)置障礙物比例為5%。設(shè)定機(jī)器人移動(dòng)單個(gè)柵格的路徑長(zhǎng)度需要的時(shí)間為1 s。在同一地圖場(chǎng)景下，針對(duì)不同的障礙物比例以及不同的機(jī)器人數(shù)量，隨機(jī)產(chǎn)生100 個(gè)任務(wù)，其中任何一個(gè)任務(wù)可能多次出現(xiàn)，不同任務(wù)的起點(diǎn)及終點(diǎn)可能相同。機(jī)器人的初始位置為隨機(jī)放置，完成一次任務(wù)，調(diào)度系統(tǒng)將排序機(jī)器人集合中包括正在執(zhí)行任務(wù)以及已經(jīng)完成任務(wù)的機(jī)器人移動(dòng)至任務(wù)起點(diǎn)的時(shí)間序列，選擇消耗時(shí)間最短的機(jī)器人為執(zhí)行該任務(wù)的機(jī)器人。機(jī)器人完成當(dāng)前任務(wù)后，方可執(zhí)行其他任務(wù)，即從當(dāng)前位置運(yùn)動(dòng)至新任務(wù)起點(diǎn)，然后開始執(zhí)行新任務(wù)。

4.2 實(shí)驗(yàn)場(chǎng)景搭建

在20×20 的地圖中按其規(guī)模的5%隨機(jī)產(chǎn)生障礙物，如圖6 中叉點(diǎn)，圓點(diǎn)代表任務(wù)的起點(diǎn)和終點(diǎn)，其中共有50 個(gè)任務(wù)，剩下的任務(wù)重復(fù)之前的50 個(gè)任務(wù)。隨機(jī)放置10 個(gè)機(jī)器人，分布如圖6 所示。為了保證實(shí)驗(yàn)的合理性以及任務(wù)多樣性，其中不同任務(wù)的起點(diǎn)及終點(diǎn)可能位于相同坐標(biāo)。

圖6 實(shí)驗(yàn)環(huán)境Fig.6 Experimental environment

4.3 結(jié)果分析

為驗(yàn)證基于三維時(shí)空地圖和運(yùn)動(dòng)分解的多機(jī)器人路徑規(guī)劃算法在路徑規(guī)劃方面的性能，將本文算法與A*算法［7］以及蟻群算法［10］進(jìn)行對(duì)比。A*算法和蟻群算法屬于采用集中式兩層沖突消解的多機(jī)器人路徑規(guī)劃算法，其中：第一層為路徑規(guī)劃層，先為機(jī)器人分配任務(wù)，然后在忽略其他機(jī)器人影響的情況下為每個(gè)機(jī)器人規(guī)劃自己的最優(yōu)路徑；第二層為沖突消解層，主要目的是消除第一層各機(jī)器人路徑之間的沖突。依據(jù)圖6 的實(shí)驗(yàn)環(huán)境設(shè)置，本文算法參數(shù)設(shè)置為αi=1，βi=1，wf=0.8，wu=0.2，T=1，p=0.8。A*算法中選用曼哈頓距離表示節(jié)點(diǎn)距離起始節(jié)點(diǎn)的代價(jià)。蟻群算法參數(shù)設(shè)置為：螞蟻數(shù)量20，迭代次數(shù)200，信息素傳遞參數(shù)0.5，總的信息素1 000.0。在規(guī)劃任務(wù)t1∶start=（12，2）、end=（1，16）時(shí)，系統(tǒng)選擇機(jī)器人集合中完成當(dāng)前任務(wù)及完成臨時(shí)任務(wù)耗時(shí)最小的機(jī)器人，由于系統(tǒng)初始時(shí)，機(jī)器人的當(dāng)前任務(wù)皆為空，只需考慮距離第一個(gè)任務(wù)起點(diǎn)最近的機(jī)器人r2，路徑規(guī)劃如圖7所示。

由于機(jī)器人r10所形成的動(dòng)態(tài)障礙物，本文算法規(guī)劃的路徑不能直接通過機(jī)器人r10所處位置，提前在（12，2）轉(zhuǎn)向，第二次轉(zhuǎn)向在（1，12）處，一共有兩次轉(zhuǎn)向。而A*算法首先忽略其他機(jī)器人的影響，各自規(guī)劃其路徑，其規(guī)劃的路徑會(huì)經(jīng)過機(jī)器人r10、r5、r1所處位置，然后根據(jù)沖突消解算法調(diào)整路徑，最終路徑共需3次轉(zhuǎn)向。而使用蟻群算法規(guī)劃的路徑共需10次轉(zhuǎn)向。本文算法規(guī)劃的路徑真實(shí)耗時(shí)為27 s，A*算法真實(shí)耗時(shí)28 s，蟻群算法耗時(shí)為33 s。

在機(jī)器人r2，從起點(diǎn)運(yùn)動(dòng)至下一節(jié)點(diǎn)的時(shí)間內(nèi)，系統(tǒng)開始規(guī)劃任務(wù)t2∶start=（16，15），end=（7，6），通過計(jì)算發(fā)現(xiàn)機(jī)器人r8運(yùn)動(dòng)至任務(wù)t2的起點(diǎn)耗時(shí)最短，系統(tǒng)將任務(wù)t2分配給機(jī)器人r8，機(jī)器人r8接收到任務(wù)后開始計(jì)算運(yùn)動(dòng)至任務(wù)t2起點(diǎn)的臨時(shí)路徑，得到計(jì)算結(jié)果后，將其計(jì)算結(jié)果上傳至中央處理模塊。當(dāng)下一個(gè)地圖更新周期開始后，機(jī)器人r8將按照已規(guī)劃的路徑運(yùn)動(dòng)，該臨時(shí)任務(wù)的耗時(shí)為4 s，在其運(yùn)動(dòng)的過程中，機(jī)器人r2則獨(dú)自運(yùn)動(dòng)至（12，6）處。機(jī)器人r3運(yùn)動(dòng)至任務(wù)t2的起點(diǎn)后，r3將根據(jù)當(dāng)前的地圖環(huán)境計(jì)算最優(yōu)路徑，其規(guī)劃的路如圖8所示。

圖7 任務(wù)1不同算法路徑規(guī)劃對(duì)比Fig.7 Task 1 path planning comparison of different algorithms

其中，本文算法規(guī)劃及A*算法的路徑共有兩次轉(zhuǎn)向，完成任務(wù)2的耗時(shí)都是20 s，蟻群算法耗時(shí)為24 s。

A*算法規(guī)劃的路徑與機(jī)器人r2的運(yùn)動(dòng)路徑在三維空間上距離較短，即不確定性時(shí)間較長(zhǎng)。對(duì)于計(jì)算完成任務(wù)t2的路徑在各平面的投影如圖9所示。

而蟻群算法規(guī)劃的路徑轉(zhuǎn)向次數(shù)過多，增加了機(jī)器人運(yùn)動(dòng)時(shí)間。

可以通過計(jì)算當(dāng)前路徑與其他路徑的距離計(jì)算出不確定時(shí)間，即圖8 中三種算法規(guī)劃的路徑與已存在的路徑之間的面積。對(duì)于A*算法規(guī)劃的路徑，計(jì)算出其u(map，path32)為171，蟻群算法不確定值為234，而本文規(guī)劃的路徑不確定值為229，這意味著A*算法有更大的概率產(chǎn)生碰撞，從而耗費(fèi)更多的時(shí)間。

圖8 任務(wù)2不同算法路徑規(guī)劃對(duì)比Fig.8 Task 2 path planning comparison of different algorithms

在計(jì)算任務(wù)t52∶start=（16，15）、end=（7，6）時(shí)，當(dāng)前地圖環(huán)境中仍有正在運(yùn)動(dòng)的機(jī)器人，在規(guī)劃任務(wù)t52時(shí)，必須考慮到其他機(jī)器人的運(yùn)動(dòng)影響，通過計(jì)算發(fā)現(xiàn)機(jī)器人r10運(yùn)動(dòng)至任務(wù)t52耗時(shí)最短，系統(tǒng)將該任務(wù)分配給機(jī)器人r10，分別使用本文算法、A*算法以及蟻群算法計(jì)算任務(wù)t52的路徑如圖10所示。

圖9 不同算法路徑投影Fig.9 Path projections of different algorithms

圖10 任務(wù)52不同算法路徑規(guī)劃對(duì)比Fig.10 Task 52 path planning comparison of different algorithms

可以看到，A*算法規(guī)劃的路徑在（6，14）將會(huì)與機(jī)器人r3碰撞，而蟻群算法雖然不會(huì)與其他機(jī)器人發(fā)生碰撞，但是由于轉(zhuǎn)向次數(shù)過多，機(jī)器人完成任務(wù)消耗的時(shí)間也相應(yīng)增多。本文算法將機(jī)器人路徑拓展至三維時(shí)空地圖后，可以在計(jì)算路徑時(shí)提前發(fā)現(xiàn)碰撞點(diǎn)，選擇其他路徑后可以避免碰撞。

地圖規(guī)模為20×20、30×30、40×40 時(shí)，任務(wù)集的總路程分別為1 312 m、2 264 m 和2 872 m，實(shí)驗(yàn)對(duì)比結(jié)果如表1所示。

表1 中：Len表示機(jī)器人實(shí)際運(yùn)動(dòng)的路程總長(zhǎng)度；Abs表示全部任務(wù)的起點(diǎn)與終點(diǎn)的曼哈頓距離之和；Abs/Len表示任務(wù)總長(zhǎng)度與機(jī)器人實(shí)際運(yùn)動(dòng)總長(zhǎng)度之比，其值越接近1 表示多機(jī)器人系統(tǒng)越高效，即機(jī)器人的空載率低，而其值越接近0 表示系統(tǒng)中存在過多的空載情況，效率低下。機(jī)器人實(shí)際運(yùn)動(dòng)路徑長(zhǎng)度包括執(zhí)行任務(wù)的路徑長(zhǎng)度以及機(jī)器人從自身位置移動(dòng)至任務(wù)起點(diǎn)的路徑長(zhǎng)度。根據(jù)表1 可以看出，在障礙物比例及地圖規(guī)模相同的情況下，完成同一任務(wù)集機(jī)器人數(shù)量越多，真實(shí)路徑長(zhǎng)度越小，總的時(shí)間消耗也是減少的。在地圖規(guī)模較小的情況下，對(duì)于完成同一任務(wù)集的機(jī)器人數(shù)量，其增加趨勢(shì)與總耗時(shí)時(shí)間的下降趨勢(shì)關(guān)系不明顯，但隨著地圖規(guī)模的增大，這種關(guān)系會(huì)更加明顯起來，即在增加相同數(shù)量的機(jī)器人情況下，地圖規(guī)模越大，系統(tǒng)的總耗時(shí)越少。隨著地圖規(guī)模的增大，隨機(jī)任務(wù)之間的路徑長(zhǎng)度也會(huì)增大，這將導(dǎo)致系統(tǒng)的運(yùn)行時(shí)間和真實(shí)路徑與最短路徑之比逐漸增大。

在地圖規(guī)模為20×20、30×30、40×40時(shí)，分別使用10、20、30個(gè)機(jī)器人完成100個(gè)連續(xù)任務(wù)，最終得出了本文算法與A*算法和蟻群算法相比時(shí)的總路程減少率Lper以及總耗時(shí)減少率Tper，實(shí)驗(yàn)結(jié)果如表2所示。

表1 不同地圖規(guī)模下的路徑搜索代價(jià)Tab.1 Path search cost under different map scales

表2 不同地圖規(guī)模下本文算法相較于兩種對(duì)比算法的路程和時(shí)間減少率Tab.2 Distance and time reduction rate of proposed algorithm compared with two comparison algorithms under different map scales

由此可見，本文算法規(guī)劃的路徑集總長(zhǎng)度更小，完成同一批任務(wù)耗時(shí)更短，與A*算法相比，總路程平均減少比例及總時(shí)間平均減少比例分別為21.1%、42.5%，而與蟻群算法相比，相應(yīng)的總路程和總時(shí)間平均減少比例分別為2.49%、28.5%，且在地圖規(guī)模較大的場(chǎng)景時(shí)，這種優(yōu)勢(shì)更加明顯。

5 結(jié)語

本文分析了單個(gè)機(jī)器人完成特定原子任務(wù)的運(yùn)動(dòng)時(shí)間組成因素，將其拆分為計(jì)算機(jī)解算時(shí)間、路徑運(yùn)動(dòng)時(shí)間、不確定時(shí)間，并在二維柵格地圖的垂直方向拓展成時(shí)間軸，將二維地圖拓展為三維時(shí)空?qǐng)D，有效地將路徑規(guī)劃中碰撞避免放在了第一層，減少了路徑?jīng)_突問題。通過具體分析路徑運(yùn)動(dòng)時(shí)間，將其拆分為轉(zhuǎn)向時(shí)間、原地停止時(shí)間、路徑運(yùn)行時(shí)間參數(shù)，然后使用帶條件DFS 路徑搜索中優(yōu)先使用低代價(jià)的參數(shù)，DFS可以在盡可能短的時(shí)間內(nèi)找到一條最優(yōu)路徑。本文算法與傳統(tǒng)算法相比系統(tǒng)耗時(shí)更少，并且不存在沖突情況，可以保證找到一條最優(yōu)路徑，在應(yīng)對(duì)突發(fā)隨機(jī)任務(wù)時(shí)更具靈活性。