肖自兵，屈耀紅，袁冬莉

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

0 引言

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

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

1 航路規(guī)劃問題

航路規(guī)劃問題又可稱為航跡或路徑規(guī)劃問題，通?？梢悦枋鰹椋笕∫粭l連接起點和終點、滿足一定條件（如避開障礙或威脅、長度最短、時間最短等）的航路、航跡或路徑。下面以水下無人航行器（UUV）的航路規(guī)劃為例，討論基于障礙信息的高效A-Star航路規(guī)劃算法及航路規(guī)劃的一般過程。

1.1 問題描述

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

1.2 模型建立

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

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

1.3 地圖處理與遍歷

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

柵格化后，對于圖2中的某一節(jié)點N，其相鄰的8個節(jié)點N1～N8可視為由其沿8個不同方向（如圖中箭頭所示）擴展產生。由于兩組分割線成垂直關系，可建立坐標軸沿分割線方向的直角坐標系。例如，圖2中可以以N7指向N5的方向為x軸、N7指向N1的方向為y軸建立直角坐標系。設圖中柵格邊長為L，在上述建立的直角坐標系中，設節(jié)點N的坐標為（xn，yn），則節(jié)點N的擴展節(jié)點N1～N8的坐標依次為和。

2 A-Star算法原理

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

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

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

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

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

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

使一組分割線平行于直線SE（S點和E點分別為航路的起點和終點），另一組分割線與之垂直。取S點和E點的中點O作為坐標原點，O指向E點的方向為X軸，Y軸繞O點逆時針旋轉90°為Y軸，建立直角坐標系XOY。

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

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

2）方向1繞S點逆時針旋轉45°，得擴展方向2，方向1繞S點順時針旋轉45°，得擴展方向3。如果方向2和方向3均未穿過任何障礙，則δ={方向1，方向2，方向3}。否則轉到下一步。

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

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

5）擴展方向為全集，即 δ={方向 1，方向 2，方向3，方向 4，方向 5，方向 6，方向 7，方向 8}。

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

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

這表明，在用A-Star算法進行航路規(guī)劃的過程中，對每一個最佳節(jié)點只需按照圖中的3個方向進行擴展即可。

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

如果任務地圖有自帶的坐標系xoy，由新坐標系XOY的建立方法可知，其與原坐標系xoy間的坐標轉換關系為

式（2）中，θ、x0和 y0分別為坐標系 XOY 相對于 xoy的旋轉角度、橫向偏移和縱向偏移，滿足下式：

式中，xS、yS、xE、yE分別為 S 點、E 點在坐標系 xoy 中的橫、縱坐標值。

在新坐標系XOY下規(guī)劃航路，對求出的每個航路點，按照式（2）計算，即可得到在原坐標系下的規(guī)劃結果。

需要說明的一點是，在圖1中，xoy與XOY為同一坐標系，不需要再進行坐標變換。

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

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

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

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

式中，N為障礙數(shù)量，Ii為障礙i對應的代價預估函數(shù)，ai稱為代價有效系數(shù)，表征Ii是否有效，如果當前擴展方向穿過障礙i，ai=1，否則ai=0。Ii的計算方法為：

式中，分子k為比例系數(shù)，可根據(jù)需要設置其大??；分母表示障礙i與當前最佳節(jié)點的距離，xi、yi為障礙i中心點的橫、縱坐標值，x0、y0為當前最佳節(jié)點的橫、縱坐標值。

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

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

對于圖1，已確定其需要的擴展方向為δ={方向1，方向2，方向3}。如圖4～圖6所示（圖中的點表示當前最佳節(jié)點，圓表示當前正在考查的障礙i），如果當前最佳節(jié)點處于圖中陰影區(qū)域中，擴展方向線必穿過障礙 i，于是 ai=1，反之，ai=0。

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

障礙i對方向2有效的條件為當前最佳節(jié)點坐標（x，y）滿足式（8）。

障礙i對方向3有效的條件為當前最佳節(jié)點坐標（x，y）滿足式（9）。

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

4 仿真及分析

4.1 航路仿真

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

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

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

4.2 航路光順

上述仿真產生的航路中存在若干拐角，在拐角處，UUV必須降低速度以完成轉向，這樣就拉長了任務時間，增加了暴露的風險。為解決這一問題，利用UUV最小轉彎半徑對航路進行光順。

記UUV最小轉彎半徑為r，航路光順的方法為：在航路每個拐角處做半徑為r的內切圓弧來取代該拐角。

基于最小轉彎半徑的航路光順算法描述如下。

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

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

2）由OD=r求得AD=rcotα，再結合D點在直線AB上這一條件，可求得D點坐標（xD，yD），同理可求得 E 點坐標（xE，yE）。

3）設內切圓圓心O點坐標為（xO，yO），由向量的內積為0、向量的內積為0，即

可求得O點坐標。至此，內切圓弧的圓心O和兩個端點D、E均已解出，可畫出目標圓弧。

設UUV最小轉彎半徑為10 m，對圖9中的航路進行光順，結果如圖11所示。航路長度為513.690 m，相對于原始航路的長度誤差為0.83%。

5 結論

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