潘世瑛

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

1 基本模型和算法

1.1 A*算法原理

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

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

代價函數(shù)的表達式通常為

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

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

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

1.2.1 代碼整體思路

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

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

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

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

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

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

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

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

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

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

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

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

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

2 改進A*算法

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

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

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

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

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

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

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

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

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

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

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

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

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

3 仿真過程

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

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

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

表1 路徑規(guī)劃時間對比

4 結(jié)語

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