基于動態啟發的雙向搜索A*路徑規劃算法

陳家偉 許穎婷 陳淑婧 張霖 蔡志明

DOI：10.19850/j.cnki.2096-4706.2024.02.019

收稿日期：2023-06-12

基金項目：福建工程學院校科研啟動基金（GY-Z21064）

摘? 要：針對傳統路徑規劃A*算法搜索速度慢，且所得路徑轉角過大的問題，提出了一種基于雙向搜索的A*路徑規劃算法。首先調整A*搜索算法的整體搜索方向為雙向搜索，初步提升搜索速度；其次，在規劃過程中引入動態權重系數來調節啟發式函數，通過平衡路徑長短與規劃速度之間的關系，進一步提高搜索速度；最后采用B樣條曲線對所規劃的路徑進行平滑優化，解決A*算法規劃路徑轉角過多無法滿足移動機器人實際運動控制的問題。仿真實驗結果表明，相比傳統的A*算法，該算法在搜索節點和規劃時間方面分別平均減少79.24%和62.56%。

關鍵詞：移動機器人；A*路徑規劃；雙向搜索；動態權重系數；B樣條曲線優化

中圖分類號：TP242.6? 文獻標識碼：A? 文章編號：2096-4706（2024）02-0086-06

A Bidirectional Search A* Path Planning Algorithm Based on Dynamic Heuristic Method

CHEN Jiawei1， XU Yingting1， CHEN Shujing1， ZHANG Lin1， CAI Zhiming1，2

（1.School of Electronic， Electrical Engineering and Physics， Fujian University of Technology， Fuzhou? 350118， China;

2.National Demonstration Center for Experimental Electronic Information and Electrical Technology Education， Fujian University of Technology， Fuzhou? 350118， China）

Abstract： Aiming at the problem that the traditional path planning A* algorithm has slow search speed and the resulting path turning angle too large， an A* path planning algorithm based on bidirectional search is proposed. Firstly， the overall search direction of A* search algorithm is adjusted to bidirectional search to initially improve the search speed. Secondly， dynamic weight coefficients are introduced in the planning process to adjust the heuristic function to further improve the search speed by balancing the relationship between path length and planning speed. Finally， B-spline curve is used to smooth and optimize the planned paths to solve the problem that the A* algorithm planning paths with too many turning angles cannot meet the actual motion control. The results of simulation experiments show that compared with the traditional A* algorithm， the algorithm in this paper reduces the search nodes and planning time by 79.24% and 62.56%， respectively.

Keywords： mobile robot; A* path planning; bidirectional search; dynamic weight coefficient; B-spline curve optimization

0? 引? 言

自主移動機器人（Autonomous Mobile Robot， AMR）是近年來人工智能研究領域的一個熱點研究方向。移動機器人路徑規劃是AMR的一種關鍵技術，它可以幫助機器人實現自主移動，其基本思想是通過分析機器人所處的環境，計算出最優的路徑，使機器人能夠安全、有效地抵達目標區域順利完成任務[1]。

作為移動機器人研究的核心技術之一移動機器人路徑規劃得到了國內外學者的廣泛關注和深入研究[2]。Peter等人結合了最佳優先搜索（Best First Search， BFS）算法和Dijkstra算法，提出了著名的A*算法[3]。但A*算法也存在一些不足：由于A*算法需要同時維護OPEN List和CLOSED List兩個列表，并對每個展開的節點計算代價值，這往往會導致計算量大，內存占用嚴重且計算時間長的問題。針對上述缺點，趙曉等人使用跳點搜索的方式改進A*算法，省略了A*算法大量的不必要節點，從而減少了計算量[4]。Xiang等人提出了一種改進鄰域搜索的A*算法，并將其與貪心算法結合，有效減少了路徑長度[5]。為了減少搜索節點并提高實時性，楊光永等人提出了一種使用余弦方向因子改進啟發式函數的A*搜索算法[6]。同樣地，張陽偉等人提出了一種變步長雙向搜索A*算法，減少A*算法搜索節點數[7]。

為了解決A*算法在路徑規劃過程中存在的路徑搜索節點數量多、搜索時間長、最終路徑總轉角大的問題，本文引入動態權重系數改進啟發式函數并結合雙向搜索策略對A*算法加以改進。在改進的A*算法生成規劃路徑后使用B樣條曲線對路徑進行平滑處理，使其符合移動機器人運動學模型。

1? 傳統A*搜索算法

A*搜索算法是一種啟發式的圖搜索算法，它同時具有Dijkstra算法和BFS算法的特點，該算法既可以找到最短路徑，又擁有比較較快的搜索速度。A*算法的代價函數如式（1）所示：

（1）

其中G（n）表示從起始節點到節點n的實際代價函數，H（n）表示從節點n到目標節點的啟發式評估代價函數，F（n）表示節點n的總代價函數。啟發式函數采用曼哈頓距離，其公式如式（2）所示：

（2）

在A*搜索算法中，搜索區域被轉換成一個二維數組，柵格的中心被定義為節點n。數組的每個元素都是柵格地圖的一個正方形。二維方格定義為可行區域（OPENLIST）或不可行區域（CLOSE LIST）。F（n）會評估OPEN LIST中的每個節點的代價值并確定節點的順序。OPEN LIST用于記錄已計算代價值但并未展開的節點，CLOSE LIST用于儲存已展開的節點。

本文采用如圖1所示的柵格地圖（Grid Map）進行全局環境地圖模型的構建，對環境中出現的障礙物以機器人尺寸半徑向外膨脹一定距離作為柵格的尺寸進行柵格化，柵格化后不滿一個柵格的障礙物區域也按照一個完整的柵格處理。

圖中黑色柵格代表被障礙物所占據的區域，坐標（1，1）的柵格為設定的機器人運動起始點，坐標（14，14）的柵格為機器人任務目標終點，灰色柵格代表A*算法所遍歷的節點。

A*算法的工作流程如圖2所示，通過循環判斷待檢測節點的總代價值來選擇下一個節點。節點采用八鄰域進行搜索，如圖3所示，其中PARENT表示父節點即當前節點，CHILD表示父節點的子節點即當前節點的鄰節點，圖3中CHILD的坐標是PARENT的相對坐標值。

2? A*搜索算法的改進

2.1? 雙向搜索

傳統的A*算法搜索速度雖然相較于Dijkstra算法已有了較大的提升，但搜索速度仍然較慢，且搜索的節點偏多。因此，本文使用雙向搜索的方式，提升A*算法的搜索速度，減少其遍歷的節點數。雙向搜索的方式會將起點和終點進行重映像：即起點與終點二者互為起點，也互為終點。雙向搜索的過程主要分為前向搜索和反向搜索。在初始化OPEN List時，前向搜索過程將起點加入OPEN List并定義其為PARENT節點，而反向搜索過程是將終點加入OPEN List并定義其為PARENT節點。雙向搜索A*算法實際流程如圖4所示。當前向搜索過程與反向搜索過程交匯與同一節點時，算法結束，生成路徑。

使用雙向搜索A*算法進行路徑規劃的結果如圖5所示。為了方便與傳統A*算法的效果進行對比，圖5使用了與圖1相同的柵格地圖、起點與終點。

圖中黑色柵格代表被障礙物所占據的區域，坐標（1，1）的柵格為設定的機器人運動起始點，坐標（14，14）的柵格為機器人任務目標終點。

將圖1與圖5進行對比可以明顯看出使用雙向搜索A*算法所遍歷的節點數要少于傳統A*搜索算法。

2.2? 動態啟發

本文使用歐幾里得距離作為啟發式函數，一定程度上能夠縮短規劃算法得到的距離。歐幾里得距離公式如式（3）所示：

（3）

由A*算法的總代價函數F（n）可知，當H（n） = 0時，A*算法會演變為Dijkstra算法。當H（n）≤G（n）時，A*算法所搜索的節點相較于Dijkstra算法會減少，搜索速度也會加快，且能找到一條最短路徑。但此時所搜索的節點數仍比較龐大，搜索速度仍然不夠快。當H（n）＞G（n）時，A*的搜索速度會比H（n）≤G（n）時更快，但可能無法找到一條最短路徑。當H（n）?G（n）時，A*算法會演變為BFS算法。

在移動機器人的路徑規劃中往往期望總路徑越短越好，且路徑規劃速度越快越好。因此，本文引入了動態權重系數w（n）來調整啟發函數。由w（n）來控制H（n）與G（n）的比例關系，從而平衡路徑長短與規劃速度之間的關系。引入動態權重系數后，A*總代價函數如式（4）所示：

（4）

其中的動態權重系數w（n）定義如下式（5）所示：

（5）

式中的d（g）表示當前節點到目標點的歐幾里得距離，d（s）表示起點到目標點的歐幾里得距離。當d（g）/d（s）≥0.3時，即當前節點在遠離目標節點的位置時，選用較高的權重，這樣便會使最終的H（n）值大于G（n）。此時，路徑搜索過程更加注重速度。而當d（g）/d（s）＜0.3時，選用1作為權重。此時，路徑規劃過程轉換為傳統A*的搜索過程，即搜索速度與路徑長度兼顧的模式。

圖6（a）是傳統A*算法在64×64地圖下的搜索結果，圖6（b）是雙向搜索A*算法在64×64地圖下的搜索結果，圖6（c）是添加動態權重系數后的雙向搜索A*算法在64×64地圖下的搜索結果。圖7（a）是傳統A*算法在128×128地圖下的搜索結果，圖7（b）是雙向搜索A*算法在128×128地圖下的搜索結果，圖7（c）是添加動態權重系數后的雙向搜索A*算法在128×128地圖下的搜索結果。顯然，添加權重系數后的雙向搜索A*算法所遍歷的節點數大幅下降。

2.3? 基于B樣條曲線的路徑優化

A*算法搜索得到的路徑存在轉角過多，路線不夠平滑的問題。針對A*路徑轉角過多過大的問題，Zhang等人提出了一種結合人工勢場方法的啟發式函數，提高了執行效率并減少了路徑轉折點數量[8]。Ou等人引入了一種基于幾何特征去除冗余節點進行路徑平滑的策略，該方法能夠有效地去除冗余節點保障移動機器人行駛安全[9]。劉鈺銘等人提出了一種基于動態弦定弧過渡的路徑平滑處理方法[10]。然而，更加主流的路徑優化方法是貝塞爾曲線優化[11]。

貝塞爾曲線是適用二維圖形的一種數學曲線，一般是用于一些矢量圖的設計。不過在路徑規劃中，依然可以使用貝塞爾曲線來優化路徑，使其更加平滑[12-14]。二階貝塞爾曲線原理示意圖如圖8所示。

貝塞爾曲線由起點、終點和控制點組成，根據控制點的個數和位置決定了這個曲線的最終樣式。在圖8中，P1、P2、P3為控制點，在線段P1P2中，任意選取一點 ，計算比例 ，根據計算得到的比例在線段P2P3上選取點 ，使得 ，連接 。在線段? 上選取點 ，使得 ，此時的點 即是二階貝塞爾曲線上的點。遍歷線段P1P2與線段P2P3上所有的點后，將得到的所有的點? 連接起來便可以得到二階貝塞爾曲線，如圖8中曲線所示。

但是貝塞爾曲線同樣存在許多缺陷：1）一旦確定了控制點個數，也就確定了貝塞爾曲線的階數。2）貝塞爾曲線的拼接過程比較復雜。3）貝塞爾曲線不能做局部的修改。

基于上述條件，本文采用B樣條曲線（B-spline curve）進行路徑優化。B樣條曲線本質上是一個分段連續多項式。整條曲線是一個完整的表達式，但是其內在的量是分段的，如，某條B樣條曲線是由若干三階曲線拼接而成，但兩兩之間又滿足二次連續的關系[15]。這樣既克服了波動現象，得到的曲線又是低次的，有統一表達式的。

對于n+1個控制點Pi（i = 0，1，2，…，n）和一個節點向量U = {u0，u1，u2，…，um}，依次連接這些控制點可以組成一個特征多邊形。此時K + 1階（K次）B樣條曲線的一般表達式如下式（6）所示：

（6）

其中Ni，k （u）是K次B樣條基函數，其遞歸公式如式（7）所示：

（7）

低階的B樣條曲線平滑度較差，但B樣條曲線的階數越高，其計算復雜度也將隨之升高。高階的曲線優化后的路徑還可能穿越障礙物區域[16]。經實驗，本文使用十階B樣條曲線對算法得到的路徑進行優化，并將其與使用連續二階貝塞爾曲線優化的路徑進行對比，得到如圖9所示結果。圖9（a）是64×64分辨率地圖下連續二階貝塞爾曲線路徑優化結果圖，其中折線是路徑規劃所得路徑，曲線是二階貝塞爾曲線優化后的路徑。圖9（b）是64×64分辨率地圖下十階B樣條曲線路徑優化結果圖，其中折線是路徑規劃所得路徑，曲線是十階B樣條曲線優化后的路徑。通過對比，可以很明顯地看出十階B樣條曲線的路徑更加平緩，更符合機器人實際的運動學模型。

3? 實驗結果與分析

本文所使用的硬件環境為：Intel（R） Core（TM） i7-10870H CPU @ 2.20 GHz；操作系統為Windows 10 22H2；軟件環境為：PyCharm 2021.2.3（Community Edition）；所使用的編程語言為：Python 3.9。

為驗證本文所提出的基于動態啟發雙向搜索的A*路徑規劃算法的有效性，本文設計了16×16、64×64、128×128三種分辨率的柵格地圖作為機器人環境模型的仿真，在給定同一起點和目標點的情況下，重復10次實驗，并對實驗數據進行求平均處理后得到如表1、表2、表3所示實驗數據。

與傳統A*算法和未經改進的雙向搜索A*算法相比，本文所提出的采用動態啟發雙向搜索A*算法在三種不同分辨率地圖下的性能均有所提升，搜索節點個數減少了58.93%～91.50%，規劃時間減少了34.19%～80.89%。如本文2.2節內容所述，在分辨率較大的128×128地圖環境下，本文改進的動態啟發雙向搜索A*算法由于在搜索初期啟發函數的權重較大，無法保證路徑長度的最短。

4? 結? 論

本文針對自主移動機器人使用A*算法搜索路徑時存在的搜索速度慢，生成的路徑轉角過多，安全閾值低的問題，提出了一種基于動態權重優化和B樣條曲線優化的雙向搜索A*算法。通過引入動態權重系數，修改啟發函數值來達到提高路徑規劃速度的目的。同時，使用B樣條曲線對路徑進行平滑優化，使其更符合機器人運動學模型。實驗證明，本文所提出的改進算法大幅減少了路徑規劃時間和路徑轉角，更加符合自主移動機器人的運動學模型，具有較好的實用性。但本文算法在較大分辨率的地圖場景中無法保證路徑的最優，動態啟發函數還有改進空間。進一步優化算法后，有望在找到最短路徑的情況下，大幅改善路徑規劃速度，更適合機器人的實際應用。

參考文獻：

[1] 林韓熙，向丹，歐陽劍，等.移動機器人路徑規劃算法的研究綜述 [J].計算機工程與應用，2021，57（18）：38-48.

[2] 李曉旭，馬興錄，王先鵬.移動機器人路徑規劃算法綜述 [J].計算機測量與控制，2022，30（7）：9-19.

[3] PETER H，N NILS，R BERTRAM. A Formal Basis for the Heuristic Determination of Minimum Cost Paths [J].IEEE Transactions on Systems Science and Cybernetics，1968，4（2）：100-107.

[4] 趙曉，王錚，黃程侃，等.基于改進A*算法的移動機器人路徑規劃 [J].機器人，2018，40（6）：903-910.

[5] XIANG D，H LIN，J OUYANG，et al. Combined improved A* and greedy algorithm for path planning of multi-objective mobile robot [J/OL].Scientific Reports，2022，12（1）：13273（2023-08-02）.https：//link.springer.com/article/10.1038/s41598-022-17684-0.

[6] 楊光永，戈一航，晏婷，等.基于調和A*算法在移動機器人中的研究 [J].現代電子技術，2022，45（4）：171-176.

[7] 張陽偉，喬越，李成鳳.基于四叉樹柵格環境的變步長雙向A*算法 [J].控制工程，2021，28（10）：1960-1966.

[8] ZHANG J，WU J，SHEN X，et al. Autonomous land vehicle path planning algorithm based on improved heuristic function of A-Star [J/OL].International Journal of Advanced Robotic Systems，2021，18（5）：（2021-09-14）.https：//doi.org/10.1177/17298814211042730.

[9] OU Y，Y FAN，X ZHANG，et al. Improved A* Path Planning Method Based on the Grid Map [J/OL].Sensors （Basel），2022，22（16）：6198（2022-08-18）.https：//doi.org/10.3390/s22166198.

[10] 劉鈺銘，黃海松，范青松，等.基于改進A*_DWA算法的移動機器人路徑規劃 [J/OL].計算機集成制造系統：1-20（2022-11-28）.http：//kns.cnki.net/kcms/detail/11.5946.TP.20221125.1957.004.html.

[11] 岳高峰，張萌，沈超，等.移動機器人導航規劃的雙向平滑A-star算法 [J].中國科學：技術科學，2021，51（4）：459-468.

[12] 謝春麗，高勝寒，孫學志.融合改進A*算法和貝塞爾曲線優化的路徑規劃算法 [J].重慶理工大學學報：自然科學，2022，36（7）：177-187.

[13] 李二超，齊款款.貝塞爾曲線融合雙向蟻群算法的移動機器人路徑規劃 [J].陜西師范大學學報：自然科學版，2022，50（3）：79-88.

[14] 方文凱，廖志高.基于改進A*算法的移動機器人全局路徑優化研究 [J].廣西科技大學學報，2023，34（1）：73-78+84.

[15] 朱平，汪國昭. 變次數B-樣條曲線 [J].浙江大學學報：工學版，2009，43（5）：789-795.

[16] 張躍明，薛奇，紀姝婷.滿足曲率約束的B樣條曲線連續路徑平滑方法 [J].華中科技大學學報：自然科學版，2022，50（5）：59-65+72.

作者簡介：陳家偉（1997—），男，漢族，浙江寧波人，碩士研究生在讀，研究方向：室內移動機器人定位與導航；許穎婷（2000—），女，漢族，福建漳州人，碩士研究生在讀，研究方向：室內移動機器人定位與導航；陳淑婧（1998—），女，漢族，福建漳州人，碩士研究生在讀，研究方向：室內移動機器人定位與導航；張霖（1979—），男，漢族，福建泉州人，講師，博士，研究方向：模擬/混合信號IC設計、機器識別；通訊作者：蔡志明（1977—），男，漢族，福建漳州人，教授，博士，研究方向：室內移動機器人定位與導航。