湯家軍，王忠

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

0 引言

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

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

1 問題描述

1.1 A*算法原理

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

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

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

圖1 A*算法流程圖

其具體步驟為：

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

算法開始時(shí)：

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

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

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

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

1.2 A*算法缺陷

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

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

2 算法改進(jìn)

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

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

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

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

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

3 應(yīng)用驗(yàn)證

3.1 任務(wù)要求

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

圖5 實(shí)驗(yàn)場景

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

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

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

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

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

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

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

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

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

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

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

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

如圖6所示，t+1時(shí)刻相對(duì)于t時(shí)刻的位移：

圖6 動(dòng)態(tài)避障

在ROS中對(duì)于x和y軸的運(yùn)動(dòng)距離推算：

此時(shí)的軌跡推算即為：

對(duì)軌跡的評(píng)價(jià)函數(shù):

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

3.2.3 SLAM建圖

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

圖7 建圖結(jié)果

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

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

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

rosrun rqt_reconfigure rqt_reconfigure

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

表1

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

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

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

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

3.4 實(shí)驗(yàn)結(jié)果

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

圖8 具體場景

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

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

4 總結(jié)

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