適應道路曲率多變的前饋-預測LQR橫向控制

孫福昌，邵金菊，單少飛，謝生龍

（山東理工大學交通與車輛工程學院，山東 淄博 255000）

0 引言

路徑跟蹤控制是自動駕駛關鍵核心技術之一，其控制效果是保證車輛安全穩定行駛的關鍵，是近年來自動駕駛研究的熱點之一［1－2］。目前常用的路徑跟蹤控制方法主要有比例－積分－微分（proportional-integral-derivative，PID）控制［3］、線性二次型最優控制（linear quadratic regulator，LQR）［4－5］、純跟蹤控制［6］、模糊控制（fuzzy control，FC）［7］、滑?？刂疲╯liding mode control，SMC）［8］、前饋-反饋控制［9］、模型預測控制（model predictive control，MPC）［10］等。其中LQR控制對于高中低速工況下均有優異的表現，尤其是對城市路況有著很強的適應性，且對硬件的要求并不高，應用廣泛，但在某些極端的路況和大幅度偏離跟蹤路徑的情況下仍存在響應遲滯等問題［4］。早在2009年，美國斯坦福大學的Snider等［11］考慮道路曲率將LQR應用于路徑跟蹤控制中，但由于缺少前饋控制的修正，系統仍存在穩態誤差。Kapania等［12］提出反饋＋前饋的方法，增強了LQR控制在固定道路上的跟蹤穩定性，消除了穩態誤差，但當道路變化明顯增大時，由于Q、R矩陣無法調節，跟蹤效果并不理想。馬思群等［13－14］通過模糊控制的方法實現了對Q、R矩陣的實時更新，有效地提高了控制器面對多變道路時的性能表現，但由于計算較為繁瑣，更新速度存在一定的滯后性。當道路連續變化時，很難及時更新出適合當前位置時刻的Q、R矩陣。為此，在考慮道路曲率變化的前提下，本文中提出了一種動態預測模塊，以解決Q、R矩陣更新不及時的問題。對于預測點和預測時間的確定，大多數都是采用單點預測和固定預測時間，故在面對不同曲率的道路時，控制效果適應性較低［15］。本文中采用兩點加權的方式，根據道路曲率，實時調節兩點的權重，計算出適合當前道路的預測點以及預測時間。

綜上，考慮道路曲率變化，采用誤差動力學模型取代傳統動力學模型，提出一種前饋-預測LQR反饋控制，實現了曲率多變道路的跟蹤，解決了傳統LQR方法Q、R矩陣更新不及時的問題。在給定規劃路徑的基礎上進行了硬件在環實驗，測試了單、雙移線多車速工況下傳統LQR和本文中設計的前饋-預測LQR的路徑跟蹤效果。

1 數學建模

1.1 汽車動力學模型

考慮側向平移和橫擺2個自由度，根據牛頓力學定律建立車輛動力學微分方程如式（1），車輛的受力分析如圖1所示。

圖1 汽車2自由度模型受力分析示意圖

式中：m為汽車質量；ay、δ分別為汽車橫向加速度和前輪轉角；y為汽車坐標系下的橫向位移；x為縱向位移；Fyf、Fyr分別為前、后車輪所受的側向力；Iz為汽車繞垂直于地面的z軸的轉動慣量；為橫擺角加速度；a、b分別為汽車前后軸到質心的距離。

假設δ較小，cosδ≈1，式（1）則可以化簡為：

式中：Cαf、Cαr分別為前、后車輪的側偏剛度；αf、αr為前、后輪的側偏角，由于側偏角的方向與側向力的方向相反，因此定義為負數。

為了進一步簡化式（2），根據圖2剛體運動的合成與分解，對后輪運動進行分析，可得到ay與y的關系，以及αf、αr具體表達式。

圖2 后輪運動分解示意圖

圖2中v為質心處的速度，β為質心側偏角。vx和vy分別為車輛坐標系下的縱向速度和橫向速度。前輪與后輪原理相同，可得到式（3）。

求得αf，αr為：

1.2 誤差模型

相較于經典動力學模型，基于誤差模型的跟蹤控制精度更高、控制目標更易實現。其基本控制原理是通過調節合適的控制量，使輸出的實際值與參考規劃值的偏差盡可能地小，從而達到精準控制的效果。

本文中假定參考規劃值（xryrvrθr）已知，其分別為路徑的縱向位移xr、橫向位移yr、速度vr、航向角θr。實際位置與參考軌跡的位置關系如圖3所示。

圖3 實際位置與規劃軌跡位置關系示意圖

圖3中藍色點劃線為參考軌跡，車輛實際速度為v，在參考軌跡上的投影點的切線和法線方向分別為τr和nr，投影點的速度為˙s，車輛與投影點的距離為d，d即為橫向誤差，表示為式（6）。

式中，k為曲率，由于本文中主要關注橫向控制，因此縱向速度vx在計算過程中視為常數。令ed＝d，eφ＝φ－θr，可將2.1中建立的汽車動力學模型（5）轉化為誤差模型式（9），令u＝δ，為前輪轉角。

其中：

2 橫向LQR控制器設計

在誤差模型的基礎上，設計LQR控制器，通過不斷控制前輪轉角來減小實際位置與規劃位置的橫向誤差、航向角誤差，最終實現對規劃路徑的跟蹤。

2.1 反饋控制

LQR求解原理來源于拉格朗日乘數法，將上文求得的誤差線性微分方程式（9）作為約束條件，在此約束條件下求得代價函數，將代價函數進行離散化后求解其極小值，求解出極小值處的解即為此約束下的最佳的前輪轉角控制量。

此處忽略誤差模型式（9）中的，建立的代價函數如式（10）：

式中：Q和R為對角陣，分別代表誤差狀態E和控制量u的懲罰力度，Q或R的值越大，代表對該變量的懲罰力度越大、重視程度越高。

由于式（10）中＝AE＋Bu是連續的微分方程，而給定的規劃路徑點為離散點，因此需要對其進行離散化，利用積分中值定理化簡得：

采用中點歐拉法對E（ε）進行化簡，同時采用向前歐拉法對u（ε）進行化簡，并忽略較小值1／（I－Adt／2），進而式（11）簡化為式（12）：

式中：I為單位矩陣；E（t）為當前時刻的誤差；E（t＋dt）為下一時刻的誤差；u（t）為當前時刻的控制量即前輪轉角。

依據離散后的約束方程式（13），并考慮約束范圍：E0～En，u0～un－1，可得到目標函數如下：

根據拉格朗日乘數法，由式（14）可構造代價函數如式（15）：

引入哈密頓方程對式（15）進行處理：

對代價函數式（16）求偏導：

根據向量導數求解法，對式（17）進行求解可得：

由式（18）可知，最終求解的控制量為：

式中只有λk＋1為未知量，R－和B－皆為已知量。聯立式（18）方程可推導出λk的表達通式為λk＝2PkEk。其中Pk符合Riccati方程：

Pn代表末尾時刻的Pk，且Pn＝Q。因此可由已知的Pn逆求此時刻的Pk，又因為該迭代在經過幾十次迭代后就會迅速收斂，即Pk－1＝Pk，Riccati方程（20）可轉化為方程（21）：

在此基礎上即可求解出最終所需要的LQR控制的反饋控制量uk：

2.2 前饋控制

根據誤差模型式（9），將式（22）求得的控制量uk代入的誤差模型為：

分析式（23）可以發現無論l取何值，和E無法同時為零，即誤差和誤差變化率始終不能同時為零，勢必存在較大的穩態誤差。因此在此處引入前饋控制量δf以抵消穩態誤差，式（23）可表示為式（24）：

通過使E＝0且＝0求解出的前饋控制量δf為：

式中，l3為矩陣l的第三列。δf的值與反饋控制中的l有關，因此在求解控制量時需先求解反饋控制再求解前饋控制。由此可得LQR控制器的最終控制量u為：

2.3 Q、R矩陣更新

LQR算法通過優化狀態量和控制量的加權之和來實現控制目的。但在道路曲率多變時，若不能根據道路的特征對Q、R矩陣進行實時調整，系統容易失穩。因此本文中根據路徑跟蹤的橫向位置誤差ed和航向誤差eφ建立模糊規則，實時調整Q、R矩陣，使算法更適應當前道路的變化，如圖4所示。其中，權重因子η為模糊規則輸出，R矩陣為1，Q矩陣為10η的對角矩陣。

圖4 模糊控制原理圖

建立的模糊規則如表1所示。當航向誤差和橫向位置誤差都比較大時，無論是正大還是負大都是遠離參考路徑，因此需要較大的指數因子η，以增加對誤差的重視程度，放寬對控制量的限制。當橫向位置誤差較大，而航向誤差較小時，此時的橫向位置誤差可通過較小的航向誤差自主修正。同樣，當航向誤差較大，橫向位置誤差較小時也可以實現自主修正，不需要過度重視誤差，此時只需要較小的指數因子η。

表1 模糊規則表

隸屬度函數如圖5所示，橫向位置誤差和航向誤差與η的關系如圖6所示。

圖5 變量隸屬度函數

圖6 橫向位置誤差以及航向誤差與η的關系

模糊控制器依據實時的橫向位置誤差、航向誤差和制定的模糊規則計算得出Q矩陣的指數因子η，通過不同的指數因子對Q、R矩陣比例關系進行調節，進而實現對Q、R矩陣的實時更新。

調節指數因子η對跟蹤效果的影響如圖7所示。當η較大時，系統更加注重跟蹤誤差，因此會以更大的控制量來保證跟蹤效果。當η較小時，對誤差的關注度減小，對控制量大小的重視程度增大，因此跟蹤效果會有所下降。

圖7 η變化對跟蹤效果的影響

2.4 動態預測模塊

傳統的LQR以當前時刻的位置和狀態作為輸入量進行求解，缺少預測性，在連續變化的道路條件下會存在一定的控制滯后性。因此本文中增加一個預測模塊如圖8所示，通過預測點與軌跡點之間的誤差進行預測控制，并根據道路曲率的變化對預測點以及預測時間進行實時調整，解決傳統LQR響應遲滯問題。

圖8 預測模塊

圖8中，最佳的預測位置是通過N和F兩個固定預測點加權之后得到，其中N表示近預測點，F表示遠預測點，最佳預測點可表示為：

μ∈［0，1］為決定最佳預測點位置的權重參數，與道路的曲率k有關，μ＝μ（k）。μ是關于k的正相關函數，當道路曲率越大時，為提高算法對規劃路徑的跟蹤性能，較大的μ會增大近預測點的權重，使最佳預測點較近，減小預測時間，提高預測頻率。相反，當在曲率較小乃至直線道路下，較遠的預測點和較長的預測時間可保證行駛過程更加平穩。權重μ的具體表達式為：

式中，tp（k）為駕駛員預瞄時間估計模型。選取5名不同年齡段的駕駛員操作模擬駕駛器，以20 km／h的車速進行實驗，記錄預瞄時間與道路曲率，最終將記錄數據進行擬合，擬合曲線如圖9所示，得到的駕駛員預瞄時間估計模型如式（29）所示。

圖9 駕駛員預瞄時間擬合曲線

預測時間t可通過最佳預測點計算得出：

根據計算得到的預測時間t，預測模塊可根據當前時刻狀態（x，y，φ，vx，vy，˙φ）計算得到t秒后預測狀態（xp，yp，φp，vxp，vyp，˙φp），其中（x，y，φ，vx，vy，˙φ）分別表示縱向位置、橫向位置、橫擺角、縱向速度、橫向速度和橫擺角速度，下標p表示預測點。

3 硬件在環實驗

3.1 硬件在環實驗驗證

本文中通過硬件在環實驗來驗證所設計的前饋-預測LQR算法的有效性。根據圖10所示的算法流程搭建控制仿真模型，基于圖11所示的硬件在環實驗平臺實現Carsim和Simulink的聯合仿真。將Carsim測得的航向角及前輪轉角等數據傳輸到Simulink，經本文中設計的前饋-預測LQR算法計算得到所需的前輪轉角，并將其發送給臺架進而實現方向盤的控制，然后通過NI-XNET讀取臺架傳感器信息獲得實際方向盤轉角、轉矩等信息，實現完整的硬件在環實驗，實驗原理如圖12所示。

圖10 算法流程

圖11 硬件在環實驗臺架

圖12 硬件在環實驗原理

依據40、60、80 km／h三種車速下單、雙移線工況的跟蹤效果來進行評價分析。本實驗采用的整車參數如表2所示，路面附著系數設為0.8。

表2 整車參數

3.2 多工況軌跡跟蹤仿真分析

分別用傳統LQR控制和本文中設計的前饋-預測LQR控制在40、60、80 km／h，單移線工況下進行軌跡跟蹤測試，測試效果如圖13—圖15所示。

圖13 40 km／h跟蹤效果

圖14 60 km／h工況跟蹤效果

圖15 80 km／h工況跟蹤效果

為驗證本文中所設計的前饋-預測LQR控制面對復雜道路時的跟蹤效果，采用雙移線工況進行測試，測試效果如圖16—圖24所示。

圖16 40 km／h工況橫擺角曲線

圖17 40 km／h工況前輪轉角曲線

圖18 40 km／h工況跟蹤效果

圖19 60 km／h工況橫擺角曲線

圖20 60 km／h工況前輪轉角曲線

圖21 60 km／h工況跟蹤效果

圖23 80 km／h工況前輪轉角曲線

圖24 80 km／h工況跟蹤效果

3.3 仿真結果總結和分析

如表3所示在單移線多車速工況下，本文中設計的前饋-預測LQR控制在跟蹤效果方面優于傳統LQR，能夠更好地跟蹤目標軌跡。在40 km／h的工況下，本文中設計的預測LQR控制器跟蹤效果高達99.2%，傳統LQR為96.7%，誤差減小2.5%。在60 km／h的工況下，本文中設計的預測LQR控制器跟蹤效果高達98.9%，傳統LQR為95.5%，誤差減小3.4%。在80 km／h的工況下，本文中設計的預測LQR控制器跟蹤效果高達98.4%，傳統LQR為93.9%，誤差減小4.5%。

表3 單移線多車速下傳統LQR與預測LQR %

在更復雜的雙移線多車速工況下的跟蹤測試效果如表4所示。在40 km／h的工況下，本文中設計的預測LQR控制器跟蹤效果高達96.7%，傳統LQR為91.3%，誤差減小5.4%。在60 km／h的工況下，本文中設計的預測LQR控制器跟蹤效果高達92.9%，傳統LQR為83.4%，誤差減小9.5%。在80 km／h的工況下，本文中設計的預測LQR控制器跟蹤效果高達84.5%，傳統LQR為75.6%，誤差減小8.9%。

表4 雙移線多車速下傳統LQR與預測LQR %

4 結論

針對傳統LQR橫向控制對高速工況、曲率較大道路適用性較差的問題，本文中提出了一種前饋-預測LQR反饋橫向控制方法。

1）基于誤差模型，根據航向和橫向位置誤差，通過模糊規則實時調節Q、R矩陣，解決了傳統LQR在連續變化道路上穩定性差的問題，擴大了應用范圍。

2）設計了基于2點預瞄的預測模塊，實時更新預測點及預測時間，解決了傳統LQR響應遲滯問題。

3）通過Simulink-CarSim的聯合仿真和硬件在環實驗對本文中所提算法進行了驗證。結果表明，在單移線多車速工況下，本文中設計的預測LQR相比于傳統LQR軌跡跟蹤誤差分別減少2.5%、3.4%、4.5%；在雙移線多車速工況下，本文中設計的預測LQR相比于傳統LQR軌跡跟蹤誤差分別減少5.4%、9.5%、8.9%，較好地實現軌跡跟蹤。