改進粒子群算法的移動機器人平滑路徑規劃

陳嘉林，魏國亮，田 昕

1(上海理工大學 光電信息與計算機工程學院,上海 200093)2(上海理工大學 理學院,上海 200093)

1 前 言

路徑規劃是移動機器人應用在生產實踐中所遇到的一個關鍵任務，其目的是從起點到目標點尋找可行且最優的路徑[1].它可以被看作是某些指標(例如，最短距離和最小能量)與某些約束(如無碰撞路線)的優化問題[2].目前，研究人員已經開發出了許多啟發式算法來解決這個問題[3]，其中粒子群優化算法[4]是解決該問題最常用的方法之一[5].

在實際應用中，普通路徑規劃算法主要涉及到某些簡單的最優準則(如路徑的最小長度)[6]，很少考慮路徑的其他重要性能(如路徑的平滑性)[7].傳統的路徑規劃算法可以生成一條由多條折線連接而成的路徑，由此會造成不可避免的急轉彎.沿著這樣的路徑移動會導致移動機器人在不同模式之間頻繁切換(例如，停止、旋轉和重啟)，從而導致不必要的時間浪費[8].此外，在某些情境下，為了保證機器人服務質量，不允許進行狀態切換，從而必須提高其運動平滑性[9].由以上情形可看出，除去考慮路徑長度之外，路徑平滑度也是在機器人路徑規劃問題中的需要尋優的另外一個重要指標[10].

本文針對移動機器人路徑的全局平滑規劃[11]問題，提出了一種新的基于種群進化狀態的自適應粒子群算法，并結合Bezier曲線來尋找機器人最優平滑路徑[12].在ES-PSO中采用自適應控制算法參數[13]，從而加強種群的勘測能力并結合種群群體跳出操作，從而有效避免算法早熟，提高算法對解空間的尋優能力.在算法初始階段，采用初始化策略，加快算法的尋優速度，提升算法的尋優精度.并結合帶有罰函數的適應值函數優化策略，提高平滑路徑規劃的成功率.仿真結果表明針對機器人尋找平滑路徑問題的ES-PSO算法明顯優于傳統的PSO算法.

2 ES-PSO算法

2.1 PSO算法基本原理

速度信息更新迭代公式：

Vi(k+1)=ωVi(k)+c1r1(pBesti(k)-Xi(k))+

c2r2(gBest(k)-Xi(k))

(1)

位置信息更新迭代公式：

Xi(k+1)=Xi(k)+Vi(k+1)

(2)

其中pBesti是粒子i的歷史最佳位置，gBest是整個群體歷史上的最佳位置，ω為慣性權重，k為當前迭代次數，c1和c2為學習因子，分別是衡量pBesti和gBest相對重要性的兩個參數，r1和r2為均勻分布在[0,1]上的隨機數.

2.2 改進粒子群算法

針對粒子群算法容易陷入局部最優和收斂速度慢的缺陷，本文提出了一種ES-PSO算法.該算法改進主要體現在四個方面，分別為種群進化狀態策略、自適應慣性權重、自適應學習因子策略和群體跳出策略.

1)種群進化狀態

種群進化狀態的分析是PSO算法的關鍵內容.且絕大多數改進算法用種群中粒子相互間的平均距離和粒子適應度之間的差異定義種群多樣性[15].

若僅考慮粒子當前位置的狀態，將會忽略整個種群在每一代進化過程中的進化狀態與種群在之后迭代過程中的進化潛力.例如，如果兩個粒子本次迭代中位置較近，而粒子速度矢量差異較大，則在下次迭代后，粒子的位置可能相距較遠.因此，根據種群粒子的當前位置來判斷種群的進化狀態并不合理.

在此，本文提出了一種新的種群進化狀態的測量方法.設f(gBest(k))表示第k次迭代中全局最佳粒子的適應度.對于種群的整個迭代過程， {f(gBest(0)),f(gBest(1)),…,f(gBest(k))}是一個非上升序列，即：

δgBest(k)=f(gBest(k))-f(gBest(k-1))≤0

(3)

其次，設f(pBesti(k))表示第k次迭代中粒子的歷史最佳位置的適應度.對于種群中每個粒子，{f(pBesti(0)),f(pBesti(1)),…,f(pBesti(k))}也是一個非上升序列，即：

δpBesti(k)=f(pBesti(k))-f(pBesti(k-1))≤0

(4)

因此，所有pBesti(i=1,2,…,M)的適應度之和滿足：

(5)

2)自適應慣性權重

由文獻[16]可知，較大的慣性權重有利于全局搜索，而較小的慣性權重有利于算法的局部開發，加速算法的收斂.在進化早期，算法應具有較大的慣性權重，當進化代數的增加時，慣性權重應迅速下降.在進化過程中，慣性權重應隨著進化狀態自適應改變.設ρgbest(k)為種群進化率：

ρgbest(k)=f(gBest(k))/f(gBest(k-1))

(6)

由式(6)可知，在種群勘測狀態下，0<ρgbest(k)<1；在種群收斂狀態和種群困死狀態下，ρgbest(k)=1.在此，ES-PSO引入自適應慣性權重控制器，即考慮種群進化率對慣性權重的影響，采用指數下降形式的慣性權重.自適應慣性權重控制器設計如下：

(7)

3)自適應學習因子

學習因子c1主要是將粒子吸引到自身的歷史最佳位置，幫助探索局部最優值并保持種群多樣性.學習因子c2有助于種群快速收斂[17].此兩種不同的學習機制應在不同的進化狀態下給予不同的處理.在ES-PSO中，設學習因子c1和c2初始值為2.0并進行自適應控制，策略如下：

種群勘測狀態：在勘探狀態下探索需盡可能多的最優值.因此，增加c1和減少c2可幫助粒子單獨探索并優化自身的最佳歷史位置.

種群收斂狀態：當種群處于收斂狀態時，應增加c2的值來引導其他粒子盡快到達全局最優區域.此外，應增加c1的值以防止群體將被當前最佳區域吸引，導致過早收斂.

種群困死狀態：在此狀態下，應增大c2并減小c1值，將有助于算法執行群體跳出策略，即粒子遠離局部極值點.

將c1與c2范圍限制在[1.5，2.5].此外，當c1和c2總和大于初始值之和4.0時.則將c1和c2歸一化為：

(8)

由于上述學習因子的調整需緩慢進行，故兩次迭代之間的最大增量或減量應受到如下限制：

|ci(g+1)-ci(g)|≤ξ，i=1，2

(9)

ξ為“加速率”.相關實驗表明[6]，在區間[0.005，0.01]中均勻生成的ξ隨機值在大多數測試函數中表現最佳.

4)群體跳出策略

當種群處于困死狀態時，即所有粒子圍繞在當前局部最優區域，此時算法需要新的動力來擺脫局部最優區域.在此本文設計最優值區域擾動策略以幫助粒子將自身推向可能更好的區域.由于局部最優值可能具有一些全局最優值的特性.因此，為了保護這些良好的特性，傳統算法只更新局部最優值的一個維度.然而這種方法并不能保證每次跳出操作能夠使粒子成功找到一個新的更優點，且效率較低.因此，本文設計了一種群體跳出策略，即：

(10)

Xmax和Xmin分別為搜索區域的上限和下限.j表示粒子的維度.因為粒子都收斂于當前局部最優區域內，該策略對整個群體中的粒子都執行一次跳出操作，由于本策略只隨機改變一個維度的值，因此每個更新后的粒子仍然保有良好的最優值特性.接著，本算法重新評價了粒子更新后的種群進化狀態，當種群進化狀態轉變為勘探狀態時，種群將向著新的最優區域方向進化.若粒子更新后種群進化狀態沒有得到改善，將更新后的粒子重新回溯到之前的收斂狀態，再次采用群體跳出策略對粒子位置進行更新.隨后的實驗結果證明，群體跳出策略相對于傳統的單個粒子跳出策略，能夠更有效地跳出局部極值點，尋找到更加精確的最優值.

3 Bezier曲線

Bezier曲線是一種在計算機圖形學等實踐中得到廣泛應用的參數曲線[18].Bezier曲線由許多控制點組成，除了起始點和終點，控制點不在曲線上.設一組控制點向量為P0，P1，…，Pn，則Bezier曲線可表示為：

(11)

(12)

由于Bezier曲線的光滑性與路徑的曲率函數關系密切.則在二維平面上，Bezier曲線的曲率可以表示為：

(13)

(14)

(15)

本文將Bezier曲線結合到路徑規劃過程中，將ES-PSO算法求解得到的路徑節點作為控制點，則可生成平滑路徑.

4 路徑規劃問題描述與建模

在機器人路徑規劃問題中，當用PSO算法求解該問題時，每個粒子代表一條合理的路徑[19].設有M條可行路徑，即粒子表示為p[p1(x1，y1)，p2(x2，y2)，…，pN(xN，yN)]，N代表路徑節點的個數，p1為起始點，pN為終點.每個路徑節點代表了路徑的一次轉向，大量實驗表明，在復雜的環境中，機器人只需通過3～5次轉向就可避開障礙物尋找到一條最優路徑.因此粒子維度不會隨著環境的復雜度而大幅增加.

4.1 路徑初始化策略

PSO算法的初始化策略通常為隨機策略，然而隨機選擇路徑節點很可能導致生成的初始路徑穿過障礙物，或使路徑變得較為曲折，增加算法的求解時間.本文設計了一種新的初始化策略，可以在路徑初始化時，得到接近最優解的初始路徑.具體方法如下：

連接起始點和終點，記為線段L.在線段L上均勻取粒子維度N-2個點(去除起始點和終點)，那么每個點的坐標都是確定的，記為：

(16)

在每一點處，作關于線段L的垂線.記為l1，l2，…，lN-2.在每條垂線上隨機取一個點，分別記為p2，p3，…，pN-1.最后從起始點將各個點依次連接.

由以上方法可得到盡可能符合機器人運動趨勢的路徑，但路徑仍然有可能穿越障礙物，成為非法路徑.如果初始得到的路徑是非法路徑，則將該路徑刪除，重新生成初始路徑.非法路徑存在兩種情況：

1)路徑節點位于障礙物內部：

本文默認將障礙物區域合理填充或分割為圓形區域.因此只需判定路徑節點到圓心的距離dp是否小于圓的半徑r即可.

2)路徑線路穿越障礙物：

2.4.1 控制活動步驟不健全。控制活動的步驟是否完善決定了這次活動質量完成效果如何，完整的控制活動步驟促進內部控制的實施，但在單位內部控制活動數量不僅較少，控制活動設計步驟也不完善，單位風險加大，不利于內部控制活動進一步向前推進。

即使路徑節點沒有在障礙物內部，兩路徑節點連線生成的路徑線路也有可能穿越障礙物，從而導致路徑非法.在此情況下，本文介紹一種判斷方法來判斷路徑線路是否穿越障礙物，詳細如下.

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

首先計算障礙物圓心到路徑線路之間的垂直距離dl.如果dl>r，則表示路徑線路沒有穿越障礙物.如果dl

由圖1示例可知，傳統的隨機初始化方法得到的路徑具有不確定性，生成的路徑曲折并有可能穿越障礙物，不利于之后算法迭代尋優.本文算法提出的初始化方法，能夠使算法得到較好的初始化，將會減少算法的求解時間，并提升尋優精度.

4.2 帶有罰函數的適應值函數

傳統的PSO算法，其適應值函數一般定義為路徑長度函數，即：

Fl=‖p1-p2‖+…+‖pN-1-pN‖

(17)

‖p1-p2‖表示p1與p2表示與節點之間的距離.但在實際路徑規劃中，路徑的安全性有時比路徑的長度更受重視.為使機器人預先感知障礙物，做好避障準備.本文在障礙物周圍設置一個“防護區域”，并設計了基于罰函數的適應值函數.當穿越“防護區域”時，罰函數會相應增大：

(18)

Dob代表障礙物區域，在此區域，設定罰函數為200.Dsa為防護區域，在此區域，當前點的罰函數與障礙物中心ob的距離成反比.其他區域的罰函數都為0.d為粒子距離障礙物中心的距離，rob為障礙物區域半徑，rsa為防護區域的半徑.

本文結合路徑長度函數與罰函數，將適應值函數定義為：

F=aFl+bFf，0≤a，b≤1，a+b=1

(19)

a，b代表分別為長度函數權重與罰函數權重.本文a，b取0.5.

4.3 算法流程

算法的流程描述如下：

Step 1.按照初始策略初始化粒子群，包括各參數：粒子位置Xi、速度Vi、粒子的歷史最佳位置pBesti，整個群體歷史上的最佳位置gBest，慣性權重ω，學習因子c1、c2.

Step 2.按式(1)和式(2)更新粒子速度和位置.

Step 3.將粒子位置作為控制點，得到平滑的Bezier曲線.

Step 4.計算種群中各粒子的適應度.

Step 5.根據適應度更新粒子的歷史最佳位置.

Step 6.根據適應度更新粒子的全局最佳位置.

Step 8.根據種群的進化狀態，更新慣性權重ω，學習因子c1、c2.

Step 9.判斷種群是否為困死狀態，如果種群處于困死狀態，轉Step 10.否則轉Step 11.

Step 10.根據式(10)執行群體跳出策略.

Step 11.判斷算法終止條件，如滿足則停止.否則轉Step 2.

5 實驗結果與分析

本節首先將驗證初始化策略的有效性.接著在兩個測試環境上使用ES-PSO算法與其他改進PSO算法進行路徑規劃，并比較算法的尋優精度與尋優速度.

5.1 初始化策略的效果

假設在一個測試環境中，共設置四個圓形障礙物，機器人起點坐標為[50，50]，終點坐標為[450，450].算法種群粒子數量M設為30，路徑節點N設為5，迭代200次，粒子速度限制為[-50，50].慣性權重ω=[0.9，0.4]，學習因子c1和c2初始值設為2.0.ω、c1和c2三個參數隨算法迭代過程中種群進化狀態的變化實現自適應控制.

在使用本文提出的ES-PSO算法進行路徑規劃時，折線為ES-PSO算法規劃出的路徑，曲線為結合Bezier生成的平滑路徑.在比較初始化策略的效果時，主要比較算法的尋優精度與尋優速度的差異.

圖2 初始化策略下的結果比較Fig.2 Comparison of results using initialization strategy

由圖2可知，采用初始化策略后的迭代曲線相比未采用初始化策略的迭代曲線較低，則表明初始化策略可增強算法的尋優精度.此外，采用初始化策略的曲線，率先進入平穩狀態，則表明初始化策略可縮短算法的尋優時間.

本實驗重復測試100次，得到平均適應值函數Fitness、平均迭代次數Iteration和路徑可行的成功率Success，如表1所示.由表1可得，采用初始化策略后的路徑規劃成功率比無初始化策略較高，則表明初始化策略可增強路徑規劃成功率.

表1 初始化策略效果
Table1 Initialize policy effects

ES-PSOFitnessIterationSuccess無初始化734.126683%有初始化673.482397%

5.2 算法尋優性能比較

在如圖4和圖6所示的測試環境下比較標準粒子群算法PSO[1]、全局粒子群算法GPSO[20]、自適應粒子群算法APSO[21]和本文提出的ES-PSO算法尋優性能.

為增強算法比較的公平性，所有算法種群粒子數量M設為30，路徑節點N設為5，迭代2000次，粒子速度限制為[-50，50].PSO算法中，慣性權重ω設為固定值0.4.學習因子c1和c2設為固定值2.0.GPSO算法中，慣性權重ω=[0.9，0.4].ω隨迭代次數線性遞減.APSO算法中，學習因子c1和c2初始值設為2.0.ω、c1和c2三個參數隨算法迭代過程中種群多樣性的變化實現自適應控制.ES-PSO算法中，慣性權重ω=[0.9，0.4]，學習因子c1和c2初始值設為2.0.ω、c1和c2三個參數隨算法迭代過程中種群進化狀態的變化實現自適應控制.

圖4所示的簡單測試環境，用以檢驗本文所提出的ES-PSO算法在尋優速度方面的性能.在此設置四個圓形障礙物，機器人起點坐標為[50，50]，終點坐標為[450，450].圖3和圖4分別為在簡單測試環境下四種粒子群算法適應值變化曲線與規劃所得路徑.

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

由圖4可知，四種算法都找到了可行路徑，ES-PSO算法可以跳出局部最優值，尋找到更加優化的路徑.在該測試環境下重復測試100次，實驗數據如表2所示.由表2可知，PSO算法因為結構最為簡單，收斂速度最快，但是會陷入局部最優值，導致算法尋優精度不高，甚至影響路徑規劃的成功率.ES-PSO在保持算法快速收斂的特性下，顯著提升了算法的尋優精度，并保證路徑規劃的成功率.

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

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

四種算法FitnessIterationSuccessPSO702.982590%GPSO693.812693%APSO682.675196%ES-PSO669.342998%

如圖6所示的復雜測試環境，用以檢驗本文所提出的ES-PSO算法在尋優精度方面的性能.設置大量障礙物，且障礙物形狀不僅限于圓形，機器人起點坐標為[50，50]，終點坐標為[450，450].圖5和圖6分別為復雜測試環境下四種粒子群算法適應值變化曲線與規劃所得路徑.

在圖6所示測試環境下重復測試100次，實驗數據如表3所示.由于測試環境變得復雜，PSO算法和GPSO算法因為沒有跳出局部最優值的策略，算法過早收斂，導致路徑規劃的成功率下降.APSO算法尋優精度和成功率都較為滿意，但是損害了算法快速收斂的特性.ES-PSO算法在尋優精度，尋優速度和成功率三個性能方面，相比其他三種粒子群算法，都有明顯優勢.

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

圖6 復雜環境下規劃路徑結果Fig.6 Planning path results in a complex environment

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

6 結 論

本文提出了一種基于種群進化狀態的自適應粒子群算法(ES-PSO).ES-PSO的特點是根據種群在連續迭代的過程中，粒子歷史最佳位置和種群全局最佳位置的改進情況，從而判定種群的進化狀態.并根據種群的進化狀態設計自適應控制器用于調節慣性權重與學習因子.ES-PSO保留標準PSO算法的快速收斂特性，并采用群體跳出策略擺脫局部最優區域，防止算法早熟.將ES-PSO算法應用到路徑規劃實際問題中去，并采用本文提出的初始化路徑策略和帶有罰函數的適應值函數策略，在兩種測試環境下進行實驗，檢驗算法性能.實驗測試結果表明，ES-PSO算法在測試中脫穎而出，具有顯著的優勢，并結合Bezier曲線，可得到平滑的路徑，更加符合實際機器人運動情況，是一種有效的尋優算法.此外，由于ES-PSO算法具有很好的實時性，可以進一步將ES-PSO算法應用于網絡優化、圖像處理等其它實際問題中.