基于采樣隱馬爾可夫模型的移動(dòng)機(jī)器人路徑規(guī)劃

白曉蘭,張振朋,周文全,袁 錚,任鵬博,張炳貴

(沈陽(yáng)化工大學(xué)機(jī)械與動(dòng)力工程學(xué)院,沈陽(yáng) 110142)

0 引言

在移動(dòng)機(jī)器人研究領(lǐng)域,路徑規(guī)劃是大多數(shù)移動(dòng)機(jī)器人任務(wù)所應(yīng)具備的基本但至關(guān)重要的能力。在過(guò)去,人們對(duì)它進(jìn)行了廣泛的探索,并提出了各種各樣的解決方案[1],如A*算法[2-4]、人工勢(shì)場(chǎng)法[5]、神經(jīng)網(wǎng)絡(luò)算法[6]、遺傳算法[7-9]、粒子群優(yōu)化算法[10]等。這些方法在移動(dòng)機(jī)器人路徑規(guī)劃中取得較好的效果,但面對(duì)復(fù)雜環(huán)境時(shí)依然存在一定的缺陷。

隱馬爾可夫模型(hidden markov model,HMM)[11]作為一種用參數(shù)表示,用于描述隨機(jī)過(guò)程統(tǒng)計(jì)特性的概率模型,以其十分健壯的數(shù)學(xué)結(jié)構(gòu)和強(qiáng)大的模式分類能力成為模式識(shí)別中的重要方法被廣大研究人員廣泛地使用和研究[12]。但是HMM在路徑規(guī)劃中的應(yīng)用目前研究較少,鄧旭等[13]針對(duì)復(fù)雜迷宮環(huán)境的目標(biāo)體具體位置與路徑規(guī)劃問(wèn)題為對(duì)象,提出了一種基于隱馬爾可夫模型的粒子濾波算法。該方法通過(guò)隱馬爾可夫模型的概率計(jì)算,推理出目標(biāo)體可能范圍,通過(guò)A*算法將目標(biāo)捕獲。仿真結(jié)果表明,與普通的概率計(jì)算目標(biāo)體位置,粒子濾波算法計(jì)算的目標(biāo)體位置,大大減少了總步數(shù),形成了最短路徑。王社偉等[14]用馬爾可夫模型對(duì)無(wú)人機(jī)路徑規(guī)劃問(wèn)題建模,提出了離散的馬爾可夫模型線性化的方法,并利用數(shù)學(xué)歸納法進(jìn)行了證明。仿真結(jié)果表明,對(duì)狀態(tài)流程圖進(jìn)行線性化以后,由初始狀態(tài)到終止?fàn)顟B(tài)的每條路徑的狀態(tài)轉(zhuǎn)移概率變得更加直觀,能夠從狀態(tài)流程圖中清楚地得到各個(gè)因素對(duì)整個(gè)系統(tǒng)任務(wù)的影響。岳師光等[15]針對(duì)標(biāo)準(zhǔn)模型在對(duì)手更改終點(diǎn)目標(biāo)和自上而下規(guī)劃時(shí)無(wú)法識(shí)別的問(wèn)題,提出了一種頂層策略可變的抽象隱馬爾可夫模型。為模型的頂層策略增加初始分布和策略終止變量,更改了策略終止變量間的依賴關(guān)系,使下層策略能被強(qiáng)制終止。給出了改進(jìn)后DBN結(jié)構(gòu),并通過(guò)推導(dǎo)條件概率更新和RB變量抽樣流程實(shí)現(xiàn)了模型的近似推理。仿真實(shí)驗(yàn)表明,改進(jìn)模型能準(zhǔn)確識(shí)別給定環(huán)境下的各類典型航跡,不僅在終點(diǎn)目標(biāo)不變時(shí)能較好地維持標(biāo)準(zhǔn)模型的識(shí)別準(zhǔn)確率,在提供足夠的觀測(cè)數(shù)據(jù)后還能很好地解決變目標(biāo)識(shí)別問(wèn)題。

以上基于HMM的規(guī)劃算法在不同程度上提高了算法規(guī)劃的性能,但沒(méi)有充分利用HMM的預(yù)測(cè)和學(xué)習(xí)能力。為了提高規(guī)劃路徑的平滑性、安全性,使算法能夠在大量學(xué)習(xí)訓(xùn)練后能躲避行人。本文提出了一種基于SHMM的移動(dòng)機(jī)器人路徑規(guī)劃方法,該方法基于SHMM建立移動(dòng)機(jī)器人運(yùn)動(dòng)模型;用K-means聚類對(duì)不同復(fù)雜程度的環(huán)境分類進(jìn)行初始信息素的分配,以避免蟻群算法前期收斂慢,后期陷入停滯的問(wèn)題;融合SHMM因子、K-means因子和轉(zhuǎn)角因子改進(jìn)蟻群算法,實(shí)驗(yàn)仿真結(jié)果驗(yàn)證了所提方法的可行性。

1 基于采樣隱馬爾可夫運(yùn)動(dòng)模型的建立

1.1 隱馬爾可夫模型

隱馬爾可夫模型是一種將系統(tǒng)表示為有向圖形模型的統(tǒng)計(jì)模型[11]。HMM由N個(gè)系統(tǒng)狀態(tài)S=s1,s2,…,sN,M個(gè)觀測(cè)信號(hào)V=v1,v2,…,vM和狀態(tài)轉(zhuǎn)移矩陣A、觀測(cè)概率矩陣B、以及初始狀態(tài)概率向量π構(gòu)成。狀態(tài)轉(zhuǎn)移概率分布A=aij,如式(1)所示:

aij=P(qt+1=s(j)|qt=s(i)),(1≤i≤N,1≤j≤N)

(1)

此外,狀態(tài)j,B=bij的觀測(cè)概率公式如下:

bij=P(v(i)|s(j)),(1≤i≤M,1≤j≤N)

(2)

最后,初始狀態(tài)分布π=πi定義為:

πi=P(q1=s(j)),(1≤i≤N)

(3)

因此一個(gè)隱馬爾可夫模型通常簡(jiǎn)記為λ=(π,A,B)。

隱馬爾可夫有兩個(gè)假設(shè):

(1)觀測(cè)獨(dú)立假設(shè):在t時(shí)輸出觀測(cè)值的概率只取決于當(dāng)前時(shí)刻t所處的狀態(tài)而與以前的狀態(tài)無(wú)關(guān)。

(2)馬爾可夫性假設(shè):在t時(shí)刻的狀態(tài)向t+1時(shí)刻的狀態(tài)轉(zhuǎn)移的狀態(tài)轉(zhuǎn)移概率僅僅與t時(shí)刻的狀態(tài)有關(guān)而與以往任何時(shí)刻的狀態(tài)無(wú)關(guān)。

1.2 機(jī)器人軌跡預(yù)測(cè)模型建立

在二維(2D)環(huán)境中,軌跡可以描述為一系列具有航向θ和線速度vl的x-y位置。本文考慮將2D空間分割成規(guī)則網(wǎng)格,其中可以在每個(gè)網(wǎng)格單元中計(jì)算運(yùn)動(dòng)的發(fā)生。將網(wǎng)格擴(kuò)展到包括所有4個(gè)維度,即x、y、θ和vl,將產(chǎn)生所謂的流場(chǎng)或運(yùn)動(dòng)直方圖?？梢詫?duì)直方圖中的值進(jìn)行歸一化,以獲得概率,從而得到網(wǎng)格的聯(lián)合概率分布表示為:

P(x,y,θ,vl)

(4)

式中:聯(lián)合概率分布P表示x-y-θ和vl同時(shí)出現(xiàn)的概率,該概率模型是建立在環(huán)境中獨(dú)立于時(shí)間的一種新的運(yùn)動(dòng)模式上。

該模型的變量分布復(fù)雜,需要采集大量數(shù)據(jù)并且計(jì)算量大。本文用一種概率方法來(lái)建立式(4)的估計(jì),考慮到機(jī)器人是一個(gè)具有有限視野的移動(dòng)觀察者。建立時(shí)間t內(nèi)的信念Dt(式(4)的近似值)如式(5)所示:

Bel(Dt)=P(Dt|ξt,ζt,Zt,…,ξ0,ζ0,Z0)

(5)

式中:ξt為t時(shí)間觀察所有移動(dòng)的人,ζt為t時(shí)間機(jī)器人的姿態(tài),Zt為t時(shí)間所有傳感器的輸出。

根據(jù)上述方程,可以使用貝葉斯定理推導(dǎo)出增量更新規(guī)則如式(6)所示。

(6)

式中:η為一個(gè)常數(shù)。由于Bel(Dt)是時(shí)間t時(shí)的信念,它考慮到所有過(guò)去的觀測(cè)、傳感器讀數(shù)和觀察者姿勢(shì),數(shù)據(jù)量大需要進(jìn)一步簡(jiǎn)化。假設(shè)當(dāng)前的觀測(cè)和姿態(tài)在條件上獨(dú)立于過(guò)去的觀測(cè)和姿態(tài),即系統(tǒng)是滿足馬爾可夫性質(zhì)的。

(7)

式中:最后一項(xiàng)是t-1時(shí)刻的信念,因此最終的更新規(guī)則為式(8)所示:

Bel(Dt)=ηP(ξt|Dt,ζt,Zt)Bel(Dt-1)

(8)

綜上所述,通過(guò)對(duì)影響機(jī)器人運(yùn)動(dòng)軌跡的因素分析,本文選擇用隱馬爾可夫模型來(lái)解決機(jī)器人運(yùn)動(dòng)軌跡預(yù)測(cè)問(wèn)題。以機(jī)器人觀測(cè)到的移動(dòng)的人的軌跡作為可觀測(cè)變量,機(jī)器人軌跡作為隱狀態(tài)。

1.3 采樣隱馬爾可夫運(yùn)動(dòng)模型

雖然基于網(wǎng)格的方法是Bel(Dt)信念的一種可能表示形式,但這種方法有許多缺點(diǎn)。首先,機(jī)器人的觀察的視野大,只要置信度被更新,就需要更新整個(gè)環(huán)境網(wǎng)格,這會(huì)導(dǎo)致高計(jì)算成本,并且實(shí)現(xiàn)過(guò)程低效;此外,由于不知道運(yùn)動(dòng)環(huán)境的內(nèi)部結(jié)構(gòu),建立運(yùn)動(dòng)模式的網(wǎng)格甚至需要對(duì)不可能運(yùn)動(dòng)的區(qū)域(例如墻的內(nèi)壁)也要生成環(huán)境置信度;最后,需要保證選擇網(wǎng)格的分辨率,但即使如此,也很難保證得到良好的近似結(jié)果。

考慮到基于網(wǎng)格的方法以上缺點(diǎn),提出了一種采樣的方法來(lái)表示建立的隱馬爾可夫機(jī)器人軌跡算法。該算法可以預(yù)測(cè)、加權(quán)和采樣,從而逐步學(xué)習(xí)式(4)的近似值。基于樣本的表示方法克服了基于網(wǎng)格的方法的問(wèn)題,只在感興趣的區(qū)域生成樣本,即觀察到運(yùn)動(dòng)的區(qū)域,并且可以通過(guò)降采樣來(lái)控制樣本的數(shù)量。

式(5)中定義的置信度Bel(Dt)可以表示為一組加權(quán)樣本:

(9)

(10)

綜上所述,機(jī)器人的軌跡預(yù)測(cè)概率可以用觀測(cè)到行人的軌跡樣本來(lái)統(tǒng)計(jì)計(jì)算,其中追蹤人的軌跡使用粒子濾波器。假設(shè)某一粒子位置為X=[x,y,θ,v]T,粒子的更新規(guī)則如式(11)所示。

Xt+1=Xt+R*μt+rand

(11)

式中:rand為(0,1)隨機(jī)數(shù),狀態(tài)矩陣R和μ為觀測(cè)狀態(tài)的元素,表達(dá)式為:

(12)

R=

(13)

式中:μ(i)和R(i)為第i個(gè)樣本簇的平均值和協(xié)方差,N為狀態(tài)數(shù)。

SHMM運(yùn)動(dòng)模型在預(yù)測(cè)行人軌跡之前,需要大量行人數(shù)據(jù)進(jìn)行學(xué)習(xí)和訓(xùn)練,為了減少數(shù)據(jù)訓(xùn)練量,降低處理數(shù)據(jù)和計(jì)算數(shù)據(jù)的成本,需要對(duì)相鄰運(yùn)動(dòng)狀態(tài)進(jìn)行處理。為了在降采樣的同時(shí)保證數(shù)據(jù)的完整性,本文利用KLD散度來(lái)對(duì)新的狀態(tài)下的軌跡做近似的估計(jì)。KL散度又被稱為相對(duì)熵或者信息散度[16],是兩個(gè)概率分布間差異的非對(duì)稱性度量。在信息論中,相對(duì)熵等價(jià)于兩個(gè)概率分布的信息熵的差值,若其中一個(gè)概率分布為真實(shí)分布,另一個(gè)為理論(擬合)分布,則此時(shí)相對(duì)熵等于交叉熵與真實(shí)分布的信息熵之差,表示使用理論分布擬合真實(shí)分布時(shí)產(chǎn)生的信息損耗。如式(14)所示,s*表示新的狀態(tài)或者向運(yùn)動(dòng)模式模型中已經(jīng)存在的狀態(tài)添加新的信息。

(14)

式中:1≤i≤N,1≤j≤K,K為軌跡的路徑數(shù)。該方程的第一項(xiàng)和第二項(xiàng)計(jì)算為:

(15)

(16)

如果狀態(tài)s(i)和集群s*之間的KLD低于一定的閾值,則可以對(duì)樣本集群進(jìn)行組合,并相應(yīng)地更新?tīng)顟B(tài)統(tǒng)計(jì)數(shù)據(jù)。該閾值可以根據(jù)連續(xù)狀態(tài)之間的期望距離來(lái)計(jì)算,也可以通過(guò)實(shí)驗(yàn)選擇一個(gè)固定的閾值。為了避免不斷增長(zhǎng)的模型增加計(jì)算工作量,KLD只計(jì)算位于狀態(tài)空間中的集群。

基于上述推導(dǎo),用MATLAB軟件驗(yàn)證基于SHMM建立的機(jī)器人運(yùn)動(dòng)模型預(yù)測(cè)軌跡的有效性。對(duì)行人軌跡追蹤仿真如圖1和圖2所示,初始化參數(shù)如表1所示。圖1a假設(shè)一個(gè)人在10×10環(huán)境行走,深灰色是行人軌跡起點(diǎn),淺灰色是行人軌跡終點(diǎn),將產(chǎn)生黑線的軌跡。如圖1b所示,機(jī)器人用粒子濾波的跟蹤器產(chǎn)生一系列觀察到的軌跡姿態(tài)。

表1 粒子追蹤初始化參數(shù)

(a) 10×10環(huán)境模型下行人軌跡圖 (b) 10×10環(huán)境模型下粒子追蹤圖圖1 10×10環(huán)境模型下粒子追蹤圖

(a) 20×20環(huán)境模型下行人軌跡圖 (b) 20×20環(huán)境模型下粒子追蹤圖圖2 20×20環(huán)境模型下粒子追蹤圖

深灰色是50個(gè)粒子組成的粒子云,它們的更新規(guī)則為式(11),中間的黑點(diǎn)*為軌跡追蹤收斂點(diǎn)。粒子將原來(lái)8.66 m的行人軌跡以11個(gè)姿態(tài)點(diǎn)表示,相鄰姿態(tài)點(diǎn)的更新規(guī)則為式(14)。圖2a是20×20環(huán)境下行人從兩個(gè)不同起點(diǎn)到同一終點(diǎn)的多條行人軌跡圖,用100個(gè)粒子的點(diǎn)云去追蹤。由圖2b可知,長(zhǎng)度為16.37 mm的行人軌跡1以15個(gè)姿態(tài)點(diǎn)表示,長(zhǎng)度為14.17 mm的行人軌跡2以12個(gè)姿態(tài)點(diǎn)表示,驗(yàn)證了復(fù)雜化境下多條行人軌跡追蹤預(yù)測(cè)的有效性。

以上仿真驗(yàn)證粒子可以把這些軌跡以一組姿態(tài)樣本來(lái)表示,它近似機(jī)器人在網(wǎng)格中x、y、θ和vl的聯(lián)合概率分布,并且狀態(tài)之間的轉(zhuǎn)換是可以直接觀察到的。在環(huán)境擁擠的區(qū)域粒子聚集更加密集,相鄰姿態(tài)樣本的KLD散度更小。

2 改進(jìn)蟻群算法

2.1 基于K-means聚類的初始化信息素

蟻群算法的工作原理Ant System(螞蟻系統(tǒng))是一種智能仿生優(yōu)化算法[17]。在尋路的過(guò)程中螞蟻會(huì)分泌信息素,信息素分泌的多少與路徑長(zhǎng)度成反比,距離越小則當(dāng)前點(diǎn)到該可行點(diǎn)被分配的信息素就越多,而螞蟻在選擇路徑時(shí)會(huì)根據(jù)狀態(tài)轉(zhuǎn)移概率以較大概率選擇信息素大的路徑,信息素也會(huì)隨之更新。隨著大量螞蟻個(gè)體不斷搜索,最優(yōu)路徑上的信息素越來(lái)越多,最終整個(gè)蟻群會(huì)找出一條最優(yōu)路徑。

基本蟻群算法將地圖所有可行柵格的初始信息素設(shè)為一個(gè)常數(shù),螞蟻在迭代初期主要依靠啟發(fā)函數(shù),不僅搜索盲目,還容易陷入地圖死點(diǎn)。為了解決這個(gè)問(wèn)題,在建立柵格地圖后,根據(jù)地圖復(fù)雜度對(duì)地圖環(huán)境進(jìn)行K-means聚類,按聚類結(jié)果分配初始信息素。K-means聚類是一種無(wú)監(jiān)督學(xué)習(xí)的方法[18],是許多領(lǐng)域中常用的統(tǒng)計(jì)數(shù)據(jù)分析技術(shù)。它的基本思想是通過(guò)迭代尋找k個(gè)簇(cluster),使得聚類結(jié)果對(duì)應(yīng)的損失函數(shù)最小。本文損失函數(shù)定義為各個(gè)可行點(diǎn)到起點(diǎn)與終點(diǎn)連接線之間距離差平方和:

(17)

圖3是4種不同障礙物占比K-means聚類結(jié)果,先對(duì)柵格地圖統(tǒng)計(jì)其障礙物占比,然后利用式(17)進(jìn)行聚類。圖3a是障礙物比例0.1時(shí)地圖聚類結(jié)果折線圖,類別數(shù)為3時(shí)損失函數(shù)最小為0.239 07;圖3b是障礙物比例0.3時(shí)地圖聚類結(jié)果折線圖,類別數(shù)為4時(shí)損失函數(shù)最小為0.087 359;圖3c是障礙物比例0.5時(shí)地圖聚類結(jié)果折線圖,類別數(shù)為6時(shí)損失函數(shù)最小為0.222 58;圖3d是障礙物比例0.8時(shí)地圖聚類結(jié)果折線圖,類別數(shù)為9時(shí)損失函數(shù)最小為0.048 33。

(a) 障礙物比例0.1聚類結(jié)果分析 (b) 障礙物比例0.3聚類結(jié)果分析

(c) 障礙物比例0.5聚類結(jié)果分析 (d) 障礙物比例0.8聚類結(jié)果分析圖3 4種障礙物比例下聚類結(jié)果

根據(jù)式(17)的損失函數(shù)設(shè)計(jì)初始化信息素函數(shù)的如式(18)所示,改進(jìn)后的信息素在距離起點(diǎn)和目標(biāo)點(diǎn)連線歐氏距離遠(yuǎn)的區(qū)域含量低,在距離近的區(qū)域含量高,可以兼顧廣義搜索和局部快速收斂。

(18)

式中:τ(i,j)為節(jié)點(diǎn)i到j(luò)的初始信息素,C為信息素常量,J為損失函數(shù),G為地圖大小。

2.2 融合SHMM因子的狀態(tài)轉(zhuǎn)移函數(shù)

傳統(tǒng)蟻群算法狀態(tài)轉(zhuǎn)移函數(shù)由信息素函數(shù)和啟發(fā)函數(shù)決定的,這兩個(gè)函數(shù)主要受局部或全局路徑長(zhǎng)度控制,所以距離信息對(duì)狀態(tài)轉(zhuǎn)移概率的影響很大。當(dāng)遇到U形環(huán)境時(shí)可能會(huì)出現(xiàn)死鎖,在尋路時(shí)也容易出現(xiàn)自鎖和尋得路徑為次優(yōu)路徑的問(wèn)題。為解決這些問(wèn)題,另外增加軌跡預(yù)測(cè)的功能。本文對(duì)狀態(tài)轉(zhuǎn)移函數(shù)[17]融合第2.1節(jié)聚類類別信息和SHMM預(yù)測(cè)軌跡概率信念,改進(jìn)如下:

(19)

傳統(tǒng)信息素函數(shù)在環(huán)境復(fù)雜時(shí),初期不能快速收斂。在多次迭代后,由于路徑上信息素布趨于平穩(wěn),造成螞蟻的路徑選擇趨于單一,容易造成蟻群陷入局部最優(yōu),阻礙蟻群進(jìn)一步搜索到潛在的最優(yōu)路徑。為了解決蟻群早期不能快速收斂,后期容易陷入停滯現(xiàn)象的問(wèn)題,加入柵格環(huán)境聚類的因子,如式(20)所示。當(dāng)k=1時(shí),初始信息素分布按式(18);當(dāng)k>1時(shí),若i到j(luò)的損失函數(shù)增加,信息素比重增加,反之減少;當(dāng)i到j(luò)為同一類柵格,不會(huì)按照聚類信息更新。

(20)

為了快速搜索到最優(yōu)路徑,使得機(jī)器人轉(zhuǎn)角損耗最小并且能夠預(yù)測(cè)軌跡,改進(jìn)后的啟發(fā)函數(shù)由距離因子、轉(zhuǎn)角因子和SHMM預(yù)測(cè)因子組成。

(21)

式中:d(i,j,G)為距離因子,θ(i,j,G)為角度因子,D(i,j)為SHMM軌跡預(yù)測(cè),β1、β2、β3為權(quán)重。

為了更快地獲得最短路徑,距離因子增加聚類損失函數(shù)和待選節(jié)點(diǎn)j與目標(biāo)終點(diǎn)G之間的歐式距離,設(shè)計(jì)函數(shù)如下:

(22)

式中:Ji為i節(jié)點(diǎn)的損失函數(shù),Jj為j節(jié)點(diǎn)的損失函數(shù),d(i,j)為當(dāng)前點(diǎn)i和可行點(diǎn)之間的歐氏距離,d(i,G)為可行點(diǎn)j和目標(biāo)點(diǎn)G之間的歐氏距離。

為了降低機(jī)器人運(yùn)行時(shí)的轉(zhuǎn)彎損耗,角度因子設(shè)計(jì)如下:

(23)

由第1節(jié)理論和仿真可知,SHMM能很好地預(yù)測(cè)軌跡,本文加入節(jié)點(diǎn)i和j的D信念D(i,j)來(lái)增加路徑規(guī)劃時(shí)的軌跡預(yù)測(cè)功能,規(guī)劃路徑在遇到行人時(shí)能實(shí)時(shí)進(jìn)行處理,路徑規(guī)劃更加合理、安全,D(i,j)的更新公式為:

(24)

3 路徑規(guī)劃仿真驗(yàn)證

本文假設(shè)已經(jīng)利用SHMM機(jī)器人運(yùn)動(dòng)模型進(jìn)行了大量的各種環(huán)境行人軌跡追蹤的訓(xùn)練。為驗(yàn)證本文基于SHMM運(yùn)動(dòng)模型的蟻群算法(sampled hidden markov model-ant colony optimization,SHMM-ACO)的可行性和有效性,針對(duì)不同的環(huán)境進(jìn)行了大量仿真實(shí)驗(yàn),并與傳統(tǒng)蟻群算法(ant colony optimization,ACO)、其他改進(jìn)蟻群算法[19]得到的最優(yōu)路徑進(jìn)行比較。其中SHMM-ACO的流程圖如圖4所示,參數(shù)初始化設(shè)置如表2所示。仿真環(huán)境為Windows11 64 bit,處理器Intel(R) Core(TM) i7-6300HQ,CPU頻率2.30 GHz,內(nèi)存16 GB; MATLAB R2016b。

表2 改進(jìn)SHMM蟻群算法的參數(shù)設(shè)置

圖4 SHMM蟻群算法的具體實(shí)現(xiàn)流程

3.1 10×10簡(jiǎn)單環(huán)境仿真對(duì)比和分析

利用K-means對(duì)10×10柵格環(huán)境進(jìn)行聚類預(yù)處理,圖5a為初始柵格,設(shè)起始點(diǎn)S為序號(hào)為1的可行柵格,標(biāo)記為紅色;目標(biāo)點(diǎn)G為序號(hào)100的可行柵格,標(biāo)記為綠色,障礙物柵格標(biāo)記為黑色參數(shù),可行柵格為白色。障礙物占比27%。由3.1可知,將可行柵格分為4類利用式(17)進(jìn)行類聚類,圖5b是聚類的柵格圖,圖中按到起點(diǎn)和目標(biāo)點(diǎn)連線的歐式距離從小到達(dá)的類別分別是4,1,2,3。類別4占比6%,類別1占比37%,類別2占比為占比19%,類別3占比為占比9%。然后對(duì)3個(gè)類別用式(18)進(jìn)行初始化信息素。圖5c為聚類的輪廓圖,可以對(duì)聚類效果進(jìn)行度量,橫坐標(biāo)為輪廓量(Silhouette ),縱坐標(biāo)為類別(Cluster)。由圖可知,類別4的Silhouette最接近1表明類別更傾向于屬于當(dāng)前類,數(shù)據(jù)比較集中穩(wěn)定;類別1的Silhouette最接近0表明類別更傾向于其他某一類,數(shù)據(jù)比較離散。

(a) 10×10初始柵格環(huán)境 (b) 10×10柵格環(huán)境K-mean聚類 (c) 10×10柵格環(huán)境輪廓圖圖5 SHMM蟻群算法10×10柵格環(huán)境聚類

圖6為10×10柵格環(huán)境最優(yōu)路徑對(duì)比,粒子追蹤參數(shù)為表1數(shù)據(jù),其他參數(shù)條件都相同的情況下得到的結(jié)果。由圖6a可知,在相同的迭代次數(shù)下ACO最優(yōu)路徑長(zhǎng)度為15.414,轉(zhuǎn)彎次數(shù)6次;其他改進(jìn)ACO的最優(yōu)路徑長(zhǎng)度14.312,轉(zhuǎn)彎7次;SHMM-ACO最優(yōu)路徑長(zhǎng)度12.484,轉(zhuǎn)彎次數(shù)2次。分析可知ACO和其他改進(jìn)ACO規(guī)劃的最優(yōu)路徑長(zhǎng)度長(zhǎng)、轉(zhuǎn)彎多,不利于移動(dòng)機(jī)器人的移動(dòng)。而本文改進(jìn)的SHMM-ACO比ACO和其他改進(jìn)ACO規(guī)劃的最優(yōu)路徑短19.33%、12.77%,轉(zhuǎn)彎次數(shù)少66.66%、71.42%,能大大減少機(jī)器人移動(dòng)和轉(zhuǎn)彎的能耗。

(a) 10×10柵格環(huán)境最優(yōu)路徑對(duì)比 (b) 10×10柵格環(huán)境目標(biāo)點(diǎn)更換路徑對(duì)比圖6 10×10柵格環(huán)境最優(yōu)路徑對(duì)比

為了驗(yàn)證SHMM-ACO的學(xué)習(xí)能力,圖6b為臨時(shí)更換目標(biāo)點(diǎn)3種算法最優(yōu)路徑對(duì)比。ACO和其中其他改進(jìn)ACO由于沒(méi)有學(xué)習(xí)能力,會(huì)出現(xiàn)局部路徑冗余現(xiàn)象,路徑冗余量分別為131.48%、27.34%;SHMM-ACO具有學(xué)習(xí)能力,對(duì)于臨時(shí)更換目標(biāo)點(diǎn)問(wèn)題適應(yīng)更好,未出現(xiàn)路徑冗余現(xiàn)象。另外ACO、其他改進(jìn)ACO和SHMM-ACO平均50次算法運(yùn)行時(shí)間為0.294 s、0.243 s、0.208 s,SHMM-ACO算法速度更快,驗(yàn)證算法的時(shí)間高效性。

圖7為10×10柵格環(huán)境在迭代次數(shù)為50次時(shí)50只螞蟻3種算法最短路徑和平均路徑收斂曲線,由圖7a可知,ACO最短路徑在迭代10次后開(kāi)始收斂,收斂時(shí)最短路徑為18;其他改進(jìn)ACO和SHMM-ACO雖然都是在迭代1次后就開(kāi)始收斂,但收斂時(shí)其他改進(jìn)ACO最短路徑為14,SHMM-ACO最短路徑為12;SHMM-ACO規(guī)劃的收斂路徑長(zhǎng)度更短。由圖7b可知,ACO平均路徑在迭代30次后才開(kāi)始有收斂趨勢(shì),30次以后還有波動(dòng);其他改進(jìn)ACO在迭代6次后開(kāi)始收斂趨勢(shì),但收斂后還是個(gè)別異常值,算法不夠穩(wěn)定;SHMM-ACO在迭代3次后開(kāi)始收斂,3次以后收斂曲線平穩(wěn),且收斂后的平均路徑長(zhǎng)度比ACO、其他改進(jìn)ACO短40.33%、20.22%。驗(yàn)證了簡(jiǎn)單環(huán)境下SHMM-ACO在規(guī)劃最短路徑的有效性。

(a) 各算法最小路徑長(zhǎng)度對(duì)比 (b) 各算法平均路徑長(zhǎng)度對(duì)比圖7 最短路徑和平均路徑對(duì)比

3.2 20×20復(fù)雜環(huán)境仿真對(duì)比和分析

圖8為20×20柵格帶U型障礙復(fù)雜環(huán)境最優(yōu)路徑對(duì)比,K-means將可行柵格分為6類。由圖8可知在相同的迭代次數(shù)下ACO最優(yōu)路徑長(zhǎng)度為38,轉(zhuǎn)彎次數(shù)12次;其他改進(jìn)ACO的最優(yōu)路徑長(zhǎng)度32.968,轉(zhuǎn)彎15次;SHMM-ACO最優(yōu)路優(yōu)路徑長(zhǎng)度30.312,轉(zhuǎn)彎次數(shù)6次。由式(20)和圖8仿真可知,ACO更傾向選擇水平和垂直方向的節(jié)點(diǎn),容易選擇局部最優(yōu)路徑。其他改進(jìn)ACO只考慮路徑最短,未考慮轉(zhuǎn)彎次數(shù),比ACO雖然路徑長(zhǎng)度短了13.24%,但轉(zhuǎn)彎次數(shù)卻多25%。而本文改進(jìn)的SHMM-ACO比ACO和其他改進(jìn)ACO規(guī)劃的最優(yōu)路徑短20.17%、8.05%,轉(zhuǎn)彎次數(shù)少50.00%、60%,在考慮路徑最短的同時(shí)兼顧轉(zhuǎn)彎次數(shù),規(guī)劃的全局路徑更加平滑,尋路的效果更優(yōu)。與第3.1節(jié)簡(jiǎn)單環(huán)境對(duì)比,HMM-ACO各項(xiàng)對(duì)比百分比都在提高,證明了越是復(fù)雜的環(huán)境,本文HMM-ACO規(guī)劃路徑長(zhǎng)度越短,轉(zhuǎn)角越小,路徑越平滑。

圖8 20×20柵格帶U型障礙復(fù)雜環(huán)境最優(yōu)路徑對(duì)比 圖9 20×20柵格帶U型障礙復(fù)雜環(huán)境躲避行人對(duì)比

為了驗(yàn)證SHMM-ACO對(duì)行人軌跡的預(yù)測(cè)能力,在20×20柵格帶U型障礙復(fù)雜環(huán)境進(jìn)行了躲避行人的仿真。如圖9所示,淺灰色實(shí)線是ACO規(guī)劃的最優(yōu)路徑,在22節(jié)點(diǎn)出突然出現(xiàn)行人,規(guī)劃的路徑會(huì)經(jīng)過(guò)22節(jié)點(diǎn),可能會(huì)使得機(jī)器人與行人發(fā)生碰撞,造成不安全問(wèn)題。黑色虛線是其他改進(jìn)ACO規(guī)劃的最優(yōu)路徑,當(dāng)移動(dòng)機(jī)器人行進(jìn)到11和14節(jié)點(diǎn)時(shí)突然出現(xiàn)行人,由于轉(zhuǎn)移概率中沒(méi)有行人軌跡預(yù)測(cè)的信息,機(jī)器人沒(méi)法緊急躲避行人,可能會(huì)與行人發(fā)生碰撞,因此規(guī)劃路徑對(duì)應(yīng)動(dòng)態(tài)行人也是不安全路徑?；疑?為基于SHMM軌跡預(yù)測(cè)的收斂路徑點(diǎn),在移動(dòng)機(jī)器人行進(jìn)13節(jié)點(diǎn)時(shí)突然出現(xiàn)行人,SHMM-ACO預(yù)測(cè)到行人軌跡,能夠?qū)崟r(shí)躲避行人,驗(yàn)證了SHMM-ACO具有躲避行人的能力,規(guī)劃的路徑更加安全。

圖10為20×20柵格帶U型障礙復(fù)雜環(huán)境中各算法最短路徑和平均路徑收斂曲線對(duì)比。由圖可知,ACO在復(fù)雜環(huán)境下出現(xiàn)螞蟻死鎖現(xiàn)象,即陷入U(xiǎn)型障礙無(wú)法出來(lái),這大大影響螞蟻種群的質(zhì)量和路徑搜索的效率。而其他改進(jìn)ACO和SHMM-ACO采用了回退策略,即當(dāng)螞蟻無(wú)路徑搜索時(shí)釋放禁忌表,回退一步。未出現(xiàn)螞蟻死鎖的現(xiàn)象,但SHMM-ACO最短路徑收斂速度和平均路徑收斂速度比其他改進(jìn)ACO快50%、55%。驗(yàn)證了SHMM-ACO在帶U型障礙復(fù)雜環(huán)境更具優(yōu)勢(shì),能夠減少螞蟻死鎖現(xiàn)象,搜索效率更高,算法時(shí)間更短。

(a) 各算法最短路徑收斂對(duì)比 (b) 各算法平均路徑收斂對(duì)比圖10 最短路徑收斂曲線和平均路徑收斂曲線

圖11為20×20柵格帶U型障礙復(fù)雜環(huán)境中存活螞蟻數(shù)量和路徑長(zhǎng)度方差對(duì)比,其中ACO的螞蟻在前10次迭代都幾乎全部死鎖,死亡螞蟻數(shù)量較多。

(a) 各算法存活螞蟻數(shù)量對(duì)比 (b) 各算法路徑長(zhǎng)度方差對(duì)比圖11 存活螞蟻數(shù)量和路徑長(zhǎng)度方差

雖然其他改進(jìn)ACO算法前幾代螞蟻未全部陷入死鎖,但是蟻群的質(zhì)量不高,在第20次迭代種群質(zhì)量才有所提高。而SHMM-ACO有K-means聚類因子和SHMM因子種群之間可能相互學(xué)習(xí),在第10代蟻群整體質(zhì)量達(dá)到最優(yōu)。

圖12是20×20柵格帶U型障礙復(fù)雜環(huán)境中各算法轉(zhuǎn)彎次數(shù)三維對(duì)比圖,x坐標(biāo)是螞蟻編號(hào),y坐標(biāo)是迭代次數(shù),z坐標(biāo)是轉(zhuǎn)彎次數(shù)。由12a可知,ACO轉(zhuǎn)彎次數(shù)沒(méi)有收斂,轉(zhuǎn)彎次數(shù)在20～80大范圍跳動(dòng);其他改進(jìn)ACO雖然有收斂跡象,但是在迭代30次以后仍然存在波動(dòng);SHMM-ACO在迭代8次以后,整個(gè)轉(zhuǎn)彎次數(shù)穩(wěn)定收斂在12次,一直平穩(wěn)。驗(yàn)證了復(fù)雜環(huán)境下SHMM-ACO在復(fù)雜環(huán)境下的轉(zhuǎn)彎穩(wěn)定性,規(guī)劃的路徑更有利于機(jī)器人平穩(wěn)轉(zhuǎn)彎。

(a) ACO轉(zhuǎn)彎次數(shù) (b) 其他改進(jìn)ACO轉(zhuǎn)彎次數(shù) (c) SHMM-ACO轉(zhuǎn)彎次數(shù)圖12 轉(zhuǎn)彎次數(shù)對(duì)比

4 結(jié)論

本文提出了一種基于采樣隱馬爾可夫模型的移動(dòng)機(jī)器人規(guī)劃:

(1)基于SHMM建立移動(dòng)機(jī)器人運(yùn)動(dòng)模型,以提高移動(dòng)機(jī)器人路徑規(guī)劃的學(xué)習(xí)和預(yù)測(cè)能力。

(2)用K-means聚類對(duì)不同復(fù)雜程度的環(huán)境分類進(jìn)行初始信息素的分配,以避免蟻群算法前期收斂慢,后期陷入停滯的問(wèn)題。

(3)融合SHMM因子、K-means因子和轉(zhuǎn)角因子改進(jìn)蟻群算法,改進(jìn)后的蟻群算法規(guī)劃路徑長(zhǎng)度更短,轉(zhuǎn)角更少,能夠躲避行人,更加平滑安全。

最后經(jīng)大量的仿真實(shí)驗(yàn),驗(yàn)證了本文提出算法的有效性和可行性,為移動(dòng)機(jī)器人動(dòng)態(tài)避障、規(guī)劃出更安全、平滑的路徑,給出了自己的解決方案。