繩紅強，黃海英，石小銳，崔毅剛

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

0 引言

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

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

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

1 蟻群算法的基本模型

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

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

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

(1)

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

ηij(t)=1/dij

(2)

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

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

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

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

(3)

(4)

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

1.3 信息素增量更新模型

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

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

ADS模型：

(5)

AQS模型：

(6)

ACS模型：

(7)

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

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

2 蟻群算法設計改進

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

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

n+1個頂點的n次貝塞耳曲線表達式為

(8)

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

(9)

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

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

(10)

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

(11)

2.2 信息素更新策略

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

(12)

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

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

2.3 算法過程

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

圖1 本文算法流程圖

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

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

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

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

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

3 仿真實驗與分析

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

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

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

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

圖2 10×10二維柵格地圖

圖2中黑色柵格表示障礙物，白色柵格是自由柵格。設每個柵格的中心坐標為柵格的直角坐標，每個柵格編號與直角坐標一一對應，則柵格地圖中任意一點的坐標(xi,yi)與柵格編號i的映射關系如下所示。

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

(13)

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

(14)

式中：a為每個柵格邊長；MM為橫坐標的最大柵格數(shù)值；mod(a,b)為(a/b)取余結果；ceil函數(shù)為朝正無窮大方向取整。

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

圖3 可行路徑方向圖

3.2 結果與分析

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

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

表1 仿真實驗參數(shù)設置

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

圖5 算法收斂變化曲線

表2 仿真實驗結果

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

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

4 結語

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

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