融合改善型可行性檢驗模型的換道跟蹤方法

向云豐，賀巖松，孔偉偉，陳 健，羅禹貢

(1.重慶大學 汽車工程學院， 重慶 400044;2.清華大學 汽車安全與節(jié)能國家重點實驗室，北京 100084)

近年來無人駕駛技術不斷發(fā)展，自主換道是其中重要的技術組成部分[1]。自主換道[2-3]由換道決策[4]、換道軌跡規(guī)劃[5]和換道軌跡跟蹤[6]三部分組成。換道決策決定是否執(zhí)行換道[4,7]，包括換道意圖、換道目標車道選擇和換道可行性檢驗。換道可行性檢驗作為是否執(zhí)行換道的最后關卡，其完備性直接影響換道的安全性，換道軌跡跟蹤是無人駕駛汽車運動控制的核心所在。因此對換道可行性檢驗和換道軌跡跟蹤進行研究是極其重要的。

張帥[3]提出了一種換道可行性檢驗模型，模型中考慮了本車距本車道前后車輛和目標車道前后車輛的最小安全距離。王文霞[8]推導了換道車輛與周圍車輛應滿足的起始最小縱向安全距離。張榮輝等[9]推導了車輛換道匯入車隊前與車隊中匯入點前、后車輛的最小安全距離。王政[10]使用換道車輛與目標車道前后車輛的間距大于最小安全距離作為換道的可行性條件。以上模型均假設周圍車輛處于車道保持狀態(tài)，未考慮周圍車輛換道對本車換道可行性的影響。

綜上所述，現(xiàn)有的換道可行性檢驗模型往往假設周圍車輛處于車道保持狀態(tài)，且只考慮本車道和目標車道前后車輛的影響。在軌跡跟蹤方面，現(xiàn)有的很多跟蹤器不能求解帶約束的問題。針對上述問題，重點研究全面考慮周圍車輛影響的換道可行性檢驗模型和軌跡跟蹤控制方法。

首先分析車道變換的邏輯架構，然后充分考慮處于不同車道和不同狀態(tài)的周圍車輛對本車換道的影響，推導改善型的換道可行性檢驗模型，其次使用能求解帶約束問題的MPC控制器進行換道軌跡跟蹤，最后設計仿真對比試驗，對所研究的模型和方法進行驗證。

1 車道變換邏輯架構

通過分析駕駛員的換道過程，建立車輛自動換道邏輯架構，如圖1所示。根據(jù)換道邏輯架構，需要建立換道意圖產(chǎn)生前的跟車模型、駕駛員是否對當前車道滿意的換道意圖模型、執(zhí)行換道前的目標車道選擇模型和換道可行性檢驗模型、換道過程中的換道軌跡規(guī)劃模型和換道軌跡跟蹤模型。

圖1 車道變換邏輯架構Fig. 1 Logic framework of lane-changing

在跟隨前車過程中，采用Gipps安全距離模型[20]進行縱向速度控制。Gipps安全距離模型由兩部分組成:加速子模型與減速子模型。通常將兩者同時計算出來，跟馳速度取較小者。

加速子模型：

(1)

減速子模型：

(2)

在換道意圖方面，使用文獻[3]中的駕駛員不滿意度pk表示本車對換道的渴望程度。計算公式如下：

(3)

式中：Yu為駕駛員不滿意度閾值；kf∈{0,1,2,3}為本車道前方車輛的種類，0,1,2,3分別代表無車、事故車輛或施工造成的靜態(tài)障礙區(qū)、商用車、乘用車；lf為本車距離前車的縱向距離；lsafe為最小跟車安全距離[20]，其大小與實際車速vs(單位：m/s)相關；ax為縱向加速度。根據(jù)文獻[3]取Yu=500，當pk≥Yu時，無人駕駛汽車產(chǎn)生換道意圖，需進行目標車道選擇。

lsafe=0.002 9(vS×3.6)2+0.304 9(vS×3.6)。

(4)

使用文獻[3]中的最大安全速度V(單位：m/s)進行車道選擇。將本車道與左右兩車道的最大安全速度對比，最大安全速度值最大的車道即為目標車道，若本車道最大安全車速最大，則繼續(xù)跟隨前車，等待換道時機。

(5)

2 換道可行性檢驗模型

在三車道高速公路場景中，按照換道車輛所處車道的位置可將換道場景分為中間車道車輛換道場景(如圖2)和邊緣車道車輛換道場景(如圖3)。兩類換道車輛的差別在于是否可能存在相間車道的車輛向目標車道換道(如圖3中最左側車輛可以向本車的目標車道換道)。

圖2 中間車道車輛換道場景Fig. 2 Lane-changing scene of the vehicle in the middle lane

圖3 邊緣車道車輛換道場景Fig. 3 Lane-changing scene of the vehicle in the edge lane

2.1 中間車道車輛換道可行性檢驗

對于中間車道車輛換道場景，非目標車道(如圖2中的右車道)的車輛如果處于換道狀態(tài)，其換道前已經(jīng)對周圍車輛進行了換道可行性檢驗，即其與本車距離較大；且本車換道的橫向運動方向與非目標車道相反，所以非目標車道的車輛換道與否不會影響中間車道車輛的換道。同理，不需要考慮本車道前后車輛往非目標車道換道。因此只需要討論其他情形下本車道前后車輛和目標車道前后車輛對本車換道的影響。

2.1.1 目標車道前車可行性檢驗

當目標車道前車處于車道保持狀態(tài)時，假設目標車道前車沿車道中心線行駛。如圖4所示，定義本車S的左上角第一次觸碰目標車道前車邊線LFt時的點為C點，則該點為本車S與目標車道前車Ft的臨界碰撞點，定義該時刻為tC。即在本車左上角未到達邊線LFt之前(0～tC)，本車不會與目標車道前車發(fā)生任何形式的碰撞。在tC～T(設換道結束時刻為T)，如果本車最前點(左前點或者右前點)在目標車道前車最后點之后，本車不會與目標車道前車發(fā)生任何形式的碰撞。即：

圖4 本車與Ft的碰撞示意圖Fig. 4 Collision diagram of Ft and S

(6)

式中：xS(t)、xFt(t)分別為t時刻本車和目標車道前車的縱向位置；θ為本車車頭方向與水平方向的夾角；lS與lFt分別為本車和目標車道前車長度；wS為本車寬度。下標S、Ft、Rt、Fp、Rp、Fi和Ri分別表示本車、目標車道前車、目標車道后車、本車道前車、本車道后車、相間車道前車和相間車道后車。cos(θ(t))≤1，|sin(θ(t))|≤max|sin(θ(t))|，因此，?t∈(tC,T)，若滿足(7)式，則(6)式成立。

(7)

(8)

(9)

若

DFt(0)≥lFt，

(10)

則本車不會與目標車道前車發(fā)生任何形式的碰撞。

由于本車在對比本車道和目標車道的速度優(yōu)勢后，選擇向目標車道換道，因此目標車道前車只可能向本車道前車前面換道，不可能往本車前換道。當目標車道前車處于車道變換狀態(tài)時，其離本車道前車有一定的安全距離，離本車的距離更大，所以目標車道前車的換道狀態(tài)不會影響本車換道的可行性。因此只需滿足式(10)，則本車不會與目標車道前車發(fā)生碰撞。

2.1.2 目標車道后車可行性檢驗

本車橫向位置處目標車道的最優(yōu)車速大于本車道，目標車道后車處最優(yōu)車速更大，所以目標車道后車不會選擇往本車后面變道，只需考慮目標車道后車處于車道保持狀態(tài)。與目標車道前車一樣，本車左上角未到達目標車道后車邊線LRt前(0～tC)，本車不會與目標車道前車發(fā)生任何形式的碰撞。同理定義：

DRt(0)=xS(0)-xRt(0)-LT2，

(11)

(12)

若

DRt(0)≥lRt，

(13)

則本車不會與目標車道后車發(fā)生任何形式的碰撞。

2.1.3 本車道前車可行性檢驗

當本車道前車往本車的目標車道換道時，如果本車繼續(xù)換道，仍然跟隨的是目前的前車，換道完成后駕駛員不滿意度仍然很大，因此此時本車不進行換道，等待前車換道完成后再決定是否換道。

當本車道前車處于車道保持狀態(tài)時，定義本車S的右上角第一次觸碰本車道前車右邊線LFp的點為C1點(見圖5)，則該點為本車S與本車道前車的臨界碰撞點，定義該時刻為tC1。即在本車右上角未到達邊線LFp之前(0～tC1)，本車可能與本車道前車發(fā)生正碰或者角碰。在tC1～T本車不會與本車道前車發(fā)生任何形式的碰撞。

圖5 本車與Fp的碰撞示意圖Fig. 5 Collision diagram of Fp and S

定義

DFp(0)=xFp(0)-xS(0)-LT3。

(14)

(15)

DFp(0)≥lFp，

(16)

則本車不會與本車道前車發(fā)生任何形式的碰撞。

因此若本車道前車往本車目標車道變道，則本車放棄變道，等待前車換道完成之后再確定是否變道；其他情形下，只需滿足(16)式時，本車道前車不會影響本車換道。

2.1.4 本車道后車可行性檢驗

當本車道后車處于車道保持狀態(tài)時，如圖6所示，定義本車S的右下角第一次觸碰本車道后車右邊線LRp時的點為C2點，則該點為本車S與本車道后車的臨界碰撞點，定義該時刻為tC2。同理定義：

圖6 本車與Rp的碰撞示意圖Fig. 6 Collision diagram of Rp and S

DRp(0)=xS(0)-xRp(0)-LT4。

(17)

(18)

若

DRp(0)≥lRp，

(19)

則本車不會與本車道后車發(fā)生任何形式的碰撞。

當本車道后車往目標車道變道，為了確保換道安全，可使本車的最后點在本車道后車最前點之前，即

(20)

式中：θ1為本車道后車換道過程車頭方向與水平方向的夾角；wRp為本車道后車的寬度。

由cos(θ(t))≤1,|sin(θ(t))|≤max|sin(θ(t))|，cos(θ1(t))≤1，|sin(θ1(t))|≤max|sin(θ1(t))|，可使

(21)

定義

DR1p(0)=xS(0)-xRp(0)-LT41。

設(0,T)時間段內(nèi)本車道后車與本車之間最大相對縱向位移為：

若

DR1p(0)≥lR1p，

(22)

則本車不會與本車道后車發(fā)生任何形式的碰撞。

本車可以根據(jù)周圍車輛轉(zhuǎn)向燈信息判斷周圍車輛是否想換道，且能判斷往哪一方向換道。如果本車處于中間車道，可以根據(jù)圖7的邏輯判斷流程圖檢驗車輛換道可行性。

圖7 中間車道換道可行性檢驗流程圖Fig. 7 Flow chart of lane-changing feasibility test in the middle lane

2.2 邊緣車道車輛換道可行性檢驗

對于邊緣車道的車輛，除了受本車道車輛和目標車道車輛影響外，還會受到相間車道車輛換道的影響。如圖3所示，當本車S想要往目標車道換道時，如果相間車道車輛也往目標車道換道，則本車S與相間車道車輛可能在目標車道發(fā)生碰撞，因此必須討論相間車道的車輛對本車換道的影響，確保換道安全。

2.2.1 相間車道前車可行性檢驗

當相間車道前車向目標車道變道時，本車左上角達到目標車道邊界線前(臨界點C3)，本車不會與相間車道前車發(fā)生碰撞。在tC3～T，為了確保換道安全，只需使本車的最前點在相間車道前車最后點之后，即

DFi(0)≥lFi，

(23)

式中：DFi(0)為換道前本車與相間車道前車的初始縱向距離，lFi為(tC3,T)時間段內(nèi)本車與相間車道前車之間最大相對縱向位移。

DFi(0)=xFi(0)-xS(0)-LT5；

式中：xFi(0)為換道前相間車道前車縱向位置，wFi相間車道前車寬度，θ2為相間車道前車換道過程車頭方向與水平方向的夾角。

(24)

2.2.2 相間車道后車可行性檢驗

當相間車道后車往本車目標車道變道時，跟相間車道前車一樣，為了使本車換道安全，在tC3～T，只需使本車的最后點在相間車道后車最前點之前，即

DRi(0)≥lRi，

(25)

式中：DRi(0)為換道前本車與相間車道后車的初始縱向距離，lRi為(tC3,T)時間段內(nèi)相間車道前車與本車之間最大相對縱向位移。

DRi(0)=xS(0)-xRi(0)-LT6；

式中：xRi(0)為換道前相間車道后車縱向位置，wRi為相間車道后車寬度，θ3為相間車道后車換道過程車頭方向與水平方向的夾角。

(26)

綜上所述，當車輛處于邊緣車道時，可以根據(jù)圖8的邏輯判斷流程圖檢驗車輛換道可行性。邊緣車道的車輛在滿足中間車道換道可行性條件的基礎上，還需滿足相間車道前后車輛可行性檢驗才能換道。

圖8 邊緣車道換道可行性檢驗流程圖Fig. 8 Flow chart of lane-changing feasibility test in the edge lane

3 軌跡規(guī)劃與軌跡跟蹤

3.1 軌跡規(guī)劃

設換道過程中縱向速度不變，橫向軌跡為五次多項式，即：

(27)

3.2 基于MPC的軌跡跟蹤控制

將車輛簡化為三自由度模型(如圖9)。根據(jù)力學平衡關系，建立如下動力學模型。

圖9 三自由度動力學模型Fig. 9 Three degrees of freedom dynamics model

(28)

(29)

整理可以得到以下微分方程：

(30)

(31)

設車輛坐標與全局坐標的夾角為φ，則全局坐標下的速度和局部坐標下的速度轉(zhuǎn)換關系如下：

(32)

(33)

圖10 MPC控制方法Fig. 10 Control method of MPC

4 仿真驗證和結果分析

4.1 仿真工況

仿真工況1(如圖2)，本車處于中間車道，除本車外其他車輛處于車道保持狀態(tài)，所有車輛縱向初速度和初始位置如表1所示，且假設所有車輛縱向勻速，本車按五次多項式規(guī)劃的軌跡向目標車道換道。

表1 仿真工況1

仿真工況2(如圖11)，本車處于中間車道，1.50 s時刻本車道前車打左轉(zhuǎn)向燈，即往左側車道變道，其他車輛處于車道保持狀態(tài)。所有車輛的縱向速度和初始位置如表2所示。

圖11 仿真工況2Fig. 11 Simulation condition 2

表2 仿真工況2

仿真工況3(如圖12)，本車處于最右側車道，2.00 s時刻相間車道后車打右轉(zhuǎn)向燈，即往中間車道變道，其他車輛處于車道保持狀態(tài)。所有車輛的縱向速度和初始位置如表3所示。

圖12 仿真工況3Fig. 12 Simulation condition 3

表3 仿真工況3

仿真工況4，本車處于中間車道， 1.40 s時本車道后車打左轉(zhuǎn)向燈，即往左側車道變道。1.50 s時本車道前車打右轉(zhuǎn)向燈，即往右側車道變道，其他車輛處于車道保持狀態(tài)。所有車輛的縱向速度和初始位置如表4所示。

表4 仿真工況4

4.2 換道可行性檢驗仿真結果

對于仿真工況1，周圍車輛均處于車道保持狀態(tài)，使用改善型換道可行性檢驗模型和對比模型均能在安全的情況下正常換道。

對于仿真工況2，1.50 s時本車道前車向本車目標車道換道，在1.91s時，本車駕駛員不滿意度達到不滿意度閾值。在1.91 s時，本車距本車道前后車、目標車道前后車的距離滿足對比模型的換道可行性檢驗條件，因此在1.91 s時向目標車道換道,其換道過程的橫向位置和縱向位置如圖13和圖14所示。在5.91 s時本車換道完成，但本車距離前車的距離比換道前更小(如圖13)，駕駛員不滿意度仍高于駕駛員不滿意度閾值，本車仍然想請求換道，很可能再往原車道換道，因此之前的換道為無效的換道，沒有降低駕駛員不滿意度。對于改善型換道可行性檢驗模型，在1.91 s時本車發(fā)現(xiàn)前車正在往本車目標車道換道，因此本車繼續(xù)處于車道保持狀態(tài)，等待前車換道完成后再決定是否換道，杜絕了向目標車道換道完成后馬上又向本車道換道的無效換道過程。因此相對于對比模型，使用改善型換道可行性檢驗模型更加高效。

圖13 對比模型仿真工況2縱向位移Fig. 13 Longitudinal displacement of the reference model in simulation condition 2

圖14 對比模型仿真工況2橫向位移Fig. 14 Lateral displacement of the reference model in simulation condition 2

對于仿真工況3，2.00 s時相間車道后車向本車目標車道換道，在2.55 s時，本車駕駛員不滿意度達到不滿意度閾值。在2.55 s時，本車距本車道前后車、目標車道前后車的距離滿足對比模型的換道可行性檢驗條件，因此在2.55 s時向目標車道換道。對比模型的仿真結果如圖15和16，在5.62 s時，本車和相間車道后車縱向位置相同、橫向位置很近，所以在5.62 s時兩車已經(jīng)發(fā)生碰撞。對于改善型換道可行性檢驗模型，增加了相間車道車輛的檢驗，在2.55 s時刻，相間車道后車距本車距離小于最小安全距離，因此不進行換道，從而避免了本車與相間車道后車的碰撞。

圖15 對比模型仿真工況3縱向位移Fig. 15 Longitudinal displacement of the reference model in simulation condition 3

圖16 對比模型仿真工況3橫向位移Fig. 16 Lateral displacement of the reference model in simulation condition 3

對于仿真工況4，在1.62 s時本車駕駛員不滿意度達到不滿意度閾值，此時本車與周圍車輛的距離和可行性檢驗閾值如表5所示。由表可知，對比模型的可行性檢驗結果為通過，而本車距本車道后車的距離不能通過改善型換道可行性檢驗模型的檢驗。對比模型的仿真結果如圖17～18所示，在4.32 s時，本車和本車道后車縱向位置相同、橫向位置相近，所以在4.32 s時兩車已經(jīng)發(fā)生碰撞。而對于改善型換道可行性檢驗模型，在1.62 s時其通不過可行性檢驗，本車不進行換道，從而避免了本車與本車道后車發(fā)生碰撞。因此相對于對比模型，使用改善型換道可行性檢驗模型更加安全。

表5 本車與周圍車輛的距離和可行性檢驗閾值

圖17 對比模型仿真工況4縱向位移Fig. 17 Longitudinal displacement of the reference model in simulation condition 4

圖18 對比模型仿真工況4橫向位移Fig. 18 Lateral displacement of the reference model in simulation condition 4

仿真工況2～4中改善型換道可行性檢驗模型對不同狀態(tài)的周圍車輛和相間車道的換道車輛進行了可行性檢驗，結果表明該模型比對比模型更加安全高效。

4.3 軌跡跟蹤仿真結果

對仿真工況1中的換道軌跡，換道車速為100 km/h，規(guī)劃橫向軌跡如圖19所示。MPC控制器的軌跡跟蹤結果如圖20～21所示。圖20中可以看出前輪轉(zhuǎn)角無毛刺，即駕駛過程中方向盤平滑過渡，轉(zhuǎn)向系統(tǒng)能按照輸入的方向盤轉(zhuǎn)角響應，最大前輪轉(zhuǎn)角不超過8°，具有很好的駕乘舒適性。由圖21可知，最大橫向跟蹤誤差不超過1 cm，具有非常高的跟蹤精度。

圖19 橫向軌跡規(guī)劃Fig. 19 The lateral trajectory planning

圖20 MPC跟蹤方向盤轉(zhuǎn)角Fig. 20 The steering angle of the MPC control method

圖21 MPC跟蹤橫向誤差Fig. 21 The lateral tracking error of the MPC control method

5 結 論

為實現(xiàn)安全自主換道，建立了車道變換邏輯框架，提出了改善型換道可行性檢驗模型和基于MPC的軌跡跟蹤方法，最后設計了仿真對比試驗對所研究的模型和方法進行驗證。該研究得到以下結論：

1)提出了中間車道車輛換道可行性檢驗模型。分析了本車道和相鄰車道處于不同狀態(tài)(車道保持和車道變換)的前后車輛對本車換道的影響，提出了其不影響本車換道的檢驗要求，確保車輛在不同的駕駛環(huán)境下均能安全換道。

2)提出了邊緣車道車輛換道可行性檢驗模型。分析了相間車道處于車道變換的前后車輛對本車換道的影響，提出了其不影響本車換道的檢驗要求，確保換道過程中本車不與相間車道換道車輛發(fā)生碰撞。

3)建立了車道變換邏輯框架，在該框架下融合改善型換道可行性檢驗模型，使用MPC方法對換道軌跡進行跟蹤，然后根據(jù)該框架使用Simulink和PreScan建立自主換道模型。仿真結果表明，提出的自主換道模型在安全高效換道的前提下能夠?qū)Q道軌跡進行精準跟蹤。

目前只使用仿真方法對所研究的模型和方法進行了驗證，未來需要設計實車實驗對所提出的模型進行驗證，最終將其用于實際工程中。