一種改進PSO的室內(nèi)機器人路徑規(guī)劃方法

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

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

0 引言

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

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

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

1 PSO算法及改進的PSO算法

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

1.1 PSO

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

(1)

(2)

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

1.2 改進的PSO算法

因此，針對上節(jié)中PSO算法所存在的問題，本文利用動態(tài)權(quán)重、三次樣條差值、罰函數(shù)等方法對經(jīng)典PSO進行算法綜合改進。

1.2.1 動態(tài)權(quán)重

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

u=wmin+(wmax-wmin)*rand

(3)

w=u+rande*randn()

(4)

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

1.2.2 三次樣條插值

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

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

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

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

2)將數(shù)據(jù)節(jié)點和指點的的首位端點條件帶入矩陣方程;

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

4)計算樣條曲線系數(shù):

ai=yi

(5)

(6)

(7)

(8)

其中，i=0,1,…,n-1在每個子區(qū)間xi≤x≤xi+1中，創(chuàng)建方程：

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

(9)

1.2.3 粒子編碼

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

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

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

1.2.4 帶有罰函數(shù)的適應度函數(shù)

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

根據(jù)以上兩個條件構(gòu)造文章的適應度函數(shù)為：

(10)

1.2.5 改進PSO算法流程

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

改進PSO通過以下幾個步驟完成室內(nèi)移動機器人路徑的最優(yōu)規(guī)劃。

Step1：根據(jù)室內(nèi)具體布局確定路徑節(jié)點的個數(shù)n，確定插值點個數(shù)m。

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

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

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

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

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

Step7：如果達到最大迭代次數(shù)，則算法結(jié)束，輸出最優(yōu)路徑；如果沒有達到最大迭代次數(shù)，則轉(zhuǎn)向Step3。

具體流程如圖1所示。

圖1 改進PSO算法流程圖

2 實驗與結(jié)果分析

2.1 實驗環(huán)境及參數(shù)初始化

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

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

仿真參數(shù)選擇：五種算法的種群規(guī)模、最大迭代次數(shù)保持一致：Npop=150，MaxIt=500?；綪SO的慣性權(quán)重和學習因子w=1,c1=c2=1.5；改進算法的慣性權(quán)重和學習因子為動態(tài)的，wmin=0.4，wmax=0.9 ；三次樣條插值點個數(shù)為100，邊界為非節(jié)點邊界。

經(jīng)典PSO算法記為PSO，基于收斂因子與三次樣條插值改進的PSO算法記為YSPSO-SP?；诜蔷€性凹函數(shù)慣性權(quán)重與三次樣條插值改進的PSO記為NCFPSO-SP，基于線性遞減慣性權(quán)重與三次樣條插值改進的PSO記為LinWPSO-SP，基于隨機慣性權(quán)重與三次樣條插值改進的PSO記為RandWPSO-SP。

2.2 室內(nèi)環(huán)境路徑規(guī)劃及算法性能分析

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

2.2.1 第1次實驗

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

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

圖2 第1次實驗結(jié)果五種算法實驗對比

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

表1 第1次實驗路徑長度對比數(shù)據(jù)表

2.2.2 第2次實驗

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

圖3 第2次實驗結(jié)果五種算法實驗對比

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

表2 第2次實驗路徑長度對比數(shù)據(jù)表

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

2.2.3 第3次實驗

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

圖4 第3次實驗結(jié)果五種算法實驗對比

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

表3 第3次實驗路徑長度對比數(shù)據(jù)表

2.2.4 第4次實驗

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

圖5 第4次實驗結(jié)果五種算法實驗對比

表4 第4次實驗路徑長度對比數(shù)據(jù)表

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

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

3 結(jié)束語

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

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