融合改進(jìn)A*蟻群和滾動窗口法的平滑路徑規(guī)劃

殷紹偉，彭 力+，戴菲菲

1.物聯(lián)網(wǎng)技術(shù)應(yīng)用教育部工程研究中心（江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院），江蘇 無錫 214122

2.臺州市產(chǎn)品質(zhì)量安全檢測研究院，浙江 臺州 318000

路徑規(guī)劃是移動機器人領(lǐng)域的一個重要研究方向，其主要目標(biāo)就是通過一定的智能算法求得避開所有障礙物的最優(yōu)安全路徑。目前對于已知靜態(tài)障礙物環(huán)境下的全局路徑規(guī)劃研究已經(jīng)有很多，比如人工勢場法[1]、遺傳算法[2-3]、神經(jīng)網(wǎng)絡(luò)[4]等。人工勢場法相對簡單，且便于底層實時控制，但是對于稍微復(fù)雜的地圖，就會出現(xiàn)死鎖、停滯的現(xiàn)象，而且極易陷入局部最優(yōu)；遺傳算法雖然全局搜索能力強，但是計算量大，收斂慢；神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)能力強，但是地圖變化或者存在動態(tài)障礙物時，網(wǎng)絡(luò)結(jié)構(gòu)復(fù)雜且需要不斷地改變閾值。

蟻群算法（ant colony optimization，ACO）[5]是Dorigo受生物進(jìn)化啟發(fā)提出，其因為正反饋原理、天然的并行性以及較強的魯棒性等優(yōu)點，在路徑規(guī)劃[6-8]、網(wǎng)絡(luò)路由[9]等方面得到廣泛應(yīng)用。但是蟻群算法也有一定的缺點，比如收斂速度慢、易陷入局部最優(yōu)。針對蟻群算法的不足，國內(nèi)外不少學(xué)者提出過改進(jìn)，文獻(xiàn)[10]提出將人工勢場法與ACO 結(jié)合，并且對路徑進(jìn)行幾何優(yōu)化；文獻(xiàn)[11]引入動態(tài)搜索誘導(dǎo)算子控制算法的搜索方向，提高算法多樣性與收斂速度，再結(jié)合k 交換算子避免早熟。雖然文獻(xiàn)[10-11]的實驗結(jié)果表明各自改進(jìn)后算法收斂速度加快，規(guī)劃的路徑長度變短，但是未解決改進(jìn)后算法參數(shù)多而且難調(diào)的問題，并且文獻(xiàn)[10-11]都只是在靜態(tài)環(huán)境下路徑規(guī)劃，最后規(guī)劃出的路徑也并不平滑。

針對蟻群算法的不足以及動態(tài)路徑規(guī)劃難題，本文提出一種融合改進(jìn)A*蟻群算法與滾動窗口法的平滑路徑規(guī)劃方法。首先以改進(jìn)A*算法的評價函數(shù)為標(biāo)準(zhǔn)，規(guī)劃出一條長度相對較短的路徑，增強該路徑上的初始信息素濃度，優(yōu)化信息素初值，再用改進(jìn)的蟻群算法規(guī)劃出全局路徑。然后結(jié)合滾動窗口法，制定一系列的動態(tài)避障策略，在全局路徑的基礎(chǔ)上進(jìn)行實時的局部路徑規(guī)劃。最后，引入貝塞爾曲線對路徑進(jìn)行平滑處理，使移動機器人更加準(zhǔn)確地向目標(biāo)點移動。為確保改進(jìn)蟻群算法展現(xiàn)出最好的性能，提出使用遺傳算法（genetic algorithms，GA）對本文算法中重要參數(shù)進(jìn)行自主優(yōu)化選擇。

1 環(huán)境建模

在二維移動區(qū)域內(nèi)，假設(shè)移動機器人占地是半徑為r的圓形，對于各種形狀的障礙物，對其進(jìn)行膨脹處理，膨脹寬度為r。然后用柵格法對二維環(huán)境進(jìn)行建模，其中不滿一個柵格的障礙按一個柵格計算。因為建模中有對障礙物進(jìn)行膨脹處理，所以本文移動機器人可以視為質(zhì)點，并且假設(shè)機器人有勻速運動與暫停兩種狀態(tài)，且這兩種狀態(tài)可以任意切換。

利用柵格法進(jìn)行環(huán)境建模，能夠很好地描述任何障礙物，而且可用二值信息存儲障礙物的特征和位置，方便計算機存儲與計算。計算機存儲中，每個柵格的信息包括狀態(tài)和索引。其中狀態(tài)為障礙物柵格和可行柵格，而索引包括坐標(biāo)和序號兩種。如圖1所示，對柵格地圖建立直角坐標(biāo)系，以地圖的左下角作為坐標(biāo)原點，水平向右為X軸正方向，垂直向上為Y軸正方向。假設(shè)柵格邊長為λ，以λ為基本單位來劃分X軸和Y軸，則地圖的行數(shù)為Ny=ymax/λ，列數(shù)為Nx=xmax/λ，其中xmax和ymax分別為X軸和Y軸上的最大值。同時標(biāo)記原點柵格序號為1，柵格序號從左到右、從下到上依次遞增。坐標(biāo)法與序號法是同一個柵格的兩種不同表示方法，之間可以互相轉(zhuǎn)換，如i號柵格對應(yīng)的柵格中心坐標(biāo)(xi,yi)為：

Fig.1 Grid index圖1 柵格索引

其中，mod 表示取余操作，即取(i-1)/Nx的余數(shù)，ceil表示取整操作。

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

傳統(tǒng)的蟻群算法通過每代螞蟻不斷嘗試尋找目標(biāo)點，并且在路線上留下信息素。下一代螞蟻總是會優(yōu)先沿著信息素濃度高的道路追尋目標(biāo)點。蟻群算法在用于路徑規(guī)劃時，整個尋找最優(yōu)路徑的過程有以下幾點不足：第一，初始幾代螞蟻由于道路上信息素匱乏且濃度差別不大，搜索路線盲目，導(dǎo)致效率低；第二，它的正反饋機制雖有利于加快收斂，但是容易陷入局部最優(yōu)，對于稍微復(fù)雜地圖尤其是含有凹型障礙物的地圖，還有可能出現(xiàn)死鎖現(xiàn)象；第三，信息素更新方式過于籠統(tǒng)，優(yōu)秀螞蟻與劣質(zhì)螞蟻之間的差距體現(xiàn)不明顯，導(dǎo)致算法收斂速度較慢。

2.1 改進(jìn)A*算法初始化信息素

蟻群在前幾次搜索尤其是首次搜索時，路徑上的信息素都是均勻分布的，故蟻群只能進(jìn)行盲目搜索，效率極低。因此提出用改進(jìn)A*算法對信息素進(jìn)行初始化，使蟻群算法在初期的收斂速度加快。

A*算法是一種全局路徑規(guī)劃算法，它的主要優(yōu)點是速度快。其使用的評價函數(shù)為：

式中，g(n) 表示起點s到當(dāng)前柵格n的移動消耗；h(n)表示當(dāng)前柵格n到終點g的預(yù)估移動消耗（啟發(fā)函數(shù)）[12]。

A*算法是一種啟發(fā)式搜索算法，啟發(fā)函數(shù)h(n)對算法的路徑規(guī)劃性能有較大影響。假設(shè)H(n)為當(dāng)前柵格n到終點g的實際移動消耗，當(dāng)h(n)H(n)時搜索節(jié)點少，運行效率高，但一般難以找到最優(yōu)路徑。

傳統(tǒng)的A*算法的啟發(fā)函數(shù)通常采用Euclidean 距離或Manhattan 距離定義，即：

式中，nx、ny、gx、gy分別為當(dāng)前柵格點和終點。

A*算法每次搜索當(dāng)前柵格的8 個相鄰柵格，如果采用曼哈頓距離來定義啟發(fā)函數(shù)，則只允許朝著上下左右四個方向移動，而本文允許對角線方向移動，因此曼哈頓距離定義啟發(fā)函數(shù)并不合適；而歐幾里德距離則允許朝著任意方向移動，不滿足本文采用柵格法對環(huán)境建模的基本條件，因此也不適合作為啟發(fā)函數(shù)。

考慮到這兩種啟發(fā)函數(shù)各自的優(yōu)點與不足，本文設(shè)計了一種更接近實際移動消耗H(n)的啟發(fā)函數(shù)。首先考慮沿著對角線運動，朝著對角線移動一個柵格代價為（假設(shè)直線移動代價為1），則總的對角線移動代價為×min(|nx-gx|,|ny-gy|)；然后考慮沿著直線運動，得出總的直線移動代價為hM(n)-2×(min(|nx-gx|,|ny-gy|))。本文設(shè)計的啟發(fā)函數(shù)為總的對角線移動代價與總的直線移動代價的算術(shù)和，即：

式中，dx(n)=|nx-gx|，dy(n)=|ny-gy|。

本文將A*算法所求路徑上的初始信息素設(shè)置為：

其余路徑上的初始信息素保持不變，仍然為常數(shù)c。

2.2 改進(jìn)狀態(tài)轉(zhuǎn)移概率函數(shù)

經(jīng)典蟻群算法的狀態(tài)轉(zhuǎn)移概率函數(shù)主要考慮了信息素分布以及當(dāng)前點與相鄰可行柵格點之間的距離。其應(yīng)用于復(fù)雜環(huán)境時，尤其是有凹型障礙物的環(huán)境中，機器人很可能陷入如圖2 所示的死鎖狀態(tài)。

Fig.2 Robot deadlock state圖2 機器人死鎖狀態(tài)

針對死鎖問題，常見的解決辦法是允許陷入死鎖的螞蟻后退，更新禁忌表后再依據(jù)狀態(tài)轉(zhuǎn)移函數(shù)重新選擇下一個柵格[13]。如果是實時在線路徑規(guī)劃，這種方法在復(fù)雜環(huán)境下得到的路線很可能不是最優(yōu)的；即使是離線規(guī)劃，螞蟻也需要判斷是否處于死鎖狀態(tài)，如果是則需后退重新選擇，搜索效率降低，而且死鎖點的信息素濃度需要經(jīng)過多次更新后才能與其他柵格點拉開差距，從而避免下一代的螞蟻再次進(jìn)入死鎖點，顯然采用這種策略來規(guī)避死鎖點可行性不高。文獻(xiàn)[14]通過設(shè)置進(jìn)入死鎖點的螞蟻死亡，并把死亡螞蟻留下的信息素清除，可以有效避免死鎖。但是如果地圖較為復(fù)雜，每一代都會有大量螞蟻被設(shè)置為死亡，這將不利于算法得到收斂，并且會影響算法對最優(yōu)解的探索能力。

本文改進(jìn)狀態(tài)轉(zhuǎn)移函數(shù)，在其中加入“活躍度”因子，具體如下：

其中τij(t)為在時刻t柵格i到j(luò)之間路徑的信息素濃度[5]；Aj(t)為柵格j的“活躍度”；ηij(t)為啟發(fā)函數(shù)，式（10）表示傳統(tǒng)蟻群算法的啟發(fā)函數(shù)，很明顯該公式只考慮柵格i到柵格j之間的距離，因此，正方向的相鄰可行柵格比斜方向的相鄰柵格更容易被啟發(fā)函數(shù)選中。這樣選擇過于片面，故此處考慮終點柵格位置，本文將啟發(fā)函數(shù)改為式（11）。

式（8）中l(wèi)iveness表示與柵格j相鄰的柵格個數(shù)，地圖中間與之相鄰的柵格個數(shù)為8，而四周（不包括頂點）與之相鄰的柵格個數(shù)為5，頂點則為3；obj表示柵格j周邊最大連續(xù)障礙物的個數(shù)，如圖2 中的柵格41 為4，柵格75 為3。如此對于柵格41 與55，如果它們非終點，其“活躍度”經(jīng)過式（8）、式（9）計算為0，代入式（7）得死鎖點被選擇的概率為0。因此加入“活躍度”因子后，根據(jù)式（7）得出的狀態(tài)轉(zhuǎn)移概率是否為0，就可以判斷該點是否為死鎖點。一旦判斷為死鎖點，螞蟻就不會經(jīng)過該柵格，也就不用使陷入死鎖點的螞蟻后退或直接把進(jìn)入死鎖點的螞蟻設(shè)置為死亡等操作，使得該算法的效率和收斂速度得到較大提高。同時為了保證選擇的多樣性，根據(jù)計算出的狀態(tài)轉(zhuǎn)移概率值，用輪盤賭算法對下一個柵格進(jìn)行選擇。

2.3 基于不平等原則的信息素更新機制

傳統(tǒng)的蟻群算法信息素更新策略是將前幾代螞蟻留下的信息素之和按照一定的比例揮發(fā)后加上當(dāng)代螞蟻留下的信息素，具體公式如下：

式中，ρ∈[0,1]為信息素?fù)]發(fā)系數(shù)，Q是信息素強度，Lk表示第k只螞蟻本次迭代所走的路徑長度。由式（14）可知，當(dāng)代螞蟻留下的信息素只是與走過的路徑長度成簡單反比關(guān)系。當(dāng)?shù)貓D變得復(fù)雜，起點與終點相距較遠(yuǎn)時，Lk取值必將變大。同時如果信息素強度Q取值不變大，當(dāng)代優(yōu)秀螞蟻與劣質(zhì)螞蟻留下的信息素將幾乎沒什么差距，放緩了算法的收斂的速度。故此處將式（14）改為式（15），對螞蟻之間的差距進(jìn)行放大，并且直接清除掉每代最差螞蟻信息素。

算法中對于未找到終點的螞蟻，其路徑長度定義為inf，式中Lmax是指除inf以外的最大值。當(dāng)螞蟻路徑長度為Lmax時，也就是在找到終點的螞蟻中最差的，其經(jīng)過式（15）計算產(chǎn)生的信息素為0。當(dāng)Lk∈[Lmin,Lmax)時，計算式（14）與式（15）之間的差值，記為E，如式（16）。將式（16）轉(zhuǎn)化為式（17），當(dāng)時，將E對Lk求導(dǎo)記為E′，且易得E′<0，如同式（18）；同理當(dāng)時，得E′>0。由此可得，相對于傳統(tǒng)的信息素更新策略，式（15）拉大了不同螞蟻之間的差距，使優(yōu)質(zhì)螞蟻與劣質(zhì)螞蟻差距變大。路程更短的螞蟻，遺留的信息素相比傳統(tǒng)的更新方式更多；路程更長的螞蟻，遺留的信息素比傳統(tǒng)的更新方式更少。當(dāng)路程短的螞蟻可以留下更多的信息素時，經(jīng)過算法的信息素更新機制數(shù)次迭代之后，路程短的與路程長的螞蟻之間的差距迅速增大。較短路徑由于經(jīng)過的螞蟻數(shù)量較多，柵格上的信息素越來越多；而相對長的路徑由于只有較少的螞蟻會經(jīng)過，柵格上的信息素得不到顯著增加，而且由于信息素會慢慢揮發(fā)，以至于最后較長路徑上的信息素可以忽略不記。因此在數(shù)次迭代之后，最短路徑上的信息素濃度會比其他路徑上的更多，而且差距顯著，代入式（7）計算，得到的狀態(tài)轉(zhuǎn)移概率會逐漸接近于1，路徑規(guī)劃算法也得到收斂。因此，基于不平等原則的信息素更新機制使得改進(jìn)后的算法更容易收斂。

3 基于遺傳算法的參數(shù)優(yōu)化

本文改進(jìn)的A*蟻群算法需要對較多參數(shù)進(jìn)行初始化，并且目前還沒有嚴(yán)格的理論依據(jù)對蟻群算法參數(shù)進(jìn)行正定，一般都是根據(jù)具體情況進(jìn)行分析，依據(jù)實驗仿真效果與自身經(jīng)驗不斷調(diào)試所得。然而這樣做不僅費時、主觀性強，而且選擇的參數(shù)容易是局部最優(yōu)參數(shù)。本文需要初始化的參數(shù)共有7 個，其中較為重要的參數(shù)有：改進(jìn)A*算法初始化信息素放大倍數(shù)k，信息素與啟發(fā)函數(shù)的權(quán)重α和β，信息素?fù)]發(fā)系數(shù)ρ，信息素強度Q。這些參數(shù)在很大程度上會影響算法的性能，雖然根據(jù)經(jīng)驗各自都有取值范圍，但是每個參數(shù)排列組合難以調(diào)取，因此本文提出用帶精英策略的簡單遺傳算法對這5 個較為重要參數(shù)進(jìn)行自主優(yōu)化選擇。

3.1 帶精英策略的遺傳算法

選擇遺傳算法主要是因為遺傳算法尋優(yōu)能力強，參數(shù)少，且參數(shù)取值范圍小，易調(diào)節(jié)。文獻(xiàn)[15]用粒子群算法（particle swarm optimization，PSO）優(yōu)化蟻群算法中的參數(shù)α和β，但是PSO 本身要對5 個參數(shù)進(jìn)行初始化，其中慣性權(quán)值w，學(xué)習(xí)因子c1和c2對算法性能有較大影響，并且PSO 易早熟于局部最優(yōu)。

傳統(tǒng)遺傳算法主要參數(shù)只有交叉概率pc和變異概率pm，且這兩個參數(shù)的取值范圍較小，一般pc∈[0.7,1.0)，pm∈(0,0.3]。當(dāng)交叉概率過小時，種群不能進(jìn)行有效的更新，收斂結(jié)果不具有說服力；而交叉概率過大時，隨機性增大，容易破壞已有的最優(yōu)解，錯失最優(yōu)個體。變異概率過小時，種群的多樣性下降快，會導(dǎo)致收斂不穩(wěn)定；而變異概率過大時，雖然會增加種群的多樣性，但是可能會使優(yōu)秀個體尚未得到保存就被破壞，最后得到的結(jié)果并不是最優(yōu)解。但是總的來說遺傳算法參數(shù)相對于蟻群算法個數(shù)少，取值范圍小，因此更易選擇。本文遺傳算法的參數(shù)是根據(jù)自身經(jīng)驗以及經(jīng)過不斷調(diào)試所得，同時為加快遺傳算法的收斂速度，本文在經(jīng)典遺傳算法的基礎(chǔ)上添加精英策略，即從父代種群中抽取一定比例的精英不經(jīng)過遺傳算子操作直接復(fù)制到子代。

3.2 參數(shù)優(yōu)化步驟

本文遺傳算法的適應(yīng)度函數(shù)設(shè)置為路徑的長度，依據(jù)一般經(jīng)驗與算法本身的約束條件，將改進(jìn)的A*蟻群算法中5個重要參數(shù)限定為以下取值范圍：k∈(1,5),α∈(0,2),β∈(5,25),ρ∈(0,1),Q∈(5,35)。具體步驟如下：

步驟1初始化帶精英策略的遺傳算法最大迭代次數(shù)為200，交叉概率pc與變異概率pm分別為0.8 和0.1，種群大小m為200。

步驟2對改進(jìn)蟻群算法中5 個重要的參數(shù)進(jìn)行有約束的編碼，編碼方式為二進(jìn)制編碼，約束條件是各參數(shù)的取值范圍。

步驟3計算適應(yīng)值，依據(jù)適應(yīng)值進(jìn)行排序，將前5%的精英不經(jīng)過交叉變異直接復(fù)制給下一代。

步驟4父代進(jìn)行交叉與變異操作，將產(chǎn)生的子代與父代合并，用二元錦標(biāo)賽法從中選出種群大小m的95%傳入下一代。

步驟5判斷遺傳算法是否達(dá)到最大迭代次數(shù)或者收斂。若是，則選取出最優(yōu)解作為本文改進(jìn)A*蟻群算法的參數(shù)，否則轉(zhuǎn)到步驟3。

4 基于滾動窗口法的動態(tài)防碰策略

設(shè)滾動窗口刷新時間為T，窗口的大?。礄C器人的探測范圍）為以機器人為中心半徑為R的圓形區(qū)域，機器人在時間T內(nèi)的最大移動距離為Sr，動態(tài)障礙物在相同時間內(nèi)的移動距離為So[16]。根據(jù)滾動窗口的定義，想要滾動窗口有效，需滿足So≤Sr的前提，其中Sr∈(0,R/2]。

4.1 對直線運動的障礙物防碰策略

對于以速度V0沿直線運動的障礙物，可以預(yù)測出在滾動窗口周期內(nèi)障礙物的運動軌跡。根據(jù)此軌跡與滾動窗口內(nèi)機器人的全局規(guī)劃路徑，可以判斷出二者是否有碰撞的風(fēng)險。

（1）若無碰撞，則機器人按照全局規(guī)劃路徑移動一個周期，進(jìn)入下次窗口刷新。

（2）若側(cè)面碰撞，則在預(yù)估碰撞點前一個柵格點等待Δt。Δt為：

式中，d0為動態(tài)障礙物的長度。

（3）若正面碰撞，將全局路徑與滾動窗口的交點視為局部規(guī)劃的子目標(biāo)點，預(yù)估碰撞點視為靜態(tài)障礙物，以當(dāng)前點為起始點，用本文算法進(jìn)行局部路徑規(guī)劃。此時，分為兩種情形：局部路徑規(guī)劃有解與無解。針對無解的情形，如圖3 所示，在滾動窗口內(nèi)，機器人r與動態(tài)障礙物O相向運動，預(yù)估二者將在A點相遇。按照上面局部規(guī)劃方法，子目標(biāo)點B將到達(dá)不了。因此，針對上述局部路徑規(guī)劃無解的情況，將不再進(jìn)行局部規(guī)劃，重新開始全局規(guī)劃。

Fig.3 Sub-goal point is unreachable圖3 子目標(biāo)點不可達(dá)

4.2 對運動方向不確定的障礙物防碰策略

對于以速度V0運動且方向不確定的動態(tài)障礙物，文獻(xiàn)[16]的策略是將動態(tài)障礙物的活動范圍全部視為靜態(tài)障礙物。這種策略過于保守，且動態(tài)障礙物的整個運動范圍可能包含機器人當(dāng)前的位置。

本文的策略是：當(dāng)滾動窗口內(nèi)檢測到動態(tài)障礙物時，則開始以其所在位置為圓心，以速度V0向外進(jìn)行膨脹，膨脹區(qū)域則可視為靜態(tài)障礙物。機器人因此可以判斷滾動窗口內(nèi)的全局規(guī)劃路徑是否會有與動態(tài)障礙物碰撞的風(fēng)險。若有，則按照5.1 節(jié)的策略進(jìn)行局部路徑規(guī)劃，若無法規(guī)劃出可行的局部路徑，則需要重新進(jìn)行全局路徑規(guī)劃。

5 貝塞爾曲線平滑路徑

路徑規(guī)劃是求得移動機器人從起始點到終點的最優(yōu)路徑。因此，其所產(chǎn)生的路徑需滿足平滑性的需求，盡可能地保證所規(guī)劃出的路徑與實際運動路徑相同。由于本文采用柵格法表示環(huán)境地圖，可能會在路徑轉(zhuǎn)折處產(chǎn)生尖峰。

貝塞爾曲線的最大優(yōu)點是具有優(yōu)良的擬合特性，即如果控制點構(gòu)成的多邊形是凸的，那么貝塞爾曲線也會是凸的。因此，本文采用貝塞爾曲線來平滑規(guī)劃出的路徑。

5.1 貝塞爾曲線方程

給定R3:B:[0,1]→R3，它代表n+1 個控制點，i=0,1,…,n。n次貝塞爾曲線可由n次Bernestein 基多項式組成的參數(shù)曲線表示，定義為[17]：

式中，t表示歸一化時間變量；Bi,n(t)是Bernestein 基多項式，代表貝塞爾曲線中的基函數(shù)，其表達(dá)式定義如下：

貝塞爾曲線的導(dǎo)數(shù)由控制點決定，其一階導(dǎo)數(shù)可以表示為：

在二維空間中，貝塞爾曲線相對于t的曲率可以表示為：

式中，R(t)為曲率半徑；分別為貝塞爾曲線坐標(biāo)分量對x和y的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)。

5.2 貝塞爾曲線平滑路徑

在改進(jìn)A*蟻群算法在存在靜態(tài)或動態(tài)障礙物的情況下得到最短路徑后，由貝塞爾曲線對路徑進(jìn)行平滑處理，稱為路徑的后續(xù)處理環(huán)節(jié)，其具體流程圖如圖4 所示。

Fig.4 Smoothing process of Bessel curve圖4 貝塞爾曲線的平滑流程

6 仿真分析

本文對傳統(tǒng)蟻群算法在路徑規(guī)劃應(yīng)用上的一些不足之處進(jìn)行了改進(jìn)，下面借助Matlab，在CPU 為i5-6500、系統(tǒng)為Win10 的電腦上進(jìn)行仿真實驗。

6.1 與原蟻群算法比較

將本文改進(jìn)的A*蟻群算法與原蟻群算法在20×20 的柵格地圖上進(jìn)行對比實驗，本文算法的初始條件為：最大迭代數(shù)為100，蟻群個數(shù)為30，遺傳算法的最大迭代數(shù)為20，種群大小為25，交叉概率為0.8，變異概率為0.05，剩余5 個重要參數(shù)的變化范圍見表1。由帶精英策略的遺傳算法得到它們的最佳組合為k=3.9，α=0.98，β=14.01，ρ=0.69，Q=18.53。設(shè)起始點為柵格1，終點為柵格400。對于原有的蟻群算法，所有參數(shù)都需按照經(jīng)驗自行設(shè)定，現(xiàn)設(shè)置具體參數(shù)為：最大迭代數(shù)為100，蟻群個數(shù)為30，α=0.60，β=11.00，ρ=0.50，Q=15.50 。圖5 為未進(jìn)行參數(shù)優(yōu)化的原蟻群算法和本文改進(jìn)的A*蟻群算法路徑圖的對比結(jié)果。將本文算法得到的最佳組合參數(shù)（k除外）應(yīng)用于原蟻群算法，其得到的路徑與本文改進(jìn)的A*蟻群算法路徑對比圖如圖6 所示。以上三種算法分別對應(yīng)的收斂曲線圖如圖7 所示。圖8 為本文改進(jìn)的A*蟻群算法規(guī)劃出的路徑和采用貝塞爾曲線平滑后的路徑對比圖，對比原路徑可知貝塞爾曲線對其平滑后的路徑更加符合實際運動路線。

Table 1 Variation range of important parameters表1 重要參數(shù)的變化范圍

Fig.5 Compared with ACO without parameter optimization圖5 與未參優(yōu)蟻群算法對比

Fig.6 Compared with ACO after parameter optimization圖6 與參優(yōu)后蟻群算法對比

Fig.7 Convergence curves of three algorithms圖7 三種算法的收斂曲線

Fig.8 Smooth path diagram under simple obstacles圖8 簡單障礙物下的平滑路徑圖

Table 2 Compared with original ACO表2 與原蟻群算法對比

圖5～圖7 的具體對比數(shù)據(jù)見表2，對比參數(shù)優(yōu)化與非參數(shù)優(yōu)化情況下原蟻群的路徑長度，可知蟻群算法的性能很大程度上受參數(shù)的影響，雖然原蟻群的參數(shù)是依據(jù)經(jīng)驗而非胡亂選得，但是也是局部最優(yōu)參數(shù)，可見本文用遺傳算法優(yōu)化參數(shù)有一定的必要性與優(yōu)越性。對比本文算法與參數(shù)優(yōu)化后的原蟻群算法，不僅路徑長度由30.041 6 縮短到28.627 4，而且收斂情況也由未收斂變?yōu)榈?1 次后收斂，可見本文算法尋優(yōu)能力與收斂速度都大幅度提高。

6.2 與改進(jìn)蟻群算法比較

為了證明本文算法在復(fù)雜靜態(tài)環(huán)境中的優(yōu)越性，選用其他改進(jìn)的蟻群算法進(jìn)行對比。文獻(xiàn)[11]提出了一種動態(tài)搜索策略的蟻群算法（ant colony algorithm for dynamic search strategy），并在50×50 的復(fù)雜環(huán)境中進(jìn)行驗證。由于在文獻(xiàn)[11]中已用其方法與其他改進(jìn)算法在相同環(huán)境下進(jìn)行了對比實驗，證明了其優(yōu)越性，故在此只需與文獻(xiàn)[11]進(jìn)行對比。本文算法的參數(shù)初始化同6.1 節(jié)（下文同），圖9 為兩種方法的路徑對比圖，其中本文算法的收斂曲線圖如圖10 所示，相關(guān)數(shù)據(jù)對比見表3，其中標(biāo)準(zhǔn)差按照文獻(xiàn)[11]中的定義，為算法運行10 次的誤差。而圖11 為本文算法規(guī)劃出的路徑與采用貝塞爾曲線對其進(jìn)行平滑處理后的路徑對比圖。

Fig.9 Compared with algorithm in ref.[11]圖9 與文獻(xiàn)[11]算法對比

Fig.10 Convergence curve of algorithm in this paper圖10 本文算法收斂曲線

Table 3 Compared with improved ACO表3 與改進(jìn)蟻群算法對比

Fig.11 Smooth path diagram under complex obstacles圖11 復(fù)雜障礙物下的平滑路徑圖

從路徑對比圖與表3 中的數(shù)據(jù)對比可知，本文算法無論是路徑長度、收斂迭代次數(shù)，還是穩(wěn)定性（標(biāo)準(zhǔn)差比較）都略優(yōu)于文獻(xiàn)[11]。而且文獻(xiàn)[11]中的參數(shù)是通過經(jīng)驗與不斷調(diào)試所得，本文改進(jìn)A*蟻群的重要參數(shù)是自主尋優(yōu)所得。

6.3 動態(tài)條件下驗證可行性

仿真實驗1在20×20 和40×40 的柵格地圖中，矩形動態(tài)障礙物沿豎直方向，以小于機器人的速度做勻速往返運動。當(dāng)動態(tài)障礙物與全局規(guī)劃路徑未發(fā)生碰撞時，機器人的路徑如圖12 所示；當(dāng)動態(tài)障礙物影響到既定的全局規(guī)劃路徑時，路徑圖如圖13 所示。圖13 中虛線是原定路線，實線是根據(jù)動態(tài)避障策略，進(jìn)行局部路徑規(guī)劃的實際路線。

仿真實驗2實驗1 驗證了障礙物沿直線運動的情況，下面驗證障礙物運動的方向隨機的情況。如圖14 所示，在50×50 的復(fù)雜環(huán)境中，機器人1 從S1(0.5,0.5)到G1(49.5,49.5)，而機器人2 從S2(0.5,31.5)到G2(49.5,13.5) 。兩個機器人相互獨立，速度相同（滿足滾動窗口前提），對于兩個機器人而言，另一方均可視為運動方向不確定的動態(tài)障礙物。圖中的交點為（17.730 8,20.730 8），由于兩機器人速度相同，故二者并未同時到達(dá)，圖中路徑為安全路徑。

7 結(jié)束語

本文針對靜態(tài)與動態(tài)環(huán)境下移動機器人路徑規(guī)劃問題，提出一種融合改進(jìn)A*蟻群算法與滾動窗口法的平滑路徑規(guī)劃方法。該方法克服了傳統(tǒng)蟻群算法收斂慢、易陷入局部最優(yōu)等一系列問題。通過對三組仿真數(shù)據(jù)的分析與對比，證明了該算法每個環(huán)節(jié)改進(jìn)的必要性與優(yōu)越性。仿真結(jié)果表明，本文算法不僅能實現(xiàn)復(fù)雜靜態(tài)障礙物環(huán)境下機器人的路徑規(guī)劃，而且對于一定的動態(tài)障礙物也能實現(xiàn)高效的避讓。

Fig.12 Path planning without collision圖12 未發(fā)生碰撞時路徑規(guī)劃圖

Fig.13 Path planning when avoiding dynamic obstacles圖13 避開動態(tài)障礙物時路徑規(guī)劃圖

Fig.14 Dynamic obstacles avoidance diagram with uncertain motion direction圖14 對運動方向不確定的動態(tài)障礙物避障圖

本文也尚存在不足之處，例如本文在對動態(tài)障礙物的仿真實驗中假設(shè)移動機器人一直處于勻速運動狀態(tài)，然而在實際生活中機器人并不會一直處于勻速運動狀態(tài)。在以后的研究工作中，針對這個問題將會引入不同速度以及存在加速度的機器人進(jìn)行各種仿真實驗，進(jìn)一步提高該算法的實際應(yīng)用價值。