基于雙層蟻群算法和軌跡優(yōu)化的移動機器人導(dǎo)航算法

郝 延 春

(吉林大學(xué)電子科學(xué)與工程學(xué)院 吉林 長春 130012) (吉林鐵道職業(yè)技術(shù)學(xué)院質(zhì)量監(jiān)控處 吉林 吉林132200)

0 引 言

全局路徑規(guī)劃是機器人技術(shù)中的一個重要問題，其主要滿足在如何避免與障礙物碰撞并滿足約束條件下，找到從起始位置到目標(biāo)位置的最優(yōu)路徑，目前全局路徑規(guī)劃已經(jīng)被廣泛應(yīng)用于自動駕駛、民用搜索、緊急救援和資源開發(fā)等領(lǐng)域[1]。

傳統(tǒng)的全局路徑規(guī)劃方法包括人工勢場法、細(xì)胞分解法以及計算智能算法，其中計算智能算法又包括神經(jīng)網(wǎng)絡(luò)、模糊邏輯技術(shù)、遺傳算法(GA)、粒子群優(yōu)化(PSO)和蟻群優(yōu)化(ACO)算法等[2]。對于路徑規(guī)劃算法而言不僅需要其擁有良好的穩(wěn)定性，而且要具有較強的全局搜索能力[3]。然而，在一些計算智能算法中如PSO和GA，其初始值均隨機獲得，而這種隨機性在很大程度上影響最終的路徑優(yōu)化結(jié)果。

ACO算法是一種高效、穩(wěn)健的優(yōu)化技術(shù)，在不同的領(lǐng)域得到了廣泛的應(yīng)用[4]。近年來，國內(nèi)外眾多專家學(xué)者致力于利用蟻群算法解決全局路徑規(guī)劃問題。Duan等[5]提出了一種基于蟻群算法和差分進化的無人機三維路徑規(guī)劃方法，并引入模糊成本函數(shù)實現(xiàn)動態(tài)環(huán)境下無人機路徑規(guī)劃。韓顏等[6]提出一種粒子群算法和蟻群算法改進求解路徑規(guī)劃問題的融合算法，有效提升了移動機器人在全局靜態(tài)環(huán)境下搜尋到達指定目標(biāo)點的最優(yōu)路徑的能力。李理等[7]針對傳統(tǒng)蟻群算法易陷入局部最優(yōu)提出基于路徑長度、轉(zhuǎn)彎次數(shù)和坡度平滑性三種因素共同影響的改進啟發(fā)函數(shù)，并根據(jù)三因素綜合指標(biāo)分配各路徑上的信息素量，有效實現(xiàn)全局路徑規(guī)劃。Chen等[8]介紹了一種兩階段蟻群算法，并將啟發(fā)式搜索引入到預(yù)處理階段和路徑規(guī)劃階段，以避免算法陷入局部最小值。Abdulkader等[9]提出了一種混合的ACO算法，該算法將局部搜索與現(xiàn)有的ACO算法相結(jié)合，從而在出現(xiàn)較大問題時減少了計算時間，提高了性能。

雖然蟻群算法具有良好的搜索特性，但存在收斂速度慢、收斂過早等缺點。此外，螞蟻在單次運動中所能行走的最大距離是有限的，這會導(dǎo)致路徑的扭曲和轉(zhuǎn)彎，并影響路徑的長度和平滑度[10]。鑒于此，本文將路徑規(guī)劃問題分為路徑生成和軌跡優(yōu)化兩部分，提出了一種雙層蟻群優(yōu)化算法(DL-ACO)。首先，提出了并行蟻群優(yōu)化算法用于產(chǎn)生初始無碰撞路徑。其次，根據(jù)初始路徑曲折程度，提出了轉(zhuǎn)折點優(yōu)化算法(TPOA)，以對初始路徑進行進一步的改進。最后針對拐角處路徑設(shè)計了分段B樣條平滑器，使路徑中不連續(xù)部分更為平滑，從而有效提高路徑優(yōu)化質(zhì)量。

1 環(huán)境模型構(gòu)建

本文構(gòu)建了基于網(wǎng)格的二維(2-D)場作為移動機器人運行區(qū)域。如圖1所示，其中分布了多個靜態(tài)障礙物，因此網(wǎng)格被劃分為自由空間網(wǎng)格Tf和障礙網(wǎng)格To，而障礙物四周的網(wǎng)格被定義為高風(fēng)險網(wǎng)格Th。移動機器人被視為質(zhì)點R，并以固定速度移動，出于安全考慮機器人應(yīng)與障礙物保持安全距離，但若通過狹窄通道時可進入高風(fēng)險網(wǎng)格Th區(qū)域。

圖1 網(wǎng)格二維場示意圖

基于網(wǎng)格的路徑規(guī)劃問題定義如下：

給定一個起始網(wǎng)格S和一個目標(biāo)網(wǎng)格E，找到一組網(wǎng)格π(t)：[0，t]→Tf，使得π(0)=S且π(t)=E，路徑用π(t)中的網(wǎng)格表示，這些網(wǎng)格從S到E依次連接，并且連接的網(wǎng)格不能與任何To碰撞。為了獲取最佳路徑，需要從路徑長度、平滑度和安全性等方面對路徑進行優(yōu)化，因此本文的全局路徑規(guī)劃問題實質(zhì)上為多目標(biāo)優(yōu)化問題。

2 算法設(shè)計

2.1 基本蟻群算法

蟻群算法模擬了螞蟻“探索”的行為特征，即螞蟻將使用前螞蟻在覓食過程中留下的信息素，根據(jù)一定的概率選擇路徑或探索新的路徑并同時分泌信息素。假設(shè)螞蟻k在t時刻位于節(jié)點i，則路徑轉(zhuǎn)換的概率可由式(1)表示，然后利用輪盤賭模型選擇下一個節(jié)點。

(1)

式中：C為螞蟻允許在下一步選擇的節(jié)點集；τij為路徑(i，j)的信息素值；α為信息素激勵因子，它反映了信息素濃度對路徑選擇的重要性，α越大則信息素對于路徑選擇越重要，螞蟻將更傾向于選擇大多數(shù)螞蟻采取的路徑；β是預(yù)期的啟發(fā)因子，它反映了啟發(fā)式信息在路徑上的重要性。ηij(t)為啟發(fā)式值，表示螞蟻從節(jié)點j轉(zhuǎn)移到目標(biāo)節(jié)點g的期望程度，其計算式為：

(2)

式中：djg是節(jié)點j和目標(biāo)節(jié)點g之間的歐幾里得距離。每個螞蟻在使用路徑上的信息素的同時也會不斷分泌信息素，因此路徑上的信息素將不斷積累。為了防止過多的信息素導(dǎo)致啟發(fā)信息泛濫，在每個螞蟻采取一步或完成一個周期之后更新路徑上的信息素，并且從t+1時刻獲得路徑上的信息素，其計算式為：

τij(t+1)=(1-ρ)τij(t)+ρΔτij(t)

(3)

式中：ρ為信息素?fù)]發(fā)系數(shù)，主要模擬螞蟻信息素隨時間的自然揮發(fā)；1-ρ為信息素殘留因子；Δτij(t)為循環(huán)路徑(i，j)上信息素的增量。當(dāng)信息素更新策略時，有不同的計算方法。螞蟻循環(huán)模型計算式表示為：

(4)

式中：Q為信息素的強度；Lk為螞蟻在此循環(huán)中所采用路徑的總長度。

(5)

2.2 并行蟻群優(yōu)化算法

為了在復(fù)雜環(huán)境中生成初始的無碰撞路徑，本文在基本蟻群基礎(chǔ)上提出了一種改進的并行蟻群優(yōu)化算法。與傳統(tǒng)的ACO不同，并行蟻群算法借助3-opt鄰域搜索算法將所需求解的任務(wù)劃分為若干個并行的子任務(wù)，并分配給多個獨立的蟻群，每個蟻群之間獨立迭代同時進行信息互換，從而對全局信息進行更新，直到算法獲得最優(yōu)解。

從算法原理上來看，3-opt鄰域搜索算法是一種局部搜索算法，該算法執(zhí)行效率高，其基本過程為：假定q為初始回路，算法隨機產(chǎn)生3個位置q1、q3和q5，則下一節(jié)點可表示為q2、q4和q6，此時對應(yīng)的三條邊可表示為{(q1,q2)，(q3,q4)，(q5,q6)}，算法在運行過程中通過將這三條邊斷開，然后嘗試尋找另外三條邊的集合，從而使形成的新的回路路徑更短，如圖2所示。

圖2 3-opt局部搜索

將3-opt應(yīng)用到ACO算法中，不僅能夠有效提高ACO算法的運行效率，同時能夠有效地避免算法進入局部最優(yōu)。改進后的ACO算法步驟如下所示：

步驟1集群環(huán)境初始化。主節(jié)點計算路徑i、j之間的歐幾里得距離，由式(3)計算i、j之間的信息素初始值，并將其廣播到從計算節(jié)點上。

步驟2將m只螞蟻隨機地分配到集群的g個計算節(jié)點上，從而形成g個并行蟻群。

步驟3對算法進行迭代。g個并行的蟻群各自進行迭代尋找最優(yōu)解，并進行局部信息素更新。

步驟4利用3-opt鄰域搜索算法對g個并行蟻群所獲得的最優(yōu)解進行二次尋優(yōu)。

步驟5每個并行蟻群在完成各自迭代之后，按照式(3)和式(4)完成各自的全局信息素更新。

步驟6判斷迭代條件是否滿足，若滿足進入步驟7，否則返回到步驟3繼續(xù)迭代。

步驟7各并行蟻群進行蟻群間信息的交互，并判斷當(dāng)前算法是否收斂，若收斂則輸出最優(yōu)結(jié)果，否則轉(zhuǎn)入步驟2。

3 軌跡優(yōu)化

3.1 轉(zhuǎn)折點優(yōu)化算法(TPOA)

圖3所示為原始無碰撞路徑和消除了不必要轉(zhuǎn)折點后的路徑，其中S→I1→A→B→I2→E是從起點到目標(biāo)的初始路徑，顯然該路徑并非最佳路徑。例如S上的螞蟻可以不經(jīng)過I1就直接到達A，而B也可以不經(jīng)過I2直接到達E。如果刪除I1和I2，則路徑S→A→B→E的長度將大幅縮短，并且步數(shù)較少。其中點I1和I2被定義為不必要的轉(zhuǎn)折點。

圖3 TPOA路徑

對于具有許多轉(zhuǎn)彎的復(fù)雜路線，消除這些不必要的轉(zhuǎn)彎點可以有效地改善路徑長度和平滑度。因此，本文提出的TPOA如下：

對于初始路徑，節(jié)點集包括起點、目標(biāo)和所有轉(zhuǎn)折點，表示為：

T={S,T1,T2，…,Tn,E}

(6)

考慮到路線的方向，只允許螞蟻在單個方向上行駛。因此當(dāng)一只螞蟻到達節(jié)點Ti時，節(jié)點的集合表示為：

T(i)={Ti+1,Ti+2,…,Tn,E}

(7)

如果節(jié)點Ta和Tb之間的直線不穿過任何障礙網(wǎng)格To，則認(rèn)為Ta上的螞蟻會直接到達Tb，即：

(8)

式中：Ta和Tb為路徑上的任意兩個節(jié)點。此時節(jié)點Ti上螞蟻的可到達節(jié)點Tr(i)的集合可定義為：

Tr(i)={T|T∈T(i)∩con(Ti,T)=1}

(9)

此時，螞蟻從節(jié)點Ta走到節(jié)點Tb的概率為：

(10)

式中：τab為信息素濃度，ηab為啟發(fā)式信息，且:

ηab=dab

(11)

式中：dab為Ta和Tb之間的距離。此時信息素更新計算式表示為：

(12)

式中：n為轉(zhuǎn)折點的數(shù)量；Lk為路徑的長度；sfk為螞蟻經(jīng)過的高風(fēng)險區(qū)域網(wǎng)格的數(shù)量；smk為路徑平滑度。

(13)

式中：

θi+1=atan[(yi+1-yi)/(xi+1-xi)]

θi=atan[(yi-yi-1)/(xi-xi-1)]

(14)

圖4所示為TPOA實現(xiàn)的偽代碼，首先輸入節(jié)點T的坐標(biāo)和路徑上的節(jié)點數(shù)n，進行參數(shù)進行初始化處理，并計算函數(shù)con(Ta,Tb)。然后依次將所有的螞蟻放入集合S中，當(dāng)螞蟻k在節(jié)點Ti上是選擇可達到的節(jié)點Tr(i)的集合。最后當(dāng)所有螞蟻均到達E時，此時對信息素進行更新，當(dāng)算法達到最大迭代次數(shù)或滿足終止條件時，輸出最佳的路徑。

圖4 TPOA偽代碼

3.2 分段B樣條路徑平滑器

B樣條曲線是折線平滑度中最常用的曲線，但是在某些情況下(例如較大的轉(zhuǎn)向角)，由B樣條曲線平滑的路徑與原始路徑不太吻合，從而增加碰撞風(fēng)險。因此本文設(shè)計了一種分段B樣條路徑平滑器，以實現(xiàn)更高的擬合度。傳統(tǒng)的B樣條曲線由n+1個控制點Bi和一個節(jié)點向量u組成，其參數(shù)方程表示為：

(15)

式中：Bi(i=0,1,…，n)為控制頂點，Nik(u)為k次規(guī)范B樣條基函數(shù)，基函數(shù)由節(jié)點u的序列決定，u：u0≤u1≤…≤un+k+1。

B樣條曲線的基函數(shù)為：

(16)

B樣條擬合曲線為:

(17)

與傳統(tǒng)B樣條不同，分段B樣條分別平滑了每個角附近的路徑。如圖5所示，Ti-1→Ti→Ti+1為原始路徑，Ti為轉(zhuǎn)折點，在Ti-1、Ti之間增加P1、P2兩點，在Ti、Ti+1之間增加P3、P4兩點，其位置可由下式計算得到：

(18)

式中：Xsafe為安全距離。在運行過程中機器人通過配置的紅外測距傳感器測量小車與障礙物的安全距離后，在每個拐角處計算P1、P2、P3和P4的位置，這樣可以使小車在平滑拐角的過程中避免與障礙物碰撞。

圖5 拐角點分段B樣條曲線處理結(jié)果

圖6顯示了通過常規(guī)B樣條和分段B樣條平滑的相同路徑。顯然，通過分段B樣條曲線平滑的路徑比通過常規(guī)B樣條曲線平滑的路徑優(yōu)化效果更好。分段B樣條的總體優(yōu)勢主要包括：

(1) 平滑后的路徑與原始路徑相切，即機器人在跟蹤路徑期間不必轉(zhuǎn)彎。

(2) 僅拐角處的兩條線會對曲線產(chǎn)生影響，可以更改任何其他路徑而無須變換平滑路徑。

(3) 當(dāng)轉(zhuǎn)向角較大時，原始路徑和平滑路徑之間的擬合度仍然很高。

圖6 拐角點曲線平滑

4 實驗與結(jié)果分析

為了驗證本文提出算法的可行性和有效性，在不同的地圖下與文獻[11]的APACA和文獻[12]的MO-FA進行對比仿真實驗，并將本文算法應(yīng)用到真實環(huán)境的機器人導(dǎo)航中實現(xiàn)路徑規(guī)劃。

4.1 仿真實驗與分析

為了驗證本文的算法在不同環(huán)境中的適應(yīng)性，選擇了8幅50 cm×50 cm地圖進行仿真，其中每幅地圖的障礙物數(shù)量、障礙物形狀以及道路寬度均不同。隨機設(shè)置起始點和終點坐標(biāo)，如表1所示。并行蟻群優(yōu)化算法的參數(shù)設(shè)置如下：n=100、m=20、α=1、β=3、ρ=0.03，TPOA的參數(shù)設(shè)置為：m=10、α=0.3、Nmax=100、β=0.8、ρ=0.1、Nmax=100、Xsafe=1。

表1 仿真環(huán)境描述

圖7所示為三種算法生成的最佳路徑，可以看出本文所提出的算法產(chǎn)生的路徑具有較少的步數(shù)和較短的路徑長度。從圖7(c)和圖7(e)可以看出APACA容易陷入“U”型陷阱，且在復(fù)雜環(huán)境中APACA所規(guī)劃的路徑質(zhì)量較差，這是因為啟發(fā)式信息使螞蟻直奔終點，并且螞蟻直到遇到障礙物才改變方向。相較而言，MO-FA可以提供更好的路徑規(guī)劃結(jié)果，但是由于粒子是隨機放置的，拐角處的路徑通常會有些曲折，這會影響路徑的質(zhì)量。相比之下DL-ACO針對拐角點規(guī)劃的路徑更為平滑，由此也可以看出分段B樣條平滑器的巨大作用。從算法穩(wěn)定性上來看，MO-FA僅可以在簡單的環(huán)境中保持穩(wěn)定(參見圖7(b)、(e)和(g))，而APACA的穩(wěn)定性很差，由此也可以看出在復(fù)雜地圖中本文提出的方法優(yōu)勢更加明顯。

(a) (b) (c) (d)

(e) (f) (g) (h)圖7 三種算法生成的最佳路徑

表2和圖8所示為本文算法與其他兩種算法特征(包括長度，平滑度和Rateg)之間的定性比較。其中，長度由式(5)計算，路徑的平滑度為路徑上所有內(nèi)部點的線段之間的角度之和，由式(13)計算。Rateg為算法穩(wěn)定性指標(biāo)，即算法全局收斂次數(shù)與算法總運行次數(shù)的比值，該指標(biāo)可反映算法的全局收斂性，實驗中針對各地圖每種算法分別運行了20次，以檢驗各算法的收斂性。

表2 三種算法仿真結(jié)果

續(xù)表2

(a) 路徑長度對比

(b) 平滑度對比

(c) 算法穩(wěn)定性對比圖8 三種算法性能比較

總之，盡管APACA可以生成可行的路徑，但其避免陷阱的能力很差，這影響了其解決方案的質(zhì)量和穩(wěn)定性。雖然MO-FA可以在一段時間內(nèi)提供類似的解決方案，但其在復(fù)雜地圖中的穩(wěn)定性也會迅速下降。而本文提出的算法可以在長度、平滑度和穩(wěn)定性方面始終如一地產(chǎn)生更好和更穩(wěn)健的路線。

4.2 真實環(huán)境下的路徑規(guī)劃

為了驗證算法的實際可行性，利用機器人模型在真實環(huán)境下進行實驗。如圖9所示，本文采用的機器人由樹莓派驅(qū)動，并使用激光雷達檢測障礙物，分別選取一個室內(nèi)和六個室外環(huán)境作為實驗區(qū)域。圖10描述了路徑生成的過程，首先從真實地圖中提取自由空間區(qū)域，然后將真實地圖建模為基于2D網(wǎng)格的地圖，并輸入到樹莓派中。生成路線后，運動指令將被傳輸?shù)綑C器人，并使其沿路線移動直至到達目標(biāo)。圖10中，(a)為原始地圖；(b)為提取可用空間；(c)為基于網(wǎng)格的地圖；(d)為生成的路徑。

圖9 樹莓派機器人

(a) (b)

(c) (d)圖10 地圖建模和路徑生成的插圖

圖11所示為室內(nèi)實驗結(jié)果，實驗選取10 m×10 m的室內(nèi)場地作為實驗場所，并將其轉(zhuǎn)換為二維網(wǎng)格模型，顯示了機器人運行畫面和相應(yīng)的軌跡。實驗中機器人成功地避開了障礙物并保持一定距離，此外機器人在整個導(dǎo)航過程中都要經(jīng)過五個高風(fēng)險網(wǎng)格，這表明本文的算法可以有效地權(quán)衡路徑長度和安全性。

圖11 室內(nèi)實驗結(jié)果

室外環(huán)境中的軌跡如圖12所示，機器人在每個地圖中運行五次，并且軌跡幾乎相同，拐角處的路徑也很順暢，由此也可以證明本文算法在實際應(yīng)用中的有效性。此外，在實驗中還發(fā)現(xiàn)算法的運行時間與網(wǎng)格圖的分辨率直接相關(guān)，例如在10 m×10 m室內(nèi)地圖下，運行時間小于0.5 s，當(dāng)將室外地圖建模為20 m×25 m時，運行時間增加到3 s。顯然分辨率越高運行時間越長，但是高分辨率又可以提高路徑規(guī)劃的準(zhǔn)確性，因此在實際應(yīng)用中，需要在運行時間和路徑質(zhì)量之間保持平衡。

圖12 室外環(huán)境中的軌跡

5 結(jié) 語

路徑規(guī)劃是當(dāng)前機器人在復(fù)雜環(huán)境下行進的關(guān)鍵技術(shù)，蟻群算法作為一種仿生算法能夠有效實現(xiàn)機器人的路徑規(guī)劃。針對基本蟻群算法在路徑規(guī)劃中存在的收斂速度慢、易陷入局部最優(yōu)的問題提出了一種雙層蟻群優(yōu)化算法，并采用轉(zhuǎn)折點優(yōu)化算法和分段B樣條實現(xiàn)軌跡優(yōu)化。實驗中將改進的蟻群算法應(yīng)用于機器人路徑規(guī)劃，通過模擬多個移動環(huán)境，并與其他算法進行比較，實驗結(jié)果表明改進的蟻群算法可以生成長度較短、更平滑和穩(wěn)定性較好的路徑。