基于改進(jìn)蟻群優(yōu)化算法的自動(dòng)駕駛多車協(xié)同運(yùn)動(dòng)規(guī)劃

宋佳艷，蘇圣超

（上海工程技術(shù)大學(xué)電子電氣工程學(xué)院，上海 201620）

0 概述

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

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

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

1 自動(dòng)駕駛車輛運(yùn)動(dòng)學(xué)模型

為保證自動(dòng)駕駛車輛實(shí)際行駛軌跡平滑，在進(jìn)行自動(dòng)駕駛運(yùn)動(dòng)規(guī)劃時(shí)需要滿足車輛運(yùn)動(dòng)學(xué)約束條件，對(duì)車輛運(yùn)動(dòng)學(xué)模型進(jìn)行分析。自動(dòng)駕駛車輛運(yùn)動(dòng)學(xué)模型如圖1 所示。

圖1 自動(dòng)駕駛車輛運(yùn)動(dòng)學(xué)模型Fig.1 Kinematics model of autonomous driving vehicle

自動(dòng)駕駛車輛運(yùn)動(dòng)學(xué)約束方程［13］如式（1）所示：

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

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

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

2.1 概率選擇

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

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

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

其中：dgh為下一個(gè)可行節(jié)點(diǎn)到目標(biāo)點(diǎn)的歐式距離。

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

其中：Tl=(Xl，Yl)表示目標(biāo)點(diǎn)的坐標(biāo)；Rh=(whx，why)表示下一個(gè)可行節(jié)點(diǎn)的坐標(biāo)。

2.2 信息素更新

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

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

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

3 基于改進(jìn)ACO 的自動(dòng)駕駛多車協(xié)同規(guī)劃

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

3.1 多目標(biāo)優(yōu)化模型設(shè)計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)與結(jié)果分析

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

表1 實(shí)驗(yàn)相關(guān)參數(shù)設(shè)置Table 1 Setting of experiment correlation parameters

4.1 非結(jié)構(gòu)化道路場(chǎng)景下的實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

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

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

4.2 結(jié)構(gòu)化道路場(chǎng)景下的實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

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

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

4.3 性能對(duì)比

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

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

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

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

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