殷紹偉，彭 力+，戴菲菲

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

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

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

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

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

1 環(huán)境建模

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

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

Fig.1 Grid index圖1 柵格索引

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

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

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

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

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

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

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

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

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

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

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

考慮到這兩種啟發(fā)函數(shù)各自的優(yōu)點(diǎn)與不足，本文設(shè)計(jì)了一種更接近實(shí)際移動(dòng)消耗H(n)的啟發(fā)函數(shù)。首先考慮沿著對(duì)角線運(yùn)動(dòng)，朝著對(duì)角線移動(dòng)一個(gè)柵格代價(jià)為（假設(shè)直線移動(dòng)代價(jià)為1），則總的對(duì)角線移動(dòng)代價(jià)為×min(|nx-gx|,|ny-gy|)；然后考慮沿著直線運(yùn)動(dòng)，得出總的直線移動(dòng)代價(jià)為hM(n)-2×(min(|nx-gx|,|ny-gy|))。本文設(shè)計(jì)的啟發(fā)函數(shù)為總的對(duì)角線移動(dòng)代價(jià)與總的直線移動(dòng)代價(jià)的算術(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)前點(diǎn)與相鄰可行柵格點(diǎn)之間的距離。其應(yīng)用于復(fù)雜環(huán)境時(shí)，尤其是有凹型障礙物的環(huán)境中，機(jī)器人很可能陷入如圖2 所示的死鎖狀態(tài)。

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

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

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

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

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

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

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

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

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

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

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

3.1 帶精英策略的遺傳算法

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

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

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

本文遺傳算法的適應(yīng)度函數(shù)設(shè)置為路徑的長(zhǎng)度，依據(jù)一般經(jīng)驗(yàn)與算法本身的約束條件，將改進(jìn)的A*蟻群算法中5個(gè)重要參數(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對(duì)改進(jìn)蟻群算法中5 個(gè)重要的參數(shù)進(jìn)行有約束的編碼，編碼方式為二進(jìn)制編碼，約束條件是各參數(shù)的取值范圍。

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

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

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

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

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

4.1 對(duì)直線運(yùn)動(dòng)的障礙物防碰策略

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

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

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

式中，d0為動(dòng)態(tài)障礙物的長(zhǎng)度。

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

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

4.2 對(duì)運(yùn)動(dòng)方向不確定的障礙物防碰策略

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

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

5 貝塞爾曲線平滑路徑

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

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

5.1 貝塞爾曲線方程

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

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

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

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

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

5.2 貝塞爾曲線平滑路徑

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

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

6 仿真分析

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

6.1 與原蟻群算法比較

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

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

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

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

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

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

Table 2 Compared with original ACO表2 與原蟻群算法對(duì)比

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

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

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

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

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

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

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

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

6.3 動(dòng)態(tài)條件下驗(yàn)證可行性

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

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

7 結(jié)束語(yǔ)

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

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

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

Fig.14 Dynamic obstacles avoidance diagram with uncertain motion direction圖14 對(duì)運(yùn)動(dòng)方向不確定的動(dòng)態(tài)障礙物避障圖

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