基于ROS與融合算法的AGV路徑規(guī)劃研究

馮浩然,吳瑞明,傅陽,陳凱旋,程強,李陽

(浙江科技學(xué)院機械與能源工程學(xué)院，浙江杭州 310023)

0 前言

隨著人工智能的應(yīng)用和發(fā)展，傳統(tǒng)生產(chǎn)運輸方式將逐步被淘汰，自動引導(dǎo)車(Automated Guided Vehicle，AGV)的應(yīng)用與開發(fā)得到了越來越多的關(guān)注，并應(yīng)用于各個領(lǐng)域。AGV在智能制造業(yè)中占有極其重要的地位，是現(xiàn)代智能倉儲和物流系統(tǒng)的關(guān)鍵組成部分。AGV具有自動化程度高、柔性化程度高以及可靠性強等優(yōu)點，作為一種用于物流運輸?shù)囊苿訖C器人，它已廣泛應(yīng)用于物流倉儲及煙草等行業(yè)，而其路徑規(guī)劃問題也已成為當(dāng)下研究的熱點。

AGV已廣泛應(yīng)用于各種工程、運輸和物流等領(lǐng)域，而路徑規(guī)劃在AGV的運動控制中極為關(guān)鍵，且路徑規(guī)劃算法的效率直接影響移動機器人的尋路效率。路徑規(guī)劃可以分為全局和局部規(guī)劃，取決于環(huán)境和目標(biāo)的性質(zhì)。針對靜態(tài)環(huán)境，目前已經(jīng)有許多算法能夠解決全局路徑規(guī)劃問題，如動態(tài)窗口算法、A算法、D算法、蟻群算法、遺傳算法等。遺傳算法在尋找全局最優(yōu)解的過程中，是一個隨機搜索的過程，當(dāng)節(jié)點增多時，搜索時間會變得較長。而A算法是一種啟發(fā)式搜索算法，也是目前應(yīng)用最廣泛的算法之一，實現(xiàn)容易，搜索效率高，具有一定方向性。傳統(tǒng)的A算法也存在轉(zhuǎn)折點過多、節(jié)點冗余、避障安全性考慮不充分等問題。傳統(tǒng)A算法傾向于關(guān)注最短路徑，通常產(chǎn)生一條接近障礙物的路徑。 而在實際應(yīng)用中，AGV可能會偏離計劃軌跡，這是由傳感器測量誤差和車輪的滑移引起的。選用動態(tài)窗口法進(jìn)行路徑規(guī)劃雖然可以使AGV具有優(yōu)良的避障安全性，路徑平滑，但很容易陷入局部最優(yōu)，無法以最短距離到達(dá)目標(biāo)點。針對上述問題，對傳統(tǒng)A算法和動態(tài)窗口法進(jìn)行改進(jìn)并融合。利用A算法進(jìn)行全局路徑搜索后，去除多余節(jié)點和改進(jìn)安全避障距離；結(jié)合改進(jìn)動態(tài)窗口法完成局部路徑規(guī)劃，為AGV規(guī)劃出一條避障安全性好、平滑度高、具有實時性的最優(yōu)路徑。

1 改進(jìn)A*算法

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

A算法因簡單高效而被廣泛應(yīng)用。它是一種典型的啟發(fā)式搜索算法，也是一種圖形搜索算法，通過評價函數(shù)來確定搜索方向，并將啟發(fā)式信息用于評價函數(shù)。這不僅利于找到最優(yōu)路徑，而且可減少搜索時間。該算法從起始節(jié)點開始構(gòu)建路徑，不斷將當(dāng)前節(jié)點的周圍柵格中心點加入OpenList，且不斷選取OpenList列表中值最小的點作為當(dāng)前節(jié)點，直到當(dāng)前節(jié)點的周圍節(jié)點出現(xiàn)目標(biāo)節(jié)點或者OpenList為空時停止。該算法公式為

(,)=(,)+(,)

(1)

距離函數(shù)(,)一般取歐氏距離或曼哈頓距離，為提高算法準(zhǔn)確率，文中采用歐氏距離：

(2)

其中：、分別為起點的坐標(biāo)與坐標(biāo)；、分別為目標(biāo)點的坐標(biāo)與坐標(biāo)。

1.2 設(shè)置OpenList訪問時間閾值

在傳統(tǒng)A算法中，當(dāng)當(dāng)前節(jié)點擴(kuò)展子節(jié)點時，每次都會擴(kuò)展所有候選節(jié)點。這種方式在復(fù)雜的環(huán)境中可能會存在無效搜索，影響算法運行效率。因此，在傳統(tǒng)A算法基礎(chǔ)上加以改進(jìn)，避免陷入局部最優(yōu)解，以真正規(guī)劃全局路徑，并提高搜索能力。具體改進(jìn)方法：每次檢測OpenList中的第一個插入節(jié)點，判斷它是否在經(jīng)過一定時間閾值后被擴(kuò)展，如果沒有，則該節(jié)點將被視為最高優(yōu)先級擴(kuò)展。

1.3 Floyd算法優(yōu)化

Floyd算法指采用動態(tài)規(guī)劃的思想來搜索最短路徑，用于求解兩點之間的最優(yōu)距離問題。采用Floyd算法和A算法相結(jié)合的改進(jìn)策略，可以減小路徑長度，滿足應(yīng)用需求。Floyd算法原理如圖1所示。

圖1 Floyd算法原理示意

設(shè)(,)為點與點之間的距離，(,)為點到點的路線，而在圖1中，如果與之間存在障礙，則設(shè)(,)=∞，與之間無法直接連接通行。

設(shè)點是點與點之間的計劃節(jié)點，如果:

(,)+(,)=(,)

(3)

則：

(,)=(,)+(,)

(4)

(,)=→→

(5)

設(shè)點是點與點之間的計劃節(jié)點，如果：

(,)+(,)<(,)+(,)

(6)

則：

(,)=(,)+(,)

(7)

(,)=→→

(8)

刪除點，優(yōu)化路徑為從點到點再到點。采用Floyd算法優(yōu)化A算法規(guī)劃的路徑，可以去除冗余點，進(jìn)一步優(yōu)化路徑。

1.4 安全避障路徑的改進(jìn)

AGV運動過程中可能會與障礙物發(fā)生碰撞，對此進(jìn)行算法優(yōu)化。若相鄰節(jié)點和當(dāng)前節(jié)點的父節(jié)點關(guān)于當(dāng)前節(jié)點的方向相同，則減小(,)值，反之則增加(,)值。在擴(kuò)展子節(jié)點的過程中，優(yōu)先擴(kuò)展水平方向上的節(jié)點，隨后判斷水平方向上是否存在障礙物，如果存在，則不擴(kuò)展與障礙物相鄰的對角線節(jié)點；如果不存在，則擴(kuò)展對角線方向節(jié)點。安全避障路徑生成策略的流程如圖2所示。

圖2 避障優(yōu)化流程

2 動態(tài)窗口法

在倉儲物流環(huán)境中，室內(nèi)大多數(shù)物體按照一定設(shè)計放置，其位置已知，但存在一些移動物體如工人，因此很難獲取完整環(huán)境信息。為避免撞傷人，保證AGV的安全性，在全局路徑規(guī)劃的基礎(chǔ)上，采用動態(tài)窗口法進(jìn)行局部路徑規(guī)劃。

由于動態(tài)窗口法具有符合AGV的運動特征、靈活性強等優(yōu)點，動態(tài)窗口法成為動態(tài)環(huán)境下局部避障的主要算法。在AGV運動過程中，動態(tài)窗口算法實時預(yù)測AGV的速度與狀態(tài)空間，獲取其多組預(yù)測時間內(nèi)的可行運動軌跡，再根據(jù)設(shè)計的評價函數(shù)選取最優(yōu)運動軌跡的線速度與角速度，完成路徑規(guī)劃，從而獲得一條更安全且平滑的最優(yōu)局部路徑。

2.1 AGV運動模型

動態(tài)窗口算法將AGV的位置控制轉(zhuǎn)換為速度控制。在利用速度模式對AGV運動軌跡進(jìn)行預(yù)測時，首先需要對機器人的運動模型進(jìn)行分析，通過對AGV的速度進(jìn)行空間采樣，模擬出其運動軌跡。AGV的線速度和角速度的變化能夠反映出AGV的運動狀態(tài)，(,)代表AGV在世界坐標(biāo)系下的平移速度與角速度，表征其軌跡。通過評價函數(shù)在所有可行軌跡里選取最佳軌跡，在采樣周期Δ內(nèi)，假設(shè)AGV作勻速直線運動，則其運動模型為

(9)

2.2 AGV速度采樣

在速度搜索空間中,根據(jù)AGV及其環(huán)境所得到的一定范圍內(nèi)的無窮多組(,)對采樣速度范圍進(jìn)行約束，其限制因素可分為3種：AGV本體速度約束、加速度約束以及障礙物約束。

AGV速度約束:

={(,)|∈[,],∈[,]}

(10)

AGV加減速度約束。動態(tài)窗口下的采樣周期內(nèi)，由于AGV加速度受電機力矩限制，AGV受到該限制所帶來的最大、最小速度限制：

={(,)|∈[-Δ,+Δ],∈[-Δ,+Δ]}

(11)

其中：和分別為當(dāng)前時刻的線速度和角速度；和分別為當(dāng)前時刻的最大速度和最大加速度；和分別為當(dāng)前時刻的最小速度和最大減速度。

AGV制動約束。考慮到AGV安全性，需要保證AGV不與障礙物發(fā)生碰撞，則在局部路徑規(guī)劃時的最大減速度條件下，其速度約束為

(12)

式中：(,)是(,)對應(yīng)軌跡中離障礙物的最近距離。

2.3 AGV評價函數(shù)

對于速度空間內(nèi)若干組可行的采樣速度，設(shè)計使AGV盡可能規(guī)避障礙物及最短時間內(nèi)到達(dá)指定點的評

價函數(shù)，以選取最優(yōu)路徑軌跡。設(shè)計的評價函數(shù)為

(,)=(,)+(,)+(,)

(13)

其中：(,)為當(dāng)前速度方向與目標(biāo)位置之間的偏差角的評價函數(shù)；(,)為速度軌跡中與障礙物之間的最短距離；(,)為當(dāng)前軌跡速度大小的評價函數(shù);、、分別為各項的加權(quán)系數(shù)。

3 融合算法

利用傳統(tǒng)動態(tài)窗口算法進(jìn)行規(guī)劃時，如果沒有預(yù)先進(jìn)行全局路徑規(guī)劃而只有指定位置方向的指引，則在障礙物較多情況下，運動過程中容易陷入局部最優(yōu)，導(dǎo)致規(guī)劃軌跡無法達(dá)到全局最優(yōu)。而A算法可以得到全局最優(yōu)解，故將兩者融合進(jìn)行路徑規(guī)劃，充分發(fā)揮兩算法的優(yōu)點，實現(xiàn)路徑長度、平滑度和安全性的全方面優(yōu)化，提升動態(tài)規(guī)劃路徑的全局最優(yōu)性。

3.1 環(huán)境模型描述

環(huán)境建模采用柵格法，如圖3所示，利用單位柵格對地圖進(jìn)行劃分，形成正方形柵格地圖，并根據(jù)實際環(huán)境將AGV可自由移動區(qū)域定為白色柵格，障礙區(qū)域定為黑色柵格，如圖3所示。

圖3 環(huán)境建模柵格圖

3.2 仿真實驗

為驗證所提出的融合算法在復(fù)雜環(huán)境中進(jìn)行路徑規(guī)劃的有效性，進(jìn)行仿真對比驗證。為充分體現(xiàn)對比效果，設(shè)AGV的最大線速度為1 m/s，最大角速度為20.0 rad/s，最大線加速度為0.2 m/s，最大角加速度為50.0 rad/s，線速度為0.01 m/s，角速度為1 rad/s，周期為3.5 s，各算法中設(shè)定的AGV運行參數(shù)相同。實驗環(huán)境為運行內(nèi)存為16 GB的64位WIN10操作系統(tǒng)，實驗平臺為MATLAB2019a。

在建立的柵格模型中，假設(shè)AGV路徑規(guī)劃中的起點坐標(biāo)為(30,30) mm、終點坐標(biāo)為(8,2) mm。在靜態(tài)環(huán)境中運行傳統(tǒng)A算法，結(jié)果如圖4所示。在靜態(tài)環(huán)境中運行改進(jìn)A算法，如圖5所示，成功地完成了靜態(tài)全局路徑規(guī)劃。為驗證動態(tài)環(huán)境中的路徑規(guī)劃，在靜態(tài)環(huán)境運行結(jié)束的基礎(chǔ)上，設(shè)置黃色柵格的臨時動態(tài)障礙物。圖6所示為AGV局部動態(tài)環(huán)境下運行后的路徑規(guī)劃運行過程，圖7所示為AGV局部動態(tài)環(huán)境下運行后的結(jié)果。表1所示為3種算法性能指標(biāo)對比。利用傳統(tǒng)A算法得到的路徑總長度為44.384 3 mm,用時0.535 31 s，而改進(jìn)后的A算法路徑總長度為41.218 9 mm，用時0.290 545 s，相比傳統(tǒng)A算法，改進(jìn)A算法轉(zhuǎn)折點和冗余路段更少，路徑更短，平滑性更好且經(jīng)過障礙物時可保持一定距離，安全性更好。利用動態(tài)窗口算法可實現(xiàn)路徑的實時規(guī)劃，彌補A算法時效性差的缺點，可以實現(xiàn)規(guī)避已規(guī)劃路徑上動態(tài)障礙物的功能，得到一條平滑、安全的最優(yōu)路徑。

圖4 傳統(tǒng)A*算法路徑規(guī)劃結(jié)果

圖5 靜態(tài)環(huán)境路徑規(guī)劃結(jié)果

圖6 動態(tài)避障路徑規(guī)劃過程

圖7 動態(tài)避障路徑規(guī)劃結(jié)果

表1 各算法性能參數(shù)比較

3.3 輸出運動參數(shù)

所提出的改進(jìn)A算法與動態(tài)窗口法的融合算法可實時輸出AGV的控制參數(shù)，有利于AGV閉環(huán)控制。AGV各參數(shù)變化如圖8—圖10所示，當(dāng)AGV在局部路徑中躲避障礙物時，可以看出線速度和角速度均減小了，且位姿發(fā)生對應(yīng)的變化，驗證了算法的可行性。

圖8 AGV姿態(tài)變化曲線 圖9 AGV線速度變化曲線

圖10 AGV角速度變化曲線

4 實驗驗證

將改進(jìn)后的融合算法應(yīng)用到所研究的AGV上，AGV車體框架采用6061鋼材制作，車輪采用聚氨酯制作，AGV控制系統(tǒng)組成如圖11所示。本地PC與AGV上的樹莓派4B進(jìn)行數(shù)據(jù)交互，樹莓派控制AGV在未知環(huán)境中通過思嵐A2激光雷達(dá)實時收集周圍環(huán)境信息，并實時構(gòu)建地圖。通過WiFi，AGV能夠接收本地PC發(fā)布的控制指令，向控制單元的STM32F103單片機發(fā)送速度控制指令，實現(xiàn)對電機的驅(qū)動控制。驅(qū)動單元部分采用兩個中間驅(qū)動輪差動驅(qū)動，驅(qū)動電機為AGV提供主要的驅(qū)動力。驅(qū)動電機決定AGV的功率性能和負(fù)載能力，從性能方面應(yīng)選擇中大24 V直流無刷電機作為AGV的驅(qū)動電機。

圖11 AGV控制系統(tǒng)組成

融合算法的驗證結(jié)合ROS機器人操作系統(tǒng)進(jìn)行，通過激光雷達(dá)掃描實際環(huán)境并建圖，如圖12所示；再設(shè)定機器人作業(yè)起點與目標(biāo)點。

圖12 構(gòu)建的實際環(huán)境地圖

通過SLAM算法實時獲取機器人當(dāng)前位置信息，采用優(yōu)化后的算法規(guī)劃AGV當(dāng)前位置至終點的最優(yōu)路徑，如圖13所示。

圖13 AGV初始路徑規(guī)劃圖

在AGV運動過程中，通過激光雷達(dá)實時檢測動態(tài)環(huán)境變化，轉(zhuǎn)為障礙物信息發(fā)送給AGV，得到環(huán)境變化后的最優(yōu)路徑，AGV按照最優(yōu)路徑進(jìn)行作業(yè)。實際環(huán)境中AGV運動時會產(chǎn)生略微滑移現(xiàn)象，轉(zhuǎn)角位置會有些許偏移，但是并不影響AGV的路徑規(guī)劃和避障，因此可忽略不計。實驗過程如圖14所示，首先AGV規(guī)劃好最優(yōu)路徑，在遇到障礙物后實時更改路徑，實時選擇最優(yōu)路徑。通過實驗驗證，改進(jìn)算法后的AGV能有效避開所有障礙物，平穩(wěn)到達(dá)目標(biāo)點，具有良好的避障能力和安全性，且路徑平滑，滿足實際作業(yè)要求。

圖14 AGV路徑規(guī)劃過程

5 結(jié)語

為提高在復(fù)雜環(huán)境下AGV的運動規(guī)劃效率，并提高其安全合理的動態(tài)路徑規(guī)劃能力，提出了一種改進(jìn)融合算法。通過改進(jìn)A算法的啟發(fā)函數(shù)，去除多余節(jié)點和改進(jìn)安全避障距離，有效克服了傳統(tǒng)算法轉(zhuǎn)折角過多而影響AGV的正常運行作業(yè)問題，提高了AGV的安全性。同時，采用動態(tài)窗口算法實現(xiàn)路徑的實時規(guī)劃，彌補了傳統(tǒng)A算法時效性差的缺點，實現(xiàn)規(guī)避已規(guī)劃路徑上的動態(tài)障礙物的功能，可規(guī)劃出一條具有實時性的最優(yōu)路徑。通過仿真實驗，驗證了該方法切實可行，更符合實際應(yīng)用。進(jìn)一步地，可將它與機器視覺相結(jié)合，對復(fù)雜環(huán)境的AGV路徑規(guī)劃進(jìn)行更深入的研究。