改進人工勢場法的AGV路徑規(guī)劃算法研究

牛秦玉，李美凡，趙勇

(西安科技大學(xué)機械工程學(xué)院，陜西西安710054)

0 前言

路徑規(guī)劃是指AGV根據(jù)給定地圖，通過某些指令要求，搜索出一條從起始點到目標點的無碰撞路徑。常用的路徑規(guī)劃算法有：人工勢場法、遺傳算法、RRT算法、A算法等。其中，人工勢場法以計算量小、實時控制性好、規(guī)劃出的路徑相對平滑等優(yōu)點被廣泛使用。

傳統(tǒng)的人工勢場法存在局部極小值和目標不可達的問題，為解決這兩大問題，國內(nèi)外學(xué)者做了大量的研究。文獻[9]提出模糊人工勢場法來解決局部極小點問題；文獻[10]將免疫算法與人工勢場法結(jié)合來解決局部極小值問題；文獻[11]采用混沌優(yōu)化方法對傳統(tǒng)人工勢場法的局部極小值和目標不可達問題進行優(yōu)化；文獻[12]引入了預(yù)測距離，在機器人未陷入局部極小值之前設(shè)置虛擬目標點，牽引機器人逃離局部極小值陷阱。

上述文獻大多直接對人工勢場法兩大缺陷進行優(yōu)化，當實際行車環(huán)境復(fù)雜、障礙物密集分布時，AGV極易陷入障礙物陷阱，導(dǎo)致路徑規(guī)劃失敗。在上述研究基礎(chǔ)上，本文作者考慮到實際行車環(huán)境中道路邊界對路徑規(guī)劃的影響，將道路邊界障礙化，在障礙物密集區(qū)域?qū)φ系K物進行連鎖處理，使AGV能夠成功逃脫障礙物陷阱，在此基礎(chǔ)上再對傳統(tǒng)人工勢場法存在的目標不可達和局部極小值問題進行優(yōu)化。

1 傳統(tǒng)人工勢場法

人工勢場法起源于1986 年，它最初被KHATIB博士用于解決機械臂的運動規(guī)劃問題。該算法的思想是以物理學(xué)中的抽象勢場為基礎(chǔ)，將AGV的運行環(huán)境抽象為勢場空間，目標位置產(chǎn)生一個引力場，障礙物附近產(chǎn)生一個斥力場。AGV同時受到引力場產(chǎn)生的引力和斥力場產(chǎn)生的斥力，在二力的共同作用下朝目標點前進，最終規(guī)劃出一條從起始點到目標點且無碰撞的路徑。

常見的人工勢場法中，引力勢場函數(shù)的具體形式可表示為

(1)

式中:為引力勢場正比例系數(shù)；為AGV坐標位置；為目標點坐標位置；(,)為AGV與目標點之間的距離。

由引力勢場的負梯度得到引力：

=-grad[]=-(,)

(2)

AGV離目標點越遠，受到目標點的吸引力就越大；相反，當AGV離目標點很近時，此時AGV受到的吸引力幾乎為零。AGV所受到的吸引力隨著它與目標點之間的距離增加成正比增加。

斥力勢場函數(shù)具體形式可表示為

(3)

式中:為斥力勢場正比例系數(shù)；為障礙物坐標位置；(，)為AGV與障礙物的距離；為障礙物的影響范圍。

由斥力勢場的負梯度得到斥力：

=-grad[]=

(4)

與目標點相反，在障礙物影響范圍內(nèi)，障礙物對AGV的斥力隨著兩者之間距離的增大而逐漸減小。為障礙物對AGV產(chǎn)生斥力的最大影響范圍，當兩者之間的距離大于時，斥力場為0，對應(yīng)產(chǎn)生的斥力也為0。

如圖1所示，AGV在勢場中所受合力為=+，AGV沿著合力的方向向前移動，該力決定了AGV的運動。當AGV逐漸接近目標點時，所受合力也逐漸減小，AGV到達目標點時所受合力變?yōu)榱恪?/p>

圖1 傳統(tǒng)人工勢場法受力分析

2 改進人工勢場法

2.1 障礙物處理

2.1.1 障礙物膨脹處理

傳統(tǒng)人工勢場法假定的行車環(huán)境是無限大的平面區(qū)域，忽視了實際行車環(huán)境中道路邊界對路徑規(guī)劃的影響，因此適用范圍有限。為便于實際路徑規(guī)劃，改進算法將道路邊界障礙化，對包含道路邊界在內(nèi)的所有障礙物進行圓形膨脹處理。將全部障礙物切割成若干半徑為的障礙圓，所切障礙圓相鄰兩者必相交(即兩障礙圓圓心距離小于2)，要求障礙物影響范圍大于障礙圓半徑，且障礙物可全部被障礙圓覆蓋。

如圖2所示為模擬的實際行車環(huán)境，包含道路邊界及若干形狀大小各異的障礙物。圖3是將實際行車環(huán)境中道路邊界障礙化并且對包含道路邊界在內(nèi)的所有障礙物進行了膨脹處理之后的環(huán)境。

圖2 實際道路邊界及障礙物 圖3 膨脹處理后道路邊界及障礙物

2.1.2 障礙物陷阱優(yōu)化

傳統(tǒng)人工勢場法對復(fù)雜障礙物環(huán)境的適應(yīng)性差。在障礙物密集區(qū)域，AGV在多個障礙物共同作用下，極易掉進障礙物陷阱無法逃脫，從而導(dǎo)致路徑規(guī)劃失敗。針對這一問題，采用障礙物連鎖處理，將密集區(qū)域中距離較近的多個障礙物連鎖起來，形成新的障礙物。具體連鎖過程：設(shè)置閾值，搜索某一障礙物周圍是否存在與它距離小于的其他障礙物，若存在，則將該障礙物與符合連鎖條件的其他障礙物沿著障礙圓切線進行連接，用虛擬障礙圓填補連接空缺形成新的障礙物，如圖4所示。

圖4 連鎖處理障礙物

2.2 改進勢場斥力

采用傳統(tǒng)人工勢場法進行路徑規(guī)劃，當AGV到達目標點附近，目標點附近存在障礙物時，AGV在該處所受障礙物斥力遠大于目標點引力，AGV將一直在目標點附近徘徊而無法到達目標點。針對傳統(tǒng)人工勢場法的目標不可達問題，在斥力勢場函數(shù)中引入AGV與目標點間的距離因數(shù)。改進斥力勢場受力分析如圖5所示。

圖5 改進斥力受力分析

改進后斥力勢場函數(shù)如下：

=

(5)

式中：為大于零的任意常數(shù);其余參數(shù)含義同公式(1)(2)。

斥力勢場函數(shù)求負梯度得到改進后的斥力函數(shù)如下：

(6)

式中：

(7)

(8)

方向為障礙物指向AGV；方向為AGV指向目標點。

2.3 局部極小值問題優(yōu)化

AGV在多個障礙物疊加的斥力場和引力場的共同作用下，極易達到力的平衡狀態(tài)，此時AGV陷入局部極小值陷阱，在該處徘徊不前，導(dǎo)致AGV路徑規(guī)劃失敗。

如圖6和圖7所示:當AGV到達所受障礙物斥力和目標點引力大小相同、方向相反的某個位置時，AGV陷入局部極小值點。

圖6 單障礙物局部極小值問題 圖7 多障礙物局部極小值問題

針對傳統(tǒng)人工勢場法路徑規(guī)劃中的局部極小值問題，采用增加虛擬障礙物的方法進行優(yōu)化。針對圖6所示的單障礙物局部極小值問題，當AGV、目標點、障礙物位于同一條直線時，如圖8所示，求取障礙物當前位置坐標以及AGV位置坐標，障礙圓(障礙物膨脹處理所得)半徑為，首先求取當前AGV在障礙圓上的切點，在AGV與障礙圓切點的連線上取距離(AGV最大幾何尺寸)設(shè)置虛擬點。在虛擬點與當前AGV連線反方向距離(AGV最大幾何尺寸與AGV與障礙物間安全余量之和)處產(chǎn)生虛擬障礙物點。

圖8 單障礙物局部極小值改進

虛擬障礙物點建模方法如下：AGV當前坐標(,),障礙物坐標(,),虛擬障礙物坐標(,)，建立模型如下：

先求出AGV與障礙物間的距離：

(9)

AGV與障礙圓切點距離：

(10)

分別求出3個夾角、、：

=arctan()

(11)

=arcsin()

(12)

(13)

從而求得夾角:

=90°---

(14)

根據(jù)三角形余弦定理得出虛擬障礙物坐標：

(15)

將公式(9)—(14)代入公式(15)得到虛擬障礙物位置坐標(,)：

=-(+)·

(16)

=-(+)·

(17)

針對圖7所示的多障礙物局部極小值問題，首先比較兩相鄰障礙物之間的距離與2+(為AGV最大幾何尺寸，為AGV與障礙物間安全余量)的大小。若<2+，則虛擬障礙物的設(shè)置同上述單障礙物局部極小值問題；若>2+，則采用在目標點與當前AGV位置延長線設(shè)置虛擬障礙物的方法。如圖9所示，在目標點與當前AGV位置延長線處設(shè)置虛擬障礙物點。具體建模方法如下：AGV當前坐標(,),目標點坐標(,),虛擬障礙物坐標(,)，建立模型如下：

圖9 多障礙物局部極小值改進

求出夾角：

(18)

根據(jù)三角形余弦定理得出虛擬障礙物坐標：

(19)

點(,)即為此時虛擬障礙物位置坐標點。

3 實驗驗證及結(jié)果分析

針對傳統(tǒng)人工勢場法存在的缺陷，提出了3個方面的改進，改進的人工勢場法路徑規(guī)劃流程如圖10所示。

圖10 改進人工勢場法路徑規(guī)劃流程

為驗證改進算法的有效性，對傳統(tǒng)人工勢場法和改進算法在MATLAB平臺進行仿真分析。仿真環(huán)境為15×15的區(qū)域，路徑規(guī)劃的起點位置是(1，1)，目標點是(13，13)。其中黑色三角形表示散布在運動環(huán)境中的多處障礙物，左下側(cè)紅色五邊形表示起始點，右上方藍色五邊形表示目標點，紅色圓圈表示障礙物連鎖過程中的虛擬填充障礙物，紫色三角形表示目標不可達實驗中設(shè)置的虛擬障礙物。

實驗驗證分為三部分：障礙物陷阱問題驗證、目標不可達問題驗證以及局部極小值問題驗證。改進算法有效性的評價標準是：在障礙物密集區(qū)域，通過障礙物連鎖處理能夠使AGV逃離障礙物陷阱并且實現(xiàn)路徑優(yōu)化；目標點附近存在障礙物時，使用改進算法后AGV可以成功到達目標點位置；AGV陷入局部極小值陷阱時，利用改進算法能夠成功逃脫極小值陷阱，最終到達目標點。實驗中2種算法參數(shù)設(shè)置如表1所示。

表1 實驗參數(shù)名稱及數(shù)值

圖11和圖12所示為存在多處障礙物陷阱時的實驗分析對比。由圖11可知：在利用傳統(tǒng)人工勢場法進行路徑規(guī)劃時，受障礙物群影響，規(guī)劃出的路徑相對冗長。且在障礙物密集區(qū)域，AGV掉進障礙物陷阱無法前進，導(dǎo)致路徑規(guī)劃失敗。圖12中，在利用改進算法進行路徑規(guī)劃時，對障礙物密集區(qū)域符合連鎖條件的障礙物進行連鎖處理后，AGV能夠成功避開障礙物陷阱，并且規(guī)劃出來的路徑相對縮短。

圖11 傳統(tǒng)障礙物陷阱路徑規(guī)劃 圖12 改進障礙物陷阱路徑規(guī)劃

圖13和圖14所示是目標不可達實驗分析對比，在目標點(13，13)附近(13.2，13.2)處設(shè)置障礙物。圖13所示為使用傳統(tǒng)人工勢場法進行的路徑規(guī)劃，AGV在目標點附近由于斥力遠遠大于引力，AGV陷入目標不可達陷阱，無法到達目標點。圖14所示為使用改進算法進行的路徑規(guī)劃，對傳統(tǒng)算法斥力函數(shù)進行改進后，AGV能夠成功到達目標點，完成了從起始點到目標點的路徑規(guī)劃。

圖13 傳統(tǒng)目標不可達路徑規(guī)劃 圖14 改進目標不可達路徑規(guī)劃

圖15和圖16所示是局部極小值實驗分析對比。圖15所示是存在多處局部極小值問題的傳統(tǒng)算法路徑規(guī)劃，當AGV到達點時，所受障礙物斥力和目標點引力大小相同、方向相反，AGV出現(xiàn)徘徊不前等狀況，導(dǎo)致路徑規(guī)劃失敗。采用改進算法后，如圖16所示，當AGV陷入局部極小值時，在點附近合適位置設(shè)置虛擬障礙物，使AGV在處所受合力不為0，成功跳出局部極小值陷阱。到達處，AGV再次陷入局部極小值點，根據(jù)算法判定兩障礙物距離大于預(yù)設(shè)最小值，因此在目標點與點連線延長線合適位置設(shè)置虛擬障礙物，使AGV跳出局部極小值點，成功到達目標點。

圖15 傳統(tǒng)局部極小值路徑規(guī)劃 圖16 改進局部極小值路徑規(guī)劃

綜上，使用傳統(tǒng)人工勢場法容易掉進障礙物陷阱以及局部極小值陷阱，當目標點附近存在障礙物時，容易出現(xiàn)目標不可達問題。使用改進算法，成功解決傳統(tǒng)人工勢場法存在的缺陷，規(guī)劃出一條從起始點到目標點的無碰撞路徑。

4 結(jié)論

對傳統(tǒng)人工勢場法存在的問題進行分析，通過將道路邊界障礙化，障礙物密集區(qū)域采用障礙物連鎖處理，改善了實際環(huán)境適應(yīng)性差的問題；針對目標不可達問題，引入目標點與AGV間的距離因數(shù)，使AGV接近目標點時，斥力逐漸減小到零，解決了目標不可達問題；針對實際規(guī)劃中的局部極小值問題，采用引入虛擬障礙物的方法，使AGV在到達目標點途中能夠跳出合力為零的局部極小值點，順利到達目標點。最后通過實驗對改進算法進行了驗證，實驗結(jié)果表明：改進算法可以使AGV成功避開障礙物陷阱，規(guī)劃出一條從起始點到目標點的無碰撞路徑。