融合改進A*算法和DWA算法的全局動態路徑規劃

董曉東，李 剛，宗長富，李永明，李云龍，李 祥

（1.遼寧工業大學汽車與交通工程學院，遼寧 錦州 121001；2.吉林大學汽車仿真與控制國家重點實驗室，長春 130022）

0 引言

無人駕駛技術是指利用機器人替代人類在工業、交通、物流、醫療、軍事等領域中完成各種工作任務。實現無人駕駛最重要的環節之一就是路徑規劃，目標是通過搜索尋找最佳路徑，連接起點和終點［1］。目前，無人駕駛路徑規劃研究主要分為2類：一類是基于車輛動力學模型，采用運動學優化的方法進行路徑規劃；另一類是基于全局環境建模和動態障礙物避障的方法進行路徑規劃［2］。在整個控制系統中，路徑規劃環節承上啟下，是無人駕駛技術的核心［3］。

路徑規劃算法根據適用的場景分類為全局路徑規劃和局部路徑規劃［4］。全局路徑規劃一般用來生成全局參考軌跡，如傳統Dijkstra算法［5］、A*算法［6］、D*算法［7］、快速擴展隨機樹RRT算法、蟻群算法、粒子群算法、遺傳算法等［8］；局部路徑規劃一般是在參考路徑的引導下，規劃出未來短暫時間段內可通行的局部路徑，常見的有動態窗口算法、人工勢場法、TEB算法等。

雖然A*算法具有較好的求解效率而被用于全局尋優，但該尋優路線仍面臨著拐點過多、節點冗余、與障礙間距離過于接近等問題。同時，如果應用動態窗口法（DWA）進行路徑規劃可以使無人駕駛汽車具備良好的避障能力，并生成平滑的路徑，無需額外優化。但是，該方法可能會陷入局部最優解，無法按全局最優路徑到達目標點。隨著上述問題造成的無人駕駛汽車路徑規劃局限性越來越大，有的專家學者對傳統A*算法進行了相應改進優化，克服了上述問題衍生出了各種新的路徑規劃方法，但需要聯合全局與局部2種方法［9］；Guruji等［10］提出了一種名為時效A*的改進算法，該算法通過使用斜率檢測的方式來減少相鄰節點的搜索次數，從而提高了算法的效率；姚進鑫等［11］提出了一種將跳躍點搜索與動態窗口法相融合的路徑規劃算法，該方法能較好地克服最優A*方法在曲線轉彎處存在的曲率不連續性問題，改善了軌跡的光滑度和整體優化；合肥工業大學的賈嶼等［12］將A*算法與跳點策略相結合，給出了一種新的優化策略，該策略在提高尋優速度和路徑安全性方面取得了顯著的進展；重慶理工大學的尹婉秋等［13］基于A*算法，將人工勢場法和障礙的位置信息相結合，利用粒子群算法自適應地選取參數，解決了傳統A*算法“先移后避”的難題，降低了A*算法的計算復雜度；重慶理工大學的李長庚等［14］引入一種雙向交叉搜索策略和基于指數衰減的啟發函數權重方法，解決了現有A*算法存在搜索時間長、轉彎角度大和曲線不連續等問題；遼寧工業大學的白云龍等［15］提出一種通過擴展搜索鄰域的方法來提高A*算法的規劃效率，并將人工勢場法融入其中來避障，最后通過3次B樣條曲線平滑路徑中出現的尖銳節點；海南大學的田晃等［16］提出了一種基于多目標節點的改進動態窗口算法，在提高路徑平滑度的同時，相對于傳統算法具有更好的局部避障能力；張振等［17］提出了一種實時路徑規劃方法，該方法將改良的A*算法與動態窗口法相結合，將經過優化的關鍵點作為臨時目標點，能夠生成一條基于全局最優的光滑曲線路徑；郭翰卿等［18］在傳統A*算法的評價函數中引入安全估值，通過運用改進的動態窗口評價函數，成功將安全性A*算法與動態窗口法相融合，實現了在全局路徑行進中的動態避障。

路徑規劃算法常用于機器人領域，無人駕駛的路徑規劃算法受其啟發［19］。在無人駕駛系統中，路徑規劃算法作為其關鍵技術之一，引起了國內外學者的廣泛關注，并呈現出百家爭鳴的局面［20］。雖然大部分研究人員對現有的A*算法、DWA算法作了一定的修改，但是仍然不能徹底解決實際規劃中的某些問題?；诖?，本文中對A*算法進行了改進，使其可以尋找到最優的全局路線節點，同時利用動態窗口方法對鄰近節點進行了局部規劃。該方法可使無人車在全局路徑中遭遇任意的障礙時合理繞開障礙，返回到全局路徑，這樣不僅可以保證路徑的整體最優性，而且可以有效地克服各種隨機障礙對規劃路徑的影響。

1 算法基礎

1.1 傳統A*算法

A*算法被廣泛應用于優化路徑的搜索，因為它具有高效的搜索效率和快速的規劃速度［21］。A*算法基本思想是從起始柵格點開始，尋找與起點相鄰的子柵格點。每一次都會從周圍的子柵格點中選擇一個評估函數值最小的點作為下一步的搜索節點，即當前節點。然后，在當前節點的基礎上，生成與其鄰接的子柵格點，并以目標節點為終點進行下一步的搜索。

A*算法的評價函數對檢索空間的大小有影響，而檢索空間的大小、訪問量反過來又對A*算法的運算速度有影響。所以，A*算法評價函數的優劣對其尋優效果及尋優效率有著很大的影響。

設f（n）為A*算法評價函數，公式如下：

式中：f（n）為起始位置到當前位置n之間總和的代價函數；h（n）為當前節點位置n到目的地位置之間估計路徑代價的啟發函數［22］。

啟發函數的選擇會影響A*算法的效率和準確性。如果沒有障礙物阻擋，那么用歐幾里得距離作為啟發函數，就能使得估計路徑和實際最短路徑一致，這樣算法就能快速而精確地找到目標，不會浪費時間在多余的節點上。但是在實際情況中，往往有很多障礙物使得實際路徑比估計路徑要長，這時候算法就會產生很多冗余節點，增大了搜索空間，降低了搜索速度［23］。如果能夠調整啟發函數的值，使其接近于實際路徑的值，就能夠有效地提高搜索效率。由于不同的地圖環境中障礙物的分布不同，如果能夠對障礙物進行量化，并根據量化數據調節啟發函數，就能夠提高算法的效率和適應性。

針對傳統A*算法在路徑規劃中存在的缺陷，本文對其進行了改進和優化。具體方法如下：

1）通過柵格法對環境進行建模，并根據式（9）計算障礙率P。同時，設定無人駕駛汽車的起始節點和目標節點，并建立Open列表和Close列表。將起始節點添加到Open列表中，而Close列表為空。

2）算法開始遍歷起始節點周圍的節點，并根據優化后的子節點選擇方式生成可選子節點。隨后，將起始節點從Open列表中移到Close列表中，若Open列表為空，表示不存在待搜索節點，算法將終止，表明無可行的路徑。

3）若Open列表不為空且存在待搜索節點，則分別計算各子節點的f（n）值，并選取具有最小f（n）值的節點作為下一個路徑節點，同時將該節點從Open列表中移到Close列表中。

4）判斷該擴展節點是否為目標節點。若為目標節點，則從目標節點開始逆向搜索其父節點，并對路徑進行雙向平滑度優化。最后，輸出規劃路徑。若非目標節點，則繼續擴展其周圍節點并計算子節點的f（n）值，將不在Close列表中的周圍子節點添加到Open列表中。

5）重復步驟4），直至擴展到目標節點并輸出最終規劃路徑。

改進A*算法的流程如圖1所示。

圖1 改進A*算法流程

1.2 動態窗口（DWA）算法

動態窗口（DWA）法是一種局部動態路徑規劃算法，它通過對無人駕駛汽車的位置變化進行簡化，僅考慮線速度和角速度控制。它將避障問題簡化為空間中的運動約束問題，從而可以根據運動約束條件選擇最佳的局部路徑［24］。假設一輛無人駕駛汽車在時間段（t1－t2）內運動，那么它對應的位姿變化信息如下：

式中：xt2和yt2分別為t2處無人駕駛汽車的x、y坐標位置；θt2為t2時刻無人駕駛汽車的航向角；v為無人駕駛汽車的線速度；ω為無人駕駛汽車的角速度；Δt為（t1－t2）的時間間隔。

一旦在預測時間內對速度空間進行了采樣，就可以使用評價函數來評估預測軌跡的優劣。DWA算法的評價函數通常需要根據實際情況進行設置，主要包括目標距離、速度、障礙物和方位角的代價。本文中根據式（5）設置DWA算法的評價函數，即總成本越小，路徑規劃過程越好。

式中：C（v，ω）為對速度采樣的總成本；D（v，ω）為目標點位置與速度取樣空間之間的距離成本；S（v，ω）為速度采樣中的速度成本；O（v，ω）為速度采樣空間和障礙物之間的距離成本；α，β，λ分別表示目標距離、速度、障礙物距離的單位成本增益。

各代價函數D（v，ω）、S（v，ω）、O（v，ω）的具體函數表示為：

式中：（xt，yt）為預測節點的（x，y）坐標位置；（xd，yd）為目標點的（x，y）坐標位置；vmax為無人駕駛汽車最大速度；vt為預測速度空間的速度；（xoi，yoi）為第i個障礙物的（x，y）坐標位置。

2 改進傳統A*算法

2.1 量化柵格地圖障礙物信息

柵格地圖是一種用網格單元表示實際地圖的方法，每個網格單元可以表示一個區域是否有障礙物。有障礙物的網格單元叫做障礙格，障礙格的多少會影響地圖上可供選擇的路徑的數量，也會使得最短路徑的長度大于起點和終點之間的直線距離。為了估計最短路徑的長度，需要定義障礙率P，它可以反映地圖環境的復雜程度。障礙率P是指當前點和終點構成的矩形局部環境中，障礙格的數量占該局部地圖總格數的比例。假設當前位置與目的地位置組成的柵格地圖內的障礙柵格個數為N，初始位置坐標（xs，ys）、目的地位置坐標（xg，yg），其表達式如式（9）所示。

2.2 改進A*算法的評價函數

評價函數是A*算法的核心，為了提高A*算法的搜索效率和精度，本文中針對A*算法的評價函數進行了優化，提升搜索速度和準確度。評價函數包括代價函數和啟發函數2部分，其中啟發函數是決定搜索效果的重要因素。啟發函數的值要根據地圖情況中的障礙物密度進行變化，障礙物密度越高，啟發函數越低，搜索范圍越廣，搜索精確度越好。障礙物密度由障礙物個數和起點、終點的位置共同確定，它使得A*算法的搜索范圍具有變化性。通過將障礙物密度納入評價函數中，根據障礙物密度自適應調節代價函數和啟發函數的比重，實現評價函數的自動調節，從而保證路徑規劃的靈活性、快速性和最優性。

式中：g（n）為初始位置（xg，yg）到當前位置（xn，yn）之間累計距離的代價函數；h（n）為當前位置（xn，yn）到目的地位置（xg，yg）之間的啟發函數。

2.3 優化搜索點選取策略

A*算法在搜索過程中，需要維護一個開放列表，其中包含了所有當前點周圍可達的子節點。每次選擇評價函數值最小的節點作為下一個路徑節點。在生成子節點時，需要判斷子節點是否位于障礙柵格上。如果是，將不會將該子節點加入開放列表。這樣做的結果會導致規劃出的路徑可能沿著障礙物的邊緣走，甚至還可能穿過2個相鄰障礙物的頂點。這樣就會增加無人駕駛汽車與障礙物碰撞的風險，如圖2所示。

圖2 碰撞風險示例

如圖3所示，展示了父節點及其8個子節點之間的位置關系。根據子節點相對于父節點的位置，將其分為3種類型。第Ⅰ類是位于父節點正上方和正下方的子節點2和6，第Ⅱ類是位于父節點正左方和正右方的子節點4和8，第Ⅲ類是位于父節點斜對角線上的子節點1、3、5和7。

圖3 節點移動方向示意圖

為了解決傳統A*算法可能導致路徑穿過障礙物頂點的問題，本文中提出了一種新的搜索點選取策略，根據子節點和障礙物的位置關系，排除一些不優的子節點。以圖3為例，為了防止路徑穿過障礙物頂點，假設某個子節點是障礙物，那么子節點選取規則如下：

1）如果障礙子節點在父節點的正上方或正下方（第Ⅰ類位置），則舍棄該障礙子節點左右兩側的子節點（如子節點1、3或子節點5、7）。

2）如果障礙子節點在父節點的正左方或正右方（第Ⅱ類位置），則舍棄該障礙子節點上下兩側的子節點（如子節點1、7或子節點3、5）。

3）如果障礙子節點在父節點的斜對角線上（第Ⅲ類位置），則不做任何處理。通過這種優化方式，改進A*算法可以有效地規避風險路徑，不會讓路徑斜穿過障礙物頂點，從而減少無人駕駛汽車與障礙物碰撞的可能性。圖2中的風險路徑經過優化后變成了圖4中的安全路徑。

圖4 優化后的安全路徑

2.4 路徑平滑度優化

本文中針對柵格地圖中路徑的轉折和長度問題，設計了一種雙向節點判斷的路徑平滑度優化方法。該方法的核心思想是，對于路徑中的任意2個不相鄰的節點，如果它們之間的連線長度小于它們之間的規劃路徑長度，并且連線不與障礙物相交，那么就可以刪除它們之間的所有節點。這樣可以有效地減少路徑的轉折和長度，提高路徑的平滑度。此外，該算法還考慮了防碰撞的要求，通過設置安全距離和計算障礙點到連線的垂直距離和縱軸距離，以及利用單元障礙柵格的外接圓半徑，判斷路徑是否安全，避免與障礙物發生碰撞。

以圖5所示的柵格地圖路徑規劃軌跡為例，采用A*算法規劃的路徑由若干柵格節點組成，圖中優化前的路徑為（S→n1→n2→n3→n4→n5→n6→n7→n8→n9→n10→G），該路徑存在冗余節點、過多轉折和節點僅能在柵格中心等問題，不利于無人駕駛汽車路徑跟隨。

圖5 柵格地圖路徑規劃軌跡

針對以上問題本文中對A*算法得到的路徑節點進行優化處理，包括雙向刪除冗余節點、障礙距離判斷和雙向平滑度優化，使得路徑節點可以在柵格中任意位置選擇，減少了路徑的轉折和長度，提高了路徑的平滑度。具體優化步驟如下：

步驟1刪掉路徑節點中同一直線上的中間點，只保留起點、拐點和終點。這樣可以簡化路徑，減少不必要的節點，處理后路徑為S→n7→n8→n9→G。

步驟2 從起始點S方向，在保留的2拐點ni、nj之間每隔k步長取一節點。這樣可以在2拐點之間生成一系列候選節點，用于優化路徑。

步驟3判斷2點之間路徑是否有障礙物。如果有，就不選擇當前節點為路徑節點，如果沒有，就繼續下一步。

步驟4計算路徑旁障礙物與路徑的距離，并使用大小關系來判斷是否選擇當前節點作為路徑節點。這樣可以保證路徑不會太靠近障礙物，避免碰撞危險，處理后的路徑為S→n11→G。

步驟5反向取點后，按照與步驟2、3、4相同的判斷方式，從目的地點G方向進行判斷。這樣可以優化路徑，使其變得更短或更平滑。處理后的優化路徑為S→n12→G。

步驟6輸出優化路徑Path。

3 融合算法

根據前文所述，改進后的A*算法在全局信息已知的情況下，能夠找到最優的全局路徑。然而，當存在未知障礙物的環境中，依賴改進后的A*算法單獨進行路徑規劃并不有效。DWA算法可以實現路徑的平滑和實時避障，但很容易陷入局部最優，成功規劃路徑的概率較低。因此，本文中提出了一種融合改進A*算法和DWA算法的方法。通過巧妙地結合這2種算法，可以得到一種既能保留全局最優性，又能適應局部實時變化的路徑規劃算法。

3.1 優化DWA算法評價函數

根據無人車的速度范圍和運動模型，在仿真中使用了多個軌跡，并使用估計函數進行篩選，以選擇最優軌跡［25］。針對傳統動態窗口法容易出現局部最優的問題，對動態窗口算法的估計函數進行了改進。新的評估函數結合了2.2節改進的A*算法提供的全局軌跡信息，確保最終的局部軌跡以全局最優軌跡為基礎。其中DWA算法的基本參數設定如下：最大速度限制設置為1 m／s；最大角速度限制定為20（°）／s；速度分辨率設置為0.01 m／s；角速度分辨率設置為1（°）／s；加速度限制為0.2 m／s2；角加速度限制為50（°）／s2；評價函數的各參數分別為：α＝0.1，β＝0.05，γ＝0.2，預測時間的周期設定為3.0 s。改進后的估計函數如式（17）所示：

式中：PHead（v，ω）為軌跡終點方向與當前目標點之間的角度差；dist（v，ω）為軌跡與最近障礙物之間的距離；vel（v，ω）為當前速度評價函數；σ為平滑函數；α、β、Υ為加權系數。

3.2 算法融合策略

為了同時實現全局路徑優化和隨機避障目標，本文中提出了一種結合改進A*算法和動態窗口法的路徑規劃方法。改進A*算法利用了啟發式搜索算法來在復雜環境中快速找到全局最優路徑。動態窗口法是一種局部避障算法，能夠根據被控對象的運動狀態和環境信息，實時生成適應的速度空間。利用改進的A*算法進行全局路徑規劃后，獲得全局最優節點序列后，可以使用動態窗口法在相鄰節點之間進行局部路徑規劃。本文中將這2種算法結合起來，所采用的最終融合算法流程如圖6所示。

圖6 融合算法流程

4 仿真驗證

4.1 改進A*算法實驗仿真分析

為了驗證本文中改進A*算法的有效性，在柵格地圖環境信息相同的前提下，分別用改進后的A*算法、傳統A*算法、蟻群算法、Dijkstra算法進行路徑規劃仿真對比實驗，并通過改變對應柵格地圖信息進一步仿真驗證。柵格地圖模型大小分別為20×20、30×30和50×50。在柵格地圖環境中，黑色區域表示障礙，白色區域為可移動區域。計算機配置為：Windows 11操作系統，處理器為AMD Ryzen 7 5800H with Radeon Graphics，主頻為3 201 MHz，運行內存為16 G。

1）實驗1：柵格地圖20×20，障礙物覆蓋率p＝20%。路徑規劃軌跡如圖7所示。

圖7 實驗1的4種算法路徑規劃軌跡

2）實驗2：柵格地圖30×30，障礙物覆蓋率p＝13%。路徑規劃軌跡如圖8所示。

圖8 實驗2的4種算法路徑規劃軌跡

3）實驗3：柵格地圖30×30，障礙物覆蓋率p＝25%。路徑規劃軌跡如圖9所示。

圖9 實驗3的4種算法路徑規劃軌跡

4）實驗4：柵格地圖50×50，障礙物覆蓋率p＝25%。路徑規劃軌跡如圖10所示。

圖10 實驗4的4種算法路徑規劃軌跡

本文中改進A*算法在評價函數中增加了環境信息的引導，搜索空間對比4組仿真實驗比傳統A*算法分別降低29.2%、45.83%、61.17%、60.36%；相比Dijkstra算法分別降低72.8%、83.18%、84.23%、88.37%。同時增加子節點優化規則及路徑平滑度優化，使改進A*算法搜索的路徑轉折次數更少，轉折角度更小，在保證與障礙物保持在安全距離之外的同時，規劃出的路徑有效避免了可能會沿著障礙物的邊緣走，甚至穿過2個障礙物相鄰的頂點的情況。從圖8—圖10可知，隨著柵格地圖擴大和障礙物的增多，蟻群算法所規劃路徑由于信息素高低的影響，出現折返路徑、規劃時間長、路徑折點多等局限性。Dijkstra算法與傳統A*算法規劃的路徑均一直朝著目標點方向，沒有考慮障礙物對路徑的影響，選擇了局部路徑最短的路線，出現多次轉折，且不能避免規劃出的路徑可能會沿著障礙物的邊緣走，甚至穿過2個障礙物相鄰的頂點的情況。4種算法性能數對比如表1所示。

表1 4種算法性能數

4.2 融合算法實驗仿真分析

通過在柵格地圖中添加不同的隨機障礙物，分別設置3組改進A*和DWA算法融合算法在障礙物覆蓋率p＝20%的20×20柵格地圖中進行路徑規劃，進而對融合算法的隨機避障能力進行分析驗證。

1）實驗1：柵格地圖20×20，障礙物覆蓋率p＝20%，無障礙物。路徑規劃軌跡如圖11所示。

圖11 無障礙物路徑規劃軌跡

2）實驗2：柵格地圖20×20，障礙物覆蓋率p＝20%，寬闊地帶出現障礙物。路徑規劃軌跡如圖12所示。

圖12 寬闊地帶出現障礙物路徑規劃軌跡

3）實驗3：柵格地圖20×20，障礙物覆蓋率p＝20%，狹窄地帶出現障礙物。路徑規劃軌跡如圖13所示。

圖13 狹窄地帶出現障礙物路徑規劃軌跡

4）實驗4：柵格地圖20×20，障礙物覆蓋率p＝20%，模擬較為復雜環境。路徑規劃軌跡如圖14所示。

圖14 較為復雜環境路徑規劃軌跡

通過上述4組對比實驗分析可知，當地圖中無隨機障礙物出現時，改進A*算法與融合算法均可以規劃一條從起點到終點的路徑規劃方法。然而，在廣闊的區域中添加隨機障礙物后，單獨改進A*算法無法有效避免突然出現的隨機障礙物。相比之下，融合算法實現了隨機避障，同時確保規劃路徑保持全局最優性，并且使路徑平滑，以適應無人駕駛車輛的跟蹤行駛要求。后續通過改變障礙物的數量和位置來驗證融合算法的魯棒性。結果表明，在較為復雜的情況下本文融合算法仍然能夠合理避開隨機障礙物并到達目標點。融合算法路徑規劃性能參數如表2所示。

表2 融合算法路徑規劃性能參數

5 結論

1）本文中通過改進傳統A*算法，在搜索空間上實現了大幅的降低，相較于Dijkstra算法的降低幅度在較為復雜的柵格地圖環境中更是最大達到了88.37%。同時通過增加子節點優化規則和路徑平滑度優化，使得改進后的A*算法搜索路徑與障礙物保持安全距離，規劃出的路徑有效避免沿著障礙物邊緣走或穿過相鄰障礙物頂點的情況，路徑轉折次數更少，轉折角度更小。這表明本研究中改進的A*算法具有一定的優勢和有效性。

2）改進后的A*算法與動態窗口法相結合，能夠成功避開隨機障礙物到達目的地點。它不僅能根據全局路徑修正局部路徑，還能滿足無人駕駛汽車在尋路過程中避免碰撞的要求。由此可見，該算法的可行性和有效性都具有一定的優勢。

本文中的研究成果可用于復雜環境中無人駕駛汽車的路徑規劃。未來的研究可以通過調整動態窗口法的權值組合，提升環境適應能力，根據障礙物的實際分布情況進行對應優化。