基于改進A*算法的物流無人機航跡規(guī)劃研究

陳繼偉，包長春，趙子恒

（內(nèi)蒙古工業(yè)大學 航空學院，內(nèi)蒙古 呼和浩特 010051）

0 引言

隨著科學技術和人民生活水平提高，無人機不僅廣泛應用在航拍、特技表演等領域，而且應用在物流配送領域，具有配送效率高、成本低、適應性強等突出優(yōu)點，在低空飛行中不易受復雜路況影響，可實現(xiàn)較快速度運輸，極大節(jié)約了人力成本。

無人機航跡規(guī)劃主要指無人機根據(jù)相關地圖信息計算出一條從起點位置到終點位置最優(yōu)、安全可行的路徑［1］。常用全局路徑規(guī)劃算法［2-3］包括RRT 算法［4］、遺傳算法［5-6］、蟻群算法［7-8］、Dijkstra 算法［9］、D*算法［10］、A*算法［11-12］等。其中，RRT 算法搜索速度快，但路徑通常遠離最優(yōu)路徑；遺傳算法、蟻群算法在收斂速度方面相對較慢；D*算法在動態(tài)環(huán)境中的搜索效果非常好，但在靜態(tài)全局環(huán)境下搜索效果不如A*算法；A*算法結(jié)構(gòu)簡單、搜索效率高，在靜態(tài)全局環(huán)境下能得到最優(yōu)解［13］，但路徑規(guī)劃需要一直查詢Openlist、Closelist 中的節(jié)點，并對其進行插入和刪除操作，隨著地圖信息增多，搜索節(jié)點大量增加，將使得算法運算時間較長［14］。

為此，相關學者針對A*算法的問題提出了很多改進方法。Korf［15］提出融合A*算法和迭代加深算法優(yōu)化狀態(tài)判重和估價排序，提升了算法效率。Duchon 等［16］采取一種新的節(jié)點搜索方法提升路徑搜索速度。高民東等［17］采用多鄰柵格距離公式和改進雙向時效的方法提升路徑規(guī)劃效率。王文明等［18］構(gòu)建正六邊形柵格地圖，修改算法的鄰居剪枝和強迫鄰居判斷規(guī)則，提升路徑規(guī)劃的質(zhì)量和效率。辛煜等［19］利用無限領域的思想解決A*算法的八領域搜索方向問題。

研究發(fā)現(xiàn)，現(xiàn)有路徑規(guī)劃算法具有各自優(yōu)勢，但均存在一定缺陷。例如，蟻群收斂速度慢，易陷入局部最優(yōu)；遺傳算法可獲得最優(yōu)解，但計算量大，因此不適用于大型實時性地圖。為此，本文根據(jù)A*算法優(yōu)點對其進行改良，具體為對搜索策略、啟發(fā)函數(shù)、起點搜索方向和搜索方法進行設計，使改進算法在物流無人機航跡規(guī)劃方面具有更優(yōu)路徑、更短的搜索時間。

1 算法基礎

1.1 A*算法

A*算法是一種基于柵格的啟發(fā)式搜索算法，結(jié)合Dijkstra、BFS 算法的理論優(yōu)勢，按照f(n) 值從小到大排序Openlist 的節(jié)點，選取代價最小的節(jié)點作為下一個父節(jié)點，循環(huán)處理該步驟直至搜索到終點位置完成路徑規(guī)劃，最終生成規(guī)劃路徑。A*算法的評價函數(shù)為：

A*算法的基本流程如下：

步驟1：創(chuàng)建、初始化Openlist、Closelist 集合，將起點位置放入Openlist。

步驟2：計算Openlist 中所有節(jié)點的f(n)值，按照f(n)值排序Openlist的節(jié)點，將f(n)最小的節(jié)點設為當前節(jié)點。

步驟3：搜索當前節(jié)點周圍8 個方向的鄰域節(jié)點（見圖1），將處理后符合要求的節(jié)點加入Openlist，并將該節(jié)點加入Closelist。

Fig.1 Search direction圖1 搜索方向

步驟4：判斷當前節(jié)點是否為終點位置，如果是終點位置則開始溯源每一個節(jié)點的父節(jié)點直至起始位置，連接所有父節(jié)點就是最終規(guī)劃路徑，如果不是終點位置則繼續(xù)執(zhí)行步驟2、步驟3。

1.2 跳點搜索算法

Harabor 等［20］基于A*算法的路徑規(guī)劃策略，通過減少搜索過程產(chǎn)生的大量中間節(jié)點，從而減少計算量的全局路徑規(guī)劃，提出跳點搜索（Jump Point Search）算法。JPS 算法的規(guī)劃策略包含跳點篩選規(guī)劃和評價函數(shù)。

1.2.1 自然鄰域節(jié)點

在柵格地圖中，JPS 算法搜索方向分為水平、豎直和斜向搜索，如圖2 所示。水平搜索時，x節(jié)點是由其父節(jié)點（4號節(jié)點）擴展得到，當x節(jié)點的八鄰域節(jié)點無障礙物時，x節(jié)點擴展的5 號節(jié)點為其自然鄰域節(jié)點；斜向搜索時，x節(jié)點由其父節(jié)點（6 號節(jié)點）擴展得到，當x節(jié)點的八鄰域節(jié)點無障礙物，則x節(jié)點擴展的2、3、5 號節(jié)點為其自然鄰域節(jié)點。

Fig.2 JPS algorithm search direction圖2 JPS算法搜索方向

1.2.2 強制鄰域節(jié)點

在柵格地圖中，強制鄰域節(jié)點搜索方向同自然鄰域節(jié)點相同，如圖3 所示。水平搜索時，當x節(jié)點的八鄰域節(jié)點有障礙物，擴展的3 號節(jié)點為其強制鄰域節(jié)點；斜向搜索時，當x節(jié)點的八鄰域節(jié)點有障礙物，擴展的1 號節(jié)點為其強制鄰域節(jié)點。

Fig.3 Mandatory neighborhood nodes圖3 強制鄰域節(jié)點

1.2.3 跳點

JPS 算法規(guī)劃路徑時，當x為當前點，p(x)為x的父節(jié)點，搜索方向為水平、豎直或斜向，滿足以下3 個條件之一即為跳點：①節(jié)點x為終點，則x為跳點；②節(jié)點x至少有一個強制鄰域節(jié)點，則x為跳點；③斜向搜索時，按照x節(jié)點水平或豎直方向能到達跳點，則x為跳點。

2 改進A*算法

在較大范圍無人機物流配送環(huán)境，從郊區(qū)倉庫投送物資到城區(qū)投送點的過程中，大部分為空曠區(qū)域，而到投送終點附近存在大量建筑或其它不可穿越的障礙，使用A*算法規(guī)劃路徑存在計算量巨大、規(guī)劃實時性差等問題。

為此，本文根據(jù)無人機物流配送飛行路線和環(huán)境特點設計算法，在A*算法搜索策略的基礎上融入JPS 算法，并設計起點搜索方法和改進評價函數(shù)，從而提升A*算法路徑搜索性能。

2.1 融入JPS搜索策略

A*算法搜索過程會對每一步符合要求的鄰居節(jié)點加入Openlist，然后循環(huán)排序并選取Openlist 中節(jié)點代價最小值進行節(jié)點搜索。隨著地圖增大，算法計算量將呈指數(shù)增長，極大降低了算法的實時性。如圖4所示，A*算法搜索過程中加入了Openlist 中的37 個節(jié)點，融入JPS 算法的搜索策略可使A*算法搜索時先篩選符合要求的跳點，再將跳點放入Openlist 列表，算法只需處理Openlist 中少量的跳點即可實現(xiàn)路徑搜索。如圖5 所示，JPS 算法的搜索過程中加入了Openlist中的8個節(jié)點。

Fig.4 A * algorithm search process圖4 A*算法搜索過程

Fig.5 Search process of JPS algorithm search strategy圖5 JPS算法的搜索策略搜索過程

2.2 設計起點搜索方法

當使用JPS 算法的搜索策略搜索時，起點作為當前節(jié)點進行搜索時無法判斷當前節(jié)點的搜索方向，因此會對水平、豎直或斜向8 個方向進行跳點搜索。當起點位置附近環(huán)境障礙物很少時，會消耗大量時間在8 個方向搜索跳點。為了解決該缺陷，本文設計了一種起點搜索方法，將起點的非障礙物八鄰域節(jié)點加入Openlist，然后計算起點到終點直線方向的角度，根據(jù)角度范圍選擇8 個方向中最接近的方向作為起點搜索方向，如圖6所示。

Fig.6 Starting point search direction圖6 起點搜索方向

2.3 改進評價函數(shù)

A*算法的評價函數(shù)由實際代價函數(shù)g(n)和預計代價函數(shù)h(n)組合而成，如式（1）所示。h(n)常用的預計代價函數(shù)包括曼哈頓距離（見式（3））、歐幾里距離（見式（4））、切比雪夫距離（見式（5））、Octile 距離（見式（6））等，其中(x1，y1)為當前點坐標，(x2，y2)為目標點坐標。

h(n)函數(shù)是A*算法的核心，當h(n)小于等于實際代價時，算法能保證尋找到最優(yōu)路徑，但運算時間較長。當h(n)大于實際代價時，算法無法保證尋找到最優(yōu)路徑，但運算時間很短。為此，本文根據(jù)這個特性設計一個權重值w乘以h(n)以調(diào)節(jié)預計代價值。由圖7 可見，在此類柵格地圖中使用Octile 距離公式（見式（6））更符合要求。

Fig.7 Euclidean distance and Octile distance圖7 歐氏距離和Octile距離

2.4 改進A*算法

改進A*算法基于A*算法結(jié)構(gòu)和JPS 搜索策略，融入跳點搜索算法的搜索策略的目的是減少A*算法搜索過程中產(chǎn)生的大量中間節(jié)點；設計起點搜索方法目的是減少起點在空曠區(qū)域搜索的時間，結(jié)合這兩種改進策略能明顯縮短算法的搜索時間。當算法規(guī)劃的預計代價h(n)小于等于實際代價時，算法保證能尋找到最優(yōu)路徑，改進評價函數(shù)通過設計一個權重值w乘以調(diào)節(jié)預計代價值，從而平衡算法搜索時間并判斷是否為最優(yōu)路徑。本文將3 種方法結(jié)合構(gòu)成改進A*算法，如圖8所示。

Fig.8 Flow of improved A * algorithm圖8 改進A*算法流程

創(chuàng)建Openlist 和Closelist 兩個列表，設置起點、終點坐標和權重值w改進A*算法的步驟如下：

步驟1：初始化Openlist、Closelist，將起點加入Openlist。

步驟2：將起點周圍8 個方向非障礙節(jié)點加入Openlist，計算起點得到終點直線方向，選擇8 個方向中最接近的方向作為起點搜索方向。

步驟3：選取起點作為當前節(jié)點，按照起點搜索方向改進A*算法搜索跳點，若有跳點將其加入Openlist，并將起點從Openlist中移除，然后加入到Closelist。

步驟4：按照f(n)值從小到大排序Openlist 節(jié)點，選取f(n)最小值作為當前節(jié)點。

步驟5：判斷當前節(jié)點是否為目標節(jié)點，若是將轉(zhuǎn)到步驟10，否則從當前節(jié)點開始擴展搜索。

步驟6：改進A*算法搜索一步，尋找跳點。

步驟7：是否尋找到跳點，若找到將跳點加入Openlist。

步驟8：將當前節(jié)點從Openlist 移出，然后加入Closelist。

步驟9：轉(zhuǎn)到步驟4。

步驟10：回溯目前最短路徑，尋路完成。

3 仿真驗證與分析

物流無人機實際配送環(huán)境屬于三維環(huán)境，但對大范圍三維環(huán)境進行建模較為困難、計算量大、對仿真計算平臺要求高。二維環(huán)境是三維環(huán)境的分支，可在降低仿真平臺要求的同時，準確獲得算法的各項比較數(shù)據(jù)，并且本文算法應用場景主要在固定高度進行無人機實時規(guī)劃及動態(tài)避障，因此選取在二維環(huán)境下對規(guī)劃算法進行仿真。

為了驗證改進A*算法與原始A*、JPS算法的優(yōu)勢，分別在不同尺寸的柵格地圖進行仿真分析，實驗計算機操作系統(tǒng)為Windows 10，運行內(nèi)存為16 GB。

首先，建立一個50×50 的柵格地圖，仿真中將物流無人機視為一個質(zhì)點，初始點設置為（15，15），目標點設置為（49，49），權重值w=1。保持地圖障礙不變，分別使用A*算法、JPS 算法和改進A*算法進行實驗仿真（見圖9-圖11），實驗數(shù)據(jù)如表1所示。

Table 1 Path planning data of three algorithms in 50×50 map environment表1 3種算法的路徑規(guī)劃在50×50地圖環(huán)境下的數(shù)據(jù)

Fig.9 Path planning of A* algorithm in 50×50 map environment圖9 A*算法在50×50地圖環(huán)境下的路徑規(guī)劃

Fig.10 Path planning of JPS algorithm in 50×50 map environment圖10 JPS算法在50×50地圖環(huán)境下的路徑規(guī)劃

由表1 可知，保持預計代價小于等于實際代價時，改進A*算法在生成路徑長度上保持了A*算法的最優(yōu)路徑，起點搜索方法和JPS 算法搜索策略使改進A*算法在搜索時間上相較于A*算法、JPS 算法減少90.33%、55.51%。仿真結(jié)果表明，改進A*算法顯著提升了搜索效率，實現(xiàn)了路徑優(yōu)化目標。

下一步，在仿真中建立一個100×100 的柵格地圖，將物流無人機視為一個質(zhì)點，初始點設置為（30，30），目標點設置為（99，99），權重值w=1。保持地圖障礙不變，分別使用A*算法、JPS 算法、和改進A*算法進行實驗仿真（見圖12-圖14），實驗數(shù)據(jù)如表2所示。

Table 2 Path planning data of three algorithms in 100×100 map environment表2 3種算法的路徑規(guī)劃在100×100地圖環(huán)境下的數(shù)據(jù)

Fig.12 Path planning of A* algorithm in 100×100 map environment圖12 A*算法在100×100地圖環(huán)境下的路徑規(guī)劃

Fig.13 Path planning of JPS algorithm in 100×100 map environment圖13 JPS算法在100×100地圖環(huán)境下的路徑規(guī)劃

Fig.14 Path planning of improved A* algorithm in 100×100 map environment圖14 改進A*算法在100×100地圖環(huán)境下的路徑規(guī)劃

由表2 可知，在地圖變大的情況下，改進A*算法依然在生成的路徑長度上保持了A*算法的最優(yōu)路徑，并在搜索時間上相較于A*算法、JPS 算法分別減少97.01%、54.35%。仿真表明，地圖變大后改進A*算法依然能顯著提升搜索效率，實現(xiàn)路徑優(yōu)化目標。

4 結(jié)語

本文通過融入JPS 算法搜索策略，設計起點搜索方法和改進評價函數(shù)，提出一種改進A*算法。仿真表明，改進A*算法相較于A*算法、JPS 算法，既能顯著減少搜索時間，又能保持A*算法的最優(yōu)路徑，在更復雜或更大尺寸地圖環(huán)境下算法優(yōu)勢更明顯，也可應用于港口、工廠等場景下的移動機器人路徑規(guī)劃領域。

然而，改進A*算法的路徑規(guī)劃目前并未進行優(yōu)化，未考慮無人機轉(zhuǎn)彎半徑、航程等因素，后續(xù)將對算法進行優(yōu)化，以進一步增加無人機在實際飛行中的適應性。