一種改進PSO的室內機器人路徑規劃方法

李 玽，吳丹丹，趙征凡2，王曉華，張 蕾

(1.西安工程大學 電子信息學院，西安 710048;2.工業與信息化部電子第五研究所 可靠性數據中心, 廣州 510610)

0 引言

路徑規劃是機器人研究中的關鍵基本問題，旨在具有障礙物的環境中，按照預定義的評價標準，尋找一條從起始位置到目標位置的無碰撞最優(次優)路徑[1]。近年，仿生智能算法是機器人路徑規劃中應用較為廣泛的方法，常用的有：微分進化算法[2]、模擬退火算法[3]、遺傳算法[4-5]、蟻群算法[6-7]、人工魚群算法[8]、蜂群算法[9]、粒子群算法( particle swarm optimization，PSO)[10-11]等。眾多算法中，PSO具有收斂速度快、設置參數少、實現簡單等優點，但同時也存在收斂度低、易早熟、魯棒性差等缺陷，影響了路徑規劃的運行效率和可靠性[12]。

目前，針對PSO的缺陷，國內外學者和研究機構提出了較多的改進方法。文獻[13]提出極坐標建模和PSO，改善了算法的收斂性，但極坐標系和直角坐標系轉換增加了算法的計算量，降低了路徑的運行效率；文獻[14]提出的利用粒子個體極值的加權平均值，同時加入慣性權重的PSO方法，在一定程度上改善了算法的全局和局部搜索能力，但算法依然存在早熟的問題；文獻[15]提出將差分算法引入PSO，避免了PSO早熟的問題，但算法收斂度低，路徑規劃不精確；文獻[16]提出了改進的二階振蕩粒子群算法，在一定程度上改善了算法易陷入局部最優解問題，但規劃的路徑不平滑；翻閱文獻[17]的算法時，發現該文獻的算法在一定程度上存在過早收斂的現象，沒有很好的平衡全局搜索和局部搜索能力，搜索后期容易停滯陷入局部最優解等現象。此外，室內空間的局限性對于移動機器人速度控制精度、路徑運行平滑性等要求較高，已有的研究對于本文的研究對象存在一定的不適用性。

因此，為提高PSO收斂速度，避免陷入局部最優解，提高路徑的平滑度、精確度等，使移動機器人路徑規劃方法更好的適用于室內環境中，文章將分別引入收斂因子、線性遞減、非線性凹函數、隨機分布方式等方法對慣性權重進行改進，并結合三次樣條插值方法建立了帶有罰函數的適應度函數，將綜合改進后的PSO算法用于求解室內全局路徑規劃問題，研究能夠獲得曲線更為平滑、在急停急轉時，具有更好的動力學特性等路徑規劃方法。

1 PSO算法及改進的PSO算法

PSO算法是一種應用于機器人路徑規劃的經典仿生智能算法。其思想源于對鳥群捕食行為的研究。

1.1 PSO

PSO方法的基本核心是利用群體中的個體對信息的共享從而使整個群體的運動在問題求解空間中產生從無序到有序的演化過程，從而獲得問題的最優解。每個尋優的問題解都是“粒子”。 每個粒子通過公式(1)和(2)進行速度和位置更新。

(1)

(2)

然而，當經典PSO算法應用在室內機器人路徑規劃研究時，算法易陷入局部最優解，造成路徑規劃效率不高，最優值迭代次數較多甚至不收斂，魯棒性差，代價大等問題。

1.2 改進的PSO算法

因此，針對上節中PSO算法所存在的問題，本文利用動態權重、三次樣條差值、罰函數等方法對經典PSO進行算法綜合改進。

1.2.1 動態權重

為平衡較大的慣性權重的高全局搜索能力和低權重w的強局部搜索能力，解決因w過大容易導致“早熟”收斂與后期全局最優解附近的振蕩現象。本文引入動態改變w的值，其思想在于：搜索初期w取值較大，隨著迭代次數的增加逐漸降低w的值，從而達到全局最優和收斂。為使全局搜索和局部搜索達到最佳的平衡狀態，文章利用隨機分布方法[18]對經典PSO 進行改進。表達式為：

u=wmin+(wmax-wmin)*rand

(3)

w=u+rande*randn()

(4)

其中，wmin是最小權值；wmax是最大權值；rand()為[0,1]的均勻分布隨機數；randn()為正態分布隨機數；rande為隨機權重方差，文章設置rande=0.5。正態隨機變量調整w，不僅可以快速跳出局部最大值，而且能保持種群的多樣性提高算法的全局搜索能力。

1.2.2 三次樣條插值

在選擇以隨機分布方法進行動態權值設置的基礎上，仿真實驗中發現經典的PSO規劃的路徑存在轉折點較多，路徑不平滑，在急停，急轉彎時動力學特性差等缺陷。三次樣條曲線是由若干基于三次多項式的插值區間而擬合出一條光滑的曲線。因此，引入三次樣條插值，對前述改進的PSO算法進行進一步的完善，通過結合兩種方法的優勢規劃出一條更為光滑的路徑，保障機器人運動過程中較好的動力學特性。

三次樣條插值的定義與算法如下：

在區間[a,b]上，有n+1個數據節點(x1,y1),(x2,y2),…,(xn,yn)

1)計算步長hi=xi+1-xi(i=0,1,…,n-1);

2)將數據節點和指點的的首位端點條件帶入矩陣方程;

3)解矩陣方程，求得二次微分值。該矩陣為三對角矩陣;

4)計算樣條曲線系數:

ai=yi

(5)

(6)

(7)

(8)

其中，i=0,1,…,n-1在每個子區間xi≤x≤xi+1中，創建方程：

gi(x)=ai+bi(x-xi)+ci(x-xi)2+di(x-xi)3

(9)

1.2.3 粒子編碼

三次樣條曲線的個數等于路徑節點的個數。 路徑節點為：三次樣條段與段的交接處。 由于三次樣條曲線為一階連續，結點處為二階連續。因此路徑轉向的最大次數為路徑結點的個數。即使在復雜的環境下，路徑經過3到5次轉向就能避開障礙物。因此文章用路徑結點對粒子編碼，即一個粒子個體為對應路徑上所有的路經結點。

假設有n個路徑結點坐標，橫坐標與縱坐標分別為：(xn1,xn2,…,xnn)，(yn1,y,…,ynn),路徑的起點與終點的坐標分別為：(xs,ys),(xt,yt)。分別通過三次樣條插值在區間(xs,xn1,xn2,…,xt)與(ys,yn1,yn2,…,yt)上取插值點，坐標為：(x1,y1),(x2,y2),…,(xm,ym)。

根據三次樣條插值的特性，文章用路徑節點和插值點以及路徑的起點和終點的連線作為機器人的運行軌跡。

1.2.4 帶有罰函數的適應度函數

判斷一條路經是否最優一般需要滿足以下兩個條件：①路徑是否最短；②沒有和障礙物碰撞。

根據以上兩個條件構造文章的適應度函數為：

(10)

1.2.5 改進PSO算法流程

通過上述的綜合改進，動態慣性權重很好的平衡了全局搜索和局部搜索能力，避免了粒子收斂速度快，容易陷入局部最優點的缺點；引入了三次樣條插值方法使得規劃的路徑是一條平滑的曲線，改善了經典PSO規劃的路徑存在轉折點多，路徑不平滑等缺陷，使機器人更好地適應了室內環境；引入罰函數，簡化了非法度的計算，避免了路徑和障礙物碰撞，規劃出一條合法的路徑。

改進PSO通過以下幾個步驟完成室內移動機器人路徑的最優規劃。

Step1：根據室內具體布局確定路徑節點的個數n，確定插值點個數m。

Step2：設置 PSO 算法中的各個參數，初始化粒子種群、位置和速度。

Step3：計算每個粒子x、y方向的m個插值點坐標。

Step4：根據式(10)計算粒子的適應度值。

Step5：根據式(1)和(2)分別與(3)～(4)更新粒子的速度和位置，更新局部最優值Pbest和全局最優值Gbest。

Step6：根據式(10)判斷更新后的粒子是否合法并計算粒子的罰函數和適應度值，得出更新后由n個路徑節點坐標組成的路徑。迭代次數加1。

Step7：如果達到最大迭代次數，則算法結束，輸出最優路徑；如果沒有達到最大迭代次數，則轉向Step3。

具體流程如圖1所示。

圖1 改進PSO算法流程圖

2 實驗與結果分析

2.1 實驗環境及參數初始化

實驗仿真環境以所在實驗室空間環境為背景，創建仿真實驗路徑規劃有限二維空間地圖如圖2(a)所示。為驗證本文算法的先進性，實驗首先從本文改進的算法選一個最優的算法，然后用最優的算法分別與基于經典的PSO進行對比。

為保證算法對比的客觀與公平，所有算法均采用相同的軟硬件平臺，運行環境為Windows10， core i7，CPU(2.2 GHz)，內存8 GB，編程環境為Matlab R2018a。同時為保證實驗數據的真實可信，對于每種算法進行10次實驗，對實驗數據進行均值化處理進行對比。

仿真參數選擇：五種算法的種群規模、最大迭代次數保持一致：Npop=150，MaxIt=500。基本PSO的慣性權重和學習因子w=1,c1=c2=1.5；改進算法的慣性權重和學習因子為動態的，wmin=0.4，wmax=0.9 ；三次樣條插值點個數為100，邊界為非節點邊界。

經典PSO算法記為PSO，基于收斂因子與三次樣條插值改進的PSO算法記為YSPSO-SP。基于非線性凹函數慣性權重與三次樣條插值改進的PSO記為NCFPSO-SP，基于線性遞減慣性權重與三次樣條插值改進的PSO記為LinWPSO-SP，基于隨機慣性權重與三次樣條插值改進的PSO記為RandWPSO-SP。

2.2 室內環境路徑規劃及算法性能分析

為驗證算法路徑規劃的普適性，將實驗路徑分為起點與終點在實驗室的對角，并進行起點(★)、終點(■)位置互換的二次實驗，4次實驗起點、終點設置如圖2(a)、3(a)、4(a)、5(a)所示。對應不同算法條件下路徑規劃最優路徑值如表1～4所示。

2.2.1 第1次實驗

通過圖2(a)可以直觀的看出，較其它四種算法RandWPSO-SP方法規劃出的機器人運動路線拐點更少、拐點弧度較大。這是因為利用三次樣條曲線可以對路徑的角點進行平滑，所以產生的是一條光滑的路徑。

迭代曲線圖2(b)中，RandWPSO-SP算法規劃的路徑最短，這是因為該算法引入隨機慣性權重，不僅平衡了全局搜索和局部搜索的能力，還增加了搜索的靈活性和范圍，使算法的搜索能力提高，而且擾動效果明顯，增強了粒子多樣性，更容易跳出局部最優值；YSPSO-SP方法收斂速度最快，這是因為該算法引入收斂因子，雖然使收斂速度加快，但加速度因子是基于線性擾動，擾動小，粒子容易陷入局部最優解；收斂速度第二快的為LinWPSO-SP算法，該方法的擾動屬于線性擾動，擾動不明顯，粒子容易出現“早熟”；NCFPSO-SP算法引入非線性慣性權重，雖然非線性慣性權重擾動效果明顯，但是該方法收斂精度低。

圖2 第1次實驗結果五種算法實驗對比

表1是每個算法在相同的環境下，獨立運行500次得出的路徑結果。從中可以可以看出RandWPSO-SP算法規劃的最短路徑、平均路經以及平均仿真運行時間都比其它四種算法少。這是因為隨機慣性權重的引入，不但平衡了全局搜索和局部挖掘能力，而且擾動效果明顯，增加了粒子多樣性。引入三次樣條插值使得算法更好得適用于室內環境。開始規劃的路徑最長，這是因為起點處空間大，提高了搜索范圍，改善了粒子陷入局部極值的現象。

表1 第1次實驗路徑長度對比數據表

2.2.2 第2次實驗

本次實驗與第一次實驗的不同之處在于起點和終點互換。從圖3(a)、(b)可以看出。該次實驗仿真結果幾乎和第一次一樣，驗證了較其它四種算法，RandWPSO-SP方法規劃出的機器人運行軌跡拐點更少、拐點弧度較大等優點。

圖3 第2次實驗結果五種算法實驗對比

表2是五種算法分別在相同環境下，獨立運行500次得出的路徑結果。從中可以看出RandWPSO-SP算法較其它四種算法的的最短路徑、平均路徑以及平均仿真運行時都最短。

表2 第2次實驗路徑長度對比數據表

表2數據和表1數據比較發現，表2的路徑和平均仿真運行時間比表1長，這是因為雖然兩次實驗只是調換了起點和終點，但第一次實驗起點處，由于室內空間布局大，非法路徑少，而第二次實驗開始階段空間布局小，非法路徑多，要多次排除非法路徑，所以算法路徑、運行時間長。

2.2.3 第3次實驗

本次實驗五種方法規劃的路徑比較集中，這是由于實驗起點處的空間小，可供選擇路徑少。該算法的仿真結果再次驗證了RandWPSO-SP方法規劃出的機器人運行軌跡拐點更少、拐點弧度較大。該算法規劃的路徑能使機器人再室內更好的運行。

圖4 第3次實驗結果五種算法實驗對比

本次實驗起點處空間小，可供選擇的路徑少，各算法選擇相似的空間規劃的路徑，這樣更容易比較規劃的最長路徑、最短路徑、平均路徑。從表3可以看出：獨立運行500次實驗，RandWPSO-SP規劃的最長路徑、最短路徑、平均路徑都最短，且仿真運行時間也最短。

表3 第3次實驗路徑長度對比數據表

2.2.4 第4次實驗

從圖5(a)、(b)可以看出本次實驗再次驗證了RandWPSO-SP方法規劃出的機器人運行軌跡拐點更少、拐點弧度較大，魯棒性更好等優點。

圖5 第4次實驗結果五種算法實驗對比

表4 第4次實驗路徑長度對比數據表

從表4可以看出：本次實驗驗證了RandWPSO-SP算法規劃的最短路徑，平均路徑，最長路徑較其它四個算法規劃的路徑較優，且仿真時間也最短的。較第三次實驗，本次規劃的路徑及平均仿真運行時間都比第三次時間長，這是因為空間布局上本次實驗起點空間大，算法進行了大范圍的搜索。

上述四次實驗結果表明雖然本文提出RandWPSO-SP的算法收斂速度不是最快的，但擾動小，搜索能力強，改善了粒子早熟現象，收斂路徑最短，收斂效率更高，規劃的路徑拐點更少，拐點弧度更大。這是由于該算法引入隨機慣性權重，不僅平衡了全局搜索和局部搜索的能力，還增加了搜索的靈活性，使算法的搜索能力提高，而且擾動效果明顯，增強了粒子多樣性，更容易跳出局部最優值。此外，該算法引入三次樣條插值很好改善了室內環境造成的路徑不光滑，轉折點較多等問題。

3 結束語

本文根據室內環境局限性特征，引入三次樣條插值，將三次樣條插值和PSO結合規劃機器人路徑。因為造成基本的PSO存在收斂精度低，搜索停滯兩個原因[19]：①算法搜索后期，粒子多樣性少，易“早熟”。 ②粒子容易陷入較差的搜索區并很難跳出，造成搜索停滯，收斂效率低。本文給傳統的PSO分別引入收斂因子、線性遞減權重、非線性凹函數權重、隨機權重并與三次樣條插值結合。增加粒子的擾動效果，提高粒子的多樣性，平衡粒子全局和局部搜索能力。把所有的改進的算法進行仿真實驗比較，從中選出一個最優的算法即RandWPSO-SP，再與經典的PSO進行路徑規劃仿真實驗比較。實驗結果表明：RandWPSO-SP即基于隨機慣性權重和三次樣條插值的PSO算法規劃的路徑最短，用時最短，算法最穩定。對于室內環境有更好的有效性和優越性。

下一步實驗將會把RandWPSO-SP算法應用于實際環境中，軟件背景為ROS(robot operation system)操作系統，實驗平臺為類turtlebot機器人。使智能算法更好應用于室內環境。此外，還會進行實驗：同時改進慣性權重和學習因子并將其用在真實的實驗環境中，使學習因子和慣性權重達到更好的平衡，使路徑規劃效率更高，可行性更好。