韓儀灑,張 莉,譚海燕,薛旭璐,郭瑞鴻,郭 倩

(西安工程大學(xué) 電子信息學(xué)院,陜西 西安 710048)

0 引 言

移動(dòng)機(jī)器人路徑規(guī)劃是自主機(jī)器人導(dǎo)航的核心問題之一[1],是機(jī)器人完成人類所賦予任務(wù)的前提,在一定程度上體現(xiàn)了機(jī)器人的智能化水平。如智能工廠[2]、物流配送[3]等領(lǐng)域,都和路徑規(guī)劃緊密相關(guān)。所謂路徑規(guī)劃就是機(jī)器人在有障礙物的工作環(huán)境中,能夠規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的安全無(wú)碰路徑[4],并且盡量?jī)?yōu)化運(yùn)行軌跡[5]。

路徑規(guī)劃具有多約束性、多目標(biāo)性和復(fù)雜性等特點(diǎn)[6]。目前,國(guó)內(nèi)外學(xué)者已針對(duì)移動(dòng)機(jī)器人路徑規(guī)劃做了大量研究和改進(jìn),所采用的方法主要分為傳統(tǒng)方法和智能方法兩大類[7]。文獻(xiàn)[8-9]運(yùn)用人工勢(shì)場(chǎng)法對(duì)機(jī)器人進(jìn)行路徑規(guī)劃,該方法簡(jiǎn)單,運(yùn)算速度快,但存在易陷入局部極小值和目標(biāo)不可達(dá)問題；文獻(xiàn)[10-12]通過快速隨機(jī)樹在環(huán)境空間中搜索路徑,隨機(jī)性強(qiáng),理論上具有完備性,但是生成的路徑是一條條小的折線,光滑程度低,對(duì)機(jī)器人行進(jìn)不利；文獻(xiàn)[13-14]通過柵格法對(duì)環(huán)境進(jìn)行建模,采用A*算法尋找最優(yōu)路徑,該方法為啟發(fā)性搜索算法,但是該方法在復(fù)雜環(huán)境下為了存儲(chǔ)open集和close集,占用的內(nèi)存開銷非常大,運(yùn)算速度慢；文獻(xiàn)[15-16]運(yùn)用改進(jìn)的遺傳算法進(jìn)行路徑規(guī)劃,該方法具有魯棒性強(qiáng)、隱性并行性等優(yōu)點(diǎn),但容易出現(xiàn)早熟收斂現(xiàn)象；文獻(xiàn)[17]運(yùn)用蟻群算法進(jìn)行路徑規(guī)劃,通過環(huán)境中留下的信息素的多少尋找最優(yōu)路徑,該方法并行性強(qiáng),但是在搜索過程中容易出現(xiàn)停滯現(xiàn)象；文獻(xiàn)[18]采用模擬鳥群覓食行為的粒子群算法進(jìn)行路徑規(guī)劃,該方法具有參數(shù)調(diào)節(jié)少、搜索速度快等優(yōu)點(diǎn),但在搜索后期易出現(xiàn)早熟收斂現(xiàn)象。

路徑規(guī)劃方法很多,但有各自的優(yōu)缺點(diǎn),本文采用粒子群算法進(jìn)行路徑規(guī)劃。雖然文獻(xiàn)[19-20]通過自適應(yīng)調(diào)整慣性權(quán)重和加速因子等方法對(duì)粒子群算法做了改進(jìn),然而該算法在應(yīng)用時(shí)仍然存在早熟現(xiàn)象,收斂精度低,導(dǎo)致搜索到的路徑不是最優(yōu)路徑，且路徑不平滑。針對(duì)早熟問題,本文采用極坐標(biāo)系下極角的方式進(jìn)行規(guī)劃,并在粒子群算法后期通過對(duì)粒子進(jìn)行變異操作,增加粒子群種群的多樣性,使種群中的粒子易于脫離局部極值點(diǎn),有效規(guī)避了算法后期出現(xiàn)早熟現(xiàn)象。針對(duì)所得到的路徑點(diǎn)中存在很多冗余路徑點(diǎn),使路徑不平滑,存在無(wú)效路徑的問題,本文通過路徑點(diǎn)冗余篩選算法對(duì)搜索到的路徑點(diǎn)進(jìn)行去冗余處理,使所規(guī)劃路徑更平滑更短,即移動(dòng)機(jī)器人在行進(jìn)過程中所消耗能量更少。

1 移動(dòng)機(jī)器人路徑規(guī)劃環(huán)境模型

在移動(dòng)機(jī)器人進(jìn)行路徑規(guī)劃之前,首先對(duì)移動(dòng)機(jī)器人的工作空間進(jìn)行建模，即建立機(jī)器人工作空間的環(huán)境模型,進(jìn)而在此環(huán)境模型的基礎(chǔ)上應(yīng)用粒子群算法,規(guī)劃移動(dòng)機(jī)器人的路徑。

機(jī)器人通過傳感器或其他設(shè)備獲取工作空間中所有障礙物的位置,障礙物個(gè)數(shù)有限。移動(dòng)機(jī)器人在和障礙物保持安全距離的前提下，繞開障礙物所占據(jù)的位置從起始點(diǎn)移動(dòng)到目標(biāo)點(diǎn)。在整個(gè)移動(dòng)的過程中,不考慮障礙物高度的影響,建立二維環(huán)境模型,如圖1所示。

圖1 環(huán)境模型Fig.1 Environmental model

以起始點(diǎn)為原點(diǎn)建立極坐標(biāo)系,所有障礙物的位置Xobs均用極徑和極角表示：

(1)

將移動(dòng)機(jī)器人視為一個(gè)質(zhì)點(diǎn),障礙物的影響區(qū)域膨化為一個(gè)圓形,該圓半徑R等于障礙物半徑、安全距離和移動(dòng)機(jī)器人的二倍半徑之和:

R=Ro+Rs+2Rr

(2)

(3)

式中:Ro為障礙物半徑;Rs為安全距離半徑;Rr為移動(dòng)機(jī)器人半徑;ρi和θi為第i個(gè)障礙物的極徑和極角;ρ和θ為該障礙物影響區(qū)域圓上點(diǎn)的極徑和極角。

2 基于粒子群算法的路徑規(guī)劃

2.1 自主路徑規(guī)劃算法思想

在建立好的二維環(huán)境模型中,從起始點(diǎn)到目標(biāo)點(diǎn)的路徑可以用環(huán)境模型中起始點(diǎn)、起始點(diǎn)和目標(biāo)點(diǎn)之間的路徑點(diǎn)以及目標(biāo)點(diǎn)所組成的點(diǎn)序列來表示。為了將粒子群算法中的粒子和路徑聯(lián)系起來,以障礙物位置極徑為半徑建立路徑點(diǎn)圓,每一個(gè)障礙物都有一個(gè)路徑點(diǎn)圓,如圖1中的虛線圓,在這些圓上找到路徑點(diǎn),再把起始點(diǎn)和這些路徑點(diǎn)依次連接起來,最后和目標(biāo)點(diǎn)連接起來,便構(gòu)成了一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑p,完成了一次路徑規(guī)劃。

p=[θs,ρs;r(θ,ρ);θg,ρg]

(4)

式中:θs,ρs,θg,ρg分別為起始點(diǎn)和目標(biāo)點(diǎn)的極角和極徑;r(θ,ρ)為所有路徑點(diǎn)圓上路徑點(diǎn)的極角和極徑的集合。

由于起始點(diǎn)和目標(biāo)點(diǎn)是已知的,需要搜索的路徑點(diǎn)位于環(huán)境模型中建立的路徑點(diǎn)圓上,因此有n個(gè)障礙物,就要找到n個(gè)路徑點(diǎn)。粒子群中每一個(gè)粒子都是優(yōu)化問題的一個(gè)解,每一個(gè)粒子就是一條路徑,粒子的維數(shù)就是障礙物的個(gè)數(shù)n,每一維的值就是該維所對(duì)應(yīng)的路徑點(diǎn)圓上路徑點(diǎn)的極角。建立粒子和路徑的聯(lián)系后,通過粒子群算法搜索,找到最優(yōu)解,即最優(yōu)路徑,所以式(4)可改寫為式(5):

p=[θs,ρs;P(θ,ρ);θg,ρg]

(5)

式中:P(θ,ρ)為粒子群算法在所有路徑點(diǎn)圓上所搜索到的路徑點(diǎn)的極角和極徑的集合。

2.2 構(gòu)造適應(yīng)度函數(shù)

為了評(píng)價(jià)粒子群算法搜索出的路徑優(yōu)劣,構(gòu)造適應(yīng)度函數(shù),通過適應(yīng)度值評(píng)價(jià)當(dāng)前路徑是否優(yōu)于上一次搜索的路徑,同時(shí)也是算法更新粒子個(gè)體極值和群體全局極值的依據(jù)。

在移動(dòng)機(jī)器人路徑規(guī)劃中,要求移動(dòng)機(jī)器人繞開障礙物,路徑點(diǎn)不能位于障礙物影響區(qū)域內(nèi),而且要在能繞開障礙物的前提下,盡可能的優(yōu)化路徑軌跡。由此可以構(gòu)造適應(yīng)度函數(shù)，即

(6)

式中:ffit為適應(yīng)度值;f(d)為起始點(diǎn)到目標(biāo)點(diǎn)的歐氏距離;f(d′)為粒子群算法搜索到的路徑總距離,用歐氏距離計(jì)算;g1(p),g2(p)分別為粒子群算法中搜索的路徑點(diǎn)占據(jù)和穿越障礙物的懲罰函數(shù);α,β分別為占據(jù)懲罰系數(shù)和穿越懲罰系數(shù),如果穿越時(shí),β取1,否則取0;γ用來調(diào)節(jié)穿越懲罰值和距離值的關(guān)系;ρo,θo,ρi,θi分別為第i個(gè)障礙物的極徑、極角和該路徑點(diǎn)圓上路徑點(diǎn)的極徑、極角。從式(6)中可以得出,適應(yīng)度值ffit的變化區(qū)間為(0,1],ffit越大代表找到的路徑越優(yōu)。

2.3 路徑規(guī)劃與仿真

用粒子群算法在環(huán)境模型中對(duì)移動(dòng)機(jī)器人進(jìn)行路徑規(guī)劃,其中學(xué)習(xí)因子c1,c2取2,慣性權(quán)重w在區(qū)間[0.4,0.9]中線性遞減，規(guī)劃路徑如圖2所示。

(a) 能走通

(b) 穿越障礙物圖2 基本粒子群算法路徑規(guī)劃Fig.2 Path planning of basic PSO

圖2中原點(diǎn)為機(jī)器人的起始點(diǎn),右上角三角形為目標(biāo)點(diǎn),從圖2(a)中可以明顯看出基本粒子群算法所規(guī)劃的路徑不是最優(yōu)路徑,在障礙物密集的區(qū)域不能做到有效路徑規(guī)劃,繞了很遠(yuǎn)的路程。圖2(b)規(guī)劃的路徑有嚴(yán)重的問題,其中有2個(gè)路徑點(diǎn)在障礙物所占據(jù)的位置上,還有一段路徑穿越了障礙物。通過增加迭代次數(shù)并沒有改善這個(gè)問題,經(jīng)過對(duì)大量實(shí)驗(yàn)數(shù)據(jù)分析,這種問題是由于粒子陷入了局部極值,并難以脫離局部極值,造成算法誤認(rèn)為此時(shí)已經(jīng)找到最優(yōu)解,找到了最優(yōu)路徑。

3 改進(jìn)粒子群算法的機(jī)器人路徑規(guī)劃

3.1 變異操作

為了解決粒子群算法在運(yùn)行后期,種群的多樣性很小,易陷入局部極值,并且很難脫離的問題,提出了一種變異操作方法。在種群多樣性下降到一定程度之后,使粒子進(jìn)行變異。其中種群的多樣性[21]用粒子種群中粒子距種群中心位置的距離來表示:

(7)

式中:m為粒子個(gè)數(shù);xi為粒子的位置;pc為粒子群的中心位置,D為粒子群種群的多樣性。

在每一次迭代后計(jì)算種群的多樣性,如果多樣性下降到多樣性閾值以下則對(duì)粒子進(jìn)行變異操作,為了不使粒子失去過多以往搜索經(jīng)驗(yàn),對(duì)粒子的某一維度進(jìn)行有概率的變異操作

xid=xid+δr(bmax-bmin),p>pt

(8)

式中:xid為第i個(gè)粒子第d維的位置;δ為精搜索系數(shù),用來調(diào)節(jié)變異的幅度;r為區(qū)間[-1,1]之間的隨機(jī)數(shù),正負(fù)代表了變異的方向;bmax,bmin為第i個(gè)粒子在第d維的原始邊界;p為產(chǎn)生的概率,pt為變異閾值。

變異閾值pt決定發(fā)生變異的概率,pt越小,粒子變異的可能性越大,種群的多樣性也就越大,粒子更容易脫離局部極值,但是過小,會(huì)使粒子損失過多以往搜索經(jīng)驗(yàn),不利于搜索全局極值;pt越大,粒子變異的可能性也越小,種群的多樣性也就越小,粒子可利用更多以往的搜索經(jīng)驗(yàn),但是過大,不利于脫離局部極值。

3.2 路徑點(diǎn)冗余篩選算法

通過變異操作可以解決算法早熟的問題,但是在障礙物較多的情況下,待優(yōu)化問題會(huì)變得十分復(fù)雜,各項(xiàng)參數(shù)的設(shè)置及有限次的迭代過程會(huì)導(dǎo)致算法所得到的最優(yōu)解與真實(shí)的最優(yōu)解可能還存在一定的距離,致使搜索到的路徑點(diǎn)中會(huì)存在一些冗余路徑點(diǎn)。針對(duì)這一問題,通過對(duì)路徑點(diǎn)進(jìn)行去冗余處理,去除路徑中冗余路徑點(diǎn),使路徑更優(yōu)。

將起始點(diǎn)、目標(biāo)點(diǎn)、算法搜索到的路徑點(diǎn)根據(jù)極徑由小到大依次排列開來,第一點(diǎn)和第三點(diǎn)連接(兩點(diǎn)之間直線最短),如果連線的路徑上存在障礙物,則保留第二點(diǎn),將第二點(diǎn)作為直線的起點(diǎn),間隔一個(gè)點(diǎn)再與下一點(diǎn)相連為直線,如果連線的路徑上沒有障礙物,則刪除間隔的那個(gè)路徑點(diǎn),繼續(xù)與下一點(diǎn)相連,直到直線的終點(diǎn)為目標(biāo)點(diǎn),處理結(jié)束,輸出篩選后的路徑點(diǎn)。

3.3 算法流程

本文算法的流程圖如圖3所示。在粒子群算法每一次迭代后,計(jì)算種群多樣性,判斷多樣性是否低于多樣性閾值,如果是,則加入變異操作;如果否,則繼續(xù)下一次迭代。在粒子群算法搜索完成后,在得到的路徑點(diǎn)中運(yùn)用冗余篩選算法去除冗余路徑點(diǎn)。

圖3 算法流程圖Fig.3 Algorithm flowchart

4 仿真驗(yàn)證與結(jié)果分析

為了驗(yàn)證算法的有效性,本文通過Matlab R2016a軟件,建立環(huán)境模型,運(yùn)用改進(jìn)前后PSO對(duì)移動(dòng)機(jī)器人路徑規(guī)劃進(jìn)行仿真實(shí)驗(yàn)。PSO的種群大小m為10,迭代次數(shù)為1 200次,學(xué)習(xí)因子c1,c2均為2,慣性權(quán)重w在區(qū)間[0.4,0.9]線性遞減,精搜索系數(shù)δ取0.056,pt取0.8。

4.1 改進(jìn)前后種群多樣性對(duì)比

圖4對(duì)比了變異操作前后系統(tǒng)仿真結(jié)果的收斂性變化,顯示了有無(wú)變異操作時(shí)粒子群種群多樣性隨迭代次數(shù)的變化情況。

圖4 種群多樣性隨迭代次數(shù)變化Fig.4 Population diversity varies with number of iterations

從圖4(a)可以看出,在沒有加入變異操作時(shí),迭代次數(shù)在300多次后,種群多樣性下降為0,且直到最大迭代次數(shù),種群多樣性都沒有再變化,如果此時(shí)粒子陷入局部極值,將無(wú)法脫離出來。在加入了變異操作的算法中,如圖4(b)所示，種群多樣性始終未下降到0,表明粒子還可以繼續(xù)在周圍搜索,可使粒子有效脫離局部極值。

4.2 改進(jìn)前后600次迭代時(shí)路徑對(duì)比

圖5顯示了迭代次數(shù)600次時(shí)有無(wú)變異操作時(shí)搜索到的路徑,每一條線代表一個(gè)粒子搜索到的路徑。從圖5(a)可以看出,在沒有加入變異操作時(shí),此時(shí)并未找到最優(yōu)路徑,甚至有一段路徑還穿越了障礙物,但是所有路徑已處于重合狀態(tài),所有粒子到達(dá)了同樣的位置,陷入了局部極值,并且很難脫離,以后的迭代已沒有貢獻(xiàn),迭代效率很低。但是在加入變異操作時(shí),從圖5(b)可知，此時(shí)仍有很多條不同路徑,粒子仍可以進(jìn)行搜索,繼續(xù)尋找最優(yōu)路徑,也可通過增加精搜索系數(shù),增大變異范圍,增加粒子脫離局部極值的概率。

(a) 無(wú)變異操作

(b) 有變異操作

4.3 改進(jìn)前后路徑和適應(yīng)度值對(duì)比

圖6(a)中虛線路徑為基本PSO規(guī)劃的路徑,實(shí)線路徑為改進(jìn)PSO的路徑規(guī)劃,加入變異操作后搜索到的路徑要優(yōu)于沒有加入變異操作的路徑，在障礙物密集的區(qū)域也能搜索到有效路徑。圖6(b)為圖6(a)中PSO每一步迭代后的適應(yīng)度值,虛線為沒有加入變異操作時(shí)的適應(yīng)度值,迭代次數(shù)在260次左右時(shí),適應(yīng)度值已維持恒定,不再變化,意味著算法此時(shí)搜索到的虛線全局極值位置不再發(fā)生變化,對(duì)應(yīng)圖6(a)中搜索到的虛線路徑,顯然還沒有搜索到最優(yōu)路徑,但此后的迭代不能改變路徑位置。圖6(b)中實(shí)線部分為加入變異操作后適應(yīng)度值的變化,適應(yīng)度值在后期也在增加,意味著粒子群算法始終在搜索新路徑并逐步靠近最優(yōu)路徑。改進(jìn)后的算法迭代效率明顯比改進(jìn)前要高很多。

(a) PSO路徑規(guī)劃

(b) PSO適度度值

改變障礙物的位置和個(gè)數(shù),運(yùn)用改進(jìn)前后的粒子群算法進(jìn)行路徑規(guī)劃。圖7(a)和圖7(b)中障礙物個(gè)數(shù)為8個(gè)和9個(gè),從圖7可以看出,針對(duì)不同的環(huán)境,改進(jìn)后的算法規(guī)劃的路徑比改進(jìn)前更優(yōu),說明了改進(jìn)的算法在環(huán)境變化時(shí)仍能有效搜索路徑,證明了該方法的適應(yīng)性很強(qiáng)。

(a) 8個(gè)障礙物

(b) 9個(gè)障礙物圖7 不同環(huán)境下路徑規(guī)劃Fig.7 Path planning in different environments

4.4 去冗余前后路徑對(duì)比

圖8是通過路徑點(diǎn)冗余篩選算法對(duì)搜索到的路徑點(diǎn)去冗余處理。虛線為改進(jìn)粒子群算法搜索到的路徑,該路徑中的一些路徑點(diǎn)會(huì)使機(jī)器人轉(zhuǎn)彎次數(shù)增多,也增加了一些無(wú)效的路程。通過路徑點(diǎn)冗余篩選算法后得到實(shí)線路徑,去除路徑中不必要的冗余路徑點(diǎn),路徑更平滑,減少轉(zhuǎn)彎次數(shù)的同時(shí)也減少了路徑的長(zhǎng)度。從一定程度上也提升了移動(dòng)機(jī)器人的智能化水平。

圖8 去冗余前后路徑Fig.8 Path before and after deredundancy

5 結(jié) 論

本文對(duì)移動(dòng)機(jī)器人的工作空間進(jìn)行分析,建立自主移動(dòng)機(jī)器人環(huán)境模型,根據(jù)移動(dòng)機(jī)器人移動(dòng)過程中的約束條件,構(gòu)造粒子群算法的適應(yīng)度函數(shù),通過加入變異操作解決粒子群算法早熟的問題,并用路徑點(diǎn)冗余篩選算法去除冗余路徑點(diǎn)。通過仿真實(shí)驗(yàn)得出以下結(jié)論:

(1) 標(biāo)準(zhǔn)PSO在障礙物較少的情況下,即優(yōu)化問題復(fù)雜度低的情況下,表現(xiàn)良好;障礙物多的情況下,易陷入局部極值,難以脫離。

(2) 改進(jìn)PSO算法,通過變異操作,增加種群多樣性,使粒子有效脫離局部極值,在一定程度上解決算法早熟問題,提高算法收斂精度,提升算法魯棒性,算法實(shí)現(xiàn)簡(jiǎn)單,效果明顯。

(3) 算法搜索到的路徑點(diǎn)在大部分情況下存在冗余路徑點(diǎn),對(duì)路徑點(diǎn)進(jìn)行去冗余后,可增加路徑平滑度,減少路徑長(zhǎng)度,提升機(jī)器人的智能化水平。

此方法雖然可以在一定程度上解決上述問題,但尚存在以下問題，在障礙物很多的情況下,需要大量的迭代次數(shù)以保證算法的收斂精度,后續(xù)研究中可以繼續(xù)對(duì)算法進(jìn)行改進(jìn),在保證路徑最優(yōu)的條件下進(jìn)一步減少迭代次數(shù)。目前僅考慮了機(jī)器人在靜態(tài)障礙物環(huán)境下運(yùn)動(dòng)的情況,但在實(shí)際環(huán)境中不僅有靜態(tài)障礙物,還有動(dòng)態(tài)障礙物以及突發(fā)性障礙物的存在,因此可在后續(xù)研究中解決移動(dòng)機(jī)器人在混合復(fù)雜環(huán)境下的路徑規(guī)劃問題。