采用余弦相似度和變預(yù)測(cè)時(shí)域的汽車軌跡追蹤控制

鐘聰，彭育輝，黃煒

(福州大學(xué)機(jī)械工程及自動(dòng)化學(xué)院，福建 福州 350108)

0 引言

作為支撐汽車自動(dòng)駕駛的關(guān)鍵技術(shù)之一，軌跡追蹤要同時(shí)具有優(yōu)異的行駛穩(wěn)定性和滿足追蹤精度的要求.軌跡追蹤的橫向控制方法主要包括PID控制、滑?？刂?、線性二次高斯控制、模型預(yù)測(cè)控制(model predictive control，MPC)等[1-4].由于MPC能夠系統(tǒng)地處理汽車模型存在的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)約束問題，故被廣泛地應(yīng)用于自動(dòng)駕駛汽車的軌跡追蹤控制中.Lin等[5]設(shè)計(jì)一種MPC自適應(yīng)控制器，基于線性輪胎模型在線實(shí)時(shí)估計(jì)輪胎側(cè)偏剛度，并更新控制器預(yù)測(cè)模型，但未考慮中高速工況下輪胎的非線性特性，故在中高速工況下的追蹤精度與穩(wěn)定性并不理想.蔡英鳳等[6-7]利用可拓集的關(guān)聯(lián)函數(shù)動(dòng)態(tài)分配軌跡追蹤精度和橫向穩(wěn)定性的權(quán)重系數(shù)，建立智能車輛軌跡追蹤的多目標(biāo)模型預(yù)測(cè)可拓協(xié)調(diào)控制方法.Wang等[8]設(shè)計(jì)一種改進(jìn)型MPC控制器，結(jié)合模糊控制來實(shí)時(shí)改變MPC的權(quán)重參數(shù)，在保證追蹤精度的同時(shí)，也提高車輛的轉(zhuǎn)向穩(wěn)定性.Yuan等[9]設(shè)計(jì)一種車速自適應(yīng)的MPC控制器，通過模糊控制和神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)器獲得期望航向偏差，以解決控制器在不同車速下的過早轉(zhuǎn)向和轉(zhuǎn)向超調(diào)問題.白國星等[10]基于非線性模型預(yù)測(cè)控制算法，設(shè)計(jì)一種可變預(yù)測(cè)時(shí)域的軌跡跟蹤控制器，提高控制器對(duì)中低速工況的自適應(yīng)性.Rokonuzaman等[11]以MPC控制器為基礎(chǔ)，利用神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)車輛動(dòng)力學(xué)模型使用過的數(shù)據(jù)，得到被控車輛的前輪轉(zhuǎn)角，在參數(shù)不確定的情況下該方法的控制性能優(yōu)于傳統(tǒng)MPC，但神經(jīng)網(wǎng)絡(luò)的參數(shù)設(shè)置問題仍有待解決.

綜上，目前基于MPC的汽車軌跡追蹤算法仍面臨兩個(gè)關(guān)鍵問題：一是控制策略未考慮預(yù)測(cè)模型狀態(tài)矩陣變化和預(yù)測(cè)時(shí)域變化對(duì)軌跡追蹤精度、車輛穩(wěn)定性的影響；二是控制器對(duì)高速工況的自適應(yīng)能力較差.本研究提出一種基于余弦相似度和變預(yù)測(cè)時(shí)域的軌跡追蹤控制策略，能根據(jù)實(shí)際行駛工況變化對(duì)預(yù)測(cè)模型、預(yù)測(cè)時(shí)域、控制時(shí)域進(jìn)行相應(yīng)的在線優(yōu)化，從而提高控制器對(duì)高速工況的自適應(yīng)能力和軌跡追蹤精度.

1 控制策略的總體思路

可在線優(yōu)化MPC軌跡追蹤控制方法的算法包括線性時(shí)變MPC控制器的求解和預(yù)測(cè)模型的在線優(yōu)化兩個(gè)模塊.基于初始預(yù)測(cè)模型，根據(jù)車輛當(dāng)前狀態(tài)、參考位置和參考橫擺角，通過線性時(shí)變MPC控制器求解出最優(yōu)前輪轉(zhuǎn)角；再根據(jù)車輛實(shí)時(shí)狀態(tài)、縱向車速和橫向車速，預(yù)測(cè)模型優(yōu)化模塊，計(jì)算出當(dāng)前預(yù)測(cè)模型狀態(tài)矩陣和初始預(yù)測(cè)模型狀態(tài)矩陣的余弦相似度.如果余弦相似度低于預(yù)定閾值，就將當(dāng)前的狀態(tài)矩陣更新到MPC的預(yù)測(cè)模型中.同時(shí)，根據(jù)當(dāng)前的縱向車速和橫向車速，基于模糊控制規(guī)則對(duì)預(yù)測(cè)時(shí)域和控制時(shí)域進(jìn)行相應(yīng)的調(diào)整優(yōu)化，以期提升控制器對(duì)高速工況的自適應(yīng)能力、提高汽車軌跡追蹤的精度和橫擺穩(wěn)定性.

2 基于MPC的軌跡追蹤控制方法

圖1 車輛動(dòng)力學(xué)模型Fig.1 Model of vehicle dynamics

車輛側(cè)向運(yùn)動(dòng)和橫擺運(yùn)動(dòng)的動(dòng)力學(xué)微分方程[12]為：

(1)

式中：m為車輛質(zhì)量；Iz為車輛相對(duì)z軸的轉(zhuǎn)動(dòng)慣量；Cf，Cr分別為前后輪的側(cè)偏剛度.

連續(xù)時(shí)間狀態(tài)空間方程[12]為：

(2)

狀態(tài)空間表達(dá)式為：

(3)

為簡化車輛模型，提高M(jìn)PC控制器的計(jì)算速度，假定車輛前、后輪側(cè)偏剛度為定值，且車輛載荷不發(fā)生變化，即狀態(tài)矩陣A為線性定常矩陣.離散化后的狀態(tài)空間表達(dá)式[12]為：

(4)

系統(tǒng)預(yù)測(cè)時(shí)域?yàn)镹p，控制時(shí)域?yàn)镹c，系統(tǒng)預(yù)測(cè)時(shí)域內(nèi)的輸出序列可表示為：

(5)

根據(jù)公式(5)，以前輪轉(zhuǎn)角增量為新的控制變量，可以得到一組新的狀態(tài)空間方程：

Y0(k)=R0ξ0(k)+Z0ΔU0(k)

(6)

參考橫向位置Yr和參考橫擺角φr為已知量，作為參考信號(hào)輸入，用來計(jì)算汽車的實(shí)時(shí)橫向位置和橫擺角度.軌跡追蹤控制是為實(shí)現(xiàn)預(yù)測(cè)時(shí)域內(nèi)車輛狀態(tài)的預(yù)測(cè)，由價(jià)值函數(shù)J計(jì)算出控制時(shí)域內(nèi)的輸出序列，使車輛的實(shí)際行駛路徑盡可能地貼近參考路徑.即：

(7)

考慮車輛實(shí)際特性，還需對(duì)系統(tǒng)的狀態(tài)量和控制量進(jìn)行一定的約束，包括：1) 橫向位置約束：ymin≤y(k)≤ymax；2) 橫擺角約束：φmin≤φ(k)≤φmax；3) 控制量約束：umin≤u(k)≤umax；4) 控制增量約束：Δδmin≤Δu(k)≤Δδmax.

將價(jià)值函數(shù)(7)改寫成一個(gè)標(biāo)準(zhǔn)的二次規(guī)劃問題：

(8)

在每一個(gè)優(yōu)化周期內(nèi)，優(yōu)化目標(biāo)為求取價(jià)值函數(shù)J的最小值，得到控制時(shí)域內(nèi)控制變量的增量：

(9)

如下式所示，選取控制序列的第一個(gè)元素作為實(shí)際的控制變量(即前輪轉(zhuǎn)角)作用于被控對(duì)象.

u(k)=u(k-1)+Δu*(k)

(10)

3 可在線優(yōu)化的方法

3.1 采用余弦相似度的狀態(tài)矩陣在線優(yōu)化

上述線性時(shí)變MPC控制器的狀態(tài)矩陣是基于預(yù)先設(shè)定的縱向車速來獲取的.當(dāng)縱向車速變化率過大或車速達(dá)到30 m·s-1時(shí)，將不能滿足無人駕駛追蹤精度和穩(wěn)定性的要求，甚至?xí)霈F(xiàn)車輛失穩(wěn)的現(xiàn)象[13].

為提高控制器對(duì)不同車速的自適應(yīng)能力和模型預(yù)測(cè)的精度，要求控制器能根據(jù)汽車實(shí)際車速變化對(duì)預(yù)測(cè)模型的狀態(tài)矩陣進(jìn)行優(yōu)化.因此，提出一種基于余弦相似度的狀態(tài)矩陣在線優(yōu)化策略，即用矩陣的余弦相似度來度量兩個(gè)矩陣的差異程度，從而確定預(yù)測(cè)模型的優(yōu)化節(jié)點(diǎn)，獲得最符合當(dāng)前車速的預(yù)測(cè)模型.兩個(gè)狀態(tài)矩陣A和A*的余弦相似度r為：

(11)

式中：A為用歷史車速計(jì)算得到的狀態(tài)矩陣；A*為根據(jù)實(shí)時(shí)車速計(jì)算得到的狀態(tài)矩陣.

余弦相似度的值介于0和1之間，該值越接近1，說明矩陣A*越接近A，兩個(gè)矩陣相似度越高.一旦兩個(gè)狀態(tài)矩陣的余弦相似度低于預(yù)設(shè)閾值，就將車輛此時(shí)的實(shí)際速度代入式(3)中，以此更新優(yōu)化狀態(tài)矩陣A.

3.2 預(yù)測(cè)模型的在線優(yōu)化

控制器參數(shù)的選取對(duì)軌跡跟蹤效果影響較大，預(yù)測(cè)時(shí)域和控制時(shí)域是其中最重要的兩個(gè)參數(shù)[10].預(yù)測(cè)時(shí)域過大，控制器將會(huì)過多地考慮前方距當(dāng)前位置較遠(yuǎn)處的誤差，導(dǎo)致追蹤精度下降；預(yù)測(cè)時(shí)域過小，將會(huì)導(dǎo)致控制量非常接近甚至超過約束值，致使汽車無法及時(shí)轉(zhuǎn)向，降低車輛穩(wěn)定性.若控制時(shí)域較大，在精確度沒有明顯提升的情況下，會(huì)增加計(jì)算量；若控制時(shí)域較小，控制器會(huì)對(duì)誤差過于敏感，降低車輛穩(wěn)定性.綜上，車輛在橫向車速較大時(shí)，控制時(shí)域應(yīng)該減小，以保證車輛的行駛穩(wěn)定性；在橫向車速較小時(shí)，控制時(shí)域應(yīng)該增大；在縱向車速較大時(shí)，為提高追蹤精度，預(yù)測(cè)時(shí)域應(yīng)該增大；在轉(zhuǎn)彎的時(shí)候，預(yù)測(cè)時(shí)域要相應(yīng)地減小.因此，預(yù)測(cè)時(shí)域和控制時(shí)域的大小應(yīng)該通過實(shí)際縱向車速和橫向車速共同決定.

模糊控制不需要建立精確的數(shù)學(xué)模型，且其自適應(yīng)能力和魯棒性較強(qiáng).因此，根據(jù)縱向車速和橫向車速，采用模糊控制方法對(duì)時(shí)域系數(shù)τ進(jìn)行調(diào)節(jié)，進(jìn)而調(diào)節(jié)預(yù)測(cè)時(shí)域和控制時(shí)域，以適應(yīng)軌跡跟蹤精度和穩(wěn)定性的要求.經(jīng)過Matlab仿真，最終制定的模糊規(guī)則如表1所示.獲得的輸入縱向車速、橫向車速與輸出時(shí)域系數(shù)的響應(yīng)關(guān)系如圖2所示.

表1 時(shí)域系數(shù)的模糊控制規(guī)則Tab.1 Fuzzy control rules of the time domain coefficients

圖2 時(shí)域系數(shù)與車速的關(guān)系Fig.2 Diagram of time domain coefficient and speed

為進(jìn)一步提升時(shí)域系數(shù)的自適應(yīng)能力，需要充分考慮縱向車速和橫向車速對(duì)預(yù)測(cè)時(shí)域和控制時(shí)域的影響.當(dāng)車輛在直線道路上行駛時(shí)，橫向車速vy很小，近似為0，此時(shí)預(yù)測(cè)時(shí)域和控制時(shí)域可設(shè)置大一些；當(dāng)汽車轉(zhuǎn)彎時(shí)，vy會(huì)隨著道路曲率的增加而增加，此時(shí)轉(zhuǎn)向角較大，預(yù)測(cè)時(shí)域和控制時(shí)域要相應(yīng)地減小.因此，預(yù)測(cè)時(shí)域和控制時(shí)域的計(jì)算式為：

(12)

式中：T為采樣時(shí)間，取0.01 s；ω為權(quán)重系數(shù).

根據(jù)式(12)得到當(dāng)前時(shí)刻t的狀態(tài)空間模型和價(jià)值函數(shù)分別為：

(13)

4 仿真驗(yàn)證

4.1 預(yù)測(cè)模型狀態(tài)矩陣在線優(yōu)化的有效性驗(yàn)證

表2 不同閾值對(duì)余弦相似度及其變化率的影響Tab.2 Influence of different thresholds on cosine similarity and its change rate

當(dāng)前狀態(tài)矩陣和歷史狀態(tài)矩陣的余弦相似度低于優(yōu)化閾值(TH)時(shí)，就對(duì)當(dāng)前狀態(tài)矩陣進(jìn)行優(yōu)化更新.為分析不同優(yōu)化閾值對(duì)余弦相似度及其變化率的影響，對(duì)4組不同閾值(0.80、0.85、0.90和0.95)[14]下的狀態(tài)矩陣余弦相似度及其變化率進(jìn)行Matlab/Simulink仿真分析，其具體數(shù)值和仿真所得對(duì)比數(shù)據(jù)如表2所示，仿真結(jié)果對(duì)比如圖3所示.由表2和圖3可以看出，隨著閾值的增大，狀態(tài)矩陣的優(yōu)化次數(shù)明顯增加，相鄰優(yōu)化節(jié)點(diǎn)間的最大速度差(Δvmax)和余弦相似度的變化率呈現(xiàn)先增大后減小的趨勢(shì).閾值為0.90時(shí)的優(yōu)化次數(shù)、Δvmax和余弦相似度的變化率都要優(yōu)于閾值為0.80、0.85時(shí)的值.與閾值為0.95時(shí)相比，閾值為0.90時(shí)的Δvmax有很大提升，說明此時(shí)速度的適應(yīng)范圍較廣.閾值為0.90和0.95時(shí)的優(yōu)化次數(shù)相同，說明這兩種情況下的計(jì)算量相當(dāng).如果繼續(xù)增大閾值，當(dāng)前狀態(tài)矩陣和歷史狀態(tài)矩陣的余弦相似度將更難達(dá)到閾值，優(yōu)化次數(shù)會(huì)相應(yīng)增加，從而導(dǎo)致計(jì)算量的顯著增加，影響控制器的實(shí)時(shí)性.綜上，狀態(tài)矩陣優(yōu)化后的預(yù)測(cè)模型對(duì)實(shí)時(shí)變化的車速具有更強(qiáng)的適應(yīng)能力.同時(shí)，當(dāng)閾值取0.90時(shí)，預(yù)測(cè)模型既能保證較強(qiáng)的速度自適應(yīng)性，又無需頻繁更新狀態(tài)矩陣，進(jìn)而減小MPC控制器的計(jì)算強(qiáng)度.

圖3 不同閾值對(duì)余弦相似度及其變化率的影響Fig.3 Influence of different thresholds on cosine similarity and its change rate

4.2 預(yù)測(cè)時(shí)域和控制時(shí)域的權(quán)重系數(shù)的確定

為合理確定權(quán)重系數(shù)，需先研究預(yù)測(cè)時(shí)域和控制時(shí)域的權(quán)重系數(shù)對(duì)軌跡追蹤控制器的精度和穩(wěn)定性的影響.對(duì)4組不同權(quán)重系數(shù)(0.6、0.7、0.8和0.9)[15]下的軌跡追蹤精度和穩(wěn)定性進(jìn)行Matlab/Simulink和CarSim聯(lián)合仿真分析.圖4為選取的縱向速度，速度變化范圍為5～30 m·s-1.圖5為不同權(quán)重系數(shù)下追蹤時(shí)的橫向速度變化，仿真工況選取雙移線工況.圖6為不同權(quán)重系數(shù)下追蹤時(shí)的橫向位置和橫擺角變化.

圖4 縱向車速Fig.4 Longitudinal velocity

圖5 不同權(quán)重系數(shù)下的橫向速度Fig.5 Lateral velocity with different weight coefficients

圖6 不同權(quán)重系數(shù)對(duì)追蹤精度和穩(wěn)定性的影響Fig.6 Influence of different weight coefficients on tracking accuracy and stability

表3 橫擺角均方根和橫向速度均方根對(duì)比Tab.3 Root mean square of yaw angle and root mean square of transverse velocity

由圖5、6和表3可以看出，隨著權(quán)重系數(shù)的增大，汽車橫擺角均方根和橫向速度均方根先減小后增大.當(dāng)ω=0.8時(shí)，汽車橫擺角均方根和橫向車速均方根的值都要小于其他3組值，汽車橫擺角曲線和橫向速度曲線的平滑度和波動(dòng)都要優(yōu)于其他3組曲線，說明此權(quán)重系數(shù)下的車輛穩(wěn)定性較好.進(jìn)一步分析圖5可知，當(dāng)ω=0.8時(shí)，橫向速度能適應(yīng)道路曲率的變化，說明此權(quán)重系數(shù)下的車輛穩(wěn)定性較好，對(duì)道路彎道的敏感性較強(qiáng)，車輛橫向車速能隨著彎道的增大而增大.在大轉(zhuǎn)角工況下，即橫向車速處于相對(duì)較高的區(qū)域，控制時(shí)域不會(huì)過小，可保證車輛在軌跡追蹤時(shí)的穩(wěn)定性.因此，ω取0.8較為合適.

4.3 控制器整體的有效性驗(yàn)證

在Matlab/Simulink和CarSim聯(lián)合仿真平臺(tái)中，選取雙移線工況和圖4所示的縱向速度，驗(yàn)證所構(gòu)建的可在線優(yōu)化MPC控制器對(duì)不同車速和不同道路曲率的自適應(yīng)能力.

圖7、8分別顯示軌跡追蹤結(jié)果和狀態(tài)矩陣在線優(yōu)化的節(jié)點(diǎn).如圖7所示，在第一彎道和第二彎道處，可在線優(yōu)化MPC控制器的追蹤精度和平穩(wěn)性都要優(yōu)于線性時(shí)變MPC控制器.如圖8所示，在第三彎道之前，可在線優(yōu)化MPC控制器預(yù)測(cè)模型的狀態(tài)矩陣進(jìn)行5次優(yōu)化，每次優(yōu)化都會(huì)使得當(dāng)前的預(yù)測(cè)模型更加匹配實(shí)際車速，從而提高軌跡追蹤的精度和穩(wěn)定性.在第三彎道處，實(shí)際車速已達(dá)30 m·s-1.由于線性時(shí)變MPC控制器沒有進(jìn)行模型優(yōu)化，故預(yù)測(cè)模型不能很好匹配當(dāng)前高速工況，導(dǎo)致軌跡追蹤失敗，車輛失去穩(wěn)定性；而可在線優(yōu)化MPC控制器在第三彎道前進(jìn)行預(yù)測(cè)模型優(yōu)化，使得整個(gè)預(yù)測(cè)模型與實(shí)際高速工況有很高的匹配度，因此在第三彎道處仍有較好的軌跡追蹤能力.利用平均絕對(duì)百分比誤差對(duì)橫向位置和橫擺角進(jìn)行評(píng)價(jià)對(duì)比，可在線優(yōu)化MPC控制器比線性時(shí)變MPC控制器的追蹤精度和橫擺穩(wěn)定性分別提高29.45%和18.19%.仿真結(jié)果表明：與線性時(shí)變MPC控制器相比，所提出的可在線優(yōu)化MPC控制器在不同車速下的軌跡追蹤精度和車輛穩(wěn)定性均有明顯的提升.

圖7 軌跡追蹤結(jié)果Fig.7 Trajectory tracking results

圖9表示軌跡追蹤過程中的車輛橫向速度變化；圖10表示軌跡追蹤時(shí)，預(yù)測(cè)時(shí)域和控制時(shí)域隨縱向、橫向車速變化響應(yīng)圖.由圖9和圖10可知，在第一彎道處，隨著縱向車速的下降和橫向車速的變化，可在線優(yōu)化MPC控制器的預(yù)測(cè)時(shí)域和控制時(shí)域都能相應(yīng)的減小，提高軌跡追蹤精度和穩(wěn)定性.在第二彎道處，隨著縱向車速的增加和橫向車速的正向增加，預(yù)測(cè)時(shí)域和控制時(shí)域也相應(yīng)的增大.同時(shí)，控制時(shí)域的增長幅度相比于預(yù)測(cè)時(shí)域要低，可避免因控制時(shí)域過大而引起計(jì)算量增加.在第三彎道處，車輛橫向車速達(dá)到負(fù)向最大，說明車輛正處于大彎道區(qū)域.此時(shí)，預(yù)測(cè)時(shí)域和控制時(shí)域都需要相應(yīng)減小，使可在線優(yōu)化MPC控制器能夠在短時(shí)間內(nèi)求解出最穩(wěn)定的前輪轉(zhuǎn)角，確保車輛在穩(wěn)定的前提下能夠以較高的精度通過第三彎道.顯然，基于模糊規(guī)則的預(yù)測(cè)時(shí)域和控制時(shí)域控制方法能較好地適應(yīng)縱向、橫向車速的變化.

圖8 追蹤時(shí)的狀態(tài)矩陣優(yōu)化節(jié)點(diǎn)Fig.8 State matrix optimization node during tracking

圖9 追蹤時(shí)的橫向車速Fig.9 Lateral speed during tracking

圖10 軌跡追蹤時(shí)的預(yù)測(cè)時(shí)域和控制時(shí)域Fig.10 Prediction time domain and control time domain during trajectory tracking

5 結(jié)語

本研究基于線性時(shí)變MPC算法設(shè)計(jì)可在線優(yōu)化MPC軌跡追蹤控制器，所得結(jié)論如下：

1) 基于線性時(shí)變MPC控制器，采用余弦相似度和基于模糊控制的時(shí)域系數(shù)對(duì)控制器的預(yù)測(cè)模型和控制參數(shù)進(jìn)行在線優(yōu)化，使得預(yù)測(cè)模型、預(yù)測(cè)時(shí)域和控制時(shí)域能夠主動(dòng)適應(yīng)車速和路況的變化，提高控制器對(duì)不同車速的自適應(yīng)能力；

2) 通過對(duì)狀態(tài)矩陣余弦相似度的優(yōu)化閾值和預(yù)測(cè)時(shí)域、控制時(shí)域的權(quán)重系數(shù)進(jìn)行分析和合理取值，控制器的軌跡追蹤精度和穩(wěn)定性有所提高；

3) 計(jì)算機(jī)仿真結(jié)果表明，與線性時(shí)變MPC控制器相比，可在線優(yōu)化MPC控制器在不同車速下的追蹤精度和穩(wěn)定性均得到明顯提升，預(yù)測(cè)時(shí)域和控制時(shí)域也能較好地響應(yīng)車速和道路的變化，滿足高速工況下的追蹤精度和穩(wěn)定性需求.