基于改進粒子群算法的電動機器人全局路徑規劃

陳玲玲，張占奎，鐘元辰，張忠瑞

(1.吉林化工學院信息與控制工程學院，吉林 吉林 132000；2.國網盤錦供電公司，遼寧 盤錦 124000；3.國網遼寧省電力有限公司，遼寧 沈陽 110006)

自主導航的電動機器人應用范圍包括工業、軍事、救援、太空、農業和家庭[1]，在這些實際應用中，電動機器人的智能能力決定其能否在碰撞移動下順利完成任務。在這些智能能力中，地圖建模和路徑規劃是最智慧、最重要的特征。路徑規劃是智能機器人和車輛自主運行的關鍵要素，是指在一個有障礙物的運動環境中，機器人參考傳感器掃描到的環境信息，尋找1條不觸碰障礙物的幾何路徑，按照這條路徑能夠安全移動到目標位置，其要素包括：①存在能夠移動到終點的安全路線；②路徑不碰到障礙物且長度盡可能小；③耗時少；④無碰撞。路徑規劃算法一般包括環境建模和路徑搜索，每次規劃任務都會有多條到達目標點的路徑，但是最佳路徑的計算取決于一些決策標準，例如路徑長度、航行時間或能量最小化。除了避免碰撞的安全性和效率之外，評估路徑的標準還可以是其平滑性和可行性、計算路徑所需的計算資源。許多優化算法已被用于路徑規劃問題[2-3]。這些算法分為經典方法、元啟發式方法2種。通常，由于計算花費時間較長且無法處理環境中偶然性情況，經典方法不常用于實時移動路徑規劃。與不能很好解決復雜環境問題的經典算法相比，元啟發式算法易于實現，具有處理環境中存在的不確定性的能力[4-5]。

為了有效解決這個問題，學者們提出了不同的方法和思路。Mohanata提出了一種新穎的基于Petri-Net模型與遺傳算法(genetic algorithm, GA)相結合的方法，構成了集成導航控制器[6]。Oleiwi開發了一種將蟻群優化算法與GA相結合的混合方法，使用蟻群優化算法獲得無障礙的次優路徑，然后作為GA的初始路徑[7]。ZHANG B提出了一種受鴿子啟發的算法來規劃UCAV的三維路徑[8]。李志錕等[9]對信息素分配和信息素含量的方法進行優化，并在引入權重參數改進轉移概率公式。

粒子群優化算法(particle swarm optimization，PSO)是由Kenned博士和Eberhart教授通過觀察群體動物捕食行為發明的一個進化計算方法[10]，靈感來自于自然生物的運動表征，研究員發現個體之間通過某些簡單的交互方式能夠完成整個群體的繁雜活動，局部最優通過某種搜索規則和個體之間相互作用產生全局最優解。PSO最初用于解決非線性連續優化問題，主要應用之一是機器人的路徑規劃[11-12]。由于PSO需要配置的參數較少，并且數學模型較為簡單，不涉及比較復雜的數學計算等優點，吸引了很多學者的關注并對其進行研究和改進，以方便實際應用。然而，標準的PSO存在著提前收斂和全局收斂缺乏保證的缺點，這限制了其在實際系統中的應用，于是很多學者對其進行了改進。文獻[13]引入多個因子參數動態調整算法參數，并使用魚群的跳躍機制增加粒子的多樣性。XIE M將最大化采集數據量、路徑安全性和最短行駛距離作為優化目標，并使用最小-最大歸一化方法計算適應度[14]。Girija提出了將人工勢場和粒子群結合的混合算法[15]，該方法將PSO與有源濾波器相結合，提高了在高障礙物密度環境下確定可行且代價最小路徑的速度，并且能夠在較短的時間里，規劃出安全的可行駛路徑。本文提出了將粒子適應度差值作為算法的反饋信號來動態調節慣性權重和引入隨機因子的改進粒子群算法(improved particle swarm optimization，I-PSO)，為自主電動機器人或車輛在具有許多不同形狀和大小的靜態障礙物環境中找到從起點到目的地的無碰撞全局路徑。

1 環境模型

對運動空間抽象出模型是執行路徑規劃算法的前提，其作用是將機器人的工作空間映射成路徑規劃算法可以使用的抽象空間。將工作空間抽象成合理的模型能夠方便計算機存儲和計算，并且可以縮短優化算法探索的時間。為了方便對空間環境建立數學模型，本文采用的是根據柵格法建立的二維地圖模型。將周圍環境看成二維平面，將平面進行等面積的柵格化處理，空間中無障礙物位置對應的單元格，用白色填充，障礙物位置對應的單元格，用黑色填充，柵格中的數字代表機器人通行該位置所需要的能量值。目標是在基于柵格的地圖上找到1條能夠使機器人安全通行并最終到達目標點的路徑點集合，且這個集合組成的路徑是所有能到達目標位置的路徑中長度最短的。路徑規劃中需要考慮的重點是所選路徑既不能在障礙物位置，也不能與障礙物位置交叉。地圖模型如圖1所示。

圖1 地圖模型

為了保證機器人不與障礙物發生碰撞，單元格的大小應綜合考慮實際環境空間和機器人的尺寸大小，并且在構建柵格地圖時需要對空間中的物體及機器人活動區域邊界進行放大，障礙物的大小應向外延伸到機器人自身物理半徑與安全距離之和，這樣在路徑規劃時就可以把機器人看作質點。

2 路徑規劃數學模型

規劃路徑的目的是尋找1條滿足多個約束條件的路徑曲線。本文將最短路徑和障礙物碰撞風險作為量化尋找路徑的標準。

2.1 路徑長度函數

在路徑規劃方向，最短路徑意味著最小化出發位置到目標位置的路徑長度，使用歐幾里得距離計算2個位置坐標的長度。

(1)

式中：(xi(t),yi(t))代表粒子i在t時刻的坐標位置。

在迭代中，如果第i個粒子的位置與目標點和起點之間的距離最短，則被選為最佳點。路徑規劃算法計算出中間節點、起始位置和目標位置之間的連線長度的累加。

(2)

式中：f1是最短路徑的目標函數；(xs,ys)是規劃任務的起點坐標；(xg,yg)是規劃任務的終點坐標。

2.2 碰撞風險

機器人最重要的目標是不能與工作環境中的障礙物接觸并保持一定的安全距離。因此，需要判斷得到的路線是否與障礙物有交點，或者與障礙物之間的距離太近。為了判斷路徑是否可靠，本文采用下式計算路徑的碰撞風險。

(3)

(4)

(5)

式中：(xi,yi)是第i個粒子搜索到的路徑節點；(xod(k),yod(k))是地圖中第k個障礙物的中心坐標。

基于以上2個數學模型，構造包含避免與其路徑上的障礙物碰撞和最小化機器人規劃出的路徑歐氏距離的目標函數。函數是通過2個目標函數的加權獲得。

F=λ1f1+λ2f2

(6)

式中：λ1、λ2分別是最短路徑權重和碰撞風險權重。

3 I-PSO的路徑規劃

PSO的消息分享方式可以看成一種協作共生的行為，也就是每個粒子都不斷探尋搜索空間，在其探尋過程中，種群中其他粒子對它的動作也會有不同程度的影響，同時粒子還會記錄過往的最優答案，即其探尋最佳答案的過程會同時受其他粒子影響和自身經驗的指引。假設在1個D維搜索空間中，存在N個粒子，每個粒子都會根據下式來迭代更新自身的坐標和速度。

vi(t+1)=ωiυi(t)+c1r1(t)[pi(t)-xi(t)]+c2r2(t)[pg(t)-xi(t)]

(7)

xi(t+1)=xi(t)+vi(t+1)

(8)

式中：ωi是粒子i的慣性權重；pi(t)是粒子i在t時刻搜索到的最優位置；pg(t)是t時刻搜索到的全局最優位置；c1是設置粒子向個體最優方向每次運動長度的群體學習因子；c2是設置粒子向群體最優方向每次運動長度的群體學習因子；r1和r2為[0,1]范圍內的均勻隨機數；vi∈[-vmax,vmax]，vmax是粒子的最大速度。

粒子的個體極值pi更新函數定義為式(9)。

(9)

式中：f(xi,(t))是適應度值。

整個群體的全局極值pg的更新函數見式(10)。

pg(t)=min{f(p1(t)),f(p2(t)),…，f(pD(t))}

(10)

式中：f(p1(t))是個體極值。

3.1 慣性權重的調整

為了權衡粒子的全局探索和開發特性，可以改變每個粒子移動速度的慣性權重因子，而且慣性權重ω的值越大，種群的全局探索能力就會較好，當ω的值越小時，粒子群的局部搜索能力會較強。因此可以通過靈活地調整ω的方式，決定種群在搜索過程中何時增大全局搜索或者加強局部搜索。

(11)

(12)

3.2 學習因子的調整

在種群搜索過程中，讓c1維持較大的值能不斷擴大種群的探索區域，但是這樣會導致算法過慢收斂。讓c2維持比較大的值能使粒子盡可能的獲取鄰近粒子的信息，從而加快算法的尋優速度，但是得到的解可能不是最優的。因此加速度的取值方法對是否能夠找到最優解有很大作用。為了讓粒子在算法初期盡量多的執行全局搜索，在算法后期擁有顯著的收斂速度，應在算法前期適當增大c1且減小c2，在算法后期適當減小c1，同時增大c2。加速因子的更新公式設計見式(13)和式(14)。

(13)

(14)

式中：c1b，c1e，c2b，c2e分別是粒子自身和群體學習因子的初始值和最終值。

為了降低掉入局部陷阱的概率，本文在更新粒子位置時，引入一個隨機因子，以增加粒子探索的隨機性。

Li(t)=Li(t)(1+r)

(15)

式中：Li(t)是第i個粒子在第t次的位置；r是[-0.1, 0.1]的隨機值。

3.3 PSO流程

PSO流程如圖2所示。

圖2 路徑規劃流程圖

a.設置N、D、tmax、Vmax，并初始化ω、c1b、c1e、c2b、c2e、v和x；

b.根據式(9)和式(10)計算粒子的F、p，并選出種群的pg；

c.根據式(11)—(14)，動態調整每個粒子的ω、c1和c2。

d.根據式(7)和式(8)，計算下一次迭代粒子的v和x。

e.判斷是否達到規劃任務的終止要求。若達到，結束迭代，否則轉到步驟b。

4 仿真試驗

使用MATLAB模擬二維柵格地圖,使用I-PSO和標準PSO規劃路徑，結果如圖3所示。為滿足PSO的收斂和穩定條件[18]，參數設置為：ωmax=0.8，ωmin=0.01，c1b=1.5，c1e=1，c2b=1，c2e=1.5，tmax=100，N=100，D=1，環境中設置較少的障礙物。

圖3中，藍色曲線是基于標準PSO規劃的路徑，紅色曲線是I-PSO規劃的路徑，從圖3可看出在障礙物較少時，2個算法找到的路徑基本相同，都規劃出了較優的路徑。

圖3 規劃的路徑1

圖4是2種PSO在迭代搜索最優路徑時函數適應度值變化圖形。I-PSO的適應度值迭代曲線，在迭代開始階段，其值明顯小于標準PSO的適應度值。在迭代中期，I-PSO的適應度值已經趨近于穩定。可以看出I-PSO較標準PSO在收斂速度和收斂效果上都更有優勢，在簡單的環境下能更快的規劃出較優的全局路徑。

圖4 適應度值1

為了進一步驗證算法的可行性，增加地圖中障礙物的數量，以測試I-PSO在面對不同環境下規劃路徑的能力。搜索空間維度D=2，其他參數不變，然后進行仿真試驗，得到規劃路徑如圖5所示。

圖5 規劃的路徑2

圖5是改變工作環境內障礙物的密度后，2種算法在較為復雜環境下所規劃的全局路徑。圖5中，I-PSO搜索到的安全路徑相比于標準PSO規劃的路徑更趨近于直線，且軌跡長度更小。從圖5可看出，當地圖中的障礙物較多，環境較為復雜情況下，I-PSO相比標準PSO能找到更短的安全路徑，且路徑的拐點角度更小。

圖6是障礙物較多時，2種算法在迭代尋找路徑時的適應度值變化曲線。從圖6可看出，當障礙物增多后，標準PSO在起始階段收斂性較差，且算法的穩定性也不好，而I-PSO在起始階段就能很快找到較好的解。

圖6 適應度值2

圖7和圖8是在障礙物密度更大、活動空間更有局限性的環境下，2種算法規劃的路徑和對應的適應度值迭代變化圖形。可以看出，I-PSO在更為復雜的環境下也能規劃出安全、平滑和長度更短的全局路徑。

由上述在3種環境下仿真得到的試驗結果表明，本文提出的I-PSO較傳統PSO能更快的規劃出安全且距離更短的全局路徑，并且在算法的收斂性和穩定性上都更優。

圖7 規劃的路徑3

圖8 適應度值3

5 結語

本文使用柵格法對機器人移動空間進行環境建模，為了使規劃的路徑更加安全、更適合機器人移動，建立了考慮路徑長度、碰撞風險的路徑規劃數學模型。對于傳統PSO在規劃路徑時存在易陷入局部最優以及收斂速度慢的問題，本文采用將粒子適應度差值作為算法的反饋信號來動態調整慣性權重的方式調節種群對搜索空間的整體或部分區域的搜索需求，并且引入隨機因子盡可能避開局部陷阱，這種方法在處理PSO過早收斂和最優解質量較差的問題。MATLAB試驗結果表明,提出的I-PSO具避障性能優越、規劃的移動路徑長度更短、路徑更平滑等優點。

然而，本文主要解決的是在靜態空間環境下的路徑規劃問題，缺少對動態障礙物的處理。在之后的研究中將對存在障礙物移動的環境進行數學建模，并將本文的算法進行改進，使其能在動態工作空間中進行規劃路徑。