一種能有效規避威脅區的雙層A路徑規劃算法*

(1.華中科技大學自動化學院多譜信息處理技術國防科技重點實驗室 武漢 430074)

(2.中國運載火箭技術研究院 北京 100076)

一種能有效規避威脅區的雙層A路徑規劃算法*

陽志如1陸宏澤2周成平1

(1.華中科技大學自動化學院多譜信息處理技術國防科技重點實驗室 武漢 430074)

(2.中國運載火箭技術研究院 北京 100076)

針對路徑規劃中A*算法遇到威脅區易陷入局部搜索的問題,對擴展點的估計代價計算方式進行了改進,提出了一種基于A*的雙層A*規劃算法。在該算法的雙層機制中,第一層規劃的擴展點估計代價用第二層規劃的結果來計算,使得搜索過程中擴展結點的估計代價更接近于真實代價,從而得到該結點更加準確的全代價值,引導算法向更合適的方向擴展,提高了搜索效率。實驗表明：在較復雜的規劃空間中,該算法能有效解決A*算法遇到威脅區陷入局部搜索的弊病。

A*算法;路徑規劃;威脅區;雙層A*

ClassNumberU666

1 引言

在路徑規劃問題[1]中,適宜的路徑搜索算法在沒有人工干預的條件下,能夠自動、有效地完成路徑規劃任務,使運動體能有效地規避威脅、高效地到達目的地。常用的規劃算法有A*搜索算法、動態規劃法、Dijkstra算法、遺傳算法、粒子群算法、數學規劃等。所有這些算法中,A*搜索算法由于計算簡單、算法易實現,并且在理論上可以保證全局最優解的收斂性,因此得到了廣泛的應用。在實際規劃任務的約束和需求中,自動和有效地規避威脅區,是A*算法規劃的難點之一,它在搜索過程中遇到威脅區易陷入局部搜索問題[3]。

針對A*算法規避威脅區的問題,Khammapun Khantanapoka[4]、Basem M.ElHalawany[5]等提出了基于格網空間的A*規劃方法,該類方法能有效威脅建模與裁剪空間,從而更好地規避威脅,但是不能有效結合運動體的運動約束條件;李季[6]、李春華[7]等提出了改進的A*規劃算法,該類方法結合飛行器簡化運動學方程,更全面地考慮到了飛行器的約束,能有效削減搜索空間,實現自動的威脅回避,但威脅建模只考慮了簡單的威脅源的情況,不能解決遇到復雜的威脅區而陷入局部搜索的問題。嚴江江[8]、劉新[9]等提出了一種導引點集的策略,在威脅區比較密集的地方,用導引點來引導飛行器有效避開威脅區,該方法減小了局部搜索,大大減少了搜索時間,但導引點集的設置沒有統一標準,過于依賴人的經驗,沒有實現自動威脅規避。針對以上情況提出了雙層A*規劃算法。雙層A*的機制是：第一層是標準A*流程,根據代價值引導擴展點去生成子結點；第二層是利用A*的擴展機制,尋找一條從當前子結點到目標點的近似路徑,將該路徑代價作為該子結點的估計代價。這種計算方式能引導算法更高效地進行擴展。經仿真,證明了算法能有效解決A*算法遇到威脅區陷入局部搜索的弊病,更好地規避威脅。

2 A*算法的基本原理

A*算法是一種典型的啟發式圖搜索算法[10]。A*算法本身的特性是：在狀態空間中進行搜索時,它對每一個搜索的位置利用啟發式信息(即代價)進行評估,得到當前最好的位置。每次都選擇最好位置進行下一步擴展,直到到達目標(或稱找到問題的解),從而省略一些無謂的搜索,提高搜索效率。A*算法能夠利用擴展策略,將約束條件、任務需求與搜索過程緊密結合,能在有限的可選狀態中,選擇最優解,并且在理論上滿足全局最優解的收斂性[11]。

2.1 擴展策略

路徑規劃的研究對象包括飛行器、水面艦艇、地面車輛以及機器人等。考慮一般運動體的約束條件：最小/最大轉彎角度、最小/最大轉彎半徑、最小/最大轉彎弧長、距威脅區的安全距離。根據這些約束條件,將A*擴展策略與之緊密結合。同時考慮到最優性和高效性兩個特點：最優性指的是擴展策略能夠充分覆蓋包含最優路徑的解空間；高效性指的是擴展策略能夠利用約束條件有效裁剪空間,提高搜索效率,降低計算的時空復雜度[12]。基于以上兩個特點,結合約束條件來制定擴展策略。圖1是擴展策略示意圖。

圖1 結點擴展示意圖

A*的擴展區域S是運動體作一次機動能到達的區域。如圖1所示,P是當前擴展點,直線L1代表P點的當前運動方向,顯然S是以L1為中心對稱。R為運動體最小的轉彎半徑,L2是沿最小半徑機動時的擴展弧線,L3是最小弧長擴展線,L4是最大弧長擴展線,L5是最小轉彎角的擴展線,L6是最大轉彎角的擴展線。S的近端由最小轉彎半徑和最小弧長限定,遠端由最大弧長限定；左右兩邊由最大轉彎角限定,考慮到最小轉彎角а內是不可到達的,則L5將圖示а角范圍限定在可達區域外。由此可見,擴展區域S是由L1～L6這幾條弧或線包絡圍成的兩片對稱的區域組成,擴展點是通過網格劃分在S中選擇點來進行的。

圖1中Length為長度步長,θ為角度步長。劃分格網時,具體做法是在S內,過P點作偏離L1的角度為K倍θ的一系列直線和以P為圓心作半徑為K倍Length長度的一系列圓弧,其中,K為1～n的常數。用所作的直線和圓弧對網格進行劃分,把網格交點作為P的擴展子結點。

若M為當前點P的坐標點擴展子結點,則其坐標值與運動方向MFly的計算公式如下所示：

(1)

MFly=PFly+2ξ

(2)

其中L為MP連線的距離,ξ為MP連線與L1的夾角,逆時針為正,順時針為負。

2.2 路徑評價與擴展基本流程

路徑規劃本質上是最優路線問題,以f(n)表示從出發點V到目標點T的路徑上的代價值,則路徑規劃問題為找到V到T的路徑,使代價函數最小。

對A*算法來說,其關鍵在于設計出一個如下形式的合適的啟發函數,這個函數即代價函數。

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

(3)

g(n)是從起始節點到當前節點n的代價,h(n)為從當前節點n到目標點的實際代價。通過比較f(n),選擇合適的結點進行擴展[13]。實際情況中,因為從當前節點到目標點的代價無法計算,一般是用估計代價h*(n)代替h(n),則相應的代價為f*(n)。

理論上,為了收斂到全局最優解,必須滿足收斂條件[14]：

h*(n)≤h(n)

(4)

在本文中,代價只考慮距離長度,所以式(4)可以具體化為

(5)

其中li為實際的第i段實際的已規劃的路徑。li的轉彎半徑為Ri,第i個點的Mi坐標為Mi(MiX,MiY,MiFly)。li是由Mi和M(i+1)組成。

(6)

(7)

在擴展過程中創建兩個表：OPEN表和CLOSE表,OPEN表保存所有已經生成但未被擴展的點,CLOSE表記錄已經被擴展的點。

它實現的步聚是：

1)將起點置入OPEN,然后利用擴展策略生成與之相關的子結點；

2)對每一個可行子結點都計算相應的代價f*(n),依次置入OPEN,并選出最小值的點作為新的擴展點,并將其置入CLOSE表;

3)如果該結點是目標點,就結束搜索,轉4),否則對新的擴展點進行擴展,并進入步驟2);

4)從目標點回溯記錄最短路徑,直到出發點。

3 改進的雙層機制的A*算法

本文的算法是在二維空間中進行搜索。設(x,y)為規劃空間某一點的坐標,則規劃空間可以表示為集合{(x,y)|0≤x≤MaxX,0≤y≤MaxY},它代表了一個空間區域。在實際規劃過程中,還需要將該空間區域離散化,即將x,y分別按不同的分辨率進行劃分,從而得到離散化的規劃空間。

結合具體環境,擴展過程中綜合考慮的約束條件一般主要有威脅區約束、運動體約束。

3.1 威脅區模型

二維區域的威脅區都具有幾何形狀,威脅區都是一個二維的幾何圖形。因此,本文利用多個點組成的多邊形來近似建模。這種建模方法雖然一定程度上擴大了威脅區區域,但大大簡化了威脅區的表達,能提高效率；另一方面,用多邊形建模的方法靈活多變,能表示各種不同的形狀信息,使仿真環境更貼近于實際環境,從而提高規劃的準確性。

威脅區可以采用的多邊形描述為n個點的集合S(P1,P2,…,Pn),Pn表示組成多邊形的第n個坐標點,如圖2所示,虛線是實際的障礙物模型,用多邊形直線進行擬合。

圖2 多邊形擬合實際的威脅區示意圖

由此可見,用這種方法能較好地貼合實際形狀,而且,如果坐標點越多的話,越能與實際形狀相一致。這種威脅區的建模方法,簡單直觀,大大簡化計算,給規劃、編程都帶來便利。

3.2 改進思路

在A*規劃的規避威脅處理上,要求對所有的威脅均作規避,即要求規劃路徑滿足代價上最優的同時,不能穿越任何威脅區,并將穿越威脅區的點定義為無效點。

為了滿足式(4)的全局收斂條件,常規A*的做法是以當前點P到目標點T的直線距離作為啟發函數的估計代價項h*(n)。這種做法簡單、易于實現,但由于估計代價偏小,導致局部搜索區內(如圖3所示)的點被優先擴展,但這些點的子結點又容易落在威脅區內,成為無效點,致使當前路徑搜索失敗。此情況下,規劃極易陷入局部搜索,難以跳出,耗時太大,存在很多的無效點的搜索,搜索效率太低。

圖3 大面積遮擋示意圖

為了更有效地利用環境信息,得到擴展點更準確的代價信息,從而避免在一些遮擋面積較大的威脅區前大量無效點的擴展,更快地跳出局部搜索,節省規劃時間。在A*算法的基礎上進行改進時,必然要對當前擴展點能否通過威脅區作一個判斷,把不能通過的點排除在可行點之外；另一方面是要對估計代價的計算方式進行改進。

改進方法是對于第一層擴展點生成的子結點,先判斷其能否規避當前威脅區,能規避的點再用第二層A*去計算估計代價值h*(n)。第二層A*的結果是一條由威脅區切點組成的估計路徑折線,用這個折線去評估能否規避威脅區,同時,去更準確估算代價。

3.3雙層A*規劃

判斷當前點P能否規避當前威脅區M時,如圖4所示,每個威脅區都具有左、右兩個可以規避的方向,過P作M的左、右兩個切點S、S′,所得切線與PT連線的兩個夾角定義為左切角α和右切角β,設置一種機制：給α、β設置一個角度的上限閾值λ,若α或β滿足小于或等于λ的條件,則認為當前點P可以通過M的相應方向；否則,認為此方向不可行。兩個方向均不可行的擴展點則不能規避當前威脅區。

設置威脅區的角度的上限閾值λ的機制,如圖4所示,所有左、右兩邊切角等于λ點的集合組成的軌跡是兩段圓弧,數學意義上,表示ST、S′T弧分別在相應圓上對應的圓周角等于λ。定義兩段圓弧與威脅區共同圍起來的這個區域為威脅區的局部擴展區。顯然,落在局部擴展區內的子結點的切角均大于相應的角度閾值λ。且λ越大局部擴展區越小,大λ對應的局部擴展區包含在相應的小λ的局部擴展區內。

圖4 威脅區擴展示意圖

這種方法中威脅區的范圍有所擴大,人為地把落在局部擴展區的點等效為無效點,把易陷入局部搜索的區域排除在可達區域之外。這樣做的優點是：

1)這種方法能避免一些無效擴展,更快地跳出局部搜索,啟發的路徑能更好地規避威脅區。

2)另一方面這種方法得到的擬合折線在可行性上面是有保障的,不會出現很短的距離作很大拐彎的現象,進一步提高估計代價的準確度。

顯然,角度閾值λ的大小決定著局部擴展區的大小。也可以設置λ的不同數值,由此控制局部擴展區的范圍的大小,調整擴展和規劃速度。λ越小,局部擴展區的范圍就越大,可行點的區域就會變小,擴展的速度相對來說會更快。但如果λ過小,導致局部擴展區過大,可行點的選擇范圍被大大壓縮,就會影響路徑的全局最優性。λ越大,被排除在可行區域之外的局部擴展區就越小,可行點的范圍就越大,路徑的全局最優性就更能得到保障。λ的值的范圍設置在0°～90°之間,為了更好引導運動體從兩邊規避威脅區,產生更加平滑的路徑,λ不宜超過60°。

圖5 第二層A*示意圖

若折線規劃到達目標點,且總共有2N個點,則P-S1-P1-S2-P2-…-SN-PN-T即為當前點P的預估折線。利用這條預估折線對當前點P的預估代價進行計算。

(8)

3.4第二層A*流程

第二層中跟第一層擴展一樣,也創建功能相似的兩個表：D-OPEN表和D-CLOSE表。

子結點P的第二層A*的過程如下：

1)置P入D-OPEN表中；

2)若D-OPEN表為空,則規劃失敗；否則從D-OPEN表中取出代價最小的點S；

3)求S到目標點T之間的連線,按順序記錄此連線穿行的威脅區,若沒有穿過威脅區,置目標點入D-CLOSE表,規劃成功,并轉第5)步,否則轉下一步；

5)規劃成功,由目標點回溯,得到當前P點的預估折線路徑。

4 實驗結果

本算法在CPU為Core E7200 2.53GHz、內存2.0GB的PC機上進行了實驗,運行環境為Windows XP,編程環境為VS2005。實驗使用了分辨率為30*30、大小為900km×350km的地圖和模擬生成的威脅區數據,仿真的基本參數如下：

運動體的轉彎角度范圍為10°～45°,起始點到目標點的直線距離為740km。其中,擴展的最小半徑為100km,最小弧長為70km,最大弧長為250km。角度步長為5°,擴展步長為25km,安全距離為30km。

在同樣的環境下,利用兩種方法進行路徑規劃。從方法上比較,常規的A*是以當前點P到目標點T的直線距離作為啟發函數的估計代價項,而改進的方法是用第二層A*來計算估計代價,并對角度閾值λ設置不同的數值進行比較。表1給出了以上實驗的部分數據。其中,擴展結點的個數是指總共嘗試去擴展的節點,有效節點是與無效點相對的概念,是指那些成功加入OPEN表,能組成有效路徑的點。

實驗參數設置如下：λ=20°,25°,…,45°。實驗結果及部分數據如表1、圖6、圖7所示。

表1 實驗數據

圖6 常規A*路徑的水平投影圖

圖7 雙層A*路徑的水平投影圖

其中,深色區域表示威脅區,旗幟和三角形分別表示出發點和目標點,灰色圓點表示機動變化點(轉彎起始點/轉彎結束點)。

從表1數據可知：與常規A*相比,雙層A*的規劃速度、擴展效率均占有優勢。對λ進行調整時,λ過小,時間大大減小,但規劃路徑效果變差。同樣,λ增加到一定程度后收斂到全局最優解,不會再改變規劃效果,但會相應增加規劃時間。

實驗結果表明,改進算法大大減小了無效點的擴展,能有效地提高規劃速度、提升擴展點的效率,更好地實現規避威脅區的目的。同時,可以更靈活地通過調整λ的大小的方式來調節規劃的速度與規劃效果。

5 結語

本文提出了一種雙層A*算法路徑規劃方法,該方法采用雙層機制,利用第二層A*對第一層A*的代價進行更準確的計算,從而加快了收斂速度,提高搜索效率。實驗結果表明,在滿足運動體自身約束、環境約束的條件下,該方法能夠有效解決遇到相對較大威脅區陷入局部搜索的弊病。

[1]馬仁利,關正西.路徑規劃的現狀與發展綜述[J].現代機械,2008(3):22-27.

[2]Chabini, Lan shan.Adaptation of the algorithm for the computation of fastest paths in deterministic discrete-time dynamic networks[J].IEEE Trans on intelligent Transportation Systems,2002,3(1):60-74.

[3]蒙波,皮亦鳴,曹宗杰.基于改進算法的無人機航跡規劃[J].計算機仿真,2010(9):29-32.

[4]Khantanapoka K, Chinnasarn K.Path finding of 2D &3D game real-time strategy with depth direction algorithm for multi-layer[C]//2009 Eighth International Symposium on Natural Language Processing,2009:184-188.

[5]Basem M.ElHalawany, Hala M.Abdel-Kader, Adly TagEldeen, et al.Modified Algorithm for Safer Mobile Robot[C]//Navigation, 2013 Proceeding of International Conference on Modelling, Idetification &control(ICMIC),2013:74-78.

[6]李季,孫秀霞.基于改進A-star算法的無人機航跡規劃算法研究[J].兵工學報,2008(29):788-792.

[7]李春華,鄭昌文,周成平,等.一種三維航跡快速搜索方法[J].宇航學報,2002(3):12-17.

[8]嚴江江,丁明躍,周成平,等.一種基于可行優先的三維航跡規劃方法[J].宇航學報,2009(30):139-144.

[9]劉新,周成平,俞琪,等.基于分層策略的三維航跡快速規劃方法[J].宇航學報,2010(11):2524-2529.

[10]Steven M.LaVatle Planning Algorithms[M].London: Cambridge University Press,2006.

[11]杜萍,楊春.飛行器航跡規劃算法綜述[J].飛行力學,2005(2):10-14.

[12]宋建梅,李侃.基于算法的遠程導彈三維航跡規劃算法[J].北京理工大學學報,2007(7):613-617.

[13]漆陽華,楊戰平,黃清華.A*的改進路徑規劃[J].信息與電子工程,2009(4):326-329.

[14]Peter E.Hart, Nils J.Nilsson.Bertram Raphael A formal basis for the heuristic determination of minimum cost paths in graphs[J].IEEE Transactions of Systems Science and Cybernetics,1968,SSC-4(2):100-107.

ABi-levelA*PathPlanningAlgorithmforEffectivelyAvoidingtheObstacle-areas

YANG Zhiru1LU Hongze2ZHOU Chengping1

(1.State Key Laboratory for Multi-Spectral Information Processing Technologies,
School of Automation, Huazhong University of Science and Technology, Wuhan 430074)
(2.China Academy of Launch Vehicle Technology, Beijing 100076)

In the case that path A*planning algorithm falling into local search problems in search process when encountering threat areas, the estimated-cost calculation method of the expansion point is improved and a bi-level planning algorithm based on original A*algorithm is proposed.In the bi-level mechanism, the estimated-cost of the expansion node in the first-level is calculated by the outcome of the second-level to make the estimated-cost of search process closer to real cost so to obtain more accurate whole-cost of the current node, thereby guiding expansion of the algorithm to a more appropriate direction, and to improve the search efficiency.Experiments show that in the planning space containing complex threat areas, the algorithm can effectively solve the algorithm encountering threat area into local search problems.

A*algorithm, path planning, threat area, bi-level mechanism

2014年1月3日,

:2014年2月21日

陽志如,男,碩士研究生,研究方向:無人飛行器的航跡規劃。陸宏澤,女,博士研究生,工程師。周成平,男,副教授,研究生導師,研究方向:無人飛行器航跡規劃。

U666DOI:10.3969/j.issn1672-9730.2014.07.011