PSO 優化算法在平滑路徑規劃中的應用

周釗揚，穆平安，張仁杰

（上海理工大學光電信息與計算機工程學院，上海 200093）

0 引言

路徑規劃（Path Planning）是指運用所編制的算法使目標在障礙環境中，根據任務要求和評判準則自行找到并規劃從起點至目標點的最優路徑［1］。隨著相關智能技術的一系列突破和發展，其在軍事、醫療等高新科技領域都已得到了廣泛應用［2］，在未來也有著廣闊的發展前景。

研究者們又將路徑規劃算法與移動機器人相結合，主要涉及到的智能算法有：模擬退火算法、人工魚群算法、粒子群算法等［3］，以更好地解決障礙環境下移動過程中的路徑規劃問題。其中，粒子群（PSO）算法有著結構簡單、通用性強、在迭代過程中搜索效率更高等優點［4］，但同時也存在著缺乏對速度的動態調節、收斂精度低、搜索停滯等不足［5］，這對于路徑規劃效率和準確度都會產生一定影響。

實際應用中的粒子群算法大多先通過隨機解來初始化粒子，再在反復迭代過程中尋找最優方案，而忽略了路徑是否平滑等指標［6］，這將導致只有最優粒子參與其中，且規劃產生的路線較依賴于直角坐標系環境［7］。當環境為球面坐標或極坐標時，PSO 算法則常生成多折角相連的不平滑路徑，從而造成機器人功耗加大及效率降低［8］。通過綜合考量，對機器人行進路徑的平滑度進行優化是眼下粒子群算法改進的重要方向之一。

本文針對上述問題，提出一種改進的自適應粒子群算法，并結合貝塞爾曲線［9］以提高機器人尋找最優路徑的平滑度和效率。為了更快找到局部最優解并避免收斂緩慢的問題，在ES-PSO 中采用較大的控制參數值以平衡慣性權重，進而提高粒子的有效性。此外，通過帶有罰函數的適應值函數優化策略，使機器人可以更快速、準確地得到全局最優路徑，且優化過后的平滑路徑更適合移動機器人行進。

1 粒子群算法優化

1.1 算法基本原理

PSO 算法始于對鳥類捕食狀態的研究，其使N維搜索區域中的每個粒子都具有表示位置的xi和表示速度的vi兩個矢量［10］。每次迭代時，粒子都將學習自身的歷史最佳位置并與群體交換，從而對信息進行搜索與更新［11］。令分別為第i個粒子的速度矢量和位置矢量，M為種群中的粒子數量。在標準PSO 算法中，可通過以下公式對粒子的兩個矢量進行更新：

速度向量更新的數學模型為：

位置向量更新的數學模型為：

式中，pBesti是粒子i(i=1,2,…,M)歷史上的最佳位置，gBest(gBest1,gBest2,…,gBestn)是整個群體歷史上的最佳位置，k為當前迭代次數，c1、c2為學習因子，分別是衡量pBesti和gBest相對重要性的兩個參數，r1、r2則為［0，1］區間內的隨機數。

式（1）中的Vi(k)為“自身記憶項”，表示粒子由于自身慣性，將向前一次迭代的方向移動；c1r1(pBesti(k) -xi(k))為“自身認知項”，表示粒子會向著歷史最佳位置處移動，代表了粒子自我學習的能力；c2r2(gBest(k) -xi(k))為“群體認知項”，表示粒子向種群最佳位置處移動的能力［12］。因此，其整體可視作粒子在解空間內不斷移動以尋找最優解的過程。

在每次迭代中，每個粒子自身所在位置都對應一個適應度值f：

其中，f(xi)為算法實際需要求解的函數。

1.2 PSO 優化算法

針對粒子群算法在二維場景下缺乏對速度的動態調節以及容易出現局部最優等局限性［13］，提出一種自適應粒子群算法，該算法對種群進化狀態評估、自適應慣性權重選擇和抑制最優值區域擾動3 個方面進行了優化。

1.2.1 種群進化狀態評估

傳統PSO 算法是根據種群中心位置計算進化因子，而對種群的進化狀態往往考慮欠佳。這將導致當兩個距離較近的粒子開始進化時，若其速度矢量差別較大，則迭代后的位置可能相距較遠［14］。因此，針對這種評估方法存在的問題，本文提出改進的種群進化狀態判斷方法。

設f(gBest(k))表示第k代時種群中最佳粒子的適應值，因此在全部迭代中，{f(gBest(0)),f(gBest(1)),…,f(gBest(k))}是一個非升序列，即：

設f(pBesti(k))表示第k代時粒子i的歷史最佳位置適應值，則對于每個粒子，{f(pBesti(0)),f(pBesti(1)),…,f(pBesti(k))}也是一個非升序列，即：

因此，全部pBesti(i=1,2,…,M)的適應值之和滿足：

在3 個序列中，δgBest(k)、δpBesti(k)和(k)依次為全局最佳粒子改進、單個粒子歷史最佳位置改進以及集體歷史最佳位置改進，由此改進可以有效地對種群進化狀態進行更實際的判斷。

1.2.2 自適應慣性權重選擇

慣性權重在PSO 算法中起著調整局部搜索和全局搜索的重要作用，通常呈遞減狀態變化。但合理的慣性權重需同時考量搜索空間維度、當前搜索狀態、種群大小等多種因素才能提高搜索效率，避免粒子慣性分量造成的不利影響［15］。因此，為了同時提高收斂精度和效率，本文針對慣性權重的特點為學習因子設計一個自適應控制器。

學習因子包括c1和c2。c1通過自我學習使自身找到歷史最佳位置，有助于搜索新區域；c2則幫助粒子群提高全局搜索能力，加快收斂速度。在不同的種群進化狀態下，具體策略如下：

（1）當處于探索狀態時，應探索盡可能多的最優值，提高模型求得最優解的能力。此時，為提高粒子搜索速度并進行精細化搜索，應增加c1值并減少c2值，避免其趨向于局部最優。

（2）當處于收斂狀態時，理論上應增加c2值并減少c1值，使粒子群朝全局最優位置移動。但由多次實驗結果來看，這種策略會加快參數變化，不利于平衡搜索能力和收斂速度。因此，此時應增大c1和c2值。

（3）當處于困死狀態時，應盡量使群體脫離當前全局最優。此時應減少c1值并增加c2值，從而促使粒子跳離困死狀態，在其它位置再次生成全局最優。

綜上所述，本文設計的自適應控制器決策樹如圖1 所示。

基于該決策樹，并通過大量實驗嘗試，將學習因子c1和c2初始化為2.0。實驗結果表明，動態自適應學習系數可使系統尋找到更好的全局最優點。

1.2.3 抑制最優值區域擾動

當處于困死狀態時，粒子均困在當前全局最佳區域，應盡快使粒子脫離該狀態，向更優區域移動［16］。因此，本文設計了一種跳出局部最優的策略，可以改善傳統策略成功率及效率較低的缺點。具體策略如下：

式中，Xmax、Xmin依次表示搜索區域上限和下限，j為粒子維度。當粒子跳出但并未尋至更優區域時，將種群還原回之前的狀態并在下一代時重新跳出，直至進化狀態得到優化。大量實驗結果表明，該策略可以更高效地跳出當前全局最佳區域，抑制最優區域的擾動，尋找到更精確的最優值。

Fig.1 Adaptive controller decision tree圖1 自適應控制器決策樹

1.3 算法流程

算法具體流程如下：

步驟1：對種群中的粒子位置與速度等參數進行初始化，令迭代次數k=0。

步驟2：計算各粒子的適應度值。

步驟3：根據式（1）所示模型對速度向量進行更新。

步驟4：根據式（2）所示模型對位置向量進行更新。

步驟5：按式（8）計算上一次迭代后的適應度值，根據適應度更新粒子的歷史最佳位置。

步驟6：計算δgBest(k)、δpBesti(k)和(k)，并評估種群進化狀態。

步驟7：根據評估結果，更新當前條件下的慣性權重ω以及學習因子c1、c2。

步驟8：判斷收斂情況，若種群處于困死狀態，執行步驟9；若非困死狀態，跳至步驟10。

步驟9：依照式（7）執行跳出策略。

步驟10：判斷終止情況，若滿足條件，則算法結束；若不滿足，跳回步驟2。

2 路徑平滑度優化

通過傳統PSO 算法規劃出的路徑常常存在轉彎折線多、路徑不夠平滑、對機器人耗損大等問題，因此路徑規劃研究具有重要的理論意義與實際價值［17］。其不僅能提高機器人行進過程中的靈活性，而且是機器人完成其他任務的先決條件。本次研究將通過ES-PSO 算法求解出的路徑節點與Bezier 曲線相結合，以提高機器人移動的平滑度。

Bezier 曲線是一種參數曲線，是由伯恩斯坦多項式發展演化而來的，因其具有獨特的控制方式和對曲線強大的擬合能力，被大量應用于工業領域［18］。在組成它的眾多控制點中，只有起始點和終點在曲線上，其它點均不在曲線上。曲線表示方法如下：

設一組控制點向量為P0,P1,…,Pn，則Bezier 曲線可表示為：

其中，t是歸一化的時間變量，Pi=(xi,yi)T代表第i個控制點的坐標向量，xi、yi分別為X和Y的坐標分量，Bni(t)為Bernstein 多項式：

在二維平面上，Bezier 曲線的曲率可表示為：

其中，R(t)為曲率半徑為Bezier 曲線P(t)對x 和y 的一階、二階導數。

Bezier 曲線的平滑度與路徑的曲率函數息息相關，接下來將融合Bezier 曲線與ES-PSO 算法對機器人路徑規劃進行仿真。

3 路徑規劃相關描述與建模

3.1 路徑初始化

傳統PSO 算法的路徑初始化策略往往會規劃出不必要的路線和曲折的拐彎，但這樣的路徑并非最優路徑，且會增加算法花費的時間和機器人損耗［19］。因此，本文為路徑初始化策略提供了新思路，以期在路徑初始化階段即得到更好的規劃效果。

將路徑起點與終點之間的線段看作線段D。在連線D上除起點和目標點外，取維度N-2 個點，則所有點的坐標都可記作：

從每一點作連線D的垂線，標記為l1,l2,…,lN-2。在l1,l2,…,lN-2上任意各取一點，標記為p2,p3,…,pN-1。從起點按順序連接p2,p3,…,pN-1等所有點。

根據該方案規劃出的路徑可以消解多余的曲折，更貼近機器人實際的最佳運動軌跡。如果路線穿過障礙物，則需要對其進行排除。將根據此方案及其它方案初始化得到的初始路徑進行對比，如圖2 所示。其中，紫色路徑為起始點至目標點間的最短距離，綠色路徑為隨機初始化的初始路徑，紅色路徑為根據本文策略初始化的初始路徑。

由圖2 可知，基于傳統PSO 算法路徑初始化策略規劃的路線往往具有隨機性，會對接下來機器人行進造成一定影響。而采用本文策略進行初始化，則能更好地得到接近最優的初始路徑，很大程度上避免了機器人在折線上的能量損耗，并節約了時間成本。

Fig.2 Initial path under different initialization strategies圖2 不同初始化策略下的初始路徑

3.2 結合罰函數定義適應值函數

適應值函數的選用對提高收斂精度和速度來說至關重要，而機器人行進過程中的安全保障也應作為必要因素考慮進路徑規劃中。在常規的PSO 算法中，往往選取路徑長度函數對適應值函數進行定義，具體如下：

式中，Dob表示障礙存在區域，在此設罰函數為200。設Dsa為保護區域，路徑穿過此區域時罰函數會變大，余外區域的罰函數都為0。d表示粒子至障礙物中心的長度，rob表示障礙存在區域的半徑，rsa表示保護區域的半徑。則由本文障礙物的環境地圖映射得到罰函數地圖，如圖3 所示。

將上述兩個函數相結合，重新定義適應值函數如下：

其中，a、b依次為長度函數權重與罰函數權重，本文均取值為0.5。

4 仿真與分析

基于MATLAB 建立仿真平臺，在相同設定下對本文提出的改進ES-PSO 算法與傳統PSO 算法作仿真測試和對比分析，并證明了本文初始化策略的可行性。

Fig.3 Penalty function map圖3 罰函數地圖

4.1 初始化策略效果

設機器人運動的起點坐標為[50,50]，終點坐標為[450,450]。根據本文提出的自適應粒子群ES-PSO 算法進行路徑規劃，折線為粒子群算法規劃出的路徑，曲線為結合Bezier 曲線生成的平滑路徑，比較有初始化策略和無初始化策略時算法尋優精度與尋優速度的差異。

設算法種群粒子數量M為30，路徑節點N為5，迭代200 次，限制粒子速度范圍為[-50,50]。慣性權重ω=[0.9,0.4]，設學習因子c1和c2的初始值為2.0。ω、c1和c23個參數則通過算法迭代過程中種群進化狀態的變化實現自適應控制。

如圖4 所示，實線和虛線分別表示采用初始化策略和無初始化策略時的適應值變化曲線對比。

Fig.4 Initialization strategy fitness value change curve comparison圖4 初始化策略適應值變化曲線對比

由圖4 可得，二者雖變化趨勢相同，但采用初始化策略后的曲線在另一曲線下方，且更早趨于平緩，表示采用初始化策略有效提高了PSO 算法的尋優能力和運算效率。

為了提高測試準確度，采用兩種方式重復進行100 次測試，得到平均適應值函數Fitness、平均迭代次數Iteration和路徑規劃成功率Success，如表1 所示。由表可知，執行初始化策略后的路徑規劃成功率更高，表明初始化策略可增強路徑規劃的成功率。

Table 1 Initialization policy effects表1 初始化策略效果

4.2 算法尋優性能比較

分別在簡單和復雜環境下比較PSO 算法、GPSO 算法、APSO 算法及本文提出的改進ES-PSO 算法的尋優性能。設定4 個規則的圓形障礙區域為簡單測試環境，圖5、圖6（彩圖掃OSID 碼可見，下同）分別為簡單測試環境下4 種粒子群算法的路徑規劃結果與適應值變化曲線。其中，藍色曲線表示PSO 算法，紅色曲線表示GPSO 算法，黃色曲線表示APSO 算法，紫色曲線表示ES-PSO 算法。

Fig.5 Planning path results in a simple environment圖5 簡單環境下路徑規劃結果

Fig.6 Adaptive value curve in a simple environment圖6 簡單環境下適應值變化曲線

由圖5 可知，4 種算法均可規劃出規避障礙的有效路徑，但ES-PSO 算法能夠跳出局部最優，得到更佳路徑。在該環境下重復測試100 次，實驗結果如表2 所示。

由表2 可知，PSO 算法在4 種方法中收斂速度最快，但易陷入局部最優，成功率較低。而ES-PSO 算法在明顯提高尋優效率和保證收斂速度的同時，規劃正確率也維持在一個較高水平。

Table 2 Simple environment path planning表2 簡單環境路徑規劃

設定大量障礙物，且障礙物形狀不僅局限于圓形的復雜測試環境，如圖7 所示。圖7、圖8 分別為復雜測試環境下4 種粒子群算法的路徑規劃結果與適應值變化曲線。

Fig.7 Path planning results in a complex environment圖7 復雜環境下路徑規劃結果

Fig.8 Adaptation value curve in complex environment圖8 復雜環境下適應值變化曲線

在該復雜環境下重復測試100 次，實驗數據如表3 所示。

Table 3 Complex environment path planning表3 復雜環境路徑規劃

由表3 可知，當障礙物變得復雜及不規則時，PSO 算法和GPSO 算法都過早進入收斂且成功率有所降低。APSO算法規劃的結果雖維持了精度和成功率，但收斂效率較低。而本文的ES-PSO 算法在效率、尋優精度、成功率3 個指標上均表現良好，相比其它算法的規劃結果展示出顯著的改進效果。

5 結語

圍繞移動機器人如何規劃出一條從起始點到目標點的無碰撞最優路徑這一關鍵問題，本文分析了傳統算法存在的不足，提出一種基于種群進化狀態的自適應粒子群算法（ES-PSO），從種群動態進化狀態判定、自適應控制慣性權重調整、跳出策略選擇幾方面對算法進行優化，提出初始化路徑策略和帶有罰函數的適應值函數策略，并引入貝塞爾曲線以提升路徑平滑度。在簡單環境和復雜環境下將改進后的算法與傳統算法進行仿真比較，結果均表明改進后的算法在測試中成功率較高，能夠在保留標準PSO 算法優點的同時，更加迅速地得到平滑路徑，符合實際需要，具有一定研究價值。另外，如何在動態環境下對粒子群算法的監測及判斷能力等進行改進，以及對三維空間中移動機器人的路徑規劃進行建模等問題，有待后續進一步研究。