基于改進A*算法的AUV 路徑規劃研究

潘世瑛

（上海交通大學，上海 201100）

1 基本模型和算法

1.1 A*算法原理

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

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

代價函數的表達式通常為

式中：f（n）為評估從n 點到目標點消耗的代價函數；g（n）為從起點到下一路徑點的消耗；h（n）為從當前點到目標點的直觀消耗。

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

1.2 傳統A*算法實現過程

1.2.1 代碼整體思路

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

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

圖1 隨機生成的環境矩陣

（3）調用編寫的createFigure 函數，利用initialize原Field 函數生成的數據進行創建環境，也就是繪制隨機生成的要進行路徑規劃的場景，包括障礙物，起始點，終止點等。

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

（5）開始路徑規劃，從起始點開始，利用循環進行迭代來尋找終止點，在進行點的拓展時調用編寫的findFValue 函數來拓展尋找子節點以及子節點的代價。

（6）如果在上一步的路徑規劃中找到了從起始點到終止點的路，就調用編寫的findWayBack 函數進行路徑回溯，并繪制出從起始點到終止點的路徑曲線。1.2.2 環境中點的設置

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

1.2.3 路徑規劃的實現過程

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

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

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

初始化結束后，接下來進行拓展，從setOpen 矩陣中的待選點中選取一個最優點進行拓展，選取標準就是這個待選點距離起始點和終止點的代價的和最小，那么這個待選點就認為他是最優的待選點，那么就將這個點作為下一次拓展的父節點，每次拓展把拓展出來的符合要求的點放到矩陣setOpen 中作為待選點，這樣不斷的迭代循環，直到沒有可拓展的點或者直到終止點，結束拓展，路徑規劃結束。

2 改進A*算法

2.1 啟發函數改進

啟發式函數可以控制A*的行為：

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

（2）如果h（n）經常都比從n 移動到目標的實際代價?。ɑ蛘呦嗟龋?，則A * 保證能找到一條最短路徑。h（n）越小，A*擴展的結點越多，運行就得越慢。

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

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

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

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

2.2 路徑規劃拐角優化

無論是傳統的A 星算法還是動態衡量啟發式的他找出來的路只是數學上的最優路徑（或者說近似最優路徑，隨著搜索速度的提高，找到的路可能不是最短的），規劃出來的路線有一些轉彎是完全可以省去的（用一個轉彎去取代多個轉彎），因為最終的目標還是要控制實物去移動，所以要盡量減少轉彎的次數，使其在保證不增加路程的基礎上盡量減少轉彎次數。

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

圖2 拐角優化的不同情況

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

3 仿真過程

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

圖3 傳統A*算法、改進啟發式函數算法、拐角優化后的路徑規劃情況1

圖4 傳統A*算法、改進啟發式函數算法、拐角優化后的路徑規劃情況2

表1 路徑規劃時間對比

4 結語

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