馮浩然,吳瑞明,傅陽,陳凱旋,程強(qiáng),李陽

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

0 前言

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

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

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

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

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

(,)=(,)+(,)

(1)

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

(2)

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

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

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

1.3 Floyd算法優(yōu)化

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

圖1 Floyd算法原理示意

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

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

(,)+(,)=(,)

(3)

則：

(,)=(,)+(,)

(4)

(,)=→→

(5)

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

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

(6)

則：

(,)=(,)+(,)

(7)

(,)=→→

(8)

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

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

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

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

2 動(dòng)態(tài)窗口法

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

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

2.1 AGV運(yùn)動(dòng)模型

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

(9)

2.2 AGV速度采樣

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

AGV速度約束:

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

(10)

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

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

(11)

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

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

(12)

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

2.3 AGV評(píng)價(jià)函數(shù)

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

價(jià)函數(shù)，以選取最優(yōu)路徑軌跡。設(shè)計(jì)的評(píng)價(jià)函數(shù)為

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

(13)

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

3 融合算法

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

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

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

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

3.2 仿真實(shí)驗(yàn)

為驗(yàn)證所提出的融合算法在復(fù)雜環(huán)境中進(jìn)行路徑規(guī)劃的有效性，進(jìn)行仿真對(duì)比驗(yàn)證。為充分體現(xiàn)對(duì)比效果，設(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運(yùn)行參數(shù)相同。實(shí)驗(yàn)環(huán)境為運(yùn)行內(nèi)存為16 GB的64位WIN10操作系統(tǒng)，實(shí)驗(yàn)平臺(tái)為MATLAB2019a。

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

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

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

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

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

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

3.3 輸出運(yùn)動(dòng)參數(shù)

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

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

圖10 AGV角速度變化曲線

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

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

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

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

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

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

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

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

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

5 結(jié)語

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