基于改進(jìn)A*算法的路徑規(guī)劃研究

摘" 要：研究了A*算法在二、三維模型路徑規(guī)劃中的優(yōu)化方法。通過實(shí)時(shí)閾值法和懲罰因子法減少開放列表中不必要的搜索空間和冗余路徑；采用自定義優(yōu)先級(jí)隊(duì)列、二叉堆法和哈希表替代傳統(tǒng)A*算法中的處理方式；在對(duì)二維地圖的研究中，采用局部A*算法避免大面積搜索。實(shí)驗(yàn)結(jié)果表明，經(jīng)過改進(jìn)的A*算法顯著提高了搜索和路徑規(guī)劃速度，減少了計(jì)算時(shí)間和內(nèi)存消耗，驗(yàn)證了該算法的可行性和有效性。

關(guān)鍵詞：路徑規(guī)劃；三維規(guī)劃；懲罰因子；二叉堆與自定義優(yōu)先級(jí)隊(duì)列；實(shí)時(shí)閾值；局部A*算法

中圖分類號(hào)：TP18；TP242 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：2096-4706（2024）10-0051-06

Research on Path Planning Based on Improved A* Algorithm

CAI Zifeng， ZHANG Yansheng， LIANG Xianzhang， LUO Shihao

（Zhuhai College of Science and Technology， Zhuhai" 519040， China）

Abstract： It studies the optimization method of A* algorithm in path planning of 2D and 2D models. Reduce unnecessary search space and redundant paths in open lists through real-time threshold method and penalty factor method. Adopting custom priority queues， binary heap methods， and hash tables to replace the processing methods in traditional A* algorithms. In the study of 2D maps， local A* algorithm is used to avoid large-scale searches. The experimental results show that the improved A* algorithm significantly improves the speed of search and path planning， reduces computational time and memory consumption， and verifies the feasibility and effectiveness of the algorithm.

Keywords： path planning; 3D planning; penalty factor; binary heap and custom priority queue; real-time threshold; local A* algorithm

0" 引" 言

路徑規(guī)劃是移動(dòng)機(jī)器人實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵技術(shù)之一。路徑規(guī)劃是指在有障礙物的環(huán)境中，按照一定的評(píng)價(jià)標(biāo)準(zhǔn)（如距離、時(shí)間、代價(jià)等）尋找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的無碰撞路徑[1-4]。經(jīng)實(shí)驗(yàn)驗(yàn)證發(fā)現(xiàn)，傳統(tǒng)的A*算法[5，6]在二維和三維模型中生成的路徑存在多個(gè)拐點(diǎn)問題，路徑規(guī)劃所需時(shí)間較長(zhǎng)，對(duì)移動(dòng)機(jī)器人的性能和安全性產(chǎn)生負(fù)面影響。復(fù)雜的路徑規(guī)劃可能導(dǎo)致機(jī)器人在應(yīng)對(duì)環(huán)境變化時(shí)響應(yīng)速度較慢，增加過多的轉(zhuǎn)彎操作可能帶來誤差并加劇機(jī)器人硬件的磨損。此外，過多的拐角可能造成視野盲點(diǎn)，增加潛在風(fēng)險(xiǎn)。

針對(duì)這些規(guī)劃時(shí)間過長(zhǎng)、效率低下等問題，常用的路徑規(guī)劃算法人工勢(shì)場(chǎng)法、模糊邏輯算法、自由空間法、遺傳算法等[7，8]在解決路徑規(guī)劃問題方面表現(xiàn)出優(yōu)勢(shì)，但計(jì)算效率仍有其他的較好的優(yōu)化方式。為了改善算法的計(jì)算效率和路徑優(yōu)化問題，本文采用柵格法構(gòu)建環(huán)境模型，從兩個(gè)方面來優(yōu)化A*算法：從性能優(yōu)化的角度，采用實(shí)時(shí)閾值法、二叉堆與自定義優(yōu)先級(jí)隊(duì)列優(yōu)化，引入哈希表、局部A*算法；從路徑優(yōu)化的角度，通過設(shè)計(jì)懲罰因子來減少路徑拐點(diǎn)數(shù)，算法會(huì)更偏向于走直線路段，進(jìn)一步優(yōu)化生成的路徑。

1" 模型建立

1.1" 二維模型建立

在二維地圖模型的研究中，構(gòu)建一個(gè)n行m列的柵格地圖。其中，空白柵格表示可行走區(qū)域，而黑色柵格代表障礙物，即無法穿越的區(qū)域。研究?jī)H考慮機(jī)器人在上下左右4個(gè)正方向上的移動(dòng)，不涉及對(duì)角線或其他方向的移動(dòng)。圖1展示了一個(gè)7×7大小的柵格地圖，起點(diǎn)為S（0，0），終點(diǎn)為G（4，4）。

1.2" 三維模型

如圖2所示，構(gòu)建了一個(gè)大小為5×5×5的三維空間，其中黑色區(qū)塊表示障礙物的位置，起點(diǎn)（0，0，0）終點(diǎn)（4，4，4）。

2" A*算法的性能優(yōu)化

2.1" 實(shí)時(shí)閾值

在圖1中，使用曼哈頓距離作為啟發(fā)式函數(shù)，代表從當(dāng)前位置到目標(biāo)點(diǎn)水平和垂直移動(dòng)所需的步數(shù)。最大曼哈頓距離為14步（例如從S到G），設(shè)置啟發(fā)式值的閾值為14，在A*搜索中超過閾值的格子將被濾除，濾除的節(jié)點(diǎn)將不會(huì)參與下一步的計(jì)算中。曼哈頓距離公式為：

再例如，假設(shè)機(jī)器人已經(jīng)移動(dòng)到點(diǎn)（2，3），此時(shí)的曼哈頓距離為d1 = 7。因此，實(shí)時(shí)閾值將設(shè)為7。在算法推進(jìn)中，計(jì)算出（3，1）和（2，2）兩點(diǎn)的曼哈頓距離均大于閾值，那么直接忽略這兩個(gè)節(jié)點(diǎn)，如表1所示。

在二維模型中，當(dāng)啟發(fā)式值大于等于某一閾值時(shí)，節(jié)點(diǎn)將從開放列表中濾除，以節(jié)省計(jì)算資源。在三維模型中，采用三維模型下的曼哈頓距離式（2）計(jì)算周圍點(diǎn)的值，即可達(dá)到相同的效果。這種策略特別適用于啟發(fā)式函數(shù)能夠快速確定無前景路徑的情況。

2.2" 自定義優(yōu)先級(jí)隊(duì)列與二叉堆優(yōu)化

對(duì)于傳統(tǒng)的A*算法，隨著算法推進(jìn)，需要管理大量節(jié)點(diǎn)數(shù)據(jù)，導(dǎo)致時(shí)間復(fù)雜度增加，計(jì)算效率降低。為了優(yōu)化這一問題，可以通過建立一個(gè)自定義的優(yōu)先級(jí)隊(duì)列PriorityQueue來改進(jìn)傳統(tǒng)的A*算法。這個(gè)自定義的優(yōu)先隊(duì)列基于Python中的堆實(shí)現(xiàn)，同時(shí)利用哈希表來快速進(jìn)行元素查找操作，避免了對(duì)整個(gè)列表的遍歷來查找特定元素。通過表2來呈現(xiàn)優(yōu)化前后A*算法的對(duì)比。

2.2.1" 二叉堆優(yōu)化

二叉堆被視為一種特殊的堆數(shù)據(jù)結(jié)構(gòu)，其結(jié)構(gòu)類似于完全二叉樹。其滿足堆的重要特性：父節(jié)點(diǎn)的值始終小于或等于（或者大于或等于）任何一個(gè)子節(jié)點(diǎn)的值。

如圖3所示[9]，在給定的二叉堆中，假設(shè)節(jié)點(diǎn)的值表示了A*算法中的F值。最小的F值節(jié)點(diǎn)10位于堆的頂部，其次是20作為其子節(jié)點(diǎn)。第三小的節(jié)點(diǎn)是24，距離堆頂有兩步的距離。盡管它小于30，但30位于第二層左側(cè)，距離堆頂只有一步距離。實(shí)際上，僅需滿足每個(gè)單獨(dú)子節(jié)點(diǎn)的值都比其父節(jié)點(diǎn)大或相等的條件即可，其他節(jié)點(diǎn)在堆中的位置并不影響這一性質(zhì)。這種特性使得二叉堆成為一種高效的數(shù)據(jù)結(jié)構(gòu)，能夠在A*算法中快速找到最小的節(jié)點(diǎn)。

2.2.2" 哈希表

當(dāng)引入哈希表用于A*算法時(shí)，其中節(jié)點(diǎn)的狀態(tài)作為鍵，與該節(jié)點(diǎn)相關(guān)的信息（如代價(jià)、路徑等）作為值。

哈希函數(shù)將節(jié)點(diǎn)的狀態(tài)映射到存儲(chǔ)桶的位置，其中NodeState是節(jié)點(diǎn)狀態(tài)的集合，Indices是存儲(chǔ)桶的索引集合[10]。通常可以表示為：

存儲(chǔ)桶仍然代表哈希表的存儲(chǔ)位置，每個(gè)存儲(chǔ)桶包含一個(gè)鏈表，其中存儲(chǔ)的是與特定狀態(tài)相關(guān)的信息，如代價(jià)和路徑：

當(dāng)需要查找節(jié)點(diǎn)時(shí)，通過哈希函數(shù)找到節(jié)點(diǎn)狀態(tài)對(duì)應(yīng)的存儲(chǔ)桶位置i，在存儲(chǔ)桶的鏈表中搜索節(jié)點(diǎn)信息。同理，當(dāng)需要將新的節(jié)點(diǎn)信息插入哈希表時(shí)，通過哈希函數(shù)找到存儲(chǔ)桶的位置i。

搜索操作：

插入操作：

2.3" 局部A*算法

針對(duì)大規(guī)模二維地圖的問題，無論是經(jīng)過優(yōu)化的A*算法還是傳統(tǒng)A*算法都需要處理大量候選節(jié)點(diǎn)，從而導(dǎo)致搜索的時(shí)間復(fù)雜度增加。因此，提出了一種針對(duì)大型二維地圖的局部A*算法思路。該方法的主要目標(biāo)是根據(jù)當(dāng)前節(jié)點(diǎn)的位置為中心（xs，ys），裁剪出一塊可自定義邊長(zhǎng)rs的區(qū)域作為子地圖（x s- rs，ys - rs） （xs + rs，ys + rs），如圖4所示，深灰色區(qū)域?yàn)镈1點(diǎn)的子地圖。這樣算法不再需要處理大量的節(jié)點(diǎn)，只需對(duì)當(dāng)前所在子地圖進(jìn)行計(jì)算即可。同時(shí)，啟發(fā)式函數(shù)被修改為當(dāng)前節(jié)點(diǎn)到最終目標(biāo)點(diǎn)的曼哈頓距離，以更好地適應(yīng)局部搜索的特點(diǎn)。

將子地圖定義為（left，right，top，bottom），子地圖劃分計(jì)算：

確保了子地圖邊界不會(huì)超出地圖的合法范圍。

為了展示該方法的有效性，構(gòu)建一個(gè)50×50的二維地圖，并設(shè)置起點(diǎn)為（0，0），終點(diǎn)為（49，49）。根據(jù)當(dāng)前點(diǎn)D1的位置，重新修改起始坐標(biāo)并進(jìn)行應(yīng)用A*算法得到D1到D2點(diǎn)為路徑，如圖5所示。

當(dāng)?shù)竭_(dá)子地圖邊界時(shí)，當(dāng)前節(jié)點(diǎn)D2坐標(biāo)以其為中心劃出新的子地圖，如圖6所示。當(dāng)劃出的區(qū)域超出大地圖時(shí)，會(huì)做出越界處理靈活改變子地圖的大小，如圖7所示，黑色框?yàn)殪`活變化后的實(shí)際子地圖。

如圖8所示，將所有小地圖的路徑拼接在一起得到最終的路徑。局部的A*算法有效地減少了對(duì)大量節(jié)點(diǎn)的管理和搜索，從而降低了計(jì)算復(fù)雜度，提高了搜索效率，并優(yōu)化了資源利用。

3nbsp; A*算法的路徑優(yōu)化

在復(fù)雜環(huán)境中，由于路徑中可能存在大量拐點(diǎn)，導(dǎo)致算法效率降低。圖8、圖9（a）和圖10（a）展示了二維模型和三維模型中傳統(tǒng)算法輸出路徑，其拐點(diǎn)數(shù)量顯著。

3.1" 二維模型

為解決該問題，引入節(jié)點(diǎn)懲罰因子成為改進(jìn)A*算法路徑選擇的一項(xiàng)策略。通過添加拐點(diǎn)懲罰因子，算法更傾向于選擇直線路徑而非頻繁轉(zhuǎn)向的路徑，從而減少拐彎次數(shù)，提高路徑規(guī)劃效率。

引入父節(jié)點(diǎn)P，即當(dāng)前節(jié)點(diǎn)的前一個(gè)節(jié)點(diǎn)，假設(shè)父節(jié)點(diǎn)P為（xp，yp），當(dāng)前節(jié)點(diǎn)C為（xc，yc）。

3.1.1" 計(jì)算轉(zhuǎn)向角度

1）計(jì)算當(dāng)前節(jié)點(diǎn)C到父節(jié)點(diǎn)P的向量和到相鄰節(jié)點(diǎn)N的向量：

2）計(jì)算向量的點(diǎn)積d和長(zhǎng)度l：

3）計(jì)算余弦值：

4）進(jìn)行范圍修正：

如果余弦值cosθ＞1.0，則設(shè)置cosθ = 1.0。

如果余弦值cosθ＜-1.0，則設(shè)置cosθ = -1.0。

5）計(jì)算轉(zhuǎn)向角度：

3.1.2" 計(jì)算轉(zhuǎn)向懲罰因子

由上一節(jié)得到轉(zhuǎn)向角度θ，假設(shè)轉(zhuǎn)向懲罰因子為K（θ）。假設(shè)θ的取值范圍是從0到360度（0到2π弧度），K（θ）的取值范圍是從0到1。

其中，m和b是線性函數(shù)的斜率和截距，用于將轉(zhuǎn)向角度映射到轉(zhuǎn)向懲罰因子的范圍內(nèi)。為了滿足條件：

可以確定m和b的值。得到以下兩個(gè)方程：

因此得到b = 0；k = 1/360；最后得到的線性函數(shù)可以表示為：

最后融入f （n）式子中為：

3.2" 三維模型

在三維模型中，可以采用相同的方法計(jì)算轉(zhuǎn)向角度和轉(zhuǎn)向懲罰因子，確保計(jì)算的角度適用于三維空間，并將角度值代入相同的線性函數(shù)中計(jì)算轉(zhuǎn)向懲罰因子。這種轉(zhuǎn)向懲罰因子的計(jì)算仍然遵循相同的線性函數(shù)，只需在三維空間中應(yīng)用?？梢詫⑷S模型分解為z-x和x-y或其他兩個(gè)二維模型組合，通過計(jì)算轉(zhuǎn)向角度和轉(zhuǎn)向懲罰因子的方法得到K1（θ）與K2（θ），最后融入三維模型中的f （n）中：

4" 實(shí)驗(yàn)仿真

為驗(yàn)證改進(jìn)的A*算法的有效性，在二維和三維模型下分別針對(duì)A*算法和融合了實(shí)時(shí)閾值、二叉堆與優(yōu)先級(jí)隊(duì)列、懲罰因子的改進(jìn)A*算法進(jìn)行仿真實(shí)驗(yàn)。實(shí)驗(yàn)將在5個(gè)不同尺寸的柵格地圖中展開，以全面評(píng)估算法在不同場(chǎng)景下的性能表現(xiàn)和適用性。

4.1" 優(yōu)化路徑實(shí)驗(yàn)仿真

圖9（a）所示的7×7柵格地圖下進(jìn)行了A*算法的仿真實(shí)驗(yàn)。其中，S代表起始節(jié)點(diǎn)，G代表目標(biāo)節(jié)點(diǎn)，黑色柵格表示障礙物，而藍(lán)色線段展示了經(jīng)過A*算法生成的最終路徑。在實(shí)驗(yàn)中，通過引入懲罰因子，進(jìn)一步優(yōu)化了路徑的生成，如圖9（b）所示。此外，在50×50的大型柵格地圖中同理，如圖9（c）所示，相比之前的實(shí)驗(yàn)結(jié)果（圖8），得到了路徑規(guī)劃方面的優(yōu)化效果。

圖10（a）表示在圖2所建立的三維柵格地圖中進(jìn)行A*算法的仿真實(shí)驗(yàn)，通過引入三維下的懲罰因子，得到優(yōu)化的三維路徑，如圖10（b）所示。

4.2" 優(yōu)化性能實(shí)驗(yàn)仿真

建立一個(gè)20×20的二維柵格地圖，起點(diǎn)S（0，0）終點(diǎn)G（19，19），將障礙物的數(shù)量設(shè)置為自變量，隨機(jī)放置在地圖中并每個(gè)數(shù)量進(jìn)行2 000次實(shí)驗(yàn)記錄下處理的時(shí)間，并且進(jìn)行死路處理，得到如圖11所示的結(jié)果。

其次，5×5×5的三維模型地圖，起點(diǎn)為S（0，0，0），終點(diǎn)為G（4，4，4），并在地圖中隨機(jī)放置不同數(shù)量的障礙物。進(jìn)行10次實(shí)驗(yàn)并記錄，計(jì)算出處理時(shí)間的平均值。表3展示了實(shí)驗(yàn)結(jié)果。

通過實(shí)驗(yàn)驗(yàn)證，在生成相同路徑的基礎(chǔ)上，優(yōu)化后的自定義優(yōu)先隊(duì)列的A*算法明顯表現(xiàn)出更高的效率和更短的處理時(shí)間，甚至在二維模型中平均優(yōu)化率達(dá)到225.98%。這種差距對(duì)于實(shí)際應(yīng)用中的實(shí)時(shí)性和效率至關(guān)重要，特別是在需要快速求解的實(shí)時(shí)路徑規(guī)劃和導(dǎo)航等場(chǎng)景中。因此，在大規(guī)?；驈?fù)雜問題的求解中，選擇本文提出的優(yōu)化方法能夠顯著提升算法的執(zhí)行效率和性能。

5" 結(jié)" 論

A*算法在尋路過程中存在內(nèi)存開銷大、計(jì)算時(shí)間長(zhǎng)等缺點(diǎn)，不能滿足移動(dòng)機(jī)器人在較大場(chǎng)景路徑規(guī)劃中的實(shí)時(shí)性要求。為了提高路徑規(guī)劃的速度，本文分別從性能和路徑優(yōu)化上進(jìn)行討論，性能上通過實(shí)時(shí)閾值、二叉堆優(yōu)先級(jí)隊(duì)列和局部A*算法，提高了尋路效率。路徑優(yōu)化上，通過引進(jìn)懲罰因子減少拐點(diǎn)數(shù)量，從而使。通過在不同規(guī)格柵格環(huán)境下的仿真實(shí)驗(yàn)證明改進(jìn)A*算法是可行的、有效的，特別是在較大二維場(chǎng)景和三維模型下效果更加明顯。

參考文獻(xiàn)：

[1] 趙曉，王錚，黃程侃，等.基于改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃 [J].機(jī)器人，2018，40（6）：903-910.

[2] 曲道奎，杜振軍，徐殿國，等.移動(dòng)機(jī)器人路徑規(guī)劃方法研究 [J].機(jī)器人，2008（2）：97-101+106.

[3] 王殿君.基于改進(jìn)A*算法的室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃 [J].清華大學(xué)學(xué)報(bào)：自然科學(xué)版，2012，52（8）：1085-1089.

[4] JEDDISARAVI K，ALITAPPEH R J，PIMENTA L C A，et al. Multi-Objective Approach for Robot Motion Planning in Search Tasks [J].Applied Intelligence，2016，45（2）：305-321.

[5] 周偉，潘金寶，王林琳.基于改進(jìn)鯨魚算法和A*算法的地面放線機(jī)器人路徑規(guī)劃 [J].現(xiàn)代制造工程，2023（12）：68-75+83.

[6] 王洪斌，尹鵬衡，鄭維，等.基于改進(jìn)的A*算法與動(dòng)態(tài)窗口法的移動(dòng)機(jī)器人路徑規(guī)劃 [J].機(jī)器人，2020，42（3）：346-353.

[7] 劉建華，楊建國，劉華平，等.基于勢(shì)場(chǎng)蟻群算法的移動(dòng)機(jī)器人全局路徑規(guī)劃方法 [J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，2015，46（9）：18-27.

[8] 朱大奇，顏明重.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)綜述 [J].控制與決策，2010，25（7）：961-967.

[9] 周小鏡.基于改進(jìn)A*算法的游戲地圖尋徑的研究 [D].重慶：西南大學(xué)，2011.

[10] 安彥哲，朱妤晴，王建民.物聯(lián)網(wǎng)大數(shù)據(jù)場(chǎng)景下的分布式哈希表適用條件分析 [J].計(jì)算機(jī)學(xué)報(bào)，2021，44（8）：1679-1695.

作者簡(jiǎn)介：蔡梓豐（2001—），男，漢族，廣東佛山人，本科在讀，研究方向：移動(dòng)機(jī)器人路徑規(guī)劃。