唐 龍，肖 翠，呂宣宣，劉有余

(安徽工程大學(xué) 機(jī)械與汽車工程學(xué)院，安徽 蕪湖 241000)

自動(dòng)導(dǎo)引運(yùn)輸車(Automated Guided Vehicle，AGV)最早用于運(yùn)載貨物，隨著信息技術(shù)的發(fā)展，其應(yīng)用領(lǐng)域也在不斷擴(kuò)展。其中，路徑規(guī)劃作為AGV的基礎(chǔ)功能，是實(shí)現(xiàn)其尋優(yōu)路徑及自主避障的關(guān)鍵，一直是學(xué)者們研究的熱點(diǎn)。目前，國(guó)內(nèi)外比較成熟的是針對(duì)已知環(huán)境的全局路徑規(guī)劃方法。文獻(xiàn)[1-2]利用改進(jìn)粒子群算法，避免陷入局部最優(yōu)；文獻(xiàn)[3]利用改進(jìn)螞蟻算法，加快收斂速度；文獻(xiàn)[4-5]引入柵格地圖，建立了直觀的環(huán)境模型。針對(duì)未知?jiǎng)討B(tài)環(huán)境，主要是通過傳感器獲取局部環(huán)境信息，進(jìn)而將外界信息傳給中央控制器，進(jìn)而做出避障行為;文獻(xiàn)[6]結(jié)合勢(shì)場(chǎng)法和蒙特卡羅定位法，使機(jī)器人避開動(dòng)態(tài)障礙物；文獻(xiàn)[7]利用模糊邏輯控制在動(dòng)態(tài)環(huán)境下規(guī)劃出全局較優(yōu)路徑。這些方法主要針對(duì)勻變速運(yùn)動(dòng)，但對(duì)于非勻變速物體的避障效果不是很理想。

結(jié)合上述方法，首先建立環(huán)境與AGV的數(shù)學(xué)模型，基于粒子群算法，規(guī)劃靜態(tài)環(huán)境；針對(duì)非勻變速物體，引入逐步判別法，對(duì)隨機(jī)障礙物逐一判別避障，進(jìn)而實(shí)現(xiàn)多隨機(jī)障礙物環(huán)境的路徑規(guī)劃。

1 靜態(tài)環(huán)境的路徑規(guī)劃

1.1 建立環(huán)境模型

針對(duì)AGV運(yùn)行環(huán)境，采用柵格[4]模擬固定障礙物，利用平面坐標(biāo)系規(guī)劃地圖，建立AGV的運(yùn)行環(huán)境模型。為了便于計(jì)算仿真，將AGV簡(jiǎn)化為一個(gè)質(zhì)點(diǎn)，同時(shí)擴(kuò)大固定障礙物的邊界尺寸，實(shí)際上是把AGV的實(shí)際尺寸和排斥區(qū)域Ⅰ折算進(jìn)障礙物的面積里。

圖1 柵格地圖

柵格地圖如圖1所示。對(duì)柵格地圖作了以下定義：黑色柵格為障礙柵格，白色柵格為自由柵格；不滿一個(gè)柵格時(shí)算一個(gè)柵格；把地圖的邊界當(dāng)成障礙物來處理。

1.2 路徑規(guī)劃步驟

(1)輸入由0和1組成的矩陣，構(gòu)建柵格地圖(0表示自由域，1表示障礙物)。

(2)初始化粒子群[1]，粒子群規(guī)模N，確定每個(gè)粒子的位置xi和速度vi。

(3)設(shè)置目標(biāo)函數(shù)，根據(jù)式(1)計(jì)算得出每個(gè)粒子的適應(yīng)度值fi。

(1)

(4)比較每個(gè)隨機(jī)粒子的適應(yīng)度值fi，取較小值。

(5)根據(jù)式(2)更新粒子的位置xi和速度vi。

(2)

式中，c1和c2為學(xué)習(xí)因子;ω為慣量因子;r1和r2為[0，1]范圍內(nèi)的均勻隨機(jī)數(shù)。

(6)如果滿足結(jié)束條件就退出運(yùn)行，否則返回(3)。

1.3 仿真運(yùn)行

表1 仿真參數(shù)表

參數(shù)取值種群粒子數(shù)量N50學(xué)習(xí)因子c1、c21.494[2]參數(shù)取值慣量因子ω0.729[2]最大迭代次數(shù)M200

收斂曲線變化趨勢(shì)如圖2所示。大約迭代50次時(shí)，最小路徑長(zhǎng)度基本穩(wěn)定在41個(gè)單位；AGV運(yùn)動(dòng)軌跡如圖3所示。AGV在通往目標(biāo)點(diǎn)的過程中，盡量選取了較短路徑，而且避開了所有障礙物。

圖2 收斂曲線變化趨勢(shì) 圖3 AGV運(yùn)動(dòng)軌跡

2 數(shù)學(xué)建模

2.1 建立AGV的超聲波定位模型

在AGV中安裝超聲波傳感器[8]，使AGV能夠利用回聲定位的方法來判斷障礙物的位置，進(jìn)而將信息傳給中央控制器，從而做出避障行為。同時(shí)考慮到運(yùn)行效果，模型采用3個(gè)超聲波測(cè)距模塊來改進(jìn)控制效果，分別放在AGV的左側(cè)、前方、右側(cè)3個(gè)方向。

AGV在運(yùn)行過程中，依靠發(fā)射和接收信號(hào)的時(shí)間差估計(jì)障礙物的位置，即回聲定位。AGV中的超聲波信號(hào)具有一定的指向性，利用兩個(gè)超聲波接收器接收信號(hào)，使其具有方位分辨能力，方位估計(jì)精度取決于兩個(gè)超聲波接收器的距離。

中遠(yuǎn)場(chǎng)的二元測(cè)向模型如圖4所示。根據(jù)中遠(yuǎn)場(chǎng)的二元測(cè)向模型，利用式(3)估計(jì)目標(biāo)的方位角θ，即：

(3)

近場(chǎng)時(shí)的三元測(cè)向模型如圖5所示。根據(jù)近場(chǎng)時(shí)的三元測(cè)向模型，可以求出目標(biāo)的聲信號(hào)到達(dá)各個(gè)陣元的時(shí)間差(c表示聲速)：

(4)

(5)

(6)

圖4 中遠(yuǎn)場(chǎng)的二元測(cè)向模型 圖5 近場(chǎng)時(shí)的三元測(cè)向模型

聯(lián)立式(4)、式(5)、式(6)可推導(dǎo)出目標(biāo)方位式(7)：

(7)

通過模型分析可知，距離較遠(yuǎn)時(shí)，根據(jù)二元測(cè)向模型，AGV的超聲波回聲定位只能判斷障礙物的方位，定位效果較差；距離較近時(shí)，根據(jù)三元測(cè)向模型，AGV的超聲波回聲定位能較精確地定位到障礙物的位置。

2.2 建立AGV運(yùn)動(dòng)模型

通過AGV的超聲波回聲定位模型，使得AGV自身擁有了一定的感知范圍。在感知范圍內(nèi)，根據(jù)感知距離的大小將AGV的感知范圍設(shè)定為兩個(gè)區(qū)域，分別是：①當(dāng)AGV和障礙物之間的距離到達(dá)一個(gè)臨界值，為防止相撞，AGV會(huì)改變運(yùn)動(dòng)軌跡，該區(qū)域稱為排斥區(qū)域Ⅰ；②當(dāng)AGV與障礙物距離過遠(yuǎn)，超過其感知范圍，則此時(shí)AGV和障礙物之間無任何聯(lián)系，該區(qū)域稱為無用區(qū)域Ⅱ。

在平面坐標(biāo)系中，將AGV看做一個(gè)具有平面坐標(biāo)和運(yùn)動(dòng)方向的矢量點(diǎn)。初始時(shí)刻t=0，設(shè)置AGV的運(yùn)動(dòng)速度且大小不變(AGV載重時(shí)所需速度不需要太大，近似認(rèn)為作曲線運(yùn)動(dòng)與直線運(yùn)動(dòng)時(shí)速度大小相等)，初始運(yùn)動(dòng)方向隨機(jī)給定。

針對(duì)運(yùn)行中的AGV，設(shè)置了三大運(yùn)動(dòng)規(guī)則：(1)慣性運(yùn)動(dòng)規(guī)則。運(yùn)動(dòng)物體具有維持先前運(yùn)動(dòng)狀態(tài)的趨勢(shì)。因此AGV具有慣性運(yùn)動(dòng)。

AGV的初始單位慣性運(yùn)動(dòng)矢量為：

(8)

式中，(x0,y0)為AGV在t=0時(shí)刻隨機(jī)給定的初始慣性運(yùn)動(dòng)矢量。

(2)排斥運(yùn)動(dòng)規(guī)則。在排斥區(qū)域Ⅰ內(nèi)，由于AGV在t時(shí)刻與障礙物距離過近，為了避免碰撞，AGV與障礙物之間會(huì)產(chǎn)生一種“排斥力”，從而改變AGV當(dāng)前行進(jìn)的運(yùn)動(dòng)方向：

(9)

式中，(mt,nt)為障礙物的平面坐標(biāo)。

(3)吸引運(yùn)動(dòng)規(guī)則。在整體路徑規(guī)劃中，AGV會(huì)被預(yù)先設(shè)置目標(biāo)點(diǎn)，由此目標(biāo)點(diǎn)與AGV之間會(huì)產(chǎn)生一種“吸引力”，運(yùn)行期間所有行為均是為了能夠無碰撞且快速地到達(dá)目標(biāo)點(diǎn)：

(10)

式中，(xMM,yMM)為設(shè)置目標(biāo)點(diǎn)的平面坐標(biāo)。

圖6 AGV的運(yùn)動(dòng)規(guī)則

AGV的運(yùn)動(dòng)規(guī)則如圖6所示。取3個(gè)運(yùn)動(dòng)規(guī)則下的運(yùn)動(dòng)方向的合方向?yàn)锳GV在t時(shí)刻的運(yùn)動(dòng)方向，表示為：

(11)

式中，M0為AGV初始慣性運(yùn)動(dòng)方向;Mt為AGV在t時(shí)刻的運(yùn)動(dòng)方向。

但是考慮到各個(gè)運(yùn)動(dòng)規(guī)則對(duì)AGV的影響效果不同，各規(guī)則對(duì)AGV進(jìn)行運(yùn)動(dòng)決策的影響程度應(yīng)通過層次分析法進(jìn)行分析求解。故對(duì)公式進(jìn)行改進(jìn)，加入3個(gè)運(yùn)動(dòng)規(guī)則的貢獻(xiàn)度，則AGV在t時(shí)刻的運(yùn)動(dòng)方向?yàn)椋?/p>

(12)

式中，λ1為慣性運(yùn)動(dòng)的貢獻(xiàn)度;λ2為排斥運(yùn)動(dòng)的貢獻(xiàn)度;λ3為吸引運(yùn)動(dòng)的貢獻(xiàn)度；且λ1+λ2+λ3=1。

AGV在t時(shí)刻的平面坐標(biāo)為：

(13)

2.3 建立動(dòng)態(tài)障礙物運(yùn)動(dòng)曲線擬合模型

考慮到動(dòng)態(tài)障礙物運(yùn)動(dòng)方向和速度的隨機(jī)性，采用加權(quán)最小方差(WLS)擬合原理[9]來擬合動(dòng)態(tài)障礙物的運(yùn)動(dòng)曲線。通過超聲波傳感器實(shí)時(shí)監(jiān)測(cè)動(dòng)態(tài)障礙物的位置信息，然后利用加權(quán)最小方差擬合原理進(jìn)行數(shù)據(jù)分析，最后得到其運(yùn)動(dòng)曲線，這種數(shù)據(jù)擬合方法得到的曲線更加平滑且貼合運(yùn)動(dòng)規(guī)律。對(duì)應(yīng)N階多項(xiàng)式的擬合公式，所需要求解的擬合系數(shù)需要求解線性方程組，其中線性方程組的系數(shù)矩陣和需要求解的擬合系數(shù)矩陣分別為：

(14)

使用加權(quán)最小方差法求解得到擬合系數(shù)為：

(15)

其對(duì)應(yīng)的加權(quán)最小方差表達(dá)式為：

Jm=[Aθ-y]TW[Aθ-y]。

(16)

利用Matlab進(jìn)行隨機(jī)數(shù)據(jù)的曲線擬合，通過調(diào)用rand()函數(shù)來實(shí)現(xiàn)基礎(chǔ)數(shù)據(jù)的隨機(jī)性。

研究分別設(shè)置比較1個(gè)與3個(gè)動(dòng)態(tài)障礙物，均隨機(jī)取10個(gè)隨機(jī)數(shù)據(jù)進(jìn)行曲線擬合，得到擬合曲線如圖7所示。即使遇到多個(gè)動(dòng)態(tài)障礙物，該模型仍能很好地?cái)M合各自的運(yùn)動(dòng)曲線。

圖7 動(dòng)態(tài)障礙物運(yùn)動(dòng)擬合曲線

3 動(dòng)態(tài)環(huán)境的路徑規(guī)劃

3.1 模型分析

引入逐步判別法來實(shí)現(xiàn)動(dòng)態(tài)避障控制。簡(jiǎn)化AGV為一個(gè)質(zhì)點(diǎn)，定義圓為不確定域[10]，即動(dòng)態(tài)障礙物在下一時(shí)刻可能出現(xiàn)的任意位置，其半徑為動(dòng)態(tài)障礙物的最大步長(zhǎng)。

假設(shè)AGV在通往目標(biāo)點(diǎn)的可運(yùn)行區(qū)域中(AGV此刻離動(dòng)態(tài)障礙物較遠(yuǎn))，在n次不同時(shí)刻對(duì)動(dòng)態(tài)障礙物的位置進(jìn)行了n次監(jiān)測(cè)[11]。

圖8 逐步判別法避障

逐步判別法避障如圖8所示。選取第n次監(jiān)測(cè)，此時(shí)AGV在A點(diǎn)，O點(diǎn)位置為動(dòng)態(tài)障礙物。下一時(shí)刻，按原定路線，AGV應(yīng)到達(dá)B點(diǎn)。然而，B點(diǎn)在圓O的不確定域內(nèi)，那么AGV在運(yùn)行過程中存在一定概率與障礙物相撞，所以實(shí)際運(yùn)行時(shí)AGV應(yīng)偏轉(zhuǎn)一定的角度θ，才能繼續(xù)前進(jìn)(如圖8所示，θ=∠BAC,為AB與圓O切線AC的夾角)。

若B點(diǎn)在圓O的不確定域外，則AGV按原定路線運(yùn)行。重復(fù)以上步驟，直到AGV安全繞過所有障礙物到達(dá)目標(biāo)點(diǎn)。

3.2 仿真運(yùn)行

通過仿真分析，得到單個(gè)障礙物的運(yùn)動(dòng)擬合曲線和AGV的路徑規(guī)劃曲線。多隨機(jī)避障曲線如圖9所示。由圖9a可知，在自由柵格區(qū)域，AGV通過監(jiān)測(cè)數(shù)據(jù)進(jìn)行曲線擬合，判斷動(dòng)態(tài)障礙物下一時(shí)刻可能出現(xiàn)的位置，進(jìn)而有效地繞過了動(dòng)態(tài)障礙物。由圖9b可知，再增加一個(gè)動(dòng)態(tài)障礙物，仍然能夠完成基礎(chǔ)避障工作，驗(yàn)證了該方案的可行性。

圖9 多隨機(jī)避障曲線

研究的方案對(duì)比文獻(xiàn)[7]的方案。其中，文獻(xiàn)[7]動(dòng)態(tài)障礙物避障過程如圖10所示。文獻(xiàn)[7]的方案是對(duì)勻變速物體進(jìn)行避障規(guī)劃的，而研究的方案可以規(guī)劃非勻變速的物體，并且具有較好的避障效果。

圖10 文獻(xiàn)[7]動(dòng)態(tài)障礙物避障過程

4 結(jié)論

研究從靜態(tài)和動(dòng)態(tài)兩個(gè)方面對(duì)AGV的最優(yōu)路徑及避障控制進(jìn)行分析：針對(duì)靜態(tài)環(huán)境，基于粒子群算法，通過柵格地圖簡(jiǎn)化環(huán)境，得到AGV最短路徑的收斂曲線和運(yùn)動(dòng)軌跡，驗(yàn)證了靜態(tài)環(huán)境規(guī)劃的有效性。針對(duì)動(dòng)態(tài)環(huán)境，利用傳感器，建立AGV的超聲波定位模型和運(yùn)動(dòng)模型，推導(dǎo)出障礙物和AGV的位置信息，并通過加權(quán)最小方差原理擬合運(yùn)動(dòng)曲線，最后引入逐步判別法來實(shí)現(xiàn)動(dòng)態(tài)避障控制。通過仿真分析，得到AGV多隨機(jī)障礙物的避障曲線，驗(yàn)證了動(dòng)態(tài)環(huán)境規(guī)劃的可行性。