多機器人協(xié)同路徑的時域協(xié)作-勢場A*規(guī)劃

吳靜莉

（鶴壁職業(yè)技術(shù)學(xué)院，河南 鶴壁 458030）

1 引言

在物流倉儲中使用機器人執(zhí)行搬運任務(wù)能夠有效減少公司的人力投入和運營成本，由于單機器人工作能力有限且倉庫規(guī)模越來越大，多機器人協(xié)同工作勢在必行［1］，而且多機器人系統(tǒng)存在著效率高、魯棒性強、經(jīng)濟、可移植等諸多優(yōu)點。但是機器人之間彼此為動態(tài)障礙物，在保證多機器人系統(tǒng)運行安全基礎(chǔ)上，研究多機器人路徑規(guī)劃問題具有重要的理論價值和經(jīng)濟價值［2］。

多機器人系統(tǒng)路徑規(guī)劃主要從環(huán)境建模和規(guī)劃算法兩個角度進行研究。路徑規(guī)劃早期研究中更加注重環(huán)境建模方法，通過環(huán)境模型的簡化實現(xiàn)路徑優(yōu)化，成熟的環(huán)境建模法包括可視圖法、拓?fù)浞?、柵格法等?］。當(dāng)前路徑規(guī)劃研究更加注重規(guī)劃算法的研究，從大的方面講，路徑規(guī)劃算法主要分為采樣規(guī)劃法、搜索規(guī)劃法、智能優(yōu)化法等3類。采樣規(guī)劃法是通過在環(huán)境中隨機采樣而搜索最優(yōu)路徑的規(guī)劃方法，比如擴展隨機樹［4］、概率路線圖等。搜索規(guī)劃法首先將連續(xù)空間轉(zhuǎn)化為離散空間，然后再離散空間進行搜索得到最優(yōu)路徑，包括A*、D*等方法［5］。智能優(yōu)化方法目前研究最多，是基于仿生算法進行規(guī)劃的方法，包括蟻群算法、遺傳算法等。文獻［6］針對環(huán)境已知的全局路徑規(guī)劃問題，提出了動態(tài)RRT*算法，該方法降低了路徑搜索成本，同時提高了算法的路徑規(guī)劃速度。文獻［7］在遺傳算法中提出了新型遺傳操作，并將改進粒子群算法應(yīng)用于多機器人路徑規(guī)劃，經(jīng)驗證改進粒子群算法的優(yōu)化能力和魯棒性強于DE算法和傳統(tǒng)PSO。文獻［8］以多機器人系統(tǒng)的總長度、平滑度、總耗時為優(yōu)化目標(biāo)，引入合作型協(xié)同算法框架，使用非支配排序進行優(yōu)化，經(jīng)驗證該方法可以有效實現(xiàn)機器人系統(tǒng)的多目標(biāo)優(yōu)化。上述方法在各自設(shè)定環(huán)境下取得了較好的規(guī)劃效果，但是多機器人路徑規(guī)劃需考慮防撞、路徑長度優(yōu)化、轉(zhuǎn)彎優(yōu)化等多方面問題，因此仍需進行深入和多方面研究。

這里研究了多機器人工作路徑的協(xié)同規(guī)劃問題，在A*算法基礎(chǔ)上引入了障礙物和目標(biāo)柵格的勢場信息作為輔助信息，有效提高了A*算法的路徑規(guī)劃質(zhì)量。針對多機器人之間的避撞問題，使用三維時空圖代替二維平面圖，可以有效判斷出機器人之間是否碰撞，并根據(jù)不同碰撞類型制定了避撞措施，有效實現(xiàn)了多機器人的協(xié)同路徑規(guī)劃。

2 環(huán)境建模與A*算法分析

2.1 環(huán)境模型

基于柵格的環(huán)境建模方法具有原理簡單、易于實現(xiàn)等優(yōu)點，因此這里使用柵格法建立機器人工作環(huán)境模型。柵格法是將環(huán)境劃分為大小均等的柵格進行表示，根據(jù)柵格形狀的不同，柵格可以分為方形柵格、蜂巢柵格、三角柵格等，這里采用應(yīng)用最為廣泛的方形柵格。

在柵格地圖中使用二元值區(qū)分自由柵格和障礙物柵格，比如將障礙物柵格賦值為1，將自由柵格賦值為0，那么通過（0～1）二元陣列可以有效區(qū)分環(huán)境中的障礙物柵格和自由柵格。柵格大小根據(jù)環(huán)境精度、機器人速度、機器人尺寸等確定，一般根據(jù)經(jīng)驗設(shè)置合理即可。按照上述建模方法和原則，設(shè)置柵格邊長為10cm，得到某倉儲空間柵格模型，如圖1所示。

圖1 倉儲柵格模型Fig.1 Grid Model of Storage

傳統(tǒng)柵格模型中，二元陣列即可有效區(qū)分環(huán)境中的自由位置和障礙物位置。但是在倉儲環(huán)境中，需要區(qū)分已存放貨物的貨架、未存放貨物的貨架、過道3種狀態(tài)，因此作以下定義：過道柵格賦值為0，存放貨物的柵格賦值為1，未存放貨物的柵格賦值為-1。

按照上述定義可以將倉儲環(huán)境建模為三元陣列模型。在柵格環(huán)境下，機器人運動方向有4 鄰域法、8 鄰域法還有擴展鄰域法，本文使用常見的8鄰域法。

2.2 A＊算法原理及分析

與傳統(tǒng)圖搜索算法相比，A*算法借助輔助信息優(yōu)化節(jié)點的搜索方向，是一種基于輔助信息的快速尋路方法。輔助信息一般使用評估函數(shù)f(k)進行衡量［9］為：

式中：g(k)—機器人當(dāng)前柵格到待選柵格k的距離；h(k)—待選柵格k到目標(biāo)柵格的距離。

距離的計算有曼哈頓距離、歐幾里得距離、切比雪夫距離等，在柵格環(huán)境的8鄰域法中使用歐氏距離更加合理為：

式中：(xk，yk)—柵格k的坐標(biāo)；(xg，yg)—目標(biāo)柵格g的坐標(biāo)。

基于A*算法的路徑規(guī)劃過程為：機器人從當(dāng)前柵格開始，按照式（1）所示的輔助信息計算方法，計算8個鄰域柵格的輔助信息，并選擇輔助信息最小的柵格作為下一柵格。

分析A*算法原理可知，A*算法以輔助信息為依據(jù)，使用貪婪準(zhǔn)則進行柵格選擇。但是這種方法存在以下缺陷：

（1）當(dāng)障礙物分布復(fù)雜，尤其是起點柵格到目標(biāo)柵格連線上障礙物分布復(fù)雜時，A*算法難以規(guī)劃出全局最優(yōu)路徑，甚至出現(xiàn)規(guī)劃失敗的情況；

（2）使用的輔助信息不夠全面。在式（1）中主要以柵格之間的距離為輔助信息，但是障礙物及目標(biāo)點的位置信息是更具有參考作用的輔助信息，但是卻沒有充分利用。

3 勢場A*算法

3.1 勢場信息構(gòu)建

針對2.2節(jié)分析的A*算法存在的問題，本節(jié)參考勢場法構(gòu)造障礙物及目標(biāo)分布的輔助信息。

在勢場法中障礙物具有斥力作用，目標(biāo)柵格具有引力作用［10］。在傳統(tǒng)勢場法中，障礙物的斥力勢場函數(shù)為：

式中：X0—障礙物柵格的坐標(biāo)；Urep(Xk)—柵格k受到的斥力作用；m2—障礙物質(zhì)量；d(Xk，X0)—柵格k與障礙物柵格的距離；d0—斥力作用的距離閾值，即超出距離d0不再有斥力勢場。目標(biāo)柵格的引力勢場函數(shù)為：

式中：Xk—柵格k的坐標(biāo)；Xg—目標(biāo)柵格g的坐標(biāo)；m1—目標(biāo)質(zhì)量；Ustt(Xk)—柵格k受到的引力作用；d(Xk，Xg)—柵格k與目標(biāo)柵格g的距離。

根據(jù)柵格環(huán)境的特殊性，對引力勢場函數(shù)和斥力勢場函數(shù)做適應(yīng)性定義。由斥力勢場表達式可知，在距離閾值d0范圍內(nèi)，與障礙物柵格距離越近則受到的斥力越大，與障礙物柵格距離越遠則受到的斥力越小。按照上述規(guī)律，在柵格環(huán)境下對斥力勢場做以下改進：將斥力勢場作用范圍定義為障礙物柵格2步可達范圍內(nèi)的柵格，其中1步可達柵格的斥力全部為2.5，2步可達柵格的斥力全部為0.5，斥力方向定義為由障礙物柵格中心指向受力柵格中心的連線，當(dāng)某柵格同時受到多個力作用時，使用三角形規(guī)則計算合力大小和方向，如圖2所示。

圖2 障礙物斥力場Fig.2 Repulsion Field of Obstacle

圖2（a）中一種柵格為障礙物柵格，一種柵格為1 步可達柵格，所受斥力為2.5；一種柵格為2 步可達柵格，所受斥力為0.5。在圖2（b）中一種柵格為受到合力的柵格，其大小和方向由三角形準(zhǔn)則決定。

按照相同的設(shè)置方法，將目標(biāo)柵格的引力作用也進行適應(yīng)性定義為：1步可達的鄰域柵格引力定義為10，而后每增加一步則引力減小1，直至引力為0為止。

3.2 勢場A＊算法及流程

將3.1節(jié)構(gòu)建的勢場信息作為輔助信息添加到A*算法中，則勢場A*算法的輔助信息為：

式中：fsc(k)—柵格k的新型輔助信息。對比式（5）、式（1）可知：

（1）新型輔助信息中添加了障礙物的斥力作用和目標(biāo)柵格的引力作用，有效使用了環(huán)境中障礙物和目標(biāo)柵格的先驗知識；

（2）在斥力和引力作用下，機器人能夠有效避開障礙物而趨向目標(biāo)點，有效提高規(guī)劃路徑的合理性和安全性。

綜合上述分析，得到勢場A*算法的流程為：

（1）建立柵格環(huán)境模型，定義起始柵格和目標(biāo)柵格；

（2）將機器人放置在起始柵格位置，并計算鄰域自由柵格的新型輔助信息值；

（3）比較鄰域自由柵格的輔助信息值，選擇最小值對應(yīng)的柵格；

（4）機器人前進1步并判斷是否到達目標(biāo)柵格，若機器人沒有到達目標(biāo)柵格則轉(zhuǎn)至（3）；若是則繼續(xù)；

（5）機器人輸出規(guī)劃路徑，規(guī)劃過程結(jié)束。

4 時域協(xié)作的勢場A*算法

4.1 時域協(xié)作勢場A＊算法原理

根據(jù)前文分析，勢場A*算法充分利用了柵格距離信息、障礙物位置信息、目標(biāo)柵格位置信息作為輔助信息，對于單機器人路徑規(guī)劃問題具有很好的求解效果。但是對于多機器人系統(tǒng)，機器人之間彼此為動態(tài)障礙物，由于勢場A*算法使用的是二維平面圖，當(dāng)兩個機器人路徑出現(xiàn)重疊時，難以判斷是否會發(fā)生碰撞，因此勢場A*算法無法解決機器人之間的碰撞問題。

針對上述問題，本節(jié)提出了時域協(xié)作的勢場A*算法，其核心思想是：在二維倉儲地圖中加入第三維度—時間維度，從而構(gòu)造一個“橫-縱-時間”的三維時空圖，如圖3 所示。在三維時空圖中，從時間軸上的投影即為二維倉儲地圖；沿時間軸進行切割，可以得到相應(yīng)時刻地圖中機器人的分布位置。

圖3 三維時空圖Fig.3 3-Dimension Spatiotemporal Diagram

加入時間維度后，機器人的運動方向仍是二維的，但是為了滿足“時間只能流逝不能倒退”的約束，規(guī)定時間軸上的方向只能是正的。

4.2 碰撞的確定與解決方案

勢場A*算法使用的是二維平面圖，由于缺少時間維度，因此當(dāng)兩個機器人路徑出現(xiàn)重疊時無法判斷是否發(fā)生碰撞。而時域協(xié)作勢場A*算法使用的三維時空圖，當(dāng)兩個機器人路徑在三維時空圖中出現(xiàn)重疊時，即可得出兩個機器人會發(fā)生碰撞的結(jié)論。

機器人發(fā)生碰撞時，一般可以分為側(cè)面碰撞和正面碰撞兩種情況。當(dāng)機器人發(fā)生側(cè)面碰撞時，優(yōu)先級低的機器人等待1個節(jié)拍，優(yōu)先級高的機器人正常行駛。

機器人可能發(fā)生的正面碰撞具有兩種情況，如圖4所示。當(dāng)機器人發(fā)生正面碰撞時，優(yōu)先級高的機器人正常行駛，優(yōu)先級低的機器人將碰撞柵格設(shè)置為障礙物柵格，以目標(biāo)柵格為終點，重新進行路徑規(guī)劃。

圖4 正面碰撞兩種情況Fig.4 Two Cases of Frontal Collision

5 仿真及驗證

5.1 勢場A＊算法規(guī)劃性能驗證

首先驗證勢場A*算法在單個機器人路徑規(guī)劃中的性能，仿真環(huán)境如下：運行環(huán)境為Windows10，處理器為Intel Core 15-7300HQ，內(nèi)存16GB，仿真軟件為Matlab。柵格規(guī)模為（20×20），其中起點柵格為（1，1），目標(biāo)柵格為（20，20），柵格邊長設(shè)置為1。環(huán)境中的障礙物柵格比設(shè)置為15%和35%兩種情況，分別代表簡單障礙物環(huán)境和復(fù)雜障礙物環(huán)境。分別使用傳統(tǒng)A*算法、勢場A*算法、文獻［11］改進蟻群算法進行路徑規(guī)劃，每種算法各自獨立規(guī)劃10次，最優(yōu)路徑，如圖5所示。

圖5 不同復(fù)雜度環(huán)境下路徑規(guī)劃Fig.5 Path Planning Result Under Different Complexity Environments

統(tǒng)計三種算法在15%障礙物和35%障礙物兩種柵格環(huán)境下的規(guī)劃路徑長度結(jié)果，如表1所示。

表1 路徑規(guī)劃結(jié)果Tab.1 Path Planning Result

由圖5和表1可以看出，基于A*算法規(guī)劃的路徑存在較多曲折和往返現(xiàn)象，因此其路徑長度也最大。這是因為A*算法在選擇柵格時，依據(jù)輔助信息按照貪婪準(zhǔn)則進行選擇時會自動選擇距離目標(biāo)柵格較近的柵格，而沒有關(guān)注障礙物的分布情況，因此路徑曲折和轉(zhuǎn)彎較多。改進蟻群算法規(guī)劃路徑質(zhì)量居中和，這是因為在文獻［11］中螞蟻視野較大，可以關(guān)注兩步范圍內(nèi)的障礙物分布，并具有針對性進行路徑規(guī)劃。而這里提出的勢場A*算法規(guī)劃的路徑最短且路徑最光滑，這是因為勢場A*算法充分利用了障礙物和目標(biāo)點的位置信息，相當(dāng)于全場的障礙物和目標(biāo)點的先驗信息得到了充分利用，因此其路徑質(zhì)量好于改進蟻群算法和傳統(tǒng)A*算法。

5.2 多機器人協(xié)同規(guī)劃驗證

使用時域協(xié)作-勢場A*算法對多機器人工作路徑進行協(xié)同規(guī)劃。5.1節(jié)已經(jīng)驗證了勢場A*算法的路徑規(guī)劃性能，本節(jié)驗證時域協(xié)作-勢場A*算法的避撞性能。

在20×20柵格環(huán)境下放置4個機器人進行工作，機器人之間互為動態(tài)障礙物。

機器人優(yōu)先級設(shè)置為機器人1>機器人4>機器人2>機器人3。機器人i起點記為Si，終點記為Gi，則機器人1～4的起點和終點設(shè)置為：S1（8，13），G1（12，3），S2（4，5），G2（17，14），S3（17，14），G3（4，5），S4（4，10），G4（14，5）。時域協(xié)作-勢場A*算法的路徑規(guī)劃和避撞結(jié)果，如圖6所示。圖6中柵格A為機器人2與機器人3可能發(fā)生碰撞的柵格，柵格B為機器人2與機器人4可能發(fā)生碰撞的柵格，柵格C為機器人1與機器人4可能發(fā)生碰撞的柵格。

圖6 多機器人協(xié)同規(guī)劃結(jié)果Fig.6 Multi-Robot Collaborative Planning Result

首先各機器人基于勢場A*算法規(guī)劃出各自的最優(yōu)路徑，而后使用三維時空圖判斷各機器人是否發(fā)生碰撞。當(dāng)兩個機器人發(fā)生碰撞時，優(yōu)先級高的機器人按原路徑行駛，優(yōu)先級低的路徑按照避撞策略進行避撞，4 個機器人最終規(guī)劃的路徑，如圖6 所示。圖6中柵格A處機器人2與機器人3會發(fā)生正面碰撞，機器人2優(yōu)先級更高按照原路徑行駛，機器人3重新進行路徑規(guī)劃，結(jié)果如圖中所示。柵格B處機器人2與機器人4可能發(fā)生側(cè)面碰撞，機器人4優(yōu)先級高按照原路徑行駛，機器人2等待1個節(jié)拍后繼續(xù)按最優(yōu)路徑行駛。柵格C處機器人1與機器人4可能發(fā)生側(cè)面碰撞，機器人1優(yōu)先級高按照原路徑行駛，機器人4等待1個節(jié)拍后繼續(xù)按最優(yōu)路徑行駛。由上述分析可以看出，時域協(xié)作-勢場A*算法不僅能夠規(guī)劃出單個機器人的最優(yōu)路徑，而且在多機器人協(xié)同規(guī)劃中能夠有效解決碰撞問題，驗證了時域協(xié)作-勢場A*算法在多機器人路徑規(guī)劃中的優(yōu)越性和有效性。

6 結(jié)論

這里研究了多機器人的路徑協(xié)同規(guī)劃問題，提出了基于時域協(xié)作-勢場A*算法的協(xié)同路徑規(guī)劃方法，經(jīng)驗證可以得出以下結(jié)論：

（1）勢場A*算法由于充分利用了障礙物和目標(biāo)柵格位置信息等先驗知識，其路徑規(guī)劃質(zhì)量（長度和平滑度方面）高于傳統(tǒng)A*算法。

（2）時域協(xié)作-勢場A*算法將二維平面環(huán)境圖改進為三維時空圖，能夠有效判斷和解決機器人之間的碰撞問題，驗證了時域協(xié)作-勢場A*算法在多機器人路徑協(xié)同規(guī)劃中的有效性。