基于多層雙向A*的未知環境路徑規劃算法研究

杜婉茹 王瀟茵 賈福凱 鄭 重 李慧妍

(中國航天系統科學與工程研究院 北京 100048)

0 引 言

現代戰場環境錯綜復雜，在對敵方目標進行打擊的過程中，很多隱蔽的障礙無法通過高空手段預先探測得知；另外作戰環境通常不會一成不變，無人智能體前往目的地的行進過程中出現預料之外的障礙狀況時有發生，若不加處理，往往會影響智能體的作戰質量與作戰效率。在此背景下，如何迅速響應變化的戰場環境并做出決策，是對敵方目標進行有效打擊的重要保障環節。

路徑規劃問題[1]按照算法思路通常分為盲目搜索路徑規劃和啟發式搜索路徑規劃。路徑規劃要求智能體能夠依據一定的優化原則，找出一條從運動起點到終點且能夠規避障礙物的最優路徑。盲目搜索定義了固定的搜索策略，通常通過迭代進行全路徑搜索，是傳統的路徑規劃技術,因此效率很低,一般只適用于求解環境障礙單一的路徑規劃問題。啟發式搜索定義了評價函數，其中融合了所求解問題的相關指標信息，并將其加入搜索過程來指導路徑節點的選擇具有更優的搜索效率。

A*算法是典型的啟發式搜索算法，在目前研究中，該算法多用于環境已知的條件下，面對未知突發態障礙時該算法具有局限性，且當目標區域較大的情況下,此算法的收斂速度會明顯變慢，搜索過程往往會產生過多的冗余節點，并且可能會因為產生相同代價的節點，從而造成規劃路徑抖動現象。文獻[2]通過引入分區加權信息，更改障礙搜索矩陣的尺寸來獲得不同的安全間距，解決了不同機器人在不同地圖環境下的安全性問題。然而該算法在路徑長度上有所犧牲，在規劃過程中產生較多的冗余節點，在復雜環境中效率并未有較大的提升。文獻[3]提出一種動態離散勢場路徑規劃算法。使機器人在迷宮環境中快速、高效地規劃出一條折線少、轉折角度小的優化路徑。但是該算法的仿真場景基于迷宮模型設計，未考慮實際戰場環境的動態復雜性，其應用可行性有待提高。文獻[4]提出了一種基于方向約束的A*算法，提高了方向約束路徑規劃問題的求解能力和路徑質量。該算法的局限性在于其設計實現建立于環境障礙均已知，面對路徑中出現的未知障礙并未提出解決方案。文獻[5]采用了導引律控制的思想,通過法向加速度閾值約束轉彎半徑。因為導引律的設置，該方法只能處理無障礙物環境, 目前僅應用于高空飛行器或制導武器。

以上算法能夠一定程度地解決路徑規劃問題，但在實際應用中受環境制約影響較大，無法處理出現動態障礙的未知環境，且大規模復雜場景下收斂速度慢，會產生大量冗余節點，影響路徑規劃速度?；诖?，本文提出一種考慮拐角行進安全的多層雙向A*算法，引入分層策略，當驅使智能體行走遇到未知障礙時，將觸發下層搜索進行避障路徑重規劃；采用雙向搜索，為了保證雙向搜索在起點和目標點的中間位置附近重合，動態定義了啟發式搜索的目標節點，添加同步機制降低算法復雜度；改進啟發式代價函數減少冗余節點產生，加速了算法的收斂，保證路徑規劃時效性；最后，使用Hermite差值對所得路徑進行平滑處理，保證了智能體在尖銳拐角處的行進安全。

1 多層雙向A*算法框架

1.1 A*算法分析

A*算法[6]是一種典型的啟發式搜索算法， 其結合了Dijkstra 和BFS 兩種算法各自的優勢，在保證搜索效率的同時，可以得到最優的路徑規劃。A*的核心思想體現在綜合考慮了起始點到當前節點的真實代價和當前節點到終點的估計代價，因此可以引導搜索方向。A*算法的代價函數為：

f(n)=g(n)+h(n)

(1)

式中：f(n)為經過當前節點n的全局評估代價值；g(n)為起始節點按照當前選擇的路徑行進至當前節點n真實代價值；h(n)為當前節點n到目標節點的估計代價。

基本思路為：以起點為第一個計算點，計算其八鄰域中每個節點的代價值f(n)，若某個節點被占用即為障礙物則不入棧。然后取其中f(n)值最小的節點作為下一個計算點，并存儲其父節點，直到搜索到終點。最后從終點追溯其父節點獲取規劃路徑。

傳統的A*算法只能在環境障礙已知的條件下正常應用，在實際使用場景下，任何阻礙前進道路的障礙均會造成作戰終止。并且在應用于作戰場景時沒有考慮周圍障礙物的不規則形狀對避障的影響、未知障礙及行進安全性等因素，在面對地圖很大的場景時,此算法搜索過程往往會產生過多的冗余節點，從而導致收斂速度會明顯變慢，路徑規劃的效率將大幅下降。本文針對以上缺陷提出一種考慮拐角行進安全的多層雙向A*路徑規劃算法。

1.2 改進啟發函數的多層雙向A*算法設計

考慮到實際作戰場景的不確定性，對突發狀況(前進道路產生未知障礙)添加應對方案，采用分層策略，當驅使智能體沿規劃路徑行進過程中遇到障礙時，觸發下層搜索，完成有效避障路徑重規劃；針對算法在大規模復雜未知環境中因冗余節點過多而產生計算效率低下的問題，改進算法單向計算過程為雙向計算，降低算法復雜度，同時，為了保證雙向搜索在理論中點位置附近的重合，提高算法收斂時效，動態定義了啟發式搜索的目標節點，添加同步機制。

為了提高算法在大規模環境下的工作效率，對當前節點到目標節點的代價估計函數進行改進：以分區加權方式來表示距離信息，避免相鄰節代價相同而造成的路徑不確定性，在路徑規劃中產生抖動現象；以叉積距離來表示角度信息[7]，設計新的啟發函數。智能體按改進的啟發函數可以提高在復雜環境的路徑規劃效率，縮短遇到動態障礙的再次規劃耗費的時間。

新的代價估計函數定義如下：

(2)

式中：

X1=|n.x-goal.x|

Y1=|n.y-goal.y|

X2=|start.x-goal.x|

Y2=|start.y-goal.y|

式中：n為當前的節點，goal為目標節點，x、y分別時節點的行列號，p、q、w是權值。

在原始的啟發函數中，g(n)與h(n)共同決定了代價值f(n)，因此g(n)和h(n)必須要保持量綱的統一，保證真實代價與估計代價的同等重要性。同理，角度信息和距離信息需要保持量綱統一，才能保證其貢獻量一致。

算法框架圖如圖1所示。

圖1 改進的多層雙向A*算法框架

首先，將改進啟發函數及添加同步機制的雙向A*算法與初始環境數據加載至智能體，智能體根據環境信息規劃出一條行進路徑，計入路徑集P。按照該路徑行進，并實時探測行進中是否出現未知障礙，當未知障礙發生時，智能體對障礙進行探測并確定位置及形狀，觸發下層搜索并同時更新環境數據庫信息。以當前節點為起始點規劃新的避障路徑，并同樣計入路徑集P。當智能體成功到達終點時，將路徑集合P中的子路徑合并，獲得最終PATH。

2 算法實現

2.1 改進啟發函數的同步雙向A*算法

雙向A*算法指搜索沿著正逆兩個方向進行，正向是指從起點開始向目標點行進的路徑搜索方向，反之為逆向。正向與逆向搜索過程交替進行。當正向和逆向搜索路徑相遇，且該節點滿足兩個方向搜索的約束條件時，搜索結束。該算法在理想狀態下，正向搜索與逆向搜索會在起始節點與目標節點的連線中心相遇。然而，由于實際作戰環境中障礙物復雜多變，正向與逆向搜索通常不在中心點相遇，極端情況下，A*搜索代價將類似于盲目搜索，效率大大降低。所以確保搜索在起始與目標點連線中點位置附近相遇是保證算法收斂效率的重要環節。

針對傳統雙向A*算法在實際作戰場景中應用產生的局限性，提出同步雙向A*算法，為了保證雙向搜索在理論中點位置附近的相遇，將兩個方向上的當前節點的前一個節點作為兩個方向搜索的目標節點。動態定義了啟發式搜索的目標節點，添加同步機制降低算法復雜度，加速了算法的收斂，保證正反兩個方向的搜索同時進行。

為了避免相鄰節點代價相同而造成的路徑不確定性在路徑規劃中產生抖動現象，即當前節點要將其周圍的代價相同的節點均遍歷之后才能進入下一節點狀態的現象，提高算法在大規模環境下的工作效率，對當前節點到目標節點的代價估計函數h(n)進行改進，以分區加權方式來表示距離信息；以叉積距離來表示角度信息[7]，加入距離信息和角度信息設計新的啟發函數：

F(n)=G(n)+H(n)

(3)

F(n)為經過當前節點n的全局評估代價值。

圖2 真實代價值計算示意圖

H(n)為添加了距離信息與角度信息的改進代價函數，用于表示當前節點到目標節點的估計代價。該函數的改進能夠有效減少當某些路徑具有相同的F值時均被搜索，從而產生大量冗余搜索節點出現的路徑抖動現象，公式如下：

H(n)=p×X+q×Y+w×|X×Y-X′×Y′|

(4)

式中：(p×X+q×Y)為路徑信息，在曼哈頓距離的計算基礎加入權值，進行一定的方向引導；X為當前節點與目標節點的橫向距離，即X=abs(n.x-goal.x)；Y為當前節點與目標節點的縱向距離，即Y=abs(n.y-goal.y)；(w×|X×Y-X′×Y′|)為角度信息；X′、Y′為起始節點與目標節點的橫向、縱向距離。

智能體按改進的啟發函數可以提高在復雜環境的路徑規劃效率，縮短遇到動態障礙的再次規劃耗費的時間，具體算法步驟如下：

Step1維護兩個Open_list及Close_list，將起始節點和目標節點分別加入Open_list1 和Open_list2。

Step2判斷兩個Open_list是否為空，若為空。則搜索失??；若不為空，則轉入Step3。

Step3彈出兩個Open_list中F值最小的節點，分別放入Close_list中。

Step4若該方向的當前節點的相鄰節點中，被選為下一輪搜索的節點，與相對方向上的當前搜索節點為同一節點，則搜索成功。

Step5若Open_list長度不再改變，無節點參與擴展，則轉Step2。

Step6選取當前節點鄰域的所有子節點，并且判斷其合法性，對比其代價函數，確定最小F值的節點為下一節點，將合法的節點放對應的Open_list中。

Step7修改啟發式搜索的目標節點，改為相對方向上的當前節點的前一個節點，轉Step2。

2.2 A*算法的未知障礙應對方法——分層策略

傳統A*算法的路徑規劃實現需要建立在環境與障礙物已知的條件下，但在實際的作戰場景中，環境通常不會一成不變，突發狀況會引發未知的障礙，當未知障礙橫亙于先前規劃的路線上，會導致智能體無法完成既定任務，為了解決A*算法無法繞過未知障礙物問題，增強算法應用的健壯性，提出A*算法的未知障礙的應對方案——分層策略。

首先根據現有的已知環境信息利用改進啟發函數的同步雙向A*算法，規劃出一條從起點A到終點S最優路徑l1。其次，驅使智能體按照該路徑行進，并實時探測行進路線的環境。若在行進路徑中遇到未知障礙阻攔，則探測該障礙物的位置及形狀信息，并實時更新環境數據庫。之后，將智能體當前位置B作為新的路徑起點，調用改進啟發函數的同步雙向A*算法規劃出從B點到S點的最優路徑l2，以此類推，直至智能體成功抵達目標位置。此時，將路徑集合l={l1,l2,…,ln}進行合并，即可得到最終的從A到S的規劃路徑PATH。

2.3 規劃路徑平滑處理

由于算法理論規則為在下一個探索方向存在障礙區時轉換探索方向，因此規劃所得路徑中免不了會存在較為尖銳的拐角，這對實際行進的智能體會產生較大威脅。例如無人機的飛行轉彎，通過機身傾斜產生空氣動力差側向力，從而實現飛機方向的更改，傾斜角度過大會造成機體的失速失控，很容易與障礙物產生刮蹭，智能車同理。所以帶有尖銳拐角的路徑無法滿足實際的飛行需求，會對智能體實際行進安全產生較大威脅，因此需要在繞開障礙物時選擇更為安全的行進角度。

Hermite曲線[8]為利用Hermite插值從一個點到另一個點產生的一個三次多項式，在坐標系中曲線表示平滑，符合現實環境里物體的運行軌跡，因此使用Hermite插值對規劃出的路徑進行處理，最終得出實際路徑軌跡，保證智能體的安全行進。

在函數值與一階導數給定的情況下，n個節點x1,x2,…,xn的Hermite插值多項式的表達式如下：

(5)

式中：

yi=y(xi)

2.4 面向未知環境的多層雙向A*算法

將上述機制結合，提出一種改進的多層雙向A*算法，能夠在解決因未知障礙導致路徑規劃崩潰問題，增強算法健壯性的同時能夠有效減少搜索過程產生的冗余節點數量，提高算法在大規模復雜環境中路徑規劃的效率，在計算所得理論路徑的基礎上，使用Hermite插值得出滿足實際行進的最終軌跡，保證了智能體的行進安全。

改進后的面向未知環境的多層雙向A*算法流程如圖3所示。

圖3 面向復雜未知環境的多層雙向A*算法流程圖

3 實 驗

3.1 作戰場景建模方法

路徑規劃依賴于地圖，建立環境模型是路徑規劃的前提。軍事作戰的實際場景是一個復雜且龐大的環境系統，障礙物形狀各異，若單純地將其轉化為理想的圓形或矩形，在應用算法時會產生路徑冗余、高能耗等現象?？紤]算法在實際環境中的普適性，需要在建模過程中對不規則障礙物進行處理，保證智能體在躲避障礙物時的安全性。

(1) 不規則形狀障礙物的規則建模。為了解決算法在實際環境中，路徑規劃因不規則形狀障礙物而產生的路徑冗余現象的發生，特別是U型障礙物等凹邊形，需要引入對不規則障礙物的規則化處理[12]，步驟如下：

Step1對不規則障礙物凹凸點進行識別，記錄個數cnt及凹點坐標pos。

Step2若cnt為0，則記錄凸點坐標轉向Step4，否則轉Step3。

Step3連接凸點，將障礙物凸形化，重復Step1。

Step4結束。

不規則形狀障礙物凸形化處理效果如圖4所示。

圖4 不規則形狀障礙凸型化效果

(2) 柵格環境建模。應用A*算法時，為了更好地獲取智能體的前進方向，并且盡可能減少智能體在避障拐角處與障礙物的剮蹭，考慮使用添加智能體大小指標的柵格法將實際場景進行建模。柵格法[9-11]可用于有限運行區域進行建模，在柵格建模中，柵格粒度的大小直接影響著柵格法的效率。該建模方法數學描述如下。

設作戰區域為A，智能體的可行區域記為B，障礙物集合記為O，O∈A,O∈B。實際環境中的柵格記為g(a,b),其中a為g所在的水平橫坐標(a=1,2,…,n)，b為g所在的水平縱坐標(b=1,2,…,n)，對實際環境場景進行采樣，將采樣的環境信息記為G(g1,g2,…,gi)，i=1,2,…,n，其中n為采樣序號。在柵格法的建模中，柵格的寬度δ與智能體的長寬有關，實際環境坐標與柵格坐標存在以下關系約束：

a=int(x/δ)

(6)

b=int(y/δ)

(7)

依照上述柵格理論，便可以將實際環境中的坐標(x,y)映射為柵格建模后的坐標g(a,b)∈O∈A，基于此，我們即可建立相應的作戰場景的柵格模型。

在使用柵格環境建模法對該作戰場景進行建模時，為了保證智能體在行進過程中不刮蹭拐角處障礙，將柵格寬度定義為智能體寬度的5倍進行坐標映射。

根據上述不規則障礙凸型化處理結果，按照上述環境建模理論，將其映射至柵格環境中，如圖5所示。

圖5 不規則形狀障礙柵格化映射

最終定義實驗案例的場景建模結果如圖6所示。

圖6 使用柵格化建模后的作戰場景

3.2 實驗結果

編程實現面向未知環境的多層雙向A*算法。當智能體行進中遇到障礙，將觸發算法的下層搜索，規劃新的避障路徑。

在該案例中，初始的驅動路徑Path1經過了四段未知障礙覆蓋區，如圖7(a)中禁行符號所示。驅動智能體按Path1行進，圖7(b)中實線為智能體實際行走路線，當遇到未知障礙時，觸發下層搜索，產生第二次避障路徑Path2，即虛線所示?？梢钥闯?，Path2仍將通過兩段未知障礙覆蓋區。同理，當行進過程再次遇到障礙時觸發重規劃，得出Path3，如圖7(c)所示。以此類推，直到智能體成功行進至目標點。圖中：虛線為路徑規劃結果，實線為智能體實際行走路線，禁行標志為路徑中所遇為未知障礙的位置。

圖7 四次路徑重規劃示意圖

最終，將實驗結果所得的四條分路徑Path1-Path4進行合并，得出最終智能體行進的避障路徑，如圖8所示。

圖8 面向未知環境的多層雙向A*算法路徑規劃結果

實驗數據統計如表1所示。

表1 實驗結果統計

3.3 實驗結果分析及對比

實驗結果表明，相較于傳統的A*算法及目前研究現狀中一些改進的路徑規劃算法，本文算法在應用場景適應性、算法健壯性及計算效率方面均有改進，具體統計如表2所示。

表2 本文算法與現有算法的改進對比

在實驗仿真環境中，編碼復現了文獻[13]中所提的路徑規劃算法，將彈、壓棧節點數、最終PATH的節點數以及算法收斂時間三個指標數據進行統計，實驗結果如表3所示。

表3 本文算法與現有算法的實驗結果數據對比

為了驗證該改進算法在大規模復雜環境中的路徑規劃效果，分別在地圖信息為100×100，200×200，300×300三種規模的環境模型中進行實驗，隨機生成不同形狀的障礙物，進行實驗結果統計，結果如下：

表4 不同地圖規模中本文算法與文獻[13]算法實驗結果對比

結果表明，本文提出的面向未知環境的多層雙向A*算法相對于傳統及現有改進的路徑規劃算法，在相同的實驗環境中，彈、壓棧節點總數、最終規劃路徑長度、算法計算耗時三個指標實驗結果均有提升。統計可知，實驗過程處理冗余節點數減少12.9%，總用時縮短了17.4%，極大提高了路徑規劃的效率。在復雜未知的環境中，隨著地圖規模的增大，本文算法對于避障路徑的規劃效率提升得到更好的驗證。

4 結 語

針對未知戰場環境打擊敵方目標的路徑規劃問題提出一種考慮拐角行進安全的多層雙向A*算法。該算法突破了對環境已知的依賴，能夠使智能體在行進過程中實時響應突發障礙，進行有效的避障路徑重規劃；采用同步雙向搜索，動態定義了搜索方向上的目標節點，保證搜索在起點和目標點的中間位置附近重合；改進啟發式代價函數，加入位置及角度信息，避免多個代價相同節點造成的搜索節點冗余，提高算法在大規模環境下的工作效率；使用Hermite差值對所得路徑進行平滑處理，保證了智能體在拐角的行進安全。

實驗環境建模考慮了實際環境中路徑規劃因不規則形狀障礙物而產生的路徑冗余現象的發生，結果表明，與現有路徑規劃算法相比，本文提出的算法通過分層策略解決了未知環境出現動態障礙的路徑規劃問題，通過對啟發函數及同步雙向機制的改進，使得算法收斂速度提升17.4%，產生冗余節點數減少12.9%。使用Hermite差值對路徑進行平滑處理，能夠驅使智能體以更加安全的角度通過拐角，躲避障礙。同時，在不同規模的地圖環境下開展的實驗結果表明，該算法的路徑規劃準確度和效率均有較大提高。

未來的工作將結合實際三維戰場環境，考慮時效性、隱蔽性及能耗等多要素，實現最優路徑的綜合規劃技術，并考慮加入多智能體，通過多智能體協同工作，進一步提高大規模復雜環境的路徑規劃效率。