劉艷紅， 陳田田， 張方方

(鄭州大學(xué) 電氣工程學(xué)院 河南 鄭州 450001)

0 引言

移動(dòng)機(jī)器人在工業(yè)、農(nóng)業(yè)、航空航天、醫(yī)療、服務(wù)等領(lǐng)域的應(yīng)用越來越廣泛。 路徑規(guī)劃對(duì)提高移動(dòng)機(jī)器人運(yùn)動(dòng)的準(zhǔn)確性和運(yùn)行效率起著關(guān)鍵作用，已經(jīng)成為相關(guān)研究領(lǐng)域中的重要課題[1]。 移動(dòng)機(jī)器人路徑規(guī)劃是在有障礙物的環(huán)境中從起點(diǎn)到目標(biāo)點(diǎn)規(guī)劃出一條無碰撞路徑。 路徑規(guī)劃包括工作空間建模和最優(yōu)路徑規(guī)劃。 常用的工作空間建模方法有柵格法、幾何空間法、拓?fù)浞ǖ萚2]。 柵格法和幾何空間法不能有效地表達(dá)環(huán)境的復(fù)雜性。 拓?fù)浞梢员硎鋈汁h(huán)境的連通性，最具代表性的是MAKLINK圖。 常用的路徑規(guī)劃方法有Dijkstra算法[3]、A*搜索算法[4]、快速搜索隨機(jī)樹法(RRT)[5]、粒子群優(yōu)化(PSO)算法[6]等。 PSO算法是模擬鳥群飛行覓食行為的群智能搜索算法，具有易實(shí)現(xiàn)、精度高、收斂快等特點(diǎn)[7]，但也存在算法易陷入早熟收斂問題。 針對(duì)這個(gè)問題，文獻(xiàn)[8]將遺傳算法和隨機(jī)編碼的交叉算子(RCPSO)引入PSO算法中；文獻(xiàn)[9]提出了基于跳出機(jī)制和牽引操作的粒子群優(yōu)化(JMPOPSO)算法；文獻(xiàn)[10]提出了加速收斂的粒子群優(yōu)化(ACPSO)算法，使粒子從本地最小的吸引力區(qū)域逃脫。 此外，文獻(xiàn)[11]通過采用動(dòng)態(tài)分組的粒子群(DGPSO)優(yōu)化機(jī)制，提高了路徑搜索的安全性和實(shí)時(shí)性。

本文針對(duì)基本粒子群優(yōu)化(basic particle swarm optimization, BPSO)算法易陷入局部最優(yōu)、規(guī)劃路徑較長等問題，提出了優(yōu)于平均適應(yīng)度值的改進(jìn)粒子群優(yōu)化(superior average particle swarm optimization, SAPSO)算法，該算法中粒子的更新速度是向適應(yīng)度值優(yōu)于平均適應(yīng)度值的粒子學(xué)習(xí)，并對(duì)低于平均適應(yīng)度值的粒子進(jìn)行變異處理。 仿真結(jié)果表明該方法提高了粒子的多樣性，并且規(guī)劃的路徑最短。

1 工作空間建模和次優(yōu)路徑規(guī)劃

1.1 移動(dòng)機(jī)器人工作空間建模

移動(dòng)機(jī)器人工作在400 cm*400 cm的室內(nèi)環(huán)境中，空間分布著不同形狀的障礙物，障礙物形狀、位置等信息已知，采用MAKLINK圖建立移動(dòng)機(jī)器人工作空間的障礙物頂點(diǎn)模型。 為了實(shí)現(xiàn)基于PSO算法的移動(dòng)機(jī)器人路徑規(guī)劃，給出以下假設(shè)：① 移動(dòng)機(jī)器人的運(yùn)動(dòng)空間中分布著有限個(gè)已知的障礙物，并且障礙物的高平行于Z軸，即可以忽略障礙物的高度信息，只用(X,Y)平面進(jìn)行描述；② 為保證路徑不太接近障礙物，把障礙物的邊界擴(kuò)展為機(jī)器人本體在長、寬方向上最大尺寸的一半，此時(shí)機(jī)器人可當(dāng)作質(zhì)點(diǎn)，且尺寸大小忽略不計(jì)。 基于以上假設(shè)，使用MAKLINK圖建立機(jī)器人的工作空間模型，得到如圖1所示的機(jī)器人運(yùn)動(dòng)空間鏈路圖。圖1中的多邊形是工作環(huán)境中的靜態(tài)障礙物，S為起始點(diǎn)，T為目標(biāo)點(diǎn)，虛線是障礙物的自由鏈接，實(shí)線是構(gòu)成無碰撞路徑網(wǎng)絡(luò)的自由鏈路中點(diǎn)之間的連線，連接S和T形成一個(gè)集成的網(wǎng)絡(luò)圖。v1,v2, …,v27分別為每個(gè)自由鏈路的中心，表示機(jī)器人的可達(dá)點(diǎn)。

1.2 基于Dijkstra算法的路徑規(guī)劃

建立機(jī)器人的工作空間模型后，路徑規(guī)劃問題就轉(zhuǎn)化為最短路徑搜索問題。 采用Dijkstra算法得到如圖2所示的由G=(ωi,j,V,E)表示的權(quán)重?zé)o向圖的全局次優(yōu)路徑，其中：ωi,j是兩個(gè)相鄰中間點(diǎn)的距離；V是一系列的中點(diǎn)，表示圖中所有頂點(diǎn)的集合；E是一組線，表示兩個(gè)相鄰自由鏈接的中點(diǎn)彼此連接的線。 由Dijkstra算法得到的全局次優(yōu)路徑如圖2中實(shí)線所示，表示為S→v24→v22→v23→v16→v14→v5→v4→T。

圖1 基于自由凸多邊形的MAKLINK圖Figure 1 MAKLINK diagram based on free convex polygon

圖2 Dijkstra算法得到的全局次優(yōu)路徑Figure 2 Global suboptimal path obtained by Dijkstra algorithm

由于Dijkstra算法得到的機(jī)器人全局次優(yōu)路徑是沿著自由鏈路中心的連線行走，而不是在整個(gè)網(wǎng)絡(luò)路徑上行走。 因此，該算法得到的并不是整個(gè)路徑規(guī)劃空間的最短路徑。 下一節(jié)將采用PSO算法對(duì)獲得的次優(yōu)路徑進(jìn)行二次優(yōu)化，從而得到全局最優(yōu)路徑。

2 基于改進(jìn)粒子群算法的最優(yōu)路徑規(guī)劃

2.1 基本粒子群算法

1995年Kennedy和Eberhart根據(jù)鳥群飛行覓食行為提出了BPSO算法。 在BPSO算法中，粒子被初始化為一群隨機(jī)粒子(隨機(jī)解)，然后通過迭代找到最優(yōu)解。 在每一次迭代中，粒子通過追隨兩個(gè)“極值”來更新自己，第一個(gè)極值是粒子本身所找到的最優(yōu)解，稱為個(gè)體極值pbest；另一個(gè)極值是整個(gè)種群找到的最優(yōu)解，稱為全局極值pgbest。 所有粒子都有一個(gè)適應(yīng)度值和速度，決定它們飛翔的方向和距離，然后粒子跟隨當(dāng)前的最優(yōu)粒子在D維空間中搜索以尋求最佳方案[7]。 BPSO算法描述為：假設(shè)在D維空間中，粒子總數(shù)為n，第i個(gè)粒子在D維搜索空間的位置表示為xi=[xi1,xi2,…,xiD]，速度表示為vi=[vi1,vi2,…,viD]，在D維搜索空間中，從t時(shí)刻到t+1時(shí)刻粒子的速度和位置更新公式可以分別表示為

vid(t+1)=ω×vid(t)+c1×rand()×[pibest(t)-xid(t)]+c2×rand()×[pgbest(t)-xid(t)],

(1)

xid(t+1)=xid(t)+vid(t+1),

(2)

式中：i=1, 2,…,n；d=1, 2,…,D；rand()為[0,1]的隨機(jī)數(shù)；ω為慣性系數(shù)；c1、c2為學(xué)習(xí)因子；vid表示D維空間中粒子的速度；xid表示D維空間中粒子的位置；pibest表示粒子本身所找到的最優(yōu)解；pgbest表示整個(gè)種群中粒子所找到的最優(yōu)解。

2.2 改進(jìn)粒子群算法的路徑規(guī)劃

由于BPSO算法在進(jìn)行路徑規(guī)劃時(shí)選擇最優(yōu)的粒子進(jìn)行學(xué)習(xí)，導(dǎo)致粒子數(shù)目急劇減少，造成了粒子的多樣性降低，使粒子陷入局部最優(yōu)。 本文提出了SAPSO算法，即粒子更新速度時(shí)，粒子選擇適應(yīng)度值優(yōu)于平均適應(yīng)度值的粒子進(jìn)行學(xué)習(xí)，并對(duì)低于平均適應(yīng)度值的粒子進(jìn)行變異處理。 該方法增加了粒子的多樣性，避免粒子陷入局部最優(yōu)，使粒子快速收斂到全局最優(yōu)。 由圖1可知，路徑與鏈接線的交點(diǎn)有7個(gè)，故搜索空間為7維，粒子的適應(yīng)度函數(shù)可以表示為

(3)

式中：i為維度序號(hào)。 SAPSO算法的速度和位置更新公式可以分別表示為

(4)

(5)

(6)

(7)

綜上所述，采用SAPSO算法進(jìn)行最優(yōu)路徑規(guī)劃的步驟如下。

Step 1: 初始化粒子種群大小n，慣性因子ω，加速因子c1、c2、α1、α2及迭代次數(shù)t。

Step 2: 根據(jù)式(1)、式(2)初始化粒子的位置x和速度v。

Step 3: 根據(jù)式(3)計(jì)算每個(gè)粒子的適應(yīng)度值，并計(jì)算出粒子的平均適應(yīng)度值。

Step 4: 選出優(yōu)于平均適應(yīng)度值的粒子，并根據(jù)式(4)、式(5)更新適應(yīng)度值優(yōu)于平均適應(yīng)度值的粒子的位置和速度。 適應(yīng)度值低于平均適應(yīng)度值的粒子根據(jù)式(6)更新位置。

Step 5: 若滿足停止條件則停止搜索，并輸出結(jié)果，否則返回Step 3。

3 仿真實(shí)驗(yàn)

為驗(yàn)證本文所提出的SAPSO算法的有效性，在CPU為2.50 GHz、內(nèi)存為4.00 GB的個(gè)人計(jì)算機(jī)上進(jìn)行仿真驗(yàn)證。 首先驗(yàn)證SAPSO算法的收斂性；其次在圖1所設(shè)置的障礙物環(huán)境中采用SAPSO算法進(jìn)行仿真，并與其他改進(jìn)PSO算法在適應(yīng)度值、路徑長度及運(yùn)行時(shí)間方面進(jìn)行對(duì)比；最后在不同障礙物環(huán)境中分別采用改進(jìn)A*算法、改進(jìn)RRT算法和SAPSO算法進(jìn)行路徑規(guī)劃，并比較其性能。 在仿真過程中SAPSO算法參數(shù)設(shè)置為：粒子總數(shù)n為100，迭代次數(shù)t為1 000。 根據(jù)文獻(xiàn)[13]提出的保證粒子群算法收斂的參數(shù)選擇原則，將加速度系數(shù)設(shè)為c1=c2=1.4，ω=0.9，α1=α2=1.49。

為了降低算法隨機(jī)性帶來的影響，基于文獻(xiàn)[9]，采用單峰函數(shù)Rosenbrock和多峰函數(shù)Griewwank分別測試BPSO算法和SAPSO算法的收斂性。 所采用的Rosenbrock函數(shù)和Griewwank函數(shù)可以分別表示為

(8)

(9)

收斂性能對(duì)比結(jié)果如圖3所示?？梢钥闯?，在單峰函數(shù)Rosenbrock和多峰函數(shù)Griewwank下，SAPSO算法比BPSO算法具有更快的收斂速度。

圖3 收斂性能對(duì)比Figure 3 Convergence performance comparison

圖4 基于3種算法得到的全局最優(yōu)路徑Figure 4 Global optimal path obtained by three algorithms

在圖1所設(shè)置的障礙物環(huán)境中采用SAPSO算法進(jìn)行路徑規(guī)劃，并與Dijkstra算法和BPSO算法進(jìn)行比較。 圖4為基于3種算法得到的全局最優(yōu)路徑，其中黑色帶三角路線為基于Dijkstra算法得到的路徑，其長度為495.013 cm；黑色帶加號(hào)路線為基于BPSO算法得到的路徑，其長度為464.747 cm；黑色帶圓圈路線為基于SAPSO算法得到的路徑，其長度為449.506 cm。

此外，在圖1所設(shè)定的障礙物環(huán)境中，將SAPSO算法與BPSO、RCPSO[8]、JMPOPSO[9]、ACPSO[10]、DGPSO[11]等其他改進(jìn)粒子群算法進(jìn)行仿真比較[14]，不同算法的路徑規(guī)劃性能對(duì)比結(jié)果如表1所示。 表1記錄了多種改進(jìn)粒子群算法進(jìn)行路徑規(guī)劃的適應(yīng)度的最大值、平均值、最小值，以及路徑長度和平均運(yùn)行時(shí)間。 由表1可以看出，SAPSO算法的適應(yīng)度值最大，規(guī)劃的路徑最短。 由于SAPSO算法的時(shí)間復(fù)雜度只與粒子的維數(shù)有關(guān)[12]，而其他改進(jìn)算法的復(fù)雜度除了與粒子維數(shù)有關(guān)外，還與粒子數(shù)目、迭代次數(shù)有關(guān)，如DGPSO算法的時(shí)間復(fù)雜度為O(n*D*itermax)[11]，當(dāng)粒子數(shù)目較多時(shí)，復(fù)雜度將會(huì)增加。 因此，SAPSO算法的復(fù)雜度相對(duì)較低，路徑規(guī)劃時(shí)間最短。

表1 不同改進(jìn)粒子群優(yōu)化算法的路徑規(guī)劃性能對(duì)比Table 1 Comparison of path planning performance among different improved particle swarm optimization algorithms

移動(dòng)機(jī)器人工作空間障礙物設(shè)置如圖5所示。在圖1、圖5所設(shè)置的障礙物環(huán)境中分別采用改進(jìn)A*算法、改進(jìn)RRT算法和SAPSO算法，在n=100、迭代次數(shù)為1 000的條件下進(jìn)行路徑規(guī)劃，3種算法的路徑規(guī)劃性能對(duì)比結(jié)果如表2所示。 可以看出，SAPSO算法在3種障礙物環(huán)境中得到的路徑均最短且規(guī)劃成功率最高。 改進(jìn)A*算法由于計(jì)算量大而導(dǎo)致內(nèi)存占用嚴(yán)重，且計(jì)算時(shí)間較長，并且改進(jìn)A*算法中啟發(fā)函數(shù)的選取非常重要，引入的啟發(fā)信息過大或過小都會(huì)影響最優(yōu)路徑的搜索效率。 改進(jìn)RRT算法主要是在樹的生長方式等方面進(jìn)行了完善，提高了收斂速度，但是由于隨機(jī)采樣的性質(zhì)，改進(jìn)RRT算法前后兩次得到的結(jié)果可能完全不同。 相比于改進(jìn)A*算法、改進(jìn)RRT算法，本文所提出的SAPSO算法具有收斂速度快、參數(shù)少、實(shí)現(xiàn)簡單等特點(diǎn)，且粒子在更新速度時(shí)不是向最優(yōu)的粒子學(xué)習(xí)，而是向適應(yīng)度值優(yōu)于平均適應(yīng)度值的粒子學(xué)習(xí)，并對(duì)低于平均適應(yīng)度值的粒子進(jìn)行變異處理，故該方法能夠提高粒子的多樣性，避免粒子陷入局部最優(yōu)，實(shí)現(xiàn)了最短路徑規(guī)劃。

圖5 移動(dòng)機(jī)器人工作空間障礙物設(shè)置Figure 5 Obstacle environments of the mobile robot workspace

表2 3種算法的路徑規(guī)劃性能對(duì)比Table 2 Path planning performance comparison of three algorithms

4 結(jié)論

本文針對(duì)BPSO算法在路徑規(guī)劃時(shí)易陷入局部最優(yōu)、規(guī)劃路徑較長等問題，提出了改進(jìn)粒子群優(yōu)化算法SAPSO，即當(dāng)粒子更新速度時(shí)，粒子不是向最優(yōu)的粒子學(xué)習(xí)，而是向適應(yīng)度值優(yōu)于平均適應(yīng)度值的粒子學(xué)習(xí)，并對(duì)低于平均適應(yīng)度值的粒子進(jìn)行變異處理。將指數(shù)變量權(quán)重加入改進(jìn)粒子群算法中，使粒子以最短的時(shí)間得到最優(yōu)解。 改進(jìn)的粒子群算法提高了粒子的多樣性，使粒子逃離局部最優(yōu)。 仿真結(jié)果表明，所提出的SAPSO算法具有適應(yīng)度高、規(guī)劃路徑短、運(yùn)行時(shí)間短等優(yōu)點(diǎn)。