基于改進A*算法的三維航跡規(guī)劃技術(shù)研究*

唐曉東，吳 靜

（1.西南科技大學 信息工程學院，四川 綿陽621010；2.特殊環(huán)境機器人技術(shù)四川省重點實驗室，四川 綿陽621010）

基于改進A*算法的三維航跡規(guī)劃技術(shù)研究*

唐曉東1，2，吳 靜1，2

（1.西南科技大學 信息工程學院，四川 綿陽621010；2.特殊環(huán)境機器人技術(shù)四川省重點實驗室，四川 綿陽621010）

A*算法在實現(xiàn)節(jié)點搜索時執(zhí)行的是大空間搜索，該方式在三維空間中對時間和內(nèi)存的消耗都較大。結(jié)合無人機的機動性能限制以及飛行任務(wù)來改進A*算法，可以達到縮小搜索空間的目的，同時對open表的管理進行改進，以減少擴展節(jié)點排序所花時間，從而整體縮短規(guī)劃所需時間。通過此種方式規(guī)劃出來的航跡能夠最大程度地滿足無人機的機動性能要求，仿真結(jié)果表明，此種方式計算速度快且能保證性能接近最優(yōu)。

無人機；航跡規(guī)劃；改進A*算法

0 引言

隨著無人機技術(shù)的不斷完善，無人機的應(yīng)用越來越廣泛。如何使無人機在規(guī)劃的某片區(qū)域中尋找出一條從起始點到目標點既安全又滿足相應(yīng)約束條件且所花費代價最小的航跡一直是研究的熱點[1]。標準解決航跡規(guī)劃問題的順序是按照預(yù)先選定好的航跡代價函數(shù)，通過一定的搜索算法，選出總的代價函數(shù)值最小的路線作為規(guī)劃出的航跡[2-3]。

在路徑規(guī)劃中，常用的搜索算法有A*算法、動態(tài)規(guī)劃法、Dijkstra算法、遺傳算法等。在這些算法中，由于A*算法計算簡單，算法實現(xiàn)容易且在理論上能夠保證規(guī)劃出的路徑全局最優(yōu)，因此得到廣泛應(yīng)用[3]。但是把A*算法應(yīng)用在航跡規(guī)劃中搜索空間將急劇增加，由此導(dǎo)致收斂時間變長，所需內(nèi)存空間增加。針對這個問題，本文提出把無人機的機動限制結(jié)合到搜索空間中從而縮小搜索空間，同時對A*算法中的open表的管理進行改進，以提高收斂時間，縮小內(nèi)存使用量。

1 改進A*算法實現(xiàn)航跡規(guī)劃

航跡規(guī)劃的本質(zhì)是在規(guī)劃的區(qū)域內(nèi)，在給定的約束條件下尋找一條從起點到目標點的最優(yōu)或次優(yōu)的飛行航跡。傳統(tǒng)的規(guī)劃算法是基于預(yù)先確定的航跡代價函數(shù)生成一條具有最小代價的航跡[4-5]。然而，在許多應(yīng)用中，這樣得到的最小代價的航跡并不能滿足實際需求。在實際應(yīng)用中，航跡規(guī)劃需考慮無人機的機動性能、碰撞概率、飛行時間等約束條件，同時由于無人機航跡規(guī)劃的地域廣闊，因此形成的搜索空間大。通常的搜索算

法要獲得一條最優(yōu)航跡需要很長的收斂時間和極大的內(nèi)存空間，所以在實際應(yīng)用中需對搜索算法進行相應(yīng)的改進。由于A*算法計算速度快、實現(xiàn)容易且能達到全局最優(yōu)的特點，因此選擇對A*算法進行改進，使其適用于無人機航跡規(guī)劃。

1.1 改進A*算法

無人機由于受機動性能、最小飛行距離、航跡距離、飛行高度等的約束，通過A*規(guī)劃出來的航跡不一定能夠滿足無人機的實際飛行條件。在擴展節(jié)點時通常把相關(guān)的約束條件結(jié)合到搜索算法中，這樣既有效減少了搜索空間，也縮短了規(guī)劃所需的時間。無人機飛行時為達到最佳狀態(tài)一般需要滿足的約束條件有：最小飛行距離、最大拐彎角、最大爬升/下滑角、最長飛行距離、最低離地高度[6]。

假設(shè)給定了起始點和目標位置，一條從起始位置到當前節(jié)點的航跡，最小航跡段長度為L，最大拐彎角為φ，最大爬升/下滑角為 θ，則從當前位置在最小飛行距離、最大拐彎角以及最大爬升/下滑角的約束條件下能夠到達的就只有有限的位置空間。如圖1所示，節(jié)點o處的縱向的張角為 2θ2，在水平面上的張角為2θ1，扇面的半徑為最小飛行距離。

圖1 當前節(jié)點可搜索空間

圖1是未經(jīng)離散化的可搜索空間示意圖。在離散規(guī)劃空間時，柵格的邊長長度為無人機的最小飛行距離。把規(guī)劃空間離散化后結(jié)合無人機的約束條件可縮小算法搜索空間。無人機飛行是有方向性的，在進行當前節(jié)點擴展時，飛行反方向的節(jié)點以及垂直于該飛行方向的鄰接節(jié)點都可以裁剪掉。這樣在水平方向上的最大拐彎角就可以限制在3個搜索空間中，垂直方向上的最大爬升/下滑角也同樣可以限制在3個搜索空間中，這樣把擴展當前節(jié)點的后繼節(jié)點的搜索空間縮小為9個。如圖2所示，B節(jié)點是當前新擴展節(jié)點，A點是B點的父親節(jié)點，C點為新擴展節(jié)點的鄰接計算節(jié)點。

圖2 節(jié)點擴展圖

在三維空間中，每個柵格都是一個立方體，在水平剖面和豎直剖面上都要滿足最小步長、最大拐彎角和最大爬升/下滑角。但是在實際控制中，最大轉(zhuǎn)彎角和最大爬升/下滑角可以通過一定的關(guān)系轉(zhuǎn)換，轉(zhuǎn)化為其他直接控制的參數(shù)來滿足要求。下面就分別對水平面和豎直平面進行轉(zhuǎn)換。

1.1.1 水平面關(guān)系轉(zhuǎn)化

假設(shè)無人機當前節(jié)點的坐標為（x，y，z），新擴展的節(jié)點坐標為（x1，y1，z1），航跡偏角為 θ，航跡傾斜角為 β，轉(zhuǎn)彎半徑為 R，最大側(cè)向過載為 Nymax，S1、S2分別為水平面上兩節(jié)點的擴展步長。三維航跡規(guī)劃是基于地球坐標系，水平方向是通過改變航向來躲避障礙物，因此在水平方向上的轉(zhuǎn)化關(guān)系如圖3所示。

圖3 水平面轉(zhuǎn)彎幾何關(guān)系

根據(jù)圖3的幾何關(guān)系可知新擴展的節(jié)點坐標為：

1.1.2 縱向關(guān)系轉(zhuǎn)化

無人機的縱向機動性能主要受到最大爬升/下滑角的限制。在低空飛行時，可以通過對地形坡度的限制來達到對最大爬升角的限制。

如圖4所示，假設(shè)當前節(jié)點的空間坐標為（xi，yi，zi），待擴展節(jié)點的空間坐標為（xi+1，yi+1，zi+1），兩節(jié)點之間的距離為 l，其爬升角為 β，則由幾何關(guān)系可知：

其中：

最大下滑角度可以通過類似關(guān)系建立。

圖4 縱向爬升/下滑幾何關(guān)系

1.1.3 對oPen表結(jié)構(gòu)進行改進

由于在進行節(jié)點擴展尋找最優(yōu)航跡時頻繁地對open表中的節(jié)點執(zhí)行插入、刪除、排序操作，同時 open表中的節(jié)點數(shù)目也相對較多，每次執(zhí)行插入、刪除操作后都需要對open表進行排序。順序存儲結(jié)構(gòu)執(zhí)行插入、刪除、排序操作均較費時。執(zhí)行插入、刪除操作的時間復(fù)雜度是O（n），排序的時間復(fù)雜度為O（n2）。由于在搜索時從open表中刪除的是代價值最小的節(jié)點，而open表是按照節(jié)點代價值大小來組成的有序表，因此實際在執(zhí)行刪除操作時的時間復(fù)雜度是O（1），當有新節(jié)點插入時即需對 open表進行排序以保證 open表一直處于有序狀態(tài)。由此分析得出對open表的管理主要時間花銷是在節(jié)點

排序上，為提高整體的運行效率，這里對 open表的數(shù)據(jù)結(jié)構(gòu)進行一定的改進，通過采用樹形的數(shù)據(jù)結(jié)構(gòu)來管理open表。最小二叉堆是一種典型的樹形數(shù)據(jù)結(jié)構(gòu)，通過最小二叉堆對節(jié)點進行排序的時間復(fù)雜度為O（n log2n），通過此種數(shù)據(jù)結(jié)構(gòu)可減少open表節(jié)點排序的時間花銷。

1.2 代價函數(shù)的建立

軍用無人機航跡規(guī)劃的目標是在滿足無人機物理特性約束以及具體飛行任務(wù)約束的前提下生成超低空地形跟隨、地形回避以及威脅回避的飛行軌跡，以提高無人機的生存概率。其數(shù)學表達式為：

其中 PCi為無人機在第i航跡的撞地概率；PDi是無人機在第i段航跡被敵方雷達探測到的概率，PKi為無人機在第i段航跡被敵方雷達探測到并被擊毀的概率[7]。由于這些概率和地區(qū)的地形、威脅的分布密度、發(fā)現(xiàn)威脅的能力等都存在著很大關(guān)系，同時這些概率和無人機的各項狀態(tài)（飛行高度、速度、油量等）之間的關(guān)系很難定義，即使找出他們之間關(guān)系，該公式也將十分復(fù)雜，勢必將增加代價函數(shù)的計算難度。由此需要對上述的代價函數(shù)進行優(yōu)化。無人機在低空突防時飛行的高度越低，被敵人發(fā)現(xiàn)的幾率就越小，規(guī)劃出的航跡飛行時間越短，越能達到出其不意的效果，同時也提高了無人機的生存概率；若飛行時間過長，會增加累計威脅，同時也增加油量的消耗。基于上述原因再結(jié)合啟發(fā)式A*算法的表達式，把g（n）和h（n）分別簡化為如下形式：

起始節(jié)點到當前節(jié)點的代價函數(shù)值：

其中 λ1，λ2，λ3∈[0，1]，Hi∈[Hmin，Hmax]，F(xiàn)∈[0，F(xiàn)max]，Ti∈[0，Tf]。

當前節(jié)點（xn，yn，zn）到目標節(jié)點（xg，yg，zg）的代價函數(shù)估值即啟發(fā)式函數(shù)為：

其中：

上述代價函數(shù)表達式中Li表示從起始點到當前節(jié)點飛行過的路程，F(xiàn)max表示無人機油箱中的總的油量，Hmin、Hmax分別表示無人機為防止撞地的最小飛行高度和為防止被敵人雷達探測到的最高飛行高度，Tf表示無人機能接受威脅的最大門限值。啟發(fā)函數(shù)中Ln表示當前節(jié)點到目標節(jié)點的距離，hn表示目標節(jié)點的高程值，λ1、λ2、λ3、μ1、μ2為表達式中各項的加權(quán)系數(shù)。設(shè)定不同的加權(quán)系數(shù)，獲得的航跡也有所不同：如果增大高度值的系數(shù)λ1，則規(guī)劃出來的平均航跡高度就會越低，增大威脅值的系數(shù)值λ2，則規(guī)劃出來的航跡將遠離威脅，同理增大航跡長度系數(shù)值λ3，這樣規(guī)劃出來的航跡長度將減少。通過對這些權(quán)重系數(shù)的不同組合，可以得到所期望的滿足條件的最優(yōu)航跡。

1.3 算法實現(xiàn)

通過結(jié)合無人機的自身限制以及任務(wù)要求，在三維航跡搜索過程中將大大縮小搜索空間，從而規(guī)劃出滿足條件的航跡。A*算法的實現(xiàn)主要是維護兩個表：open表以及closed表。在三維空間中算法實現(xiàn)的具體實現(xiàn)步驟如下：

（1）把地理威脅等環(huán)境信息初始化到規(guī)劃空間中。

（2）把起始點添加到open表中，同時把closed表置空。

（3）判斷open表是否為空，若為空則算法結(jié)束；反之，則找出open表中代價值最小的節(jié)點作為當前節(jié)點，同時把該節(jié)點從open表中刪除，放入closed表中。

（4）判斷當前節(jié)點是否為目標節(jié)點，若當前節(jié)點到目標節(jié)點的距離小于最小飛行距離，則將目標節(jié)點的父親節(jié)點當作當前節(jié)點，航跡搜索過程結(jié)束。

（5）對當前節(jié)點進行擴展：

①根據(jù)約束條件產(chǎn)生當前節(jié)點待擴展區(qū)；

②對待擴展區(qū)中的每個節(jié)點，根據(jù)式（5）、（6）計算每個節(jié)點的航跡代價；

③如果待擴展區(qū)域中的某個節(jié)點已經(jīng)在closed表中且其代價估值小于closed表中的估價值，則更新 closed表中的估價值，同時把該節(jié)點移出 closed表，放入 open表中；如若不在 closed表中，則判斷該節(jié)點是否在 open表中，如果不在則把該節(jié)點插入到open表中；

④如果該節(jié)點在open表中且open表中的g（n）值都大于當前計算的g（n）值，則更新 open表中節(jié)點 g（n）的值，更新后需保證open表仍然有序。

（6）接著繼續(xù)跳轉(zhuǎn)到步驟（3）執(zhí)行上述操作直至找到目標節(jié)點。

由于在節(jié)點擴展時，每個被擴展的節(jié)點都會記錄其父節(jié)點，當算法找到目標節(jié)點后則算法結(jié)束。接著通過從目標節(jié)點向前回溯直到起始位置，這樣就得到了從起始點到目標點的最小代價航跡。

2 仿真結(jié)果

對上述改進后的算法在 Intel Core i5、3.1 GHz的PC上進行驗證試驗，運行環(huán)境是Windows7操作系統(tǒng)。規(guī)劃的空域大小為 60 km×60 km，假設(shè)起始節(jié)點的坐標為（0，0，0），目標節(jié)點的坐標為（60，60，0），最低離地間隙為 100 m，最大轉(zhuǎn)彎角為 45°，最大爬升/下滑角為 30°。實驗通過用基本A*算法和改進后的A*算法分別設(shè)置二組不同的 λ1，λ2，λ3，u1，u2值來規(guī)劃最優(yōu)航跡并對算法改進前后的性能進行比較分析。仿真圖5的 λ1，λ2，λ3，μ1，μ2分別為 λ1=0.1，λ2=0.3，λ3=0.6，μ1=0.45，μ2=0.55，由于λ3所占權(quán)重較大，所以規(guī)劃出來的航跡總的路程較小。 仿真圖6的 λ1，λ2，λ3，μ1，μ2分別為 λ1=0.4，λ2= 0.5，λ3=0.1，μ1=0.45，μ2=0.55，由于 λ1、λ2所占權(quán)重較大，所以規(guī)劃出的航跡飛行高度較低，安全性較高。

圖5 第一組參數(shù)三維仿真圖

圖6 第二組參數(shù)三維仿真圖

兩種參數(shù)的飛行高度分別如圖7、圖8所示。通過對比可知，第一組數(shù)據(jù)的平均飛行高度要高于第二組，這就印證了參數(shù)λ1的大小影響無人機的平均飛行高度，飛行高度越低也間接提高了無人機的安全性，飛行高度越低，越難被敵人雷達發(fā)現(xiàn)。

圖7 第一組參數(shù)高度仿真圖

圖8 第二組參數(shù)高度仿真圖

基本A*算法與改進后的A*算法性能比較如表1所示。通過各項數(shù)據(jù)對比可以看出，改進后的算法規(guī)劃時間較改進前的少很多，并且總的航跡路程也縮減了不少，這樣能夠在最快的時間內(nèi)規(guī)劃出滿足需求的最優(yōu)航跡。通過改變參數(shù) λ1、λ2、λ3的權(quán)重比例可以規(guī)劃出更側(cè)重于飛行高度、安全性以及飛行距離的航跡。

表1 基本A*算法與改進后的A*算法性能比較

3 結(jié)束語

本文通過把無人機的各項機動性能限制、飛行路程以及飛行高度等約束條件相結(jié)合的方式來縮小節(jié)點擴展時的搜索空間，同時對節(jié)點水平方向和縱向方向的空間劃分，使規(guī)劃出的航跡節(jié)點包含了無人機在該節(jié)點的各項狀態(tài)。在三維空間中，經(jīng)過限制后滿足要求的節(jié)點已很少，大大提高了搜索效率，對 open表結(jié)構(gòu)的改進減少了open表中節(jié)點排序的時間。同時對評價代價函數(shù)進行優(yōu)化，使算法能夠更快地收斂到最優(yōu)解。

[1]董文洪，易波，栗飛.無人機航路規(guī)劃環(huán)境模型研究[J].計算機工程與應(yīng)用，2012，48（15）：236-239.

[2]高暉，陳欣，夏云程.無人機航路規(guī)劃研究[J].南京航空航天大學學報，2001，33（2）：135-138.

[3]宋建梅，李侃.基于 A*算法的遠程導(dǎo)彈三維航跡規(guī)劃算法[J].北京理工大學學報，2007，27（7）：613-617.

[4]趙鋒，楊偉，楊朝旭，等.無人機三維航路動態(tài)規(guī)劃及導(dǎo)引控制研究[J].計算機工程與應(yīng)用，2014，50（2）：58-64.

[5]李季，孫秀霞.基于改進 A-Star算法的無人機航跡規(guī)劃算法研究[J].兵工學報，2008，29（7）：789-792.

[6]杜萍，楊春.行器航跡規(guī)劃算法綜述[J].飛行力學，2005，23（2）：10-14.

[7]辛貴州.無人飛行器航跡規(guī)劃算法研究[D].哈爾濱：哈爾濱工程大學，2010.

Research on three-dimensional route planning based on improved A*algorithm

Tang Xiaodong1，2，Wu Jing1，2
（1.School of Information Engineering，Southwest University of Science and Technology，Mianyang 621010，China；2.Robot Technology Used for Special Environment Key Laboratory of Sichuan Province，Mianyang 621010，China）

When A*search algorithm is executed in path-searching in 3D environment，it will cost a lot of time and memory because of large searching space.This paper represents an improved A*algorithm which uses mobility restrictions and UAV mission to achieve the purpose of narrowing the search space，while the management of open tables are modified to reduce the time which costs by sorting node.It could carry out a route which can meet the performance requirements of UAV in this way.Simulation results show that this approach can ensure faster to carry out the route which is close to optimal.

UAV；route planning；improved A*algorithm

E926

A

0258-7998（2015）05-0163-04

10.16157/j.issn.0258-7998.2015.05.041

2015-02-02）（

2015-01-05）

唐曉東（1989-），男，碩士研究生，主要研究方向：航跡規(guī)劃算法。

2014年四川省科技廳科技計劃項目（2014JY0215）

吳靜（1963-），女，副教授，主要研究方向：通信技術(shù)、三網(wǎng)融合。