楊聿壬 郭江宇 董曉峰 趙陽

摘要：由于傳統(tǒng)的A*算法存在搜索角度固定、經(jīng)過障礙物時過于靠近障礙物、易導致碰撞、轉(zhuǎn)向角度不夠平滑的問題，提出一種改進A*算法的安全路徑規(guī)劃方法。對A*算法中的搜索鄰域進行擴展，使搜索方向之間的夾角不再局限于45°；基于二維高斯分布設(shè)計安全距離矩陣，在啟發(fā)函數(shù)中加入當前節(jié)點危險值；采用三次均勻B樣條曲線對路徑進行平滑處理。經(jīng)過仿真驗證，改進的A*算法能夠減少算法迭代次數(shù)，提升路徑平滑程度，能夠有效避免路徑存在障礙物，提高了路徑的安全性。

關(guān)鍵詞：改進A*算法；路徑規(guī)劃；擴展搜索方向；安全距離矩陣；三次均勻B樣條曲線

中圖分類號：TP301.6? ? ?文獻標識碼：A

文章編號：1009-3044（2024）09-0001-04

開放科學（資源服務(wù)）標識碼（OSID）

0 引言

路徑規(guī)劃是綜合考慮環(huán)境和目的地信息，為無人機設(shè)計出一條最優(yōu)的無碰撞行動路線。A*算法作為靜態(tài)路網(wǎng)中搜尋最短路徑的常用算法，具有搜索性能好、準確度高的優(yōu)點[1]。在A*算法中無人機通常被等效為柵格中心的一個質(zhì)點，未考慮無人機本身的體積和機動可能與障礙物產(chǎn)生碰撞。安全路徑是在考慮了無人機本身實際大小的情況下，設(shè)計出一條與障礙物保持一定安全距離的行動路線，為無人機與障礙物之間留下可以機動的空間，同時避免從障礙物之間狹窄縫隙處通過。

針對A*算法中的不足，國內(nèi)外許多學者進行不同的改進。文獻[2]和文獻[3]對A*算法的搜索鄰域進行擴展，使得路徑的拐點減少，路徑總長度變短，路徑也更加平滑，能夠適應(yīng)無人機的運動。文獻[4]提出稀疏A*算法，通過設(shè)置路徑的約束條件，裁剪搜索空間中的冗余節(jié)點，減少了搜索時間。文獻[5]采用球形節(jié)點拓展法，基于無人機最小轉(zhuǎn)彎半徑和最大探測距離，采用變步長搜索策略，縮短了規(guī)劃時間，減少了規(guī)劃步長。文獻[6]提出了一種A*波紋減少算法，降低了分辨率的變化造成的路徑長度變化的波紋效應(yīng)，并通過平滑算法的改進縮短了路徑長度。文獻[7]提出了一種組合式啟發(fā)函數(shù)，采用曼哈頓距離和歐氏距離的線性組合描述距離代價；同時采用雙向搜索策略，對傳統(tǒng)A*算法進行了優(yōu)化。文獻[8]提出了一種改進A*與人工勢場法相結(jié)合的算法，將A*算法的解作為引力之一，引入偏航引力增益系數(shù)和柵格危險度，提高了路徑的安全系數(shù)。文獻[9]提出一種帶四角安全距離矩陣的改進A*算法，但該方法局限于矩形障礙物檢測，較小或細長的障礙物在轉(zhuǎn)角處檢測效果不好。文獻[10]對A*算法子節(jié)點擴展方法進行改進，不向障礙物方向進行搜索，該方法局限于擴展節(jié)點的8鄰域，保持的安全距離固定，缺乏靈活性。文獻[11]針對無人機三維空間路徑規(guī)劃，采用自適應(yīng)搜索方向，縮減搜索空間；重新設(shè)計了威脅函數(shù)，將威脅空間以概率的形式加入代價函數(shù)中。

上述的改進算法在不同程度上提高了傳統(tǒng)A*算法的性能，但是并沒有很好地解決路徑規(guī)劃中安全距離的問題。當前考慮安全距離的改進A*算法大多仍考慮的是節(jié)點的4鄰域或8鄰域，不能保證更遠的安全距離，且避障過程中轉(zhuǎn)向角度固定、轉(zhuǎn)折點過多，不利于復雜環(huán)境下規(guī)避障礙。針對這些不足，本文提出了擴展搜索方向矩陣，使得改進A*算法的步長更靈活，轉(zhuǎn)向角度不再局限于45°，提高了算法效率；提出基于二維高斯分布的安全距離矩陣，優(yōu)化啟發(fā)函數(shù)，在選擇節(jié)點時遠離障礙物，同時提出路徑危險評估方法，對算法改進前后的路徑進行對比評估；采用三次均勻B樣條曲線對路徑進行平滑處理。

1 傳統(tǒng)A*算法

A*算法在Dijkstra算法的基礎(chǔ)上，通過添加啟發(fā)函數(shù)來優(yōu)化代價函數(shù)，既考慮了從起點到當前節(jié)點的累計代價，又考慮了當前節(jié)點到終點的估計代價，從而在保證了搜索效率的同時，又得到了較短的路徑。因此，在靜態(tài)的網(wǎng)格地圖路徑規(guī)劃中，A*算法成為求解最短路徑的常用算法之一，其啟發(fā)函數(shù)如式（1） 所示：

[f（N）=g（N）+h（N）] （1）

其中，[f（N）]為當前節(jié)點的總代價，[g（N）]為起點到當前節(jié)點的代價，[h（N）]為當前節(jié)點到終點的啟發(fā)函數(shù)值，通常有歐幾里得距離、曼哈頓距離、切比雪夫距離三種計算方法。

傳統(tǒng)A*算法的流程圖如圖1所示，對其詳細說明如下。

1） 采用柵格法構(gòu)建環(huán)境地圖，用固定行數(shù)和列數(shù)的矩陣表示環(huán)境地圖。矩陣中值為0表示可通行區(qū)域，顯示為白色；值為1表示障礙物區(qū)域，不可通行，顯示為黑色。

2） 建立open列表和close列表。open列表用來存儲當前點下一步運動的候選節(jié)點，close列表用來存儲算法計算出的待定路徑節(jié)點。將起點放入open列表，設(shè)置close列表為空。

3） 從open列表中選擇總代價最小的節(jié)點N，將其移動到close列表。在算法第一輪迭代中，open列表中只有起點，可直接將起點移動到close列表中。

4） 判斷選出的節(jié)點N是否為終點，如果是終點，則根據(jù)父節(jié)點矩陣回溯至起點，生成路徑，結(jié)束循環(huán)。如果節(jié)點N不是終點，則生成當前點下一步運動的候選點列表M。候選點通常為當前點的4鄰域或8鄰域。

5） 判斷M中的候選點是否在柵格地圖中的障礙物上，如果在障礙物上，則從M列表中去除；不在則保留。更新M列表，并計算M列表中各候選點的總代價。

6） 判斷M列表中的候選點是否已經(jīng)存在于open列表中。如果open列表中沒有相同節(jié)點，則保留；反之，比較候選點在open列表中和M列表中的總代價，選擇總代價小的一方更新到M列表中。

7） 將M列表中的候選點坐標及總代價更新到open列表中，同時把當前節(jié)點N設(shè)置為M列表中各候選點的父節(jié)點，記錄到父節(jié)點列表中。返回步驟3） ，循環(huán)至終點被找到。

2 改進A*算法

傳統(tǒng)A*算法中，各個搜索方向之間的角度限定在45°，即8鄰域內(nèi)，限制了搜索步長和方向，使得路徑搜索不夠靈活，也使得路徑的轉(zhuǎn)角過大，所得路徑并非最優(yōu)解。同時，在經(jīng)過障礙物時，路徑通常緊貼障礙物邊緣，在實際應(yīng)用中，容易產(chǎn)生碰撞風險。因此，本文采用擴展搜索方向矩陣、安全距離矩陣和三次均勻B樣條曲線平滑路徑，對上述傳統(tǒng)A*算法中的不足進行改進。

2.1 擴展搜索方向矩陣

傳統(tǒng)A*算法中，當前節(jié)點的下一待擴展候選節(jié)點通常位于當前節(jié)點的8鄰域中，如圖2中的圖（a） 所示。為了擴展搜索的方向，本文采用了如圖2中右圖的擴展搜索方向矩陣作為待擴展候選節(jié)點。圖中坐標（0，0） 處為當前節(jié)點，最內(nèi)圈與傳統(tǒng)A*算法中的8鄰域相同。次內(nèi)圈的16個節(jié)點中，去掉了與最內(nèi)圈方向相同的8個節(jié)點，保留方向不同的其余8個節(jié)點。以此類推，可以繼續(xù)向外側(cè)擴展，矩陣的半徑（即步長參數(shù)）越大，搜索方向之間的夾角越小，搜索過程就越精準，能夠有效減少路徑長度和轉(zhuǎn)向角度。由式（2） 可求得不同步長參數(shù)對應(yīng)的搜索方向數(shù)。

[S=8×1+R×R-12] （2）

其中，[R]為步長參數(shù)，表示當前節(jié)點到最遠待擴展的候選節(jié)點的切比雪夫距離，[S]表示當前節(jié)點可以擴展的方向的個數(shù)。

2.2 安全距離矩陣和評估方法

為了計算每個節(jié)點的危險值，以該節(jié)點為中心建立基于二維高斯分布的安全距離矩陣[MS]，矩陣的每個元素可以由式（3） 求得。

[MS（m，n）=e-12m2+n2a2×b，-d0≤m，n≤d0] （3）

其中，[m]和[n]表示矩陣元素相對于矩陣中心的位置，[d0]表示安全距離閾值，與無人機的機身大小有關(guān)，[a]和[b]為比例參數(shù)。依照本文仿真實驗中的參數(shù)設(shè)置，[d0=2，a=1，b=2]，可以得到如圖3所示的安全距離矩陣。

使用得到的安全距離矩陣，依照圖4所示示意圖，可以計算出當前節(jié)點N的危險值。圖中黃色區(qū)域為安全距離矩陣的范圍，橙色區(qū)域為處于安全距離矩陣中的障礙物區(qū)域，由于柵格地圖中，可通行區(qū)域的值為0，障礙物區(qū)域的值為1，使用安全距離矩陣中元素與柵格地圖對應(yīng)位置的值相乘，再求和，即可得到節(jié)點N的危險值[RiskN]，表達式如式（4） 所示。

[RiskN=xN-d0≤m≤xN+d0yN-d0≤n≤yN+d0MSm，n×MAPm，n] （4）

其中，[MAP]為柵格地圖矩陣，[xN]和[yN]分別表示當前節(jié)點N的橫坐標和縱坐標。

對A*算法中的啟發(fā)函數(shù)[h（N）]進行優(yōu)化，使其能更合理地反映當前節(jié)點到終點的距離估計代價。歐幾里得距離為兩點間線段的距離，路徑規(guī)劃中起點和終點之間通常都存在著障礙物，因此實際距離比歐幾里得距離大；曼哈頓距離為兩點橫縱坐標之差的絕對值，再求和，類似沿坐標軸方向繞開障礙物的距離，但本文使用的改進A*算法可以沿多個角度的斜線進行路徑規(guī)劃，實際距離比曼哈頓距離要小。因此本文的啟發(fā)函數(shù)為當前節(jié)點到終點的加權(quán)歐幾里得距離與加權(quán)曼哈頓距離之和，再加上式（4） 計算出的危險值，更貼合實際距離的同時更傾向遠離障礙物的路徑，[h（N）]的計算如式（5） 所示。

[h（N）=ω1×EuN+ω2×MhN+RiskN] （5）

其中[EuN]表示歐幾里得距離，[MhN]表示曼哈頓距離，加權(quán)系數(shù)[ω1=ω2=0.5]。

完成路徑規(guī)劃后，需要對整體路徑進行危險評估。在路徑上等距離取[T]個采樣點，計算每一個采樣點的危險值并累加，得到路徑危險評估值[FR]，計算式如下：

[FR=i=1TRρNi，Nobs] （6）

[RρNi，Nobs=e-12ρa2×b， ρ0，ρ≥d0

其中，[ρNi，Nobs]為第[i]個采樣點到最近的障礙物的距離。

2.3 基于B樣條（B-spline） 路徑平滑

采用柵格地圖進行路徑規(guī)劃，計算出的路徑通常轉(zhuǎn)彎較多，且轉(zhuǎn)向角度比較大，不利于應(yīng)用至實際工程中。而B樣條路徑優(yōu)化具有幾何不變性、仿射不變性等優(yōu)點，使得擬合曲線與原始路徑的差異最小。因此，本文采用三次均勻B樣條曲線對A*算法計算出的路徑進行優(yōu)化，將原始路徑的所有節(jié)點作為B樣條曲線的控制頂點，輸出經(jīng)過平滑后的路徑。

B樣條曲線可以表示為下面的表達式：

[Bsplineu=i=0nPiNi，k（u）] （8）

其中[Pi]為控制頂點的坐標，[Ni，k]是[k]次曲線的B樣條基函數(shù)，[u]是歸一化的非遞減節(jié)點向量，長度為[n+k+1]?；瘮?shù)是由節(jié)點向量[u]所決定的[k]次分段多項式。

基函數(shù)通常采用Cox-deBoor遞推公式計算，計算公式如式（9） （10） 所示：

[Ni，0u=1， if ui≤u≤ui+10， others] （9）

[Ni，k=u-uiui+k-uiNi，k-1u+ui+k+1-uui+k+1-ui+1Ni+1，k-1u] （10）

式（9） 為0次基函數(shù)，式（10） 為[1～k]次基函數(shù)。其中[i]為節(jié)點序號，[k]為基函數(shù)的次數(shù)。

3 仿真實驗

為了驗證本文提出的改進A*算法的有效性，使用MATLAB 2017a 進行仿真，并與傳統(tǒng)A*算法做比較。仿真的地圖為[100×100]的規(guī)則障礙物柵格地圖，本文的改進A*算法的步長參數(shù)[R=3]，安全距離矩陣的比例參數(shù)[a=2]，[b=1]，安全距離閾值[d0=2]。傳統(tǒng)A*算法的步長參數(shù)[R=1]，即下一步待探索的候選點為當前點的8鄰域，啟發(fā)函數(shù)中不包括危險值。兩種方法的對比仿真結(jié)果如圖5所示。

圖中紅色路徑為使用傳統(tǒng)A*算法計算得出，可以看到在圖中紅色圓圈處，該路徑與障礙物邊緣十分靠近，甚至從兩個障礙物的狹窄縫隙間通過，在實際應(yīng)用中容易產(chǎn)生碰撞危險。藍色路徑為本文改進A*算法計算結(jié)果，可以明顯對比出，該路徑繞開了障礙物狹窄處，且與障礙物邊緣保持一定距離。對兩條路徑都使用式（6） 和式（7） 提出的安全評估方法，得到的結(jié)果如表1所示。

從表1中可以看出，本文提出的改進A*算法在使用安全距離矩陣計算每個節(jié)點的危險值并添加到啟發(fā)函數(shù)中后，可以有效避開狹窄通道等危險路徑，也可以有效繞開障礙物。但為了保證路徑安全，不可避免地需要繞路，導致路徑長度略有增加。改進A*算法相比傳統(tǒng)算法，迭代次數(shù)略微下降，路徑長度略微增大了3.76%，但是能夠完全避免路徑周圍2個單元格內(nèi)存在障礙物，極大地提高了路徑的安全性，造成的路徑長度提升在可以接受的范圍內(nèi)。

表2中給出了相同的安全距離矩陣參數(shù)設(shè)定下，改進A*算法選擇不同的步長參數(shù)時，計算出的路徑結(jié)果的數(shù)據(jù)。當[R=1]時，即為傳統(tǒng)A*算法只采取安全距離優(yōu)化，相比傳統(tǒng)A*算法，不存在路徑碰撞危險，迭代次數(shù)只增加了1.26%，但為了繞開障礙物，路徑長度增加了7.22%。通過本文提出的擴展搜索方向，隨著步長參數(shù)的增大，迭代次數(shù)和路徑長度均減少。但步長參數(shù)增大到一定值時，由于兩個節(jié)點之間距離較大，使得兩點間的路徑可能靠近障礙物，使路徑存在碰撞的危險。

表3中給出了不同安全距離閾值[d0]計算出的路徑的對比數(shù)據(jù)。隨著[d0]增大，驅(qū)使路徑遠離障礙物，使得路徑長度進一步增大；同時本文的仿真地圖較為狹窄，過大的安全距離閾值使得計算危險值時將過多的障礙物內(nèi)部的單元格計算在內(nèi)，降低了路徑安全的優(yōu)化效果。因此，安全距離閾值需要考慮柵格地圖的障礙物密度，不宜過大。

圖6為三次均勻B樣條曲線優(yōu)化對比圖，圖（b） 中的局部路徑比較圖為圖（a） 中藍色方框處放大后的示意圖。從圖中的對比可以看出，優(yōu)化后的曲線更加平滑。通過計算，優(yōu)化前的轉(zhuǎn)角總和為396.9°，優(yōu)化后的轉(zhuǎn)角總和為362.8°，降低了8.59%。

4 總結(jié)

本文針對傳統(tǒng)A*算法中，搜索角度固定、路徑與障礙物碰撞風險較大以及路徑不夠平滑的問題，對A*算法進行了改進。在選擇下一個擴展節(jié)點時，引入擴展搜索方向矩陣，使得搜索的步長和方向更靈活；在啟發(fā)函數(shù)中增加安全距離矩陣計算出的節(jié)點危險值，使得規(guī)劃的路徑能夠遠離障礙物；使用三次均勻B樣條曲線對計算出的路徑進行平滑優(yōu)化。通過仿真實驗表明，改進后的A*算法規(guī)劃出的路徑，安全性顯著上升，平滑程度有一定提升，同時算法的迭代次數(shù)略微下降。

參考文獻：

[1] 路晶，史宇，張書暢，等.無人機航跡規(guī)劃算法綜述[J].航空計算技術(shù)，2022，52（4）：131-134.

[2] 辛煜，梁華為，杜明博，等.一種可搜索無限個鄰域的改進A*算法[J].機器人，2014，36（5）：627-633.

[3] 張敬寒，陶兆勝，彭澎，等.基于擴大搜索鄰域A*算法的平滑路徑規(guī)劃[J].長春理工大學學報（自然科學版），2018，41（6）：124-127，146.

[4] SZCZERBA R J，GALKOWSKI P，GLICKTEIN I S，et al.Robust algorithm for real-time route planning[J].IEEE Transactions on Aerospace and Electronic Systems，2000，36（3）：869-878.

[5] 馬云紅，張恒，齊樂融，等.基于改進A？算法的三維無人機路徑規(guī)劃[J].電光與控制，2019，26（10）：22-25.

[6] ZAMMIT C，VAN KAMPEN E J.Advancements for A* and RRT in 3D path planning of UAVs[C]//AIAA Scitech 2019 Forum.7-11 January 2019，San Diego，California.Reston，Virginia：AIAA，2019：0920.

[7] 姜月秋，李紫嫣，關(guān)啟學，等.基于改進A*算法的無人機路徑規(guī)劃研究[J].兵器裝備工程學報，2020，41（9）：160-164.

[8] 劉光才，馬寅松，齊福強，等.基于改進A～*-人工勢場法的城市物流無人機路徑規(guī)劃[J].飛行力學，2022，40（06）：16-23.

[9] 段書用，王啟帆，韓旭，等.具有確保安全距離的A*路徑優(yōu)化方法[J].機械工程學報，2020，56（18）：205-215.

[10] 高九州，徐威峰，張立輝，等.基于改進A*算法的無人機避障航線規(guī)劃[J].現(xiàn)代電子技術(shù)，2023，46（8）：181-186.

[11] 卞強，孫齊，童余德.一種新的改進A～*算法無人機三維路徑規(guī)劃[J].武漢理工大學學報，2022，44（7）：80-88.

【通聯(lián)編輯：梁書】