宋佳艷，蘇圣超

（上海工程技術大學電子電氣工程學院，上海 201620）

0 概述

智能交通系統(tǒng)（Intelligent Transportation System，ITS）是利用數(shù)據(jù)通信傳輸、電子傳感等技術對傳統(tǒng)運輸系統(tǒng)進行改進而形成的一種信息化、智能化、社會化的新型運輸系統(tǒng)，對于緩解當前大中城市的交通壓力起到了關鍵作用。自動駕駛技術的不斷發(fā)展，能夠有效減少交通事故，提高通行效率［1］。多車協(xié)同規(guī)劃是自動駕駛中的重要部分，在很大程度上決定了整個交通系統(tǒng)的智能程度［2］。協(xié)同規(guī)劃不同于單車運動軌跡的疊加［3］，主要是要求多車在行駛過程中考慮與其他車輛及環(huán)境之間的信息，通過規(guī)避障礙物且與其他車輛保持安全距離，最終到達各自的目的地［4-5］。自動駕駛的協(xié)同規(guī)劃能夠在提高安全性的同時，減少交通擁堵［6］。由于計算復雜度隨著車輛數(shù)量增加，且需要確保車輛間的協(xié)同，因此如何在復雜交通環(huán)境中規(guī)劃多車實時運動軌跡是亟需解決的問題［7-8］。

目前，國內(nèi)外學者已對自動駕駛車輛協(xié)同運動規(guī)劃展開研究。CHEN 等［9］根據(jù)優(yōu)先級順序?qū)Χ嘬囕v進行規(guī)劃，將高優(yōu)先級車輛視為低優(yōu)先級車輛的時變障礙物，計算復雜度隨著車輛數(shù)量線性縮放，由于該方法中的優(yōu)先級算法一次只規(guī)劃一輛車，因此當面對高度沖突的大規(guī)模規(guī)劃情況時，整個系統(tǒng)的協(xié)同能力不足。LI等［10］提出基于順序計算框架的增量約束動態(tài)優(yōu)化方法，通過引入更多的避碰約束使每一個子問題比前一個子問題更難，將每一個子問題的最優(yōu)解作為下一個子問題的初始解，直到得到最終解。由于該方法將原問題分散為多個部分逐步解決，因此無法確保在實際情況下能夠進行在線規(guī)劃。TAHIROVIC 等［11］提出一種基于雙層區(qū)域覆蓋的多車運動規(guī)劃算法，上層為每輛車粗略規(guī)劃行駛軌跡，下層對多車之間進行協(xié)調(diào)，為每輛車生成無碰撞軌跡。該算法由于協(xié)調(diào)過程不同時執(zhí)行，因此導致了解決方案質(zhì)量較差。HUANG 等［12］針對協(xié)同駕駛問題，提出一種基于人工勢場（Artificial Potential Field，APF）和模型預測的協(xié)同駕駛方法，設置成本函數(shù)，從多目標多約束優(yōu)化角度出發(fā)，很好地解決了自動駕駛運動規(guī)劃問題，但該方法未考慮軌跡的實時優(yōu)化。由上述研究可知，目前協(xié)同規(guī)劃能有效保證運行車輛與障礙物及其他車輛之間避免發(fā)生碰撞并保持安全距離，但車輛之間的協(xié)同能力以及在線規(guī)劃能力仍需進一步提升。

本文提出一種基于改進蟻群優(yōu)化（Ant Colony Optimization，ACO）算法的自動駕駛多車協(xié)同運動規(guī)劃方法。將蟻群優(yōu)化算法應用于自動駕駛車輛運動規(guī)劃中，并將多目標優(yōu)化函數(shù)引入蟻群優(yōu)化算法的信息素更新過程中，通過空間協(xié)同和軌跡優(yōu)化的目標函數(shù)對信息素進行實時更新，同時對蟻群優(yōu)化算法中的揮發(fā)因子進行自適應調(diào)整，以實現(xiàn)多車自動駕駛的在線協(xié)同運動規(guī)劃。

1 自動駕駛車輛運動學模型

為保證自動駕駛車輛實際行駛軌跡平滑，在進行自動駕駛運動規(guī)劃時需要滿足車輛運動學約束條件，對車輛運動學模型進行分析。自動駕駛車輛運動學模型如圖1 所示。

圖1 自動駕駛車輛運動學模型Fig.1 Kinematics model of autonomous driving vehicle

自動駕駛車輛運動學約束方程［13］如式（1）所示：

其中：(x，y)表示自動駕駛車輛后輪中點位置；φ表示轉(zhuǎn)角速度；θ表示前進方向；v表示車輛運行速度；L表示車輛前后輪軸距；u1、u2分別表示自動駕駛車輛加速度與前輪偏轉(zhuǎn)角速度。

2 改進的蟻群優(yōu)化算法

蟻群優(yōu)化算法［14］是指自然界中螞蟻可以在沒有任何提示的情況下找到一條最優(yōu)路徑，這主要是由于螞蟻在覓食路線上會分泌信息素［15］，隨著迭代次數(shù)的增多，螞蟻會在較短的路徑上積累更多的信息素，這種信息素會在其他螞蟻面臨覓食路線選擇時發(fā)出強烈信號進行準確的路線引導，從而大幅提高后續(xù)螞蟻選擇這條路線的概率，隨著該過程不斷重復，該算法構(gòu)成一個正反饋回路［16-17］。蟻群優(yōu)化算法主要包括概率選擇和信息素更新2 個步驟。

2.1 概率選擇

螞蟻通過輪盤賭選擇規(guī)則［18］進行下一個方向的選擇，螞蟻k從g走向下一個位置h的概率計算如下：

其中：Tgh為路徑(g，h)上的信息素量；allowedk為禁忌表之外螞蟻可以走的點；α為信息素因子；β為啟發(fā)因子，表示下一個節(jié)點對當前節(jié)點的影響程度。

ηgh為啟發(fā)函數(shù)［19］，計算公式如下：

其中：dgh為下一個可行節(jié)點到目標點的歐式距離。

兩個節(jié)點之間的歐氏距離定義如下：

其中：Tl=(Xl，Yl)表示目標點的坐標；Rh=(whx，why)表示下一個可行節(jié)點的坐標。

2.2 信息素更新

在所有螞蟻完成一次迭代后，路徑上的信息素進行全局更新，信息素更新公式如下［20］：

其中：ρ為信息素揮發(fā)因子，ρ∈(0，1)，(1-ρ)為信息素殘留因子；τgh(t)為當前時刻信息素濃度；Δτgh為信息素增量；為當前迭代周期螞蟻k的信息素增量；Q為常數(shù)，表示信息素強度；Lk表示循環(huán)結(jié)束時螞蟻k所走的路徑長度；tr表示螞蟻r已完成的任務集合。

將每一次迭代產(chǎn)生的最優(yōu)解與歷史最優(yōu)解進行對比，產(chǎn)生新的最優(yōu)解，在迭代次數(shù)達到所設定值時，輸出最優(yōu)路線［21］。

3 基于改進ACO 的自動駕駛多車協(xié)同規(guī)劃

采用蟻群優(yōu)化算法進行自動駕駛車輛的運動規(guī)劃，根據(jù)自動駕駛車輛數(shù)量產(chǎn)生多個螞蟻種群，各種群相互獨立并規(guī)劃相應的路線，在規(guī)劃過程中通過多目標優(yōu)化模型確保多輛汽車的協(xié)同性，對軌跡代價和空間協(xié)同約束進行數(shù)學建模，設立目標優(yōu)化函數(shù)，并將目標函數(shù)應用于信息素更新規(guī)則，確保多輛自動駕駛汽車的協(xié)同安全性與軌跡平滑性。

3.1 多目標優(yōu)化模型設計

3.1.1 空間協(xié)同代價與軌跡代價模型

車輛在進行運動規(guī)劃時，為保證安全性，首先應考慮車輛與障礙物之間無碰撞。假設整個協(xié)同環(huán)境中存在Nn輛車和Nobs個障礙物，設置車輛與障礙物之間的安全距離dsafe1。理想的最優(yōu)軌跡應適當與障礙物保持一定的距離，當車輛與障礙物之間的距離大于安全距離時，車輛與障礙物互不影響；當車輛與障礙物之間的距離小于安全距離時，應保持距離盡可能遠。車輛與障礙物空間協(xié)同函數(shù)的計算公式如下：

其中：dik表示車輛i與障礙物k之間的距離；κ1為距離系數(shù)，表示期望車輛i遠離障礙物k的程度。

車輛與障礙物之間的空間協(xié)同代價模型建立如下：

同樣地，在考慮車輛與障礙物之間無碰撞的前提下，多車協(xié)同運動規(guī)劃要求各車輛在行駛過程中應盡量保持距離。車輛與障礙物之間的安全距離為dsafe2，車輛間空間協(xié)同函數(shù)的計算公式如下：

其 中：dij為車輛i與車輛j之間的距離；κ2為距離系數(shù)，表示期望車輛i與車輛j之間互相遠離的程度。

車輛間的空間協(xié)同代價模型建立如下：

軌跡平滑性與前輪偏轉(zhuǎn)角的變化幅度有關，通過設置軌跡平滑程度的代價模型，表示車輛i在運動時域內(nèi)因車輪轉(zhuǎn)角變化而消耗的能量。軌跡代價函數(shù)的計算公式如下：

其中：φi(t)表示t時刻車輛i的前輪偏轉(zhuǎn)角；tf表示車輛到達終點的時刻。

3.1.2 目標函數(shù)優(yōu)化

基于上文描述，構(gòu)建自動駕駛協(xié)同控制的軌跡優(yōu)化數(shù)學模型，將空間協(xié)同代價模型與軌跡代價模型進行加權(quán)匯總，如式（13）所示，建立以空間協(xié)同代價模型與軌跡代價模型為目標函數(shù)的數(shù)學模型。

其中：f為綜 合代價；vi表 示車輛i的運行速度；vmax表示安全情況下車輛的最大運行速度；Φmax表示最大轉(zhuǎn)角速度；q1、q2、q3為各性能指標對應的權(quán)重系數(shù)，權(quán)重越大表示該部分在代價函數(shù)中越受重視，q1、q2、q3滿足非負性和歸一性

3.2 信息素更新規(guī)則改進

蟻群優(yōu)化算法作為一種啟發(fā)式智能算法，通過模擬自然界中螞蟻尋找食物的過程來尋找問題的最優(yōu)解［22］。螞蟻在覓食過程中會在經(jīng)過的道路上釋放信息素，后續(xù)經(jīng)過的螞蟻會根據(jù)所留下的信息素濃度對下一個位置進行選擇，因此信息素的分布對全局的規(guī)劃起著至關重要的作用［23］。采用蟻群優(yōu)化算法進行協(xié)同規(guī)劃，信息素對各車輛起著引導作用，決定汽車的運動方向與路線，直接影響車輛間的協(xié)同能力，因此本文對信息素進行改進。

上文通過構(gòu)造多目標優(yōu)化函數(shù)，考慮了空間協(xié)同代價與軌跡代價，能夠確保多車自動駕駛的自適應協(xié)同能力與安全性，同時又保證了乘車人的舒適度。因此，考慮將目標函數(shù)加入蟻群優(yōu)化算法的信息素更新規(guī)則中，改進的信息素更新規(guī)則如式（14）所示：

將多目標優(yōu)化所涉及空間協(xié)同代價和軌跡代價模型引入信息素更新過程中，通過多目標優(yōu)化函數(shù)所得的實時數(shù)據(jù)對信息素進行相應的懲罰和獎勵：若加入目標函數(shù)后的蟻群優(yōu)化算法出現(xiàn)更優(yōu)軌跡，則表示其對信息素具有正反饋，因此對信息素進行獎勵，增加信息素的導向性，即可加快算法收斂速度；若加入目標函數(shù)后得到軌跡較差，則表示對信息素具有負反饋，在信息素更新時進行懲罰，弱化最優(yōu)軌跡上所積累的信息素，給予次優(yōu)或較優(yōu)軌跡更大的被選擇概率，提高軌跡選擇的多樣性，避免陷入局部最優(yōu)。

3.3 揮發(fā)因子調(diào)整

蟻群優(yōu)化算法中固定的揮發(fā)因子ρ實際上是按一種固定不變的模式確定每次的路徑選擇概率以及更新的信息量，不能很好地保證自動駕駛車輛方向選擇的靈敏度以及軌跡的最優(yōu)。面對復雜多變的環(huán)境，需要提高各種群對路徑的全局搜索能力，提升算法實時性。

在蟻群優(yōu)化算法中揮發(fā)因子ρ始終保持不變，表示信息素的蒸發(fā)程度，反映了螞蟻個體之間的互相影響程度。若ρ設置不合理，螞蟻可能會喪失全局搜索能力，也會影響收斂速度。當ρ過小時，信息素揮發(fā)慢，若螞蟻一開始找到的是次優(yōu)路徑而非最優(yōu)路徑，受信息素揮發(fā)度的影響，次優(yōu)路徑的信息素逐漸積累，很難再找到最優(yōu)路徑，易陷入局部最優(yōu)解，影響算法的隨機性能；當ρ過大時，信息素揮發(fā)快，過多的無用搜索不利于螞蟻選擇下一個方向的路徑，搜索能力降低，收斂速度減慢。為提高自動駕駛車輛在實際運動規(guī)劃中的全局搜索能力以及收斂速度，對揮發(fā)因子的自適應更新調(diào)整如下：

其中：ρ∈(0，1)；s為當前迭代次數(shù)，表示s服從參數(shù)σ和μ的高斯函數(shù)，σ和μ為常數(shù)。

構(gòu)造的新的信息素揮發(fā)因子遵循高斯分布，可以提高螞蟻搜索的目的性以及搜索速度。高斯函數(shù)分布曲線如圖2 所示，當搜索開始時，揮發(fā)因子較小，路徑上信息素濃度對螞蟻影響較大，使得蟻群向信息素多的路徑靠攏。隨著信息素不斷更新，為避免陷入局部最優(yōu)，揮發(fā)因子取得較大值，螞蟻能夠自由搜索更多路徑。在搜索后期，為加快收斂速度，揮發(fā)因子取得一個較小值。經(jīng)過多次的迭代，螞蟻快速地集中于信息素濃度較高的路徑。

圖2 高斯函數(shù)分布曲線Fig.2 Gaussian function distribution curve

4 實驗與結(jié)果分析

為驗證本文方法的有效性，采用Matlab2019a 軟件進行仿真實驗，基于非結(jié)構(gòu)化道路以及結(jié)構(gòu)化道路場景進行運動規(guī)劃，非結(jié)構(gòu)化道路是指邊緣比較規(guī)則，沒有明顯車道線的行車道路，例如園區(qū)、學校等場所內(nèi)的道路，結(jié)構(gòu)化道路是指具有清晰道路標志線的行車道路，例如高速公路、城市干道等道路。實驗相關參數(shù)設置如表1 所示。

表1 實驗相關參數(shù)設置Table 1 Setting of experiment correlation parameters

4.1 非結(jié)構(gòu)化道路場景下的實驗結(jié)果與分析

在非結(jié)構(gòu)化道路場景下，為更好地描述動態(tài)障礙物在平面地圖中的運動，將時間軸視為運動狀態(tài)，使運動狀態(tài)沿著各個方向維度離散化，從而將整個場景的變化統(tǒng)一記錄在X-Y-T三維狀態(tài)空間中。在規(guī)格為20 m×20 m 的地圖中，靜態(tài)障礙物所在的位置不隨時間變化而移動，動態(tài)障礙物隨著時間序列進行移動。設置自動駕駛車輛數(shù)量為Nn，障礙物數(shù)量為Nobs，為驗證本文方法的有效性，在兩種不同道路場景（Nn=2 和Nobs=3、Nn=4 和Nobs=4）下進行仿真實驗。

設置自動駕駛車輛的起始點為A1，A2，…，An，終點為B1，B2，…，Bn。使用改進的蟻群優(yōu)化算法進行自動駕駛運動規(guī)劃，在兩種場景下所規(guī)劃的軌跡分別如圖3、圖4 所示。由圖3、圖4 可知，基于改進蟻群優(yōu)化算法的自動駕駛協(xié)同運動規(guī)劃能夠有效規(guī)避障礙物，并為各自動駕駛車輛規(guī)劃出無碰撞的平滑路線。各種群所代表的自動駕駛車輛在迭代過程中的綜合代價變化曲線如圖5、圖6 所示。由圖5、圖6 可知，隨著迭代次數(shù)的增加，綜合代價快速下降，表明改進的蟻群優(yōu)化算法在迭代初期能夠快速收斂，在經(jīng)過一定的迭代次數(shù)后，綜合代價逐漸趨于穩(wěn)定，最終收斂于特定值。

圖3 Nn=2 和Nobs=3 條件下協(xié)同運動規(guī)劃結(jié)果Fig.3 Results of collaborative motion planning under the conditions of Nn=2 and Nobs=3

圖4 Nn=4 和Nobs=4 條件下協(xié)同運動規(guī)劃結(jié)果Fig.4 Results of collaborative motion planning under the conditions of Nn=4 and Nobs=4

圖5 Nn=2 和Nobs=3 條件下綜合代價變化曲線Fig.5 Change curves of comprehensive cost under the condition of Nn=2 and Nobs=3

圖6 Nn=4 和Nobs=4 條件下綜合代價變化曲線Fig.6 Change curves of comprehensive cost under the condition of Nn=4 and Nobs=4

4.2 結(jié)構(gòu)化道路場景下的實驗結(jié)果與分析

構(gòu)建簡單結(jié)構(gòu)化道路場景進行仿真實驗，如圖7（a）～圖7（g）所示，3 輛自動駕駛車輛行駛在一條雙車道的直線道路上，設置初始速度v1=v2=v3=20 m/s，各自動駕駛車輛速度可變并根據(jù)環(huán)境做出適當?shù)目刂疲皂憫獎討B(tài)環(huán)境的變化。采用本文方法進行自動駕駛協(xié)同運動規(guī)劃，仿真結(jié)果如圖7（h）所示。在圖7 中，實線、虛線以及點劃線分別表示車輛1、車輛2、車輛3 的規(guī)劃路線，所生成的軌跡平滑，且各車輛之間保持協(xié)同控制并滿足約束條件。

圖7 結(jié)構(gòu)化道路場景下的協(xié)同運動規(guī)劃結(jié)果Fig.7 Result of collaborative motion planning under the scene of structured road

在模擬過程中，當T=0 s 時，各自動駕駛車輛分別位于0 m、40 m 以及80 m 處；當T=4 s 時，車輛2 繼續(xù)保持v2=20 m/s 的速度向前行駛，此時車輛1 由于速度的增加，距離車輛2 越來越近，為避免發(fā)生事故，車輛1 采取變道措施；當T=8 s 時，車輛1 完成變道，避免了與車輛2 發(fā)生碰撞；當T=16 s 時，由于車輛3 以低于車輛1 的速度前進，阻礙了車輛1 的行駛，車輛1 在保持安全的前提下繼續(xù)變道；當T=20 s時，由于車輛1 的變道，車輛2 若繼續(xù)保持勻速行駛，距離車輛1 過近，存在安全隱患，為確保行車安全，采取減速措施；當T=24 s 時，車輛1 完成變道。

結(jié)構(gòu)化道路場景中各自動駕駛車輛的綜合代價曲線如圖8 所示，在迭代初期，代價曲線的值快速下降，表明經(jīng)過迭代次數(shù)的增加，各種群已能規(guī)劃出較優(yōu)軌跡。在后續(xù)迭代過程中，綜合代價逐漸達到穩(wěn)定值。在仿真過程中，各車輛速度及加速度的變化如圖9（a）和圖9（b）所示，各自動駕駛車輛的速度根據(jù)動態(tài)環(huán)境做出自適應調(diào)整。

圖8 結(jié)構(gòu)化道路場景下的綜合代價變化曲線Fig.8 Change curves of comprehensive cost under the scene of structured road

圖9 車輛參數(shù)變化Fig.9 Changes of vehicle parameters

4.3 性能對比

為驗證本文方法性能，將其與APF 方法［12］進行對比。在相同參數(shù)設置條件下，通過代價函數(shù)對比協(xié)同收斂性能，分別記錄第i次迭代得到的代價函數(shù)均值Ji與初次迭代得到的代價函數(shù)均值Jo，并用Ji/Jo表示算法收斂速度。兩種方法在不同車輛數(shù)量情況下的收斂曲線如圖10 和圖11 所示，可以看出本文方法收斂速度明顯優(yōu)于APF 方法，且所得解的適應性更強，在復雜道路場景下，隨著自動駕駛車輛數(shù)量增多，代價函數(shù)均值比減小，且收斂于更小的值，表明車輛間協(xié)同效果更好。

圖10 Nn=3 時2 種方法的收斂曲線Fig.10 Convergence curves for two methods when Nn=3

圖11 Nn=6 時2 種方法的收斂曲線Fig.11 Convergence curves for two methods when Nn=6

5 結(jié)束語

本文針對復雜道路場景下的自動駕駛多車運動規(guī)劃問題，提出一種基于改進蟻群優(yōu)化算法的協(xié)同控制方法。在蟻群優(yōu)化算法的信息素更新過程中，引入以空間協(xié)同和軌跡代價為目標函數(shù)的多目標優(yōu)化模型，使各自動駕駛車輛保持協(xié)同控制，同時對蟻群優(yōu)化算法的揮發(fā)因子進行自適應調(diào)整，加快算法收斂速度。實驗結(jié)果表明，該方法為多輛自動駕駛汽車的協(xié)同運動規(guī)劃提供了一種有效的解決方案。后續(xù)將設計協(xié)同控制器，對多輛自動駕駛汽車的規(guī)劃路線進行跟蹤控制，并將本文方法應用于實際自動駕駛系統(tǒng)，進一步提升其穩(wěn)定性。