，

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

0 引言

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

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

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

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

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

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

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

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

(1)

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

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

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

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

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

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

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

(2)

(3)

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

(4)

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

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

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

(5)

(6)

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

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

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

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

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

(7)

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

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

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

(8)

(9)

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

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

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

(10)

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

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

(11)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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

參考文獻(xiàn)：

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

[2] 康 冰，王曦輝，劉 富.基于改進(jìn)蟻群算法的搜索機(jī)器人路徑規(guī)劃[J]. 吉林大學(xué)學(xué)報(bào)(工學(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é)報(bào)， 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)工程理論與實(shí)踐，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.