一種面向AGV路徑規(guī)劃的改進(jìn)A*算法

湯家軍，王忠

（火箭軍工程大學(xué)，陜西西安，710025）

0 引言

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

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

1 問題描述

1.1 A*算法原理

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

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

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

圖1 A*算法流程圖

其具體步驟為：

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

算法開始時：

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

（2）n是目標(biāo)解嗎？是，找到一條路徑。

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

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

1.2 A*算法缺陷

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

圖2 傳統(tǒng)A*算法尋路路徑

2 算法改進(jìn)

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

圖3 改進(jìn)A*算法尋路路徑

其提出的改進(jìn)后的算法如圖4所示。

圖4 改進(jìn)后的A*算法

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

3 應(yīng)用驗證

3.1 任務(wù)要求

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

圖5 實驗場景

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

3.2 關(guān)鍵技術(shù)分析

3.2.1 A*算法應(yīng)用

根據(jù)改進(jìn)的A*算法，本文采用Python語言去實現(xiàn)相關(guān)的任務(wù)。

其中設(shè)置了以下幾個模塊：

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

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

Obstacle():動態(tài)避障功能。

3.2.2 動態(tài)避障技術(shù)

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

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

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

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

圖6 動態(tài)避障

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

此時的軌跡推算即為：

對軌跡的評價函數(shù):

按照此方法可以有效實現(xiàn)AGV的動態(tài)避障。

3.2.3 SLAM建圖

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

圖7 建圖結(jié)果

通過在地圖中標(biāo)記等待區(qū)，裝貨區(qū)，卸貨區(qū)，實現(xiàn)小車在兩個區(qū)之間的路徑規(guī)劃。

3.3 參數(shù)調(diào)節(jié)

通過以上改進(jìn)的A*算法，可以調(diào)節(jié)ROS中小車的路徑規(guī)劃參數(shù)。用以下命令啟動參數(shù)調(diào)節(jié)器：

rosrun rqt_reconfigure rqt_reconfigure

以下列出調(diào)整參數(shù)過程中比較重要的一些參數(shù)，如表1所示。

表1

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

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

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

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

3.4 實驗結(jié)果

通過在虛擬地圖中調(diào)整參數(shù)，將參數(shù)應(yīng)用于實際的場景，圖9是小車在地圖中規(guī)劃的路線。

圖8 具體場景

圖9 路徑規(guī)劃結(jié)果

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

4 總結(jié)

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