基于變鄰域的改進(jìn)AGV路徑規(guī)劃算法

陳遠(yuǎn)浩，吳明暉，李 陽，洪孔林

（上海工程技術(shù)大學(xué)機(jī)械與汽車工程學(xué)院，上海 201620）

0 引言

在移動(dòng)機(jī)器人領(lǐng)域中，自動(dòng)引導(dǎo)車（Automated Guided Vehicle，AGV）廣泛應(yīng)用于工業(yè)場景的物料搬運(yùn)工作［1］。路徑規(guī)劃是指移動(dòng)機(jī)器人通過即時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）獲得環(huán)境地圖信息后，通過路徑規(guī)劃算法獲得最優(yōu)路徑，決定AGV 在工廠環(huán)境中的工作效率［2］

目前，國內(nèi)外主流路徑規(guī)劃算法包括遺傳算法（Genetic Algorithm，GA）［3］、快速隨機(jī)搜索樹（Rapidly-exploring Random Tree，RRT）［4］、A*算法［5］等。

（1）GA 為全局智能優(yōu)化搜索算法。Elhoseny 等［6］提出一種在動(dòng)態(tài)環(huán)境下基于貝塞爾曲線的改進(jìn)GA 路徑規(guī)劃算法，提高生成解的多樣性。在染色體多樣性度量值低于閾值時(shí)，才在每次迭代中接受染色體多樣性，從而幫助GA 跳出局部最優(yōu)解。但由于工廠環(huán)境復(fù)雜，GA 算法會(huì)隨著種群增加，運(yùn)算速度降低，最終還是會(huì)陷入局部最優(yōu)解。

（2）RRT 屬于采樣搜索算法。Wang 等［7］提出一種基于強(qiáng)化學(xué)習(xí)的多RRT 方法，用于規(guī)劃窄道機(jī)器人路徑，將樹的選擇過程抽象為多臂賭博機(jī)問題，使用優(yōu)化的強(qiáng)化學(xué)習(xí)算法進(jìn)行選擇。該算法運(yùn)算速度快、搜索能力強(qiáng)，但在工廠環(huán)境下搜索時(shí)盲目性大、在高維度環(huán)境下耗時(shí)長、易陷入局部最優(yōu)。

（3）A*算法屬于啟發(fā)式搜索算法。現(xiàn)已經(jīng)在無人機(jī)［8］、智能工廠［9］、智慧農(nóng)業(yè)［10］等領(lǐng)域廣泛應(yīng)用。Bing等［11］通過后階段局部路徑平滑算法提高A*算法的規(guī)劃成功率。Xin 等［12］基于無限鄰域思想優(yōu)化傳統(tǒng)A*算法的搜索方向，但降低了運(yùn)算速度。陳偉華等［13］提出一種基于雙重A*算法的移動(dòng)機(jī)器人路徑規(guī)劃方法，使機(jī)器人在動(dòng)態(tài)環(huán)境中也能實(shí)現(xiàn)安全導(dǎo)航，提高了路徑規(guī)劃效率。

雖然A*算法已廣泛應(yīng)用于工廠AGV 作業(yè)場景，但仍存在以下不足：①當(dāng)SLAM 建立的環(huán)境地圖較大時(shí)，傳統(tǒng)A*算法會(huì)計(jì)算許多冗余節(jié)點(diǎn)，計(jì)算時(shí)間顯著增長；②廠區(qū)規(guī)劃AGV 安全行駛區(qū)域，相較于其它環(huán)境地圖更規(guī)律，但A*算法的評價(jià)函數(shù)尚未對此進(jìn)行優(yōu)化；③A*算法的路徑冗余節(jié)點(diǎn)較多，所規(guī)劃的路徑平滑度較差。

針對上述問題，本文提出一種基于變鄰域的改進(jìn)A*路徑規(guī)劃算法。首先，將傳統(tǒng)3×3 鄰域搜索方式改進(jìn)為7×7 鄰域搜索；然后，基于終點(diǎn)向量的變鄰域搜索方法動(dòng)態(tài)優(yōu)化搜索鄰域范圍；接下來，融合預(yù)先建立的地圖信息優(yōu)化代價(jià)函數(shù)；最后，通過插點(diǎn)法優(yōu)化冗余節(jié)點(diǎn)。

1 改進(jìn)A*路徑規(guī)劃算法

1.1 傳統(tǒng)A*算法

Hart［14］提出的A*路徑規(guī)劃算法在Dijkstra［15］算法的基礎(chǔ)上增加啟發(fā)式函數(shù)，在保證最優(yōu)性的同時(shí)，增加目標(biāo)節(jié)點(diǎn)信息以提升搜索效率。算法具體步驟如下：

步驟1：初始化柵格地圖及起點(diǎn)終點(diǎn)信息。

步驟2：初始化close 及open兩個(gè)空列表。

步驟3：將起點(diǎn)坐標(biāo)設(shè)置為當(dāng)前坐標(biāo)并存入open。

步驟4：判斷當(dāng)前節(jié)點(diǎn)是否為終點(diǎn)，若是則輸出路徑結(jié)束算法，反之執(zhí)行步驟5。

步驟5：搜索當(dāng)前可擴(kuò)展的所有子節(jié)點(diǎn)。

步驟6：計(jì)算子節(jié)點(diǎn)代價(jià)值F(n)并將子節(jié)點(diǎn)信息存入open。

步驟7：判斷是否遍歷所有可擴(kuò)展節(jié)點(diǎn)，若遍歷完執(zhí)行步驟8，反之執(zhí)行步驟5。

步驟8：計(jì)算F(n)最小子節(jié)點(diǎn)并從open 中刪除該節(jié)點(diǎn)，之后存入close。

步驟9：設(shè)置該節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)，并執(zhí)行步驟4。

算法的代價(jià)值表達(dá)式為：

式中，n代表各個(gè)節(jié)點(diǎn)，F(xiàn)(n)代表當(dāng)前節(jié)點(diǎn)n的總體代價(jià)值，G(n)代表AGV 的起點(diǎn)坐標(biāo)到當(dāng)前節(jié)點(diǎn)n所在坐標(biāo)的代價(jià)值，H(n)代表AGV 當(dāng)前節(jié)點(diǎn)n所在節(jié)點(diǎn)的坐標(biāo)到終點(diǎn)坐標(biāo)的代價(jià)值。

常見的H(n)計(jì)算方法包括曼哈頓距離、歐幾里得距離、對角線距離等［16］。本文選擇歐幾里得距離作為H(n)的代價(jià)函數(shù)，數(shù)學(xué)表達(dá)式如式（2）所示：

式中，(xn，yn)為當(dāng)前節(jié)點(diǎn)所在坐標(biāo)，(xd，yd)為終點(diǎn)所在坐標(biāo)。

1.2 多鄰域拓展及變鄰域搜索方法

如圖1（a）所示，傳統(tǒng)A*算法采用3×3 鄰域搜索方式，當(dāng)轉(zhuǎn)角過大時(shí)，會(huì)對AGV 的運(yùn)動(dòng)會(huì)造成影響，因此設(shè)置當(dāng)前節(jié)點(diǎn)到子節(jié)點(diǎn)的轉(zhuǎn)角均為45°的整數(shù)倍。

當(dāng)面對大且復(fù)雜的環(huán)境地圖時(shí)，該方式會(huì)計(jì)算冗余搜索節(jié)點(diǎn)，浪費(fèi)計(jì)算資源。為此，通過多領(lǐng)域拓展搜索方法將當(dāng)前節(jié)點(diǎn)到子節(jié)點(diǎn)的轉(zhuǎn)角從45°的整數(shù)倍優(yōu)化為多角度。同時(shí)，采用7×7 鄰域搜索方法減少冗余節(jié)點(diǎn)的計(jì)算，如圖1（b）所示。此外，將原有8 個(gè)子節(jié)點(diǎn)增加到48 個(gè)子節(jié)點(diǎn)，并將AGV 的移動(dòng)方向從8個(gè)優(yōu)化到32個(gè)。

在AGV 實(shí)用場景中，由于受安全區(qū)域限制，AGV 所處環(huán)境較為簡單，不容易陷入“死胡同”，死鎖發(fā)生可能性較低。因此，大量與終點(diǎn)方向相反的子節(jié)點(diǎn)往往是冗余搜索節(jié)點(diǎn)。針對該問題，本文基于終點(diǎn)向量的變鄰域搜索方法，根據(jù)節(jié)點(diǎn)到終點(diǎn)的向量，將48 鄰域搜索優(yōu)化至27 鄰域搜索，如圖1（c）所示。

Fig.1 A*algorithm neighborhood search methods圖1 A*算法鄰域搜索方式

設(shè)(xn，yn)為當(dāng)前節(jié)點(diǎn)所在的坐標(biāo)，(xd，yd)為終點(diǎn)所在坐標(biāo)，則終點(diǎn)向量可定義為：

設(shè)θ為終點(diǎn)向量與x軸正向方向的夾角，則根據(jù)終點(diǎn)向量優(yōu)化的搜索鄰域如表1、圖2所示。

Table 1 Endpoint vector optimization neighborhood表1 終點(diǎn)向量優(yōu)化鄰域

Fig.2 Neighborhood coordinate number圖2 鄰域坐標(biāo)編號

1.3 代價(jià)函數(shù)優(yōu)化

傳統(tǒng)A*算法的啟發(fā)函數(shù)在代價(jià)函數(shù)中會(huì)影響A*算法的效率。當(dāng)H(n)＜G(n)時(shí)，搜索節(jié)點(diǎn)多、效率低，路徑短；當(dāng)H(n)＞G(n)時(shí)，搜索節(jié)點(diǎn)少、效率高，但無法保證規(guī)劃的是最優(yōu)路徑。綜上所述，當(dāng)環(huán)境地圖較為規(guī)則且搜索節(jié)點(diǎn)接近終點(diǎn)時(shí)，需要較大的啟發(fā)函數(shù)；當(dāng)?shù)貓D環(huán)境較復(fù)雜且算法搜索節(jié)點(diǎn)較多時(shí)，則需要較小的啟發(fā)函數(shù)［17］。為此，融合當(dāng)前節(jié)點(diǎn)及環(huán)境地圖信息，將代價(jià)函數(shù)改進(jìn)為：

式中，d為當(dāng)前節(jié)點(diǎn)到終點(diǎn)的距離，D為起點(diǎn)到終點(diǎn)的距離，N為可行區(qū)域占柵格地圖的比重。

1.4 插點(diǎn)法優(yōu)化

插點(diǎn)法又稱Floyd 算法，利用動(dòng)態(tài)規(guī)劃思想尋求兩點(diǎn)之間最短路徑［18］。如圖3 所示，為進(jìn)一步優(yōu)化A*算法規(guī)劃的冗余節(jié)點(diǎn)，在AB之間插入D點(diǎn)。其中，A、B、C為初次規(guī)劃的路徑，dist(A，C)為A到C的最短距離。

由于A、C間存在障礙物無法直接形成路徑，因此dist(A，C) 為+∞。通過計(jì)算可得dist(A，C) ＞dist(A，D) +dist(D，C)，并且滿足dist(A，B) +dist(B，C) ＞dist(A，D) +dist(D，C)，因此ADC為最優(yōu)路徑。

Fig.3 Redundant path optimized by Floyd algorithm圖3 插點(diǎn)法優(yōu)化的冗余路徑

1.5 改進(jìn)A*算法流程

基于變鄰域的改進(jìn)A*算法流程如下：

步驟1：初始化柵格地圖及起點(diǎn)終點(diǎn)信息。

步驟2：初始化close 及open兩個(gè)空列表。

步驟3：將起點(diǎn)坐標(biāo)設(shè)置為當(dāng)前坐標(biāo)并存入open。

步驟4：判斷當(dāng)前節(jié)點(diǎn)是否為終點(diǎn)，若是則插點(diǎn)法優(yōu)化冗余路徑、輸出路徑結(jié)束算法，反之執(zhí)行步驟5。

步驟5：搜索當(dāng)前基于終點(diǎn)向量優(yōu)化的7×7 鄰域子節(jié)點(diǎn)。

步驟6：計(jì)算子節(jié)點(diǎn)代價(jià)值F(n)并將信息存入open。

步驟7：判斷是否遍歷所有可擴(kuò)展節(jié)點(diǎn)，若遍歷完則執(zhí)行步驟8，反之執(zhí)行步驟5。

步驟8：計(jì)算F(n)最小的子節(jié)點(diǎn)，并將其從open 中刪除，然后存入close。

步驟9：設(shè)置該子節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)，并執(zhí)行步驟4。

2 實(shí)驗(yàn)結(jié)果及分析

2.1 實(shí)驗(yàn)設(shè)計(jì)

本文使用MATLAB 建立3 個(gè)30×30 的AGV 工廠仿真柵格地圖A、B、C，起點(diǎn)坐標(biāo)均為（1.5，1.5），終點(diǎn)坐標(biāo)均為（30.5，30.5），如圖4 所示。在3 組不同地圖中分別使用GA算法、RRT 算法、傳統(tǒng)A*算法和改進(jìn)A*算法進(jìn)行AGV 路徑規(guī)劃。并且，分別比較4 種算法的路徑長度、搜索節(jié)點(diǎn)個(gè)數(shù)、運(yùn)行時(shí)間、累積轉(zhuǎn)角度數(shù)等性能指標(biāo)。

2.2 結(jié)果分析

圖5 為地圖A 的實(shí)驗(yàn)效果，3 組實(shí)驗(yàn)的具體數(shù)據(jù)見表2。

Table 2 Experimental data表2 實(shí)驗(yàn)數(shù)據(jù)

續(xù)表

通過比較每組實(shí)驗(yàn)數(shù)據(jù)可得出以下結(jié)論：

（1）RRT 算法由于基于隨機(jī)采樣搜索，導(dǎo)致其搜索時(shí)間不穩(wěn)定，且輸出的路徑隨機(jī)相較于傳統(tǒng)A*算法及改進(jìn)A*算法更長。

（2）在柵格地圖環(huán)境中，RRT 算法及遺傳算法的復(fù)雜度較高，計(jì)算時(shí)間相較于A*算法及改進(jìn)A*算法更長。

（3）改進(jìn)A*算法相較于傳統(tǒng)A*算法，所規(guī)劃路徑長度更短，顯著提高了AGV 的作業(yè)效率。

（4）改進(jìn)A*算法遍歷節(jié)點(diǎn)數(shù)更少，算法運(yùn)算時(shí)間更短，算法性能存在一定提升。

（5）改進(jìn)A*算法累計(jì)轉(zhuǎn)角度數(shù)更小，AGV 能夠規(guī)劃更合理的路徑，以減少不必要的轉(zhuǎn)向。

Fig.4 Simulation grid map of three different AGV plants圖4 3種不同AGV工廠仿真柵格地圖

Fig.5 Experimental results of different algorithms on map A圖5 不同算法在地圖A的實(shí)驗(yàn)效果

2.3 實(shí)驗(yàn)驗(yàn)證

將改進(jìn)A*算法應(yīng)用于激光雷達(dá)AGV 中，AGV 實(shí)物主體如圖6（a）所示，圖6（b）為AGV 導(dǎo)航過程。

Fig.6 Application of A*algorithm on lidar AGV圖6 A*算法應(yīng)用于激光雷達(dá)AGV

此外，利用改進(jìn)A*算法在SLAM 算法建立的工廠環(huán)境地圖中，規(guī)劃初始點(diǎn)到終點(diǎn)的最優(yōu)路徑。為了減少實(shí)際誤差和實(shí)驗(yàn)偶然性，分別通過不同算法進(jìn)行3 組由多個(gè)導(dǎo)航點(diǎn)組合而成的任務(wù)。由圖7 可知，改進(jìn)A*算法相較于其它算法，有效減少了規(guī)劃路徑的長度及累計(jì)轉(zhuǎn)角。

3 結(jié)語

本文針對AGV 作業(yè)的環(huán)境特點(diǎn)，設(shè)計(jì)一種基于變鄰域的改進(jìn)A*路徑規(guī)劃算法。該算法首先擴(kuò)展傳統(tǒng)A*算法的搜索鄰域；然后引入終點(diǎn)向量優(yōu)化冗余搜索鄰域，基于環(huán)境信息及當(dāng)前節(jié)點(diǎn)信息優(yōu)化代價(jià)函數(shù)；最后通過插點(diǎn)法優(yōu)化冗余節(jié)點(diǎn)，提高路徑平滑度。

Fig.7 Experimental results of different algorithms圖7 不同算法實(shí)驗(yàn)結(jié)果

仿真實(shí)驗(yàn)及實(shí)物驗(yàn)證表明，該算法在AGV 工廠環(huán)境下，路徑長度、遍歷節(jié)點(diǎn)數(shù)、運(yùn)算用時(shí)、累計(jì)轉(zhuǎn)角等方面均優(yōu)于傳統(tǒng)A*算法，工作效率及AGV 運(yùn)動(dòng)平滑度得到了顯著提升。