狹窄空間無人機動態(tài)三維A*算法研究

李兆強，張 拓

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

0 引言

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

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

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

1 問題描述及環(huán)境設定

1.1 問題描述

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

1.2 環(huán)境設定

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

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

(1)

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

(2)

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

2 A*傳統(tǒng)算法與局部算法

2.1 A*傳統(tǒng)算法

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

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

(3)

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

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

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

(4)

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

(5)

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

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

2.2 A*局部算法

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

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

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

圖1 局部規(guī)劃和全局規(guī)劃的流程對比圖

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

圖2 局部路徑規(guī)劃算法程序流程

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

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

3.1 改進評價函數(shù)的權值分配

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

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

(6)

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

α+β=1

(7)

(8)

(9)

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

(10)

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

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

(11)

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

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

(12)

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

3.2 改進路徑生成策略

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

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

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

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

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

4 仿真分析

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

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

圖4 環(huán)境空間模型

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

4.1 實驗一改進評價函數(shù)權值分配

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

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

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

表1 相關參數(shù)對比表

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

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

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

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

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

圖6 原算法仿真實驗結(jié)果

圖7 改進算法仿真實驗結(jié)果

兩種算法所的參數(shù)對比如表2所示。

表2 相關參數(shù)對比表

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

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

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

圖8 原算法仿真實驗結(jié)果

圖9 改進算法仿真實驗結(jié)果

兩種算法所的參數(shù)對比如表3所示。

表3 相關參數(shù)對比表

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

4.3 實驗三局部最優(yōu)對比

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

圖10 原算法仿真實驗結(jié)果

圖11 改進算法仿真實驗結(jié)果

兩種算法所的參數(shù)對比如表4所示。

表4 相關參數(shù)對比表

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

5 結(jié)束語

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

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