巫光福，萬路萍

（江西理工大學(xué)信息工程學(xué)院，江西贛州 341000）

在移動(dòng)機(jī)器人領(lǐng)域中，導(dǎo)航技術(shù)是非常關(guān)鍵的一個(gè)研究點(diǎn)，而路徑規(guī)劃是導(dǎo)航中一個(gè)很重要的任務(wù)。所謂的路徑規(guī)劃[1-2]就是在一個(gè)包含障礙物的環(huán)境中，從起點(diǎn)到終點(diǎn)尋找到一條最優(yōu)或者近似最優(yōu)的路徑，并滿足一定的約束條件，如最短路徑、耗時(shí)最少和安全性最高等等。隨著國(guó)內(nèi)外學(xué)者對(duì)機(jī)器人路徑規(guī)劃領(lǐng)域越來越深入的研究，提出的解決路徑規(guī)劃的方法也越來越多。由于A*算法、人工勢(shì)場(chǎng)法等傳統(tǒng)算法[3-5]在解決路徑規(guī)劃問題存在很大不足，如在復(fù)雜環(huán)境中，計(jì)算量大，占用內(nèi)存多，尋找符合標(biāo)準(zhǔn)的最優(yōu)路徑的效率比較差等。由于仿生智能算法的出現(xiàn)，越來越多的學(xué)者研究智能算法及其改進(jìn)方法，并將其應(yīng)用在解決機(jī)器人路徑規(guī)劃問題上。實(shí)驗(yàn)證明，灰狼算法[6]、蟻群算法[7-8]、遺傳算法[9-11]、粒子群算法[12-13]等智能算法在優(yōu)化移動(dòng)機(jī)器人路徑問題上很有效。

粒子群優(yōu)化算法（Particle swarm optimization algorithm，PSO）最早于1995年11月由Kennedy 和Eberhart 提出[14]，它是基于自然界中鳥類、魚類等群體的思想的進(jìn)化計(jì)算方法。與遺傳算法等群智能優(yōu)化算法相比，粒子群優(yōu)化算法具有相對(duì)簡(jiǎn)單性、可調(diào)參數(shù)少和收斂速度較快的特點(diǎn)，這也使得其成為解決移動(dòng)機(jī)器人路徑規(guī)劃問題的一個(gè)應(yīng)用比較廣泛的算法。但是粒子群優(yōu)化算法在搜索過程中也容易出現(xiàn)早熟現(xiàn)象[15]，使得路徑規(guī)劃陷入局部最優(yōu)，因而也出現(xiàn)了很多改進(jìn)算法。文獻(xiàn)[16]將蟻群算法與粒子群優(yōu)化算法融合，通過蟻群算法得到全局最優(yōu)路徑，改進(jìn)的粒子群算法在全局范圍求最優(yōu)解，并將其作為螞蟻的初始信息素，從而蟻群算法能進(jìn)一步尋優(yōu)，兩者優(yōu)勢(shì)互補(bǔ)，提高了算法尋找最優(yōu)解的能力。文獻(xiàn)[17]設(shè)計(jì)了在全局最優(yōu)位置引入擾動(dòng)全局更新機(jī)制，避免了算法的停滯。文獻(xiàn)[18]利用機(jī)器人到障礙物和目標(biāo)點(diǎn)的距離，構(gòu)造了一個(gè)新的目標(biāo)函數(shù)，機(jī)器人能夠成功地避開障礙物并朝著目標(biāo)點(diǎn)運(yùn)動(dòng)。盡管這些改進(jìn)算法都取得了不錯(cuò)的效果，但是粒子群優(yōu)化算法在移動(dòng)機(jī)器人路徑規(guī)劃的仍然存在收斂速度慢和易陷入局部最優(yōu)等問題。

針對(duì)目前粒子群優(yōu)化算法的不足，對(duì)算法進(jìn)行了一些改進(jìn)。首先對(duì)全局最佳粒子的速度進(jìn)行了一個(gè)輕微的擾動(dòng)，以免其陷入局部最?。蝗缓笫褂昧朔蔷€性慣性權(quán)重來平衡算法全局搜索和局部搜索能力；最后定義了考慮路徑長(zhǎng)度和平滑的適應(yīng)度函數(shù)。實(shí)驗(yàn)證明，改進(jìn)的粒子群優(yōu)化算法能較快的尋找到符合要求的路徑。

1 環(huán)境模型

路徑規(guī)劃是移動(dòng)機(jī)器人在復(fù)雜環(huán)境下完成自主移動(dòng)導(dǎo)航的重要必備條件之一，在環(huán)境已經(jīng)確定的情況下，按照一定的標(biāo)準(zhǔn)找到一條從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑，在這個(gè)過程中，要能夠避開與障礙物的碰撞。對(duì)機(jī)器人在二維工作環(huán)境進(jìn)行了簡(jiǎn)單的建模，整個(gè)環(huán)境在一個(gè)二維平面坐標(biāo)中，考慮這是個(gè)沒有自身尺寸的點(diǎn)機(jī)器人，在搜索區(qū)域內(nèi)，隨機(jī)放置一些圓形障礙物，圓形障礙物的數(shù)量和半徑可根據(jù)實(shí)際設(shè)置。如圖1所示，粒子群算法初始化路徑采用隨機(jī)策略，粒子隨機(jī)分散在搜索區(qū)域，而隨機(jī)生成的路徑節(jié)點(diǎn)可表示為 p=（x,y），假設(shè)粒子經(jīng)過了m個(gè)節(jié)點(diǎn)，生成的路徑則表示為 P=（p1,p2,···,pm）。

圖1 環(huán)境模型

路徑在隨機(jī)初始化的時(shí)候，可能生成的路徑是經(jīng)過障礙物的。在避障方面，用重定位法代替罰分法進(jìn)行避障。在罰分法中，我們需要把掉在障礙物內(nèi)的粒子或者路徑從障礙物內(nèi)通過的粒子去除。然而，在重定位方法中，粒子的位置會(huì)被重新定位，因此不會(huì)丟失粒子。當(dāng)障礙物與連接粒子的新舊位置的直線有交點(diǎn)時(shí)，舊位置的粒子就需要重新尋找位置，反之，沒有交點(diǎn)時(shí)，那么該位置就是粒子的新位置。這樣的判定方法就可以在避免障礙物的同時(shí)還能不損失粒子。

2 粒子群優(yōu)化算法的移動(dòng)機(jī)器人路徑規(guī)劃

2.1 標(biāo)準(zhǔn)粒子群優(yōu)化算法

在粒子群優(yōu)化算法中，每個(gè)粒子會(huì)根據(jù)自己和同伴的飛行經(jīng)驗(yàn)來調(diào)整自己的行進(jìn)路徑。每個(gè)粒子經(jīng)過了的最佳位置就是個(gè)體最優(yōu)值，而整個(gè)種群所經(jīng)過的最佳位置叫全局最優(yōu)值。每個(gè)粒子通過這兩個(gè)最優(yōu)值來不停地更新自己的位置。根據(jù)所需要優(yōu)化的問題，提出合適的適應(yīng)度函數(shù)，得到的適應(yīng)度值則用來評(píng)價(jià)粒子的優(yōu)劣。粒子群算法采用的是速度-位置的搜索模型，每個(gè)粒子都有一個(gè)速度來決定運(yùn)動(dòng)的方向和大小的更新。粒子尋優(yōu)過程就是根據(jù)自我經(jīng)驗(yàn)學(xué)習(xí)和社會(huì)經(jīng)驗(yàn)學(xué)習(xí)來更新自己的位置和速度。若該粒子群有N 個(gè)粒子且每個(gè)粒子有n維，則它的速度和位置更新式分別為：

標(biāo)準(zhǔn)粒子群算法如圖2所示。

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

標(biāo)準(zhǔn)粒子群算法具體的步驟如下：

步驟1在優(yōu)化問題的d 維環(huán)境下隨機(jī)初始化粒子的位置和速度。

步驟2根據(jù)適應(yīng)度函數(shù)計(jì)算粒子的適應(yīng)度值。

步驟3對(duì)于每次迭代，將每個(gè)粒子的適應(yīng)度與其先前獲得的最佳適應(yīng)度（pbest）進(jìn)行比較。如果當(dāng)前值優(yōu)于pbest，則將pbest設(shè)置為當(dāng)前值。

步驟4比較每個(gè)粒子的pbest值，最佳值則更新為全局最優(yōu)值gbest。

步驟5 根據(jù)式（1）和式（2）更新粒子速度和位置。

步驟6更新迭代次數(shù)，直到達(dá)到設(shè)置的最大迭代次數(shù)，則停止搜索，輸出全局最優(yōu)值的位置，否則，轉(zhuǎn)至步驟2，繼續(xù)執(zhí)行。

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

2.2.1 速度更新

粒子群算法中最基本的概念是每個(gè)粒子在每一次迭代過程中，以一定的權(quán)重的速度朝著局部和全局最佳位置移動(dòng)，如圖3所示，群中一個(gè)粒子以一定的速度移動(dòng)的示意圖。

圖3 一個(gè)粒子位置移動(dòng)概念

粒子的速度決定粒子的運(yùn)動(dòng)的方向和距離，而速度也會(huì)根據(jù)自身以及其他粒子的經(jīng)驗(yàn)來調(diào)整，并實(shí)現(xiàn)個(gè)體在可行解空間搜索到最優(yōu)解。但是隨著迭代次數(shù)的增加，粒子可能陷入局部最優(yōu)值。為了保證算法收斂速度更快和盡可能避免算法陷入局部最小值，提出了速度更新方式，即當(dāng)粒子陷入局部值時(shí)，對(duì)全局最優(yōu)粒子的速度進(jìn)行一個(gè)輕微的擾動(dòng)，速度更新方式表達(dá)式為

式中： r3是單位隨機(jī)值；α =0.2，β =0.3。

2.2.2 慣性權(quán)重

在標(biāo)準(zhǔn)粒子群算法中，慣性權(quán)重能夠調(diào)節(jié)粒子的速度，影響著粒子的全局搜索能力和局部搜索能力，為了防止粒子發(fā)生早熟現(xiàn)象，保證能夠找到最優(yōu)解，那么粒子的搜索能力都應(yīng)該隨著進(jìn)化過程改變。Shi第一次提出粒子群優(yōu)化算法中慣性權(quán)重的概念，并且分析了慣性權(quán)重越大越有利于全局搜索，越小則越有利于局部搜索[19]。為了更好的平衡局部和全局的搜索能力，提出了線性遞減權(quán)重（Linear decreasing inertia weight,LDIW），表達(dá)式為

式中：慣性權(quán)重 ω的值是從 ωmax遞 減到 ωmin； Tmax為最大迭代次數(shù)。

隨著迭代次數(shù)的增加，慣性權(quán)重線性遞減。在最開始的迭代過程中，慣性權(quán)重值較大，保證了算法比較強(qiáng)的全局搜索能力，在后期的迭代過程，慣性權(quán)重值較小，提高算法在局部的搜索精度。為了使粒子群優(yōu)化算法在避免陷入局部最優(yōu)和獲得更好的搜索能力，本文提出了非線性遞減慣性權(quán)重（Nonlinear decreasing inertial weight，NDIW），表達(dá)式為

式中c =2.3。

兩種慣性權(quán)重的變化曲線如圖4所示。

圖4 兩種慣性權(quán)重的變化曲線

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

要規(guī)劃好機(jī)器人的路徑，一個(gè)合適的適應(yīng)度函數(shù)是非常重要的。根據(jù)移動(dòng)機(jī)器人在實(shí)際環(huán)境中的要求，過多的轉(zhuǎn)彎會(huì)使得機(jī)器人能耗增大，移動(dòng)時(shí)間也會(huì)增加，因此最優(yōu)路徑應(yīng)該要滿足最短性和平滑性這樣的約束條件。本文提出了一個(gè)新的適應(yīng)度函數(shù)，路徑的距離計(jì)算方式為每次迭代過程中每個(gè)粒子與目標(biāo)點(diǎn)之間的歐幾里得距離，路徑的平滑度判定則是迭代過程中目標(biāo)點(diǎn)與機(jī)器人兩個(gè)連續(xù)的位置連接而成的兩條假想直線的夾角，目標(biāo)點(diǎn)與兩個(gè)連續(xù)位置的夾角如圖5所示。

圖5 目標(biāo)點(diǎn)與兩個(gè)連續(xù)位置的夾角

這兩個(gè)連續(xù)位置即為gkbest和gk-1best。最短距離表達(dá)式為

式中k 為迭代次數(shù)。

平滑路徑的數(shù)學(xué)表達(dá)式為

這里平滑度的表示為在兩次連續(xù)迭代過程中機(jī)器人最小移動(dòng)角度，而不是一條樣條曲線。最后的適應(yīng)度函數(shù)公式為

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

為了驗(yàn)證算法的正確性和有效性，設(shè)置了一個(gè)3000×3000單位邊界的二維正方形搜索空間。通過反復(fù)的實(shí)驗(yàn)和大量參數(shù)的使用，確定了在更快的收斂速度、有效避障、生成可行性路徑和避免局部最優(yōu)等條件下，所使用的最終參數(shù)。

具體設(shè)置如下：粒子群規(guī)模N=100，最大迭代次數(shù)Tmax=50，學(xué)習(xí)因子 c1=c2=2，慣性權(quán)重ωmax=0.9，ωmin=0.4，常數(shù) c =2.3， 適應(yīng)度函數(shù)中λ1=1，λ2=0.25。障礙物個(gè)數(shù)為5和8的路徑尋優(yōu)結(jié)果分別如圖6和圖7所示，起點(diǎn)位置是（1000,1000），終點(diǎn)位置是（-500,-1000），圖中圓形是障礙物，粒子的顏色基于迭代。每次迭代時(shí)，粒子的顏色從藍(lán)色（初始粒子分布）更改為紅色（最終粒子分布）。實(shí)驗(yàn)中，環(huán)境中的障礙物的位置是動(dòng)態(tài)變化的，個(gè)數(shù)自行設(shè)置，不同的障礙物個(gè)數(shù)，該算法都能找到符合要求的路徑，拐彎的次數(shù)不多，粒子都?jí)虮荛_障礙物，收斂速度很快。初始粒子都比較分散，可以看到，粒子（紅色）是最終都在目標(biāo)點(diǎn)附近聚集。

圖6 障礙物個(gè)數(shù)為5的路徑尋優(yōu)結(jié)果

圖7 障礙物個(gè)數(shù)為8的路徑尋優(yōu)結(jié)果

為了對(duì)比改進(jìn)粒子群優(yōu)化算法與標(biāo)準(zhǔn)優(yōu)化算法，在障礙物個(gè)數(shù)為10的相同的環(huán)境下實(shí)驗(yàn)，尋優(yōu)結(jié)果如圖8和圖9所示，在經(jīng)過50次迭代過程后，兩個(gè)算法都找到了路徑，但是，改進(jìn)粒子群優(yōu)化算法搜到的路徑相對(duì)來說更平滑，粒子最終分布都比較集中。而在標(biāo)準(zhǔn)粒子群算法中，粒子的最終位置分布相對(duì)較散。

圖8 改進(jìn)PSO 算法路徑尋優(yōu)結(jié)果

圖9 基本PSO的路徑尋優(yōu)結(jié)果

粒子在尋找路徑的時(shí)候，有的時(shí)候還是會(huì)無法找到路徑，或者找到的路徑是經(jīng)過了障礙物的，如表1是在50次迭代的實(shí)驗(yàn)結(jié)果，可知，改進(jìn)的算法在這樣已知搜索空間內(nèi)，對(duì)路徑的搜索的成功率比標(biāo)準(zhǔn)算法要高一些。由于算法收斂速度很快，實(shí)驗(yàn)時(shí)在控制臺(tái)觀察看到的是比較少的迭代結(jié)果。

表1 路徑規(guī)劃的實(shí)驗(yàn)結(jié)果比較

4 結(jié)論

仿真結(jié)果表明，所實(shí)現(xiàn)的方法能夠滿足路徑規(guī)劃的要求，尋找到合適的最優(yōu)路徑。當(dāng)粒子陷入局部最優(yōu)的時(shí)候，給了全局最優(yōu)速度一個(gè)輕微的擾動(dòng)，并使用了非線性遞減慣性權(quán)重，使得粒子的收斂速度加快，也平衡了全局和局部的搜索能力，從而粒子能搜索到最優(yōu)路徑。在環(huán)境中隨機(jī)放置障礙物，生成的路徑能夠避免所有障礙物，最終粒子的位置也不在障礙物邊界內(nèi)。該方法易于實(shí)現(xiàn)，算法速度快，適用于復(fù)雜多變的移動(dòng)障礙物環(huán)境。正如本文所展示的，該方法可以在所有類型的環(huán)境中執(zhí)行。這種方法是靈活的，這樣我們可以改變?nèi)魏螀?shù)，來實(shí)現(xiàn)一定需求的路徑規(guī)劃。