基于自適應(yīng)粒子群算法的機(jī)器人路徑規(guī)劃

高岳林，武少華

(1.北方民族大學(xué) 寧夏智能信息與大數(shù)據(jù)處理重點(diǎn)實(shí)驗(yàn)室,寧夏 銀川 750021；2.寧夏大學(xué) 數(shù)學(xué)統(tǒng)計(jì)學(xué)院,寧夏 銀川 750021)

0 引言

機(jī)器人技術(shù)是近年來興起的一種現(xiàn)代科學(xué)技術(shù)，它的發(fā)展結(jié)合了許多高新技術(shù)學(xué)科，主要有機(jī)械電子、信息學(xué)、控制論、軟件開發(fā)、人工智能等。移動(dòng)機(jī)器人路徑的研究是機(jī)器人學(xué)科中一個(gè)重要的研究領(lǐng)域，研究的問題是從工作空間中尋找一個(gè)從開始位置到目標(biāo)位置的一個(gè)最優(yōu)路徑。本質(zhì)上屬于路徑規(guī)劃問題。智能算法是近年來提出用來解決機(jī)器人路徑規(guī)劃問題的算法，主要使用的方法有遺傳算法、蟻群算法、粒子群算法、神經(jīng)網(wǎng)絡(luò)等等。

國內(nèi)外學(xué)者對機(jī)器人路徑規(guī)劃也作了許多研究。Willms等[1]提出了一種高效的動(dòng)態(tài)實(shí)時(shí)機(jī)器人路徑規(guī)劃系統(tǒng)，進(jìn)行實(shí)時(shí)機(jī)器人路徑規(guī)劃。Fujimura等[2]提出了一種基于人工勢場的機(jī)器人路徑規(guī)劃。Saska等[3]提出了一種采用Ferguson 樣條的粒子群優(yōu)化算法,該方法得到的路徑比傳統(tǒng)方法得到的路徑光滑，而且易于執(zhí)行。劉廣瑞等[4]利用蟻群算法來進(jìn)行最優(yōu)路徑的搜索，并對算法進(jìn)行收斂性分析，從而提高了算法的收斂效果。趙開新等[5]通過梯度計(jì)算、節(jié)點(diǎn)探測、路徑評(píng)估3種方式對機(jī)器人路徑規(guī)劃進(jìn)行研究。陳志軍等[6]將機(jī)器人路徑規(guī)劃是建立在三維空間里，建立了三維路徑規(guī)劃的評(píng)價(jià)指標(biāo)和優(yōu)化函數(shù),提出了一種基于模糊神經(jīng)網(wǎng)絡(luò)和遺傳算法的機(jī)器人路徑規(guī)劃。強(qiáng)寧等[7]采用了三次樣條插值和粒子群算法來進(jìn)行多機(jī)器人全局路徑規(guī)劃。

粒子群算法在解決機(jī)器人路徑規(guī)劃問題時(shí)主要存在兩個(gè)缺陷：①算法迭代到后期,粒子群多樣性下降，當(dāng)路徑陷入局部較差路徑時(shí)，很難自動(dòng)跳出，出現(xiàn)“早熟”現(xiàn)象。②算法的收斂性差，算法迭代后期，許多粒子適應(yīng)度相差不大，導(dǎo)致粒子搜索進(jìn)入停滯狀態(tài)，路徑尋優(yōu)也會(huì)停滯，造成了路徑的精度低。因此對算法進(jìn)行改進(jìn)十分必要。粒子群優(yōu)化算法(PSO)和模擬退火算法(SA)是兩種不同的智能優(yōu)化算法，粒子群優(yōu)化算法通過追隨當(dāng)前搜索到的最優(yōu)解來尋找全局最優(yōu)，模擬退火算法的機(jī)制便是以一定的概率來接受一個(gè)比當(dāng)前解要差的解，從而可能跳出這個(gè)局部的最優(yōu)解，達(dá)到全局的最優(yōu)解。針對以上問題，筆者提出一種基于模擬退火的改進(jìn)粒子群算法(APSO-SA)，通過充分發(fā)揮粒子群算法和模擬退火算法的優(yōu)勢從而實(shí)現(xiàn)機(jī)器人全局最優(yōu)路徑規(guī)劃。

1 標(biāo)準(zhǔn)粒子群算法

粒子群算法(PSO)是一種啟發(fā)式隨機(jī)算法。鳥群中每一個(gè)鳥相當(dāng)于粒子群算法中的一個(gè)粒子。每個(gè)粒子好比尋找食物的鳥都有速度和位置，通過自身和社會(huì)兩種學(xué)習(xí)方式，粒子在搜索空間中運(yùn)動(dòng)，從而得到全局最優(yōu)解。假設(shè)種群有N個(gè)粒子在D維空間搜索，標(biāo)準(zhǔn)粒子群算法更新公式為:

(1)

(2)

式中：t為迭代次數(shù)；vij表示第i個(gè)粒子j維的速度大小；xij表示第i個(gè)粒子j維的位置大小；w表示慣性權(quán)重；c1、c2表示學(xué)習(xí)因子；r1、r2表示隨機(jī)數(shù)。粒子群算法雖然簡單，但是算法迭代后期，由于粒子多樣性差導(dǎo)致粒子容易陷入局部最優(yōu)解，所以要對粒子群算法進(jìn)行改進(jìn)。

2 粒子群算法的改進(jìn)

2.1 帶壓縮因子的粒子群算法

為了有效地控制粒子飛行速度使算法全局搜索和局部搜索平衡，Clerc等[8]提出了帶壓縮因子的PSO算法，其速度更新公式如下:

(3)

其中搜索因子:

帶壓縮因子的PSO算法相當(dāng)于線性遞減權(quán)重的PSO算法，該算法的優(yōu)勢在于去掉了慣性權(quán)重w，減少了算法參數(shù)，只有兩個(gè)學(xué)習(xí)因子參數(shù),所以只需對兩個(gè)學(xué)習(xí)因子進(jìn)行自適應(yīng)調(diào)節(jié)。其調(diào)節(jié)公式如下：

當(dāng)f(xi)≤favg，粒子xi需要進(jìn)行局部搜索，學(xué)習(xí)因子取:

(4)

當(dāng)f(xi)>favg，粒子xi要進(jìn)行全局搜索，所以學(xué)習(xí)因子?。?/p>

(5)

式中：cmin=1.5；cmax=2.5；f(xi)表示粒子xi的適應(yīng)度大小;favg表示所有粒子的平均適應(yīng)度。

2.2 模擬退火算子

模擬退火算法是由Metropolis等于1953年提出的，該算法在搜索過程中具有概率突跳的能力，能夠在一定程度上避免搜索過程中陷入局部最優(yōu)解，由于粒子群算法對pg全局最優(yōu)粒子依賴性很強(qiáng)，所以為了避免粒子搜索過程陷入pg，所以對pg引入模擬退火操作，其中Metropolis準(zhǔn)則如下：

(6)

式中：f(pi)表示pi粒子的適應(yīng)度；f(pg)表示當(dāng)前粒子群算法迭代的全局最優(yōu)適應(yīng)度。采用輪盤賭策略從pi中確定全局最優(yōu)的某個(gè)個(gè)體代替pg，初溫和退溫公式為：

(7)

以上為APSO-SA算法的基本思想，標(biāo)準(zhǔn)粒子群算法的時(shí)間復(fù)雜度為O(M，D，N)，其中M為最大迭代次數(shù)，D為粒子維數(shù)，N為粒子個(gè)數(shù)。由于該算法是建立在壓縮因子的粒子群算法的基礎(chǔ)上，壓縮因子的粒子群算法時(shí)間復(fù)雜度為O(M，D，N),學(xué)習(xí)因子的自適應(yīng)調(diào)節(jié)策略和模擬退火操作的時(shí)間復(fù)雜度均為O(N)，所以APSO-SA算法的時(shí)間復(fù)雜度為O(M，D，N)。

3 機(jī)器人路徑規(guī)劃模型建立

3.1 三次樣條插值編碼設(shè)計(jì)

兩點(diǎn)法對機(jī)器人路徑進(jìn)行規(guī)劃[9]會(huì)造成路徑平滑性差，機(jī)器人運(yùn)動(dòng)不平穩(wěn)，頻繁轉(zhuǎn)向會(huì)造成能源的極大浪費(fèi)，本文機(jī)器人路徑規(guī)劃是建立在二維平面上，提出的APSO-SA算法在二維平面進(jìn)行三次樣條插值編碼，根據(jù)文獻(xiàn)[3]提出的Ferguson三次樣條插值對路徑進(jìn)行編碼優(yōu)化，其原理如下。

假設(shè)有以下節(jié)點(diǎn)(xi,yi)，其中xi

(1)每個(gè)分段區(qū)間[xi,xi+1]，S(x)=Si(x)為三次多項(xiàng)式。

(2)每一個(gè)端點(diǎn)滿足S(xi)=yi，i∈N∩i≤n。

(3)S(x)的1、2階導(dǎo)數(shù)為S′(x)、S″(x),而且在[a,b]區(qū)間內(nèi)連續(xù)，即Si(x)可以寫成：

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

式中：i∈N∩i

機(jī)器人在搜索空間搜索最優(yōu)路徑可以看作在三次樣條空間中搜索最優(yōu)樣條，APSO-SA算法中的每一組粒子表示為一組路徑的節(jié)點(diǎn)，每一組路徑節(jié)點(diǎn)個(gè)數(shù)便為三次樣條曲線的個(gè)數(shù)，由于三次樣條插值的性質(zhì)，每一組路徑節(jié)點(diǎn)的個(gè)數(shù)體現(xiàn)了路徑可以轉(zhuǎn)向的最大次數(shù)。粒子編碼的路徑節(jié)點(diǎn)個(gè)數(shù)會(huì)隨著工作環(huán)境的復(fù)雜而增加，實(shí)際生活中機(jī)器人轉(zhuǎn)向3～5次便可以繞過所有的障礙物，本文的障礙物模型設(shè)置轉(zhuǎn)向節(jié)點(diǎn)為3。

3.2 路徑初始化

路徑初始化決定著算法的好壞，PSO算法機(jī)器人路徑規(guī)劃均是在機(jī)器人工作空間隨機(jī)取粒子，但是導(dǎo)致算法收斂性差，于是對粒子的初始化進(jìn)行改進(jìn)，提出基于高斯分布的隨機(jī)初始化，初始化公式如下：

x=(xs+xt)/2+r3；

(8)

y=U(ymin,ymax,1,n)，

(9)

式中：xs、xt分別為開始位置和目標(biāo)位置的橫坐標(biāo)；r3為高斯分布產(chǎn)生的隨機(jī)數(shù)；n表示產(chǎn)生粒子的個(gè)數(shù)；ymax、ymin分別為粒子初始化縱坐標(biāo)的上下界；U表示在給定上下界中均勻產(chǎn)生n個(gè)粒子縱坐標(biāo)的值。

3.3 障礙物處理

障礙物對機(jī)器人路徑規(guī)劃有很大的影響，若沒有障礙物，機(jī)器人在二維空間中從開始位置到目標(biāo)位置運(yùn)動(dòng)，直線運(yùn)動(dòng)最短，無須進(jìn)行路徑規(guī)劃，然而現(xiàn)實(shí)機(jī)器人工作時(shí)，障礙物是客觀存在的，筆者研究的是靜態(tài)工作環(huán)境下的機(jī)器人路徑規(guī)劃，由于實(shí)際工作環(huán)境中障礙物形狀千差萬別，導(dǎo)致建模十分困難，為了方便統(tǒng)一研究，筆者對障礙物進(jìn)行預(yù)處理，在二維空間中，障礙物均進(jìn)行膨脹處理，即用原障礙物圖形的外接圓表示該障礙物，具體見圖1。

圖1 不同形狀障礙物膨脹處理Figure 1 Different obstacle with different shapes expansion

3.4 適應(yīng)度函數(shù)

適應(yīng)度函數(shù)的值可以看作路徑的長度，粒子通過三次樣條編碼，不斷尋找最優(yōu)適應(yīng)度，而有些路徑與障礙物相交會(huì)成為非法路徑，如果直接去掉這些路徑，那么存在的可行路徑就會(huì)變得非常少，路徑的多樣性就會(huì)減弱，所以構(gòu)造帶有罰函數(shù)的適應(yīng)度函數(shù)，通過附加較大的懲罰值，使路徑在優(yōu)化過程中自動(dòng)淘汰非法路徑。文獻(xiàn)[8]中使用了一種帶有罰函數(shù)的適應(yīng)度函數(shù)，筆者對其做了一些簡單的修改，得出適應(yīng)度函數(shù)如下：

z=L(1+beta·V),

(10)

式中：L為機(jī)器人工作運(yùn)行路徑的長度；beta為懲罰值，一般設(shè)置為100；V為路徑的非法度，當(dāng)V=0時(shí)，z=L，此時(shí)適應(yīng)度的大小就為路徑的大小。

4 算法流程

APSO-SA算法機(jī)器人路徑規(guī)劃與粒子群算法機(jī)器人路徑規(guī)劃流程相似，具體算法流程如下：

步驟1創(chuàng)建障礙物模型,初始化機(jī)器人開始位置以及機(jī)器人目標(biāo)位置，初始化機(jī)器人工作空間。

步驟2初始化APSO-SA算法的參數(shù)，包括粒子個(gè)數(shù)、學(xué)習(xí)因子、最大迭代次數(shù)、退火系數(shù)、路徑節(jié)點(diǎn)個(gè)數(shù)。

步驟3按照式(8)、(9)初始化每組粒子的位置，以及對粒子進(jìn)行三次樣條編碼。

步驟4按照式(4)、(5)自適應(yīng)處理學(xué)習(xí)因子，同時(shí)按照式(3)更新粒子速度，按照式(2)更新粒子的位置。

步驟5按照式(10)計(jì)算每組粒子當(dāng)前的適應(yīng)度，并根據(jù)式(6)和式(7)對當(dāng)前適應(yīng)度最好的粒子進(jìn)行退火處理。

步驟6判斷算法是否達(dá)到最大函數(shù)評(píng)價(jià)次數(shù)，若滿足，則停止搜索，輸出結(jié)果，否則返回步驟4繼續(xù)迭代。

5 仿真模擬

為了驗(yàn)證APSO-SA算法在機(jī)器人路徑尋優(yōu)中的優(yōu)越性，筆者設(shè)計(jì)了以下實(shí)驗(yàn),在3種障礙物模型中對APSO-SA算法與PSO算法進(jìn)行比較測試。其中3種障礙物模型的最短路徑長度通過幾何計(jì)算得到，保留小數(shù)點(diǎn)后2位。見表1。算法比較采用固定粒子個(gè)數(shù)和最大函數(shù)迭代次數(shù)，試驗(yàn)在windows 7系統(tǒng)MATLAB 2012a中完成。電腦配置為Intel(R) Core(TM) i5-3317U CPU@1.70 GHz。

5.1 參數(shù)設(shè)置

筆者建立的3個(gè)障礙物模型參數(shù)如表1～3所示。

表1 3個(gè)模型基本參數(shù)Table 1 Basic parameters of three models

表2 3個(gè)模型圓心及其半徑Table 2 Center and rabius of three models

表3 PSO和APSO-SA算法參數(shù)Table 3 Algorithm parameters of PSO and APSO-SA

5.2 數(shù)值實(shí)驗(yàn)

5.2.1 單障礙物模型

兩種算法獨(dú)立運(yùn)行30次，數(shù)值結(jié)果用SPSS進(jìn)行統(tǒng)計(jì)分析，結(jié)果如表4、5所示。

表4 單障礙物模型數(shù)值結(jié)果比較Table 4 Comparison of numerical results of single obstacle model

表5 單障礙物模型單因素方差分析結(jié)果比較Table 5 One-way ANOVA results of single obstacle model

圖2為單障礙物模型兩種算法比較結(jié)果圖，從圖2可以看出，PSO算法和APSO-SA算法都能夠在該模型中規(guī)劃出一條無碰路徑。PSO算法搜索出的最優(yōu)路徑長度為7.47 m，而APSO-SA算法得出的最優(yōu)路徑長度為7.41 m，同時(shí)APSO-SA路徑搜索范圍廣，路徑搜索前期變化比較大，10代以后趨于平穩(wěn)，表4、5也表明APSO-SA算法的路徑尋優(yōu)能力優(yōu)于PSO算法，通過方差同質(zhì)性檢驗(yàn)后進(jìn)行單因素方差分析，結(jié)果表明，兩種算法具有顯著性差異。

圖2 單障礙物模型兩種算法比較結(jié)果Figure 2 The results of the two algorithms of single obstacle model

5.2.2 多個(gè)均勻障礙物模型

圖3分別給出了多個(gè)均勻障礙物模型兩種算法得出的最優(yōu)路徑和最優(yōu)路徑長度比較。從實(shí)驗(yàn)結(jié)果可以看出，APSO-SA算法的路徑平滑度明顯優(yōu)于PSO算法。下面兩種算法各自獨(dú)立運(yùn)行30次，運(yùn)行結(jié)果及統(tǒng)計(jì)分析記錄如表6、7所示。

圖3 多個(gè)均勻障礙物模型兩種算法比較結(jié)果Figure 3 The results of the two algorithms of multiple uniform obstacle models

由于該模型存在許多局部最優(yōu)路徑，所以獲得的路徑為局部最優(yōu)路徑不可避免。從表6可以看出,APSO-SA有很好地跳出局部最優(yōu)路徑的性能，得到的路徑為局部最優(yōu)路徑的概率僅為20%.從時(shí)間上來看兩種算法相差不大，僅比傳統(tǒng)的PSO算法高了0.04 s，總體路徑平均長度卻減少了0.27 m。這充分體現(xiàn)了APSO-SA算法多樣化策略的優(yōu)勢。單因素方差分析也可以得到兩種算法具有顯著性差異。

表6 多個(gè)均勻障礙物模型數(shù)值結(jié)果比較Table 6 Comparison of numerical results of multiple uniform obstacle models

表7 多個(gè)均勻障礙物模型單因素方差分析結(jié)果比較Table 7 One-way ANOVA results of multiple uniform obstacle models

5.2.3 多個(gè)不均勻障礙物模型

從圖4和表8可以看出，在多個(gè)不均勻障礙物模型中，APSO-SA算法的路徑尋優(yōu)能力也優(yōu)于PSO，這充分說明了在復(fù)雜的工作環(huán)境中PSO算法中的粒子在搜索過程中極易從可行域飛到不可行域，導(dǎo)致種群中大量的粒子進(jìn)行約束處理后不能按最優(yōu)方向去搜索最優(yōu)值，最終影響搜索效果。

圖4 多個(gè)不均勻障礙物模型兩種算法比較結(jié)果Figure 4 The results of the two algorithms of multiple inbomogeneous obstacle models

表8 多個(gè)不均勻障礙物模型數(shù)值結(jié)果比較Table 8 Comparison of numerical results of multiple inbomogeneous obstacle models

表9 多個(gè)不均勻障礙物模型單因素方差分析結(jié)果比較Table 9 One-way ANOVA results of multiple inbomogeneous obstacle models

6 結(jié)論

筆者提出了一種基于模擬退火與自適應(yīng)粒子群混合的算法，用來解決機(jī)器人路徑規(guī)劃問題。其中采用高斯分布和三次樣條插值法來初始化并編碼粒子,采用退火策略和罰函數(shù)策略進(jìn)一步提高了算法的運(yùn)行效率，使機(jī)器人路徑更加合理化。通過對適應(yīng)度函數(shù)的調(diào)整，APSO-SA算法也可以解決不同的應(yīng)用問題。將來有可能應(yīng)用于動(dòng)態(tài)實(shí)時(shí)路徑規(guī)劃問題中。