摘要：為在嵌入式控制器開發(fā)環(huán)境下提高智能車輛的路徑跟蹤精度，采用車輛動力學(xué)模型和多點(diǎn)道路預(yù)瞄模型，以 預(yù)瞄窗口內(nèi)的跟蹤偏差為目標(biāo)函數(shù)，結(jié)合 LQR 最優(yōu)控制原理，提出了一種基于多點(diǎn)預(yù)瞄最優(yōu)控制路徑跟蹤控制方法。針對實(shí)車應(yīng)用，通過離線計(jì)算最優(yōu)增益的方法，提高算法實(shí)時(shí)性。在仿真及紅旗 H7 實(shí)車環(huán)境下進(jìn)行試驗(yàn)，結(jié)果顯示，該方法在保證跟蹤精度的同時(shí)具有良好的算法實(shí)時(shí)性。

1.前言

路徑跟蹤作為自動駕駛系統(tǒng)中的關(guān)鍵執(zhí)行層控制技術(shù)，是影響智能車輛安全性與舒適性的關(guān)鍵技術(shù)[1-3]。常見的路徑跟蹤方法按照使用模型不同可分為基于幾 何/運(yùn)動學(xué)模型的方法和基于動力學(xué)模型的方法，其中 基于幾何/運(yùn)動學(xué)模型的方法，通常將車輛簡化為四輪 機(jī)器人剛性結(jié)構(gòu)，由于相對簡單計(jì)算量小，已有較多實(shí) 車應(yīng)用，如名古屋大學(xué)的自動駕駛開源項(xiàng)目Autoware 中使用的純跟蹤方法[4]。

基于動力學(xué)模型的方法，考慮車 輛輪胎側(cè)偏等動力學(xué)特性，通常結(jié)合最優(yōu)控制理論，如線性二次型調(diào)節(jié)器（LQR）[5]、模型預(yù)測控制（MPC）[6]等。百度的自動駕駛開源項(xiàng)目Apollo 中橫向控制器即提供了LQR 和MPC 兩種方法[7]。LQR 方法未考慮預(yù)瞄前方目標(biāo)路徑，易出現(xiàn)跟蹤偏差較大問題；MPC方法需要消耗大量計(jì)算資源，在車輛嵌入式控制器的計(jì)算環(huán)境中難以實(shí)現(xiàn)。而基于預(yù)瞄的LQR控制方法[8]，既能夠綜合考慮前方路徑特征，又不需要在線優(yōu)化求解占用大量計(jì)算 資源，具有較強(qiáng)的嵌入式環(huán)境實(shí)車應(yīng)用價(jià)值。

因此，本文在文獻(xiàn)[8]LQR預(yù)瞄控制方法的基礎(chǔ)上， 提出了一種新型基于多點(diǎn)預(yù)瞄的最優(yōu)路徑跟蹤控制方 法，改進(jìn)了道路模型中的預(yù)瞄偏差計(jì)算方式，以及采用 離線計(jì)算最優(yōu)增益的方法，提高了算法的適應(yīng)性及實(shí)時(shí) 性，更加適合實(shí)車路徑跟蹤控制應(yīng)用。

2.車輛及道路預(yù)瞄模型

2.1車輛動力學(xué)模型

本文采用的車輛動力學(xué)模型如圖1 所示，假設(shè)車輛是一個(gè)在平面內(nèi)沿一定速度向前行駛的剛體，可通過前輪轉(zhuǎn)角進(jìn)行橫擺旋轉(zhuǎn)和側(cè)向平移運(yùn)動。

圖1 車輛動力學(xué)模型

通過如圖1 所示的幾何關(guān)系，可推導(dǎo)出前后軸的側(cè)偏角αf、αr關(guān)系式：

式中，y為車輛橫向位置；a、b分別為車輛質(zhì)心到前后軸的距離；ψ為車輛航向角；u為車輛縱向速度；δsw為轉(zhuǎn)向盤轉(zhuǎn)角；isteer為轉(zhuǎn)向傳動比。

然后由車輛前后軸的側(cè)偏剛度Cf、Cr ,可知前后軸的側(cè)向力Fvf、Fvr:

最后，根據(jù)牛頓第二定律進(jìn)行側(cè)向力和橫擺力矩分析即可得到式（5）所示的動力學(xué)狀態(tài)方程。

式中：

其中，M為整車質(zhì)量；I為車輛繞 Z軸橫擺的轉(zhuǎn)動慣量。

式（5）狀態(tài)方程為連續(xù)量，在控制器中采用的是離散數(shù)字控制，需要根據(jù)實(shí)際采樣時(shí)間T 對狀態(tài)方程進(jìn)行離散化處理，如式（6）所示：

式中：

由此狀態(tài)方程即可確定車輛在一定速度下轉(zhuǎn)向盤轉(zhuǎn)角作為控制量，對車輛橫向位置及橫擺角速度狀態(tài)產(chǎn)生的影響。

2.2 道路預(yù)瞄模型

圖2（a）所示的為文獻(xiàn)[8]所使用的道路預(yù)瞄模型，以道路在局部坐標(biāo)系下的未來n個(gè)周期的參考y軸方向橫向偏移作為道路狀態(tài)量，當(dāng)前車輛在局部坐標(biāo)系中的橫向坐標(biāo)為y，朝向角為ψ，假設(shè)車輛縱向速度u恒定，當(dāng)前車輛坐標(biāo)為 y，第一個(gè)道路狀態(tài)量即為 yr0，第二個(gè)狀態(tài)量即車輛沿 x 軸方向移動 uT 后，對應(yīng)的道路 y 坐標(biāo)，即為圖中的yr1，以此類推再下一周期同樣x向移動uT，狀態(tài)量變?yōu)閥r2，yr3，yrn。

此模型采用的局部坐標(biāo)系方向固定不變，車體坐標(biāo) 系與局部坐標(biāo)系夾角位置關(guān)系隨著車輛運(yùn)動不斷變化， 當(dāng)車輛朝向角與x軸夾角過大時(shí)，車輛的每一時(shí)刻x向參考位置的變化將與uT 差別較大，使得算法對于這種情況的適應(yīng)性較差。因此本文的局部坐標(biāo)系直接采用車體坐標(biāo)系作為參考，車輛的初始y向坐標(biāo)為0，航向角ψ也為0，這樣車輛沿x軸的參考位置uT對應(yīng)橫向參考坐標(biāo)隨著車體坐標(biāo)系的不斷變化而不斷變化，縱向速度假設(shè)更加合理，算法適應(yīng)性更強(qiáng)。

圖2 道路預(yù)瞄關(guān)系圖

對于車輛而言，k時(shí)刻的道路狀態(tài)yrn即為k+1時(shí)刻的道路狀態(tài)yrn-1，只需將前一時(shí)刻的道路狀態(tài)整體向前移動一個(gè)周期，同時(shí)補(bǔ)充最后一時(shí)刻的狀態(tài)yr_n+1，可將道路狀態(tài)整理為如下矩陣形式：

式中：

3.LQR最優(yōu)預(yù)瞄控制

整合第2 章的車輛模型和道路預(yù)瞄模型的狀態(tài)方程（6）和（7），可得下式：

此狀態(tài)方程僅描述了車輛和道路間狀態(tài)量的變化關(guān)系，而車輛和道路則需要通過式（9）所示的目標(biāo)函數(shù)產(chǎn)生關(guān)聯(lián)：

式中：

該目標(biāo)函數(shù)為LQR 標(biāo)準(zhǔn)的二次型形式，一共包含兩項(xiàng)，第一項(xiàng)目標(biāo)為路徑跟蹤的橫向與角度的綜合偏差，C矩陣的第一行與式（8）的狀態(tài)相乘，即連接了車輛的y軸坐標(biāo)與道路參考點(diǎn)的y 軸坐標(biāo)差，得到車輛橫向偏差；第二行連接了車輛的航向角與道路朝向角的差， 得到車輛角度偏差，同時(shí)Q矩陣中的兩個(gè)元素q1和q2決定了橫向偏差與角度偏差的權(quán)重；第二項(xiàng)表示控制量轉(zhuǎn) 向盤轉(zhuǎn)角的懲罰項(xiàng)，目的為減少轉(zhuǎn)向盤轉(zhuǎn)角輸出，提高系統(tǒng)穩(wěn)定性，R為該項(xiàng)權(quán)重標(biāo)量。

由上述目標(biāo)函數(shù)和線性狀態(tài)方程，即可構(gòu)成標(biāo)準(zhǔn)的離散LQR 控制問題，從而求得由最優(yōu)增益 K 和狀態(tài)量[X(k),yr(k)]T構(gòu)成的最優(yōu)控制量u(k)：

最優(yōu)增益K=[R+BTPB]-1BTPA，其中P矩陣可通過代數(shù)黎卡提方程求解：

當(dāng)?shù)缆奉A(yù)瞄窗口較大時(shí)，上述矩陣維數(shù)將明顯增多， 求解黎卡提方程將消耗較多計(jì)算資源，在嵌入式硬件環(huán) 境中實(shí)時(shí)求解難度較大。通過對最優(yōu)增益矩陣中的元素 進(jìn)行分析，可以發(fā)現(xiàn)只有車速u是動態(tài)變化的，其他元素均為車輛固有參數(shù)，因此，本文采用離線計(jì)算不同速度u下的增益矩陣K，然后通過在線查表的方式使用與當(dāng)前車速u最鄰近的離線增益K，然后與當(dāng)前車輛狀態(tài)、道路狀態(tài)[X(k),yr(k)]T按式（10）計(jì)算即可獲得控制輸出轉(zhuǎn)向盤轉(zhuǎn)角，大幅提升算法實(shí)時(shí)性。

4.仿真及實(shí)車驗(yàn)證

4.1仿真結(jié)果

為驗(yàn)證本文算法的跟蹤精度與算法實(shí)時(shí)性，在CarSim 和Simulink 聯(lián)合仿真的環(huán)境下，將本文方法與文獻(xiàn)[9]所提出的模型預(yù)測控制方法進(jìn)行了對比測試，仿真對比結(jié)果見圖3 和圖4。

仿真所使用的為CarSim中的車輛，其質(zhì)量為1 723 kg，轉(zhuǎn)動慣量為4175kg·m2，質(zhì)心距前后軸距分別為1.232m和1.468m，前后軸側(cè)偏剛度分別為119552N/rad和109 548 N/rad。

仿真試驗(yàn)工況選用雙移線軌跡進(jìn)行車輛路徑跟蹤能力測試。分別選用常規(guī)小側(cè)向力工況和達(dá)到路面附著極限的工況進(jìn)行仿真，縱向速度分別為 60km/h和80km/h，路面附著系數(shù)為1，控制周期為20ms，MPC控制器的預(yù)測時(shí)域和控制時(shí)域均為50，權(quán)重矩陣Q和R的取值與文獻(xiàn)[9]中保持一致；本文LQR控制器預(yù)瞄周期也為1s，權(quán)重矩陣系數(shù)q1=0.001，q2=0，R=1。

圖4 側(cè)向加速度仿真結(jié)果曲線

從路徑跟蹤結(jié)果中可以看出60km/h時(shí)LQR控制器與MPC控制器均能較好的跟蹤目標(biāo)路徑，其中LQR控制器的最大跟蹤偏差為0.36m，而MPC控制器的最大跟蹤偏差為0.64m；而圖4中可以看出最大側(cè)向加速度約為0.7g，尚未達(dá)到路面附著極限，提出的LQR控制器能夠較好地跟蹤目標(biāo)路徑；而在80km/h的仿真試驗(yàn)可以看出最大側(cè)向加速度達(dá)到1g左右，已經(jīng)達(dá)到了路面附著極限，LQR控制器由于不具有MPC控制器的側(cè)偏角約束，未能保持車輛穩(wěn)定性，出現(xiàn)側(cè)滑現(xiàn)象。另一方面，本文提出的LQR控制器與MPC控制器仿真單步運(yùn)行時(shí)間分別為8.5ms和89ms，LQR方法具有明顯的實(shí)時(shí)性優(yōu)勢。由此可見，本文提出的LQR控制器由于具有與MPC 控制器相同的優(yōu)化目標(biāo)，較為適合實(shí)車在附著良好的且未達(dá)到附著極限的行駛工況下進(jìn)行實(shí)時(shí)控制。

4.2實(shí)車試驗(yàn)

實(shí)車控制器采用的是MicroAutoBox快速原型，LQR控制方法可在該平臺下良好地實(shí)時(shí)運(yùn)行，傳感器為RTK- IMU 組合導(dǎo)航記錄位姿，目標(biāo)路徑采用人工駕駛過程中錄制組合導(dǎo)航的位置姿態(tài)數(shù)據(jù)，試驗(yàn)場地為一汽NBD 園區(qū)內(nèi)，試驗(yàn)車輛為紅旗H7 摯途試驗(yàn)車，如圖5 所示。

圖5 紅旗H7 摯途試驗(yàn)車

園區(qū)內(nèi)試驗(yàn)最大車速為30km/h，覆蓋了長直線、長彎、環(huán)島和直角彎等城市道路典型工況，試驗(yàn)結(jié)果如圖6所示，整車最大側(cè)向加速度在0.2 g 以內(nèi)，橫向控制偏差在0.35 m 以內(nèi)，具有良好的跟蹤精度；以120 s 附近的過環(huán)島工況為例，轉(zhuǎn)向盤最大轉(zhuǎn)角為281°，路徑跟蹤的最大橫向偏差為0.28m，轉(zhuǎn)向盤轉(zhuǎn)動較為平穩(wěn)。

圖6 實(shí)車路徑跟蹤結(jié)果曲線

圖7所示為LQR控制器在彎曲道路換道工況下的實(shí)車路徑跟蹤結(jié)果圖，圖7a 所示為參考路徑和跟蹤路徑，車速及跟蹤的橫向偏差如圖7b所示，換道時(shí)車速約為60km/h，整體跟蹤偏差在0.3m以內(nèi)，最大側(cè)向加速度約3.2m/s2。說明本文方法適合在實(shí)車嵌入式實(shí)時(shí)環(huán)境下，平穩(wěn)地控制車輛實(shí)現(xiàn)良好精度的路徑跟蹤功能。

圖7 彎曲道路換道工況路徑跟蹤結(jié)果圖

5.結(jié)束語

本文針對嵌入式控制器環(huán)境下的智能車輛路徑跟蹤問題，基于LQR 最優(yōu)控制方法并采用離線優(yōu)化的方式，提出了一種基于多點(diǎn)預(yù)瞄最優(yōu)路徑跟蹤控制方法， 構(gòu)建了車輛及道路預(yù)瞄模型。仿真及實(shí)車試驗(yàn)表明該 方法能夠適應(yīng)常規(guī)工況下實(shí)車路徑跟蹤控制，具有良好 的跟蹤精度。下一步研究將針對輪胎和路面附著極限 工況下，優(yōu)化輪胎模型，約束最終控制量輸出保證車輛 穩(wěn)定性，提高算法對極限工況的適應(yīng)能力。