基于改進A＊算法室內移動機器人路徑規劃

劉子豪，趙 津，劉 暢，賴坤城，王璽喬

貴州大學 機械工程學院，貴陽550025

在機器人領域中，路徑規劃一直是國內外研究的熱點話題。路徑規劃是指移動機器人在通過傳感器獲得所處環境的全局信息之后，通過路徑規劃算法來規劃出一條最優路徑，可以使得移動機器人安全且快速地由起始點運動到目標點[1]。

A*算法作為一種智能啟發式算法，由于自身的可移植性和可塑性較強，應用領域廣泛[2]，例如航空航天[3]、農業[4]、制造業[5]、倉儲物流[6-7]等。但傳統A*算法有著先天的不足之處。在前期進行路徑點搜索時，需要維護Openlist 和Closedlist 兩個列表。在進行子節點計算時，需將父節點周邊八個點全部加入到Openlist 進行計算，尋找啟發代價最小的點作為下一個父節點，迭代運行[8]。因此當環境信息復雜，場景比較大時，許多不必要的節點都會被計算，造成了內存開銷過大、尋路時間變長的問題；在由父節點搜尋下一個子節點的過程中，子節點只能在父節點周邊八個相鄰的節點中產生，因此，父節點與子節點之間存在著π/4的角度限制[9]，會造成一些不必要的轉折，使得路徑變長；傳統A*算法中，沒有對路徑進行平滑處理，因此，規劃好的路徑的轉折比較劇烈，不利于移動機器人的路徑跟蹤。針對上述問題，國內外研究人員提出了一些改進方法。例如，文獻[10-11]提高了運算速度，文獻[12]提高了路徑的平滑程度，文獻[13]提高了轉彎的曲折程度，但沒有對上述三個問題一起改進的研究成果。

因此針對傳統A*算法在尋路過程中所需時間過長、路徑轉折多以及所規劃的路徑不平滑的問題，本文首先結合跳躍點搜索理論，剔除傳統A*算法中所不需要計算的節點，減小了計算量，提高了運算速度，其次對所得到的路徑進行二次規劃，刪除不必要的轉折點，降低路徑長度。接著將二次規劃得到的路徑，在拐點處進行優化處理，使所得到的路徑更加平滑。最后，將本文提出的算法應用于Turtlebot3 移動機器人中，在室內進行全局路徑規劃實驗，實驗結果表明，本文提出的算法在運行時間、路徑長度、平滑程度上相比于傳統的A*算法都有很大的提高。

1 傳統A＊算法

機器人進行路徑規劃前，需建立自身所用地圖，根據地圖種類可分為柵格、拓撲地圖等，柵格地圖由于自身模型簡單且對環境有很好的表達，在路徑規劃時被廣泛采用，因此本文將采用柵格法來構建地圖。柵格地圖是將機器人得到的2D環境信息地圖進行相同尺寸的柵格劃分。根據柵格中是否有障礙物將柵格分為白色可通行柵格和黑色不可通過柵格。

柵格地圖中路徑規劃實例如圖1所示，周邊環境固定，綠色的點為起點，紅色的點為終點，黃色的線為規劃的路徑。

圖1 規劃路徑

A*算法是一種在靜態環境下獲得最優路徑的啟發式算法。該算法通過計算估價函數，來確定接下來的搜尋和運動方向。首先，起點必須放入Openlist列表，將起點作為父節點，放入Closelist列表，向周邊擴展計算，得到周邊八個節點的代價值，選擇最小的節點作為下一個父節點，再以此節點向周邊進行擴展計算，得到代價值最小的節點作為下一個父節點，依次迭代，直到終點。由于在路徑搜尋的過程中，每一個節點的代價值均是最小。因此，得到的路徑一定為代價最小的最優路徑。A*算法估價函數為：

Fn表示起始點到任意節點的估價值，Gn表示起始點到某一個節點的實際代價，Hn表示起始該點到終點的估計代價值。本文采用歐式距離作為啟發式函數：

起始點橫坐標為xi，起始點縱坐標yi。終點橫坐標xn，終點縱坐標yn。

圖2為A*算法的實例。

圖2 傳統A*算法尋路

由上述傳統A*算法步驟可知，在運行過程中，需要對Openlist和Closelist兩個列表進行維護。圖2中，綠色為選出的路徑點，粉色區域為A*算法需要計算的點，黑色的線條為規劃好的路徑。由圖2得出，粉色的區域為A*在尋路過程中需要計算的節點，但大部分節點都與生成的路徑無關，造成傳統的A*算法在大范圍尋路過程中存在計算量過大、內存消耗嚴重的問題。在規劃好的路徑中存在著不必要的轉折點，例如點S 可以直接到點C。并且，傳統A*算法規劃好的路徑轉折劇烈，不利于機器人進行路徑跟蹤。

2 改進A＊算法

本文針對傳統A*算法計算量大、轉折點過多、路徑不平滑的問題，提出了一種改進算法。首先基于關鍵點選取的策略來代替傳統A*算法中的Openlist和Closelist兩個列表，之后將得到的路徑進行二次規劃，刪除冗余點，最后對轉折處進行路徑平滑處理，得到一條最優路徑。

2.1 關鍵點選取

本文受跳躍點算法的啟發，提出了一種關鍵點搜索的策略。如圖3 所示，起始點為S，終點為G 。關鍵點搜索策略的本質就是給A*尋路算法一個先驗信息，利用選取所得到的關鍵點來代替傳統A*中Openlist 的點進行計算，如本例中，S 為起點，G 為終點。按照傳統的A*算法，應首先計算（1，2），（2，1），（2，2）這三個點的代價值，選擇（2，2）點作為下一個父節點，并計算其周邊的八個點的代價值，再選擇代價最小的路徑，即圖3 中所有的點都需要進行計算。但在圖中，可以明顯得出，S →(2,2)→G 為一條最優路徑，與之無關的點均不需要計算。在下文提出了具體策略，使得在尋路過程中，路徑僅僅經過（2，2）點。這樣就解決了傳統A*算法在尋路上存在的內存開銷過大的問題。在文獻[14]中提出了一種策略，本文基于此，做出了調整，提出了自己的關鍵點策略。通過對A*算法和實際路徑規劃情況分析，可知，當路徑點周邊存在障礙物和不存在障礙物時搜尋路徑的關鍵點不同，因此提出了下列兩種情況關鍵點的選取策略。

圖3 關鍵點

2.1.1 兩點共線，且周邊不存在障礙物

在圖4 中J 和G 是A*算法中的任意節點，J 為父節點。根據圖可以得出，在J 到達G 時，一共存在著兩類路徑。一種是直線路徑，由J 經由x 直接到G 點。另一種為折線路徑，不經過x，由J 折線到達G 點。對比在這兩種情況中，沿直線運動的方式花費的時間最短。在傳統的A*算法中，途中所有的柵格點都需要進行計算，但圖中的灰色部分計算無意義。因此為了刪減這些不必要的點，采用下面策略：

jx 與jg 均為向量。因此，當共線時，僅選取該線段的端點，比如圖中的G 點，作為關鍵點。

圖4 選取關鍵點

2.1.2 兩點不共線，但周邊存在障礙物

在進行A*算法時，需要越過障礙物進行尋路，如圖5 所示，因此當周邊存在障礙物時，上述所給出的策略就需要做出相應的調整，提出了強制關鍵點策略：

jx、jf、jn、nf 均為向量。n 為除x 點外任意一點。當F 點滿足上述關系式時，將其定義為強制關鍵點，在進行路徑規劃時，這些強制關鍵點必須進行計算。

圖5 強制關鍵點

在應用改進的A*進行路徑規劃時，由起始點開始檢測，先搜尋水平方向，當水平方向上有障礙物或者碰觸到邊界，且不能搜尋到強制關鍵點時，搜尋停止，改為搜尋對角線方向，當對角線到一個新節點時，按照之前的策略再進行搜尋，尋找到關鍵點后，將關鍵點作為下一個搜尋的起始點，進行搜尋，一直搜尋到終點為止。尋路過程如圖6所示。

圖6 關鍵點尋路實例

2.2 二次路徑規劃

在傳統的A*算法中，路徑只進行一次規劃，得到一條在A*算法約束條件下的最優路徑。由于自身在搜尋過程中只能向周邊八個點進行擴展搜索，因此存在π/4的角度限制，使得路徑中依然存在著冗余轉折點。在這種情況下，進行二次路徑規劃，將得到的原始路徑，進行冗余點刪除的處理，使得路徑長度變短。在二次路徑規劃時，本文提出了一種反向動態搜索的策略。將路徑的終點作為二次路徑規劃的起始點，同理，路徑的起始點作為二次規劃搜尋的終點。由起始點向下一個頂點連接，假如沒有遇到障礙物，迭代進行。直到遇到障礙物時，將其路徑中心點相連，檢查是否穿過障礙物，如果沒有，連接兩點，如果有障礙物，則連接中點與靠近終點端四分之一處。一直取原來的二分之一長度作為路徑的搜尋點，直到兩個路徑的長度之間沒有較大的變化為止（視地圖尺寸為定，本文地圖大小為7.5 m×7.5 m，經多次實驗驗證，選擇長度比為0.9）。將最終得到的點連接，得到二次規劃的路徑。

2.3 路徑平滑

在經過2.1和2.2節處理后，得到了一條基于本文搜索策略的最優的路徑，但在圖6 中可以看出，二次規劃的路徑轉折十分劇烈，室內機器人在進行路徑跟蹤時，沒有好的跟蹤效果。因此，需要對得到的路徑進行平滑處理。Hybird A*算法[15]對路徑平滑有很好的處理效果，但時間花費高，計算開銷大，并且容易陷入局部最優。在本文中，提出了一種動態相切圓策略。在2.2 節的基礎上，選取與障礙物不相交的拐點和路徑與障礙物的交點作為優化點。對比優化點兩側的路徑，選取小的路徑的二分之一長度，分別做垂直，選取兩條垂線的交點作為圓心，畫一條圓弧來代替原來的轉折點，達到路徑平滑的作用。實驗可以得知，雖然平滑效果不如Hybird A*，但運算效率高，并且完全可滿足室內移動機器人的路徑跟蹤。

3 實驗

3.1 仿真實驗

經本文算法得出的路徑與傳統A*算法的路徑如圖7所示。

圖7 A*與Improved A*

由圖7可以看出，本文提出的改進算法在路徑長度和彎折角度較傳統A*算法有著明顯的提高，在運算時，傳統A*算法所需的時間為3.02 ms，改進的A*算法為0.994 ms，運算效率提高了67%；傳統A*算法路徑長度為301.5 cm，本文算法為293.4 cm，減小了2.7%。為了驗證該算法的通用性，再選取柵格邊長為10 cm，地圖大小分別為15×15、30×30、50×50 的環境中進行仿真實驗。實驗選擇固定起點與終點，隨機環境進行實驗，每一組地圖均進行15組隨機實驗。時間單位為1×10-6s，路徑單位為厘米。實驗結果如圖8、9所示。

通過45 組仿真實驗數據可以得出，本文提出的算法在時間上平均為A*算法運算時間的56.09%，路徑長度降低為原來的97.94%。并且，根據地圖大小和障礙物數量的不同，可以得出本文所提出的算法，在大尺寸、多障礙物的環境下，運算速度、路徑長度明顯優于傳統A*算法。

3.2 真實環境實驗

圖8 三種環境時間

圖9 三種環境路徑長度

實驗所采用的機器人為Turtlebot3buger。該機器人搭載Raspberry Pi 3樹莓派3開發板、OpenCR底層控制器，重量為0.995 kg，大小為138 mm×178 mm×192 mm。機器人運動時通過OpenCR 主板控制底部的兩個帶編碼器的減速電機，進行速度控制，使其轉彎或者直行。機器人如圖10所示。

圖10 Turtlebot3buger

實驗環境如下，采用邊長15 cm、大小為12×12的柵格模型進行建圖，得到的柵格環境地圖及規劃好的路徑如圖11所示。設定起點坐標為（2，2），終點坐標為（7，11）。將規劃好的路徑移植到機器人的開發板中，使機器人自主進行路徑跟蹤。實驗效果表明，在運算速度，路徑長度、平滑程度上，較傳統A*算法均有明顯提高，如表1所示。

圖11 實驗環境（左）柵格地圖路徑規劃結果（右）

表1 A*與Improved A*對比

4 結論

本文以提出的關鍵點策略、二次路徑規劃以及路徑平滑策略，對傳統A*算法進行改進。關鍵點策略減小了傳統A*算法中的內存消耗，提高了機器人的尋路效率。二次路徑規劃及路徑平滑降低了傳統A*算法的路徑長度以及轉折角度，使機器人在路徑跟蹤時效率和跟蹤效果提高。并且，本文提出的算法在場景大且環境較為復雜時，提升的效果尤為顯著。將本文算法應用于turtlebot3buger機器人中，實驗結果表明，改進的A*算法較傳統A*算法優勢明顯。