基于APF的AGV局部路徑規劃改進算法研究

丁承君，閻欣怡，2，馮玉伯，2，賈麗臻

1.河北工業大學 機械工程學院，天津 300130

2.天津通信廣播集團有限公司 智慧云網事業部，天津 300143

3.中國民航大學 航空工程學院，天津 300300

AGV（automated guided vehicle）即“自動導引車”，一般來說能通過磁傳感器、激光雷達等掃描周邊環境生成地圖，依靠地圖生成路徑，之后通過控制器沿規定的路徑行駛，是一種高效、安全、可靠、具有柔性的移動機器人[1]。路徑規劃在AGV的研究領域中占有很大比重，在實際工程應用中除了在先驗信息已知的建筑平面圖基礎上可以進行全局路徑規劃外，對于局部未知的障礙物又分為靜態不可移動障礙物和動態可移動障礙物兩種，如何在復雜的環境中規劃出一條最優路徑是當前AGV路徑規劃領域的重要問題之一。

盡管局部路徑規劃的環境十分復雜，但是在國內外學者大量研究的基礎上也出現了一些的方法來解決這個問題。陳秋蓮等[2]通過神經網絡模型對障礙物進行碰撞檢測，在原有粒子群算法的基礎上通過調整慣性權重的方法改進，使算法可以快速規劃出路徑，此外將三次樣條曲線平滑路徑應用于靜態和動態的障礙物環境。王洪斌等[3]為實現移動機器人在動態復雜環境中局部動態避障，提出將改進動態窗口算法與全局路徑規劃信息相結合的在線路徑規劃法，采用預瞄偏差角追蹤法成功捕捉移動目標點，成功提高了路徑規劃效率。Liu等[4]提出一種改進人工勢場法，當無人機遇到較大的障礙物無法前進或者當傳感器受到環境因素的干擾而短暫失效時，通過擴大障礙物范圍和動態改變距離，改變斥力函數使算法有效讓無人機安全飛行。

人工勢場法（artificial potential field，APF）是由Khatib提出的一種適用于局部路徑規劃的方法，可以規劃出一條較為平滑的曲線避開障礙物，具有便于理解等優點，但與此同時也帶來許多問題。Azmi等[5]提出了當目標被具有特定特征的障礙物阻擋時，APF方法有時會遇到無限循環問題。他們采用地圖擴充的方法來重新規劃，當無限循環的情況發生時，地圖被轉換并開始搜索可行駛路徑，但是沒考慮到因障礙物離目標點過近不可達問題。Wang等[6]提出了一種改進的人工勢場法局部路徑規劃，為了使USV（unmanned surface vehicle）能夠到達靜態目標并跟蹤動態目標，在重力場中引入了USV與目標之間的相對速度和相對加速度，但是沒有考慮到算法的局限性。Ma等[7]在斥力場模型中，引入了相對位置和相對速度等影響因素，降低了目標附近障礙物的斥力勢場強度，解決了目標附近障礙物不可接近的問題。引入隨機擾動勢能的調節機制，解決了在局部極小點難以移動的問題。然而對局部路徑規劃中動態障礙物的避障問題并沒有給出解決方案。

在此基礎上本文提出了APF改進算法，使用人工勢場法并在位置勢場的基礎上加上了速度勢場，可使AGV動態避開不同速度的移動障礙物。針對APF算法的局限性，采用粒子群算法與之結合優化算法，解決避障效果不良的問題。之所以選用粒子群算法來優化APF算法是因為AGV在局部路徑規劃時，所要求的時間較短，所以要選擇一種收斂速度較快的算法解決在AGV避障過程中遇到的問題，而與人工勢場法結合的粒子群算法和同類仿生學算法相比（見表1）具有搜索效率高、穩定性高、路徑規劃速度快的優勢，與其他收斂速度較快的算法相比，易于改進和提高收斂效率。提高收斂效率方面，引入可調整慣性權重因子、學習因子，可以快速規劃出最短路徑。

表1 優化算法優缺點對比Table 1 Comparison of advantages and disadvantages of optimization algorithms

1 算法起始條件

AGV與障礙物之間的距離計算。在路徑規劃過程中，障礙物的形狀為各不相同的不規則多邊形，為了便于分析，用外接圓代替障礙物的形狀，并且從安全角度[8]考慮將障礙物膨脹化處理，如圖1所示。

圖1 膨脹的障礙物示意圖Fig.1 Schematic diagram of expanding obstacle

將膨脹后的障礙物半徑設為r，障礙物的影響邊距為d0，則障礙物的影響半徑為ρ0=r+d0。同時AGV也用外接圓表示，其半徑為ra。當d＜ra+ρ0時，AGV開始進行局部路徑規劃。

2 人工勢場法

人工勢場（APF）引入了物理學中勢的概念，分為引力場和斥力場[9]。與目標點之間存在引力，與障礙物之間存在斥力，兩種力共同作用在AGV上，使其能安全避開障礙物。

引力場：

式中，ε是引力尺度因子；ρ(q,qgoal)表示物體當前位置與目標點的距離。

在力學中，引力場產生的引力為引力場對距離的導數：

同樣的，斥力場公式：

式中，η是斥力尺度因子；ρ(q,qobs)代表物體和障礙物之間的距離；ρ0代表每個障礙物的影響半徑。物體與障礙物之間的距離大小決定了斥力大小。

同理，斥力公式如下：

引力和斥力相加可得作用在AGV上的合力：

合力的方向為AGV移動的方向。

3 改進人工勢場法

3.1 引入速度勢場

因為障礙物在移動過程中有不同的速度，不能只用障礙物與AGV之間的距離來判斷引力場和斥力場的大小，所以在原有力場的基礎上引入速度勢場。其斥力勢場變為：

斥力變為：

式中，η1為速度斥力的尺度因子；θ為AGV指向障礙物方向與AGV運動方向的夾角，當夾角為鈍角時障礙物并不影響AGV通行。用選擇的目標點計算引力場及引力，其計算公式不變，在此基礎上求得作用在AGV上的合力。

3.2 結合改進粒子群算法

當動態障礙物靜止在路徑中央，即AGV、障礙物、目標點共線時，有可能陷入局部最小值使AGV原地靜止或者來回移動的情況。在障礙物運動的情況下斥力一直發生改變，不存在引力與斥力一直相等的情況，而在靜態的情況下，可能會遇到引力和斥力幾乎相等并一直保持，即兩者達到平衡狀態，不能規劃出路徑。當障礙物離終點過近時，也可能因為設置斥力過大，引力過小，繞過終點向無限遠處移動的情況。這里計算合力和引力的夾角，當夾角快要接近180°時，即證明AGV不能到達終點。在這兩種情況下使用改進粒子群算法，以小車當前位置為路徑的起點，目標點為終點進行局部路徑規劃。

因為障礙物用圓形代替，在上述改進的基礎上用插值法將一系列的位置點通過多項式擬合的方法使路徑變為曲線。考慮到運算速度的問題，應選用插值公式較為簡單，同時又能保證曲線光滑性的插值函數。因此相比于生成曲線更為光滑但是運算速度較慢的五次樣條插值，選擇三次樣條曲線插值更為合適，并且三次樣條曲線插值[10]引用廣泛，更容易理解。

3.2.1 粒子群算法

由Kennedy和Eberhart提出的粒子群算法受到覓食的鳥的啟發，每只正在覓食的鳥代表粒子群的一個粒子，這個粒子可看作在問題模型中的每一個可行解[11]。這些粒子可隨迭代次數的遞增不斷調整自己的速度和位置[12]。在二維圖中，每個粒子i在k次迭代的速度和位置分別按照下列公式進行更新：

式中，vki是粒子i在第k次迭代的速度；w是慣性權重因子，較大的慣性權重有較強的全局搜索能力，較小的慣性權重則在局部搜索方面有優勢；c1、c2是學習因子，兩者分別表示向自己學習能力和向群體學習能力的大小；pk-1i、pk-1g是粒子i經k-1次迭代后的個體最優解和所有粒子i經k-1次迭代后的歷史最優解；rand()為[0,1]范圍內的隨機數；xki為粒子i在第k次迭代的位置。由式（9）可以看出，當前粒子通過上一次計算的速度、當前位置與粒子的歷史最優位置、群體所有粒子的歷史最優位置的距離計算更新速度，并以此速度代入式（10）不斷更新位置。當達到約束條件時迭代停止。這里引入適應度函數公式如下所示：

式中，n代表粒子總數；pi代表第i個歷史最優粒子的位置。

因此，適應度函數值是相鄰兩個歷史最優粒子之間的距離和，代表了路徑總長度，當適應度函數值收斂到最小且不再變化時，即路徑最短則停止迭代。

3.2.2 調整慣性權重

在AGV局部路徑規劃時需要的時間越短越好，而當用粒子群算法規劃路線時需要多次迭代收斂到最小值，因此本文在原有算法基礎上引入了可調整的慣性權重因子提高算法收斂能力和運算效率。

通過文獻研究發現，可通過減小慣性權重的方法提高算法的全局收斂能力，并且PSO的慣性權值為凹函數策略，優于線性函數策略，線性策略優于凸函數策略，而在凹函數中遞減的指數曲線函數性能最優[13]。由此可以采取調整慣性權重的方法提高算法的收斂能力，進而提高算法的運行效率。

式中，i為當前迭代次數；K為最大迭代次數；wmax、wmin為最大、最小慣性權重，慣性權重隨迭代次數的遞增而不斷減小。此公式原型為y=ax(0＜a＜1)在大于0的區間內呈指數曲線趨勢遞減。

3.2.3 調整學習因子

同理調整學習因子。標準粒子群算法中學習因子c1、c2取值相同，粒子在整個過程中自我認知能力和群體學習能力相同。文獻[14]在此基礎上證明在算法前期，粒子需要較強的自我認知能力，增加種群的多樣性；到了算法后期，粒子需要較強的群體學習能力，快速收斂到最優解。

反映到c1、c2上如式（13）和（14）所示，算法前期c1＞c2,算法后期c1＜c2,c1呈線性遞增趨勢，而c2呈線性遞減趨勢。

然而適應度函數值的收斂曲線并不是呈線性遞減而是呈指數函數趨勢遞減的，導致學習因子數值相等的點并不應該出現在迭代次數的中點。因此本文提出以前后兩次迭代全局最優值之差的大小作為判斷依據，設置學習因子的相對大小值。其偽代碼如下：

其中，Δpg為前后兩次迭代全局最優值之差；k1、k2為差值常數；[cmin,cmax]為學習因子，變化范圍一般為0～4。

3.3 三次樣條曲線插值

設置每段三次樣條曲線插值函數為S(x)=ax3+bx2+cx+d∈C(x0,xn)，生成路徑點為(x0,y0),(x1,y1),…,(xn,yn)。一共n+1個點，分為n個區間，每個區間上都有一個三次樣條插值函數。

通過樣條插值的條件來求解插值函數系數。

（1）在每個路徑點處三次樣條插值函數的估計值等于給定函數值。

（2）三次樣條插值函數在路徑點的一階微分、二階微分連續。

（3）使用非扭結邊界條件[15]，即在兩個端點處的三階導與這兩端點的鄰近點的三階導相等。

根據所列條件代入矩陣方程，計算每個樣條插值函數的系數，得到三次樣條曲線。

3.4 改進算法流程

算法流程如圖2所示，具體步驟如下：

步驟1判斷是否進入障礙物影響半徑范圍，進入障礙物影響范圍開始局部路徑規劃。

步驟2改進人工勢場法初始化參數，設置小車當前位置為起點，目標點為終點。設置最大迭代次數、斥力尺度因子、引力尺度因子、速度斥力的尺度因子等。改進粒子群參數初始化，設置最大迭代次數、最大最小慣性權重因子、學習因子、兩次最優差值系數、種群大小等。

步驟3 AGV進入動態障礙物影響半徑范圍內，通過式（2）計算引力，式（6）計算斥力，式（8）計算合力。

步驟4在當前位置的基礎上，加上小車速度乘以合力在x軸方向上的夾角乘以時間間隔為預測到下一時刻的位置，計算出的速度位置在下一次迭代時按公式計算引力、斥力和合力，循環往復直到達到最大迭代次數停止。

步驟4當F(q)=0（合力為0）或者cosβ＜η（合力與小車和終點連線的夾角小于一定角度）時，陷入局部最小值，跳出循環。設置小車當前位置為起點，目標點為終點，運行改進粒子群算法。

步驟5按照式（12）計算慣性權重，以前后兩次最優值之差作為判斷依據，當差值大于設定值k1時c1=cmax,c2按式（14）計算。當差值小于設定值k2時c2=cmax,c1按式（13）計算。當差值在兩個設定值之間，c1=c2。

步驟6將步驟5求得參數代入式（9），通過式（9）用上一次的粒子速度計算這一次迭代的粒子速度，再通過式（10）計算這一次迭代的粒子位置。按式（11）計算當前適應度的值，即路徑長度。

步驟7當適應度的值較小時更新為個體最優，再與全局最優相比較，優于全局最優，則更新為全局最優。最后三次樣條插值使路徑更平滑。

步驟8輸出局部路徑。

圖2算法流程圖Fig.2 Algorithm flow chart

4 仿真驗證及分析

4.1 動態環境下驗證

在動態環境下仿真驗證，如圖3（a）所示，設置兩個圓形障礙物分別沿x軸、y軸勻速運動，設置AGV起點(0,0)位于地圖左上角，終點(10,10)位于地圖右下角，步長為0.2 m，AGV的位置用紅色小點表示。動態避障過程如圖3（a）、（b）、（c）、（d）所示，分別是迭代次數為40、50、60、70的時候AGV走過的路徑。由此可以看出AGV在靠近障礙物時可及時改變路徑，避免碰撞的產生。

圖3 動態避障過程Fig.3 Dynamic obstacle avoidance process

第一次實驗設置AGV的速度v=0.5 m/s，第二次實驗設置AGV的速度v=1 m/s。如圖4所示，在同一中間時刻，相比于慢速下，在AGV速度更快時改進算法計算出AGV受障礙物的斥力明顯增大，規劃出的路徑離障礙物較遠。圖5為不同速度下規劃出的完整路徑。原有的APF算法不受障礙物速度的影響，只會因障礙物的位置不同規劃出不同路徑。改進后的APF-PSO算法，可根據不同速度的障礙物調整AGV到障礙物的距離大小，驗證了引入速度勢場改進算法使AGV動態避障的有效性。

圖4 迭代50次時不同速度路徑對比Fig.4 Comparison of different velocity paths in 50 iterations

圖5 算法終止時不同速度路徑對比Fig.5 Comparison of different velocity paths at end of algorithm

4.2 靜態環境下驗證

4.2.1 APF-PSO算法路徑規劃

在靜態環境下，原有的APF算法很容易受到自身局限性的影響，在達到最大迭代次數之前不能使AGV到達目標點。如圖6所示，AGV當遇到局部靜態障礙物距離目標點較近的時候，因斥力過大向無限遠處移動而無法到達目標點的情況。圖7為當障礙物的斥力與終點處的合力為0時，算法陷入局部最小值無法到達目標點的情況。在上述兩種情況出現時，在原有算法的基礎上引入收斂效率較高的粒子群算法可以克服原有算法的局限性，規劃出局部路徑。圖8為面對兩種情況使用APF-PSO算法規劃出的路徑，解決了AGV不能合理避障的問題。證明了引入改進粒子群算法的APF-PSO算法的有效性。

圖6 傳統APF算法斥力過大Fig.6 Repulsive force of traditional APF algorithm is too large

圖7 傳統APF算法陷入局部最小值Fig.7 Traditional APF algorithm falls into local minimum

圖8 APF-PSO算法規劃路徑Fig.8 APF-PSO algorithm path planning

4.2.2 改進粒子群算法收斂效率驗證

因為算法應用于局部路徑規劃，所以起點與終點的距離較短。在算法規劃出路徑長度差別不大的條件下主要追求算法運行效率，給予AGV充分的反應時間避開障礙物。

實驗中初始化變量參數如表2所示。仿真結果如圖9所示，圖9（a）、（b）、（c）為蟻群算法、PSO算法、改進PSO算法在靜態障礙物環境下迭代到50次時搜索到的最短路徑。迭代次數相同的情況下3條路徑對比，本文算法搜索出的路徑最為平滑，蟻群算法和原始粒子群算法搜索出的路徑還未變成平滑的曲線，未搜索到最短路徑。圖9（d）為3種算法收斂迭代曲線對比，圖中蟻群算法在第41次迭代基本收斂，原始PSO算法第15次迭代基本收斂，本文算法在第5次迭代基本收斂，收斂速度最快，得到的路徑最短。算法具體性能參數如表3所示，對比看出本文算法收斂效率最高，局部路徑規劃效果更佳。

圖9 仿真結果Fig.9 Simulation results

表2 算法初始化參數Table 2 Algorithm initialization parameters

表3 算法性能對比Table 3 Comparison of algorithm performance

5 結束語

本文研究了一種人工勢場法（APF）與粒子群算法（PSO）相融合的改進算法，針對原有APF算法應用于局部路徑規劃的局限性，通過引入速度勢場使AGV可以動態避障。與其他算法對比，選擇粒子群算法與人工勢場法相融合，可以在傳統人工勢場法因特殊障礙物陷入局部最小值，或者障礙物距離目標點過近、斥力過大導致算法失效時，跳出算法的局限性；另外，通過調整慣性權重因子和學習因子，縮短算法運行時間，使其適用于局部路徑規劃。仿真結果表明，改進后的算法具有動態避障能力，解決原有算法在應用中可能導致AGV不可達的問題。