基于節點優化的改進全局路徑規劃A＊算法

衛 彥，晉 芳，董凱鋒，宋俊磊，莫文琴，惠亞娟

（1.中國地質大學（武漢）自動化學院，武漢 430074；2.復雜系統先進控制與智能自動化湖北省重點實驗室，武漢 430074；3.地球探測智能化技術教育部工程研究中心，武漢 430074）

0 引言

隨著機器人技術的發展，機器人的功能從最開始的只能在固定位置做重復循環工作發展到如今可以根據指令在室內室外進行移動作業，并且能夠解決突發問題，移動機器人的能夠實現的功能越來越豐富，移動機器人應用的領域越來越多，在很多行業已經使用移動機器人來代替人工作業。除了簡單的搬運、運輸工作，移動機器人還可以代替人類去做一些危險的工作，例如高空作業、危險探測、野外勘探等。隨著新冠疫情的爆發，人們再一次認識了移動機器人的重要性。疫情期間的很多物資運輸采用移動機器人就可以減少感染風險。

比起以前單純靠人力和物力進行的工作，移動機器人的工作效率和工作準確率都更高，因此在在全球的經濟貿易交流越來越頻繁的時代，很多傳統的依靠人工的方法逐漸被淘汰，掌握機器人的核心技術就相當于抓住了經濟的命脈。室內移動機器人是我們在生活中能夠經常接觸到的移動機器人。目前一些超市、餐廳已經大規模投入使用移動機器人完成送餐工作；掃地機器人也是目前很多人會購買的家用室內機器人。在2022北京冬奧會期間，投入使用了很多室內移動機器人。例如食堂采用了送餐機器人給運動員和工作人員們送餐；提供給媒體居住的酒店里應用了室內移動機器人消毒技術；各大場館還有防疫用的巡控移動機器人。

室內移動機器人要完成任務，要解決的重要問題之一就是怎么去目標地點，也就是路徑規劃問題。路徑規劃是移動機器人工作的重要保障之一，是移動機器人完成導航和其他任務的前提［4］。路徑規劃是指按照一定的標準，根據起點和終點規劃出一條規避障礙物的路徑。路徑規劃的主要內容為：預設地圖環境模型，輸入起點坐標和終點坐標，路徑規劃的輸出則是基于已設定的地圖模型得到的連接起點坐標和終點坐標，并且不與地圖模型中的障礙物產生碰撞的最優有效的序列點，最終把序列點連接將變成機器人在實際環境中的運動軌跡。路徑規劃算法分為全局路徑規劃算法和局部路徑規劃算法，常用的全局路徑規劃算法有A＊算法、蟻群算法、遺傳算法和粒子群算法，常用的局部路徑規劃算法有人工勢場法、速度障礙法和動態窗口算法等［1］。

在路徑規劃算法中，A＊算法作為一種啟發式算法，有著廣泛的應用領域［3］。但是A＊算法存在著路徑不平滑、拐點多、路徑非優等問題［2］。針對這些不足，學者們研究出了很多改進型A＊算法：文獻［3］通過改進評價函數的計算方式和權重比例，減少生成路徑中的拐點，使生成的路徑更平滑。文獻［4］在改進啟發函數的基礎上對生成的路徑做五次多項式平滑處理，減少了A＊算法的搜索時間，縮短了路徑長度。文獻［5］通過將3×3的搜索鄰域擴展成7×7，減小了路徑轉折角度。文獻［6］在障礙物膨脹處理的基礎上使用射線法簡化冗余點，減少了50%以上的路徑轉折點。文獻［7］和文獻［1］通過對A＊算法生成的路徑進行節點優化，除去冗余點，縮短了路徑長度。上述方法都減少了A＊算法生成路徑的轉折點，減短了路徑長度，但是上述文獻中去除冗余點的方法在某些情況例如貼近障礙物的路徑比較多的情況下的路徑優化效果不夠明顯。對此，本文在擴大搜索鄰域基礎上引入一種隨機數去除冗余點的方法，使得路徑長度更短，訪問的節點個數更少，運行時長更短，從而使得在貼近障礙物的路徑比較多的情況下的路徑優化效果更好。

1 A＊算法原理

A＊算法是啟發式算法中常用的一種算法，通常在柵格地圖中進行路徑規劃，對于當前節點周圍的點使用評價函數對其進行評估，選擇評價函數最小的點作為擴展節點，搜索到終點就停止搜索，最后將所有選擇的點在柵格地圖上連接起來，從而得到一條規避障礙物的完整路徑。A＊算法路徑規劃可以分為以下兩個部分進行：環境建模和路徑規劃。

1.1 環境建模

A＊算法在路徑規劃前需要做環境建模，既生成有障礙物信息的柵格地圖，如圖1所示，白色區域為可通行區域，黑色區域為障礙物區域。柵格地圖通常以矩陣或者圖片的方式保存。對于圖片格式的柵格地圖，通常讀取圖片的像素獲得一個像素值矩陣即可使用。

圖1 柵格地圖

完成環境建模后，需定義算法在該柵格地圖中的搜索方式。常用A＊算法中，移動機器人通常在柵格地圖上采用3×3鄰域搜索，有8個運動方向，即東、南、西、北、東南、東北、西南、西北，如圖2所示。將以當前節點為中心的3×3鄰域中的節點作為備選節點，經過比較后選擇擴展的節點。因此在這種3×3鄰域搜索中，路徑是由很多段長度為1或者1.4個單位長度的路徑組成的。

圖2 移動機器人的8個運動方向

1.2 A＊算法路徑規劃

接著在建立好的柵格地圖上實現路徑規劃。A＊算法路徑規劃有兩個列表，OPEN 列表和CLOSE 列表。A＊算法路徑規劃的基本思想是，從起點開始，將周圍3×3鄰域中的八個擴展點加入到OPEN 列表，選擇OPEN 列表中評價函數最小的點作為下一個節點，并將選擇的節點記錄到CLOSE 列表，直到搜索到終點為止。A＊算法的評價函數為：

其中：f（n）是當前點的評價函數，g（n）是起點到當前點的實際代價即實際路徑長度，h（n）是當前點到終點的最小估計代價，通常采用當前點和終點的歐氏距離表示，即：

其中：（x＿n，y＿n）是當前節點的坐標，（x＿t，y＿t）是終點的坐標。

A＊算法的路徑規劃實現流程如圖3 所示，具體步驟如下：

圖3 A＊算法路徑規劃實現流程

1）創建一個OPEN 列表和一個CLOSE 列表，將起點加入到OPEN 列表。

2）將除了CLOSE 列表中的節點和障礙物以外當前節點的3×3搜索鄰域中的節點加入到OPEN 列表。

3）對OPEN 列表中的節點進行評估，選出評估函數值f最小的節點n作為擴展節點。

4）將已訪問的節點n放入CLOSE 列表，并判定n是否是終點，如果n不是終點，就回到第二步，如果n是終點，就結束此次路徑規劃。

2 改進A＊算法

A＊算法路徑列表中存在冗余點，生成的路徑不是最優路徑，且轉折點較多，轉折角度較大，路徑不夠平滑，這些問題會導致A＊算法在應用在移動機器人運動控制時，存在多余拐彎，加大控制難度的問題。為了優化此問題，本文提出一種在搜索鄰域擴大到5×5的基礎上隨機數選擇節點去除冗余點的改進A＊算法。

2.1 冗余點去除方法

目前常用的去除冗余點方法有兩種：方法A 和方法B。方法A［15］就是先遍歷一遍路徑列表，找到其中的拐點，將起點、拐點和終點放在一個列表中，從起點開始，依次連接起點和各個拐點，若第n個拐點與起點的連線不經過障礙物且第n＋1個拐點與起點的連線經過障礙物，去除列表中起點與第n個拐點之間的拐點。再依次驗證列表中其他拐點之間的連線，最后提取剩余拐點，按照順序連接，生成路徑。如圖5 所示，針對［S，1，2，3，4，5，6，T］的路徑列表，生成一個只包含起點、拐點和終點的列表［S，1，2，3，5，T］，以起點為例，依次連接起點S和點3、點5、終點T，起點和點3的連線不經過障礙物且起點S和點5的連線經過障礙物，去除掉點1、點2。在去除全部冗余點之后，列表為［S，3，5，T］，最后按順序依次連接列表中拐點，生成如圖藍色路徑，路徑長度為7.16個單位長度。

方法B［4］與方法A 的原理類似。在路徑列表中從起點開始，依次連接起點和列表中的節點，若第n個節點與起點的連線不經過障礙物且第n＋1個節點與起點的連線經過障礙物，去除列表中起點與第n個節點之間的節點。再依次驗證列表中其他節點之間的連線，最后提取剩余節點，按照順序連接，生成路徑。如圖5所示，針對［S，1，2，3，4，5，6，T］的路徑列表。在去除全部冗余點之后，列表為［S，3，5，T］，最后按照順序依次連接列表中拐點，生成如圖紅色路徑。

方法A、B 去除冗余點的原理如圖4 所示。圖中len（path＿list）代表節點列表的長度。兩種方法判定冗余點的方法是一樣的，只不過方法A 在去除冗余點之前，多了一步尋找拐點的操作，接著在拐點列表中去除冗余點。

圖4 方法A 和B的工作原理流程圖

方法A、B在某一段路徑上的優化結果如圖5所示。圖中由于非拐點的節點較少，方法A 和方法B優化出來的路徑是一樣的。在稍復雜一些的環境中，方法A 在路徑長度的優化效果上會比方法B 的差，但是方法B 的運算次數會比方法A 多。

圖5 A＊算法和方法A、B去除冗余點效果對比

以上兩種去除冗余點的方式都有一定的局限性。方法A 會忽略掉非拐點，非拐點之間的連線也有不穿越過障礙物的可能。忽略掉非拐點會導致去除掉冗余點之后還是存在著路徑非最優解的問題。而方法B運算的次數太多，圖6中的路徑列使用方法A 只用運算7次，而方法B 需要運算13次，程序運行時間更長。因此需要對去除冗余點的方法進行優化，找到一種結合兩者優點的節點優化的方法。

圖6 5×5搜索鄰域

2.2 擴大搜索鄰域

A＊算法采用3×3鄰域擴展節點的方法，一共有8個擴展方向，轉折的角度不夠靈活，產生一些無效的轉彎路徑，導致生成的路徑并且不是最優路徑。

針對這個問題，本文將3×3的鄰域擴展成5×5的鄰域，即16個擴展方向，如圖6所示。

以圖7的5×5地圖為例，圖中黑色圓點為當前節點，紅色節點為目標點。在使用3×3鄰域搜索時，由于搜索方向過于局限，導致從當前節點到目標點的路徑為向東北一個單元格再向北一個單元格，如圖中黑色實線所示，有一次轉折，在轉折處需要轉北偏東45°才能到達目標點，路徑長度為2.4個單位長度。但是在使用5×5鄰域搜索時，只需要朝著北偏東26.57°運動2.2個單位長度就可以直接到達目標點，如圖中虛線所示，有效減少了轉折次數，轉折角度相比于3×3鄰域搜索也有所減小，更便于移動機器人運動。

圖7 5×5搜索鄰域優化效果

但是擴大搜索鄰域并不能完全解決A＊算法路徑列表中有冗余點的問題，如圖8所示，5×5鄰域搜索出來的路徑需要先朝正北運動1個單位長度，再朝北偏東26.57°運動2.2個單位長度到達目標點，而7×7鄰域搜索出來的路徑只需要朝北偏東18.44°運動3.16個單位長度就能到達目標點。5×5鄰域搜索出來的路徑會比7×7鄰域搜索出來的路徑長，且多一個轉折點。但是一味地擴大搜索鄰域，并不適用于所有地圖，而且優化效果有限，可以看到搜索鄰域越大，與原先生成的路徑組成的三角形的頂角越大，這樣優化的路徑是比原先的路徑短不了太多。一味地擴大搜索鄰域還可能會加大程序的計算量，使程序運行時間變長。因此需要在適度擴大搜索鄰域的基礎上去除冗余點。

圖8 7×7搜索鄰域優化效果

2.3 冗余點去除方法改進

目前廣泛使用去除冗余點的方法A、B有一個共同的缺點：過早地去除掉某些點會導致優化效果不好。如圖9所示，圖9中路徑長度為6.06個單位長度，比圖5中的三條路徑都要短。但是在方法A 中，點2在起點和點3的連線不經過障礙物的情況下被刪除，而點4和點6則早早地因為不是拐點被刪除掉了；在方法B 中點2在起點和點3的連線不經過障礙物的情況下被刪除，點4和點6分別是因為點3和點5、點5和終點T 的連線不經過障礙物被刪除。圖5中的路徑適合每兩個節點之間進行連線驗證，這樣確實可以得出圖9中的路徑，但是局限性太大，并不是所有路徑都適合兩個兩個地驗證，因為在復雜一些的環境中，A＊算法規劃出來的路徑和其最優解對比，最優解在某一小段上的路徑的節點優化的數字并不是固定的，在第一段中連接了第i個節點和第i＋a個節點，去除了這兩個點中間的節點；在第二段中連接了第n個節點和第n＋b個節點，去除了這兩個點中間的節點。因此，固定每幾個點之間連線驗證，是有非常大的局限性的。

圖9 優于方法A 和方法B處理結果的路徑

為了解決這個問題，本文在將搜索鄰域擴大至5×5的基礎上提出一種引入隨機數的去除節點列表冗余點方法。將搜索鄰域擴大至5×5之后，有效減少了路徑列表中的節點，在5×5鄰域搜索出來的路徑列表中做節點優化會減少很多運算次數，有效優化程序運行時間。該方法的流程如圖10所示。具體步驟如下：

圖10 本文優化算法去除路徑列表冗余點流程圖

1）依據地圖的大小，設定一個隨機數的取值范圍（a，b），設置一個循環次數r。

2）針對路徑列表，隨機?。╝，b）范圍內的數字c，驗證和的連線是否經過障礙物，若不經過障礙物，則刪除列表中和之間的點。

3）按照順序依次檢測列表中的點，重復2），直到檢測到，整理剩下的點，依次連接列表中的點，生成路徑，計算路徑長度。

4）循環步驟2）和步驟3）r次，最后選擇路徑長度最短的一條路徑輸出。

在面對轉折點較多的路徑時，本文的算法中隨機數這一步可以保留方法A、B中被提前刪除掉的點，找出更優的路徑。

3 仿真驗證

為了驗證本文改進A＊算法的有效性和優化效果，本文在不同環境建模的柵格地圖上進行了仿真實驗，仿真軟件平臺為Python3.9，硬件平臺為Intel（R）Core（TM）i5-9500CPU＠3.00GHz，RAM 16GB。

首先，創建一個30×30的柵格地圖，障礙物占該地圖的30.6%，其中每個柵格代表一個單位（1m）。設置待規劃路徑起點坐標為（5，17），終點坐標為（27，2）。使用3×3搜索鄰域A＊算法在此地圖上進行路徑規劃，得到的路徑如圖11所示?？梢钥闯鯝＊算法拐點較多，有8個拐點，存在明顯的冗余點。

圖11 A＊算法運算結果

其次，使用常用的去除冗余點的方法A 和方法B 去除冗余點，結果如圖12（a）、（b）所示。方法A 生成的路徑拐點數減少到5個，方法B生成的路徑拐點數減少到4個?？梢钥闯鍪褂脙煞N方法后，路徑拐點較常用方法減少、長度有所縮短，但是仍然有優化的空間，比較數據結果見表1。

表1 30×30地圖中算法運行結果對比

圖12 方法A 和方法B處理后的路徑

然后在此地圖上使用將搜索鄰域擴大至5×5的A＊算法進行路徑規劃，結果如圖13（a）所示。路徑長度比A＊算法生成的路徑長度短，但轉折點比方法A 和B 生成的路徑多。接著使用本文提出的隨機去除冗余點法對圖13（a）所示路徑進行優化，在隨機數取值范圍為（2，8）、循環次數為10的情況下，運行結果如圖13（b）所示。

圖13 5×5搜索鄰域A＊算法與隨機去除冗余點法處理后的路徑

整理上述A＊算法、方法A、方法B、5×5 搜索鄰域A＊算法與隨機去除冗余點運行結果，對比如表1所示。表1中運行時間、運行長度均為運行了10次的平均數?？梢钥吹奖疚奶岢龅碾S機去除冗余點法運算出的訪問節點個數、運行時長和路徑長度均優于另外3種算法，證明隨機去除冗余點法在減少了A＊算法生成的路徑節點數和長度，減短了運行時長。

為了驗證本文算法在不同方向路徑上的優化效果，接著在此地圖中，選擇不同方向的幾組起點和終點進行驗證，結果如表2、表3和表4所示。

表2 多組起點終點的路徑長度對比

表3 多組起點終點的運行時長對比

表4 多組起點終點的訪問節點個數對比

可以看出，在此30×30柵格地圖中，面對不同方向的路徑，本文的算法在路徑長度、運行時長和訪問節點個數上均優于其他算法。結合上述4個表的數據，本文算法運行結果對比A＊算法，路徑長度平均減少了4.46%，運行時長平均減短了24.83%，訪問節點數平均減少了39.93%。

4 結束語

針對A＊算法拐點和冗余點較多的問題，本文在搜索鄰域擴大至5×5的基礎上提出了一種引入隨機數去除節點列表冗余點的改進A＊算法。通過仿真驗證并與其他算法對比，證明了本文算法有效改進了A＊算法拐點和冗余點較多的問題，縮短了路徑長度、減少了訪問節點的個數并有效減少了運行時間。但是該方法還存在一定的不足，如運行的結果與隨機數取值范圍、循環次數有很大的關聯，不同的地圖適合的隨機數取值范圍和循環次數不同等。隨后將進一步研究影響最優隨機數取值范圍和循環次數的因素，找到適用于所有地圖的隨機數取值范圍和循環次數推導公式，使得該方法適用于更多地圖。