繩紅強(qiáng)，黃海英，石小銳，崔毅剛

(1. 中國(guó)重汽集團(tuán)濟(jì)南動(dòng)力有限公司，山東 濟(jì)南 250101；2. 武漢大學(xué) 數(shù)學(xué)與統(tǒng)計(jì)院，湖北 武漢 430072；3. 山東圣翰財(cái)貿(mào)職業(yè)學(xué)院，山東 濟(jì)南 250316)

0 引言

路徑規(guī)劃是自動(dòng)駕駛車(chē)輛運(yùn)動(dòng)控制的關(guān)鍵技術(shù)，通常被描述為自動(dòng)駕駛車(chē)輛在空間環(huán)境約束下尋找一條從起始點(diǎn)通往目標(biāo)點(diǎn)的最優(yōu)無(wú)碰撞路徑[1]。自動(dòng)駕駛車(chē)輛又稱(chēng)為輪式移動(dòng)機(jī)器人，其路徑規(guī)劃算法的研究源自機(jī)器人技術(shù)。迄今為止，國(guó)內(nèi)外學(xué)者提出了諸多經(jīng)典的路徑規(guī)劃算法[2-4]，如蟻群算法、Dijastra算法、A*算法、粒子群算法、快速探索隨機(jī)樹(shù)算法(RRT)、人工勢(shì)場(chǎng)法、禁忌搜索算法、神經(jīng)網(wǎng)絡(luò)法、動(dòng)態(tài)窗口算法等。目前大多數(shù)算法針對(duì)具體問(wèn)題需要進(jìn)行優(yōu)化整合，自動(dòng)駕駛車(chē)輛安全行駛要求延時(shí)必須控制在毫秒甚至微秒級(jí)別，對(duì)算法效率、規(guī)劃路徑質(zhì)量提出了較高的要求。

蟻群算法由意大利學(xué)者DORIGO M等于1996年首先提出[5]，它源于對(duì)螞蟻群體覓食機(jī)制的研究。蟻群算法具有自組織、分布式、正反饋、魯棒性強(qiáng)等優(yōu)點(diǎn)，易于與其他優(yōu)化算法相結(jié)合，已經(jīng)廣泛用于求解車(chē)輛路徑規(guī)劃、機(jī)器人路徑規(guī)劃、無(wú)人機(jī)飛行路徑規(guī)劃等問(wèn)題[6-13]。但是，蟻群算法理論模型缺乏堅(jiān)實(shí)的數(shù)學(xué)基礎(chǔ)，其關(guān)鍵參數(shù)的選擇多依靠試驗(yàn)和經(jīng)驗(yàn)來(lái)確定，易出現(xiàn)路徑拐點(diǎn)多、轉(zhuǎn)彎角度大、收斂速度慢、局部收斂、搜索停滯等典型問(wèn)題[14]。

針對(duì)蟻群算法的不足，本文提出一種適用于自動(dòng)駕駛路徑規(guī)劃的改進(jìn)蟻群算法，以提高規(guī)劃路徑的質(zhì)量和算法性能，對(duì)實(shí)現(xiàn)自動(dòng)駕駛車(chē)輛安全行駛具有重要價(jià)值。

1 蟻群算法的基本模型

由于蟻群算法原理在DORIGO M等的著作已有詳盡的描述，在此不做贅述。下面簡(jiǎn)要說(shuō)明蟻群算法的基本數(shù)學(xué)模型和實(shí)現(xiàn)過(guò)程的關(guān)鍵步驟。蟻群算法的核心是利用狀態(tài)轉(zhuǎn)移概率和遺留的信息素濃度實(shí)現(xiàn)路徑選擇。

1.1 狀態(tài)轉(zhuǎn)移概率

定義螞蟻在t時(shí)刻由節(jié)點(diǎn)i轉(zhuǎn)移至節(jié)點(diǎn)j的狀態(tài)轉(zhuǎn)移概率為

(1)

啟發(fā)函數(shù)ηij(t)表達(dá)式為

ηij(t)=1/dij

(2)

式中dij為節(jié)點(diǎn)i和j的距離，即

1.2 信息素更新規(guī)則

當(dāng)M只螞蟻完成一次循環(huán)后，需對(duì)路徑信息素更新，其更新公式為

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

(3)

(4)

式中：τij(t)為t時(shí)刻節(jié)點(diǎn)i到相鄰節(jié)點(diǎn)j的路徑之間信息素濃度；τij(t-1)為在t-1時(shí)刻節(jié)點(diǎn)i到相鄰節(jié)點(diǎn)j的路徑之間信息素濃度；ρ為信息素?fù)]發(fā)系數(shù),ρ∈(0,1)；Δτij為在t-1時(shí)刻到t時(shí)刻節(jié)點(diǎn)i到相鄰節(jié)點(diǎn)j的路徑之間信息素濃度的增量。

1.3 信息素增量更新模型

根據(jù)信息素濃度的增量更新方式的不同，DORIGO M等提出3種不同的基本更新模型，分別稱(chēng)之為Ant-Density System(ADS)、Ant-Quantity System (AQS)和Ant-Cycle System (ACS)模型。

若第m只螞蟻當(dāng)前循環(huán)經(jīng)過(guò)路徑(i,j)節(jié)點(diǎn)的集合為X{(i,j)|i=1,2,…,n;j=1,2,…,n}，則：

ADS模型：

(5)

AQS模型：

(6)

ACS模型：

(7)

式中：Q為信息素強(qiáng)度，影響算法求解速度，為>0的常數(shù)；Lm為第m只螞蟻在本次循環(huán)中所走路徑的總距離。

上述3個(gè)模型的差別：ADS模型和AQS模型采用的是局部更新策略，即螞蟻每走一步都要更新殘留信息素的濃度；ACS模型采用的是全局更新策略，即螞蟻完成一個(gè)循環(huán)后再更新殘留信息素的濃度，在求解路徑規(guī)劃問(wèn)題中最為常用，因此ACS模型通常作為信息素更新的基本模型。

2 蟻群算法設(shè)計(jì)改進(jìn)

2.1 全局啟發(fā)函數(shù)設(shè)計(jì)

式(2)中啟發(fā)函數(shù)ηij(t)僅利用了相鄰節(jié)點(diǎn)的信息，啟發(fā)性不足，缺乏全局性，易出現(xiàn)規(guī)劃路徑拐點(diǎn)多、角度大、盲目搜索、收斂速度慢[14-15]等問(wèn)題。本文利用相鄰節(jié)點(diǎn)和目標(biāo)點(diǎn)的信息引入貝塞耳平滑曲線函數(shù)，構(gòu)造了一種全局啟發(fā)函數(shù)。

n+1個(gè)頂點(diǎn)的n次貝塞耳曲線表達(dá)式為

(8)

式中：Pi(i=0,1,2,…,n)為各頂點(diǎn)的位置向量；Bi,n(t)為伯恩斯坦基函數(shù)。

(9)

令ηij(t)=P(t),P0=1/dij和P1=1/djE，按貝塞耳曲線函數(shù)一階展開(kāi)，即取n=1，則

全局啟發(fā)函數(shù)ηij(t)表達(dá)式為

(10)

t的大小取決于車(chē)速、相鄰節(jié)點(diǎn)之間的距離。當(dāng)車(chē)輛均速行駛時(shí)，t≈dij/(dij+djE)。當(dāng)djE?dij時(shí)，t→0，則全局啟發(fā)函數(shù)ηij(t)表達(dá)式可近似為

(11)

2.2 信息素更新策略

本文提出利用螞蟻當(dāng)前路徑、最優(yōu)路徑、最差路徑和理想路徑，構(gòu)建一種動(dòng)態(tài)調(diào)整的信息素增量模型。該信息素增量模型表達(dá)式如下：

(12)

式中：n為第n次迭代；Ln,m為當(dāng)前路徑距離，即第m只螞蟻產(chǎn)生的路徑的距離；Lmin為最優(yōu)路徑距離，即第n次迭代產(chǎn)生的最短路徑距離；Lmax為最差路徑距離，即第n次迭代產(chǎn)生的最長(zhǎng)路徑距離；Lidv為理想路徑距離，即起始點(diǎn)與目標(biāo)點(diǎn)的直線距離；δ為最優(yōu)路徑距離與最差路徑的差值，即δ=Lmax-Ln,m；ε為第n次迭代可接受路徑誤差，ε值為一常數(shù)。

在迭代過(guò)程中,新的信息素增量機(jī)制可以自適應(yīng)動(dòng)態(tài)調(diào)整信息素的強(qiáng)度，使優(yōu)化過(guò)程加速向全局最優(yōu)路徑收斂。當(dāng)δ>ε時(shí)，Lmax與Ln,m差值越大，信息素強(qiáng)度越大，全局收斂時(shí)間縮短，算法求解效率得到提升；當(dāng)δ≤ε時(shí)，Ln,m與Lmin越接近，信息素濃度蒸發(fā)越快，使算法避免出現(xiàn)過(guò)早收斂陷入局部最優(yōu)。Lidv在迭代過(guò)程中起到參照作用，可作為最佳路徑?jīng)Q策依據(jù)。

2.3 算法過(guò)程

根據(jù)上述理論，按照本文提出的全局啟發(fā)函數(shù)和信息素更新策略，自動(dòng)駕駛車(chē)輛路徑規(guī)劃算法流程如圖1所示，步驟如下所述。

圖1 本文算法流程圖

步驟1：獲取環(huán)境地圖數(shù)據(jù)。定位起始點(diǎn)S和目標(biāo)點(diǎn)E；獲取空間所有節(jié)點(diǎn)信息，計(jì)算鄰接矩陣D和計(jì)算啟發(fā)式信息矩陣。

步驟2：參數(shù)初始化。初始化迭代次數(shù)N，螞蟻規(guī)模M，信息啟發(fā)式因子α，期望啟發(fā)式因子β，信息素?fù)]發(fā)系數(shù)ρ，信息素濃度ε、t，當(dāng)前路徑列表RT，禁忌表TS。將起始點(diǎn)S置于禁忌表RT和當(dāng)前路徑列表TS。

步驟4：螞蟻序號(hào)更新。若第m只螞蟻的當(dāng)前路徑列表包含了目標(biāo)點(diǎn)或無(wú)路徑且m≥M，則轉(zhuǎn)入步驟5，否則返回步驟3。

步驟5：信息素更新。計(jì)算當(dāng)前迭代最優(yōu)路徑并按式(3)和式(12)更新信息素矩陣。

步驟6：迭代或停止。若n≥N，則輸出最優(yōu)路徑并停止迭代，否則返回步驟3。

3 仿真實(shí)驗(yàn)與分析

3.1 仿真環(huán)境搭建

經(jīng)典的環(huán)境地圖建模方法有柵格法、幾何法、可視圖法等[16-17]。

柵格法是將自動(dòng)駕駛車(chē)輛行駛的空間分解成一系列具有二值信息的網(wǎng)格單元。柵格法表達(dá)簡(jiǎn)單，易于實(shí)現(xiàn)，在路徑規(guī)劃中最為常用。

下面以10×10二維柵格地圖為例，簡(jiǎn)要說(shuō)明柵格地圖數(shù)學(xué)模型和表示形式，如圖2所示。

圖2 10×10二維柵格地圖

圖2中黑色柵格表示障礙物，白色柵格是自由柵格。設(shè)每個(gè)柵格的中心坐標(biāo)為柵格的直角坐標(biāo)，每個(gè)柵格編號(hào)與直角坐標(biāo)一一對(duì)應(yīng)，則柵格地圖中任意一點(diǎn)的坐標(biāo)(xi,yi)與柵格編號(hào)i的映射關(guān)系如下所示。

xi=a·[mod(i,MM)-0.5]

(13)

yi=a·[MM+0.5-ceil(i/MM)]

(14)

式中：a為每個(gè)柵格邊長(zhǎng)；MM為橫坐標(biāo)的最大柵格數(shù)值；mod(a,b)為(a/b)取余結(jié)果；ceil函數(shù)為朝正無(wú)窮大方向取整。

在柵格地圖中，在當(dāng)前節(jié)點(diǎn)i位置路徑?jīng)Q策可選方向有“十”字型或“米”字型[18]。本文采用“米”字型方向選擇規(guī)則，如圖3所示。

圖3 可行路徑方向圖

3.2 結(jié)果與分析

通過(guò)仿真程序，下面對(duì)本文算法的有效性在柵格地圖環(huán)境下進(jìn)行試驗(yàn)驗(yàn)證。仿真環(huán)境如圖4所示的20×20二維柵格地圖。

仿真試驗(yàn)參數(shù)設(shè)置詳見(jiàn)表1，仿真試驗(yàn)結(jié)果如圖4、圖5和表2所示。

表1 仿真實(shí)驗(yàn)參數(shù)設(shè)置

圖4 算法路徑規(guī)劃圖

圖5 算法收斂變化曲線

表2 仿真實(shí)驗(yàn)結(jié)果

由圖4可見(jiàn)，本文改進(jìn)算法可以實(shí)現(xiàn)自動(dòng)駕駛車(chē)輛在復(fù)雜的障礙環(huán)境中尋找到一條從起始節(jié)點(diǎn)出發(fā)到達(dá)目標(biāo)點(diǎn)的無(wú)碰撞最優(yōu)路徑。

由圖5和表2結(jié)果顯示，本文算法迭代8次達(dá)到收斂，規(guī)劃路徑距離為31.799，明顯優(yōu)于基本算法。本文算法全路徑拐點(diǎn)有19個(gè)，拐角之和為900°，其中45°拐角18次、90°拐角1次；而基本算法的拐點(diǎn)數(shù)為27次，拐角之和為2 170°，其中直角拐角有21次，45°拐角6次，顯然本文算法規(guī)劃路徑更平滑。

4 結(jié)語(yǔ)

本文提出一種用于自動(dòng)駕駛路徑規(guī)劃的改進(jìn)蟻群算法。仿真實(shí)驗(yàn)結(jié)果表明：本文構(gòu)造的全局啟發(fā)函數(shù)，在引導(dǎo)螞蟻對(duì)目標(biāo)節(jié)點(diǎn)的感知、消除路徑搜索的盲目性和改善規(guī)劃路徑的平滑性方面效果顯著；提出的基于動(dòng)態(tài)調(diào)整的信息素更新策略，使算法優(yōu)化過(guò)程自適應(yīng)調(diào)整信息素的強(qiáng)度，全局收斂速度得到明顯提升。

未來(lái)進(jìn)一步探索建立基于自動(dòng)駕駛車(chē)輛完整約束條件的精確算法模型，提高算法的效率和穩(wěn)健性，滿足復(fù)雜環(huán)境下實(shí)時(shí)規(guī)避碰撞風(fēng)險(xiǎn)將是自動(dòng)駕駛路徑規(guī)劃研究的重點(diǎn)問(wèn)題。