林 程，汪博文，呂沛原，宮新樂(lè)，2，于 瀟

（1.北京理工大學(xué)，電動(dòng)車輛國(guó)家工程研究中心，北京 100081；2.清華大學(xué)車輛與運(yùn)載學(xué)院，北京 100084）

前言

不合理的換道行為容易造成安全事故，道路曲率和環(huán)境車速的變化也會(huì)使風(fēng)險(xiǎn)成倍增加，嚴(yán)重影響道路交通安全及通行效率。隨著車路云一體化融合系統(tǒng)的發(fā)展，打破了車輛間僅通過(guò)自身傳感器獲取周圍環(huán)境及自身信息的局限性。通過(guò)網(wǎng)聯(lián)化技術(shù)，建立自動(dòng)駕駛多車系統(tǒng)協(xié)同運(yùn)動(dòng)規(guī)劃方法，可以實(shí)現(xiàn)駕駛安全減少交通擁堵［1］。近年來(lái)，自動(dòng)駕駛運(yùn)動(dòng)規(guī)劃問(wèn)題受到了廣泛關(guān)注［2］?，F(xiàn)有的換道合流研究中大多聚焦于車輛縱向控制，并依靠于簡(jiǎn)化的質(zhì)點(diǎn)模型，其可以很好地應(yīng)用于直道場(chǎng)景中，而在變曲率道路下仍有很大挑戰(zhàn)。

在變曲率道路上進(jìn)行多車協(xié)同運(yùn)動(dòng)規(guī)劃須綜合考慮合流時(shí)機(jī)和橫向運(yùn)動(dòng)控制。目前針對(duì)選取合適合流時(shí)機(jī)問(wèn)題，傳統(tǒng)智能車輛中，基于場(chǎng)景驅(qū)動(dòng)的決策控制方法被廣泛采用。其主要是根據(jù)指定的判斷條件，如安全距離和碰撞時(shí)間（time to collision，TTC）等，進(jìn)行自適應(yīng)巡航（adaptive cruise control，ACC）、換道避障（collision avoidance，OA ）和緊急制動(dòng)（autonomous emerging braking，AEB）等駕駛模式的切換［3］。文獻(xiàn)［4］中通過(guò)考慮不同道路、車輛橫縱向狀態(tài)來(lái)對(duì)周車環(huán)境風(fēng)險(xiǎn)進(jìn)行評(píng)估和決策。另外，網(wǎng)聯(lián)化技術(shù)的發(fā)展衍生了協(xié)同自適應(yīng)巡航技術(shù)（cooperative adaptive cruise control，CACC），其通過(guò)綜合考慮多車信息，可以更加安全地實(shí)現(xiàn)車輛跟馳和換道等動(dòng)作［5］。然而，人類復(fù)雜的個(gè)性化駕駛行為難以簡(jiǎn)單地用既定規(guī)則進(jìn)行描述，多車協(xié)同駕駛下的高收益的駕駛決策仍須被進(jìn)一步探索。

隨著硬件計(jì)算能力的逐漸提高，優(yōu)化類的方法如模型預(yù)測(cè)控制（model predictive control，MPC）［6-7］，因其能夠處理多約束多目標(biāo)問(wèn)題，被嘗試用于求解高收益的協(xié)同駕駛多車決策。其核心思想是將協(xié)同運(yùn)動(dòng)規(guī)劃問(wèn)題轉(zhuǎn)化為一個(gè)具有多約束的非線性優(yōu)化問(wèn)題［8］，通過(guò)求解可以得到最優(yōu)決策控制策略。求解該問(wèn)題的方法可以分為集中式和分布式兩類架構(gòu)。集中式的方法依賴于一個(gè)中心協(xié)調(diào)單元，能夠得到系統(tǒng)最優(yōu)的協(xié)同控制策略，但是往往須具備較高的算力［9］。例如，結(jié)合期望車道等離散變量，可以將多車換道模型轉(zhuǎn)化為一個(gè)帶約束的集中式混合邏輯動(dòng)態(tài)（mix logical dynamic，MLD）系統(tǒng)［10］，通過(guò)混合整數(shù)規(guī)劃來(lái)進(jìn)行最優(yōu)換道決策求解。分布式的方法不需要中心單元，只需自身及周圍環(huán)境信息，在實(shí)際中更易于部署［11］。如文獻(xiàn)［12］中，提出了一種隨機(jī)MPC 換道決策與控制框架，通過(guò)獲得其他車輛狀態(tài)進(jìn)行概率預(yù)測(cè)并得到換道最優(yōu)運(yùn)動(dòng)控制。文獻(xiàn)［13］中利用魯棒MPC 實(shí)現(xiàn)了參數(shù)不確定下的無(wú)偏置多車運(yùn)動(dòng)控制。另外，與其他經(jīng)典交互模型相比，采用基于博弈的換道模型能對(duì)人類多車交互行為進(jìn)行更個(gè)性化的描述［14］。博弈論，如納什均衡［15］和斯塔克伯格博弈［16］，通常采用分布式的架構(gòu)來(lái)搭建車輛決策模型，兩種非合作決策博弈方法的比較在文獻(xiàn)［17］中列出。

機(jī)器學(xué)習(xí)方法如強(qiáng)化學(xué)習(xí)（reinforcement learning，RL）［18］、深度強(qiáng)化學(xué)習(xí)（deep reinforcement learning，DRL）等［19］在近幾年發(fā)展迅速，其有在復(fù)雜環(huán)境中進(jìn)行準(zhǔn)確決策的潛力。文獻(xiàn)［20］中提出了一種基于深度強(qiáng)化學(xué)習(xí)的多車協(xié)同決策框架，文獻(xiàn)［21］中采用模仿學(xué)習(xí)和強(qiáng)化學(xué)習(xí)相結(jié)合的方式進(jìn)行換道決策。由于模型的不可解釋性和對(duì)數(shù)據(jù)的依賴，RL 算法較難在實(shí)際車輛中使用。為此，文獻(xiàn)［22］中提出了一種優(yōu)化嵌入式強(qiáng)化學(xué)習(xí)，以實(shí)現(xiàn)多車自適應(yīng)決策。文獻(xiàn)［23］中設(shè)計(jì)了一種混合強(qiáng)化學(xué)習(xí)算法，結(jié)合物理模型和歷史數(shù)據(jù)得到最優(yōu)策略?？偟膩?lái)說(shuō)，機(jī)器學(xué)習(xí)方法能夠很好地處理決策問(wèn)題，尤其適用單車智能，但在多車協(xié)同的場(chǎng)景仍須進(jìn)一步地研究。

另一方面，道路曲率變化下橫向運(yùn)動(dòng)控制至關(guān)重要，車輛位姿和狀態(tài)的描述是進(jìn)行協(xié)同決策的基礎(chǔ)。為了方便描述系統(tǒng)內(nèi)多車之間的關(guān)系，虛擬隊(duì)列方法，即將處于不同車道上的車輛近似看作成一個(gè)車道上的隊(duì)列，常被用于沖突區(qū)域下的協(xié)同合流控制之中［24-25］。而在這些研究中，通常將變化的曲率道路投影視作直線，或默認(rèn)忽略不計(jì)，且較少考慮非線性橫向動(dòng)力學(xué)的影響。為了消除曲率變化帶來(lái)的影響，文獻(xiàn)［26］中利用道路中心坐標(biāo)來(lái)描述車輛的運(yùn)動(dòng)，并設(shè)計(jì)了一個(gè)面向超車避障的模型預(yù)測(cè)控制器，但是僅適用于單車決策控制。

不同于僅考慮單車或多車集中式協(xié)同控制的方法，本文綜合考慮動(dòng)態(tài)道路曲率變化和周車運(yùn)動(dòng)狀態(tài)，提出一種基于博弈的多車分布式換道合流運(yùn)動(dòng)規(guī)劃與協(xié)同控制方法。為了準(zhǔn)確描述車輛位置姿態(tài)與道路之間的關(guān)系，本文首先分析了曲率坐標(biāo)系下車輛橫縱向運(yùn)動(dòng)對(duì)車間安全的影響機(jī)制，建立了多車交互動(dòng)力學(xué)模型。之后，提出了適配變曲率道路的多車博弈速度規(guī)劃算法，設(shè)計(jì)兩步驟分布式地求解兼顧安全、效率、舒適多目標(biāo)最優(yōu)的速度軌跡及換道時(shí)機(jī)，縮小優(yōu)化問(wèn)題求解規(guī)模，提高計(jì)算求解效率。最后，通過(guò)變頻率采樣B 樣條曲線識(shí)別道路曲率，構(gòu)建基于前步最優(yōu)控制反饋的自適應(yīng)時(shí)變模型預(yù)測(cè)控制算法（adaptive time-varying MPC，ATVMPC）實(shí)現(xiàn)軌跡跟蹤，通過(guò)每一步實(shí)時(shí)迭代更新控制矩陣和狀態(tài)矩陣，減少了縱向車速和曲率變化帶來(lái)的控制偏差累積。聯(lián)合仿真結(jié)果表明，提出的方法能夠更好地適應(yīng)道路環(huán)境的變化，較傳統(tǒng)方法能夠選取高效益的合流時(shí)機(jī)并準(zhǔn)確地跟蹤目標(biāo)軌跡。

1 曲率坐標(biāo)系下多車系統(tǒng)模型

1.1 曲率坐標(biāo)系

合理地描述車輛姿態(tài)與道路之間的相對(duì)位置關(guān)系對(duì)自動(dòng)駕駛運(yùn)動(dòng)規(guī)劃起著重要作用。傳統(tǒng)笛卡爾坐標(biāo)系往往是基于大地坐標(biāo)，通常需要從大地坐標(biāo)系轉(zhuǎn)化為車體坐標(biāo)系進(jìn)行車輛狀態(tài)計(jì)算。當(dāng)車輛沿著曲率道路行駛時(shí)，大地坐標(biāo)系下的坐標(biāo)X和坐標(biāo)Y具有極強(qiáng)的非線性關(guān)系，使車身姿態(tài)的描述更加復(fù)雜，因此采用曲率坐標(biāo)系來(lái)描述車輛在全局道路下的位姿。圖1 展示了不同坐標(biāo)系之間的關(guān)系，其中曲率坐標(biāo)系以給定的任意曲率參考線為基線，以沿著基線走過(guò)的距離作為縱向位移si，以垂直于該基線的誤差作為橫向偏移ey，i。定義車輛i在曲率坐標(biāo)系下的位置坐標(biāo)向量為

圖1 不同坐標(biāo)系之間的車輛狀態(tài)關(guān)系

式中eψ，i表示為曲率坐標(biāo)系下的橫擺角。圖1 中：vx，i和vy，i代表在車身坐標(biāo)系下的車輛橫向和縱向速度；ψd，i為大地坐標(biāo)系下的車輛橫擺角為對(duì)應(yīng)的橫擺角速度；ψs，i=ψd，i-eψ，i表示中心線切線與大地坐標(biāo)系橫軸的夾角表示相應(yīng)的橫擺角速度；δf，i表示車輛的前輪轉(zhuǎn)向角；Fxf，i和Fxr，i為前后輪縱向力；Fyf，i和Fyr，i為前后輪的橫向力；lf，i和lr，i分別代表質(zhì)心到前軸和后軸的距離；βi為大地坐標(biāo)系下的質(zhì)心側(cè)偏角；αf，i和αr，i表示車輛的前后輪胎側(cè)偏角。

1.2 車輛動(dòng)力學(xué)模型

變曲率道路下須綜合考慮車輛的橫縱向動(dòng)力學(xué)性能。車輛的動(dòng)力學(xué)特性表現(xiàn)為外界作用力對(duì)車輛的影響，分別定義車輛i的動(dòng)力學(xué)狀態(tài)向量xi和控制向量ui為

式中ai表示車輛坐標(biāo)系下的縱向加速度。為了更好地考慮變曲率道路下的車輛動(dòng)力學(xué)特性，以適應(yīng)動(dòng)態(tài)工況下的精確跟蹤控制，保證多車協(xié)同駕駛的安全性和穩(wěn)定性，采用如下橫縱向動(dòng)力學(xué)模型對(duì)狀態(tài)進(jìn)行描述，其中各個(gè)狀態(tài)之間的幾何關(guān)系可以在圖1中得到。

式中：mi為汽車質(zhì)量；Iz，i表示車體轉(zhuǎn)動(dòng)慣量。

由于變曲率道路下輪胎發(fā)生側(cè)偏和滑移的概率較大，輪胎力表現(xiàn)出高度非線性橫向動(dòng)力學(xué)特征，一個(gè)高保真的非線性輪胎模型對(duì)車輛的精確控制至關(guān)重要。當(dāng)側(cè)偏角較大時(shí)，輪胎力的非線性特征明顯，由于魔術(shù)輪胎公式能準(zhǔn)確地?cái)M合不同工況下的輪胎力學(xué)特性，被廣泛應(yīng)用于車輛在極端工況下的輪胎力描述。當(dāng)側(cè)偏角較小時(shí)，線性輪胎模型可以很好地描述輪胎橫向力與側(cè)偏角之間的線性關(guān)系，由于其計(jì)算簡(jiǎn)單通常應(yīng)用于一般工況的車輛控制中。綜合線性輪胎模型和魔術(shù)輪胎公式的優(yōu)勢(shì)，采用如下混合輪胎模型對(duì)前后輪胎側(cè)向力進(jìn)行建模：

當(dāng)車速較低（＜6 m/s）且前輪轉(zhuǎn)角變化率較小時(shí)，為了降低模型計(jì)算復(fù)雜度，同時(shí)也避免車輛動(dòng)力學(xué)模型在車速為零時(shí)產(chǎn)生奇點(diǎn)的問(wèn)題，采用簡(jiǎn)化的車輛動(dòng)力學(xué)模型如下：

式中βi的計(jì)算表達(dá)式為

1.3 多車交互行為描述

為了更好地在曲率道路下描述車輛動(dòng)力學(xué)特性及多車間的相對(duì)位置關(guān)系，本部分首先推導(dǎo)曲率坐標(biāo)系下的縱向車速橫向車速和橫擺角速度的表達(dá)式。如圖2 所示，先將有橫向偏差的車輛投影至基線上，根據(jù)基線上的投影和實(shí)際所在位置之間的相對(duì)關(guān)系可得：

圖2 曲率坐標(biāo)下車間相對(duì)位置關(guān)系

式中k(si)表示曲率坐標(biāo)下沿著縱向位移si的曲率變化函數(shù)，其具體計(jì)算方法將在第3 部分進(jìn)行詳細(xì)分析。圖中(Xc，Yc)代表車身坐標(biāo)系。帶有上標(biāo)“＇”的參數(shù)表示在基線上的投影。

其次，當(dāng)在變曲率的道路環(huán)境中存在多輛車時(shí)，傳統(tǒng)笛卡爾坐標(biāo)系下常采用歐幾里得范數(shù)的方法來(lái)描述車輛之間的安全關(guān)系，即

當(dāng)虛擬隊(duì)列間存在dr≤dsafe時(shí)，說(shuō)明此時(shí)車輛i和j在基線上的投影存在重疊，此時(shí)換道會(huì)有潛在碰撞危險(xiǎn)。

為了方便后續(xù)描述，綜合姿態(tài)向量pi和動(dòng)力學(xué)向量xi，將車輛i的狀態(tài)統(tǒng)一表示為

2 多車博弈運(yùn)動(dòng)規(guī)劃方法

2.1 多車拓?fù)渫ㄐ艠?gòu)型

車間信息流拓?fù)淠Ｐ蛯?duì)多車系統(tǒng)的協(xié)同控制有著重要影響。本文中，研究對(duì)象為多輛自動(dòng)駕駛汽車，且假設(shè)車車通信的信道是理想的，不存在延時(shí)和丟包的情況，重點(diǎn)檢驗(yàn)基于博弈的控制方法對(duì)多車系統(tǒng)綜合性能的積極影響。合流區(qū)域采取的通信拓?fù)涿枋鋈鐖D3 所示，每輛跟隨車輛可以獲取對(duì)應(yīng)領(lǐng)導(dǎo)車輛的狀態(tài)信息，并通過(guò)車路協(xié)同的設(shè)備對(duì)周圍其他環(huán)境車輛信息進(jìn)行信息補(bǔ)償。

圖3 通信拓?fù)涫疽?/p>

2.2 多車博弈系統(tǒng)描述

為保證合流過(guò)程中車輛行駛安全和系統(tǒng)整體道路的通行效率，多車協(xié)同換道合流的控制目標(biāo)為規(guī)劃出理想的縱向速度軌跡并選擇合適的合流時(shí)機(jī)，并最終保持安全間距以及按照期望的車速行駛。當(dāng)不存在外界擾動(dòng)時(shí)，對(duì)于車輛i，j∈N，協(xié)同控制的目標(biāo)可以表示為

基于上述控制目標(biāo)，將變曲率道路下的多車協(xié)同速度規(guī)劃問(wèn)題轉(zhuǎn)化為多車博弈問(wèn)題，博弈的目標(biāo)是總代價(jià)最小。一個(gè)博弈問(wèn)題G主要由3 個(gè)部分組成：

式中：N 代表車輛集合；A 代表車輛動(dòng)作集合；πi表示第i個(gè)參與者的策略集；J 代表車輛代價(jià)集合。本文在速度規(guī)劃算法中綜合考慮了自動(dòng)駕駛汽車的個(gè)性化駕駛偏好及整體通行效率，通過(guò)駕駛安全性、乘坐舒適性和通行效率的比例來(lái)進(jìn)行表征，同時(shí)也作為博弈收益組成的重要參考。

首先，駕駛安全性的代價(jià)由自車與環(huán)境車之間的碰撞時(shí)間倒數(shù)和安全相對(duì)距離兩部分組成，并通過(guò)系數(shù)來(lái)調(diào)整各部分權(quán)重。其中，將碰撞時(shí)間倒數(shù)定義為

式中Δsij和Δvs，ij分別代表車輛i和車輛j在曲率坐標(biāo)系下的相對(duì)距離和速度?；谑剑?4），將駕駛安全代價(jià)寫(xiě)作

式中kΔt和kΔs為碰撞時(shí)間因子和相對(duì)距離因子的權(quán)重系數(shù)；H(?)為階躍函數(shù)，如式（16）所示。

式中κ∈R代表函數(shù)H(?)的自變量參數(shù)。

為保證系統(tǒng)內(nèi)車輛能按照期望車速運(yùn)行，快速到達(dá)合流點(diǎn)并完成合流動(dòng)作，減少速度損失，本文定義效率代價(jià)：

乘坐舒適性通常與車輛加速度和加速度增量相關(guān)，定義乘坐舒適性系數(shù)為

式中Δai表示車輛的加速度增量。

結(jié)合式（14）～式（18）綜合考慮車輛安全、舒適、效率，構(gòu)建車輛i博弈總體代價(jià)函數(shù)Ji：

2.3 多車分布式博弈算法

多車博弈的目標(biāo)是選擇合適的時(shí)機(jī)進(jìn)行合流并得到使所有交通參與者代價(jià)最小的速度曲線。對(duì)應(yīng)的最優(yōu)聯(lián)合策略集合為A*，表示為

需要說(shuō)明的是，將未來(lái)N步的多車預(yù)測(cè)狀態(tài)也考慮在內(nèi)，因此得到的結(jié)果是未來(lái)一段時(shí)間內(nèi)的納什均衡，最終得到的最優(yōu)動(dòng)作策略同樣是帶時(shí)間信息的狀態(tài)序列：

為了得到車輛i的納什均衡策略將預(yù)測(cè)最小博弈代價(jià)問(wèn)題轉(zhuǎn)化為N步預(yù)測(cè)域下的有限時(shí)域約束優(yōu)化問(wèn)題：

式中：zi=[si，vs，i]T；Ai和Bi為自車i狀態(tài)矩陣和控制矩陣；l代表虛擬隊(duì)列中車輛i的前方車輛。當(dāng)在t時(shí)刻車輛i前方無(wú)車輛時(shí)定義sl，vs，l=+∞，同時(shí)將匝道合流點(diǎn)視作在車道處的靜止障礙物為由簡(jiǎn)化的運(yùn)動(dòng)學(xué)模型推導(dǎo)得到的其他車輛的狀態(tài)矩陣和控制矩陣；amin、Δamin和amax、Δamax分別為加速度控制量和控制增量的上下邊界。

為進(jìn)一步求解得到多車系統(tǒng)的納什均衡策略集A*，本文構(gòu)建了多車分布式換道博弈速度規(guī)劃算法來(lái)對(duì)式（23）進(jìn)行求解。如算法1 所示（表1），分兩個(gè)主要步驟來(lái)得到最優(yōu)速度控制策略和換道時(shí)機(jī)。首先，通過(guò)單步求解算法1中第4行的標(biāo)準(zhǔn)二次優(yōu)化問(wèn)題得到系統(tǒng)內(nèi)每一輛車的最優(yōu)速度控制策略然后，基于該策略和式（19）計(jì)算換道時(shí)刻在t～t+N-1時(shí)的綜合代價(jià)集合：

式（24）集合中最小代價(jià)對(duì)應(yīng)的時(shí)刻即作為車輛i的最優(yōu)換道動(dòng)機(jī)不同于集中式的優(yōu)化方法，分布式的優(yōu)化方法的核心在于采用多個(gè)節(jié)點(diǎn)來(lái)優(yōu)化全局目標(biāo)函數(shù)，算法中節(jié)點(diǎn)的含義對(duì)應(yīng)系統(tǒng)內(nèi)的每一輛車。通過(guò)節(jié)點(diǎn)間的相互信息交換，使得所有節(jié)點(diǎn)上的局部目標(biāo)函數(shù)之和最終收斂于全局目標(biāo)函數(shù)的最小值。因此基于算法1，最終可以得到系統(tǒng)納什均衡的速度控制序列和換道時(shí)刻

3 多車橫向運(yùn)動(dòng)控制方法

3.1 基于B-樣條的曲率計(jì)算及路徑規(guī)劃

道路曲率的準(zhǔn)確識(shí)別和預(yù)測(cè)是影響控制因素的關(guān)鍵之一，一般可通過(guò)采樣計(jì)算來(lái)得到道路曲率，此外，也可通過(guò)擬合曲線進(jìn)行近似計(jì)算。B 樣條曲線是B-樣條基數(shù)函數(shù)的線性組合，相比于貝塞爾曲線，B 樣條曲線能指定階數(shù)，并且在改變某個(gè)控制點(diǎn)情況下不會(huì)影響大范圍的曲線變化。基于上述特征，合理設(shè)置B 樣條曲線控制點(diǎn)個(gè)數(shù)能夠準(zhǔn)確地估計(jì)道路曲率，同時(shí)可預(yù)測(cè)未來(lái)曲率狀態(tài)。然而由于道路曲率實(shí)時(shí)多變，且偶爾會(huì)伴隨著突變的發(fā)生，增加了曲率識(shí)別的難度，為了能夠在變曲率道路上快速地識(shí)別曲率并規(guī)劃出參考軌跡，本文設(shè)計(jì)了基于B 樣條曲線的自適應(yīng)采樣方法來(lái)計(jì)算和預(yù)測(cè)曲率，如圖4所示。定義生成的k階B樣條曲線如下：

圖4 曲率識(shí)別和路徑規(guī)劃示意

式中：P0～Pn表示為在車道保持和換道合流兩個(gè)典型場(chǎng)景下的采樣點(diǎn)，即可定義樣條線曲線的走向和界限范圍的控制點(diǎn)；λ是一組由非遞減的連續(xù)變化值構(gòu)成的節(jié)點(diǎn)矢量；Bi，k(λ)是第i個(gè)k階B 樣條基函數(shù)，其具有德布爾-考克斯遞推式，見(jiàn)式（26）。

為了使擬合曲線盡可能地逼近真實(shí)值同時(shí)減少計(jì)算采樣的頻率，本文采用變頻率控制點(diǎn)采樣的方法生成B 樣條參考曲線，即當(dāng)車輛檢測(cè)到當(dāng)前路段的曲率變化較小時(shí)，進(jìn)行低頻采樣；當(dāng)預(yù)測(cè)到未來(lái)路段曲率將增大時(shí)，轉(zhuǎn)化為高頻率采樣。由于B 樣條曲線的局部支撐性和可微性，增加更改控制點(diǎn)可以僅改變局部的曲線，整體曲線不會(huì)受到很大影響。因此，自適應(yīng)采樣的方法可以保證實(shí)時(shí)識(shí)別曲線的光滑性和完整度，以及曲率的準(zhǔn)確性和連續(xù)性。

基于上述討論，定義通過(guò)B 樣條曲線擬合的N步預(yù)測(cè)域下的橫向參考軌跡為

3.2 自適應(yīng)時(shí)變模型預(yù)測(cè)控制算法

道路曲率變化給車輛循跡控制增加了難度，尤其需要執(zhí)行橫向運(yùn)動(dòng)控制時(shí)，需要綜合考慮輪胎的動(dòng)力學(xué)因素、曲率和縱向車速的變化。在傳統(tǒng)線性模型預(yù)測(cè)控制中，狀態(tài)方程中的狀態(tài)矩陣和控制矩陣均為常矩陣，即將車速或輪胎剛度設(shè)為定值。這種方法可以很好地應(yīng)對(duì)一般工況，而當(dāng)?shù)缆非首兓瘜?dǎo)致車輛表現(xiàn)出非線性動(dòng)力學(xué)特征時(shí)，預(yù)測(cè)模型則無(wú)法真實(shí)表達(dá)車輛狀態(tài)，進(jìn)而會(huì)造成較大的控制誤差，甚至?xí)l(fā)算法失效。為此，本文設(shè)計(jì)了自適應(yīng)時(shí)變預(yù)測(cè)控制算法對(duì)車輛進(jìn)行協(xié)同控制。

首先，通過(guò)泰勒展開(kāi)式線性化車輛模型：

基于式（28），定義時(shí)變狀態(tài)矩陣Actrl，i和控制矩陣Bctrl，i為

式（29）可表示為標(biāo)準(zhǔn)狀態(tài)方程形式：

進(jìn)一步將式（30）離散化。下面以第k步的預(yù)測(cè)為例進(jìn)行描述，離散化式（30）后可以預(yù)測(cè)第k+1步的車輛狀態(tài)增量為

對(duì)于車輛名義狀態(tài)量和控制量的預(yù)測(cè)更新，通過(guò)前向歐拉公式可以得到離散方程。對(duì)于車輛i在時(shí)刻t下第k+1步的名義狀態(tài)量和控制量可表示為

基于式（31）和式（32），預(yù)測(cè)估計(jì)車輛i在時(shí)刻t下的第k+1步狀態(tài)ξi和輸入ui：

進(jìn)一步地，將跟蹤問(wèn)題轉(zhuǎn)化為一個(gè)N步預(yù)測(cè)域的標(biāo)準(zhǔn)凸約束優(yōu)化問(wèn)題：

上述過(guò)程只能保證在單步計(jì)算過(guò)程中得到最優(yōu)控制。為了能夠降低曲率、車速等參數(shù)的連續(xù)不斷變化造成的控制誤差，本文中提出了如圖5 所示的自適應(yīng)時(shí)變模型預(yù)測(cè)控制算法（ATV-MPC）。其特點(diǎn)在于，基于上一步預(yù)測(cè)得到的最優(yōu)控制，在每一時(shí)刻t下迭代更新?tīng)顟B(tài)矩陣和控制矩陣從而減少曲率、車速頻繁變化帶來(lái)的控制偏差累積。在當(dāng)前時(shí)刻計(jì)算結(jié)束后，將中第一個(gè)元素作為輸入作用于被控車輛之中。

圖5 自適應(yīng)時(shí)變預(yù)測(cè)算法流程

4 仿真結(jié)果與分析

4.1 仿真平臺(tái)及框架

為了驗(yàn)證所提出的面向變曲率道路的多車協(xié)同控制算法的有效性，本文基于MATLAB 和CarSim 的聯(lián)合仿真平臺(tái)進(jìn)行算法實(shí)現(xiàn)和場(chǎng)景搭建，整體算法的框架流程如圖6 所示。本文構(gòu)建了一個(gè)單向雙車道的變曲率道路場(chǎng)景，并選用4 輛自動(dòng)駕駛汽車作為實(shí)驗(yàn)對(duì)象。此外，設(shè)計(jì)了車道保持和換道合流兩種工況進(jìn)行驗(yàn)證。

圖6 整體算法框架

預(yù)測(cè)控制中的時(shí)域大小對(duì)計(jì)算效率和控制效果有很大影響，時(shí)域過(guò)短會(huì)削弱預(yù)測(cè)控制的精度；過(guò)長(zhǎng)的時(shí)域則會(huì)導(dǎo)致預(yù)測(cè)模型誤差增大，計(jì)算效率下降，難以達(dá)到理想的控制效果。因此，凸優(yōu)化問(wèn)題的預(yù)測(cè)時(shí)長(zhǎng)通常選擇在0.6～1 s 左右［27-28］。本文選擇預(yù)測(cè)時(shí)長(zhǎng)為1 s，其中預(yù)測(cè)域N=10，步長(zhǎng)ts=0.1 s。其他參數(shù)如表2所示。

表2 仿真參數(shù)設(shè)置

4.2 車道保持性能仿真

為了評(píng)估橫向運(yùn)動(dòng)控制算法的性能，在設(shè)置的車道保持場(chǎng)景中，4 輛智能網(wǎng)聯(lián)車均設(shè)置在同一車道上行駛，并采用經(jīng)典的斯坦利（Stanley）算法和未采用變頻率采樣B 樣條曲率計(jì)算的時(shí)變預(yù)測(cè)控制算法進(jìn)行對(duì)比。

圖7 展示了協(xié)同控制算法對(duì)測(cè)試道路的曲率識(shí)別結(jié)果。對(duì)比算法為未采用變頻率采樣B 樣條曲線曲率計(jì)算方法，可以看出當(dāng)曲率發(fā)生較大變化時(shí)，容易產(chǎn)生劇烈波動(dòng)，曲率是計(jì)算控制量的重要參數(shù)，數(shù)據(jù)的頻繁變化會(huì)嚴(yán)重影響控制精度。相比較，提出的算法能夠根據(jù)B 樣條曲線顯示地預(yù)測(cè)未來(lái)曲率變化從而優(yōu)化采樣頻率，消除了數(shù)據(jù)的震蕩，減少了對(duì)后續(xù)控制的影響，相關(guān)結(jié)論也可在圖8中看出。

圖7 曲率識(shí)別計(jì)算結(jié)果

圖8 橫向跟蹤結(jié)果

圖8 為本文提出的橫向跟蹤控制算法在變曲率道路上的仿真結(jié)果，圖9 為仿真過(guò)程中的轉(zhuǎn)角變化。隨著道路曲率的增大，3 種算法的跟蹤誤差都會(huì)有所增加。經(jīng)典Stanley 算法由于未系統(tǒng)地考慮車輛橫向動(dòng)力學(xué)，控制誤差較大。相比較而言，文中提出的基于最優(yōu)狀態(tài)反饋的時(shí)變預(yù)測(cè)控制算法能夠減少58%的跟蹤誤差。同時(shí)可知，更加精確和平滑的連續(xù)曲率能夠降低控制過(guò)程中轉(zhuǎn)角的頻繁波動(dòng)，是減少控制誤差的關(guān)鍵。

圖9 前輪輸入轉(zhuǎn)向角

4.3 換道合流性能仿真

本部分搭建的實(shí)驗(yàn)場(chǎng)景用于檢驗(yàn)變曲率道路下協(xié)同控制算法的換道合流性能。道路曲率變化與場(chǎng)景1 設(shè)置相同。圖10 為CarSim 中的仿真時(shí)刻截圖，在距離起點(diǎn)50 m 處增加了道路合流點(diǎn)，車輛#1和#3被設(shè)置在右車道，車輛#2 和#4 在左車道。為了便于觀察，圖11 展示了各車在曲率坐標(biāo)下的相對(duì)位置?？刂颇繕?biāo)是使道路中所有車輛高效安全地通過(guò)設(shè)定的合流點(diǎn)。測(cè)試中選用集中式MPC 算法和協(xié)同自適應(yīng)巡航算法（CACC）進(jìn)行比較。

圖10 CarSim仿真時(shí)刻場(chǎng)景圖

圖11 曲率坐標(biāo)系下投影的時(shí)刻場(chǎng)景圖

圖12～圖15 展示了在該場(chǎng)景下3 種協(xié)同控制算法下的車輛狀態(tài)變化。圖12 給出了車輛相對(duì)距離的變化大小，從投影時(shí)刻圖11 和圖12 中可以看出，車輛#4 起始的質(zhì)心處相對(duì)距離均在4～5 m 左右，在本文提出的速度規(guī)劃方法的作用下，相對(duì)距離逐漸增大，并最終能夠穩(wěn)定安全地達(dá)到理想的相對(duì)距離。相比較而言，集中式MPC 的方法也能夠更快地實(shí)現(xiàn)類似的效果。CACC 算法盡管也能最終實(shí)現(xiàn)期望值，但是在控制過(guò)程中會(huì)造成較大波動(dòng)。

圖12 相對(duì)距離變化結(jié)果

圖13顯示在3種算法下的碰撞時(shí)間倒數(shù)均維持在-0.2～0.2 范圍內(nèi)，由文獻(xiàn)［29］中可知均處于安全狀態(tài)。而提出方法下各車碰撞時(shí)間倒數(shù)值略低于另外兩種方法，且數(shù)據(jù)分布更加集中，能夠?qū)崿F(xiàn)更加安全穩(wěn)定的多車協(xié)同駕駛。

圖13 碰撞時(shí)間倒數(shù)結(jié)果

類似的結(jié)論也可以從圖14 車速變化和圖15 加速度變化中得到。車輛#1 處于最靠近合流點(diǎn)的位置，故3 種算法下車輛#1 和車輛#3 先加速，車輛#2和車輛#4 減速來(lái)創(chuàng)造更多安全換道的空間。之后，在速度調(diào)節(jié)算法的作用下，均趨向于理想車速。由圖可知，無(wú)論是集中式還是分布式，本文中提出基于博弈的速度規(guī)劃方法能夠使各車車速平穩(wěn)地變化到期望車速。而在CACC 方法下各車速度和加速度變化超調(diào)量大，波動(dòng)頻繁，鎮(zhèn)定時(shí)間長(zhǎng)。

圖14 車速變化結(jié)果

圖15 加速度變化結(jié)果

圖16 展示了車輛的橫向位移變化。從圖中可以看出，在本文提出的橫向運(yùn)動(dòng)控制算法的作用下，車輛均能夠準(zhǔn)確跟蹤期望橫向軌跡。此外，在不同算法作用下車輛#1 和車輛#3 的換道時(shí)機(jī)均不同?；诓┺牡姆椒軌蚺袛喑龈哂行б娴膿Q道時(shí)機(jī)。相比較而言，CACC 方法下的車輛換道時(shí)刻更加保守。

圖16 橫向位移變化結(jié)果

為了更加直觀的展示換道時(shí)機(jī)的選取，圖17 和圖18 給出了有換道需求的車輛#1 和車輛#3 的代價(jià)變化，其中當(dāng)換道代價(jià)突變至20 左右時(shí)表明此時(shí)車輛已通過(guò)合流點(diǎn)。對(duì)于車輛#1，如圖17 所示，博弈的算法判斷在起始時(shí)刻綜合的代價(jià)最小，最終車輛#1 在當(dāng)前時(shí)刻即實(shí)施了換道動(dòng)作，直至到接近1.7 s完成換道，此時(shí)的換道博弈代價(jià)要遠(yuǎn)高于原始車道代價(jià)，車輛即繼續(xù)保持在已換道結(jié)束的車道上行駛。CACC 算法下的車輛#1 在初始時(shí)刻換道代價(jià)顯示具有高收益，但是車輛仍然等待到2 s左右后才執(zhí)行換道行為，合流效率降低。與車輛#1 不同，車輛#3 需要合理地并入到車流內(nèi)部中，如圖18 所示，在初始時(shí)刻換道博弈的代價(jià)要高于原始車道代價(jià)，直至0.5 s左右算法判斷此時(shí)換道具有高收益，車輛#3選擇執(zhí)行換道合流。

圖17 車輛#1換道與原始代價(jià)

圖18 車輛#3換道與原始代價(jià)

圖19歸納了3種算法下的換道時(shí)機(jī)和對(duì)應(yīng)的廣義換道代價(jià)。結(jié)果顯示，相較于CACC 算法，提出的基于博弈的方法無(wú)論集中式還是分布式都能以更低的代價(jià)獲取更高效益的換道時(shí)機(jī)，節(jié)省了74%的合流時(shí)間，提高了協(xié)同換道合流的效率。

圖19 廣義換道代價(jià)結(jié)果

圖20 給出了算法分別在集中式和分布式兩種架構(gòu)下的計(jì)算效率，表3 歸納了兩種架構(gòu)下的優(yōu)化問(wèn)題規(guī)模。結(jié)果顯示，分布式算法計(jì)算效率更高，計(jì)算時(shí)間僅為集中式MPC 的10%，從而降低了算法對(duì)硬件設(shè)備的需求，更容易在實(shí)際中部署。

表3 優(yōu)化問(wèn)題規(guī)模

圖20 單步計(jì)算時(shí)間結(jié)果

5 結(jié)論

為使多自動(dòng)駕駛汽車能夠安全高效地在高動(dòng)態(tài)的道路環(huán)境中執(zhí)行換道任務(wù)，本文中提出了一種基于博弈的多車換道合流運(yùn)動(dòng)規(guī)劃與協(xié)同控制方法。主要研究?jī)?nèi)容如下：（1）分析了曲率變坐標(biāo)系下的車輛運(yùn)動(dòng)狀態(tài)，解析了車輛之間的相互影響機(jī)制，建立了多車交互動(dòng)力學(xué)模型。（2）通過(guò)系統(tǒng)地考慮道路環(huán)境信息，提出了基于博弈的多車協(xié)同速度規(guī)劃算法，并采用分布式框架計(jì)算多車最優(yōu)速度軌跡及換道時(shí)機(jī)。（3）基于變頻率采樣的B 樣條曲線識(shí)別的道路曲率及規(guī)劃的路徑，構(gòu)建了自適應(yīng)時(shí)變預(yù)測(cè)控制算法實(shí)現(xiàn)底層軌跡跟蹤，在車速和曲率頻繁變化時(shí)仍能保證控制策略的有效性。

分別在車道保持和換道合流兩種測(cè)試場(chǎng)景下對(duì)算法進(jìn)行驗(yàn)證，結(jié)果顯示提出的協(xié)同控制方法能夠選取最高效的換道時(shí)機(jī)，同時(shí)保證駕駛安全和控制精度。其中，相比于經(jīng)典橫向運(yùn)動(dòng)控制Stanley 法，提出的方法能夠改善時(shí)變工況下的軌跡跟蹤性能，減少58%的跟蹤誤差；相比于CACC 法，提出方法能夠規(guī)劃更平滑安全的速度軌跡和具備更高效益的換道時(shí)機(jī)，減少74%的合流時(shí)間；相比于集中式MPC的方法，提出方法能縮小優(yōu)化問(wèn)題規(guī)模，計(jì)算求解效率僅為集中式MPC的10%。

在后續(xù)的工作中，將考慮混合交通流中存在的各種不確定因素，并結(jié)合學(xué)習(xí)類的方法，使本文提出的協(xié)同控制方法能夠應(yīng)用于具有更加復(fù)雜多變的道路場(chǎng)景中。