狹窄空間無人機動態三維A*算法研究

李兆強，張 拓

(西安建筑科技大學 信息與控制工程學院,西安 710055)

0 引言

艦船艙室發生火災時，被困人員需要及時逃離危險，在三維空間中，智能飛行器的引導成為關鍵。在設計智能飛行器時，快速尋找最優路徑和及時躲避動態和靜態障礙物的能力起著重要作用。而啟發式搜索算法A*及其改進算法以其快速高效的特點被廣泛使用于路徑規劃問題[1-7]。

在傳統A*算法基礎上，陳豪等引入了方向矢量和并行搜索，使機器人路徑搜索同時具備方向性和并行性[8]，未能拓展到三維空間；王殿君提出采用變步長搜索方法，同時采用實時調整權值的路徑評價函數，提升了優化效率和優化結果[9]，但是未考慮動態障礙物的影響；何雨楓提出在代價函數計算、開啟集更新方式等方面對傳統A*算法進行了改進[10]，但存在收斂速度較慢導致飛行器救援時間過長，易陷入局部最優的問題。

本文提出一種基于改進的三維局部A*路徑規劃算法。針對飛行器在局部規劃路徑時環境信息未知，存在冗余點和拐點，導致收斂時間長、路徑節點擴展代價大、易陷入局部最優問題，在代價函數權值分配，局部與全局路徑結合等方面對A*算法進行改進，并與文獻[10]進行了Matlab仿真與數據對比。

1 問題描述及環境設定

1.1 問題描述

在已知模型的艦船艙室內發生火災時，由于煙霧阻擾，通道內存在的靜態障礙物和火災帶來的動態障礙物(大火，掉落的殘渣物等)，被困人員無法快速安全地逃離危險區域，這使得智能飛行器引導被困人員逃離至安全地帶成為關鍵，這一條件使得無人機需要局部路徑規劃[11]。飛行器先獲取靜態障礙物信息，在傳感器感知范圍內未檢測到動態障礙物，可利用三維A*算法在已知的障礙之間尋找一條先驗路徑，由于飛行器對環境動態障礙物沒有先驗知識，如果飛行器在有限的感知范圍內，實時獲取到有關動態障礙物的信息，則利用本文提出使用改進三維A*局部搜索算法。

1.2 環境設定

無人機路徑規劃的初始階段是圍繞無人機建立基于圖形的結構或數字模型，并在沒有碰撞障礙物的情況下移動至目標。無人機的三維工作空間的長度，寬度和高度分別是L，W，H。工作區分為R×S×T個相同大小的網格。每個網格的信息由式(1)表示。整個工作空間的信息可以表示為：

φ=∑Nijk(i∈[1,R],j∈[1,S],k∈[1,T])

(1)

每個柵格的狀態信息具體的表示如式(2)：

(2)

其中：當Nijk值為0時，表示當前網格是自由空間，沒有障礙物。當Nijk值為1時，表示當前網格是障礙物。理論上，無人機飛行時有很多方向和細微的動作。然而，考慮到模型的復雜性，每個網格的中心被用作A*算法的計算單元，稱為“節點”。當障礙物包含在一定的網格尺寸范圍內時，相應的節點被視為障礙物節點，剩余的節點為自由節點。規定無人機在任何節點時，可上下、左右、相鄰和對角相鄰節點移動，即最多有26個運動方向。

2 A*傳統算法與局部算法

2.1 A*傳統算法

A*算法是最著名的路徑規劃算法之一，它結合了基于最短路徑的搜索和啟發式搜索[12]。 A*算法定義為最佳優先級算法，通過從起點到目標點的距離f(n)來評估配置空間中的每個單元：

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

(3)

A*算法的搜索思想是在搜索過程中，選擇所有f(n)小的點作為路徑候選點，構造路徑候選點集。搜索到了目標點后，從目標點開始，在路徑候選點集中向前搜索最佳路徑。

在式(3)中，節點n的評估函數的值由f(n)表示，

從單元到目標狀態的啟發式距離由h(n)表示[11]，通過所選單元序列從初始點到目標點的路徑長度表示為g(n)。如式(4)、(5)所示。

(4)

g(n)=|x2-x1|+|y2-y1|+|z2-z1|

(5)

A*算法的具體實現取決于兩個基本集合[13]：開放集和封閉集，記錄為OPEN集和CLOSED集。開放集用于存儲要檢查的節點的信息，封閉集用于存儲已選擇且不需要再次檢查的節點的信息。A*算法的路徑搜索過程是通過反復檢查和更新開放集來實現的。

在A*算法求解路徑規劃問題的過程中，評價函數的選擇尤為重要。由式(3)可知，評價函數由現在成本函數h(n)和過去成本函數g(n)組成。當g(n)權重為1，h(n)權重為0時，A*算法可視為Dijkstra算法，此時算法更偏向起點，是一種經典的最短路徑查找算法。當g(n)權重為0，h(n)權重為1時，A*算法可視為BFS算法。此時，算法的啟發性更強，更偏向靠近目標點的節點。合理配置和改進評價函數評價比，將使規劃的路徑更快、更平滑[13]。

2.2 A*局部算法

A *算法通過檢查、比較和探測節點來實現對最佳路徑的搜索。在已知先驗環境中沒有動態障礙的情況下，可以達到較好的規劃效果。但是，其本質為全局路徑搜索方法，在局部路徑規劃領域的應用受到限制。何雨楓等提出了其在未知動態障礙物環境下的局部路徑規劃算法。

在局部路徑規劃時，傳感器探測范圍與A*算法的環境信息要求之間相矛盾，為解決這一問題，假設傳感器檢測范圍從當前節點擴展到下一個節點，擴展集在三維空間中為26個節點，即a *算法的單步擴展的步長，無人機的運動和A*算法的擴展是同步執行的，直接將A*算法計算出的每個路徑點提供給無人機。當無人機位于新節點中時，機載傳感器會掃描該節點的周圍環境信息，并將其提供給a *算法。

圖1是局部規劃和全局規劃的流程圖。其中，圖1(a)是局部規劃的流程圖，圖1(b)是全局規劃的流程圖。

圖1 局部規劃和全局規劃的流程對比圖

在局部路徑規劃中，當無人機飛向新節點時，清除開啟集，在新節點上重新掃描當前節點周圍的環境信息，找到所有可擴展的節點，并將f(n)、h(n)和g(n)存儲在開啟集中，然后完成節點擴展。這一策略保證了無人機能夠應對臨時出現的火災或火災帶來的塌方等動態障礙物，解決了未知動態障礙物環境下的局部路徑規劃的問題，局部規劃算法程序流程如圖2所示。

圖2 局部路徑規劃算法程序流程

在局部路徑規劃過程中，文獻[10]對路徑規劃過程進行了細化，改進了成本函數的選擇和開起集的更新方法，但存在收斂速度慢，導致規劃算法耗時長、路徑擴展長、易陷入局部最優的問題。針對該問題，在評價函數的權值分配和路徑生成策略方面進行改進。

3 基于改進A*全局與局部結合的算法

3.1 改進評價函數的權值分配

由2.1可知，在路徑搜索過程中，g(n)和h(n)對路徑評估的影響不同，選擇適當的加權評估方法[14]，會讓路徑規劃更加迅速，更平滑，并消除路徑擴展的冗余點。本文建立式(6)的評估方法:

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

(6)

在式(6)中，α是到達當前節點的實際成本g(n)的權重，β為當前節點到目標節點的估計代價h(n)的權值。兩項權值系數之和為1，隨著實時節點的擴展，當g(n)的影響因子越來越小，h(n)的影響因子越來越大時，既能達到平衡啟發式的目的，又能優化路徑縮短尋路時間的目的，所以評價函數中α和β應滿足式(7)～(9)。

α+β=1

(7)

(8)

(9)

e-0.1n+(1-e-0.1n)=1

(10)

構建g(n)與h(n)的權重系數如式(10)所示。在式中，n為實時路徑擴展點，取值范圍為[1,N]，N為路徑擴展節點的個數。

f(n)=e-0.1n*g(n)+(1-e-0.1n)*h(n)

(11)

將式(11)引入評價函數中可得可調節權重比例的評價函數f(n)，但是由于擴展路徑點n為一個有限值，所以α不能接近極限值0，β不能接近極限值1，達不到權值分配的目的。本文引入自變量放大和比例縮減。

f(n)=e-0.1n×ε/N*g(n)+(1-e-0.1n×ε/N)*h(n)

(12)

在式(12)中，ε取值為遠大于N的數，此時，隨著路徑節點n的擴展，α的變化范圍由1變化為0，β的變化范圍由0變化為1，實現了實時動態調節權值系數的目的。

3.2 改進路徑生成策略

在文獻[10]中，當艦艙過于復雜，障礙物過多時，無人機在局部搜索過程中不能接近目標節點，甚至陷入局部最優，從而導致無人機無法完成任務。本文將改進路徑生成策略，進一步優化路徑。

在無人機開始階段，無人機傳感器模型在Matlab中顯示為1.45r為半徑的淡灰色球形區域，即為傳感器的掃描范圍，在無人機開始階段，掃描全局環境信息，默認按照全局規劃路徑進行節點擴展，同時掃描鄰域環境信息，若未檢測到鄰域內的火災或火災帶來的塌方等動態障礙物，則繼續按照全局路徑規劃路線飛行，若檢測到火災或火災帶來的塌方等動態障礙物，進行局部路徑規劃，直到在傳感器探測范圍內無動態障礙物時，完成局部規劃進而開始全局路徑規劃……當目標點出現在OPEN集中，則完成路徑規劃。這一策略保證了無人機不會在每一步擴展節點時重新規劃路徑，縮短了規劃時間，同時避免陷入局部最優的情況。

改進三維A*全局與局部結合算法部分的程序流程圖如圖3所示。

圖3 改進三維A*全局與局部結合算法程序流程

與文獻[10]中的算法相比，評價函數權值分配的改進能夠更快逃離初始點和接近目標點，減少了路徑規劃長度；路徑生成策略的改進，不會在每一步擴展節點時，重新規劃路徑，同時避免了陷入局部最優問題，在很大程度上縮短了路徑規劃時間。改進算法有效縮短了路徑規劃長度，提高了路徑規劃效率。

4 仿真分析

為了驗證改進算法的有效性，本文采用Matlabr2014(b)軟件平臺進行仿真，硬件平臺為Intel(R)Core(TM)i5-45703.20 GHzCPU，RAM8 GB。

本文將環境信息按照柵格法將空間分解成20*20*20個小方格，起始點目標點已標出，黑色實心點為無人機的空間模型，淡灰色球形區域為無人機傳感器的感知范圍，淺色柱狀物為靜態障礙物示例，深色柱狀物為火災或火災帶來的塌方等動態障礙物示例，環境空間模型如圖4所示。

圖4 環境空間模型

本實驗分為三部分:一部分為改進的評價函數權值分配在全局A*算法的仿真對比試驗；第二部分為驗證改進的路徑擴展策略算法，與文獻10的算法進行仿真對比；第三部分為在相同障礙物環境中，文獻10算法陷入局部最優時，使用本文提出的算法的仿真對比試驗。

4.1 實驗一改進評價函數權值分配

本文改進了動態調整A*算法評價函數的權重比例，原算法與改進后的算法在A*全局尋路算法中如圖4所示。圖5(a)，(b)分別為三維仿真的XY視圖，三維視圖。在圖5中，虛線為原A*三維全局規劃算法，實線為改進評價函數的權重比的算法，明顯可以看出，改進算法減少了冗余點，優化了路徑，改進評價函數的權重比算法優于原算法。

圖5 改進權重比例在全局A*算法的對比

在相同障礙物、起始點、目標點環境下，將兩種算法的子程序分別運行50次后，得到的相關參數平均值如表1所示。

表1 相關參數對比表

在表1中，兩種算法語句執行步數相差不大，但算法耗時縮短了2%，路徑長度縮短了7.7%。這是因為在全局路徑規劃中，每一步路徑擴展時，無人機的傳感器掃描耗費一定時間，但是路徑距離縮短彌補了時間上的不足。

4.2 實驗二改進路徑生成策略

本文引入了數據標簽，改進了路徑生成策略，避免了步步計算擴展路徑，在改進局部算法仿真實驗中，無人機默認使用全局規劃算法，如果在途中無人機的探測范圍內未檢測出火災或火災帶來的塌方等動態障礙物，則不用進行局部規劃，從而更快速到達目標點；如果在途中無人機的探測范圍內檢測出火災或火災帶來的塌方等動態障礙物，則進行局部規劃，避過動態障礙物后，重新規劃全局仿真，到達目標點。

本部分仿真分為兩種情況，第一種情況為在全局規劃中未檢測出動態障礙物，算法分析如下所示。

原三維局部A*算法仿真如圖6所示，圖6(a)，(b)分別為原算法三維仿真的XY視圖，三維視圖。提出的改進路徑生成策略的算法仿真如圖7所示。圖7(a)，(b)分別為改進算法三維仿真的XY視圖，三維視圖。

圖6 原算法仿真實驗結果

圖7 改進算法仿真實驗結果

兩種算法所的參數對比如表2所示。

表2 相關參數對比表

在表2中，相比原A*算法，在改進路徑生成策略算法中，語句執行步數有所提高，但是總規劃時間約提高了20%，路徑長度縮短了12.4%。這是因為全局與局部算法相結合，提高了算法的執行次數，但是無人機在默認按照全局路徑飛行過程中，未檢測到動態障礙物，總路徑規劃時間大大提高，縮短了路徑長度。

第二種情況為在全局規劃中檢測出動態障礙物，算法仿真分析如下所示。

原三維局部A*算法仿真如圖8所示，圖8(a)，(b)分別為原算法三維仿真的XY視圖，YZ視圖，XZ視圖，三維視圖。本文提出的改進路徑生成策略的算法仿真如圖8所示。圖9(a)，(b)分別為原算法三維仿真的XY視圖，YZ視圖，XZ視圖，三維視圖。

圖8 原算法仿真實驗結果

圖9 改進算法仿真實驗結果

兩種算法所的參數對比如表3所示。

表3 相關參數對比表

由表3可看出，語句執行步數多了50%，算法耗時比原文增加了3.6%，但是路徑代價縮短了7.4%。這是因為在默認的全局路徑規劃線路中出現了火災或火災帶來的塌方等動態障礙物，于是采用局部路徑擴展算法，避過動態障礙物后，重新規劃全局路徑，所以語句執行步數有所增加，在保證算法耗時無明顯增加的前提下，路徑代價縮短幅度更大。

4.3 實驗三局部最優對比

在復雜地圖中，原文局部A*算法易陷入局部最優情況，本文提出的改進路徑生成策略算法能夠避免陷入局部最優。圖10和圖11為原算法與改進算法仿真實驗圖。圖10(a)，(b)分別為原算法三維仿真的XY視圖，三維視圖。圖11(a)，(b)分別為原算法三維仿真的XY視圖，YZ視圖，XZ視圖，三維視圖。在圖10中，原A*算法由于地圖較為復雜，局部路徑擴展陷入局部最優，在相同地圖中，改進算法有效避免陷入局部最優，如圖11所示。

圖10 原算法仿真實驗結果

圖11 改進算法仿真實驗結果

兩種算法所的參數對比如表4所示。

表4 相關參數對比表

在表4中，在改進路徑生成策略算法中，采用全局與局部相結合的路徑擴展策略，改進算法耗時大大降低，路徑長度大幅縮短，在避過靜態和動態障礙物后，快速到達目標點，取得理想效果。

5 結束語

針對三維運動無人機在動態環境下進行局部規劃收斂時間長，路徑節點擴展代價大，易陷入局部最優問題，提出了一種基于全局與局部相結合的動態三維A*尋路算法，對評價函數的權值系數動態分配，路徑生成策略進行改進，有效提高了算法效率，實現了無人機在三維動態環境中的路徑規劃。

研究結果表明，提出的基于全局與局部相結合的動態三維A*尋路算法能夠有效減少路徑規劃時間，縮短路徑規劃距離，同時能夠避免由于地圖復雜而帶來的陷入局部最優問題，在避開靜態和動態障礙物前提下，能快速到達目標點，完成路徑規劃。