基于改進A*算法的無人車路徑規劃

祁玄玄，黃家駿，曹建安

（西安交通大學電氣工程學院，西安 710049）

（*通信作者電子郵箱2787477370@qq.com）

0 引言

路徑規劃在智能車運動控制中占有核心地位，路徑規劃算法的效率將直接影響無人車的尋路效率及實施規劃能力。目前路徑規劃算法基本分為兩種類型：基于圖搜索算法的傳統算法和智能算法。圖搜索算法主要指的是Floyd 算法［1］和Dijkstra 算法［2］；智能算法包括蟻群算法［3］、粒子群算法［4］、遺傳算法［5］、神經網絡［6］、模擬退火［7］等。傳統的圖搜索算法存在隨著環境信息的增加計算復雜性呈現指數式增加的缺點，智能算法作為一種路徑規劃的新思路其對計算機性能要求過高且存在計算時間較長的缺點。A*（A-Star）算法［8-9］是基于傳統圖搜索的思維的智能啟發式算法，相比傳統圖搜索算法，它具有計算量小、規劃路徑相對最優等突出特點。但是A*算法的啟發函數考慮維度較為簡單，導致該算法在尋路過程中會出現很多冗余的擴展柵格。目前已經提出了很多改進A*算法，如：文獻［10］中通過估價函數進行指數衰減的方式加權減少了冗余的擴展；文獻［11］中通過建立禁忌表來改進A*算法的估價函數，能快速、有效地實現越野路徑規劃。與文獻［10-11］類似地通過對A*算法估價函數進行改進以達到減少計算時間的算法還有很多，但是它們的普適性并不好。文獻［12］中通過起點和終點同時運行時效A*算法尋找路徑，文獻［13］中通過并行算法改進A*尋路時間，這兩者在一定程度上可減少計算時間，但是都過于依賴計算機的性能。

從上述文獻中可以看出，過去A*算法的改進方法主要是通過優化代價函數或提高計算空間來減少計算時間；但這些方法都不具有很高的普適性，并且部分改進方法十分依賴于計算機的性能。本文從四個方面對A*算法進行改進，提出一種兼顧普適性和計算效率的A*算法：首先是優化算法的拓展方向，根據目標和擴展節點的相對位置選擇雙象限方向進行節點拓展；二是目標可見性判斷，目標可見時跳出A*算法的探索過程；其三，通過加入k個父輩估計信息改變算法的啟發函數；最后，通過引入模擬退火法改變待擴展節點的選取方略。通過Matlab 仿真實驗，在柵格地圖環境下對比A*算法與改進A*算法的計算結果，驗證了本文算法計算時間更短，擴展節點更少，尋路的目標性更強。

1 全局路徑規劃算法

1.1 A*算法

作為一種圖搜索算法，A*算法適合大面積的地圖中路徑搜索。這是一種以Dijkstra 算法和廣度優先搜索（Breadth First Search，BFS）算法為基礎的啟發式搜索算法。因為其具有Dijkstra 算法的特點，所以，A*算法可以用于搜索最短路徑。與此同時由于A*算法包含BFS 的特點，因此它在搜索路徑的判斷依據中包含啟發函數，該函數就是BFS 算法之中的得分函數。在路徑搜索過程中，A*算法使用代價函數來評估節點的質量。算法將選擇的代價函數數值最小的節點作為下一步擴展的節點，然后它將繼續從下一個節點搜索，直到到達目標點。像BFS 一樣，A*可以使用啟發式函數來引導自己。A*算法的代價函數如下：

f(n)=g(n)+h(n) （1）

其中：n代表路徑搜索過程之中最近的一個節點，g（n）代表從起始點到n節點的最短路徑，h（n）代表從n節點到目標節點的啟發函數的預測值。g（n）經常都是固定的數值，這個數值就是Dijkstra算法中的到起始節點的最短路徑；然而h（n）卻是不固定的，它的改變將會改變優化出來的路徑，因此選擇合適的h（n）可以得出最優的路徑。

h（n）代表從n節點到目標節點的啟發函數的預測值，因此最能表達其含義的就是兩點之間的距離，本文采用曼哈頓距離作為h（n）函數的估計。其計算表達式如式（2）所示：

h(n)=D(abs(n.x-goal.x)+abs(n.y-goal.y)) （2）

從式（2）可以看出，h（n）函數表示n節點到目標節點的x軸和y軸相對距離的絕對值的和。

和Dijkstra 算法一樣，A*算法在計算過程中也存在兩個核心集合。定義已經走過的點的集合為Close，待選集合為Open，集合里面的每個元素為node，該元素的屬性是該節點代價函數的估計值f（n）。每次從Open選出延伸節點，然后往四個方向或者八個方向進行擴展。圖1 為A*演示模型［14］，其中綠色點是起始點設為A，紅色點是目標點設為B，3 個藍色點是障礙物分別設為C、D、E。方格的左上角、左下角、右下角分別代表代價函數的估計值f（n），從起始點到n節點的最短路徑g（n），從n節點到目標節點的啟發函數的預測值h（n）。具體演示流程如下。

1）初始化集合。

首先把A 點加入到Open集合，初始化A 點代價函數的估計值f（n）數值為0，把障礙物加入到Close集合。

2）擴展節點。

從Open集合中選取f（n）數值最小的點，將其放入Close集合。然后往該節點相鄰的八個方向擴展方格，并計算不在Close集合中的擴展方格的f（n）數值。如果擴展方格已經在Open集合，那么根據此時計算的擴展方格的f（n）數值更新該節點，如果較小則更新，否則不更新。如果擴展方格不在Open集合，那么根據計算的結果往Open集合中添加該擴展節點。

3）循環判斷。

重復步驟2），如果擴展到目標節點或者Open集合為空則退出循環，根據回溯算法倒推出從起始點到目標點的最短路徑。

通過A*算法可以從某一點到另外一點的最短路徑，圖1中紅色圓點連接就是擴展得出的路徑。

1.2 A*算法改進

A*算法無疑是一種高效的算法，但傳統的A*仍存在一些不足，例如擴展方向盲目性、啟發函數過于簡單等，導致A*算法計算效率較低。因此，本文從以下四個方面對傳統的A*算法進行改進。

1.2.1 自適應擴展方向

在固定的環境中A*在擴展時能夠在任何一個方向上擴展，即分別進行上下左右的擴展。而改進的A*算法提出的是確定目標所在的象限，一旦確定目標象限，就會向該象限擴展從而忽略其他沒必要的象限。因此改進的A*算法在擴展時只能往上下左右的某一個方向擴展。確定好象限之后就可以確定擴展的相鄰節點。由于擴展方向縮減為原先的四分之一，因此在一定程度上可以減少算法的計算時間。

定義當前擴展節點的坐標為（n.x，n.y），目標節點的坐標為（goal.x，goal.y）。通過目標節點和擴展節點的做差得到Dx，Dy。即式（3）所示：

根據Dx，Dy的數值本文可以確定擴展方向所在的象限以及所擴展的點。具體判斷依據如式（4）所示：

如圖2 象限分布所示，當目標在第一象限時，擴展節點的擴展鄰居為｛1，2，3，4，5｝；當目標在第二象限時，擴展節點的擴展鄰居為｛1，2，3，7，8｝；當目標在第三象限時，擴展節點的擴展鄰居為｛1，5，6，7，8｝；當目標在第四象限時，擴展節點的擴展鄰居為｛3，4，5，6，7｝。

圖2 象限分布Fig.2 Quadrant distribution

1.2.2 判斷目標可見

傳統的A*在循環擴展的時候，循環截止條件是靠近目標點或者是Open集合為空，這將導致在靠近目標點的時候沒有任何障礙物依然會有額外的擴展節點的開銷，這顯然是不合理的。基于此，本文提出在擴展節點和目標節點之間連線沒有任何障礙物的時候，可以跳出A*的啟發式探索過程。判斷的依據如式（5）所示：

Newpos=(n.x，n.y)+r(cosφ，sinφ) （5）

其中：r代表更迭步長，φ代表擴展節點和目標節點之間的夾角。Newpos代表在柵格地圖中的坐標，因此只需要判斷Newpos位置處是不是存在障礙物就可以判斷擴展節點和目標節點之間連線有無任何障礙物。

1.2.3 改變啟發函數

代價函數的估計值f（n）是從起始點到n節點的最短路徑g（n）和從n節點到目標節點的啟發函數的預測值h（n）的和。起始點到n節點的最短路徑是在探索過程中從父節點累加的，因此g（n）是和前面的探索節點是有關聯的。而h（n）只是求n節點到目標節點的距離估計值，因此和前面的探索節點是沒有任何關聯的。基于此本文在代價函數的估計值f（n）中添加當前n節點父節點及其祖輩節點的啟發函數的預測值h（n）。因此代價函數的估計值f（n）如式（6）所示：

f(n)=g(n)+D×(h(n)+h(p)+h(p2)+...+h(pk)) （6）

其中：p是n節點父節點，pk代表n節點的k父輩，h（p）是n節點父節點的啟發函數的預測值，D是啟發函數的系數。D和k這兩個數都會影響A*算法的計算效率，因此選擇合適的D和k是十分重要的。

1.2.4 改變擴展節點選取方略

傳統的A*算法在從Open集合選取待擴展節點的原則是最小的代價函數的估計值的節點。從起始節點到目標節點搜索最短路徑時，g（n）的數值是從父節點累加過來的，因此它可以盡可能地保證前面走過的路徑是最短的。而h（n）代表從n節點到目標節點的啟發函數的預測值，如果可以盡可能地保證h（n）是逐漸減小的，那么在一定程度上就是保證n節點后面的路徑是在接近目的地。

為了實現n節點后面的路徑是在接近目的地，如果每次都以貪婪策略，即h（n）最小值節點來作為擴展節點是最合適的，但是這樣就忽略了g（n）的影響，該算法也就變成了傳統的深度優先搜索了，很有可能優化出來的路徑只是局部最優。為了擺脫局部異常的束縛，本文引入模擬退火算法來選擇擴展節點。

模擬退火［15］是一種解決無約束和有邊界約束的優化問題的方法。該方法模擬了加熱材料然后緩慢降低溫度以減少缺陷的物理過程，從而最大限度地降低了系統能耗。模擬退火算法包含兩個部分，即Metropolis算法和退火過程。

1）Metropolis算法過程。

圖3 是模擬退火的過程的示意圖，A 點是迭代起始點，隨著迭代次數增加，到達局部最優解B點，此時若根據梯度下降準則再更新的話是不被允許的，而模擬退火算法會在此時以一定的概率跳出這個局限，這個概率和物質能量以及迭代次數息息相關，類似情形通過C點，最終到達全局最優解D點。

圖3 模擬退火的過程Fig.3 Simulated annealing process

概率準則如式（7）所示。其中n代表迭代次數，E（n）代表第n次迭代的值。

從式（7）可以看到：如果能量減小了，那么這種轉移就被接受（概率為1）；如果能量增大了，就說明系統偏離全局最優值位置更遠了，此時算法不會立刻將其拋棄，而是進行概率操作：首先在區間［0，1］產生一個均勻分布的隨機數ε，如果ε＜P，則此種轉移被接受，否則拒絕轉移，進入下一步，往復循環。其中P以能量的變化量和T進行決定概率P的大小，所以這個值是動態的，而且隨著迭代次數的推移往往P逐漸變小。

2）退火過程。

退火意思就是溫度降低的過程，主要包含初始溫度、退火速率和終止溫度三個內容。初始溫度如果給得太高則獲得高質量的解的概率越大，耗費的時間越長。退火速率的設置形式較多，常使用指數式下降型，具體如式（8）所示。其中退火速率r是小于1的數，一般取值［0.8，0.99］。如果在若干次迭代后沒有可以更新的新狀態或者達到用戶設定的閾值，則退火完成，此時的溫度稱為終止溫度。

根據模擬退火算法的基本特點本文設計選擇擴展節點的流程如下。首先把從n節點到目標節點的啟發函數h（n）視作為模擬退火的能量函數。

1）初始化各個參數。

初始化初始溫度T=3，退火速率r=0.98，能量函數初始數值為起始點到目標節點的啟發函數的預測值。擴展初始節點，更新Close集合和Open集合，即把起始節點放入Close集合，根據自適應擴展方向算法把起始點相鄰節點放入Open集合。

2）產生新解n'。

從Open集合之中根據最小代價函數的估計值f（n）選取新的節點n'，計算新的節點目標節點的啟發函數的預測值h(n')。

3）計算增量。

因為目標函數差僅由變換部分產生，所以目標函數差的計算最好按增量計算。事實表明，對大多數應用而言，這是計算目標函數差的最快方法。根據式（9）計算能量函數的變化量：

4）判斷是否接受當前解n'。

判斷的依據是一個接受準則，最常用的接受準則是Metropolis 準則：若ΔT＜0 則接受n'作為新的當前解n；當ΔT＞0時，產生一個均勻分布的隨機數ε，若exp(-ΔT/T) ＞ε接受n'作為新的當前解n，若exp(-ΔT/T) ≤ε不接受n'作為新的當前解n，根據最小到目標節點的啟發函數的預測值h(n)選取新的節點n'。

5）更新各個參數。

改變退火溫度T=T*r，退火速率r=0.98，擴展新的節點n'，更新Close集合和Open集合，即把新的節點n'放入Close集合，根據自適應擴展方向算法把新的節點n'相鄰節點放入Open集合。

本節從四個方面具體介紹了改進A*算法的細節。改進后的A*算法偽代碼如下所示：

改進后的A*算法在搜尋從一個源點到另一個源點的最短路徑的過程中更加有目的性，可以在一定程度上減少遍歷的柵格點的數目，選擇更加優化的路徑。尤其要注意的是式（6）中pk代表n節點的k父輩。雖然越多的父輩被加入到代價函數之中可以更加優化尋找的最短路徑，但是這樣也會增加計算負擔和內存，因此需要綜合考慮k的取值。

2 仿真和分析

為了測試改進的A*算法，本文采用26 ×27 的柵格地圖作為模擬對象，如圖4 所示。圖中黑色部分為障礙物，白色空白部分為可行走地段。在仿真過程中，起始點和目標點都是固定的，障礙物位置也是固定的。在柵格地圖坐標系（橫軸為從左到右，縱軸為從下到上）下，設置規劃起始點為（24，3）。一旦確定目標點將會開始路徑規劃。本文從三個方面進行評價：一是計算時間；二是優化路徑長度；三是經歷的柵格數目。

圖4 測試柵格地圖Fig.4 Grid map for testing

2.1 確定啟發函數D和k的數值

為了確定D和k的最優數值，本文用傳統的A*算法測試不同D和k取值時系統規劃路徑時經歷的柵格數目。由于測試算法都是A*，因此經歷越多的柵格那么算法的執行時間也就越長。

圖5是D和k取不同數值時經歷柵格數目測試圖。

圖5 D和k取不同數值時經歷柵格數目測試圖Fig.5 Test chart of experienced grid number with different D and k values

從圖5中可以看出當k數值越大那么算法的執行效率基本也是越高，但是當k增加到第6代的時候算法效率的提升效果就不是很明顯了。而更多的父代數據加入進來會增加計算的復雜性，因此本文選定k的取值范圍是6。同樣的，從圖5中可以看出D的數值越大算法的執行效率基本也是越高，但是其在等于3 的時候效率提升的效果遇到了瓶頸。所以綜合考慮本文選取的D和k的數值分別是3和6。

2.2 改進的A*和A*對比仿真

圖6是改進的A*和A*路徑規劃過程對比圖，圖中灰色部分代表的是路徑規劃過程中經歷的柵格，紅色線條是規劃的路徑。起點和終點都是一樣的分別為（24，3），（6，21）。圖6（a）中灰色柵格個數為161，程序運行時間為5.58 s，計算出的最優化路徑長度為27.799 0。圖6（b）中灰色柵格個數為12，程序運行時間為0.66 s，計算出的最優化路徑長度為26.937 9。通過數據可以很清晰地看出改進的A*算法在保證最短路徑的前提下，灰色柵格個數和程序計算時間上都有了明顯下降，同時優化而出的路徑長度也較小于傳統A*算法。因此在計算效率上本文提出的算法還是得到很好的印證。

圖6 改進的A*算法和A*算法路徑規劃過程對比Fig.6 Comparison of improved A*algorithm and A*algorithm in path planning process

從圖6中還能看得出兩點：一是改進的A*算法在前一擴展節點指向后一擴展方向的向量與前一節點指向目標點的向量夾角大多數滿足銳角關系，這個特點就是自適應擴展方向導致的結果；二是在擴展后半段當目標可見時改進的A*算法可以直接跨過搜索過程。以上兩個特點可以一定程度提高A*算法的計算效率。

圖7是改進的A*和A*迭代過程對比，從圖中可以看出傳統的A*算法迭代過程中整體上到目標點的距離是逐漸減小的，但是在局部過程存在震蕩，也就是說其局部并沒有優化。而改進的A*算法到達至目標點附近所需要的迭代次數大幅減少，且雖然前期仍存在局部震蕩，但后期就不存在震蕩現象了，這一點正好印證了本文改進的A*措施中的第4）條，即模擬退火過程中，前期溫度較高，可以在一定概率上接受局部違背梯度下降的原則，隨著溫度逐漸降低，這種情況被接受的概率降低到幾乎為零的地步。

圖7 改進的A*算法和A*算法迭代過程對比Fig.7 Comparison of improved A*algorithm and A*algorithm in iterative process

為了驗證本文算法的普適性，以起始點的三個不同方位作為目標點進行傳統A*和改進A*算法的路徑規劃，表2是不同目標方位統計結果。從表中可以看出，改進的A*算法在運行時間、經歷柵格數都少于傳統的A*算法，且優化路徑長度幾乎是一樣的。

表2 不同目標方位下的路徑規劃統計Tab.2 Statistics of path planning with different target directions

從平均值的角度分析，本文提出的改進的A*算法在運行時間上減少67.06%，經歷的柵格數減少73.53%，優化路徑長度浮動范圍在±0.6%。

2.3 復合地圖下仿真

在傳統柵格地圖上，通過仿真可以看出，應用本文的改進的A*算法是非常高效的，但是當障礙物較多或者地圖面積較大的時候即使是改進的A*算法也會產生很多無用的擴展，這是十分消耗時間的。其次柵格地圖的缺點是如果分辨率太高，那么尋路過程會被拖慢，柵格地圖如果分辨率太低，或喪失很多關鍵障礙物信息，那么尋路結果會變得不準確。

基于此問題本文引入多層地圖的概念，上層地圖是節點和連線組成的拓撲地圖，下層是柵格大小相同的柵格地圖。圖8 是復合地圖的一個示意圖。上層地圖由黑色的點和黑色連線組成，分別代表采樣路標，以及路標是否連通。下層地圖由大小相等的黑白柵格組成，黑色為障礙物，白色為可行走區域。

圖8 復合地圖Fig.8 Composite map

基于復合地圖的概念，A*改進算法的擴展規則從原來的柵格的相鄰柵格變為拓撲地圖下路標的毗鄰路標，其他過程和柵格地圖是一致的。

圖9 是在起始點為（24，3），目標點為（24，21）時，復合地圖和柵格地圖下使用本文提出的改進的A*算法得到的路徑規劃圖。選擇圖形右下角作為目標點的原因是其尋路過程較為復雜，因此可以更加清晰測試出復合地圖下改進的A*算法的優勢。

從圖9中可以清晰看到復合地圖下經歷柵格數只有9，而柵格地圖下經歷的柵格數達到了80，這將大幅度縮短程序的計算時間。而優化路徑的長度幾乎是沒有差別，復合地圖下是34.192 7，柵格地圖下是34.385 4。而且組合地圖計算的路徑更加遠離障礙物，一定程度上較為合理。

以起始點的三個方位作為目標點進行不同地圖模式下改進的A*算法的路徑規劃，表3 是不同目標方位統計結果。從表3中可以看出在復合地圖下改進的A*算法在運行時間、經歷柵格數都少于柵格地圖下A*算法，且優化路徑長度幾乎是一樣的。從平均值的角度分析，本文提出的在復合地圖下運用改進的A*算法在運行時間上減少90%以上，經歷的柵格數減少86%左右，優化路徑長度浮動范圍在±0.1%。

分析其如此高效的原因是復合地圖下，改進的A*算法擁有了較大的步長，因此可以更快地尋找到目標點。但是其步長還不能自適應地去改變，因此這也是本課題下一步需要改進的地方。

圖9 不同地圖模式下改進的A*算法結果Fig.9 Results of improved A*algorithm in different map modes

表3 不同目標方位在不同地圖模式下的路徑規劃統計Tab.3 Statistical table of path planning with different target directions

2.4 實際場景驗證

為驗證本文改進的A*算法的有效性，本文在開源的硬件平臺Turtlebot3 上進行測試，Turtlebot3 是一個小型、低成本、完全可編程的移動機器人。其大小是138 mm×178 mm×192 mm，CPU 是32 位ARM Cortex-M7，具備里程計、360°激光距離傳感器LDS-01 和IMU 等基本導航設備。測試環境是實驗室環境，其范圍大概在5 m×4 m 的范圍，路上沒有障礙物。測試的過程是從室內某一點運行到走廊上某一點。圖10 是實際場景下改進的A*算法結果，圖中黑色曲線是全局規劃的路線，路徑的曲率較小的原因是在A*算法規劃出路徑后進行了路徑平滑處理。通過4 步，Turtlebot3 可以自主行進到指定位置。實驗說明本文提出的改進的A*算法具備實用價值，可以在規避障礙物的條件下高效地進行全局路徑規劃。

圖10 實際場景下改進的A*算法結果Fig.10 Results of improved A*algorithm in real scene

3 結語

在進行無人車路徑規劃設計時，傳統A*算法存在一些不足，比如擴展方向盲目性、啟發函數只考慮當前節點的信息等，導致A*算法尋路過程包含很多冗余的節點，計算效率較低。本文從四個方面改進傳統A*算法：首先越靠近目標節點越以目標節點的估計距離作為選取待擴展節點的原則，為了實現這個目的，本文引入根據模擬退火從Open列表中選取待擴展節點以避免陷入局部最優解；其次改進的A*算法的啟發函數，加入了n個父輩的信息，以此作為啟發尋路的“歷史經驗”；然后根據待擴展節點和目標點相對位置選擇擴展象限，以此來避免待擴展節點盲目往四面八方擴展；最后判斷目標可見時跳出尋路過程，以此避免接近目標點時還存在盲目的冗余擴展。

在Matlab環境下，對不同目標方位下改進的A*算法和A*算法的路徑規劃算法進行仿真，結果顯示，本文提出的改進的A*算法在運行時間上減少67.06%，經歷的柵格數減少73.53%，優化路徑長度浮動范圍在±0.6%。可以看出本文改進的A*算法在不同情況下都能夠有效地縮短尋路時間，減少冗余擴展，算法的計算效率較高。