吳靜莉

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

1 引言

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

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

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

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

2.1 環(huán)境模型

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

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

圖1 倉(cāng)儲(chǔ)柵格模型Fig.1 Grid Model of Storage

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

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

2.2 A＊算法原理及分析

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

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

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

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

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

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

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

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

3 勢(shì)場(chǎng)A*算法

3.1 勢(shì)場(chǎng)信息構(gòu)建

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

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

式中：X0—障礙物柵格的坐標(biāo)；Urep(Xk)—柵格k受到的斥力作用；m2—障礙物質(zhì)量；d(Xk，X0)—柵格k與障礙物柵格的距離；d0—斥力作用的距離閾值，即超出距離d0不再有斥力勢(shì)場(chǎng)。目標(biāo)柵格的引力勢(shì)場(chǎng)函數(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)境的特殊性，對(duì)引力勢(shì)場(chǎng)函數(shù)和斥力勢(shì)場(chǎng)函數(shù)做適應(yīng)性定義。由斥力勢(shì)場(chǎng)表達(dá)式可知，在距離閾值d0范圍內(nèi)，與障礙物柵格距離越近則受到的斥力越大，與障礙物柵格距離越遠(yuǎn)則受到的斥力越小。按照上述規(guī)律，在柵格環(huán)境下對(duì)斥力勢(shì)場(chǎng)做以下改進(jìn)：將斥力勢(shì)場(chǎng)作用范圍定義為障礙物柵格2步可達(dá)范圍內(nèi)的柵格，其中1步可達(dá)柵格的斥力全部為2.5，2步可達(dá)柵格的斥力全部為0.5，斥力方向定義為由障礙物柵格中心指向受力柵格中心的連線，當(dāng)某柵格同時(shí)受到多個(gè)力作用時(shí)，使用三角形規(guī)則計(jì)算合力大小和方向，如圖2所示。

圖2 障礙物斥力場(chǎng)Fig.2 Repulsion Field of Obstacle

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

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

3.2 勢(shì)場(chǎng)A＊算法及流程

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

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

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

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

綜合上述分析，得到勢(shì)場(chǎng)A*算法的流程為：

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

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

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

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

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

4 時(shí)域協(xié)作的勢(shì)場(chǎng)A*算法

4.1 時(shí)域協(xié)作勢(shì)場(chǎng)A＊算法原理

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

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

圖3 三維時(shí)空?qǐng)DFig.3 3-Dimension Spatiotemporal Diagram

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

4.2 碰撞的確定與解決方案

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

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

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

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

5 仿真及驗(yàn)證

5.1 勢(shì)場(chǎng)A＊算法規(guī)劃性能驗(yàn)證

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

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

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

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

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

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

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

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

機(jī)器人優(yōu)先級(jí)設(shè)置為機(jī)器人1>機(jī)器人4>機(jī)器人2>機(jī)器人3。機(jī)器人i起點(diǎn)記為Si，終點(diǎn)記為Gi，則機(jī)器人1～4的起點(diǎn)和終點(diǎn)設(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）。時(shí)域協(xié)作-勢(shì)場(chǎng)A*算法的路徑規(guī)劃和避撞結(jié)果，如圖6所示。圖6中柵格A為機(jī)器人2與機(jī)器人3可能發(fā)生碰撞的柵格，柵格B為機(jī)器人2與機(jī)器人4可能發(fā)生碰撞的柵格，柵格C為機(jī)器人1與機(jī)器人4可能發(fā)生碰撞的柵格。

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

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

6 結(jié)論

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

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

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