段敏 孫小松 張博涵

（遼寧工業(yè)大學(xué)，錦州 121001）

主題詞：智能車 離散線性二次型調(diào)節(jié)器 模型預(yù)測控制 跟蹤控制 速度規(guī)劃

1 前言

跟蹤控制是無人駕駛汽車領(lǐng)域的研究熱點，控制器必須通過同時操縱轉(zhuǎn)向角度、油門和制動來跟蹤參考路徑和速度。目前，跟蹤問題的解決方法有：無模型反饋控制法，這類方法一般不考慮車輛系統(tǒng)的特性，直接根據(jù)系統(tǒng)反饋的跟蹤誤差通過控制律得到前輪轉(zhuǎn)角，例如比例積分微分（PID）控制法，但是該方法因魯棒性較差需要不斷調(diào)整參數(shù)；純跟蹤（Pure Pursuit）控制法，該控制方法簡單易行、實時性好，但需在特定工況下才能有效控制車輛，當(dāng)面對連續(xù)變曲率道路工況時，此方法的跟蹤效果較差，難以投入實際工程應(yīng)用；非線性模型預(yù)測控制（Nonlinear Model Predictive Control，NMPC）法，該方法依靠動力學(xué)模型設(shè)計懲罰函數(shù)進行轉(zhuǎn)向與加速，指引車輛實現(xiàn)路徑與速度跟蹤，此方法雖可在低速工況下保持較高跟蹤精度，但由于其在不斷循環(huán)優(yōu)化求解的過程中會帶來較大計算量，車速較高時存在無法實現(xiàn)對車輛實時控制的風(fēng)險。Su在軌跡跟蹤控制階段采用了模糊PID 控制，此方式雖然提高了控制的可靠性，但其忽略了對橫擺穩(wěn)定性的控制。Wang等采用自適應(yīng)模型預(yù)測控制提高軌跡的跟蹤與穩(wěn)定性，但其無法適應(yīng)大曲率工況。Chen等采用一種基于最優(yōu)前輪側(cè)偏力的智能汽車線性二次型調(diào)節(jié)器（Linear Quadratic Regulator，LQR）路徑跟蹤橫向控制方法，但其在跟蹤小曲率的圓弧路徑時仍產(chǎn)生0.2 m 的橫向誤差，這對實際工程應(yīng)用來說是難以接受的。

因此，本文在Frenet坐標(biāo)系下建立二自由度動力學(xué)模型設(shè)計前饋補償離散線性二次型調(diào)節(jié)器（Discrete Linear Quadratic Regulator，DLQR）橫向控制器，在一階慣性系統(tǒng)下設(shè)計模型預(yù)測控制（Model Predictive Control，MPC）縱向控制器，并通過模糊速度規(guī)劃使控制系統(tǒng)滿足在不同曲率工況下的跟蹤精度與橫向穩(wěn)定性，并通過仿真驗證其可靠性。

2 自行車模型的建立

2.1 二自由度車輛動力學(xué)模型

由于本文對低、高速工況均有涉及，而考慮到輪胎的側(cè)偏特性，高速工況下輪胎會產(chǎn)生較大形變，因此運動學(xué)模型不再適用，應(yīng)以動力學(xué)模型作為控制模型，如圖1所示（本文坐標(biāo)系皆采用右手坐標(biāo)系）。

圖1 二自由度動力學(xué)模型

其中，v、v分別為模型質(zhì)心處的縱、橫向車速，、分別為質(zhì)心與前、后軸的距離，為質(zhì)心側(cè)偏角，、分別為橫擺角與橫擺角速度，為前輪轉(zhuǎn)角，F、F分別為前、后輪所受的側(cè)向力，、分別為前、后輪側(cè)偏角。

假設(shè)較小，則cos≈1。對模型進行受力分析：

以該模型建立力矩方程為：

式中，為轉(zhuǎn)動慣量；為整車質(zhì)量；a為側(cè)向加速度；C、C分別為前、后輪側(cè)偏剛度。

根據(jù)剛體運動學(xué)速度合成與分解可得：

將側(cè)偏角方程代入牛頓第二定律公式可得：

2.2 Frenet坐標(biāo)系下的橫向誤差模型

在Frenet 坐標(biāo)系中，以道路中心線為參考線，以參考線的切向量與法向量建立坐標(biāo)系，以車輛在參考軌跡的投影點為坐標(biāo)原點，坐標(biāo)軸相互垂直，分為方向與方向。相比于笛卡爾坐標(biāo)系，F(xiàn)renet坐標(biāo)系可與軌跡規(guī)劃的坐標(biāo)系呼應(yīng)，大幅簡化控制算法，并可起到使縱向控制與橫向控制解耦的作用。

圖2所示為車輛在Frenet坐標(biāo)系下的映射，其中、分別為投影點處單位法向量和切向量，、分別為匹配點處單位法向量和切向量，為車輛投影位矢，為車輛真實位矢，為車輛匹配位矢，為橫向誤差，、分別為車輛在絕對坐標(biāo)系和Frenet坐標(biāo)系下的航向角，、′分別為映射前、后的速度。

圖2 Frenet坐標(biāo)系

為計算Frenet 坐標(biāo)系下全局規(guī)劃路徑各離散點的信息，本文尋找與車輛距離最短的目標(biāo)路徑點作為匹配點(,,,)，通過匹配點的信息對車輛在Frenet 坐標(biāo)系下投影點的信息進行求解。設(shè)車輛坐標(biāo)為(,)，則：

式中，為投影曲率；、為車輛投影位矢的數(shù)值坐標(biāo)；、、、分別為離散軌跡與車距離最近的點在直角坐標(biāo)系下對應(yīng)的縱向位置、橫向位置、航向角、曲率。

同理，由幾何關(guān)系可得：

為建立與控制方程間的聯(lián)系，對求導(dǎo)后由Frenet公式可得：

又因為=+，(-)為小量，化簡可得：

將式（9）代入式（7）后，等式兩邊點乘得：

式中，e為航向誤差。

e的穩(wěn)態(tài)誤差為-，因此，為在控制中使縱向誤差收斂于0，且航向誤差(-)收斂于0，令縱向誤差e=，航向誤差e=-以間接達到控制效果。由此，可構(gòu)成：

代入二自由度動力學(xué)方程可得橫向誤差微分方程：

為將控制問題轉(zhuǎn)化為性能函數(shù)極小值求解問題，將式（12）按順序?qū)懗桑?/p>

3 跟蹤控制器

跟蹤控制器整體結(jié)構(gòu)如圖3所示。

圖3 控制系統(tǒng)結(jié)構(gòu)

3.1 橫向跟蹤控制器設(shè)計

3.1.1 DLQR橫向跟蹤控制設(shè)計

為避免違背連續(xù)狀態(tài)方程式（13）中所滿足的牛頓運動定律，實現(xiàn)DLQR 的反饋控制，忽略式（13）中的道路幾何信息，對等式兩邊積分后由中值定理得：

式中，為采樣周期；+、分別為積分的上、下限，且在此區(qū)間內(nèi)；()為式（13）兩邊積分后由牛頓萊布尼茲公式所得移項。

對()采用中點法，對()采用前向歐拉法，可得離散化后的微分方程：

式中，為單位矩陣。

將式（15）按順序?qū)?yīng)簡寫為離散化狀態(tài)表達式：

設(shè)計性能函數(shù)：

式中，、分別為性能函數(shù)對應(yīng)于狀態(tài)量與控制量的權(quán)重矩陣。

為求二次型的最小值，利用拉格朗日乘子法將其表示為：

式中，=0,1,2,…,-1；λ為各約束的待定系數(shù)；為保證誤差的和最小的時段上限，其趨于無窮。

分別對X、u、X、λ求偏導(dǎo)等于0，可得：

設(shè)λ=2PX，(=1,2,…,)，且P=，則通過式（19）內(nèi)等式間的相互代入可得著名的里卡蒂（Riccati）方程的解：

通過迭代求解里卡蒂方程使解收斂后，易得全狀態(tài)反饋控制：

3.1.2 前饋控制器設(shè)計

為消除穩(wěn)態(tài)誤差，需要通過額外引入前饋控制，實現(xiàn)存在合適的車輪轉(zhuǎn)角前饋量使盡可能收斂于0的效果，則重新定義全狀態(tài)反饋控制與誤差微分方程：

當(dāng)橫向誤差e=0時，易解式（23）得車輪轉(zhuǎn)角前饋量：

式中，(3)為反饋矩陣的第3列。

3.2 基于MPC的縱向跟蹤控制器設(shè)計

3.2.1 預(yù)測模型的建立

假設(shè)車輛橫擺運動幅度較小，則有：

以一階慣性環(huán)節(jié)來近似表示車輛縱向運動特性：

式中，a為縱向加速度；為a的期望值；為時間常數(shù)。

通過前向歐拉法構(gòu)建離散狀態(tài)空間，并以速度v作為系統(tǒng)輸出，建立輸出方程：

式中，()=[v a]；()=；、+1、分別為當(dāng)前采樣時刻、下一周期采樣時刻和采樣周期。

為了對加速度變化率（Jerk）進行約束，構(gòu)造狀態(tài)空間表達式：

由式（28）進行遞推迭代可得系統(tǒng)未來時刻的輸出：

3.2.2 目標(biāo)函數(shù)設(shè)計

為在確保速度跟蹤精度的同時避免產(chǎn)生大的加速度變化率，定義性能評價函數(shù)為：

式中，、為權(quán)重矩陣；為系統(tǒng)輸出參考量；||·||表示向量的歐幾里得范數(shù)和矩陣的誘導(dǎo)范數(shù)。

通過二次規(guī)劃的方法解決此優(yōu)化問題，將式（30）轉(zhuǎn)化為標(biāo)準二次型：

式中，為轉(zhuǎn)化后的常數(shù)項，在優(yōu)化求解中可忽略不計。

該目標(biāo)函數(shù)約束為：

式中，?表示克羅內(nèi)克積；、Δ分別為控制時域內(nèi)控制量、控制增量的最小值；、Δ分別為控制時域內(nèi)控制量、控制增量的最大值。

經(jīng)二次規(guī)劃（Quadratic Programming，QP）求解器對式（31）、式（32）求解，得到內(nèi)的控制輸入增量：

根據(jù)MPC 的基本原理，將控制序列內(nèi)首元素Δ()作為實際控制輸入增量作用于系統(tǒng)：

該控制量被計算至下一時刻，并在新時刻根據(jù)狀態(tài)信息重新預(yù)測下一的輸出，通過優(yōu)化過程獲得新控制增量序列，如此循環(huán)至控制過程結(jié)束。

3.2.3 油門制動標(biāo)定表的制作

基于CarSim-Simulink 聯(lián)合仿真平臺，以油門踏板開度、制動主缸壓力為自變量得到(v,a,k)、(v,a,P)的三維數(shù)據(jù)點，并擬合模型油門踏板開度=(v,a)，制動主缸壓力=(v,a)后合并，油門制動標(biāo)定表如圖4所示。

圖4 油門制動標(biāo)定

3.3 基于模糊算法的速度規(guī)劃

根據(jù)駕駛經(jīng)驗，當(dāng)前方道路曲率較大時，處于高速工況的車輛會產(chǎn)生較大的離心力，且在車輛轉(zhuǎn)向輪的轉(zhuǎn)向角速度約束影響下，被控車輛無法及時轉(zhuǎn)向，易導(dǎo)致車輛操縱穩(wěn)定性降低與側(cè)向誤差增加，甚至造成跟蹤失敗，嚴重危及道路安全。因此，車輛在跟蹤大曲率目標(biāo)路徑時若出現(xiàn)側(cè)向誤差增加的現(xiàn)象，應(yīng)適當(dāng)降低車速以提高跟蹤效能。反之，可適當(dāng)提高行車速度。

目前，隸屬函數(shù)的確定尚未有較為成熟的方法，因此本文通過不斷調(diào)試確定輸入信號采用鐘形隸屬函數(shù)()，輸出信號采用三角形隸屬函數(shù)()時控制效果較為良好：

式中，、、為廣義鐘形隸屬函數(shù)的參數(shù)，其分別決定了隸屬度為0.5時的橫軸位置、隸屬度變化速度和隸屬函數(shù)中心位置；、、為三角形隸屬函數(shù)的參數(shù)，決定了隸屬度曲線3個端點對應(yīng)的橫軸位置。

隸屬度函數(shù)與模糊規(guī)則曲面分別如圖5、圖6所示，其中橫向誤差輸入的模糊語言為S（?。?、M（中）、B（大）、VB（非常大），道路曲率輸入的模糊語言為S（?。?、M（中）、B（大）、VB（非常大），參考速度輸出的模糊語言為VS（非常?。?、S（?。?、M（中）、B（大）、VB（非常大）、UB（極其大）。

圖5 隸屬度函數(shù)示意

圖6 模糊規(guī)則曲面

4 仿真驗證

4.1 仿真參數(shù)

本文控制器使用由Fiala 開發(fā)的、由Pacejka 提出的刷式輪胎模型，且忽略在大加速度下軸荷轉(zhuǎn)移引起的前、后輪垂向力分配程度的改變，近似認為輪胎側(cè)偏剛度不發(fā)生變化。車輛所跟隨的路徑信息（道路的橫、縱向位置）通過PreScan中的采集車進行采集，仿真路面采用交通量較小的混凝土路面，其路面附著系數(shù)為0.75，仿真的主要參數(shù)如表1所示，其中依靠以往的經(jīng)驗對泛函、中的權(quán)重矩陣選取，權(quán)重矩陣的元素取值越大，意味著對應(yīng)變量的約束要求越高。

表1 主要參數(shù)設(shè)置

4.2 橫縱聯(lián)合控制效果驗證

在變曲率（僅小曲率突變）路徑下對比分析智能車在低速10 m/s、中速16.666 m/s、高速25 m/s 下跟蹤精度，結(jié)果如圖7 所示，在小曲率工況下車輛低速條件下的跟蹤誤差在8 mm 左右，中速條件下的跟蹤誤差在10 mm附近，高速條件下跟蹤誤差在50 mm以內(nèi)?？梢钥闯觯疚目刂破鞯母櫩刂凭确浅８?。本文仿真時車輛起始位置并不在路徑中心線上，F(xiàn)renet坐標(biāo)系以車輛在路徑上的投影為原點，因此在仿真開始時有固定偏移。

圖7 不同車速下的路徑跟蹤結(jié)果

圖8～圖10所示分別為不同車速下車輛油門踏板開度與橫向跟蹤誤差變化曲線、路徑曲率、速度跟蹤與縱向加速度變化曲線。可以看出，控制器輸出期望加速度后通過油門制動標(biāo)定表可以精確地實現(xiàn)對速度的跟蹤，其中圖8中車輛在高速下跟蹤路徑時橫向誤差出現(xiàn)的瞬態(tài)波動是由于道路曲率突然變大產(chǎn)生的。此外，可以看出該控制器符合速度越快，橫向誤差越大的客觀規(guī)律，當(dāng)車速較小時橫向誤差可以達到穩(wěn)態(tài)收斂。在10 m/s車速下，橫向誤差隨著路徑曲率的逐漸變大，其誤差變化范圍為0～28 mm，最終趨于穩(wěn)定。在16.666 m/s 與25 m/s車速下，橫向誤差也同樣遵循此規(guī)律，且車速越高，橫向偏差波動越大。此外，圖9 反映了路徑曲率的變化情況，可以看出仿真處于小曲率工況，曲率范圍為0～0.011 m。

圖8 油門踏板開度與橫向誤差

圖9 路徑曲率

圖10 速度跟蹤與加速度

圖11 為不同車速下轉(zhuǎn)向盤轉(zhuǎn)角的變化曲線，反映了為保證更高的跟蹤精度，橫向控制器細膩的控制方式使轉(zhuǎn)向盤轉(zhuǎn)角無大幅度、長時間的抖動，也從側(cè)面反映了該控制方式不會對車輛的穩(wěn)定性造成嚴重影響。此外，當(dāng)車速為10 m/s 時轉(zhuǎn)向盤轉(zhuǎn)角在轉(zhuǎn)向時趨于平穩(wěn)，無明顯抖動，隨著車速的提高，轉(zhuǎn)向時轉(zhuǎn)向盤的抖動略有增強。

圖11 轉(zhuǎn)向盤轉(zhuǎn)角

圖12所示為不同車速下的橫向穩(wěn)定性對比，反映了車輛在經(jīng)過變曲率的路徑時質(zhì)心側(cè)偏角與橫擺角速度隨車速的提高而增加，但始終在可控范圍內(nèi)，低速時質(zhì)心側(cè)偏角、橫擺角速度在0.01 rad、0.17 rad/s 內(nèi)循環(huán)，高速時質(zhì)心側(cè)偏角、橫擺角速度在0.03 rad、0.32 rad/s 內(nèi)循環(huán)?？梢钥闯霰疚目刂破鲗崿F(xiàn)了很好的車輛橫向穩(wěn)定性。

圖12 不同車速下的橫向穩(wěn)定性對比

4.3 大曲率工況控制效果驗證

在變曲率（大曲率突變）路徑下對比分析智能車在10 m/s 與模糊速度規(guī)劃下的跟蹤效果，結(jié)果如圖13、圖14 所示。由圖13、圖14 可知：在具有模糊速度規(guī)劃的控制器控制下，車輛跟蹤突變大曲率路徑時的跟蹤誤差穩(wěn)定在100 mm 以內(nèi)，橫、縱向跟蹤誤差均在50 mm 左右；無模糊速度規(guī)劃的車輛在道路曲率突變增大時跟蹤誤差較大，且橫向誤差在160 mm 左右，縱向誤差在300 mm 左右。此外，圖13 也反映了在保證跟蹤精度的同時，車輛可以更短的時間完成總路徑的跟蹤。

圖13 路徑跟蹤結(jié)果

圖14 橫縱向跟蹤效果

圖15所示為有速度規(guī)劃條件下的速度跟蹤與縱向加速度結(jié)果，每一時刻對速度的控制皆可達到快速、平穩(wěn)的效果，速度響應(yīng)曲線全過程無超調(diào)，其中圖15a 反映了道路曲率苛刻的變化情況（0～0.08 m）。圖15 體現(xiàn)了控制器可對車輛進行精確的縱向控制與其性能的可靠性和優(yōu)越性。

圖15 有速度規(guī)劃下的速度跟蹤與加速度

圖16 所示為2 種控制方法下橫向誤差與轉(zhuǎn)向盤轉(zhuǎn)角的變化曲線，在跟蹤過程中無模糊速度規(guī)劃時的路徑跟蹤在通過變曲率（大曲率突變）路徑時橫向誤差突增至超過1 m，而模糊速度規(guī)劃下的控制器緩解了這一問題，在轉(zhuǎn)向盤轉(zhuǎn)角方面，其轉(zhuǎn)向弧度更小，約為-4.1 rad，間接反映了具有較好的橫向穩(wěn)定性。

圖16 橫向誤差與轉(zhuǎn)向盤轉(zhuǎn)角

圖17 所示為不同控制方式下的橫向穩(wěn)定性對比，其中在速度規(guī)劃下產(chǎn)生橫擺角速度變動區(qū)域更小，在-0.55～0.30 rad/s 范圍內(nèi)，其在路徑后半段質(zhì)心側(cè)偏角略有增加，但仍可控，車輛仍具有較高的橫向穩(wěn)定性。

圖17 橫向穩(wěn)定性對比

5 結(jié)束語

本文為智能車設(shè)計了一種路徑跟蹤控制器，采用DLQR前反饋補償?shù)姆绞竭M行橫向控制，同時在模糊速度規(guī)劃下以MPC 方式進行縱向控制，既保證了小曲率工況下不同車速的跟蹤效果，又確保了大曲率工況下仍具有良好的跟蹤性能。仿真驗證結(jié)果表明，橫縱跟蹤控制器具有較強的魯棒性，保證了車輛在不同曲率工況下的跟蹤精度與橫向穩(wěn)定性，滿足實際工程應(yīng)用要求。