基于Carsim與Simulink的自動(dòng)變道聯(lián)合仿真

許 閣

（東風(fēng)汽車公司技術(shù)中心，湖北 武漢 430058）

1 前言

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

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

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

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

3 算法設(shè)計(jì)

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

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

3.1.1 車體坐標(biāo)系轉(zhuǎn)Frenet坐標(biāo)系

車體坐標(biāo)系指的是基于車縱向和橫向建立的坐標(biāo)系，Carsim輸出給Simulink的信號(hào)為車體坐標(biāo)系下的信號(hào)，需將車體坐標(biāo)系轉(zhuǎn)換到Frenet坐標(biāo)系進(jìn)行五次多項(xiàng)式規(guī)劃。

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

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

d_s：s對(duì)時(shí)間t的一階導(dǎo)；d_d：d對(duì)s的一階導(dǎo)。

dd_s：s對(duì)時(shí)間t的二階導(dǎo)；dd_d：d對(duì)s的二階導(dǎo)。

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

起始狀態(tài)指的是待轉(zhuǎn)規(guī)劃點(diǎn)起始坐標(biāo)點(diǎn)，而末狀態(tài)點(diǎn)是人為規(guī)劃時(shí)想要車經(jīng)過設(shè)定的時(shí)間后所到達(dá)的位置點(diǎn)。

轉(zhuǎn)換公式如下：

1）求d：

求d_d：

2）求dd_d：

3）求s：s=s

4）求d_s：

5）求dd_s：

3.1.2 五次多項(xiàng)式擬合

通過坐標(biāo)轉(zhuǎn)換將始末狀態(tài)點(diǎn)轉(zhuǎn)換到Frenet坐標(biāo)系下，即可進(jìn)行五次多項(xiàng)式擬合。

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

（1）先對(duì)縱向進(jìn)行s-t的5次多項(xiàng)式擬合得到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)對(duì)應(yīng)t=0時(shí)刻，終點(diǎn)時(shí)刻t=t0，解出s0，s1，s2，s3，s4，s5。

（2）再對(duì)橫向進(jìn)行d-s的五次多項(xiàng)式擬合得到d=d0+d1s+d2s2+d3s3+d4s4+d5s5。

3.1.3 過路檢測(cè)

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

3.1.4 Frenet坐標(biāo)系轉(zhuǎn)車體坐標(biāo)系

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

需使用待轉(zhuǎn)規(guī)劃點(diǎn)[s，d_s，dd_s，d，d_d,dd_d]，及待轉(zhuǎn)規(guī)劃點(diǎn)在Frenet坐標(biāo)系下的參考線上的投影點(diǎn)，在車體坐標(biāo)系下的坐標(biāo)（r_x，r_y），投影點(diǎn)與x軸方位角θ_r，投影點(diǎn)的曲率kr，投影點(diǎn)曲率對(duì)s的一階導(dǎo)kr’，從而求出待轉(zhuǎn)點(diǎn)在車體坐標(biāo)系下的坐標(biāo)（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ī)劃模塊輸出的車體坐標(biāo)系規(guī)劃軌跡y-x，結(jié)合自身車速和偏轉(zhuǎn)的角度，從規(guī)劃軌跡上選取合適預(yù)瞄點(diǎn)，并計(jì)算本車與預(yù)瞄點(diǎn)之間的角度差距和曲率差距，通過前饋加PID反饋的方式，得到控制模塊地輸出轉(zhuǎn)角，并發(fā)送給EPS轉(zhuǎn)向系統(tǒng)，使其按計(jì)算的轉(zhuǎn)角進(jìn)行轉(zhuǎn)向，從而實(shí)現(xiàn)對(duì)規(guī)劃軌跡的跟蹤控制[2]。為了更加貼近實(shí)際情況，應(yīng)設(shè)計(jì)成離散控制系統(tǒng)，因此本文后續(xù)算法公式中積分與微分采用離散點(diǎn)數(shù)值進(jìn)行計(jì)算。

3.2.1 PID反饋控制

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

PID控制器設(shè)計(jì)公式如下，其輸出的δ1為反饋轉(zhuǎn)角：

3.2.2 前饋控制

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

4 仿真分析

在本次仿真分析過程中，在Carsim中設(shè)置本車在雙車道的右車道中心線處，沿中心線方向向前以20m/s的速度行駛，并在其正前方20m處配置一輛同方向運(yùn)動(dòng)但車速為15m/s的障礙車，參考線選取兩車道的中心線通過攝像頭輸出4個(gè)點(diǎn)，變道目標(biāo)車道為左車道中心線，車體坐標(biāo)系以本車之心為原點(diǎn)，沿中心軸指向車體方向?yàn)閄軸正方向，垂直于中心軸指向車身左側(cè)為Y軸正方向。輸出接口兩個(gè)，分別為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]表示參考線上采用的四個(gè)點(diǎn)的橫縱坐標(biāo)，輸入控制接口一個(gè)為theta=IMP_STEER_SW表示方向盤轉(zhuǎn)角。仿真周期為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。

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

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

5 總結(jié)

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