基于車車通信技術的自動換道算法研究與仿真＊

王樂 王文格 彭景陽 胡小雙 郭恒宇

（湖南大學，長沙 410082）

1 前言

自動駕駛技術能夠提高車輛的安全性和交通效率[1]，自動駕駛車輛的車道跟蹤與換道方面均有較多研究成果[2-3]。換道導致的事故占美國所有交通事故的比例約為4%～10%，同時造成了10%的交通延遲[4]，所以對自動駕駛車輛換道的研究具有重要意義。

You 等人采用六次多項式進行縱向軌跡規劃避開前方障礙車輛，通過車車通信技術獲得更加準確的環境信息[5]。Yang 等人采用與時間無關的多項式方程表征車道變換軌跡曲線，可避免現有模型對速度和加速度的不切實際的假設[6]。Luo等人將車道變換軌跡描述為參數化五次多項式，通過求解非線性規劃（Non-Linear Programming，NLP）問題計算多項式參數[3]。張榮輝等人使用六次多項式進行縱向軌跡規劃[7]。在軌跡跟蹤方面，用于移動機器人的Backsteppin[8]在自動駕駛車輛的軌跡跟蹤中得到了應用[9]。葉濤等人在Backstepping 的基礎上設計了滑模控制方法進行軌跡跟蹤[3,10]，Farnaz等人[11]、姜立標等人[12]分別采用反饋誤差學習的方法和新的趨近律對其進行了改進。

目前，自動駕駛車輛的換道研究主要集中于軌跡規劃與軌跡跟蹤，且存在以下不足：大多數研究僅考慮起始車道前方車輛或典型換道場景，考慮因素較少；多數研究只在換道開始時考慮避撞安全需求；軌跡跟蹤中滑模跟蹤算法適應性較差。

本文基于車車通信技術，考慮新的換道場景，提出并行換道軌跡規劃方法。針對換道過程中的碰撞安全問題，提出碰撞檢測與并行軌跡重規劃方法，并改進滑模軌跡跟蹤算法，提高其適用性。

2 自動換道整體策略

典型的向左加速換道場景如圖1 所示，其中M 為換道車輛，Ld、Fd分別為目的車道前、后方車輛，Lo、Fo為原車道前、后方車輛。此場景下，只允許換道至Fd和Ld之間。

圖1 典型向左加速換道場景

新的換道場景如圖2和圖3所示，Fd-F為目標車道后方車輛的后方車輛，Ld-L為目標車道前方車輛的前方車輛。圖2 中，允許換道至Fd和Ld之間或Fd和Fd-F之間。圖3中，允許換道至Fd和Ld之間或Ld和Ld-L之間。當Fd-F不存在時，圖2與圖1場景相同。

圖2 向左加速換道場景

圖3 向右減速換道場景

自動換道策略如圖4所示，其中換道車輛通過車車通信技術獲得周圍車輛信息，通過GPS、加速度傳感器、速度傳感器等獲得狀態信息。根據收集到的信息，換道決策器和換道軌跡規劃器分別進行決策和軌跡規劃，軌跡跟蹤控制器對已規劃的軌跡進行跟蹤，碰撞檢測器不斷進行檢測，軌跡重規劃器在檢測到碰撞的情況下進行軌跡重規劃。

圖4 自動換道策略

3 換道軌跡規劃

3.1 基于優化模型的多項式換道軌跡

3.1.1 多項式換道軌跡

選擇五次多項式作為換道軌跡，其具有連續三階導數和曲率平滑的優點[3]：

式中，a0～a5、b0～b5為未知系數；x(t)、y(t)分別為t時刻縱向、橫向坐標；t為時間。

根據車輛換道開始和結束時刻的車輛狀態可得：

式中，x0、y0分別為初始時刻的縱向和橫向坐標；vx,0、vy,0分別為初始時刻的縱向和橫向速度；ax,0、ay,0分別為初始時刻的縱向和橫向加速度；xf為換道結束時刻的縱向位置；yf為換道結束時刻的橫向位置，等于車道寬度；vx,f為換道結束時刻的目標速度；vy,f為換道結束時刻的橫向速度，等于零；ax,f、ay,f分別為換道結束時刻的縱向和橫向加速度，均為零；tf為換道結束時刻。

式（1）有12個未知參數，式（2）有12個狀態方程，可直接解出參數a0～a5和b0～b5。將tf和xf設為變量，將換道軌跡規劃問題轉化為帶約束的優化問題，本文以向左換道為例進行說明。

3.1.2 約束條件

3.1.2.1 位置約束

換道過程中，換道車輛的橫向位置應在目標車道和本車道中心線之間：

式中，w為車道寬度。

換道終止時刻，其縱向位置應滿足：

式中，xFd-F(tf)、xM(tf)、xLd(tf)、xFd(tf)分別為Fd-F、M、Ld、Fd在換道結束時刻的縱向位移；d為車輛巡航時應保持的最小縱向距離；L為車長。

3.1.2.2 速度約束

換道過程中，換道車輛的縱向速度應小于最大車速，且為正值：

式中，vx(t)為換道車輛在t時刻的縱向車速；vx,max為允許的最大車速。

3.1.2.3 動力學約束

換道車輛的加速度、加加速度應滿足：

式中，ax(t)和ay(t)分別t時刻縱向和橫向的加速度；jx(t)和jy(t)分別t時刻縱向和橫向的加加速度；ax,max和ay,max分別為縱向和橫向最大加速度；jx,max和jy,max分別為縱向和橫向最大加加速度。

3.1.2.4 安全約束

采用動態圓方法逼近車輛二維形狀[5]，如圖5所示。

圖5 車輛形狀示意

由圖5可得動態圓圓心的一系列坐標為：

式中，μ∈[0,1]；xb、xf分別為車輛后端圓和前端圓的縱向坐標；yb、yf分別為車輛后端圓和前端圓的橫向坐標。

換道過程中換道車輛的外輪廓與其他車輛的外輪廓不重合即為安全。因此，安全約束為：

式中，xM、xo分別為換道車輛和障礙車輛（Ld、Fd、Lo、Fo、Fd-F）縱向位移；yM、yo分別為換道車輛和障礙車輛橫向位移；RM、Ro分別為換道車輛和障礙車輛動態圓的半徑，其應略大于車輛寬度的1/2。

將式（8）代入式（9）可得：

式中，μM,μo∈[0,1]；xM,b(t)、xo,b(t)分別為t時刻換道車輛和障礙車輛后端圓的縱向坐標；xM,f(t)、xo,f(t)分別為t時刻換道車輛和障礙車輛前端圓的縱向坐標；yM,b(t)、yo,b(t)分別為t時刻換道車輛和障礙車輛后端圓的橫向坐標；yM,f(t)、yo,f(t)分別為t時刻換道車輛和障礙車輛前端圓的橫向坐標。

在軌跡規劃時刻，假設各障礙車輛勻速直線行駛。

3.1.3 優化目標

考慮車輛換道過程的舒適性和對交通流的影響，設定優化目標為[3]：

式中，λ1、λ2、λ3為權重系數；ξ為常數。

該優化模型對于前述其他場景同樣適用，只需去除未出現車輛對應的約束條件。

3.2 并行換道軌跡規劃

假設車道后方車輛速度不大于前方車輛速度，分別選取Ld、Fd和Fd-F車速為vx,f，采用優化模型作并行計算，結果可能為0條、1條或多條軌跡：若是0條軌跡，按照第3.1 節處理；若是1 條軌跡，則進行換道軌跡跟蹤；若是多條軌跡，則根據駕駛風格選擇一條軌跡進行軌跡跟蹤。

3.3 碰撞檢測與并行軌跡重規劃

針對換道過程中障礙車輛車速等狀態發生非預期改變的情況，本文研究了碰撞檢測與并行軌跡重規劃方法。

在換道過程中收集周圍車輛信息和自身狀態信息后，采用式（10）不斷進行碰撞檢測。如將要發生碰撞則進行并行軌跡重規劃，同時進行換道軌跡和回到起始車道軌跡規劃，進行換道軌跡規劃的方法同3.1節，回到起始車道軌跡規劃時，式（5）改為：

對重規劃軌跡進行選擇時，首先選擇換道軌跡，無換道軌跡時，選擇回到起始車道軌跡。

4 換道軌跡跟蹤

4.1 車輛位姿誤差模型

車輛實際位姿與參考位姿誤差模型如圖6 所示。Mc與Mr分別為車輛當前時刻實際位置和參考位置，Pc=(xc,yc,φc)T和Pr=(xr,yr,φr)T分別為當前車輛的實際位姿和參考位姿，qc=(vc,ωc)T代表當前時刻實際速度和橫擺角速度，qr=(vr,ωr)T代表當前時刻的參考速度和橫擺角速度。

圖6 車輛位姿誤差模型

當前車輛坐標系中，根據坐標變換公式可得位姿誤差方程為：

式中，xe、ye、φe分別為縱向、橫向、方向角誤差；φ為方向角。

進一步，可得位姿誤差微分方程為：

式中，ω為橫擺角速度。

4.2 軌跡跟蹤控制器設計

車輛軌跡跟蹤是對任意時刻初始誤差Pe=(xe,ye,φe)T，尋找控制輸入qc=(vc,ωc)T，使其在該控制作用下有界，且

本文在Backstepping 方法的基礎上，設計滑模控制器切換函數為：

使s1→0、s2→0，然后使得xe→0、φe→-arctan(vrye)，因此ye→0，φe→0。

選擇等速趨近率：

式中，k為常數向量；sgn 為符號函數。

為減小抖動，采用連續函數取代式（16）中的符號函數：

式中，ki為向量k中的變量；δi為正的小量。

令α=arctan(vrye)，將式（17）代入式（15）可得：

最后可得到控制律為：

5 仿真分析

搭建CarSim-Prescan-MATLAB 聯合仿真平臺開展仿真分析，車長度為4.2 m，寬度為1.6 m，仿真工況為城市環境道路。

5.1 并行換道軌跡規劃仿真分析

表1 和表2 描述了換道開始時刻的車輛狀態，其中起始位置是指以換道車輛為坐標原點，周圍車輛與換道車輛的縱向位置關系，換道車輛前方為正，后方為負。

表1 向左換道場景

表2 向右換道場景

并行換道軌跡規劃方法可自主決策出換道結束時刻換道車輛與目標車道車輛的相對位置關系。其中工況1和工況3最終狀態為Ld、Fd之間，工況2最終狀態為Fd、Fd-F之間，工況4 最終狀態為Ld、Ld-L之間。該方法可提高換道可能性及軌跡合理性。對于工況2，Fd雖然在后方，但距離較近且速度較快，在典型換道情景下要加速換道至Fd前方，將無法換道。對于工況4，雖然Ld在前方，但距離較近且速度較慢，典型換道情景與本文方法分別認為應減速換道于Ld之后和換道于Ld之前，所得參數如表3所示，后者的|ax,max|、|ay,max|、|jx,max|、|jy,max|和J均小于前者，更加合理。

表3 工況對比

5.2 碰撞檢測與并行軌跡重規劃仿真分析

在換道開始后，障礙車輛狀態可能發生非預期改變，從而引起碰撞。因此，為保證換道安全，需要驗證本算法在該情況下的有效性。換道開始時刻的場景如表4所示。

表4 軌跡重規劃場景

軌跡重規劃仿真結果如圖7、圖8所示，并將換道開始后的時刻標記于圖上相應位置。在工況5 與工況6中，換道開始后Fd速度分別變為15 m/s 和22 m/s，按原軌跡行駛將會發生碰撞，如圖7a 和圖8a 所示。本文提出的碰撞檢測方法檢測到碰撞后，并行軌跡重規劃器將發送新軌跡到軌跡跟蹤器，以避開碰撞，如圖7b和圖8b所示。

圖7 向左換道，重規劃向目標車道

圖8 向左換道，重規劃向起始車道

仿真結果表明，提出的碰撞檢測與并行軌跡重規劃算法可在換道過程中進行碰撞檢測，并重新規劃軌跡避開潛在碰撞情況，保證了換道過程的安全。

5.3 軌跡跟蹤仿真分析

為研究改進軌跡跟蹤算法與文獻[3]中原軌跡跟蹤算法的區別，設立的仿真工況參數如表5 所示，結果如圖9、圖10所示。

表5 換道軌跡參數

圖9 低速換道軌跡跟蹤誤差

比較圖9 與圖10 可知，在低速換道軌跡跟蹤中，初始誤差在兩種算法的作用下都可在較短時間內收斂，但在高速換道軌跡跟蹤中，橫向誤差在原算法作用下收斂較慢，而在改進算法中會逐漸收斂到0，所以改進的軌跡跟蹤方法適用性更強。

圖10 高速換道軌跡跟蹤誤差

6 結束語

本文研究了基于車車通信的換道軌跡規劃和換道軌跡跟蹤算法，提出并行換道軌跡規劃方法，和碰撞檢測與并行軌跡重規劃方法，并對換道軌跡跟蹤方法進行改進，增加了換道的可能性和軌跡合理性，提高了換道安全性。