適用于路徑跟蹤控制的自適應MPC算法研究

石貞洪，江 洪，于文浩，柳亞子，蔣瀟杰，姜 民

1.江蘇大學 汽車與交通工程學院，江蘇 鎮(zhèn)江 212013

2.江蘇大學 機械工程學院，江蘇 鎮(zhèn)江 212013

1 引言

近年來，隨著計算機、傳感器和通信技術的發(fā)展，自動駕駛車輛已成為車輛工程領域的研究前沿和工程應用領域的落地方向[1-2]，自動駕駛技術的發(fā)展能夠有效地減少交通堵塞和擁擠，提高道路通行能力[3]。路徑跟蹤控制是實現(xiàn)自動駕駛的關鍵環(huán)節(jié)，路徑跟蹤控制是指在車輛的運動學/動力學微分方程的基礎上，通過對車輛前輪轉(zhuǎn)角、油門踏板和制動踏板的控制實現(xiàn)對期望路徑的跟蹤[4]。其中，路徑跟蹤精度和車輛行駛穩(wěn)定性是評價路徑跟蹤控制的兩個重要指標，但是兩者具有相對意義[5]，因此，在不同的工況下，合理地選擇對兩者的側(cè)重程度，能夠有效地提高自動駕駛車輛的性能。

目前，自動駕駛車輛路徑跟蹤領域的控制理論主要有 PID 控制[6]、LQR 控制[7]、預瞄控制[8]和模型預測控制等。PID控制在應用于自動駕駛路徑跟蹤領域時，其控制參數(shù)需要不斷地試湊，比較耗時[9]，且對于復雜工況的適應性比較差；LQR沒有考慮車輛運動學/動力學約束，在極端工況下，容易使得車輛出現(xiàn)側(cè)偏失穩(wěn)現(xiàn)象[10]。模型預測控制算法能夠有效地解決帶約束的最優(yōu)化問題[11]，在自動駕駛領域應用越來越多。但傳統(tǒng)的MPC控制策略中參數(shù)均為常量，在跟蹤路徑較為復雜時，路徑跟蹤精度或車輛行駛穩(wěn)定性變差。文獻[12]提出一種基于道路曲率動態(tài)調(diào)整MPC 采樣時間的方法，該方法在道路曲率比較大的地方，減小采樣時間，以提高跟蹤精度，但會導致控制量光滑度降低，車輛穩(wěn)定性變差，控制器的實時性變差，且該方法并沒有考慮在曲率比較小時，初始橫向偏差的影響，可能會導致偏差進一步變大。文獻[13]中采用基于車速和初始偏差調(diào)整MPC 中預測時域方法，但該方法應用在曲率比較大的道路環(huán)境中，車輛容易出現(xiàn)早轉(zhuǎn)現(xiàn)象，進而導致車輛的跟蹤性能變差，且預測時域的增加同樣會導致控制器實時性變差。

針對現(xiàn)有基于MPC 路徑跟蹤控制策略的不足，且考慮到MPC 控制策略中權重系數(shù)大小，同樣會影響路徑跟蹤精度和車輛行駛的穩(wěn)定性。在權重系數(shù)為常量的MPC 路徑跟蹤控制系統(tǒng)中，當車輛與期望路徑有較大初始跟蹤偏差時，由于系統(tǒng)中反映跟隨性側(cè)重程度的權重為定值，不能提高，導致跟蹤性能下降；而當車輛與期望路徑有較小的偏差或參考路徑曲率比較小時，權重系數(shù)為常量的MPC 系統(tǒng)中由于反映穩(wěn)定性側(cè)重程度的權重為定值，不能提高，導致車輛乘坐舒適性變差。

基于此，提出了一種MPC 權重系數(shù)自適應的自動駕駛車輛路徑跟蹤控制算法。該算法首先基于車輛的單軌模型與模型預測控制理論構建成本函數(shù)權重系數(shù)為固定值得路徑跟蹤控制器；然后綜合分析跟蹤偏差與道路曲率對車輛跟隨性和舒適性的影響，依據(jù)人們的駕駛經(jīng)驗，構建模糊控制器，以跟蹤偏差和道路曲率作為模糊控制器的輸入，以權重系數(shù)的調(diào)節(jié)因子作為模糊控制器的輸出，構建權重系數(shù)自適應的MPC控制器；最后通過CarSim/Simulink 聯(lián)合仿真驗證了所述權重系數(shù)自適應的路徑跟蹤控制系統(tǒng)的有效性。

2 自動駕駛車輛路徑跟蹤控制器

2.1 車輛動力學建模

對車輛進行動力學建模是進行自動駕駛車輛路徑跟蹤控制的基礎。一是可以在車輛動力學微分方程基礎上精確地推導控制器的控制律；二是基于車輛動力學模型，可以對車輛的行為進行仿真，以驗證設計的控制器性能[14]。由于車輛本身是一個復雜的非線性系統(tǒng)，為提高控制器的實時性，需要將車輛模型在能夠較為準確描述車輛動力學特性的基礎上進行理想化假設[15]：（1）假設車輛無垂向運動；（2）忽略懸架的作用；（3）忽略車輛前后軸載荷的轉(zhuǎn)移；（4）假設輪胎的側(cè)偏特性處于線性范圍；（5）假設車輛僅為前輪轉(zhuǎn)向且縱向速度保持不變。

基于上述假設，建立車輛的單軌模型，如圖1所示，其中oxyz為車輛坐標系，OXY為地面慣性坐標系。

圖1 車輛單軌模型

由上述車輛模型，分別得到沿x軸、y軸和繞z軸的車輛動力學方程：

式中，lf、lr分別為車輛質(zhì)心到前、后軸的距離；m為車輛整備質(zhì)量；φ為車輛橫擺角；為車輛橫擺角速度；δf為前輪偏角；Iz為車輛繞z軸的轉(zhuǎn)動慣量；Flf、Flr分別為前、后輪胎的縱向力；Fcf、Fcr分別為前、后輪胎的側(cè)向力。

當側(cè)偏角及縱向滑移率的數(shù)值不大時，輪胎力可線性表示為：

式中，Ccf、Ccr分別為前、后輪輪胎側(cè)偏剛度；αf、αr分別為前、后輪輪胎側(cè)偏角；Clf、Clr分別為前、后輪輪胎縱向剛度；sf、sr分別為前、后輪輪胎滑移率。

將前輪偏角作小角度假設，并將式（2）代入式（1），得到簡化后的車輛動力學模型如式（3）所示：

該控制系統(tǒng)中，選取狀態(tài)量選取為：

控制量選取為u=δ。

2.2 基于模型預測控制理論的路徑跟蹤

基于式（3）所建立的車輛動力學模型是非線性，利用非線性模型預測車輛未來狀態(tài)計算量大，會導致所設計的控制器實時性變差，故考慮對模型進行線性化處理，得到線性時變方程為：

對式（4）中方程進行離散化處理，可得：

其中，Ak=I+AT；Bk=BT，T為采樣時間。

為對控制增量進行精確約束，將離散的狀態(tài)量χ(k)與控制量u(k-1)組合為新的狀態(tài)量，即作變換ξ(k)=得到新的車輛動力學狀態(tài)空間方程為：

此時，Np時域內(nèi)系統(tǒng)的輸出量可由下式表示：

式中：

Np為預測時域；Nc為控制時域。

為了獲得最佳控制量序列，設計成本函數(shù)如下：

式中，λy、λφ、λu、ρ為權重系數(shù)（反映了各優(yōu)化目標的優(yōu)先級），ε為松弛因子。ε的作用是為了保證在每一時刻該成本函數(shù)都能得到可行解。

實際控制過程中，需要滿足的約束條件為：

式中，ΔUmin、ΔUmax分別為控制增量的最小值與最大值；Umin、Umax分別為控制量的最小值與最大值；yh,min、yh,max分別為輸出量的上下邊界；ys,min、ys,max分別為包含松弛因子的輸出量的上下邊界。

在每個控制周期完成上述求解后，將控制序列中的第一個元素作用于控制系統(tǒng)，在下一時刻，重復上述操作，以此實現(xiàn)對系統(tǒng)的持續(xù)控制。

2.3 權重系數(shù)對路徑跟蹤控制的影響

基于上述分析，搭建CarSim/Simulink 聯(lián)合仿真模型。由于成本函數(shù)中的權重系數(shù)λy、λφ和λu具有相對意義，為便于研究，設置λy為常量，將λφ和λu作為變量，分別分析其大小對路徑跟蹤的影響，圖2和圖3為對應的仿真路徑。

由圖2 中的仿真結(jié)果發(fā)現(xiàn)，當λu變小時，橫向偏差（實際值Y減去參考值Yref）相應減小，這是由于λu小時，系統(tǒng)對誤差變化更加敏感。λu變大時，橫擺角速度會變小，行駛穩(wěn)定性變好，當λu值超過一定的范圍時，系統(tǒng)會由于跟蹤誤差過大，導致不穩(wěn)定。由圖3中的仿真結(jié)果可得出，當λφ較小時，系統(tǒng)對誤差比較敏感，會迅速減小誤差，但同時會導致橫擺角速度變大，行駛穩(wěn)定性變差；當λφ變大時，系統(tǒng)的橫擺角速度會變小，但跟蹤誤差減小的比較遲緩，安全性變差。為此可以考慮在橫向偏差較大時，減小λu和λφ值，更加側(cè)重車輛的跟隨性，在橫向偏差和參考路徑曲率均較小時，應該增大λu和λφ值，以提高車輛行駛的穩(wěn)定性。

圖2 不同λu 時的路徑跟蹤對比

圖3 不同λφ 時的路徑跟蹤對比

3 權重系數(shù)自適應模糊控制

模糊控制算法能夠?qū)碗s的系統(tǒng)進行控制，且對模型的準確性要求不高，只需要根據(jù)專家知識和規(guī)則便可以進行優(yōu)化求解[16]。故采用Matlab 中的模糊邏輯工具箱（Fuzzy Logic Toolbox）對成本函數(shù)中的權重系數(shù)進行在線優(yōu)化。在具體優(yōu)化過程中，設置λy為固定值，通過調(diào)節(jié)λφ和λu的方法獲得最佳權重系數(shù)組合。又由于車輛與參考路徑橫向偏差和參考路徑曲率大小是影響系統(tǒng)跟蹤精度和穩(wěn)定性的重要影響參數(shù)，故可以將橫向偏差和參考路徑曲率作為模糊控制器的輸入，λφ的調(diào)節(jié)因子 Δλφ和λu的調(diào)節(jié)因子 Δλu作為模糊控制器的輸出，設計權重系數(shù)自適應模糊控制器。

首先將橫向偏差和參考路徑曲率進行歸一化處理，如下式：

式中，ey為橫向偏差；ρ為參考路徑曲率。

為適應復雜工況的需要，模糊控制器的輸出參數(shù)不直接作為MPC控制器權重系數(shù)，而是作為MPC控制器權重系數(shù)的修正量在線進行調(diào)節(jié)，其中λφ、λu的調(diào)整算式為：

式中，λφ0、λu0分別為原MPC 控制器的權重系數(shù)；λφ、λu分別為變權重系數(shù)MPC控制器的權重系數(shù)。

在進行權重系數(shù)具體調(diào)節(jié)時，首先，確定輸入和輸出變量的取值范圍，并將其模糊化。對于模糊控制器的兩個輸入變量，ey和ρˉ的論域均設置為[0，1]，模糊子集選擇為5 個，則模糊控制子集可表示為{VL(極 小),L( 小),M(中 ),H( 大),VH(極 大) }，并選擇三角形隸屬度函數(shù)；對于輸出變量 Δλφ和 Δλφ設定論域均為[-2，2]，模糊子集同樣選擇為5 個，模糊子集表示為{VL(極 小),L( 小),M(中 ),H( 大),VH(極 大) }，并選擇高斯型隸屬度函數(shù)。

模糊控制器規(guī)則制定的原則是控制自動駕駛車輛的路徑跟蹤偏差限定在一定的范圍內(nèi)，同時使得車輛的舒適性盡量好。即當橫向偏差較大時，為了使系統(tǒng)能夠快速響應，減小偏差，應更加側(cè)重于車輛跟蹤性的目標，適當減小λφ和λu的值；而當跟蹤的參考路徑由曲線變?yōu)橹本€時，曲率減小，為了使車輛保持較好的穩(wěn)定性，應適當增加λφ和λu的值，提高車輛的乘坐舒適性?；谝陨戏治?，設計權重調(diào)節(jié)因子 Δλφ和 Δλu隨ey和ρˉ變化的控制規(guī)則分別如表1和表2所示。

表1 Δλφ 模糊控制規(guī)則表

表2 Δλu 模糊控制規(guī)則表

4 仿真實驗

為了驗證改進后MPC控制器的性能，建立了如圖4所示的仿真系統(tǒng)，在相同的仿真條件下對兩種控制器進行仿真比較。并且考慮到車輛在實際行駛過程中，對直線路徑的跟蹤比較多，偶爾會有換道超車的情況，即對曲線路徑的跟蹤，在雙移線路徑和直線路徑兩種工況下進行跟蹤仿真實驗。

圖4 基于模糊MPC的路徑跟蹤控制系統(tǒng)

4.1 雙移線路徑跟蹤仿真

在雙移線工況下進行仿真時，車速設置為54 km/h，實驗結(jié)果如圖5 所示。圖5（a）為自動駕駛車輛在兩種控制器的作用下跟蹤路徑對比圖，由該圖可以看出兩種控制器均可以控制車輛較精確的跟蹤參考路徑。圖5（b）為跟蹤橫向偏差對比圖，由該圖可以發(fā)現(xiàn)改進后的控制器較原MPC 控制器有更小的橫向偏差，這是由于當車輛與參考路徑橫向有較大偏差時，對安全性的要求更高，改進后的控制器λφ和λu均減小，從而使得系統(tǒng)更加側(cè)重跟隨性的目標。圖5（c）橫擺角速度對比圖，可以看出改進后的控制器最大橫擺角速度相比于原控制器有所減小，從而使得車輛具有更好的舒適性和穩(wěn)定性，并且參考路徑由曲線變?yōu)橹本€時，改進后的控制器更加平穩(wěn)，這主要是因為當車輛跟蹤的路徑由曲線段變?yōu)橹本€段時，曲率減小，此時對跟蹤平穩(wěn)性的要求更高，改進后控制器λφ和λu均增大，使得系統(tǒng)對于車輛行駛穩(wěn)定性的目標有更大的側(cè)重。

由上述各組實驗對比結(jié)果可知，雖然兩個控制器均能實現(xiàn)對參考路徑的跟蹤，但改進后MPC 控制器相比于原控制器，在對曲線跟蹤時，具有更好的跟蹤精度，當車輛由曲線跟蹤變?yōu)橹本€跟蹤時，跟蹤的平穩(wěn)性更好。

4.2 直線路徑跟蹤仿真

在直線工況下進行跟蹤仿真時，車速同樣設置為54 km/h，車輛的起始位置為（0，0），初始點處車輛與參考路徑的橫向偏差為1 m，航向角偏差為0，對應的實驗結(jié)果如圖6 所示。圖6（a）自動駕駛車輛在兩種控制器的作用下跟蹤路徑對比圖，由該圖可以看出兩種控制器均在一定的時間內(nèi)控制車輛跟蹤上參考路徑。圖6（b）為跟蹤橫向偏差對比圖，由該圖可以發(fā)現(xiàn)當偏差比較大時，對行車安全性的要求更高，兩個控制器均控制車輛快速減小偏差，當偏差較小時，此時跟蹤的平穩(wěn)性要求較更高，改進后的控制器偏差減小的速度變緩，主要由于改進后的控制器λφ和λu均隨偏差變小而增大，從而使得車輛在偏差較小時，有更好穩(wěn)定性。圖6（c）是橫擺角速度對比圖，可以看出改進后的控制器最大橫擺角速度相比于原控制器有所減小，尤其是在車輛逐漸接近參考路徑，此時更加注重行駛的穩(wěn)定性，改進后的控制器此時有更大的λφ和λu，從而保證改進后的控制器有更好的行駛穩(wěn)定性。

圖5 雙移線工況下的仿真結(jié)果對比圖

圖6 直線工況下的仿真結(jié)果對比圖

由上述各組實驗對比結(jié)果可知，兩個控制器均能精確地跟蹤參考路徑，但改進后MPC 控制器相比于原控制器，橫向偏差更小，且具有更好的跟蹤平穩(wěn)性。

5 結(jié)束語

針對自動駕駛車輛路徑跟蹤精度與行駛穩(wěn)定性之間互相矛盾的問題，提出了一種權重系數(shù)自適應的自動駕駛車輛路徑跟蹤控制算法，該算法是在MPC 路徑跟蹤控制理論的基礎上進行的優(yōu)化，主要是基于實際駕駛員的操作經(jīng)驗設計模糊控制器，使得MPC 控制器中成本函數(shù)的權重系數(shù)能夠根據(jù)跟蹤偏差和道路曲率的變化自動調(diào)整，從而使得車輛具有較好跟隨性，同時能夠保證車輛的行駛穩(wěn)定性更高，更加符合人類的駕駛習慣。搭建了CarSim/Simulink 聯(lián)合仿真模型，將權重系數(shù)為常量的MPC路徑跟蹤控制器與權重系數(shù)自適應的MPC 路徑跟蹤控制器進行仿真對比，驗證了權重系數(shù)可變的MPC控制器的性能。權重系數(shù)自適應的路徑跟蹤控制器相比于傳統(tǒng)的MPC控制器具有以下特點：

（1）當車輛與參考路徑偏差比較大時，考慮到安全性的需要，系統(tǒng)反映跟隨性的權重系數(shù)會相對變大，從而使得車輛可以快速減小偏差。

（2）當參考路徑曲率比較小時，此時，為使得車輛具有更高行駛穩(wěn)定性，λφ和λu會相對變大，從而使得車輛有更好的乘坐舒適性和穩(wěn)定性。

目前的研究成果對后續(xù)的實車試驗具有一定的指導作用，下一步將通過實車試驗對所提出的控制方案進行驗證。