AGV復(fù)合自主路徑規(guī)劃方法研究

肖獻(xiàn)強(qiáng)，王鼎用，王家恩，耿奕旻

（合肥工業(yè)大學(xué)機(jī)械工程學(xué)院，安徽 合肥 230009）

1 引言

如今，工廠逐漸轉(zhuǎn)向智能化的發(fā)展趨勢(shì)愈加明顯，AGV（Au‐tomated Guided Vehicle）成為眾多學(xué)者研究的熱點(diǎn)課題。路徑規(guī)劃作為AGV智能化的核心部分，目前主要有兩種規(guī)劃方式：基于環(huán)境已知的全局路徑規(guī)劃和基于環(huán)境未知的局部路徑規(guī)劃方法。在這兩種類型的規(guī)劃方法中，主要有Dijkstra算法[1]、蟻群算法[2]、人工勢(shì)場(chǎng)法[3]、遺傳算法[4]、A*算法[5-7]、D*算法[8]等。在工廠的實(shí)際應(yīng)用中，AGV的工況具有大范圍、動(dòng)態(tài)性、人機(jī)混雜的3種特征。基于此，研究解決AGV在動(dòng)態(tài)環(huán)境下的自動(dòng)避讓和動(dòng)態(tài)路徑規(guī)劃顯得尤為重要。國(guó)內(nèi)外相關(guān)領(lǐng)域的學(xué)者提出了全局路徑規(guī)劃與局部路徑規(guī)劃相互結(jié)合的方式，文獻(xiàn)［9］提出了將A*算法與人工勢(shì)場(chǎng)法相結(jié)合的方式，其優(yōu)化了力源，將AGV后面的斥力去除及最大角度的優(yōu)化，但是仍然沒有解決局部最優(yōu)的根本問題。文獻(xiàn)［10］將文化基因算法與Morphin算法相結(jié)合，雖然作者對(duì)機(jī)器人混合路徑規(guī)劃進(jìn)行了改進(jìn)，但是此路徑規(guī)劃算法較為復(fù)雜。

針對(duì)提高AGV實(shí)時(shí)路徑規(guī)劃的效率，提出了將A*算法與D*算法相結(jié)合的自主路徑規(guī)劃方法，主要思想是：AGV在已經(jīng)創(chuàng)建的柵格地圖上設(shè)置全局路徑的起始點(diǎn)與目標(biāo)點(diǎn)，根據(jù)路徑的起始點(diǎn)與目標(biāo)點(diǎn)調(diào)用A*算法生成全局路徑；當(dāng)AGV在全局路徑上行駛檢測(cè)到障礙物時(shí)，將路徑上障礙物的下一點(diǎn)設(shè)置為局部路徑的目標(biāo)點(diǎn)，根據(jù)當(dāng)前點(diǎn)與局部路徑目標(biāo)點(diǎn)確定局部路徑搜索的柵格地圖，較小的路徑搜索范圍，提高了路徑規(guī)劃的實(shí)時(shí)性。

2 復(fù)合路徑規(guī)劃方法

2.1 A＊算法

A*算法作為經(jīng)典的路徑搜索算法，是最有效的直接搜索算法，其搜索的方式是在Dijkstra算法的基礎(chǔ)上采用啟發(fā)式搜索算法，其啟發(fā)式估價(jià)函數(shù)如下：

式中：f(n)—節(jié)點(diǎn)n的估價(jià)函數(shù)；g(n)—從起始點(diǎn)到節(jié)點(diǎn)n的實(shí)際的代價(jià)；h(n)—從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的預(yù)估代價(jià)值。其中h(n)有3種方式預(yù)估代價(jià)值，分別有曼哈頓距離、歐幾里得距離、對(duì)角線距離。

2.2 D＊算法

D*算法，即為動(dòng)態(tài)的A*算法，其通過一個(gè)維護(hù)一個(gè)優(yōu)先隊(duì)列來對(duì)場(chǎng)景中的路徑節(jié)點(diǎn)進(jìn)行搜索，用Dijstra算法從目標(biāo)節(jié)點(diǎn)向起始節(jié)點(diǎn)搜索，儲(chǔ)存路網(wǎng)中目標(biāo)點(diǎn)到各個(gè)節(jié)點(diǎn)的最短路徑和該位置到目標(biāo)點(diǎn)的實(shí)際值，如式（2）所示：

式中：h(n)—當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的實(shí)際代價(jià)值；c(next，n)—當(dāng)前點(diǎn)到下一節(jié)點(diǎn)的新權(quán)值，next—下一節(jié)點(diǎn)的原實(shí)際值。

2.3 這里的復(fù)合路徑規(guī)劃模型

在柵格地圖上，AGV自主地根據(jù)起始點(diǎn)與目標(biāo)點(diǎn)以及障礙物的位置關(guān)系，基于曼哈頓啟發(fā)搜索方式的A*算法搜索得到從起始點(diǎn)到目標(biāo)點(diǎn)且繞開障礙物的一條路徑。假設(shè)當(dāng)前點(diǎn)的位置為(x i,y i)，目標(biāo)點(diǎn)的位置為(x j,y j)，移動(dòng)單位距離的代價(jià)為P，則曼哈頓的距離求解公式如示（3）：

當(dāng)AGV在行駛過程中檢測(cè)到障礙物時(shí)，在全局路徑上搜索和設(shè)置局部路徑目標(biāo)點(diǎn)，如圖1所示。

圖1 復(fù)合自主路徑規(guī)劃示意圖Fig.1 Schematic Diagram of Composite Autonomous Path Planning Method

根據(jù)當(dāng)前點(diǎn)與局部路徑目標(biāo)點(diǎn)確定的虛線柵格地圖區(qū)域，將其設(shè)置為局部路徑搜索范圍。調(diào)用D*算法生成局部路徑，將局部路徑與全局路徑進(jìn)行拼接，實(shí)現(xiàn)AGV當(dāng)前位置到目標(biāo)位置的全局規(guī)劃路徑更新，完成實(shí)時(shí)自主避障的功能。

2.4 復(fù)合路徑規(guī)劃數(shù)學(xué)方法

在AGV行駛中，根據(jù)環(huán)境的動(dòng)態(tài)變化初始化的柵格地圖,創(chuàng)建一個(gè)M×N的數(shù)學(xué)矩陣A M×N：

矩陣式（4）中，任意元素ai j代表的含義具體如下：0—表示空閑點(diǎn)，1—障礙點(diǎn)；2—起始點(diǎn)；3—目標(biāo)點(diǎn)；8—路徑點(diǎn)。假設(shè)此時(shí)矩陣A M×N中為沒有障礙物，則A M×N中的內(nèi)部元素全為0，在柵格矩陣中設(shè)置起始點(diǎn)為A［f］［k］，設(shè)置目標(biāo)點(diǎn)為A［g］［h］，起始點(diǎn)與目標(biāo)點(diǎn)的相對(duì)距離大于2個(gè)元素距離。定義矩陣A M×N中行向量坐標(biāo)增大的方向?yàn)槁窂剿阉鞯腨軸正方向，列向量坐標(biāo)增大的方向?yàn)槁窂剿阉鞯腦軸正方向，設(shè)置起始點(diǎn)路徑搜索的方向?yàn)閅軸正方向。根據(jù)A*算法的啟發(fā)式函數(shù)搜索生成全局路徑，在生成的全局路徑中，將路徑上任意相互正交相鄰3點(diǎn)的正交點(diǎn)去除，得到如式（5）所示包含路徑的矩陣：

假設(shè)修剪的拐點(diǎn)數(shù)為x，根據(jù)起始點(diǎn)A［f］［k］與目標(biāo)點(diǎn)A［g］［h］，得到從起始到目標(biāo)點(diǎn)的總耗費(fèi)為：

若在柵格地圖坐標(biāo)為上存在障礙點(diǎn)，則在矩陣中路徑上設(shè)置障礙點(diǎn)為A M×N[i][j]=1，則表示為如下的矩陣：

根據(jù)具體生成的路徑數(shù)據(jù)，設(shè)置局部路徑的起始點(diǎn)和目標(biāo)點(diǎn)。在生成的路徑上，將AGV當(dāng)前點(diǎn)A M×N[m][n]設(shè)置為起始點(diǎn)，將障礙點(diǎn)的下一點(diǎn)A M×N[s][t]設(shè)置為局部路徑的目標(biāo)點(diǎn)，并根據(jù)局部路徑起始站點(diǎn)與目標(biāo)站點(diǎn)在矩陣中位置確定包含起始點(diǎn)與目標(biāo)站點(diǎn)的局部柵格地圖，具體確定方式如下：

截取橫向的矩陣，設(shè)矩陣E|m-s|×M的表示為：

截取縱向的矩陣，設(shè)矩陣E N×|n-t|的表示為：

則截取后的矩陣為：

此時(shí)調(diào)用D*算法在生成的局部地圖中規(guī)劃局部路徑，設(shè)局部路徑的拐點(diǎn)數(shù)為y，則局部路徑從起始點(diǎn)到目標(biāo)點(diǎn)的耗費(fèi)為：

假設(shè)全局路徑上路徑局部路徑的數(shù)量為z，每個(gè)局部路徑避開路徑上的障礙點(diǎn)數(shù)量為ei，則路徑的總耗費(fèi)值F：

3 應(yīng)用分析與仿真

3.1 應(yīng)用分析

本次測(cè)試方法為使用含有站點(diǎn)坐標(biāo)信息的二維碼標(biāo)簽，根據(jù)空間大小布置二維碼。利用二維碼標(biāo)簽構(gòu)建AGV理想行駛路徑L，路徑中Ln表示為路徑中第n個(gè)二維碼站點(diǎn)坐標(biāo)，表示方法為。調(diào)用A*算法生成一條起始點(diǎn)到目標(biāo)點(diǎn)的路徑L=[L0，...，L n]。遇到障礙物時(shí)調(diào)用D*算法生成局部路徑，假設(shè)為S=[S0，...，S n]，拼接完成后得到全局路徑L'=[L0，...，S0，...，S n，...，L n]。

3.2 路徑仿真

首先設(shè)置路徑的起始站點(diǎn)為<0，0>與目標(biāo)站點(diǎn)<2，4>，并設(shè)置AGV的路徑搜索方式為曼哈頓搜索。AGV根據(jù)起始點(diǎn)與目標(biāo)點(diǎn)相互位置關(guān)系調(diào)用A*算法搜索全局的路徑，并將拐點(diǎn)去除得到全局路徑L=[L0，L1，L3，L3，L4，L5]，當(dāng)行駛到每個(gè)站點(diǎn)時(shí)，實(shí)時(shí)解析路徑的下兩個(gè)站點(diǎn)，此處稱之為“預(yù)備站點(diǎn)”和“目標(biāo)站點(diǎn)”，并檢測(cè)柵格地圖中的目標(biāo)站點(diǎn)是否有障礙物。假設(shè)在路徑上將障礙點(diǎn)設(shè)置為<0，3>。當(dāng)AGV行駛到<0，1>坐標(biāo)點(diǎn)時(shí)，其通過報(bào)站的站點(diǎn)檢測(cè)到目標(biāo)站點(diǎn)有障礙物，調(diào)用D*算法，以<0，2>為起始點(diǎn)，以<1，4>為目標(biāo)點(diǎn)確定局部路徑搜索范圍進(jìn)行局部路徑規(guī)劃，得到局部路徑S=[S0，S1，S2]，其中S0=<0，2>，S1=<1，3>，S2=<1，4>。進(jìn)行拼接得到路徑L'=[L0，L1，S0，S1，S2，L5]，如矩陣（13）所示為進(jìn)行路徑拼接得到的矩陣表示：

根據(jù)在矩陣中規(guī)劃出的路徑站點(diǎn)坐標(biāo)及相互位置關(guān)系得到各個(gè)站點(diǎn)的信息，將規(guī)劃出的路徑信息寫入路徑數(shù)據(jù)文件中，如表1所示。

表1 路徑數(shù)據(jù)文件Tab.1 Path Data File

表中：坐標(biāo)值為站點(diǎn)在矩陣中的所處的位置，目標(biāo)速度為根據(jù)AGV的電機(jī)特性在配置文件中的標(biāo)定值，目標(biāo)方向的設(shè)定原則為：當(dāng)目標(biāo)點(diǎn)的坐標(biāo)為（x，y），目標(biāo)點(diǎn)的下一點(diǎn)坐標(biāo)為（x，y+1）,則目標(biāo)點(diǎn)的方向?yàn)?，對(duì)應(yīng)的方向角度為90°；當(dāng)目標(biāo)點(diǎn)的下一點(diǎn)為（x-1，y）,則目標(biāo)點(diǎn)的方向?yàn)?，對(duì)應(yīng)的方向角度為180°；當(dāng)目標(biāo)點(diǎn)的下一點(diǎn)為（x，y-1）,則目標(biāo)點(diǎn)的方向?yàn)?，對(duì)應(yīng)的方向角度為270°；當(dāng)目標(biāo)點(diǎn)的下一點(diǎn)坐標(biāo)為（x+1，y）,則目標(biāo)點(diǎn)方向?yàn)?，對(duì)應(yīng)的方向角度為0°。

4 試驗(yàn)

4.1 地圖構(gòu)建

AGV根據(jù)試驗(yàn)場(chǎng)地大小在系統(tǒng)界面中創(chuàng)建柵格地圖，同時(shí)在實(shí)際場(chǎng)景地面上按照比例張貼二維碼柵格，試驗(yàn)現(xiàn)場(chǎng)的地圖構(gòu)建，如圖2所示。

圖2 測(cè)試現(xiàn)場(chǎng)Fig.2 Test Site

試驗(yàn)的AGV類型為雙輪差速驅(qū)動(dòng)，有2個(gè)驅(qū)動(dòng)輪和4個(gè)萬向隨動(dòng)輪，導(dǎo)航方式為二維碼和慣性導(dǎo)航復(fù)合的形式，車載傳感器有前后6個(gè)光電開關(guān)和6個(gè)碰撞開關(guān)，AGV中間底部安裝有工業(yè)攝像機(jī)用于識(shí)別車體相對(duì)二維碼地標(biāo)位置。

4.2 復(fù)合路徑規(guī)劃測(cè)試

地圖中AGV的起始站點(diǎn)和目標(biāo)站點(diǎn)分別設(shè)置為<0，0>和<2，4>，測(cè)試生成路徑試驗(yàn)AGV軌跡，如圖3所示。

圖3 AGV測(cè)試路徑Fig.3 AGV Test Path

根據(jù)地圖的起始點(diǎn)與目標(biāo)點(diǎn)坐標(biāo)，調(diào)用A*算法曼哈頓啟發(fā)方式生成的全局路徑，如圖3（a）所示，并在拐點(diǎn)<0，4>處修剪為光滑的曲線路徑。當(dāng)在<0，3>存在障礙時(shí)，AGV在行駛到<0，1>時(shí)檢測(cè)到目標(biāo)站點(diǎn)存在障礙，將<0，2>設(shè)置為局部路徑的起始點(diǎn)，將<1，4>設(shè)置為局部路徑目標(biāo)點(diǎn)，進(jìn)行路徑拼接得到路徑，如圖3（b）所示。

通過AGV行駛軌跡可以看出，當(dāng)AGV在目標(biāo)站點(diǎn)檢測(cè)到障礙物時(shí)，確定局部路徑起始點(diǎn)為障礙物的前一站點(diǎn)，確定局部路徑目標(biāo)站點(diǎn)為障礙物的下一站點(diǎn)，并在存在拐點(diǎn)處的局部路徑修剪為光滑的曲線路徑。試驗(yàn)結(jié)果表明，所提出的方法實(shí)現(xiàn)AGV在動(dòng)態(tài)障礙場(chǎng)景中自主規(guī)避障礙的能力。

5 結(jié)論

（1）根據(jù)障礙物位置和全局規(guī)劃的路徑確定局部地圖搜索范圍，使得局部路徑規(guī)劃效率更高和避障的實(shí)時(shí)性。（2）所提的復(fù)合路徑規(guī)劃方法，可應(yīng)用于多種導(dǎo)航方式，例如：SLAM導(dǎo)航、激光導(dǎo)航、二維碼導(dǎo)航等。（3）所研究的方法目前只能在柵格地圖中實(shí)現(xiàn)，若地圖沒有采用柵格建模，則只能將地圖轉(zhuǎn)換為柵格地圖才能采用，針對(duì)此問題，我們將繼續(xù)研究解決這種問題。