障礙環(huán)境下隊形可變編隊的路徑規(guī)劃方法研究

王 祝，張振鵬，張原寧，焦嵩鳴

（華北電力大學(xué)（保定）自動化系，保定 071003）

1 引 言

移動機器人具有機動性強、負(fù)載能力高等優(yōu)點，當(dāng)前已被應(yīng)用到探索[1]、偵察[2]、搜救[3]等領(lǐng)域。由于真實環(huán)境的復(fù)雜性和機器人性能的局限性，僅靠單機器人難以高效完成復(fù)雜任務(wù)，因而需要機器人編隊通過相互配合完成復(fù)雜任務(wù)[4]。為使機器人編隊能快速到達(dá)目標(biāo)點執(zhí)行任務(wù)，需要規(guī)劃出一條避免碰撞且使特定性能指標(biāo)最優(yōu)的可行路徑[5-6]。

針對多機器人編隊路徑規(guī)劃問題，近年來研究者已經(jīng)開展了蟻群算法[7]、遺傳算法[8]、粒子群算法[9]等研究。文獻[10]針對領(lǐng)航跟隨編隊提出了一種改進的蟻群算法，為領(lǐng)導(dǎo)機器人搜索出一條最短路徑，實現(xiàn)了編隊避障。文獻[11]針對傳統(tǒng)遺傳算法路徑規(guī)劃時存在的收斂速度慢、路徑不平滑問題，在適應(yīng)度函數(shù)中加入了路徑平滑度因素，為機器人編隊規(guī)劃出一條安全無碰撞且平滑度較好的最優(yōu)路徑。文獻[12]提出了一種基于改進快速搜索隨機樹的多機器人編隊路徑規(guī)劃算法，實現(xiàn)了機器人間的編隊隊形保持，同時在規(guī)劃中實時調(diào)整隊形方向，實現(xiàn)了編隊整體避障。文獻[13]使用人工勢場法進行多個全向機器人路徑規(guī)劃，實現(xiàn)在編隊不變的情況下多機器人編隊能夠躲避障礙物和邊界到達(dá)目標(biāo)點。然而，上述研究均假設(shè)編隊隊形保持不變，未能利用隊形可變的特點，在障礙比較復(fù)雜的情況下，隊形保持可能導(dǎo)致不存在可行的編隊路徑，同時隊形保持可能需要繞行較遠(yuǎn)的路徑，以實現(xiàn)避撞，導(dǎo)致編隊抵達(dá)任務(wù)點的時間增加。

在考慮隊形變換的編隊協(xié)同研究方面，文獻[14]提出了一種基于Dubins 和粒子群優(yōu)化的路徑規(guī)劃方法，根據(jù)飛行器運動學(xué)模型對Dubins 軌跡進行降維處理，采用粒子群優(yōu)化求解編隊重構(gòu)路徑規(guī)劃。該方法需要在初始隊形和目標(biāo)隊形已知情況下，對編隊隊形實現(xiàn)重構(gòu)。文獻[15]針對協(xié)同航跡規(guī)劃中因隊形約束而忽略較窄通道的問題，提出了一種基于自適應(yīng)分布式模型預(yù)測控制的快速粒子群優(yōu)化方法，在編隊控制中根據(jù)前方環(huán)境自適應(yīng)進行編隊變換。該方法是一種局部的反應(yīng)式控制方法，并非全局規(guī)劃方法。文獻[16]提出一種改進的人工勢場法，引入實際速度、加速度改進勢場力表示方法，實現(xiàn)了編隊穿越狹窄區(qū)域的隊形變換。該方法也是在編隊控制階段進行反應(yīng)式的隊形切換，并未在全局路徑規(guī)劃階段預(yù)先考慮隊形變化對可行路徑的影響。

針對以上問題，本文提出一種考慮隊形變化的編隊路徑規(guī)劃方法，通過障礙二次膨脹構(gòu)建規(guī)劃地圖，并引入穿越軟障礙路徑代價新指標(biāo)，構(gòu)建包含路徑長度和軟障礙違背度的統(tǒng)一化代價函數(shù)，然后利用改進A*搜索算法生成一條無碰撞且長度較小的編隊參考路徑。編隊路徑規(guī)劃過程中能夠自動調(diào)整隊形來適應(yīng)障礙環(huán)境，以滿足多指標(biāo)的規(guī)劃需求。

2 問題建模

本節(jié)首先給出了本文所研究的編隊路徑規(guī)劃問題描述、機器人編隊隊形模型，然后構(gòu)建了編隊路徑規(guī)劃中需考慮的障礙約束條件、路徑長度指標(biāo)和隊形保持指標(biāo)。

2.1 編隊參考路徑規(guī)劃問題

本文研究的編隊參考路徑規(guī)劃目標(biāo)是在盡可能保證隊形不變的前提下，找到一條從起點到目標(biāo)點的安全路徑，作為編隊的參考路徑，引導(dǎo)機器人編隊通過編隊控制的方式抵達(dá)目標(biāo)點，完成給定任務(wù)。

編隊參考路徑表征編隊幾何中心點的變化路徑。根據(jù)規(guī)劃得到的中心點參考路徑，機器人編隊可以采取合適的編隊控制方法，例如領(lǐng)航跟隨法[17]、虛擬結(jié)構(gòu)法[18]等，跟蹤參考路徑從起點安全到達(dá)目標(biāo)點。

圖1給出了機器人編隊路徑規(guī)劃的示意圖，其中機器人的期望任務(wù)隊形為三角編隊。若保持三角隊形運動，需對下方的兩個障礙進行繞行，以避免碰撞，如圖1實線路徑所示；若允許編隊變換，則可以將三角隊形變換為縱向隊形，從下方的兩個障礙間通過，如圖1虛線路徑所示。

圖1 機器人編隊路徑規(guī)劃示意圖Fig.1 Diagram of robot formation path planning

2.2 機器人編隊隊形模型

在編隊參考路徑規(guī)劃時，需要根據(jù)編隊的規(guī)模和隊形計算出編隊的長度和寬度用于障礙碰撞檢測。然而不同編隊不同規(guī)模其編隊的長度和寬度均不同，例如對于圖2給出的機器人編隊典型隊形：橫向編隊、三角編隊、縱向編隊和菱形編隊。

圖2 機器人典型編隊示意圖Fig.2 Diagram of typical robot formations

為方便計算機器人編隊尺寸，將編隊中機器人沿橫向和縱向分別平移映射到一條中心線上，得到編隊機器人橫向數(shù)量NL和縱向數(shù)量NW，然后根據(jù)式(1)可計算得到編隊橫向和縱向尺寸。

式中，LF、WF為編隊橫向和縱向長度，LR、WR為機器人長寬，Lspace和Wspace為編隊個體間的長度和寬度。

2.3 障礙規(guī)避約束

機器人路徑規(guī)劃需考慮對環(huán)境中的障礙進行規(guī)避，以保證機器人安全。為了便于路徑搜索和地圖信息存儲，采用柵格地圖進行環(huán)境建模，如圖3所示。由于機器人向前不會與障礙物碰撞，只需考慮機器人兩側(cè)與障礙物碰撞，故選取機器人寬度作為障礙物膨脹尺寸，防止機器人兩側(cè)撞擊到障礙物，建圖時以機器人的寬度尺寸對實際障礙進行膨脹，從而構(gòu)建機器人路徑規(guī)劃的構(gòu)型空間，在此空間下可以將機器人看作質(zhì)點進行路徑規(guī)劃。

圖3中，黑色區(qū)域為實際障礙的占據(jù)空間，灰色區(qū)域為障礙膨脹后的占據(jù)區(qū)域。將機器人當(dāng)作質(zhì)點進行路徑規(guī)劃時，黑色區(qū)域和灰色區(qū)域均為不可通行區(qū)，白色區(qū)域記為可通行區(qū)。為保證機器人的運行安全，路徑規(guī)劃時需進行障礙檢測，以保證規(guī)劃的路徑不會穿過不可通行區(qū)。

圖3 機器人編隊路徑規(guī)劃的環(huán)境柵格圖Fig.3 Grid map of environment for robot formation path planning

2.4 路徑規(guī)劃指標(biāo)

為了使編隊在障礙環(huán)境下仍具有較高的通過性，考慮隊形可變條件下的編隊路徑規(guī)劃?？紤]到編隊任務(wù)需求，編隊運動過程中應(yīng)盡可能地提高以任務(wù)隊形運動的路徑長度，同時考慮到盡可能快地抵達(dá)目標(biāo)點，應(yīng)減小編隊的路徑總長度。對此，可以建立下述多目標(biāo)規(guī)劃指標(biāo)：

路徑長度指標(biāo)J1：編隊路徑由一系列離散的路徑點構(gòu)成，記為Path={Pi,i=1,2,...,N}，其中Pi為表示第i個路徑點，N為路徑點的總數(shù)量，則路徑長度指標(biāo)可表示為

其中，Li為第i段路徑的長度，即線段PiPi+1的長度|PiPi+1|。

隊形保持指標(biāo)J2：編隊運動過程中，應(yīng)盡可能保持任務(wù)隊形，即盡可能增大采用期望隊形運動的路徑長度比例，或者盡可能減小采用縱向隊形實現(xiàn)障礙規(guī)避的路徑長度比例，因此，隊形保持指標(biāo)可表示為

式中，L為路徑總長度，LC表示以縱向隊形運動的路徑長度。

3 隊形可變編隊的路徑規(guī)劃算法

本節(jié)介紹基于 A*框架的隊形可變編隊路徑規(guī)劃算法（記為Variable-Shape Formation-A*，VSF-A*），包括算法總體流程、構(gòu)型空間生成和統(tǒng)一化代價函數(shù)構(gòu)建。

3.1 算法總體流程

A*搜索算法作為一種應(yīng)用廣泛的啟發(fā)式路徑規(guī)劃方法，具有效率高和適應(yīng)性強等優(yōu)點，而且在滿足可接納條件下能夠保證結(jié)果的最優(yōu)性。在A*的框架下，通過兩次膨脹建立包含硬障礙區(qū)和軟障礙區(qū)的構(gòu)型空間，并構(gòu)建統(tǒng)一化代價函數(shù)，以獲得多指標(biāo)權(quán)衡的路徑結(jié)果。

VSF-A*算法流程如圖4所示，節(jié)點選擇和擴展搜索流程與A*算法相似，具體流程不再贅述，下文主要對改進的構(gòu)型空間構(gòu)建方法和節(jié)點代價計算方法進行闡述。

圖4 VSF-A*算法流程圖Fig.4 Flowchart of VSF-A* algorithm

3.2 路徑規(guī)劃構(gòu)型空間生成

如2.3 節(jié)所述，基于真實環(huán)境地圖直接進行機器人路徑規(guī)劃時，需要考慮機器人的尺寸進行碰撞檢測，會降低規(guī)劃效率。一般地，在環(huán)境地圖中根據(jù)機器人尺寸對障礙區(qū)進行膨脹獲得新的環(huán)境地圖，在該環(huán)境下可以將機器人直接作為質(zhì)點進行路徑規(guī)劃，也稱為構(gòu)型空間。

針對隊形可變的編隊路徑規(guī)劃問題，本文提出通過兩次膨脹生成構(gòu)型空間的方法，如圖5所示。圖中黑色為實際障礙區(qū)域，灰色區(qū)域為根據(jù)個體尺寸進行膨脹得到的區(qū)域（稱為硬障礙區(qū)），斜線柵格部分為根據(jù)機器人編隊占據(jù)的空間尺寸膨脹得到的區(qū)域（稱為軟障礙區(qū)）。硬障礙區(qū)表示編隊按縱向隊形運動也無法通行的區(qū)域，軟障礙區(qū)則為編隊按任務(wù)隊形運動時無法通行的區(qū)域，但采用縱向隊形是可以通過的。硬障礙膨脹尺寸

圖5 考慮障礙二次膨脹的柵格地圖Fig.5 Grid map with twice obstacles inflation

如式(5)所示，軟障礙膨脹尺寸如式(6)所示，其中，Lobstrue、Wobstrue為實際障礙長寬，Lobshard、Wobshard為硬障礙膨脹長寬，Lobssoft、Wobssoft為軟障礙長寬。

3.3 統(tǒng)一代價函數(shù)構(gòu)建

為了實現(xiàn)隊形可變編隊的路徑規(guī)劃，綜合考慮路徑長度最小化和編隊保持的需求，構(gòu)建統(tǒng)一化的代價函數(shù)，如式(7)所示。

其中，n代表當(dāng)前搜索過程中的節(jié)點，f(n)為節(jié)點代價值；g(n)為累計代價，表示從起點到當(dāng)前點的路徑長度；h(n)為估計代價，表示從當(dāng)前點到目標(biāo)點的估計路徑長度；e(n)為穿越軟障礙代價，表示路徑經(jīng)過軟障礙區(qū)Osoft的長度；ω1表示路徑總長度的權(quán)重系數(shù)；ω2表示經(jīng)過軟障礙區(qū)的路徑長度權(quán)重系數(shù)。h(n)、g(n)和e(n)均使用歐式距離估計，如式（8）～（10）所示，其中{xend,yend}表示目標(biāo)點坐標(biāo)，nf表示節(jié)點n的父節(jié)點，{x,y}和{xf,yf}分別為當(dāng)前節(jié)點和父節(jié)點坐標(biāo)。

4 仿真試驗和結(jié)果分析

為了驗證本文方法的有效性和體現(xiàn)方法的優(yōu)勢，設(shè)定兩組場景進行不同指標(biāo)權(quán)重下的對比試驗，并開展隨機障礙環(huán)境下的魯棒性測試，大規(guī)模編隊不同任務(wù)隊形下的魯棒性。

4.1 試驗1：隊形保持無可行路徑的場景1

場景1 如圖5所示，機器人運動范圍為20 m ×10 m，柵格地圖規(guī)模為 200 × 100，即分辨率0.1 m，機器人尺寸為0.4 m，起點位于（6，60），終點位于（198，40）。環(huán)境中共有5 個障礙，大小分別為1 m × 3 m，1 m × 2 m，1.5 m × 1.5 m，0.5 m × 1.5 m，0.2 m × 0.8 m。任務(wù)編隊為3 個機器人組成的三角編隊，機器人間隔0.2 m，避障是可選擇縱向隊形。圖5中，黑色區(qū)域表示根據(jù)單個機器人尺寸（0.4 m）膨脹得到的硬障礙區(qū)，灰色區(qū)域表示根據(jù)三角編隊尺寸（1.6 m）膨脹得到的軟障礙區(qū)。

針對場景 1，若采用傳統(tǒng)的不允許隊形改變的方法，將無法生成可行路徑，而采用VSF-A*算法，可以獲得經(jīng)過軟障礙但不經(jīng)過硬障礙的路徑結(jié)果。圖6給出了權(quán)重ω2取不同值時的路徑規(guī)劃結(jié)果，具體的指標(biāo)結(jié)果如表1所示。由結(jié)果可知，當(dāng)權(quán)重ω2較小時，路徑通過軟障礙區(qū)的長度較大，而隨著權(quán)重ω2的增加，路徑經(jīng)過軟障礙區(qū)的長度減少，規(guī)劃結(jié)果更傾向于規(guī)避軟障礙區(qū)。但由于該場景下不存在繞開所有軟障礙區(qū)的可行路徑，因而權(quán)重很大時，規(guī)劃路徑依然會經(jīng)過軟障礙區(qū)。

圖6 場景1 不同權(quán)重編隊路徑規(guī)劃結(jié)果Fig.6 Planned formation path for scenario 1

表1 場景1 路徑規(guī)劃結(jié)果Table 1 Path planning results for scenario 1

針對場景1，圖7在若干離散路徑點處給出了機器人編隊在環(huán)境中的運動情況，其中三角隊形的方格表示機器人的實際尺寸。需要說明的是，考慮機器人尺寸的結(jié)果圖中，機器人方格與真實障礙（黑色區(qū)域）無交集，即表明機器人不會與障礙發(fā)生碰撞。當(dāng)規(guī)劃路徑在空白可通行區(qū)時，編隊可以采用三角任務(wù)隊形運動，而當(dāng)規(guī)劃路徑穿越軟障礙區(qū)時，編隊只能以縱向隊形運動，否則機器人將與障礙發(fā)生碰撞。通過上述仿真結(jié)果可知，編隊通過改變隊形具備了更好的狹小區(qū)域通行能力。因此，提出的VSF-A*算法能夠提高編隊路徑規(guī)劃方法在復(fù)雜環(huán)境下的適用能力。

圖7 針對場景1 規(guī)劃的編隊路徑Fig.7 Planned formation path for scenario 1

4.2 試驗2：隊形保持有可行路徑的場景2

場景2 如圖8所示，其中柵格地圖尺寸、起點、終點和場景1 相同，5 個障礙的大小不變，但位置重新生成，使得從起點到目標(biāo)點存在不通過軟障礙區(qū)域的可行路徑。

表2和圖8給出了VSF-A*取不同權(quán)重ω2下的編隊路徑規(guī)劃結(jié)果。根據(jù)結(jié)果可知，隨著權(quán)重ω2的增加，規(guī)劃路徑更傾向于繞行灰色軟障礙區(qū)，編隊經(jīng)過軟障礙區(qū)的路徑長度減小。當(dāng)權(quán)重ω2足夠大時，針對場景二的路徑規(guī)劃結(jié)果將完全不經(jīng)過軟障礙區(qū)，例如ω2取100 時的結(jié)果。另一方面，隨著權(quán)重ω2的增大，路徑的總長度呈增長的趨勢，即為了滿足隊形保持的要求，編隊繞行灰色軟障礙區(qū)導(dǎo)致從起點抵達(dá)目標(biāo)點的所需時間變長。

顱內(nèi)動脈瘤開顱夾閉術(shù)后發(fā)生硬膜下并發(fā)癥較為常見，發(fā)生率為1%～5.1%，且男性、年齡、顱內(nèi)動脈瘤部位、腦萎縮及抗血小板藥物的使用是動脈瘤開顱夾閉術(shù)后硬膜下積液或血腫發(fā)生的危險因素[5-9]。本研究結(jié)果表明，男性、高齡、動脈瘤部位(大腦中動脈瘤、多發(fā)動脈瘤)是動脈瘤開顱夾閉術(shù)后硬膜下積液發(fā)生的獨立危險因素(P<0.05)；硬膜下積液演變?yōu)槁杂材は卵[則與男性、高齡相關(guān)(P<0.05)。本研究結(jié)果還表明，硬膜下積液量越大，其演變?yōu)槁杂材は卵[的比例越高，演變?yōu)槁杂材は卵[后須行鉆孔引流手術(shù)治療的比例也越高。

圖8 場景2 不同權(quán)重編隊路徑規(guī)劃結(jié)果Fig.8 Planned formation path for scenario 2

表2 場景2 路徑規(guī)劃結(jié)果Table 2 Plath planning results for scenario 2

機器人編隊在典型路徑點處的位置分布情況如圖9所示。由圖可知，編隊沿權(quán)重ω2為0 時的規(guī)劃路徑運動時，因為穿越軟障礙區(qū)，需要通過隊形變換才能避免碰撞的安全通過。而沿權(quán)重足夠大，即ω2為100 的規(guī)劃路徑運動時，由于規(guī)劃路徑?jīng)]有經(jīng)過軟障礙區(qū)，因此編隊可以始終保持期望的三角隊形抵達(dá)目標(biāo)點。

圖9 針對場景2 規(guī)劃的編隊路徑Fig.9 Planned formation path for scenario 2

4.3 試驗3：100 次隨機場景測試

隨機測試實驗的100個場景中，柵格地圖尺寸、起點、終點、障礙大小均相同，但障礙位置每次隨機產(chǎn)生，且使得硬障礙區(qū)不包含起點和終點。

針對每個隨機產(chǎn)生的場景，利用VSF-A*算法進行路徑規(guī)劃，并設(shè)置不同的軟障礙路徑長度權(quán)重進行對比。規(guī)劃結(jié)果的路徑總長度、經(jīng)過軟障礙區(qū)長度、經(jīng)過軟障礙區(qū)長度占總長度比例的統(tǒng)計結(jié)果分別如圖10～12 所示。

圖10 100 次隨機場景下路徑總長度對比Fig.10 Comparison of total path length for 100 random scenes

圖11 100 次隨機場景下軟障礙穿越長度對比Fig.11 Length comparison of path passing through soft obstacles for 100 random scenes

圖12 100 次隨機場景下LC/L 值對比Fig.12 Comparison of LC/L for 100 random scenes

對100 次隨機測試的結(jié)果進行統(tǒng)計，權(quán)重ω2為0、5、10、100 時，路徑長度均值為20.34 m、20.35 m、20.4 m、21.46 m，穿越軟障礙長度均值為10.38 m、8.48 m、7.86 m、3.7 m，穿越軟障礙長度占路徑總長度的比例均值為 51.03%、41.67%、38.53%、17.24%。隨著權(quán)重ω2的增加，編隊經(jīng)過軟障礙區(qū)的路徑長度減小，而路徑總長度由于繞行軟障礙區(qū)而增加。針對100 次隨機生成的場景，VSF-A*算法都可以快速規(guī)劃得到編隊的可行路徑，驗證了該方法具有良好的魯棒性，能夠應(yīng)用于復(fù)雜障礙環(huán)境下的編隊路徑規(guī)劃。

4.4 試驗4：大規(guī)模編隊測試

場景3 如圖13所示，機器人運動范圍為200 m × 100 m，柵格地圖規(guī)模為2000×1000，即分辨率0.1 m，機器人尺寸為0.4 m。環(huán)境中共有10個障礙，大小分別為1 m × 2 m，1 m × 3 m，4 m ×3 m，5 m × 2 m，5 m × 5 m，5 m × 6 m，6 m × 7 m，7 m × 9 m，10 m × 30 m，20 m × 20 m。編隊規(guī)模由原先的3 個提升為12 個。

編隊首先以三角編隊從起點（1698，140）運動到終點1（460，800），再更換成菱形編隊從終點1 運動到終點2（1780，800），路徑規(guī)劃時可選擇縱向隊形通過狹窄區(qū)域。其中，硬障礙膨脹大小為機器人寬度（0.4 m），三角編隊軟障礙膨脹大小為三角編隊寬度（7 m），菱形編隊軟障礙膨脹大小為菱形編隊寬度（4 m）。

針對上述需求的編隊路徑規(guī)劃結(jié)果如圖13和圖14所示。結(jié)果表明，VSF-A*算法針對規(guī)模較大的編隊協(xié)同問題，VSF-A*算法同樣可以規(guī)劃得到編隊的可行路徑，表明該方法具有良好的適應(yīng)性。

圖13 場景3 中的三角編隊路徑Fig.13 Planned triangle formation path for scenario 3

圖14 場景3 中的菱形編隊路徑Fig.14 Planned diamond formation path for scenario 3

5 結(jié) 論

通過兩次障礙碰撞構(gòu)建包含硬障礙區(qū)和軟障礙區(qū)的移動機器人編隊路徑規(guī)劃構(gòu)型空間，并設(shè)計代價函數(shù)的計算方法，提出一種適用于隊形可變編隊路徑規(guī)劃的VSF-A*算法。仿真測試結(jié)果表明，VSF-A*算法能夠利用隊形可變的特點獲得復(fù)雜障礙環(huán)境下的編隊可行路徑，實際應(yīng)用中可以根據(jù)編隊對隊形保持和任務(wù)時間的偏好設(shè)定合適的權(quán)重，從而生成多指標(biāo)權(quán)衡的可行編隊路徑。