基于Carsim與Simulink的自動變道聯合仿真

許 閣

（東風汽車公司技術中心，湖北 武漢 430058）

1 前言

對于自動變道規劃算法而言，其作用是接收來自決策模塊的決策信號，處理后生成一條變道規劃軌跡并由控制模塊按此軌跡控制本車進行自動變道。而單純對規劃模塊進行開環測試，只能證明本算法能在給定決策條件下能順利運行，無法證明生成的規劃軌跡的正確性與有效性，因此本文在Simulink環境下以自動變道規劃算法為核心并編寫控制算法模擬車輛運動，再聯合Carsim搭建仿真平臺，實現閉環仿真。

2 車輛環境建模

在Carsim中選擇車型：包含車輛的類型、參數配置等各種信息。

配置兩條直線車道，車道中間為單虛線，車道寬為4m；設置自車初始狀態，以20m/s的速度沿右車道中心線行駛，變道的目標車道為左車道中心線，通過本車前方的攝像頭可選任意一條虛線或實線車道線，采集車道線上的四個點，并輸出此四個點在本車車體坐標系下的坐標，并在本車前方20m處設置一輛速度為15m/s勻速行駛的障礙車。

3 算法設計

3.1 多項式擬合規劃算法

通過車體坐標系與Frenet坐標系的相互轉換，在Frenet坐標系下以參考線為基準采用五次多項式擬合，由給定起始點和變道終點的信息進行五次多項式擬合，生成一條連續的滿足車輛運動的規劃曲線。參考線選取兩車道的中心線，通過攝像頭輸出的四個點坐標進行三次多項式擬合得到。

3.1.1 車體坐標系轉Frenet坐標系

車體坐標系指的是基于車縱向和橫向建立的坐標系，Carsim輸出給Simulink的信號為車體坐標系下的信號，需將車體坐標系轉換到Frenet坐標系進行五次多項式規劃。

Frenet坐標系是用s，d_s，dd_s，d，d_d，dd_d來描述車輛在t時刻的狀態[1]。

s：車輛在參考線上的投影點經過時間t后的位移；d：車輛在t時刻的位置到車輛在參考線上投影點的距離。參考線還是基于車體的坐標系下的參考線。

d_s：s對時間t的一階導；d_d：d對s的一階導。

dd_s：s對時間t的二階導；dd_d：d對s的二階導。

車體坐標系下的待轉規劃點坐標（x，y）和其對應的參考線上投影點的坐標（r_x，r_y），待轉規劃點與x軸的方位角θ_x，投影點與x軸方位角θ_r，車輛的速度v_x，車輛的加速度a_x，參考線上的投影點的曲率kr，投影點曲率對s的一階導kr’，投影點距離參考線起始投影點的位移s，待轉規劃點的曲率kx，將以上物理量作為輸入，經過下面的坐標轉換公式，得到待轉規劃點（x，y）對應的Frenet坐標信息[s，d_s，dd_s，d，d_d，dd_d]。

起始狀態指的是待轉規劃點起始坐標點，而末狀態點是人為規劃時想要車經過設定的時間后所到達的位置點。

轉換公式如下：

1）求d：

求d_d：

2）求dd_d：

3）求s：s=s

4）求d_s：

5）求dd_s：

3.1.2 五次多項式擬合

通過坐標轉換將始末狀態點轉換到Frenet坐標系下，即可進行五次多項式擬合。

橫縱向規劃是指規劃出的兩條軌跡，用s-t的多項式描述縱向規劃結果，用d-s的多項式來描述橫向規劃結果。

（1）先對縱向進行s-t的5次多項式擬合得到s=s0+s1t+s2t2+s3t3+s4t4+s5t5，過程如下：

1）根據定義可知，d_s=s1+2s2t+3s3t2+4s4t3+5s5t4；dd_s=2s2+6s3t+12s4t2+20s5t3；

2）起始狀態對應t=0時刻，終點時刻t=t0，解出s0，s1，s2，s3，s4，s5。

（2）再對橫向進行d-s的五次多項式擬合得到d=d0+d1s+d2s2+d3s3+d4s4+d5s5。

3.1.3 過路檢測

車輛在變道過程中是實時變化的，隨著車輛離目標車道越來越近，采用五次多項式擬合時，會產生較大的超調從而變得不可控，降低了系統的穩定性。因此需設置過路檢測，即當五次多項式生成的軌跡超調過大時，即規劃軌跡超出目標車道時，且最高點超出超過參考線50cm時，調節起始狀態點的d_d和dd_d為零，使得其重新進行五次多項式規劃（方法同上述）生成一條不超過相鄰車道線的規劃軌跡。

3.1.4 Frenet坐標系轉車體坐標系

將上述規劃軌跡d-s由Frenet轉換為車體坐標系y-x。

需使用待轉規劃點[s，d_s，dd_s，d，d_d,dd_d]，及待轉規劃點在Frenet坐標系下的參考線上的投影點，在車體坐標系下的坐標（r_x，r_y），投影點與x軸方位角θ_r，投影點的曲率kr，投影點曲率對s的一階導kr’，從而求出待轉點在車體坐標系下的坐標（x，y），與x軸的方位角θ_x，曲率kx，速度v_x，加速度a_x。

1）求（x，y）：

2）求θx：

解得θx；

3）求kx：

4）求v_x：

5）求a_x：

3.2 前饋反饋控制算法

根據規劃模塊輸出的車體坐標系規劃軌跡y-x，結合自身車速和偏轉的角度，從規劃軌跡上選取合適預瞄點，并計算本車與預瞄點之間的角度差距和曲率差距，通過前饋加PID反饋的方式，得到控制模塊地輸出轉角，并發送給EPS轉向系統，使其按計算的轉角進行轉向，從而實現對規劃軌跡的跟蹤控制[2]。為了更加貼近實際情況，應設計成離散控制系統，因此本文后續算法公式中積分與微分采用離散點數值進行計算。

3.2.1 PID反饋控制

本算法將四輪汽車簡化成兩輪自行車模型，根據純跟蹤算法原理[3]，將預瞄點選取為規劃軌跡的第5個點，并可通過修改預瞄點的選取觀察控制效果。角度誤差e由兩部分組成，一是由橫向位置誤差造成的θ1，二是由車身方向造成的θ2。

PID控制器設計公式如下，其輸出的δ1為反饋轉角：

3.2.2 前饋控制

前饋控制器的控制輸入為規劃軌跡曲率C，用上述當前時刻的規劃軌跡的多項式系數C2的兩倍近似表示，前饋控制器設計為，其中u為當前車速，C為規劃軌跡曲率，k為穩定性因素，常通過標定決定，L為本車軸距，i為轉向系統傳動比：

4 仿真分析

在本次仿真分析過程中，在Carsim中設置本車在雙車道的右車道中心線處，沿中心線方向向前以20m/s的速度行駛，并在其正前方20m處配置一輛同方向運動但車速為15m/s的障礙車，參考線選取兩車道的中心線通過攝像頭輸出4個點，變道目標車道為左車道中心線，車體坐標系以本車之心為原點，沿中心軸指向車體方向為X軸正方向，垂直于中心軸指向車身左側為Y軸正方向。輸出接口兩個，分別為carstate[Vx，AVz]表示本車的縱向速度和橫擺角速度，和points[X_S1_1，X_S1_2，X_S1_3，X_S1_4，Y_S1_1，Y_S1_2，Y_S1_3，Y_S1_4]表示參考線上采用的四個點的橫縱坐標，輸入控制接口一個為theta=IMP_STEER_SW表示方向盤轉角。仿真周期為0.001s。

Simulink中則為具體算法的搭建，由三部分組成：Carsim交互、規劃模塊和控制模塊。

規劃周期為0.1s；控制周期為0，01s，PID反饋控制中Kp取4，Ti取2.5，Td取0.25；前饋控制中，k取0.003，L為Carsim中配置的本車的軸距為2.91m，i取5。

仿真結果：30s的時間，每次規劃的軌跡都是平滑且連續的，能滿足車輛的動力學要求；車輛在4s左右時完成變道，伴有些許振蕩并逐漸趨于平緩。

綜上可得，本次聯合仿真能實現比較穩定且效果良好的閉環控制，說明采用五次多項式擬合進行的變道軌跡規劃能滿足車輛的跟蹤條件，為將應用軟件導入實際車輛進行實車測試奠定了理論基礎。

5 總結

本文以模擬實際車輛規劃與控制的關系的基礎上，即規劃周期設為0.1s，控制周期設為0.01s，通過將車體坐標系下的感知信息轉換到Frenet坐標系下進行五次多項式軌跡規劃，并設計過路檢測防止規劃軌跡越過目標先造成系統不穩定，并將規劃結果還原到車體坐標系下輸出給前饋反饋控制模塊進行變道跟蹤，仿真結果表示，車輛能在限定時間內按照規劃軌跡進行自動變道。