融合距離引導式A^*算法與動態窗口法的移動機器人路徑規劃

摘要： 為解決移動機器人路徑規劃中效率低下等問題，提出一種距離引導式A^*算法與動態窗口法的融合算法。在改進A^*算法中，引入雙向搜索策略，采用綜合距離啟發函數，利用全局路徑篩選策略對路徑進行優化。在得到全局最優路徑的基礎上，與動態窗口法結合，實現移動機器人的動態避障。結果表明：距離引導式A^*算法和文中融合算法在路徑長度、遍歷節點數目和運行時間方面實現了顯著提升，能更好地滿足移動機器人對路徑規劃的要求。

關鍵詞： 移動機器人；路徑規劃；A^*算法；動態窗口法；動態避障

中圖分類號： TP 242文獻標志碼： A 文章編號： 1000-5013（2025）01-0087-08

Path Planning for Mobile Robots by Integrating Distance-Guided A^* Algorithm and Dynamic Window Approach

HUANG Yuhang^1，2，LI Guogang^1，2，JIAO Qi^1，2，CAO Dongping^1，2

（1. College of Information Science and Engineering，Huaqiao University，Xiamen 361021，China；2. Xiamen City Key Laboratory of Application Specific Integrated Circuit System，Huaqiao University，Xiamen 361021，China）

Abstract： To solve the problem of low efficiency in mobile robot path planning，a fusion algorithm combining a distance-guided A^* algorithm with the dynamic window approach is proposed. In the improved A^* algorithm，a bidirectional search strategy is introduced，and a comprehensive distance heuristic function is adopted，utilizing a global path filtering strategy to optimize the path. On the basis of obtaining the global optimal path，it is combined with the dynamic window approach to achieve dynamic obstacle avoidance for the mobile robot. The results show that the distance-guided A^* algorithm and the proposed fusion algorithm achieve significant improvements in terms of path length，the number of traversed nodes，and running time，which better satisfies the requirements of mobile robot for path planning.

Keywords： mobile robot；path planning；A^* algorithm；dynamic window approach；dynamic obstacle avoidance

隨著智能科技和自動化技術的不斷進步，移動機器人的路徑規劃問題逐漸成為備受關注的研究熱點。機器人路徑規劃中，Dijkstra算法^［1］雖然被廣泛使用，但在實際應用中可能會導致非最優路徑的問題，同時也會出現計算復雜度較高等問題。 D^*算法^［2］主要用于動態環境下的路徑規劃，但在靜態環境中，其效率略遜于其他算法，且計算復雜度較高。 快速擴展隨機樹（RRT）算法^［3］是一種基于采樣的算法，其主要目標是快速探索，并找到一個近似的解決方案，但并不能保證找到最短或最優路徑。相較而言，A^*算法^［4］通過選擇合適的啟發函數，能夠在室內環境下高效地搜索并找到最佳路徑，使其成為移動機器人全局路徑規劃的首選算法之一。動態窗口法（DWA）是一種常見的局部路徑規劃算法，根據機器人的運動學模型，對各個軌跡進行評分，從而選擇最優路徑，實現動態避障。

趙曉等^［5］巧妙地加入跳點搜索機制，實現長距離的跳躍，避免A^*算法尋路過程中對冗余節點的計算，顯著降低了計算的復雜度，使尋路更加快速和高效。Wang等^［6］提出一種指數加權方法，該方法充分考慮了路況的影響因素，并通過對啟發函數的改進，成功找到全局最優的路徑。田海波等^［7］提出一種考慮轉彎成本的估價評估搜索效率，得到相對較短的路徑長度，但這種方法會導致節點擴展較多，消耗大量內存。李炯逸等^［8］提出一種針對A^*算法在室內環境路徑規劃中存在問題的改進方法。辛煜等^［9］通過將A^*算法的可搜索鄰域擴展為無限個，縮短了路徑長度，減少了拐點數量。王中玉等^［10］通過檢測當前節點和其祖父節點之間的連線是否存在障礙物，消除路徑中的不必要的節點，從而優化路徑的長度和轉折角度。姬鵬等^［11］通過優化搜索點、評價函數和拐點，提高檢索速度和路徑平滑度。程傳奇等^［12］通過優化啟發函數，選擇關鍵點，減少路徑規劃的長度，并與動態窗口法結合，提高路徑平滑性和避障能力。唐嘉寧等^［13］提出一種基于雙向機制的改進A^*算法，顯著提高了路徑規劃的效率。

基于此，本文提出一種距離引導式A^*算法與動態窗口法相結合的融合算法^［14-16］。

1 A^*算法概述

1.1 A^*算法

A^*算法結合貪心策略和Dijkstra算法的特點，使用一個評價函數，通過構建和更新開放列表及關閉列表，不斷選擇代價最低的節點作為當前節點，直到找到目標節點或滿足停止條件為止。A^*算法的代價函數是其核心要素，它能夠有效地指導搜索過程朝著目標節點進行。A^*算法的代價函數f（n）可表示為

f（n）=g（n）+h（n）。（1）

式（1）中：n為當前節點；g（n）為起點到當前節點n的實際代價；h（n）為啟發函數，表示當前節點n到目標節點的估計代價。

1.2 改進A^*算法

為了克服A^*算法在搜索過程中出現的遍歷節點過多、路徑較長、效率較低等問題，采用雙向搜索策略、綜合距離啟發函數及全局路徑篩選策略來提升效率。

1.2.1 雙向搜索策略 同時從起始節點和目標節點進行搜索，加速路徑規劃過程。雙向搜索策略具體有以下7個步驟。

步驟1 在雙向搜索策略中，需要初始化兩個開放列表openlist1，openlist2，以及兩個關閉列表closelist1，closelist2，分別用于存儲待搜索的節點和已經搜索過的節點。

步驟2 將起始節點加入openlist1中，目標節點加入openlist2中。同時從起始節點和目標節點開始搜索，每次從兩個開放列表中選擇代價最小的節點進行正向搜索和反向搜索。在正向搜索中，需要更新正向搜索節點的代價函數，并判斷最優節點是否在反向搜索的路徑上。若存在，則找到路徑；若不存在，則將節點加入closelist1。

步驟3 對當前最優節點進行處理。找出當前節點的連接點m，若節點m在closelist1中，則搜索下一個節點m；若節點m不在closelist1中，則判斷節點m是否在正向搜索的openlist1中，若節點m不在openlist1中，則計算該點的實際代價g（m）、估計代價h（m）和代價函數f（m）。

步驟4 節點m的父節點設為n，并將節點m按代價函數f（m）從小到大的順序添加到正向搜索的openlist1中。

步驟5 判斷實際代價g（n）加上父節點n到節點m的距離是否大于g（m）。若大于g（m），則進一步搜索父節點n的下一連接點；若小于g（m），則重新計算g（m），h（m），f（m）。直至與父節點n的所有連接點都已搜索完畢。

步驟6 進行反向搜索，步驟與正向搜索一致。

步驟7 判斷步驟4，5的返回值，若返回openlist1與openlist2列表均為空，仍未到達目標節點，則表示尋路失敗；若當起始節點和目標節點搜索隊列中的某個節點在地圖上相遇，則找到一條從起始節點到目標節點的路徑。此時，可以根據相遇節點及其前驅節點，回溯構造出完整的路徑。

1.2.2 綜合距離啟發函數 首先，綜合距離啟發函數在計算中進行對角線方向距離的調整，通過引入一個額外的因子平衡直線和對角線移動的代價，確保搜索路徑更加均勻和平滑，能夠更好地引導搜索方向。然后，引入動態權重來控制搜索速度。最后，加入當前節點和目標節點的距離與起始節點和目標節點距離的比值，進一步優化搜索的準確性。通過對啟發函數的改進，算法可以在不同情況下靈活地調整搜索速度和尋優性能，從而找到最優解。綜合距離啟發函數公式為

式（2）～（7）中：（x_s，y_s）為起始節點坐標，（x_g，y_g）為目標節點坐標；（x_n，y_n）為當前節點坐標；當d（n）gt;5時，w（n）=2.8，當d（n）≤5時，w（n）=0.9。

1.2.3 全局路徑篩選策略 為了刪除全局路徑中產生的冗余拐點，引入全局路徑篩選策略，可有效地去除冗余拐點，從而提升路徑規劃的結果。

優化后的路徑，如圖1所示。圖1中：A～K為算法遍歷的最優節點。

原始路徑為A-B-C-D-E-F-G-H-I-J-K，優化后的路徑為A-E-F-I-K。全局路徑篩選策略具體有以下4個步驟。

步驟1 獲取雙向A^*算法規劃路徑中的每個拐點坐標{p₁，p₂，p₃，…，p_n}，將起始節點p_s（x_s，y_s）加入拐點坐標集的首位，把目標節點p_g（x_g，y_g）加入拐點坐標集的末位，得到所有關鍵節點，并將其放入集合A{p_s，p₁，p₂，p₃，…，p_n，p_g}。

步驟2 建立一個空的集合B{SetB}，用于存放經過優化后的坐標點。

步驟3 首先，判斷集合A中的首位坐標p_s與集合A中的末位坐標p_g形成的線段是否與障礙物相交；若不相交，則判斷集合A中的首位坐標p_s與集合A中的倒數第2位坐標p_n形成的線段是否與障礙物相交；若不相交，則判斷集合A中的首位坐標p_s與集合A中的倒數第3位坐標p_n-1形成的線段是否與障礙物相交。直到找到集合A中的首位坐標p_s與集合A中的某個坐標p_x形成的線段與障礙物不相交，再將p_s加入集合B中，并將集合A更新為{p_x，…，p_n，p_g}。

步驟4 將更新后的集合A重復步驟3，再次進行判斷，并持續更新集合A，B。當集合A中的坐標數量為兩個時，將最后兩個坐標加入集合B。最后，集合B為優化后的坐標點，即優化后的路徑。

路徑優化策略中的線段相交檢測采用向量叉積的性質進行判斷，p₁，p₂是拐點集中的兩個坐標，p₃，p₄是障礙物坐標集中的兩個坐標。判斷線段是否相交的過程具體有以下2個步驟。

步驟1 cp₁是向量p₁p₂和向量p₁p₃的叉積；cp₂是向量p₁p₂和向量p₁p₄的叉積；cp₃是向量p₃p₄和向量p₃p₁的叉積；cp₄是向量p₃p₄和向量p₃p₂的叉積。

步驟2 若cp₁×cp₂≤0，且cp₃×cp₄≤0，說明拐點間形成的線段與障礙物點間形成的線段相交。否則，說明拐點間形成的線段與障礙物點間形成的線段不相交。

2 動態窗口法

2.1 機器人運動學模型

機器人的運動學模型為

x（t+1）=x（t）+v_tΔtcos θ_t，（8）

y（t+1）=y（t）+v_tΔtsin θ_t，（9）

θ（t+1）=θ（t）+ω_tΔt。（10）

式（8）～（10）中：x（t+1），y（t+1）為t+1時刻的坐標；θ（t+1）為t+1時刻的偏航角；Δt為時間間隔。

2.2 速度采樣

速度采樣依賴于機器人當前所處的狀態，并需要考慮機器人自身約束及環境約束等參數。機器人的速度限制v_m為

v_m={（v，ω）v∈［v_min，v_max］，ω∈［ω_min，ω_max］}。（11）

式（11）中：v為機器人的線速度；ω為機器人的角速度；v_min，v_max分別為機器人的最小線速度和最大線速度；ω_min，ω_max分別為機器人的最小角速度和最大角速度。

機器人電機加減速限制v_d為

v_d={（v，ω）v∈［v_n-a_maxΔt，v_n+a_maxΔt］，ω∈［ω_n-α_maxΔt，ω_n+α_maxΔt］}。（12）

式（12）中：a_max，α_max分別為機器人的最大線加速度和最大角加速度；v_n，ω_n分別為機器人當前的線速度和角速度。

機器人安全距離限制v_s為

式（13）中：dist（v，ω）為當前狀態和障礙物之間的距離。

動態窗口速度限制v_w為

v_w=v_m∩v_d∩v_s。（14）

2.3 評價函數

評價函數起到決策機器人運動路徑的作用。改進后的評價函數G（v，ω）為

G（v，ω）=δhead（v，ω）+βdist（v，ω）+γvel（v，ω）+ηdist_s（v，ω）。（15）

式（15）中：head（v，ω）為方位角的評價函數，是當前采樣速度下產生的軌跡終點位置方向與目標點連線的夾角；dist（v，ω）為障礙物距離的評價函數，是當前速度下對應軌跡與障礙物之間的距離；vel（v，ω）為速度的評價函數，是當前軌跡的速度大小；dist_s（v，ω） 為當前軌跡與目標節點距離的評價函數；δ，β，γ，η均為加權系數。

3 距離引導式A^*算法與動態窗口法的融合

為了保證機器人路徑規劃的安全和效率，提出一種融合路徑規劃算法（文中融合算法），其流程圖如圖2所示。即將距離引導式A^*算法與動態窗口法結合，使用距離引導式A^*算法在已知的環境地圖中規劃出一條初始路徑，在機器人沿著這條路徑移動的過程中，動態窗口法會根據實時的傳感器數據動態地調整機器人的速度和方向，以避開突然出現的未知障礙物。這種結合使機器人能夠在保證路徑規劃效率的同時，具備一定的安全性，能夠在復雜多變的環境中靈活調整路徑，從而提高機器人的效率。

4 仿真結果與分析

為了驗證文中融合算法的有效性，使用i7-13700H，16 GB運行內存的筆記本電腦作為實驗硬件設備，并采用Python 3.10作為實驗的軟件運行環境。A^*算法采用8鄰域搜索方法，使用歐幾里得距離作為啟發函數來計算估計代價h（n）。為更好地體現距離引導式A^*算法和動態窗口法的效果及地圖適應性，分別設計4種實驗的地圖環境，并與A^*算法和文獻［12］A^*算法進行對比。

融合算法評價函數的加權系數δ=0.15，β=1.0，γ=1.0，η=2.0。融合算法的仿真機器人運動學參數，如表1所示。表1中：d_v為速度分辨率；d_ω為轉速分辨率。

不同的地圖環境下全局路徑規劃仿真結果，如圖3～6所示。全局路徑規劃實驗結果對比，如表2所示。表2中：t為運行時間；l為路徑長度；N為遍歷節點數。

不同的地圖環境下融合算法仿真結果，如圖7～10所示。圖7～10中：黃色區域表示正向遍歷節點；青色區域表示反向遍歷節點；紅色線段表示規劃的路徑；黑色區域表示障礙物；紫色區域表示臨時障礙物。

融合算法實驗結果對比，如表3所示。

由表2，3可知：距離引導式A^*算法和文中融合算法在路徑長度、遍歷節點數目和運行時間方面均有提升，能更好地完成路徑規劃任務。

5 結束語

路徑規劃技術在國家的無人系統領域具有重要應用價值，提升路徑規劃算法的性能對推動智能領域的發展具有關鍵意義。在全局路徑規劃中，采用雙向搜索策略并提出一種綜合距離啟發函數，顯著減少運行時間，利用全局路徑篩選策略對初始路徑進行優化，減少了拐點數和路徑長度。在全局路徑最優的基礎上，采用動態窗口法進行局部路徑規劃，實現動態避障，并避免陷入局部最優解的情況。由實驗結果可知，文中融合算法在路徑長度、運行時間方面均有明顯提升，能夠更好地滿足移動機器人對于路徑規劃的要求。

參考文獻：

［1］ DIJKSTRA E W.A note on two problems inconnexion with graphs［J］.Numerische Mathematik，1959，1（1）：269-271.DOI：10.1007/BF01386390.

［2］ STENTZ A.Optimal and efficient path planning for partially known environments［C］∥IEEE International Conference on Roboticsand Automation.Piscataway：IEEE Press，2002：3310-3317.DOI：10.1109/ROBOT.1994.351061.

［3］ LAVALLE S M.Rapidly-exploring random trees： A new tool for path planning［J］.Research Report，1998（7）：3632-3648.

［4］ HART P E，NILSSON N J，RAPHAEL B.A formal basis for the heuristic determination of minimum cost path［J］.IEEE Transactions on Systems Science and Cybernetics，1968，4（2）：100-107.DOI：10.1109/tssc.1968.300136.

［5］ 趙曉，王錚，黃程侃，等.基于改進A^*算法的移動機器人路徑規劃［J］.機器人，2018，40（6）：903-910.DOI：10.13973/j.cnki.robot.170591.

［6］ WANG Xingdong，ZHANG Haowei，LIU Shuo.et al.Path planning of scenic spots based on improved A^* algorithm［J］.Scientific Reports，2022，12（1）：1-7.DOI：10.1038/s41598-022-05386-6.

［7］ 田海波，李陸軍，暢科劍，等.用于無人車路徑規劃的改進A^*算法［J］.現代制造工程，2021（11）：63-68，92.DOI：10.16731/j.cnki.1671-3133.2021.11.009.

［8］ 李炯逸，李強，張新聞，等.移動機器人用改進的雙向A^*二次路徑規劃算法［J/OL］.系統仿真學報：1-11（2023-12-19）［2024-03-15］.https：∥www.cnki.com.cn/Article/CJFDTotal-XTFZ20231215007.htm.

［9］ 辛煜，梁華為，杜明博，等.一種可搜索無限個鄰域的改進A^*算法［J］.機器人，2014，36（5）：627-633.DOI：10.13973/j.cnki.robot.2014.0627.

［10］ 王中玉，曾國輝，黃勃，等.改進A^*算法的機器人全局最優路徑規劃［J］.計算機應用，2019，39（9）：2517-2522.DOI：10.11772/j.issn.1001-9081.2019020284.

［11］ 姬鵬，張新元，高帥軒，等.融合改進A^*算法與動態窗口法的路徑規劃研究［J/OL］.系統仿真學報：1-11（2023-07-09）［2024-03-15］.https：∥doi.org/10.16182/j.issn1004731x.joss.23-0619.

［12］ 程傳奇，郝向陽，李建勝，等.融合改進A^*算法和動態窗口法的全局動態路徑規劃［J］.西安交通大學學報，2017，51（11）：137-143.DOI：10.7652/xjtuxb201711019.

［13］ 唐嘉寧，彭志祥，李孟霜，等.基于改進A^*算法的無人機路徑規劃研究［J］.電子測量技術，2023，46（8）：99-104.DOI： 10.19651/j.cnki.emt.2211107.

［14］ 張旭，程傳奇，郝向陽，等．一種兼顧全局與局部特性的機器人動態路徑規劃算法［J］．測繪科學技術學報，2018，35（3）：315-320.DOI： 10.3969/j.issn.1673-6338.2018.03.018.

［15］ 劉鈺銘，黃海松，范青松，等.基于改進A^*-DWA算法的移動機器人路徑規劃［J］.計算機集成制造系統，2024，30（1）：158-171.DOI： 10.13196/j.cims.2022.0634.

［16］ 鄒文，韓丙辰，李鵬飛，等.融合改進A^*算法和優化動態窗口法的路徑規劃［J］.計算機集成制造系統，2024，30（1）：184-195.DOI： 10.13196/j.cims.2021.0498.

（責任編輯：錢筠 英文審校：陳婧）