許 閣

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

1 前言

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

2 車輛環(huán)境建模

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

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

3 算法設計

3.1 多項式擬合規(guī)劃算法

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

3.1.1 車體坐標系轉Frenet坐標系

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

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

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

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

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

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

起始狀態(tài)指的是待轉規(guī)劃點起始坐標點，而末狀態(tài)點是人為規(guī)劃時想要車經(jīng)過設定的時間后所到達的位置點。

轉換公式如下：

1）求d：

求d_d：

2）求dd_d：

3）求s：s=s

4）求d_s：

5）求dd_s：

3.1.2 五次多項式擬合

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

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

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

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

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

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

3.1.3 過路檢測

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

3.1.4 Frenet坐標系轉車體坐標系

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

需使用待轉規(guī)劃點[s，d_s，dd_s，d，d_d,dd_d]，及待轉規(guī)劃點在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 前饋反饋控制算法

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

3.2.1 PID反饋控制

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

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

3.2.2 前饋控制

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

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交互、規(guī)劃模塊和控制模塊。

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

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

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

5 總結

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