基于理論最短距離變權(quán)重A*算法的路徑規(guī)劃

，

(1.鄭州航空工業(yè)管理學(xué)院 機(jī)電工程學(xué)院， 鄭州 450015； 2.中原工學(xué)院 電子信息學(xué)院， 鄭州 450015)

0 引言

路徑規(guī)劃是一個帶約束的復(fù)雜優(yōu)化問題，是在具有障礙物的環(huán)境內(nèi)按照一定的評價準(zhǔn)則尋找一條從起點到終點的無碰撞路徑，最短通過路徑是選擇最優(yōu)路徑的重要準(zhǔn)則[1]。關(guān)于最短路徑算法常用的有蜂群算法[1]、蟻群算法[2]、遺傳算法[3]、粒子群算法[4]等啟發(fā)式算法。這類基于種群的生物啟發(fā)算法，存在早熟收斂的缺陷，容易陷入局部最優(yōu)，并且算法的函數(shù)較為復(fù)雜，若初始種群較大，則迭代次數(shù)增加，收斂時間較長[5]。A*算法是一種帶有啟發(fā)函數(shù)的最短路徑算法，因其算法簡單，搜索效率高而廣泛應(yīng)用于城市道路路徑規(guī)劃[6]。A*算法以節(jié)點作為搜索對象，只能在格子環(huán)境中應(yīng)用，如果將實際障礙物環(huán)境柵格化處理，用節(jié)點坐標(biāo)表示道路和障礙物信息，就可以利用A*算法來規(guī)劃最短路徑。但是與城市道路不同，柵格化處理的道路信息，節(jié)點是等間距兩兩相連的，因此，其搜索最壞的時間復(fù)雜度為O(n2)，當(dāng)節(jié)點數(shù)n增多時，時間復(fù)雜度也將大大增加。

為了提高A*算法的搜索效率，優(yōu)化途徑大致分為3種，一種是通過限制搜索區(qū)域減少搜索節(jié)點數(shù)量，文獻(xiàn)[6]根據(jù)起點到終點的歐式距離，在兩類不同大小的橢圓中尋找最短路徑，當(dāng)歐式距離較遠(yuǎn)時，可降低33%～47%的復(fù)雜度。但是若歐式距離較短，則僅限制搜索區(qū)域?qū)λ阉餍侍岣卟⒉伙@著。

第二種是通過改進(jìn)啟發(fā)函數(shù)減少算法遍歷的節(jié)點數(shù)，提高搜索效率。文獻(xiàn)[7]以當(dāng)前搜索節(jié)點到目的節(jié)點的距離作為估計權(quán)值，通過改變啟發(fā)函數(shù)權(quán)重來控制搜索效率和精度，獲得最優(yōu)路徑。文獻(xiàn)[8]用威脅區(qū)域半徑與航跡點到威脅區(qū)域的距離為權(quán)值，改變估價函數(shù)權(quán)重規(guī)劃出合理的飛行軌跡。上述通過改變啟發(fā)函數(shù)權(quán)重，可提高算法搜索效率，但是得到的規(guī)劃路徑可能不是最短路徑。

第三種是通過優(yōu)化open表排序過程，利用二叉堆存儲open表中節(jié)點，提高節(jié)點插入和刪除效率，縮短搜索時間[9]。在柵格地圖中，由于節(jié)點數(shù)量較大，絕大部分搜索時間用以考察節(jié)點，優(yōu)化open表排序?qū)λ阉餍实奶岣哂绊懖淮蟆?/p>

因此，本文將實際障礙物地圖柵格化后，提出了在規(guī)定的搜索區(qū)域內(nèi)基于理論最短路徑的變權(quán)重A*算法。算法不僅以橢圓區(qū)域限定了搜索范圍，而且動態(tài)改變啟發(fā)函數(shù)權(quán)重控制搜索效率和精度，通過對估計代價施加懲罰函數(shù)，優(yōu)先選擇靠近理論最短路徑的節(jié)點，保證了規(guī)劃路徑為最短路徑，同時也可將搜索范圍控制在理論最短路徑周圍較小范圍內(nèi)。通過實例證明，該算法在保證搜索精度的前提下，明顯提高了搜索效率，且規(guī)劃路徑最接近理論最短路徑。

1 障礙物環(huán)境及搜索區(qū)域規(guī)劃

1.1 障礙物環(huán)境柵格化處理

A*算法要求搜索環(huán)境為網(wǎng)格，因此利用柵格法對于實際障礙物環(huán)境建模，將已知的實際障礙物環(huán)境通過柵格化處理建立二維柵格圖[10-11]，如圖1所示，在柵格圖中路徑行進(jìn)規(guī)則為可按直線、對角線前進(jìn)。將障礙物柵格設(shè)置為黑色，自由柵格設(shè)置為白色，每個柵格的序號值m與坐標(biāo)位置(x,y)的關(guān)系為：

(1)

式中,mod為取余運(yùn)算；int為取整運(yùn)算；n為每行柵格數(shù)。

圖1 障礙物環(huán)境柵格化

本文要對實際障礙物及其環(huán)境進(jìn)行柵格化處理，由于實際障礙物的形狀可能不規(guī)則，若劃分的柵格未被障礙物全部填滿時，該柵格也不能作為自由柵格通行，因此，利用正方形柵格表達(dá)障礙物邊界時，要將未被障礙物邊界填滿柵格全部做填滿處理，如圖2所示。

圖2 障礙物邊界柵格填充處理

從圖中可知，柵格尺寸越小，填充后的障礙物邊界越平滑接近實際障礙物邊界，獲得的路徑也會更加平滑，同時節(jié)點數(shù)量也會大大增加，降低搜索效率，因此，為了平衡精度和效率，本文中柵格邊長取值為0.5。

1.2 搜索區(qū)域規(guī)劃

要提高最短路徑規(guī)劃效率，可通過限制搜索區(qū)域、減少搜索節(jié)點數(shù)量來實現(xiàn)。實驗證明，以起點O和終點D為焦點，最短路徑通過的節(jié)點(置信區(qū)間95%)都位于類似橢圓形的區(qū)域內(nèi)[6]。因此，可以用橢圓方程來限制搜索區(qū)域，橢圓標(biāo)準(zhǔn)方程如下：

(2)

(3)

在橢圓的搜索區(qū)域內(nèi)各節(jié)點i到O、D點的距離之和都應(yīng)滿足LiO+LiD≤2a。2a為橢圓長軸，確定了該值就可以規(guī)劃出確切的橢圓搜索范圍。要求解2a值，可采用統(tǒng)計分析方法，該方法要從柵格地圖中隨機(jī)抽取若干節(jié)點，節(jié)點兩兩構(gòu)成待求解最短路徑的起點和終點，求出兩點間的最短路徑記為Pab，兩點間的直線距離記為Lab，Pab與Lab的比值記為rab，選擇計算出的rab最大值計算橢圓長軸：

(4)

在障礙物尺寸接近的環(huán)境中，障礙物繞行所花費(fèi)的路程較為接近，因此通過統(tǒng)計分析得到的rab值彼此較為接近，具有普遍性，不會出現(xiàn)某一比值明顯大于其他值的情況，以此作為橢圓長軸計算依據(jù)較為可靠。但是，當(dāng)環(huán)境中有少數(shù)障礙物尺寸遠(yuǎn)大于其他障礙物尺寸，則繞行該大型障礙物所花費(fèi)的路程明顯增加，若隨機(jī)抽取的節(jié)點構(gòu)成的路徑?jīng)]有穿過該大型障礙物，則由統(tǒng)計得出的rab值與實際繞行大型障礙物的rab相比偏小，則該統(tǒng)計值rab不再具有典型性，其規(guī)劃的橢圓區(qū)域可能過小，路徑無法越過大型障礙物，使得搜索無法繼續(xù)，導(dǎo)致尋路失敗，如圖3。

圖3 橢圓搜索區(qū)域

為此，當(dāng)橢圓邊界點K到兩焦點距離之和LOK+LKD小于障礙物邊界M到兩焦點距離之和LOM+LMD時，應(yīng)以LOM+LMD與LOD的比值代替rab，即：

(5)

(6)

2 基于理論最短距離的變權(quán)重A*算法

2.1 經(jīng)典A*算法估價函數(shù)

A*算法是一種啟發(fā)算法，其搜索基本原理是：對搜索中遇到的每個新節(jié)點，按照啟發(fā)函數(shù)計算代價估值，將估值最小的節(jié)點作為當(dāng)前節(jié)點，繼續(xù)搜索。

經(jīng)典A*算法的估價函數(shù)為：

f(v)=g(v)+h(v)

(7)

在A*算法中，掃描open表中所有節(jié)點并按照f(v)值排序，找到f(v)值最小的節(jié)點作為當(dāng)前節(jié)點。g(v)為當(dāng)前節(jié)點到起點的實際代價，h(v)為當(dāng)前節(jié)點到終點的估計代價，均采用歐幾里德距離。估計代價h(v)體現(xiàn)了算法的啟發(fā)性，其大小決定了路徑搜索的方向，當(dāng)越接近終點時，估計代價值越小，其在估價函數(shù)中所占比例減小，搜索的方向性變差，效率降低。為了提高搜索效率，可適當(dāng)提高h(yuǎn)(v)在啟發(fā)函數(shù)中的比重，但又會造成在搜索初期搜索范圍過小而不能找到最優(yōu)路徑。為了平衡搜索效率和搜索精度，應(yīng)該實現(xiàn)在搜索初期以擴(kuò)大搜索范圍，提高搜索精度為主，搜索后期以快速接近終點為主的搜索策略。

2.2 變權(quán)重估價函數(shù)

為了在保證搜索精度的基礎(chǔ)上提高搜索效率，可動態(tài)改變實際代價的權(quán)重，令g(v)的權(quán)重為：

(8)

(9)

H為g(v)權(quán)重的上限值，L為g(v)權(quán)重的下限值，(xO,yO)為起點坐標(biāo)，(xD,yD)為終點坐標(biāo)。

該計算方法以實際代價g(v)與起點到終點直線距離LOD的比值作為實際代價的權(quán)重，在搜索前期該權(quán)重值過小，搜索范圍也小，搜索精度低，因此規(guī)定了權(quán)重的下限值L，保證搜索精度，越接近終點該權(quán)重越大，搜索精度高，但搜索范圍過大而效率降低，因而規(guī)定了權(quán)重的上限值H，保證搜索效率。

兩點間的直線距離為理論最短距離，在當(dāng)前節(jié)點(xi,yi)、起點(xO,yO)和終點(xD,yD)構(gòu)成的三角形中，節(jié)點到起點O和終點D的距離之和LOi+LiD越接近起點到終點直線距離LOD，則該節(jié)點位于最短路徑上的可能性越大。為了更快搜索到直線距離附近節(jié)點，對估計代價h(v)設(shè)計懲罰函數(shù)W′，該值與節(jié)點到LOD的距離成正比，遠(yuǎn)離LOD的點W′值大，接近LOD的點W′值小。W′的計算公式為：

(10)

為此，本文提出了變權(quán)重估價函數(shù)：

f(v)=Wg(v)+W′h(v)

(11)

在搜索過程中，靠近LOD直線的節(jié)點排序靠前被優(yōu)先選擇，因而算法的主要搜索區(qū)域自動控制在理論最短路徑LOD直線附近。

2.3 改進(jìn)A*算法實現(xiàn)步驟

本文提出的改進(jìn)A*算法，在經(jīng)典算法基礎(chǔ)上，限制了節(jié)點搜索范圍，并實時改變估價函數(shù)權(quán)重，在保證搜索精度的基礎(chǔ)上，加快了搜索速度。

1)獲得起點O和終點D坐標(biāo)，并計算兩點間的直線距離LOD；

2)計算橢圓參數(shù)a、b，確定搜索區(qū)域；

3)生成open列表和closed列表；

4)判斷節(jié)點是否在搜索區(qū)域內(nèi)，計算節(jié)點權(quán)重W和懲罰函數(shù)W′值，找到open列表中估價值f最小的節(jié)點作為當(dāng)前節(jié)點，如果該點為終點，則搜索結(jié)束，返回搜索路徑，如果不是終點，則向下執(zhí)行；

5)將當(dāng)前節(jié)點移出open列表，移入closed列表，將當(dāng)前結(jié)點相關(guān)節(jié)點加入open列表。

6)循環(huán)4)～5)步驟，直到當(dāng)前節(jié)點為終點，結(jié)束搜索，返回路徑。

3 路徑規(guī)劃仿真和結(jié)果分析

本文利用Matlab進(jìn)行了仿真實驗，驗證了在多種障礙物的情況下，本文提出算法的有效性。

實驗中，柵格地圖尺寸為100×100，g(v)權(quán)重的上限值H=0.8，下限值L=0.5，起點坐標(biāo)(98，58)，終點坐標(biāo)(2，40)。以搜索節(jié)點數(shù)，搜索時間、路徑長度為主要對比參數(shù)。

第一類為障礙物尺寸較為均勻的障礙物環(huán)境，在圖中任取20對點計算rab=1.45；第二類為障礙物尺寸相差較大的障礙物環(huán)境，其搜索范圍應(yīng)包含圖中最大障礙物，計算得到rab=1.71。實驗結(jié)果如圖4～11所示。

圖4 障礙物1下的經(jīng)典A*算法搜索節(jié)點范圍

圖5 障礙物1下的經(jīng)典A*算法搜索結(jié)果

圖6 障礙物1下的改進(jìn)A*算法搜索節(jié)點范圍

圖7 障礙物1下的改進(jìn)A*算法搜索結(jié)果

表1 障礙物1下的搜索參數(shù)

圖8 障礙物2下的經(jīng)典A*算法搜索節(jié)點范圍

圖9 障礙物2下的經(jīng)典A*算法搜索結(jié)果

圖10 障礙物2下的改進(jìn)A*算法搜索節(jié)點范圍

圖11 障礙物2下的改進(jìn)A*算法搜索結(jié)果

表2 障礙物2下的搜索參數(shù)

表1、表2對比了經(jīng)典A*算法和改進(jìn)A*算法在兩種不同障礙物的地圖中搜索結(jié)果。計算可知，在障礙物1的地圖中，經(jīng)典A*算法搜索節(jié)點數(shù)和搜索時間均為改進(jìn)A*算法的1.5倍，并且改進(jìn)后的A*算法搜索范圍較小，選擇搜索的節(jié)點更加靠近理論最短路徑LOD。對比兩種算法得到的路徑長度，可知，改進(jìn)后的A*算法規(guī)劃的路徑依然是全局最短路徑，但搜索效率大大提高。

在障礙物2的地圖中，經(jīng)典A*算法搜索節(jié)點數(shù)和搜索時間均為改進(jìn)A*算法的3.6倍，該地圖中存在尺寸較大的障礙物，經(jīng)典A*算法在尋找越過障礙物路徑階段搜索范圍很大，花費(fèi)了主要的搜索時間，而改進(jìn)后的A*算法，規(guī)劃了一個包含最大障礙物在內(nèi)的搜索橢圓，并且在懲罰函數(shù)的作用下，優(yōu)先選擇了靠近理論最短路徑LOD的節(jié)點，進(jìn)一步降低了搜索的復(fù)雜程度。兩種算法得到的路徑長度一致。這說明改進(jìn)后的A*算法縮小了搜索的區(qū)域，減少了搜索節(jié)點數(shù)，仍可以獲得最短路徑搜索結(jié)果。

需說明的是，在上述仿真結(jié)果中，雖然兩種算法規(guī)劃的最短路徑長度一致，但從圖中實際路線來看，其選擇的節(jié)點和路徑是不完全一致的。導(dǎo)致這種結(jié)果的原因是，本算例中節(jié)點間距是均勻的，且不限制對角線路徑，所以相同長度的路徑可能不止一條。

因此，綜合考慮搜索精度和搜索效率，改進(jìn)A*算法在保證較高搜索精度的前提下大大提高了搜索效率。

4 結(jié)論

本文將實際障礙物環(huán)境柵格化處理后，引入了改進(jìn)A*算法規(guī)劃最短路徑。本文提出的算法在規(guī)定的橢圓區(qū)域內(nèi)，基于理論最短路徑動態(tài)改變A*算法中估價函數(shù)權(quán)重，在提高搜索效率的同時，保證了規(guī)劃路徑為全局最短路徑。通過仿真實驗證明，該算法縮小了節(jié)點搜索范圍，通過對各節(jié)點g(v)賦予不同權(quán)重平衡搜索精度和搜索效率，實現(xiàn)了前期以搜索精度為主，后期以搜索效率為主的搜索策略，并且通過對h(v)施加懲罰函數(shù)，使實際規(guī)劃路徑更加靠近理論最短路徑。與經(jīng)典A*算法對比，本文提出的算法在保證搜索精度的前提下，大大提高了搜索效率。

參考文獻(xiàn)：

[1] 王海泉，胡瀛月，廖伍代,等. 基于改進(jìn)人工蜂群算法的機(jī)器人路徑規(guī)劃[J]. 控制工程， 2016，23(9)：1407-1411.

[2] 康 冰，王曦輝，劉 富.基于改進(jìn)蟻群算法的搜索機(jī)器人路徑規(guī)劃[J]. 吉林大學(xué)學(xué)報(工學(xué)版)， 2014，44(7)：1062-1068.

[3] Ahmed F, Deb K. Multi-objective optimal path planning using elitist non-dominated sorting genetic algorithm[J].Soft Computing，2013，17(7)：1283-1299.

[4] 劉貴杰，劉 鵬，穆為磊,等.采用能耗最優(yōu)改進(jìn)蟻群算法的自治水下機(jī)器人路徑優(yōu)化[J]. 西安交通大學(xué)學(xué)報， 2016，50(10)：93-98.

[5] Andre J, Siarry P, Dognon T. An improvement of the standard genetic algorithm fighting premature convergence in continuous optimization[J].Advance in Engineering Software，2011，32(1)：49-60.

[6] 王世明，邢建平，張玉婷,等. 典型城市路網(wǎng)中的橢圓最短路徑算法[J]. 系統(tǒng)工程理論與實踐，2011，31(6)：1158-1164.

[7] Qi M J, Sun H N, Gao G F. Research on an improved algorithm for shortest path searching in urban traffic based on GIS[A].Proceedings of Electrical and Control Engineering International Conference[C]，2011：1184-1187.

[8] 盧 青，周 軍，呼衛(wèi)軍. 基于改進(jìn)A*算法的滑翔飛行器軌跡規(guī)劃[J]. 系統(tǒng)工程與電子技術(shù)， 2016，38(12)：2758-2762.

[9] 潘長安. 基于改進(jìn)A 星算法的城市交通尋徑的研究[D]. 福建：華僑大學(xué)，2012.

[10] Klemm S, Oberlander J, Hermann A, et al. RRT*-Connect: Faster, asymptotically optimal motion planning[A].IEEE Conference on Robotics and Biomimetics[C]. IEEE, 2015: 1670-1677

[11] Orlin J B, Madduri K, Subramani K，et al. A faster algorithm for the single source shortest path problem with few distinct positive lengths[J]. Journal of Discrete Algorithms，2010，8(2)：189-198.