一種改進(jìn)工業(yè)自動(dòng)導(dǎo)引車(chē)路徑規(guī)劃算法

鄭 亮， 孫龍龍， 陳 雙

(1.哈爾濱工業(yè)大學(xué)蕪湖機(jī)器人產(chǎn)業(yè)技術(shù)研究院， 蕪湖 241000； 2.蕪湖哈特機(jī)器人產(chǎn)業(yè)技術(shù)研究院有限公司， 蕪湖 241000)

路徑規(guī)劃問(wèn)題是移動(dòng)機(jī)器人領(lǐng)域中的一個(gè)熱點(diǎn)問(wèn)題，是指在移動(dòng)機(jī)器人工作環(huán)境中，依據(jù)距離最短、時(shí)間最優(yōu)、能效最高等一條或多條評(píng)價(jià)指標(biāo)，規(guī)劃出一條從起始位置到目標(biāo)位置的安全無(wú)碰撞路徑[1-2]。高質(zhì)量的路徑規(guī)劃是確保移動(dòng)機(jī)器人完成既定任務(wù)的關(guān)鍵所在[3]。

移動(dòng)機(jī)器人路徑規(guī)劃的核心是路徑規(guī)劃算法，而構(gòu)建移動(dòng)機(jī)器人運(yùn)行環(huán)境模型是路徑規(guī)劃算法的前提[4]。目前較為常用的環(huán)境建模方法包括柵格法[5]、可視圖法[6]、Voronoi圖法[7]等。其中，柵格法由于其建模簡(jiǎn)單，構(gòu)建的環(huán)境柵格地圖對(duì)不規(guī)則障礙物的表示能力較強(qiáng)，因而被廣泛應(yīng)用于多種傳統(tǒng)或智能路徑規(guī)劃算法中。常用的基于柵格地圖的路徑規(guī)劃算法主要有：Dijkstra算法[8]、A*算法[9]及D*算法[10]等。相比于其他柵格地圖下的路徑規(guī)劃算法，A*算法計(jì)算量相對(duì)較小、路徑搜索時(shí)間較短且規(guī)劃路徑相對(duì)較優(yōu)[11]。

A*算法是一種啟發(fā)式路徑規(guī)劃算法，一種在靜態(tài)網(wǎng)絡(luò)中求解最優(yōu)路徑的有效搜索算法[12]。然而，傳統(tǒng)A*算法規(guī)劃路徑中存在折線多、累積轉(zhuǎn)角大且距離障礙物近等問(wèn)題。因此，文獻(xiàn)[13]提出了一種平滑A*算法，該算法在傳統(tǒng)A*算法規(guī)劃路徑的基礎(chǔ)上，通過(guò)遍歷所有路徑節(jié)點(diǎn)，并刪除路徑節(jié)點(diǎn)間的冗余節(jié)點(diǎn)，以建立平滑A*模型。文獻(xiàn)[14]提出按較小的分割步長(zhǎng)對(duì)傳統(tǒng)A*算法規(guī)劃路徑進(jìn)行分割，在得到一系列路徑節(jié)點(diǎn)后，通過(guò)判斷節(jié)點(diǎn)間連線是否經(jīng)過(guò)障礙物，以清除其中的冗余節(jié)點(diǎn)。然而，以上算法僅針對(duì)A*算法原有路徑節(jié)點(diǎn)進(jìn)行處理，對(duì)應(yīng)路徑長(zhǎng)度和轉(zhuǎn)折角度減小幅度有限，并且未能解決算法規(guī)劃路徑距離障礙物較近的問(wèn)題。

為此，通過(guò)引入Floyd算法生成路徑節(jié)點(diǎn)集合，同時(shí)基于A*算法對(duì)相鄰節(jié)點(diǎn)進(jìn)行路徑規(guī)劃，并將生成路徑進(jìn)行拼接，以解決傳統(tǒng)A*算法規(guī)劃路徑距離障礙物較近的問(wèn)題，同時(shí)，針對(duì)傳統(tǒng)A*算法轉(zhuǎn)角較多、彎曲度較大的問(wèn)題，引入貝塞爾曲線對(duì)拼接路徑進(jìn)行平滑處理，以獲取全局路徑。

1 A*路徑規(guī)劃算法

A*算法是一種典型的啟發(fā)式搜索算法，基于A*算法實(shí)現(xiàn)AGV路徑規(guī)劃的過(guò)程通?？煞譃閮刹糠郑菏紫?，引入柵格法對(duì)自動(dòng)導(dǎo)引車(chē)(automated guided vehicle,AGV)實(shí)際運(yùn)行環(huán)境進(jìn)行建模；其次，基于已構(gòu)建環(huán)境柵格地圖模型確定AGV起始點(diǎn)和目標(biāo)點(diǎn)，并引入A*算法實(shí)現(xiàn)路徑規(guī)劃。

1.1 環(huán)境柵格地圖構(gòu)建

AGV運(yùn)行環(huán)境柵格地圖構(gòu)建是A*算法實(shí)現(xiàn)路徑規(guī)劃的重要前提，其基本思想是通過(guò)將有限的規(guī)劃空間劃分為若干個(gè)大小相同并賦有二進(jìn)制數(shù)的柵格單元[15]，劃分后的工作空間如圖1所示。其中，柵格信息直接對(duì)應(yīng)環(huán)境地圖信息，白色柵格表示可通行自由區(qū)域，灰色柵格表示障礙物區(qū)域。當(dāng)AGV處于柵格地圖中任意柵格位置時(shí)，對(duì)應(yīng)下一移動(dòng)位置為該柵格相鄰8個(gè)柵格。

圖1 柵格地圖模型Fig.1 Grid map model

假設(shè)AGV二維工作空間長(zhǎng)為L(zhǎng)，寬為W，通過(guò)將此工作空間劃分為大小相同的R×S個(gè)柵格，若以Ω表示二維工作空間全體柵格信息，則有

Ω=∑Nij,i∈[1,R],j∈[1,S]

(1)

式(1)中：Nij表示柵格地圖中單個(gè)柵格信息：若Nij=0，則該柵格對(duì)應(yīng)可通行自由區(qū)域；若Nij=1，則該柵格對(duì)應(yīng)為障礙物區(qū)域；R表示橫向柵格個(gè)數(shù)，S表示縱向柵格個(gè)數(shù)。

1.2 路徑規(guī)劃

基于已構(gòu)建的環(huán)境柵格地圖模型，在確定算法起始點(diǎn)和目標(biāo)點(diǎn)后，A*算法即從起始點(diǎn)出發(fā)，通過(guò)評(píng)價(jià)函數(shù)確定算法搜索方向，同時(shí)對(duì)擴(kuò)展的每個(gè)節(jié)點(diǎn)進(jìn)行檢測(cè)，并選取其中代價(jià)最小的節(jié)點(diǎn)作為下一擴(kuò)展節(jié)點(diǎn)，循環(huán)重復(fù)以上過(guò)程直至搜索到目標(biāo)點(diǎn)，從而生成最終路徑。由于路徑上每個(gè)節(jié)點(diǎn)都是具有最小代價(jià)值的節(jié)點(diǎn)，因而A*最終生成路徑整體代價(jià)最小。A*算法的評(píng)價(jià)函數(shù)f(n)可表示為

f(n)=g(n)+h(n)

(2)

式(2)中：f(n)表示從起始點(diǎn)經(jīng)由任意節(jié)點(diǎn)n到達(dá)目標(biāo)點(diǎn)的評(píng)價(jià)值；g(n)表示在狀態(tài)空間中從起始點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價(jià)值；h(n)表示從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的啟發(fā)式估計(jì)代價(jià)值。

采用歐幾里得距離作為啟發(fā)式函數(shù)，若以(xn,yn)表示節(jié)點(diǎn)n的坐標(biāo)，(xd,yd)表示目標(biāo)點(diǎn)d的坐標(biāo)，則有

(3)

A*算法在朝向目標(biāo)點(diǎn)搜索的過(guò)程中，將對(duì)搜索方向附近的相關(guān)節(jié)點(diǎn)進(jìn)行評(píng)價(jià)。在抵達(dá)狀態(tài)空間某一節(jié)點(diǎn)時(shí)，A*算法將其周?chē)?jié)點(diǎn)添加至OpenList列表，并從中選取代價(jià)值最小的節(jié)點(diǎn)作為下一擴(kuò)展節(jié)點(diǎn)，將其添加到ClosedList列表后，重復(fù)執(zhí)行以上過(guò)程，直至目標(biāo)點(diǎn)添加至OpenList列表中。

A*算法路徑搜索過(guò)程如圖2所示。通過(guò)啟發(fā)式評(píng)價(jià)函數(shù)指引搜索方向，A*算法能夠快速搜索到從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑，但該路徑存在轉(zhuǎn)角較大，并且距離障礙物較近等問(wèn)題。

綠色柵格表示算法起始點(diǎn)；紅色柵格表示算法目標(biāo)點(diǎn)； 黃色折線為A*算法最終搜索路徑圖2 A*算法路徑規(guī)劃結(jié)果Fig.2 Path planning results of A* algorithm

2 改進(jìn)路徑規(guī)劃算法

基于已構(gòu)建的AGV運(yùn)行環(huán)境柵格地圖確定算法起始點(diǎn)rs和目標(biāo)點(diǎn)re后，首先通過(guò)在柵格地圖中設(shè)置路徑關(guān)鍵節(jié)點(diǎn)，同時(shí)引入Floyd算法進(jìn)行最短路徑規(guī)劃，并輸出路徑節(jié)點(diǎn)集合；其次，將起始點(diǎn)rs和目標(biāo)點(diǎn)re計(jì)入路徑節(jié)點(diǎn)集合，并基于A*算法對(duì)集合中相鄰節(jié)點(diǎn)進(jìn)行路徑規(guī)劃，同時(shí)對(duì)生成路徑進(jìn)行拼接；最后，引入貝塞爾曲線對(duì)拼接路徑進(jìn)行平滑處理，以生成最終路徑。

2.1 Floyd最短路徑算法

Floyd算法是典型的多源最短路徑算法，又稱為插點(diǎn)法，是一種用于尋找給定加權(quán)圖中多源點(diǎn)之間最短路徑的算法[16]。算法基本思想為：直接在圖的帶權(quán)鄰接矩陣中以插入節(jié)點(diǎn)的方式依次構(gòu)造v個(gè)矩陣D(1),D(2),…,D(v)，最終生成的矩陣D(v)即為圖的距離矩陣，同時(shí)求取插入點(diǎn)矩陣R(1),R(2),…,R(v)以獲得兩點(diǎn)間的最短路徑。

(4)

(5)

通過(guò)不斷迭代插入節(jié)點(diǎn)vk，即可求得最短距離矩陣D(n)和最短路徑矩陣R(n)，其中R(n)保存了任意兩個(gè)節(jié)點(diǎn)間最短路徑，即插入點(diǎn)。因而，通過(guò)對(duì)最短路徑矩陣進(jìn)行回溯，即可得到從節(jié)點(diǎn)vi到節(jié)點(diǎn)vj的路徑節(jié)點(diǎn)集合。假設(shè)基于路徑關(guān)鍵節(jié)點(diǎn)集合v1,v2,…,vn，生成最短路徑節(jié)點(diǎn)集合為r1,r2,…,rm。

2.2 貝塞爾曲線

通過(guò)對(duì)最短路徑節(jié)點(diǎn)集合r1,r2,…,rm進(jìn)行處理，并將起始點(diǎn)rs和目標(biāo)點(diǎn)re計(jì)入路徑節(jié)點(diǎn)集合后，引入A*算法對(duì)集合中相鄰節(jié)點(diǎn)進(jìn)行路徑規(guī)劃，并對(duì)生成路徑進(jìn)行拼接。然而，柵格地圖下的A*算法拼接路徑仍存在折線較多、轉(zhuǎn)角較大的問(wèn)題，因而AGV實(shí)際運(yùn)行效率相對(duì)較低，亟需對(duì)拼接路徑進(jìn)行平滑處理。本文通過(guò)引入貝塞爾曲線以實(shí)現(xiàn)對(duì)A*算法拼接路徑的平滑處理。

貝塞爾曲線是由伯恩斯坦多項(xiàng)式發(fā)展而來(lái)，由于其控制簡(jiǎn)單、圖形描述能力較強(qiáng)，且位置點(diǎn)連接平滑，因而在工業(yè)上得到了較為廣泛的應(yīng)用[17]。假設(shè)P(0)和P(1)分別為貝塞爾曲線起點(diǎn)和終點(diǎn)，P(u)表示貝塞爾曲線運(yùn)動(dòng)控制點(diǎn)，則n次貝塞爾曲線可表示為

(6)

式(6)中：u表示獨(dú)立變量；P(i)表示位置點(diǎn)，對(duì)應(yīng)所有位置點(diǎn)P(i)構(gòu)成了一個(gè)特征多邊形；Bi,n(u)為n次伯恩斯坦多項(xiàng)式，滿足：

(7)

式(7)中：i∈{0,1,…,n}。

對(duì)n次貝塞爾曲線表達(dá)式進(jìn)行求導(dǎo)，可得

(8)

貝塞爾曲線具有以下性質(zhì)：①起始于P(0)，終止于P(1)；②曲線起點(diǎn)和終點(diǎn)即為特征多邊形的起點(diǎn)和終點(diǎn)，且曲線起點(diǎn)和終點(diǎn)的切線與特征多邊形的起始線和結(jié)尾線方向一致；③對(duì)應(yīng)特征多邊形一旦確定，貝塞爾曲線的形狀也唯一確定，其不依賴于選取的參考系。

2.3 改進(jìn)路徑規(guī)劃算法步驟

步驟1基于柵格法構(gòu)建AGV運(yùn)行環(huán)境柵格地圖模型，并確定算法起始點(diǎn)rs，目標(biāo)點(diǎn)re。

步驟4通過(guò)對(duì)Floyd算法生成的最短路徑矩陣R={R(1),R(2),…,R(n)}進(jìn)行回溯，即可獲取任意從節(jié)點(diǎn)vk到節(jié)點(diǎn)vj的最短路徑節(jié)點(diǎn)集合，假設(shè)為r1,r2，…,rm。

步驟5將算法起始點(diǎn)rs和目標(biāo)點(diǎn)re計(jì)入節(jié)點(diǎn)集合中，處理后得rs、r1,r2，…,rm、re。

步驟6基于A*算法對(duì)集合rs、r1,r2，…,rm、re中相鄰節(jié)點(diǎn)進(jìn)行路徑規(guī)劃，并對(duì)規(guī)劃路徑進(jìn)行拼接處理。

步驟7引入貝塞爾曲線對(duì)A*算法拼接路徑進(jìn)行平滑處理。改進(jìn)路徑規(guī)劃算法流程圖如圖3所示。

圖3 改進(jìn)路徑規(guī)劃算法流程圖Fig.3 The flow chart of the improved path planning algorithm

3 實(shí)驗(yàn)與分析

為了驗(yàn)證本文算法的性能，選取兩種不同的實(shí)驗(yàn)環(huán)境，并與傳統(tǒng)A*算法進(jìn)行了對(duì)比實(shí)驗(yàn)，對(duì)應(yīng)的環(huán)境柵格地圖分別如圖4、圖5所示。

圖4 長(zhǎng)廊環(huán)境柵格地圖Fig.4 The grid map ofthe promenade environment

圖5 室內(nèi)環(huán)境柵格地圖Fig.5 The grid map of the indoor environment

圖4為建立的長(zhǎng)廊環(huán)境對(duì)應(yīng)柵格地圖，其中灰線部分表示環(huán)境障礙物輪廓，空白部分對(duì)應(yīng)為當(dāng)前環(huán)境中空閑區(qū)域；圖5表示存在較多障礙物的室內(nèi)環(huán)境柵格地圖，將以此驗(yàn)證本文算法的路徑規(guī)劃效果。運(yùn)行程序電腦配置為：CPU為i5處理器，8 GB內(nèi)存，主頻2.5 GHz，Windows 10操作系統(tǒng)。

3.1 算法性能評(píng)估

由于在實(shí)際運(yùn)行過(guò)程中，應(yīng)當(dāng)充分考慮AGV的尺寸以免AGV碰撞環(huán)境中的障礙物，因此首先根據(jù)相應(yīng)的膨脹半徑對(duì)柵格地圖中障礙物所處的柵格進(jìn)行膨脹處理，其中長(zhǎng)廊環(huán)境對(duì)應(yīng)的膨脹地圖如圖6所示。

圖6 長(zhǎng)廊環(huán)境柵格膨脹地圖Fig.6 The inflated grid map of the promenade environment

為了驗(yàn)證本文算法的相關(guān)性能，通過(guò)在長(zhǎng)廊環(huán)境膨脹地圖中設(shè)置起始點(diǎn)S和目標(biāo)點(diǎn)E，并將本文算法與傳統(tǒng)A*算法以及Floyd+A*算法進(jìn)行路徑搜索對(duì)比實(shí)驗(yàn)，其中傳統(tǒng)A*算法搜索路徑如圖7(a)所示，F(xiàn)loyd+A*算法生成路徑如圖7(b)所示，本文算法搜索路徑如圖7(c)所示。

圖7 不同算法搜索路徑Fig.7 The research path of different algorithms

如圖7(a)所示，基于給定起始點(diǎn)S和目標(biāo)點(diǎn)E，傳統(tǒng)A*算法搜索生成的路徑在轉(zhuǎn)彎處彎曲度較大，且距離障礙物較近，同時(shí)該路徑經(jīng)過(guò)柵格個(gè)數(shù)為1 121。而基于Floyd算法進(jìn)行最短路徑規(guī)劃，生成路徑節(jié)點(diǎn)集合，并利用A*算法對(duì)處理后的路徑節(jié)點(diǎn)集合進(jìn)行路徑規(guī)劃，最終生成的路徑如圖7(b)所示，該路徑經(jīng)過(guò)柵格個(gè)數(shù)為1 016，有效減少了AGV運(yùn)行時(shí)間，且規(guī)劃的路徑能有效避開(kāi)障礙物，改進(jìn)了傳統(tǒng)A*算法中“貼墻走”的問(wèn)題，但仍存在路徑轉(zhuǎn)折點(diǎn)彎曲度較高，AGV實(shí)際運(yùn)行效率較差的問(wèn)題。而本文改進(jìn)算法通過(guò)引入貝塞爾曲線進(jìn)行平滑處理，最終生成的平滑路徑在轉(zhuǎn)折點(diǎn)處彎曲度較小，更加符合AGV的運(yùn)行規(guī)則，AGV實(shí)際運(yùn)行效率更高。

3.2 室內(nèi)環(huán)境改進(jìn)算法路徑規(guī)劃分析

根據(jù)設(shè)置的膨脹半徑對(duì)圖5室內(nèi)環(huán)境柵格地圖中障礙物所處的柵格進(jìn)行相應(yīng)膨脹處理，并基于環(huán)境中給定的起始點(diǎn)S和目標(biāo)點(diǎn)E，運(yùn)用傳統(tǒng)A*算法進(jìn)行路徑搜索，最終搜索到的路徑如圖8(a)所示?；谙嗤鹗键c(diǎn)S和目標(biāo)點(diǎn)E，本文算法規(guī)劃的路徑如圖8(b)所示。

圖8 室內(nèi)環(huán)境下傳統(tǒng)A*算法、本文算法搜索路徑Fig.8 The research path of the traditional A* algorithm and the proposed algorithm under the indoor environment

如圖8(b)所示，存在較多障礙物的室內(nèi)環(huán)境中，基于給定的起始點(diǎn)S和目標(biāo)點(diǎn)E，本文算法能夠完全避開(kāi)環(huán)境中的障礙物，并規(guī)劃出合理的路徑，且生成的路徑較為平滑，同時(shí)在轉(zhuǎn)折點(diǎn)處彎曲度更小，更加符合AGV運(yùn)行規(guī)則，AGV實(shí)際運(yùn)行效率更高。

3.3 搜索時(shí)間分析

為了驗(yàn)證改進(jìn)路徑規(guī)劃算法的路徑搜索時(shí)間效率，針對(duì)兩種不同環(huán)境進(jìn)行了5次路徑搜索實(shí)驗(yàn)，對(duì)應(yīng)的搜索時(shí)間如表1所示。

表1 各種路徑規(guī)劃算法路徑搜索時(shí)間比較

如表1所示，針對(duì)兩種不同環(huán)境，在給定起始點(diǎn)S和目標(biāo)點(diǎn)E后，相比于A*算法，F(xiàn)loyd+A*算法路徑搜索時(shí)間相對(duì)較短，路徑搜索較快。然而，由于本文算法增添了貝塞爾路徑平滑處理，因而相對(duì)于Floyd+A*算法，本文算法對(duì)應(yīng)的路徑搜索時(shí)間相對(duì)較長(zhǎng)，但相比A*算法，本文方法路徑搜索時(shí)間整體相對(duì)較短，路徑搜索較快。

4 結(jié)論

在A*算法的基礎(chǔ)上，提出了一種改進(jìn)路徑規(guī)劃算法。該算法通過(guò)在全局地圖中設(shè)置路徑關(guān)鍵節(jié)點(diǎn)，繼而引入Floyd算法進(jìn)行最短路徑規(guī)劃，并輸出路徑節(jié)點(diǎn)集合，同時(shí)基于A*算法對(duì)集合中相鄰節(jié)點(diǎn)進(jìn)行路徑搜索，并對(duì)生成路徑進(jìn)行拼接，從而解決A*算法規(guī)劃路徑距離障礙物較近的問(wèn)題。其次，針對(duì)傳統(tǒng)A*算法轉(zhuǎn)角較多、彎曲度較大的問(wèn)題，引入貝塞爾曲線對(duì)拼接路徑進(jìn)行平滑處理，以獲取全局平滑路徑。實(shí)驗(yàn)結(jié)果表明本文算法規(guī)劃的路徑轉(zhuǎn)彎更少、彎曲度更小、搜索時(shí)間更短且能完全避開(kāi)障礙物行走，更加符合工業(yè)AGV的應(yīng)用環(huán)境。