基于障礙信息的高效A-Star航路規(guī)劃算法*

肖自兵，屈耀紅，袁冬莉

（1.江蘇自動(dòng)化研究所，江蘇 連云港 222061；2.西北工業(yè)大學(xué)自動(dòng)化學(xué)院，西安 710072）

0 引言

航路規(guī)劃在無(wú)人器執(zhí)行任務(wù)的過(guò)程中起著關(guān)鍵的作用。選擇算法是航路規(guī)劃的重要一環(huán)，航路規(guī)劃算法應(yīng)當(dāng)能夠使無(wú)人器有效地規(guī)避障礙[1]。常用的航路規(guī)劃算法有Dijkstra算法、Bellman Ford算法、Floyd-Warshall算法和 A-Star算法等[2]。通過(guò)構(gòu)造啟發(fā)函數(shù)，A-Star算法成為靜態(tài)路網(wǎng)中求解最短路徑的有效方法[3]，A-Star算法可以在威脅區(qū)域是任意形狀的情況下進(jìn)行航路規(guī)劃[4]，并且在理論上可以保證全局最優(yōu)解的收斂性[5，9]。

在航路規(guī)劃過(guò)程中發(fā)現(xiàn)，當(dāng)障礙較為復(fù)雜時(shí)，A-Star算法產(chǎn)生的擴(kuò)展節(jié)點(diǎn)多，導(dǎo)致搜索效率降低。目前針對(duì)提高A-Star算法搜索效率的研究較少，文獻(xiàn)[6]對(duì)A-Star算法進(jìn)行了改進(jìn)，實(shí)現(xiàn)了定長(zhǎng)航跡規(guī)劃和協(xié)同航跡規(guī)劃，改善了航跡的可飛性，達(dá)到了較小的航跡長(zhǎng)度誤差，但沒有對(duì)算法搜索效率進(jìn)行討論；文獻(xiàn)[7]結(jié)合風(fēng)場(chǎng)信息，以飛行時(shí)間為代價(jià)，對(duì)A-Star算法進(jìn)行了改進(jìn)，實(shí)現(xiàn)了航跡順風(fēng)搜索和理想耗時(shí)最少，但未涉及算法搜索效率；文獻(xiàn)[5]結(jié)合飛行器簡(jiǎn)化運(yùn)動(dòng)學(xué)方程對(duì)A-Star算法進(jìn)行了改進(jìn)，通過(guò)加大搜索步長(zhǎng)的方法減少了擴(kuò)展節(jié)點(diǎn)數(shù)量，提高了搜索效率，但作者同時(shí)指出，較大的步長(zhǎng)會(huì)帶來(lái)較大的轉(zhuǎn)彎角度，造成路徑長(zhǎng)度的增加；文獻(xiàn)[8]也是通過(guò)增大步長(zhǎng)的方法減少擴(kuò)展節(jié)點(diǎn)、節(jié)省規(guī)劃時(shí)間，但仿真結(jié)果表明，步長(zhǎng)較長(zhǎng)會(huì)導(dǎo)致航跡進(jìn)入部分威脅區(qū)域。本文提出了一種基于障礙信息的高效A-Star航路規(guī)劃算法，該算法無(wú)需加大搜索步長(zhǎng)，通過(guò)分析、使用障礙信息，制定合理的節(jié)點(diǎn)擴(kuò)展策略、引入積極的航路導(dǎo)向機(jī)制，有效減少了節(jié)點(diǎn)擴(kuò)展次數(shù)和擴(kuò)展節(jié)點(diǎn)數(shù)量，從而提高了搜索效率。

1 航路規(guī)劃問題

航路規(guī)劃問題又可稱為航跡或路徑規(guī)劃問題，通常可以描述為，求取一條連接起點(diǎn)和終點(diǎn)、滿足一定條件（如避開障礙或威脅、長(zhǎng)度最短、時(shí)間最短等）的航路、航跡或路徑。下面以水下無(wú)人航行器（UUV）的航路規(guī)劃為例，討論基于障礙信息的高效A-Star航路規(guī)劃算法及航路規(guī)劃的一般過(guò)程。

1.1 問題描述

UUV需要通過(guò)布有水雷的航道執(zhí)行對(duì)敵攻擊任務(wù)，為縮短敵方反應(yīng)時(shí)間、取得出其不意的打擊效果，為UUV規(guī)劃一條最短航路。

1.2 模型建立

上述問題可轉(zhuǎn)化為障礙存在條件下的典型航路規(guī)劃問題。實(shí)際UUV工作在三維空間，為方便問題討論，可將三維航路規(guī)劃化簡(jiǎn)到二維平面進(jìn)行（下面二維平面航路規(guī)劃的研究過(guò)程和方法同樣適用于三維情形），作如下假設(shè)：1）所有水雷為同一類型，作用半徑相等；2）UUV和水雷工作于同一深度。

如圖1所示，在長(zhǎng)為50 m×10 m，寬為30 m×10 m的矩形水域內(nèi)分布著8個(gè)水雷，Oi（i=1，2，3，4，5，6，7，8）標(biāo)識(shí)了水雷所在位置，以O(shè)i（i=1，2，3，4，5，6，7，8） 為中心的圓形區(qū)域表示水雷殺傷區(qū)域，UUV不得進(jìn)入。S點(diǎn)（用正方形表示，坐標(biāo)為（-23，0））和 E 點(diǎn)（用六角星形表示，坐標(biāo)為（23，0））分別為UUV航路起點(diǎn)和終點(diǎn)。本文最短航路規(guī)劃問題可以描述為：求取一條連接S點(diǎn)和E點(diǎn)、繞過(guò)障礙圓Oi（i=1，2，3，4，5，6，7，8）的最短航路。

1.3 地圖處理與遍歷

在研究二維航路規(guī)劃問題時(shí)，通常需要對(duì)任務(wù)地圖進(jìn)行一定的處理以得到供航路規(guī)劃使用的節(jié)點(diǎn)集。地圖柵格化是一種操作簡(jiǎn)單且行之有效的處理方法，該方法使用兩組等間距、成垂直關(guān)系的平行線分割地圖形成柵格，分割產(chǎn)生的柵格呈正方形，分割線的交點(diǎn)（柵格的頂點(diǎn)）形成節(jié)點(diǎn)，如圖2所示。

柵格化后，對(duì)于圖2中的某一節(jié)點(diǎn)N，其相鄰的8個(gè)節(jié)點(diǎn)N1～N8可視為由其沿8個(gè)不同方向（如圖中箭頭所示）擴(kuò)展產(chǎn)生。由于兩組分割線成垂直關(guān)系，可建立坐標(biāo)軸沿分割線方向的直角坐標(biāo)系。例如，圖2中可以以N7指向N5的方向?yàn)閤軸、N7指向N1的方向?yàn)閥軸建立直角坐標(biāo)系。設(shè)圖中柵格邊長(zhǎng)為L(zhǎng)，在上述建立的直角坐標(biāo)系中，設(shè)節(jié)點(diǎn)N的坐標(biāo)為（xn，yn），則節(jié)點(diǎn)N的擴(kuò)展節(jié)點(diǎn)N1～N8的坐標(biāo)依次為和。

2 A-Star算法原理

A-Star算法是一種經(jīng)典的啟發(fā)式搜索算法，其最大特點(diǎn)在于建立了代價(jià)函數(shù)，代價(jià)函數(shù)的公式表示為：

式中，f（n）為當(dāng)前節(jié)點(diǎn)n的代價(jià)函數(shù)；g（n）為從起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n已走過(guò)的路徑代價(jià)；h（n）為從當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)，稱為啟發(fā)函數(shù)。

在每一步搜索中，A-Star算法均選擇f值最小的節(jié)點(diǎn)作為最佳節(jié)點(diǎn)進(jìn)行擴(kuò)展，直至找到目標(biāo)節(jié)點(diǎn)為止。

3 基于障礙信息的高效A-Star航路規(guī)劃算法

3.1 基于障礙信息的節(jié)點(diǎn)擴(kuò)展方向最小集

在基于二維柵格地圖的航路規(guī)劃問題中，節(jié)點(diǎn)擴(kuò)展方向通常采用全集（8個(gè)方向，如圖2所示）。全方向擴(kuò)展的優(yōu)點(diǎn)在于算法越障能力強(qiáng)、能夠適應(yīng)較為復(fù)雜的障礙環(huán)境，缺點(diǎn)是擴(kuò)展方向多必然導(dǎo)致算法搜索效率下降。在大多數(shù)情況下，障礙不會(huì)太過(guò)復(fù)雜，不需要進(jìn)行全方向節(jié)點(diǎn)擴(kuò)展也能完成航路規(guī)劃。下面討論通過(guò)確定節(jié)點(diǎn)擴(kuò)展方向最小集（在不影響航路質(zhì)量的條件下完成航路規(guī)劃所需要的最少擴(kuò)展方向）的方法來(lái)提高算法搜索效率。

使一組分割線平行于直線SE（S點(diǎn)和E點(diǎn)分別為航路的起點(diǎn)和終點(diǎn)），另一組分割線與之垂直。取S點(diǎn)和E點(diǎn)的中點(diǎn)O作為坐標(biāo)原點(diǎn)，O指向E點(diǎn)的方向?yàn)閄軸，Y軸繞O點(diǎn)逆時(shí)針旋轉(zhuǎn)90°為Y軸，建立直角坐標(biāo)系XOY。

根據(jù)障礙信息確定節(jié)點(diǎn)擴(kuò)展方向最小集（記為δ）的方法如下：

1）從S點(diǎn)指向E點(diǎn)的方向作為擴(kuò)展方向1，如果該方向線未穿過(guò)任何障礙，則δ={方向1}（此時(shí)，直線段SE也即為所規(guī)劃的最短航路）。否則轉(zhuǎn)到下一步。

2）方向1繞S點(diǎn)逆時(shí)針旋轉(zhuǎn)45°，得擴(kuò)展方向2，方向1繞S點(diǎn)順時(shí)針旋轉(zhuǎn)45°，得擴(kuò)展方向3。如果方向2和方向3均未穿過(guò)任何障礙，則δ={方向1，方向2，方向3}。否則轉(zhuǎn)到下一步。

3）方向1繞S點(diǎn)逆時(shí)針旋轉(zhuǎn)90°，得擴(kuò)展方向4，方向1繞S點(diǎn)順時(shí)針旋轉(zhuǎn)90°，得擴(kuò)展方向5。如果方向4和方向5均未穿過(guò)任何障礙，則δ={方向1，方向2，方向3，方向4，方向5}。否則轉(zhuǎn)到下一步。

4）方向1繞S點(diǎn)逆時(shí)針旋轉(zhuǎn)135°，得擴(kuò)展方向6，方向1繞S點(diǎn)順時(shí)針旋轉(zhuǎn)135°，得擴(kuò)展方向7。如果方向6和方向7均未穿過(guò)任何障礙，則δ={方向1，方向 2，方向 3，方向 4，方向 5，方向 6，方向 7}。否則轉(zhuǎn)到下一步。

5）擴(kuò)展方向?yàn)槿?，?δ={方向 1，方向 2，方向3，方向 4，方向 5，方向 6，方向 7，方向 8}。

需要說(shuō)明的是，為均衡直線SE兩側(cè)的搜索概率，每次增加一對(duì)擴(kuò)展方向（如方向2和方向3、方向4和方向5、方向6和方向7），這對(duì)擴(kuò)展方向關(guān)于方向1對(duì)稱。對(duì)于圓形障礙，可通過(guò)判定圓心到方向線的距離D與圓半徑R的大小關(guān)系來(lái)判定該方向線是否穿過(guò)該障礙。

依據(jù)上述方法，可知圖1需要的最少擴(kuò)展方向有3個(gè)，如圖3所示。

這表明，在用A-Star算法進(jìn)行航路規(guī)劃的過(guò)程中，對(duì)每一個(gè)最佳節(jié)點(diǎn)只需按照?qǐng)D中的3個(gè)方向進(jìn)行擴(kuò)展即可。

減少擴(kuò)展方向可以避免算法做“無(wú)用功”，因此，可以提高算法的搜索效率。

如果任務(wù)地圖有自帶的坐標(biāo)系xoy，由新坐標(biāo)系XOY的建立方法可知，其與原坐標(biāo)系xoy間的坐標(biāo)轉(zhuǎn)換關(guān)系為

式（2）中，θ、x0和 y0分別為坐標(biāo)系 XOY 相對(duì)于 xoy的旋轉(zhuǎn)角度、橫向偏移和縱向偏移，滿足下式：

式中，xS、yS、xE、yE分別為 S 點(diǎn)、E 點(diǎn)在坐標(biāo)系 xoy 中的橫、縱坐標(biāo)值。

在新坐標(biāo)系XOY下規(guī)劃航路，對(duì)求出的每個(gè)航路點(diǎn)，按照式（2）計(jì)算，即可得到在原坐標(biāo)系下的規(guī)劃結(jié)果。

需要說(shuō)明的一點(diǎn)是，在圖1中，xoy與XOY為同一坐標(biāo)系，不需要再進(jìn)行坐標(biāo)變換。

3.2 基于障礙信息的代價(jià)函數(shù)

上一節(jié)討論了根據(jù)障礙信息確定最少擴(kuò)展方向的方法，本節(jié)將在上一節(jié)討論的基礎(chǔ)上提出基于障礙信息的代價(jià)函數(shù)，該代價(jià)函數(shù)可進(jìn)一步提高算法的搜索效率。

基于障礙信息的代價(jià)函數(shù)可以描述為：

即在原代價(jià)函數(shù)計(jì)算公式的基礎(chǔ)上加上函數(shù)I（n），I（n）稱為障礙代價(jià)預(yù)估函數(shù)，計(jì)算方法如下：

式中，N為障礙數(shù)量，Ii為障礙i對(duì)應(yīng)的代價(jià)預(yù)估函數(shù)，ai稱為代價(jià)有效系數(shù)，表征Ii是否有效，如果當(dāng)前擴(kuò)展方向穿過(guò)障礙i，ai=1，否則ai=0。Ii的計(jì)算方法為：

式中，分子k為比例系數(shù)，可根據(jù)需要設(shè)置其大?。环帜副硎菊系Ki與當(dāng)前最佳節(jié)點(diǎn)的距離，xi、yi為障礙i中心點(diǎn)的橫、縱坐標(biāo)值，x0、y0為當(dāng)前最佳節(jié)點(diǎn)的橫、縱坐標(biāo)值。

障礙代價(jià)預(yù)估函數(shù)I（n）表征了在當(dāng)前擴(kuò)展方向上存在的障礙的信息，對(duì)航路搜索的影響如下。當(dāng)前最佳節(jié)點(diǎn)在當(dāng)前擴(kuò)展方向上距障礙越近，節(jié)點(diǎn)沿該方向擴(kuò)展的代價(jià)就越大，這一機(jī)理對(duì)航路搜索有積極的導(dǎo)向作用，可引導(dǎo)航路及早規(guī)避較近的障礙；當(dāng)前擴(kuò)展方向線上分布的障礙越多，節(jié)點(diǎn)沿該方向擴(kuò)展的代價(jià)也越大，這一機(jī)理同樣對(duì)航路有積極的導(dǎo)向作用，可引導(dǎo)航路及早避開障礙密集區(qū)。上述兩種機(jī)制可減少擴(kuò)展節(jié)點(diǎn)數(shù)量，因此，可以提高算法的搜索效率。

對(duì)于圓形障礙，可由下述數(shù)學(xué)方法確定障礙i的代價(jià)Ii是否有效（即ai確定值為1還是為0）。

對(duì)于圖1，已確定其需要的擴(kuò)展方向?yàn)棣?{方向1，方向2，方向3}。如圖4～圖6所示（圖中的點(diǎn)表示當(dāng)前最佳節(jié)點(diǎn)，圓表示當(dāng)前正在考查的障礙i），如果當(dāng)前最佳節(jié)點(diǎn)處于圖中陰影區(qū)域中，擴(kuò)展方向線必穿過(guò)障礙 i，于是 ai=1，反之，ai=0。

圖中，直線1與方向線垂直且過(guò)障礙圓心，直線2、直線3與方向線平行且與障礙圓相切。由此可得，障礙i對(duì)方向1有效的條件為當(dāng)前最佳節(jié)點(diǎn)坐標(biāo)（x，y）滿足式（7）。

障礙i對(duì)方向2有效的條件為當(dāng)前最佳節(jié)點(diǎn)坐標(biāo)（x，y）滿足式（8）。

障礙i對(duì)方向3有效的條件為當(dāng)前最佳節(jié)點(diǎn)坐標(biāo)（x，y）滿足式（9）。

式（7）～式（9）中，x1、y1和 Ri分別為障礙 i的圓心橫、縱坐標(biāo)和半徑。

4 仿真及分析

4.1 航路仿真

使用本文算法和傳統(tǒng)A-Star算法分別進(jìn)行航路規(guī)劃，編程語(yǔ)言為MATLAB M語(yǔ)言，啟發(fā)函數(shù)h（n）取為當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)E的直線距離，障礙代價(jià)預(yù)估函數(shù)I（n）中比例k取2.5，仿真結(jié)果如圖7～圖9所示。

圖中的點(diǎn)表示航路規(guī)劃過(guò)程中產(chǎn)生的節(jié)點(diǎn)。圖7共進(jìn)行了2 438次節(jié)點(diǎn)擴(kuò)展，產(chǎn)生的節(jié)點(diǎn)總數(shù)為473（含航路起點(diǎn)S和終點(diǎn)E），其中，最佳節(jié)點(diǎn)數(shù)為344；圖8共進(jìn)行了666次節(jié)點(diǎn)擴(kuò)展，產(chǎn)生的節(jié)點(diǎn)總數(shù)為333，其中，最佳節(jié)點(diǎn)數(shù)為272；圖9共進(jìn)行了609次節(jié)點(diǎn)擴(kuò)展，產(chǎn)生的節(jié)點(diǎn)總數(shù)為316，其中，最佳節(jié)點(diǎn)數(shù)為245。3個(gè)圖的航路均為最短航路（航路長(zhǎng)度為517.990 m）。

采用基于障礙信息的三方向擴(kuò)展策略后，節(jié)點(diǎn)擴(kuò)展次數(shù)由2 438減少到666，表明算法搜索效率顯著提高；在三方向擴(kuò)展策略基礎(chǔ)上加入障礙代價(jià)預(yù)估，節(jié)點(diǎn)擴(kuò)展次數(shù)進(jìn)一步減少到609，表明算法搜索效率進(jìn)一步提高。

4.2 航路光順

上述仿真產(chǎn)生的航路中存在若干拐角，在拐角處，UUV必須降低速度以完成轉(zhuǎn)向，這樣就拉長(zhǎng)了任務(wù)時(shí)間，增加了暴露的風(fēng)險(xiǎn)。為解決這一問題，利用UUV最小轉(zhuǎn)彎半徑對(duì)航路進(jìn)行光順。

記UUV最小轉(zhuǎn)彎半徑為r，航路光順的方法為：在航路每個(gè)拐角處做半徑為r的內(nèi)切圓弧來(lái)取代該拐角。

基于最小轉(zhuǎn)彎半徑的航路光順?biāo)惴枋鋈缦隆?/p>

圖10中，A點(diǎn)為航路拐點(diǎn)，B點(diǎn)為A點(diǎn)左側(cè)航路上的一個(gè)節(jié)點(diǎn)，C點(diǎn)為A點(diǎn)右側(cè)航路上的一個(gè)節(jié)點(diǎn)，圓心為O、半徑為r（UUV最小轉(zhuǎn)彎半徑）的航路內(nèi)切圓與A點(diǎn)兩側(cè)航路的切點(diǎn)分別為D點(diǎn)和E點(diǎn)。

1）根據(jù)A、B、C三點(diǎn)的坐標(biāo)，可求得∠BAC，記為 2α，則。

2）由OD=r求得AD=rcotα，再結(jié)合D點(diǎn)在直線AB上這一條件，可求得D點(diǎn)坐標(biāo)（xD，yD），同理可求得 E 點(diǎn)坐標(biāo)（xE，yE）。

3）設(shè)內(nèi)切圓圓心O點(diǎn)坐標(biāo)為（xO，yO），由向量的內(nèi)積為0、向量的內(nèi)積為0，即

可求得O點(diǎn)坐標(biāo)。至此，內(nèi)切圓弧的圓心O和兩個(gè)端點(diǎn)D、E均已解出，可畫出目標(biāo)圓弧。

設(shè)UUV最小轉(zhuǎn)彎半徑為10 m，對(duì)圖9中的航路進(jìn)行光順，結(jié)果如圖11所示。航路長(zhǎng)度為513.690 m，相對(duì)于原始航路的長(zhǎng)度誤差為0.83%。

5 結(jié)論

本文提出了一種基于障礙信息的高效A-Star航路規(guī)劃算法，仿真結(jié)果表明，該算法能夠在保證航路最優(yōu)的條件下顯著提高搜索效率。雖然本文研究的是二維平面航路規(guī)劃問題，但研究過(guò)程和方法同樣適用于三維空間的情形，可將算法擴(kuò)展到三維空間執(zhí)行。提出的算法適用范圍廣、具有普遍性應(yīng)用價(jià)值。所做工作具有一定的參考和借鑒意義。如何更有效地使用環(huán)境信息、更大程度地提高航路搜索效率有待今后進(jìn)行進(jìn)一步的探討。