基于改進(jìn)A*算法的AUV 路徑規(guī)劃研究

潘世瑛

（上海交通大學(xué)，上海 201100）

1 基本模型和算法

1.1 A*算法原理

A*算法因其尋優(yōu)能力強(qiáng)、場(chǎng)景適應(yīng)度高等特性，在路徑規(guī)劃領(lǐng)域有著廣泛應(yīng)用發(fā)展[1]。陳若男等[2]結(jié)合距離及角度信息改進(jìn)傳統(tǒng)A*算法代價(jià)函數(shù)，設(shè)計(jì)鄰域選擇策略，提升了算法效率和路徑安全性。馮來(lái)春等[3]將A* 算法和快速搜索隨機(jī)樹（Rapid-Exploring Random Tree，RRT）算法結(jié)合，降低了RRT 算法的采樣盲目性。孟珠李等[4]將A*算法與B 樣條曲線結(jié)合，改善了A*算法規(guī)劃路徑的平滑性。余文凱等[5]采用K-均值（K-Means）聚類算法對(duì)地圖進(jìn)行預(yù)處理，量化不同區(qū)域地圖復(fù)雜度，依據(jù)復(fù)雜度設(shè)置搜索權(quán)重，應(yīng)用弗洛伊德（Floyd）算法對(duì)路徑進(jìn)行平滑，提高了路徑規(guī)劃效率和路徑平滑性。

A*算法是一種快速簡(jiǎn)潔的啟發(fā)式算法，其核心思想是通過代價(jià)函數(shù)的數(shù)值評(píng)估各路徑點(diǎn)，從而對(duì)路徑做出規(guī)劃。

代價(jià)函數(shù)的表達(dá)式通常為

式中：f（n）為評(píng)估從n 點(diǎn)到目標(biāo)點(diǎn)消耗的代價(jià)函數(shù)；g（n）為從起點(diǎn)到下一路徑點(diǎn)的消耗；h（n）為從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的直觀消耗。

A*算法利用Dijkstra 算法思想，每次運(yùn)算都會(huì)將已處理的點(diǎn)加入關(guān)閉列表（Close），并從待選列表（Open）中選出代價(jià)函數(shù)最小的點(diǎn)作為下一路徑點(diǎn)，重復(fù)該過程，直至抵達(dá)目標(biāo)。改進(jìn)算法將各柵格的活性值作為A*算法的代價(jià)函數(shù)運(yùn)算，快速規(guī)劃出當(dāng)前場(chǎng)景下的全局路徑，并用父節(jié)點(diǎn)優(yōu)化產(chǎn)生下一路徑點(diǎn)坐標(biāo)。

1.2 傳統(tǒng)A*算法實(shí)現(xiàn)過程

1.2.1 代碼整體思路

（1）對(duì)環(huán)境進(jìn)行設(shè)定：首先設(shè)定生成的環(huán)境的大小，生成的方格數(shù)為n*n，也就是對(duì)參數(shù)n 的設(shè)定。然后設(shè)定障礙物，也就是對(duì)參數(shù)wallpercent 進(jìn)行設(shè)定。

（2）設(shè)定（1）的2個(gè)參數(shù)后，調(diào)用編寫的initialize原Field 函數(shù)來(lái)隨機(jī)生成包含障礙物，起始點(diǎn)，終點(diǎn)等信息的矩陣（圖1）。

圖1 隨機(jī)生成的環(huán)境矩陣

（3）調(diào)用編寫的createFigure 函數(shù)，利用initialize原Field 函數(shù)生成的數(shù)據(jù)進(jìn)行創(chuàng)建環(huán)境，也就是繪制隨機(jī)生成的要進(jìn)行路徑規(guī)劃的場(chǎng)景，包括障礙物，起始點(diǎn)，終止點(diǎn)等。

（4）開始規(guī)劃路徑前，要對(duì)路徑的一些矩陣進(jìn)行初始化。

（5）開始路徑規(guī)劃，從起始點(diǎn)開始，利用循環(huán)進(jìn)行迭代來(lái)尋找終止點(diǎn)，在進(jìn)行點(diǎn)的拓展時(shí)調(diào)用編寫的findFValue 函數(shù)來(lái)拓展尋找子節(jié)點(diǎn)以及子節(jié)點(diǎn)的代價(jià)。

（6）如果在上一步的路徑規(guī)劃中找到了從起始點(diǎn)到終止點(diǎn)的路，就調(diào)用編寫的findWayBack 函數(shù)進(jìn)行路徑回溯，并繪制出從起始點(diǎn)到終止點(diǎn)的路徑曲線。1.2.2 環(huán)境中點(diǎn)的設(shè)置

環(huán)境中的一個(gè)點(diǎn)他起初的身份是有障礙物點(diǎn)，沒有障礙物點(diǎn)，以及特殊的點(diǎn)——起始點(diǎn)和終止點(diǎn)，然后在路徑規(guī)劃進(jìn)行點(diǎn)的拓展時(shí)，首先是當(dāng)被父節(jié)點(diǎn)拓展出來(lái)后放到矩陣posinds 中，如果符合要求，就升級(jí)為待選點(diǎn)，放到setOpen 矩陣中，每次循環(huán)都從se原tOpen 矩陣中找一個(gè)最優(yōu)的點(diǎn)，升級(jí)為下一次進(jìn)行拓展的父節(jié)點(diǎn)，放到矩陣setClosed 中。最后在找到終止點(diǎn)后，通過回溯出來(lái)的點(diǎn)放到矩陣p 中，利用這些點(diǎn)可以找到規(guī)劃的路徑，并將其繪制出來(lái)。

1.2.3 路徑規(guī)劃的實(shí)現(xiàn)過程

首先通過initializeField 函數(shù)生成矩陣costchart，在矩陣costchart 中起始點(diǎn)位置存放的內(nèi)容為0，其他位置存放的內(nèi)容為NaN，生成的field 矩陣起始點(diǎn)和終止點(diǎn)存放的內(nèi)容為0，沒有障礙物的點(diǎn)存放的內(nèi)容為1 到11 的隨機(jī)數(shù)，有障礙物的點(diǎn)存放的內(nèi)容為最大值Inf，生成了一個(gè)元胞數(shù)組fieldpointers 里面在有障礙物的地方存放的內(nèi)容為0，在起始點(diǎn)處存放的內(nèi)容為S，終止點(diǎn)處存放G，其他位置為空，除此之外還得到了起始點(diǎn)的索引值startposind，終止點(diǎn)的索引值goalposind。

在進(jìn)行路徑規(guī)劃時(shí)，需要初始化一些矩陣，se原tOpen 矩陣用來(lái)存放待選點(diǎn)的索引值，初始化為起始點(diǎn)的索引值，一開始的時(shí)候只能從起始點(diǎn)出發(fā)，因此剛開始的時(shí)候只有起始點(diǎn)一個(gè)待選點(diǎn)；setOpenCosts矩陣存放該待選點(diǎn)與起始點(diǎn)的代價(jià)，由于目前該待選點(diǎn)就是起始點(diǎn)，所以初始化為0，setOpenHeuristics 矩陣存放該待選點(diǎn)與終止點(diǎn)的距離，因?yàn)槟壳斑€不知道能不能找到到終止點(diǎn)的路，所以先初始化為最大值Inf，初始化的時(shí)候，這3個(gè)矩陣內(nèi)都只存放一個(gè)待選點(diǎn)，但是，隨著路徑規(guī)劃的進(jìn)行會(huì)不斷將符合要求的待選點(diǎn)串到（新增）到矩陣中，這三個(gè)矩陣是配套使用的，也就是說(shuō)這三個(gè)矩陣的同一行中存放的是同一個(gè)待選點(diǎn)的信息，分別存放該待選點(diǎn)的索引值，到起始點(diǎn)的代價(jià)，到終止點(diǎn)的代價(jià)，因此要往setOpen 中增加或者刪除待選點(diǎn)時(shí)，必須同時(shí)對(duì)另外兩個(gè)矩陣進(jìn)行增加或刪除，來(lái)保證其對(duì)應(yīng)關(guān)系。

兩個(gè)矩陣setClosed = []和setClosedCosts = []分別用來(lái)存放下一次進(jìn)行拓展的父節(jié)點(diǎn)的索引值，以及改點(diǎn)到起始點(diǎn)的代價(jià)，其實(shí)這兩個(gè)矩陣在進(jìn)行路徑規(guī)劃的時(shí)候并沒有用到，也沒有發(fā)揮任何作用，可以用來(lái)幫助進(jìn)行分析。

初始化結(jié)束后，接下來(lái)進(jìn)行拓展，從setOpen 矩陣中的待選點(diǎn)中選取一個(gè)最優(yōu)點(diǎn)進(jìn)行拓展，選取標(biāo)準(zhǔn)就是這個(gè)待選點(diǎn)距離起始點(diǎn)和終止點(diǎn)的代價(jià)的和最小，那么這個(gè)待選點(diǎn)就認(rèn)為他是最優(yōu)的待選點(diǎn)，那么就將這個(gè)點(diǎn)作為下一次拓展的父節(jié)點(diǎn)，每次拓展把拓展出來(lái)的符合要求的點(diǎn)放到矩陣setOpen 中作為待選點(diǎn)，這樣不斷的迭代循環(huán)，直到?jīng)]有可拓展的點(diǎn)或者直到終止點(diǎn)，結(jié)束拓展，路徑規(guī)劃結(jié)束。

2 改進(jìn)A*算法

2.1 啟發(fā)函數(shù)改進(jìn)

啟發(fā)式函數(shù)可以控制A*的行為：

（1）一種極端情況，如果h（n）是0，則只有g(shù)（n）起作用。

（2）如果h（n）經(jīng)常都比從n 移動(dòng)到目標(biāo)的實(shí)際代價(jià)?。ɑ蛘呦嗟龋?，則A * 保證能找到一條最短路徑。h（n）越小，A*擴(kuò)展的結(jié)點(diǎn)越多，運(yùn)行就得越慢。

（3）如果h（n）精確地等于從n 移動(dòng)到目標(biāo)的代價(jià)，則A * 將會(huì)僅僅尋找最佳路徑而不擴(kuò)展別的任何結(jié)點(diǎn)，這會(huì)運(yùn)行得非?？臁１M管這不可能在所有情況下發(fā)生，你仍可以在一些特殊情況下讓它們精確地相等。只要提供完美的信息，A * 會(huì)運(yùn)行得很完美。

（4）如果h（n）有時(shí)比從n 移動(dòng)到目標(biāo)的實(shí)際代價(jià)高，則A* 不能保證找到一條最短路徑，但它運(yùn)行得更快。

（5）另一種極端情況，如果h（n）比g（n）大很多，則只有h（n）起作用。

傳統(tǒng)的A * 算法的總代價(jià)計(jì)算公式為f（n）= g（n）+ h（n），與傳統(tǒng)的A 星不同的是，改進(jìn)后變成了w（n）* h（n），其中w（n）>= 1 。w（n）可以影響評(píng)估值。在搜索過程中，可以通過改變w（n）來(lái)影響搜索過程如上面提到的h（n）對(duì)A 星算法的影響。

2.2 路徑規(guī)劃拐角優(yōu)化

無(wú)論是傳統(tǒng)的A 星算法還是動(dòng)態(tài)衡量啟發(fā)式的他找出來(lái)的路只是數(shù)學(xué)上的最優(yōu)路徑（或者說(shuō)近似最優(yōu)路徑，隨著搜索速度的提高，找到的路可能不是最短的），規(guī)劃出來(lái)的路線有一些轉(zhuǎn)彎是完全可以省去的（用一個(gè)轉(zhuǎn)彎去取代多個(gè)轉(zhuǎn)彎），因?yàn)樽罱K的目標(biāo)還是要控制實(shí)物去移動(dòng)，所以要盡量減少轉(zhuǎn)彎的次數(shù)，使其在保證不增加路程的基礎(chǔ)上盡量減少轉(zhuǎn)彎次數(shù)。

如圖2 所示，第一種情況：如上圖的標(biāo)號(hào)于處所示，可以將其優(yōu)化成白線所示的軌跡；第二種情況：如上圖的標(biāo)號(hào)淤處所示，這種轉(zhuǎn)彎，則不能進(jìn)行類似的操作，因?yàn)锳 星算法在進(jìn)行拓展的時(shí)候是對(duì)總代價(jià)最小的點(diǎn)來(lái)拓展，在淤號(hào)標(biāo)注處向下拓展比向左拓展要花費(fèi)的總代價(jià)小，所以類似這種遠(yuǎn)離終止點(diǎn)的拐角無(wú)法得到優(yōu)化。

圖2 拐角優(yōu)化的不同情況

具體實(shí)現(xiàn)方法：從setOPen 矩陣中，找到最小總代價(jià)的點(diǎn)在setOPen 中的索引值ii 時(shí)，也就是得到將要拓展的點(diǎn)在fieldpointers 元胞數(shù)組中的索引值se原tOPen（ii），然后通過該點(diǎn)在元胞數(shù)組fieldpointers 中儲(chǔ)存的方向信息，就可以知道這個(gè)點(diǎn)是由那個(gè)點(diǎn)拓展出來(lái)的，進(jìn)而得到其父節(jié)點(diǎn)的索引值，也就可以知道其父節(jié)點(diǎn)在元胞數(shù)組fieldpointers 中儲(chǔ)存的方向信息，近而知道了期望的下一步要走的點(diǎn)的位置。得到這個(gè)期望要走的點(diǎn)位置后，就查看這個(gè)點(diǎn)是否位于待選點(diǎn)矩陣setOPen 中，如果在，那么計(jì)算一下這個(gè)點(diǎn)要花費(fèi)的總代價(jià)，如果跟原來(lái)要走的點(diǎn)代價(jià)相同，則用這個(gè)期望的點(diǎn)代替原來(lái)的點(diǎn)進(jìn)行優(yōu)化，否則不進(jìn)行優(yōu)化。

3 仿真過程

通過對(duì)比傳統(tǒng)A*算法、改進(jìn)啟發(fā)函數(shù)后的A*算法以及進(jìn)行拐角優(yōu)化的A* 算法完成同等狀態(tài)的路徑規(guī)劃情況，得到具體路徑規(guī)劃圖如圖3 和圖4，具體計(jì)算時(shí)間見表1。第一種情況中右上角為起點(diǎn)，左上角為終點(diǎn)，第二種則反之。通過迭代產(chǎn)生的探索區(qū)域可知，改進(jìn)后的A*算法明顯減少了迭代次數(shù)，規(guī)劃時(shí)間得到優(yōu)化，拐角優(yōu)化后的計(jì)算時(shí)間并未明顯優(yōu)化，但是路徑的轉(zhuǎn)彎次數(shù)有一定程度的減少，達(dá)到了預(yù)期結(jié)果。

圖3 傳統(tǒng)A*算法、改進(jìn)啟發(fā)式函數(shù)算法、拐角優(yōu)化后的路徑規(guī)劃情況1

圖4 傳統(tǒng)A*算法、改進(jìn)啟發(fā)式函數(shù)算法、拐角優(yōu)化后的路徑規(guī)劃情況2

表1 路徑規(guī)劃時(shí)間對(duì)比

4 結(jié)語(yǔ)

傳統(tǒng)A*算法存在算法時(shí)間較長(zhǎng)，搜索效率低，路徑冗余拐點(diǎn)較多、平滑度差等問題，針對(duì)這些不足，考慮多種因素，對(duì)作做出改進(jìn)。通過改進(jìn)A*算法，將傳統(tǒng)的A*算法改為動(dòng)態(tài)衡量啟發(fā)式A*算法，并對(duì)拐角次數(shù)進(jìn)行了一定程度的優(yōu)化，在不同復(fù)雜度場(chǎng)景下對(duì)比進(jìn)行仿真驗(yàn)證，經(jīng)過驗(yàn)證，改進(jìn)的A* 算法能夠更快更好地完成路徑規(guī)劃任務(wù)。