一種面向AGV路徑規劃的改進A*算法

湯家軍，王忠

（火箭軍工程大學，陜西西安，710025）

0 引言

隨著現代科學技術的發展，智能化逐漸進入大眾的視野，車輛的智能化已經受到越來越多的科研人員的關注。全局路徑規劃是車輛在室外安全行駛的關鍵技術之一，面對復雜多變的室外環境，通過算法提高車輛的路徑規劃的有效性、平滑性等就成為了路徑規劃的主要研究內容之一[1]。

本文針對智能車的路徑規劃問題，采用了一種改進的A*算法，提高了算法的效率，在智能車大賽中讓智能車能夠較好的跑完整個地圖，跑出的路徑也比較平滑。

1 問題描述

1.1 A*算法原理

A*算法作為啟發式搜索算法的一種，它是一種尋找圖平面上有多個節點的路徑的最低通過代價的算法，是1968年的文獻[2]提出來的，其核心思想是一個啟發函數：

公式(1)中f(n)是節點n的估價函數，它由組成：g(n)表示從起始結點到搜索結點的實際代價；h(n)表示當前結點到目標結點的估計值。使用這個函數不需要對每個節點都進行搜索，而是針對各個節點的代價進行搜索，從優先隊列中選取f(n)值最小的節點作為下一個要搜索的節點。

其算法流程圖如圖1所示。

圖1 A*算法流程圖

其具體步驟為：

首先將起始結點S放入OPEN表，CLOSE表置空，OPEN表記錄下所有被考慮來尋找最短路徑的節點，CLOSE表記錄下不會再被遍歷的節點。

算法開始時：

（1）如果OPEN表不為空，從表頭取一個結點n，將n放入CLOSE表中，如果為空算法失敗。

（2）n是目標解嗎？是，找到一條路徑。

（3）將n的所有擴展結點展開，即n直接關聯的結點，如果不在CLOSE表中，就將它們放入OPEN表，同時計算每一個后繼結點的估價值f(n)，將OPEN表按f(n)排序，最小的放在表頭，重復算法，回到1。

雖然A*算法可以有效的找出較優的路徑，但是其還存在著一定的缺陷。A*算法通過比較當前節點周圍的八個節點的f(n)的值來確定下一個要進行遍歷的節點，但是當多個點的f(n)的值相同的時候，A*算法并不能準確找出最優的路徑，可能會因此陷入局部最優解。

1.2 A*算法缺陷

傳統的A*算法中，尋找出來的路徑只能按照直角去走，在時間上的耗費太大，如圖2中虛線箭頭所示，這種尋路的方法看起來是很不自然的；如果起點到終點中間沒有障礙物的阻擋，如圖2中實線箭頭所示，也只能沿著45°的方向去尋路，這樣使算法的效率變低。

圖2 傳統A*算法尋路路徑

2 算法改進

文獻[3]提出了一種基于A*算法的改進，在生成路徑之后，檢測路徑上隔開的兩個點之間是否有line of sight，通俗所說即為視線，有的話便可以把中間的點刪除，讓小車直接穿過。這種方法稱之為A* with post-smoothed paths。依據如何選取這兩個點，最終優化的效果不同，本文選取的是最近的兩個可以到達的點。如圖3的虛線箭頭，在兩個方格之間沒有障礙物的阻擋，可以實現兩個點的直接穿插。

圖3 改進A*算法尋路路徑

其提出的改進后的算法如圖4所示。

圖4 改進后的A*算法

通過與A*算法的比較可以發現，除了判別是否有和父節點的視線外，其他的內容和A*算法是一樣的，A*算法計算新的點的新的g，如果比原來的更好，則將原來的parent和g換成新的。而此篇論文提出的算法，在計算前先去計算新的節點和原來的節點的parent是否直接可達。如果直接可達，則把新的節點的parent和g以原來的parent為parent進行更新。否則跟A*算法做出同樣的處理。

3 應用驗證

3.1 任務要求

本次實驗的任務是將小車放在初始等待區，讓小車自動導航到裝貨區，手動放上物品，導航通過減速帶，到達卸貨區，手動拿下物品，通過障礙物，通過S彎到達初始等待區。實驗場景如圖5所示。

圖5 實驗場景

本文主要針對路徑規劃任務做出詳細的介紹，利用A*算法，得知起始點與終止點，經過裝貨區，卸貨區，在眾多的行駛路徑中規劃處一條最合適的、最平滑的路徑，使小車的效率最高。

3.2 關鍵技術分析

3.2.1 A*算法應用

根據改進的A*算法，本文采用Python語言去實現相關的任務。

其中設置了以下幾個模塊：

Getmap()：將掃描出來的地圖數據化。

A_star()：利用改進的A*算法尋找路徑，并將坐標返回。

Obstacle():動態避障功能。

3.2.2 動態避障技術

復雜環境下的路徑規劃是智能車安全有效運行的關鍵，動態避障技術能夠使智能車有效地避開障礙物，順利地到達終點。文獻[4]提出一種基于改進蟻群與A*雙層規劃算法,用以解決AGV動態避障問題，在AGV的局部滾動預測規劃中,采用相應的避碰策略選擇局部最優子目標點,并用改進的A*算法進行局部路徑規劃，實現動態避障的有效性和實時性。

本文采用的是ROS中的dwa算法，即動態窗口法。其主要思想是在速度空間中采樣多組速度，模擬AGV在這些速度下一定時間內的軌跡，然后選取最優的軌跡去驅動AGV的運動。

參考論文[5]做出的如下推斷：

如圖6所示，t+1時刻相對于t時刻的位移：

圖6 動態避障

在ROS中對于x和y軸的運動距離推算：

此時的軌跡推算即為：

對軌跡的評價函數:

按照此方法可以有效實現AGV的動態避障。

3.2.3 SLAM建圖

本次比賽采用激光SLAM建圖，將圖5所示的地圖通過激光雷達掃描出來，建立二維柵格地圖，將地圖數據化并傳入算法中，使得其可以在地圖上進行路徑規劃，其建圖結果如圖7所示。

圖7 建圖結果

通過在地圖中標記等待區，裝貨區，卸貨區，實現小車在兩個區之間的路徑規劃。

3.3 參數調節

通過以上改進的A*算法，可以調節ROS中小車的路徑規劃參數。用以下命令啟動參數調節器：

rosrun rqt_reconfigure rqt_reconfigure

以下列出調整參數過程中比較重要的一些參數，如表1所示。

表1

·dt_ref：將此參數設置為與車輛長度差不多大的數值時可以保證更高的精度；

·global_plan_viapoint_sep：默認設置下為Disable，即只沿全局規劃向前尋找，找到的離開局部規劃器規劃范圍前的最后一個點作為局部目標。若全局規劃特殊，在無障礙空間仍不走直線，則需要將此值設置為設置為一個小的正數；

·min_turning_radius：通過測量，此小車的最小轉彎半徑為0.75；

·min_obstacle_dist：將此參數設置過大會使智能車轉彎速度變慢，轉彎半徑變大，設置過小會使智能車碰撞到障礙物。

3.4 實驗結果

通過在虛擬地圖中調整參數，將參數應用于實際的場景，圖9是小車在地圖中規劃的路線。

圖8 具體場景

圖9 路徑規劃結果

如圖9黑色的線條即為小車規劃出的路徑，可以看到小車可以比較完美的通過障礙物，最終達到終點。

4 總結

本文利用一種改進的A*算法，首先建立二維柵格地圖，通過這種算法去調整小車中路徑規劃的參數，使小車能夠在這種地圖中跑完整個路徑，實現裝貨與卸貨。此方法也可以應用于別的場景中，具體的實現讀者可以通過具體的實驗去實現。