周釗揚(yáng)，穆平安，張仁杰

（上海理工大學(xué)光電信息與計(jì)算機(jī)工程學(xué)院，上海 200093）

0 引言

路徑規(guī)劃（Path Planning）是指運(yùn)用所編制的算法使目標(biāo)在障礙環(huán)境中，根據(jù)任務(wù)要求和評(píng)判準(zhǔn)則自行找到并規(guī)劃從起點(diǎn)至目標(biāo)點(diǎn)的最優(yōu)路徑［1］。隨著相關(guān)智能技術(shù)的一系列突破和發(fā)展，其在軍事、醫(yī)療等高新科技領(lǐng)域都已得到了廣泛應(yīng)用［2］，在未來也有著廣闊的發(fā)展前景。

研究者們又將路徑規(guī)劃算法與移動(dòng)機(jī)器人相結(jié)合，主要涉及到的智能算法有：模擬退火算法、人工魚群算法、粒子群算法等［3］，以更好地解決障礙環(huán)境下移動(dòng)過程中的路徑規(guī)劃問題。其中，粒子群（PSO）算法有著結(jié)構(gòu)簡(jiǎn)單、通用性強(qiáng)、在迭代過程中搜索效率更高等優(yōu)點(diǎn)［4］，但同時(shí)也存在著缺乏對(duì)速度的動(dòng)態(tài)調(diào)節(jié)、收斂精度低、搜索停滯等不足［5］，這對(duì)于路徑規(guī)劃效率和準(zhǔn)確度都會(huì)產(chǎn)生一定影響。

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

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

1 粒子群算法優(yōu)化

1.1 算法基本原理

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

速度向量更新的數(shù)學(xué)模型為：

位置向量更新的數(shù)學(xué)模型為：

式中，pBesti是粒子i(i=1,2,…,M)歷史上的最佳位置，gBest(gBest1,gBest2,…,gBestn)是整個(gè)群體歷史上的最佳位置，k為當(dāng)前迭代次數(shù)，c1、c2為學(xué)習(xí)因子，分別是衡量pBesti和gBest相對(duì)重要性的兩個(gè)參數(shù)，r1、r2則為［0，1］區(qū)間內(nèi)的隨機(jī)數(shù)。

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

在每次迭代中，每個(gè)粒子自身所在位置都對(duì)應(yīng)一個(gè)適應(yīng)度值f：

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

1.2 PSO 優(yōu)化算法

針對(duì)粒子群算法在二維場(chǎng)景下缺乏對(duì)速度的動(dòng)態(tài)調(diào)節(jié)以及容易出現(xiàn)局部最優(yōu)等局限性［13］，提出一種自適應(yīng)粒子群算法，該算法對(duì)種群進(jìn)化狀態(tài)評(píng)估、自適應(yīng)慣性權(quán)重選擇和抑制最優(yōu)值區(qū)域擾動(dòng)3 個(gè)方面進(jìn)行了優(yōu)化。

1.2.1 種群進(jìn)化狀態(tài)評(píng)估

傳統(tǒng)PSO 算法是根據(jù)種群中心位置計(jì)算進(jìn)化因子，而對(duì)種群的進(jìn)化狀態(tài)往往考慮欠佳。這將導(dǎo)致當(dāng)兩個(gè)距離較近的粒子開始進(jìn)化時(shí)，若其速度矢量差別較大，則迭代后的位置可能相距較遠(yuǎn)［14］。因此，針對(duì)這種評(píng)估方法存在的問題，本文提出改進(jìn)的種群進(jìn)化狀態(tài)判斷方法。

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

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

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

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

1.2.2 自適應(yīng)慣性權(quán)重選擇

慣性權(quán)重在PSO 算法中起著調(diào)整局部搜索和全局搜索的重要作用，通常呈遞減狀態(tài)變化。但合理的慣性權(quán)重需同時(shí)考量搜索空間維度、當(dāng)前搜索狀態(tài)、種群大小等多種因素才能提高搜索效率，避免粒子慣性分量造成的不利影響［15］。因此，為了同時(shí)提高收斂精度和效率，本文針對(duì)慣性權(quán)重的特點(diǎn)為學(xué)習(xí)因子設(shè)計(jì)一個(gè)自適應(yīng)控制器。

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

（1）當(dāng)處于探索狀態(tài)時(shí)，應(yīng)探索盡可能多的最優(yōu)值，提高模型求得最優(yōu)解的能力。此時(shí)，為提高粒子搜索速度并進(jìn)行精細(xì)化搜索，應(yīng)增加c1值并減少c2值，避免其趨向于局部最優(yōu)。

（2）當(dāng)處于收斂狀態(tài)時(shí)，理論上應(yīng)增加c2值并減少c1值，使粒子群朝全局最優(yōu)位置移動(dòng)。但由多次實(shí)驗(yàn)結(jié)果來看，這種策略會(huì)加快參數(shù)變化，不利于平衡搜索能力和收斂速度。因此，此時(shí)應(yīng)增大c1和c2值。

（3）當(dāng)處于困死狀態(tài)時(shí)，應(yīng)盡量使群體脫離當(dāng)前全局最優(yōu)。此時(shí)應(yīng)減少c1值并增加c2值，從而促使粒子跳離困死狀態(tài)，在其它位置再次生成全局最優(yōu)。

綜上所述，本文設(shè)計(jì)的自適應(yīng)控制器決策樹如圖1 所示。

基于該決策樹，并通過大量實(shí)驗(yàn)嘗試，將學(xué)習(xí)因子c1和c2初始化為2.0。實(shí)驗(yàn)結(jié)果表明，動(dòng)態(tài)自適應(yīng)學(xué)習(xí)系數(shù)可使系統(tǒng)尋找到更好的全局最優(yōu)點(diǎn)。

1.2.3 抑制最優(yōu)值區(qū)域擾動(dòng)

當(dāng)處于困死狀態(tài)時(shí)，粒子均困在當(dāng)前全局最佳區(qū)域，應(yīng)盡快使粒子脫離該狀態(tài)，向更優(yōu)區(qū)域移動(dòng)［16］。因此，本文設(shè)計(jì)了一種跳出局部最優(yōu)的策略，可以改善傳統(tǒng)策略成功率及效率較低的缺點(diǎn)。具體策略如下：

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

Fig.1 Adaptive controller decision tree圖1 自適應(yīng)控制器決策樹

1.3 算法流程

算法具體流程如下：

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

步驟2：計(jì)算各粒子的適應(yīng)度值。

步驟3：根據(jù)式（1）所示模型對(duì)速度向量進(jìn)行更新。

步驟4：根據(jù)式（2）所示模型對(duì)位置向量進(jìn)行更新。

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

步驟6：計(jì)算δgBest(k)、δpBesti(k)和(k)，并評(píng)估種群進(jìn)化狀態(tài)。

步驟7：根據(jù)評(píng)估結(jié)果，更新當(dāng)前條件下的慣性權(quán)重ω以及學(xué)習(xí)因子c1、c2。

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

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

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

2 路徑平滑度優(yōu)化

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

Bezier 曲線是一種參數(shù)曲線，是由伯恩斯坦多項(xiàng)式發(fā)展演化而來的，因其具有獨(dú)特的控制方式和對(duì)曲線強(qiáng)大的擬合能力，被大量應(yīng)用于工業(yè)領(lǐng)域［18］。在組成它的眾多控制點(diǎn)中，只有起始點(diǎn)和終點(diǎn)在曲線上，其它點(diǎn)均不在曲線上。曲線表示方法如下：

設(shè)一組控制點(diǎn)向量為P0,P1,…,Pn，則Bezier 曲線可表示為：

其中，t是歸一化的時(shí)間變量，Pi=(xi,yi)T代表第i個(gè)控制點(diǎn)的坐標(biāo)向量，xi、yi分別為X和Y的坐標(biāo)分量，Bni(t)為Bernstein 多項(xiàng)式：

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

其中，R(t)為曲率半徑為Bezier 曲線P(t)對(duì)x 和y 的一階、二階導(dǎo)數(shù)。

Bezier 曲線的平滑度與路徑的曲率函數(shù)息息相關(guān)，接下來將融合Bezier 曲線與ES-PSO 算法對(duì)機(jī)器人路徑規(guī)劃進(jìn)行仿真。

3 路徑規(guī)劃相關(guān)描述與建模

3.1 路徑初始化

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

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

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

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

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

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

3.2 結(jié)合罰函數(shù)定義適應(yīng)值函數(shù)

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

式中，Dob表示障礙存在區(qū)域，在此設(shè)罰函數(shù)為200。設(shè)Dsa為保護(hù)區(qū)域，路徑穿過此區(qū)域時(shí)罰函數(shù)會(huì)變大，余外區(qū)域的罰函數(shù)都為0。d表示粒子至障礙物中心的長(zhǎng)度，rob表示障礙存在區(qū)域的半徑，rsa表示保護(hù)區(qū)域的半徑。則由本文障礙物的環(huán)境地圖映射得到罰函數(shù)地圖，如圖3 所示。

將上述兩個(gè)函數(shù)相結(jié)合，重新定義適應(yīng)值函數(shù)如下：

其中，a、b依次為長(zhǎng)度函數(shù)權(quán)重與罰函數(shù)權(quán)重，本文均取值為0.5。

4 仿真與分析

基于MATLAB 建立仿真平臺(tái)，在相同設(shè)定下對(duì)本文提出的改進(jìn)ES-PSO 算法與傳統(tǒng)PSO 算法作仿真測(cè)試和對(duì)比分析，并證明了本文初始化策略的可行性。

Fig.3 Penalty function map圖3 罰函數(shù)地圖

4.1 初始化策略效果

設(shè)機(jī)器人運(yùn)動(dòng)的起點(diǎn)坐標(biāo)為[50,50]，終點(diǎn)坐標(biāo)為[450,450]。根據(jù)本文提出的自適應(yīng)粒子群ES-PSO 算法進(jìn)行路徑規(guī)劃，折線為粒子群算法規(guī)劃出的路徑，曲線為結(jié)合Bezier 曲線生成的平滑路徑，比較有初始化策略和無初始化策略時(shí)算法尋優(yōu)精度與尋優(yōu)速度的差異。

設(shè)算法種群粒子數(shù)量M為30，路徑節(jié)點(diǎn)N為5，迭代200 次，限制粒子速度范圍為[-50,50]。慣性權(quán)重ω=[0.9,0.4]，設(shè)學(xué)習(xí)因子c1和c2的初始值為2.0。ω、c1和c23個(gè)參數(shù)則通過算法迭代過程中種群進(jìn)化狀態(tài)的變化實(shí)現(xiàn)自適應(yīng)控制。

如圖4 所示，實(shí)線和虛線分別表示采用初始化策略和無初始化策略時(shí)的適應(yīng)值變化曲線對(duì)比。

Fig.4 Initialization strategy fitness value change curve comparison圖4 初始化策略適應(yīng)值變化曲線對(duì)比

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

為了提高測(cè)試準(zhǔn)確度，采用兩種方式重復(fù)進(jìn)行100 次測(cè)試，得到平均適應(yīng)值函數(shù)Fitness、平均迭代次數(shù)Iteration和路徑規(guī)劃成功率Success，如表1 所示。由表可知，執(zhí)行初始化策略后的路徑規(guī)劃成功率更高，表明初始化策略可增強(qiáng)路徑規(guī)劃的成功率。

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

4.2 算法尋優(yōu)性能比較

分別在簡(jiǎn)單和復(fù)雜環(huán)境下比較PSO 算法、GPSO 算法、APSO 算法及本文提出的改進(jìn)ES-PSO 算法的尋優(yōu)性能。設(shè)定4 個(gè)規(guī)則的圓形障礙區(qū)域?yàn)楹?jiǎn)單測(cè)試環(huán)境，圖5、圖6（彩圖掃OSID 碼可見，下同）分別為簡(jiǎn)單測(cè)試環(huán)境下4 種粒子群算法的路徑規(guī)劃結(jié)果與適應(yīng)值變化曲線。其中，藍(lán)色曲線表示PSO 算法，紅色曲線表示GPSO 算法，黃色曲線表示APSO 算法，紫色曲線表示ES-PSO 算法。

Fig.5 Planning path results in a simple environment圖5 簡(jiǎn)單環(huán)境下路徑規(guī)劃結(jié)果

Fig.6 Adaptive value curve in a simple environment圖6 簡(jiǎn)單環(huán)境下適應(yīng)值變化曲線

由圖5 可知，4 種算法均可規(guī)劃出規(guī)避障礙的有效路徑，但ES-PSO 算法能夠跳出局部最優(yōu)，得到更佳路徑。在該環(huán)境下重復(fù)測(cè)試100 次，實(shí)驗(yàn)結(jié)果如表2 所示。

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

Table 2 Simple environment path planning表2 簡(jiǎn)單環(huán)境路徑規(guī)劃

設(shè)定大量障礙物，且障礙物形狀不僅局限于圓形的復(fù)雜測(cè)試環(huán)境，如圖7 所示。圖7、圖8 分別為復(fù)雜測(cè)試環(huán)境下4 種粒子群算法的路徑規(guī)劃結(jié)果與適應(yīng)值變化曲線。

Fig.7 Path planning results in a complex environment圖7 復(fù)雜環(huán)境下路徑規(guī)劃結(jié)果

Fig.8 Adaptation value curve in complex environment圖8 復(fù)雜環(huán)境下適應(yīng)值變化曲線

在該復(fù)雜環(huán)境下重復(fù)測(cè)試100 次，實(shí)驗(yàn)數(shù)據(jù)如表3 所示。

Table 3 Complex environment path planning表3 復(fù)雜環(huán)境路徑規(guī)劃

由表3 可知，當(dāng)障礙物變得復(fù)雜及不規(guī)則時(shí)，PSO 算法和GPSO 算法都過早進(jìn)入收斂且成功率有所降低。APSO算法規(guī)劃的結(jié)果雖維持了精度和成功率，但收斂效率較低。而本文的ES-PSO 算法在效率、尋優(yōu)精度、成功率3 個(gè)指標(biāo)上均表現(xiàn)良好，相比其它算法的規(guī)劃結(jié)果展示出顯著的改進(jìn)效果。

5 結(jié)語(yǔ)

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