劉 昂，蔣 近，徐克鋒

（1.湘潭大學自動化與電子信息學院，湖南湘潭 411105；2.智能計算與信息處理教育部重點實驗室（湘潭大學），湖南湘潭 411105）

（?通信作者電子郵箱liuang96@163.com）

0 引言

移動機器人路徑規(guī)劃問題是機器人研究領(lǐng)域的熱點，要求機器人在從起始點到目標點的運動過程中避免發(fā)生碰撞［1-2］。根據(jù)周圍環(huán)境信息是否已知，將移動機器人的路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃［3-4］?，F(xiàn)有的路徑規(guī)劃問題主要研究的是全局路徑規(guī)劃［5］，而當移動機器人運動過程中發(fā)現(xiàn)突發(fā)威脅時，規(guī)劃出局部路徑并減小路徑長度和評價函數(shù)值來提高路徑性能，同樣是亟須解決的問題［6-7］。

國內(nèi)外學者在這方面做出了大量工作。常見的路徑規(guī)劃算法包括蟻群算法［8］、A*算法［9］和人工勢場法［10］等。文獻［11］提出一種跳點搜索的方式來改進A*算法，減少對不必要節(jié)點的搜索，提高搜索效率；文獻［12］提出對A*算法的啟發(fā)函數(shù)引入指數(shù)衰減的加權(quán)，降低搜索節(jié)點數(shù)，提高算法搜索效率；文獻［13］提出對鴿群算法的地圖算子部分引入自適應(yīng)權(quán)重系數(shù)，提高鴿群算法的搜索效率；文獻［14］提出通過約束函數(shù)來縮小A*算法的搜索范圍，提高算法的效率；文獻［15］提出采用概率選擇的機制區(qū)分地圖算子和地標算子，提高了算法的全局搜索能力。雖然上述算法在一定程度上解決了收斂速度慢、易陷入局部最優(yōu)解等問題，但仍存在路徑不平滑以及未考慮突發(fā)障礙物等問題。

蟻群算法的正反饋、并行性等優(yōu)點，使得其更適用于機器人路徑規(guī)劃等問題。鴿群算法因其在無人機航路規(guī)劃等問題中表現(xiàn)出優(yōu)秀的尋優(yōu)能力而備受研究者青睞，2014 年Goel［16］首次提到了鴿群算法，但并沒有將其應(yīng)用到實際問題中。

鑒于此，本文提出一種基于改進A*蟻群算法與改進鴿群算法相結(jié)合的算法。首先，利用改進的A*蟻群算法進行全局靜態(tài)路徑規(guī)劃；其次，將全局靜態(tài)路徑應(yīng)用到鴿群算法初始化過程，使算法獲得更多初始信息，并且在全局路徑的基礎(chǔ)上，利用改進的鴿群算法進行局部動態(tài)避障，使得移動機器人能夠繞過動態(tài)障礙物進行局部路徑規(guī)劃；最后，引入B樣條曲線對規(guī)劃路徑進行平滑化和重規(guī)劃。

1 全局規(guī)劃算法

1.1 改進的A*算法

A*算法于1968 年由Hart 等［17］首次提出，是啟發(fā)式路徑規(guī)劃算法［18］。本文使用改進的A*算法來優(yōu)化蟻群算法的初始信息素，減少算法的搜索時間，如圖1所示。

圖1 搜索示意圖Fig.1 Schematic diagram of searching

實際操作時，將兩個方向上當前節(jié)點的前一個節(jié)點作為其相反方向搜索的目標節(jié)點。例如，正向搜索中當前節(jié)點s3是以反向搜索中節(jié)點e2作為目標節(jié)點搜索得來的，反向搜索中當前節(jié)點e4則是以正向搜索中節(jié)點s3作為目標節(jié)點搜索得來的。這樣可以保證兩個方向上搜索的目標節(jié)點同時更新。為了有效提升算法的效率，將得到的優(yōu)化路徑記作R，并將其初始信息素設(shè)為：

其中：k為大于1的系數(shù)；C表示其他路徑上的初始信息素。

1.2 引入方向評價的啟發(fā)信息

本文在改進A*算法的基礎(chǔ)上引入方向評價的啟發(fā)信息，在利用估價函數(shù)f(n)計算每個擴展節(jié)點的成本之前，構(gòu)建擴展節(jié)點的方向評價函數(shù)D(n)，利用方向評價函數(shù)D(n)對擴展的節(jié)點進行篩選，保留朝向目標點搜索的節(jié)點，舍去偏離目標點搜索的節(jié)點，提高搜索速度。其中，方向評價函數(shù)為：

式中：D(n)為擴展節(jié)點N 的方向評價函數(shù)；θ 為擴展節(jié)點與其父節(jié)點構(gòu)成的矢量與目標點與起始點構(gòu)成的矢量之間的夾角。而在本文中，起始點和目標點的坐標已知，令(x1，y1)為目標點與起始點構(gòu)成的矢量的坐標；(x2，y2)為擴展節(jié)點與其父節(jié)點構(gòu)成的矢量的坐標。則兩個矢量的夾角可以表示為：

而父節(jié)點N -1的坐標為(xn-1，yn-1)；擴展節(jié)點N的坐標為(xn，yn)；起始點的坐標為(xstart，ystart)；目標點的坐標為(xend，yend)。兩個矢量的坐標分別為：

本文采用八鄰域節(jié)點擴展，當D(n) ≥0時，則保留這些節(jié)點，將其加入OPEN 表中進行后續(xù)計算；當D(n) ＜0 時，則忽略掉這些節(jié)點，加快搜索速度。

1.3 改進的蟻群算法

傳統(tǒng)蟻群算法通過信息素的引導來進行搜索［19］。本文在傳統(tǒng)蟻群算法的轉(zhuǎn)移概率基礎(chǔ)上引入隨機選擇機制來增加解的多樣性。設(shè)q0為隨機選擇因子，q0∈(0，1)，q1為［0，1］的常數(shù)，當q0≤q1，則在排除障礙物節(jié)點和已走節(jié)點后，隨機選取當前節(jié)點周圍任意一個節(jié)點作為可行節(jié)點；否則按照概率轉(zhuǎn)移公式來選擇可行節(jié)點。具體如下：

式中：α為信息素啟發(fā)因子；β為啟發(fā)信息因子；τij(t)為連接頂點i，j 的邊上第t 次迭代開始時的信息素濃度；ηij(t)為啟發(fā)信息；rand(allowedk)表示在允許選擇的柵格集合中隨機選擇下一步節(jié)點。

為避免蟻群算法受到非最優(yōu)路徑信息素的干擾而陷入局部最優(yōu)，本文對信息素更新策略引入獎懲因子，即對當前迭代后的最優(yōu)路徑Lb增強其信息素濃度，對當前迭代后的最差路徑Lw減小其信息素濃度，并且在迭代初期更應(yīng)跳出局部最優(yōu)，在迭代后期應(yīng)逐步減少獎懲因子的影響，故引入三角函數(shù)作為系數(shù)。改進后的更新公式如下：

2 局部規(guī)劃算法

2.1 路徑評價函數(shù)

機器人路徑優(yōu)劣的評價函數(shù)如下所示：

式中：f 表示評價函數(shù)；f1表示威脅評價函數(shù)；f2表示路徑評價函數(shù)；0 ≤k ≤1，為f1與f2之間的權(quán)重比。

f1為機器人受到的威脅評價函數(shù)，將路段分為10份，取路段上標志性的五個點來表示該段路徑受到的威脅代價，則路段i受到的威脅代價［20］為：

式中：Li為路段的長度；tk為威脅因子，表示障礙物的影響程度；N 為障礙物的個數(shù)；d0.5，k為5Li/10 處的點距離第k 個障礙物的距離。

f2為移動機器人受到的路徑評價函數(shù)，借鑒A*算法中的評價函數(shù)對其改進表示如下：

式中：di，i+1表示節(jié)點i 與節(jié)點i+1 之間的距離，di+1，e表示節(jié)點i+1與目標點e之間的距離。

2.2 鴿群算法的初始化

基于鴿群在歸巢過程中的特殊導航行為，Duan 等［21］提出了一種仿生群體智能優(yōu)化算法——鴿群優(yōu)化算法。鴿群算法主要應(yīng)用于無人機航路規(guī)劃等領(lǐng)域，但是現(xiàn)有的鴿群算法在進行航路規(guī)劃時，沒有對初始路徑進行初始化操作，這樣導致最終的算法執(zhí)行效率較低［22］。基于鴿群算法優(yōu)秀的尋優(yōu)能力，本文將改進的鴿群算法應(yīng)用到機器人的路徑規(guī)劃中。靜態(tài)規(guī)劃階段得出的路徑可視為一個較為優(yōu)秀的解集，可以將此解集作為鴿群算法的初始種群，提高算法的執(zhí)行效率。

當檢測到動態(tài)障礙物時，通過靜態(tài)階段設(shè)置的局部起始點和局部目標點之間的靜態(tài)路徑來進行鴿群算法的初始化操作。具體如圖2所示。

圖2 初始化示意圖Fig.2 Schematic diagram of initialization

在實際計算中，本文通過固定橫坐標的方式來簡化計算。圖2 中O 點和A 點分別為局部起始點和局部目標點，將OA 沿橫向作n 等分，圖中的a～d 表示n -1 條等分線，路徑OA 則由這樣的等分線構(gòu)成。本文以正態(tài)分布的形式取這些等分線上下一定數(shù)量的初始數(shù)據(jù)形成鴿群算法的初始種群。

通過上述操作，鴿群算法的初始種群由以靜態(tài)路徑為中心的正態(tài)分布的初始數(shù)據(jù)組成。初始種群不失隨機性，同時整體質(zhì)量提高，能加快尋優(yōu)速度。

2.3 引入模擬退火準則的全局最優(yōu)位置

鴿群算法的起始階段使用地圖算子進行搜索，每次迭代鴿子都會得到新的位置和速度。在地圖算子的速度迭代公式中，e-Rt是一個遞減的指數(shù)函數(shù)，在迭代后期將趨近于零，因此，地圖算子十分依賴全局最優(yōu)位置x'g，本文對全局最優(yōu)位置進行改進，改進后的地磁算子更新公式如下所示：

式中：vi(t)為第i 只鴿子在第t 次迭代后的速度；xi(t)為第i 只鴿子在第t次迭代后的位置；R 是地圖因子；rand 是［0，1］的隨機數(shù)；x'g為迭代中改進的全局最優(yōu)位置。

本文對原全局最優(yōu)位置引入高斯擾動，避免鴿群算法陷入局部最優(yōu)，提高整個種群的多樣性，具體如下：

式中：xg(t)為原全局最優(yōu)位置；xG(t)為全局最優(yōu)位置受到高斯擾動后的位置；r1為［0，1］的控制參數(shù)；G 是服從均值為0、方差為1的高斯分布，其概率密度函數(shù)［23］如下：

另外，在搜索過程中，應(yīng)以一定的概率接受較差解，可以有效避免在迭代搜索過程中陷入局部最優(yōu)，本文引入模擬退火算法來解決此問題。設(shè)擾動前的原全局最優(yōu)位置xg的適應(yīng)度值為f(xg)，受到高斯擾動后的全局最優(yōu)位置xG的適應(yīng)度值為f(xG)。如果f(xG)優(yōu)于f(xg)或者下面的式（13）成立，則將受到擾動后的位置xG作為改進后的全局最優(yōu)位置xg'；否則，仍使用擾動前的全局最優(yōu)位置xg作為改進后的全局最優(yōu)位置xg'，具體如下：

式中：K 為玻爾茲曼常數(shù)；μ 是衰減因子；T(t)為當前迭代退火溫度，隨著迭代進行逐漸下降。

2.4 引入自適應(yīng)步長的鴿群數(shù)量

在地標算子中，迭代后期種群數(shù)量過少，影響算法尋優(yōu)。地標算子階段中，在迭代初期種群規(guī)?？梢陨源笠恍S著迭代次數(shù)逐漸增大，種群規(guī)模應(yīng)逐漸減小。logsig函數(shù)具有從1 到0 非線性減少的特性，故本文中引入logsig 函數(shù)作為鴿群數(shù)量的自適應(yīng)步長，改進后的地標算子更新公式如下所示：

式中：Np(t)為當前迭代次數(shù)的鴿群數(shù)量；Npmax表示種群規(guī)模的數(shù)量最大值；Ncmax2為地標算子階段的最大迭代次數(shù)；k 為logsig 函數(shù)的斜率；xc(t)代表第t 代鴿群的中心；f(xi(t))是第i只鴿子的適應(yīng)度函數(shù)。改進后的種群數(shù)量呈非線性遞減的變化趨勢，其值逐漸減少，保證了種群多樣性。

3 路徑平滑化及重規(guī)劃

3.1 三次B樣條平滑化

實際運動過程中，要讓路徑盡量平滑。本文使用三次B樣條曲線對路徑平滑化處理［24］。B 樣條曲線是Bezier 曲線的一般化形式，通過逼近多邊形而獲得曲線［25］。曲線平滑化的示意圖如圖3所示。

圖3 平滑化示意圖Fig.3 Schematic diagram of smoothing

3.2 平滑化后重規(guī)劃

平滑化后若有部分路徑進入障礙物內(nèi)部，那么可以對該段路徑中相交的碰撞部分進行重規(guī)劃使路徑更貼近原曲線。

具體操作為：如圖4 所示，當平滑化后的路徑進入障礙物內(nèi)部，而原路徑并未進入障礙物內(nèi)部時，通過選取相交的碰撞部分的4個路徑點，在4個點形成的三條邊的中點取新路徑點進行平滑化，并重復上述過程直至消除碰撞。

圖4 重規(guī)劃示意圖Fig.4 Schematic diagram of replanning

4 本文算法流程

根據(jù)上述改進方案，本文整體步驟如下：

1）用柵格法建立地圖模型，設(shè)置全局靜態(tài)階段的參數(shù)，包含蟻群數(shù)量m、信息素啟發(fā)因子α、啟發(fā)信息因子β、信息素揮發(fā)因子ρ、信息素強度系數(shù)Q、迭代次數(shù)Nc等。

2）利用改進的A*蟻群算法規(guī)劃出一條靜態(tài)離線路徑，并對路徑進行平滑化處理。

3）利用靜態(tài)路徑的節(jié)點，以正態(tài)分布的形式對改進的鴿群算法進行初始化，并設(shè)置局部動態(tài)避障階段的參數(shù)，包含鴿群數(shù)量Npmax，地圖和地標算子的迭代次數(shù)Ncmax1和Ncmax2，地圖因子R，初始退火溫度T(0)和衰減因子μ 以及l(fā)ogsig 函數(shù)的斜率k。

4）移動機器人通過自帶傳感器檢測到動態(tài)障礙物時，以檢測到障礙物的前一個柵格點為局部起始點，利用改進的鴿群算法進行局部動態(tài)避障，并對局部路徑進行平滑化處理；否則沿原靜態(tài)路徑到達目標點。

5）判斷移動機器人是否到達目標點，若到達，則結(jié)束整個路徑規(guī)劃過程；否則返回步驟4）。整體流程如圖5所示。

圖5 整體流程Fig.5 Overall flowchart

5 仿真與分析

從靜態(tài)和動態(tài)兩個方面對本文算法的性能進行仿真分析?；跂鸥穹ú⒔柚鶰atlab 2017b 平臺在Windows 10 系統(tǒng)下進行建模仿真分析。

5.1 全局靜態(tài)避障

為了驗證本文算法在全局靜態(tài)避障過程中的性能，在不同環(huán)境下進行仿真對比，要求路徑不能與障礙物的邊緣發(fā)生任何碰撞。全局靜態(tài)避障階段的蟻群數(shù)量m=20，迭代次數(shù)Nc=30，信息素啟發(fā)因子α=1，啟發(fā)信息因子β=12，信息素揮發(fā)因子設(shè)置為ρ=0.2，信息素強度系數(shù)Q=10。

5.1.1 環(huán)境1中仿真比較

在環(huán)境1 下對傳統(tǒng)蟻群算法和本文算法進行對比分析，起始點坐標為（1.5，1.5），目標點坐標為（33.5，33.5）。其中，圖6（a）為傳統(tǒng)蟻群算法規(guī)劃出的最優(yōu)路徑，圖6（b）為本文算法規(guī)劃出的最優(yōu)路徑。

圖6 靜態(tài)環(huán)境1下傳統(tǒng)蟻群算法和本文算法對比Fig.6 Comparison of traditional ant colony algorithm and the proposed algorithm under static environment 1

5.1.2 環(huán)境2中仿真比較

在環(huán)境2 下對傳統(tǒng)蟻群算法和本文算法進行對比分析，起始點坐標為（0.5，39.5），目標點坐標為（39.5，0.5），其中，圖7（a）為傳統(tǒng)蟻群算法規(guī)劃出的最優(yōu)路徑，圖7（b）為本文算法規(guī)劃出的最優(yōu)路徑。

5.1.3 性能指標對比

在靜態(tài)環(huán)境下，對環(huán)境1和環(huán)境2下的本文算法與傳統(tǒng)蟻群算法的性能指標對比分析。

表1給出傳統(tǒng)蟻群算法和本文算法在兩種環(huán)境下進行10次路徑規(guī)劃的結(jié)果?？梢钥闯?，傳統(tǒng)蟻群算法在環(huán)境1中有3次找到了最優(yōu)路徑52.527 km，在環(huán)境2 中有3 次找到了最優(yōu)路徑71.456 km；本文算法在環(huán)境1中有10次找到了最優(yōu)路徑49.941 km，在環(huán)境2 中有4 次找到了最優(yōu)路徑66.284 km，在兩種環(huán)境中的平均值分別比傳統(tǒng)蟻群算法縮短了2.977 km和5.078 km。本文算法在環(huán)境1 下的平均執(zhí)行時間2.503 s比傳統(tǒng)蟻群算法的執(zhí)行時間2.712 s，縮短了7.71%；本文算法在環(huán)境2 下的平均執(zhí)行時間4.872 s 比傳統(tǒng)蟻群算法的執(zhí)行時間5.204 s縮短了6.38%。綜合來看，本文算法性能更優(yōu)。

圖7 靜態(tài)環(huán)境2下傳統(tǒng)蟻群算法和本文算法對比Fig.7 Comparison of traditional ant colony algorithm and the proposed algorithm under static environment 2

表1 傳統(tǒng)蟻群算法和本文算法性能對比Tab.1 Performance comparison between traditional ant colony algorithm and the proposed algorithm

5.2 局部動態(tài)避障

為了驗證本文算法在局部動態(tài)避障過程中的性能，在不同環(huán)境下進行仿真對比。局部動態(tài)避障階段的鴿群數(shù)量Npmax=150，迭代次數(shù)Ncmax1=150，Ncmax2=100，初始退火溫度T(0)=100，衰減因子μ=0.99，logsig函數(shù)的斜率k=5。

5.2.1 環(huán)境1中仿真比較

在環(huán)境1 中，障礙物與移動機器人的路徑有部分重疊，如圖8所示。圖中的P點為檢測到碰撞點的前一個柵格點，M點為障礙物離開移動機器人軌跡后的下一個柵格點，即此時的障礙物僅在P點和M點之間運動，P點和M點分別為局部起始點和局部目標點，其坐標分別為（9.5，15.5）和（17.5，23.5）。

圖8 動態(tài)環(huán)境1下傳統(tǒng)蟻群算法和本文算法對比Fig.8 Comparison of traditional ant colony algorithm and the proposed algorithm under dynamic environment 1

可以看出，圖8（a）傳統(tǒng)鴿群算法得出的路徑性能較劣，并不能完全躲避障礙物，與障礙物的邊緣仍會發(fā)生碰撞。圖8（b）本文算法得出的路徑與障礙物保持一定距離，且路徑長度更短、更平滑化。

5.2.2 環(huán)境2中仿真比較

為了進一步驗證本文算法在動態(tài)避障階段的性能，在環(huán)境2 中，即在規(guī)劃空間中隨機生成大量突發(fā)動態(tài)障礙物的復雜環(huán)境下進行仿真對比，如圖9所示。圖中的S點表示規(guī)劃起始點，E 點表示規(guī)劃目標點，圓形區(qū)域表示膨脹化后的動態(tài)障礙物。從圖9 可以看出，傳統(tǒng)鴿群算法在面臨數(shù)量眾多突發(fā)障礙物時不能完全避障，本文算法得出的路徑具有更短的路徑長度且更平滑，能實現(xiàn)完全避障。

圖9 動態(tài)環(huán)境2下傳統(tǒng)蟻群算法和本文算法對比Fig.9 Comparison of traditional ant colony algorithm and the proposed algorithm under dynamic environment 2

5.2.3 性能指標對比

在動態(tài)環(huán)境下，對環(huán)境1和環(huán)境2下本文算法與傳統(tǒng)鴿群算法的性能指標對比分析。表2 是傳統(tǒng)鴿群算法和本文算法在環(huán)境1 和環(huán)境2 中進行10 次實驗的性能指標對比表?？梢钥闯觯诃h(huán)境1 下，本文算法在10 次實驗的平均路徑長度51.273 km 比傳統(tǒng)鴿群算法平均路徑長度51.406 km 要短0.133 km，并且本文算法評價函數(shù)的平均值2.621 比傳統(tǒng)鴿群算法評價函數(shù)的平均值2.792 減小了0.171。在環(huán)境2 的復雜環(huán)境下，本文算法在10 次實驗的平均路徑長度62.566 km 比傳統(tǒng)鴿群算法的70.006 km 縮短了10.62%，并且本文算法評價函數(shù)的平均值72.734 比傳統(tǒng)鴿群算法評價函數(shù)的平均值89.841減小了19.04%。

表2 傳統(tǒng)鴿群算法和本文算法性能對比Tab.2 Performance comparison between traditional pigeon inspired optimization algorithm and the proposed algorithm

綜合上述結(jié)果來看，在相同的環(huán)境下，本文算法能規(guī)劃出路徑長度更短、評價函數(shù)值更小、性能更優(yōu)的路徑，具有更強的尋優(yōu)能力。

6 結(jié)語

傳統(tǒng)蟻群算法收斂速度較慢、易陷入局部最優(yōu)解，應(yīng)用到機器人路徑規(guī)劃時不易尋得全局最優(yōu)解。本文對傳統(tǒng)蟻群算法引入改進的雙向A*算法，并對蟻群算法的轉(zhuǎn)移概率和信息素更新機制進行改進。與傳統(tǒng)蟻群算法相比，本文的全局靜態(tài)路徑算法可以得出更為優(yōu)秀的靜態(tài)路徑；并且針對運動過程中出現(xiàn)動態(tài)障礙物的情況，本文提出一種改進的鴿群算法來實現(xiàn)局部動態(tài)避障。通過對傳統(tǒng)鴿群算法的地圖和地標算子進行改進，與傳統(tǒng)鴿群算法相比，本文改進后的局部動態(tài)避障算法能更好地實現(xiàn)避障。最后，本文對整體路徑引入三次B樣條曲線進行路徑平滑化處理，使路徑更符合實際。

改進后的算法仍存在一些不足之處，如沒有將算法放到實際場景中進行實驗比較，沒有在三維環(huán)境下對算法進行推廣，今后將嘗試研究將本文方法應(yīng)用到實際場景以及三維環(huán)境下的機器人路徑規(guī)劃問題中。