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

鐘聰，彭育輝，黃煒

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

0 引言

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

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

1 控制策略的總體思路

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

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

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

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

(1)

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

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

(2)

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

(3)

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

(4)

系統(tǒng)預(yù)測時域為Np，控制時域為Nc，系統(tǒng)預(yù)測時域內(nèi)的輸出序列可表示為：

(5)

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

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

(6)

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

(7)

考慮車輛實際特性，還需對系統(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.

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

(8)

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

(9)

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

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

(10)

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

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

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

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

(11)

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

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

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

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

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

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

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

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

(12)

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

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

(13)

4 仿真驗證

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

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

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

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

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

為合理確定權(quán)重系數(shù)，需先研究預(yù)測時域和控制時域的權(quán)重系數(shù)對軌跡追蹤控制器的精度和穩(wěn)定性的影響.對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ù)下追蹤時的橫向速度變化，仿真工況選取雙移線工況.圖6為不同權(quán)重系數(shù)下追蹤時的橫向位置和橫擺角變化.

圖4 縱向車速Fig.4 Longitudinal velocity

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

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

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

4.3 控制器整體的有效性驗證

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

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

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

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

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

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

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

5 結(jié)語

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

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

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

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