基于動力學模型預測控制的鉸接車輛多點預瞄路徑跟蹤方法*

李斯旭，徐 彪，胡滿江，邊有鋼，陳曉龍，孫 寧

（湖南大學機械與運載工程學院，汽車車身先進設計制造國家重點實驗室，長沙410082）

前言

鉸接車輛指前后車體間存在擺動環和鉸接點的汽車［1］，其前后車體間夾角可達45°，機動能力好、越野能力強，加之其油耗小、成本低，在實際中被廣泛應用。然而，鉸接車輛操作困難，多用于礦山、山地等惡劣地形環境中，易引發事故，且這些作業環境往往伴隨粉塵和噪聲，也易對駕駛員的安全健康造成威脅。近年來，車輛自動駕駛技術快速發展，將其應用于鉸接車輛，不但可替代駕駛員、避免人員損傷，還可有效提升工作效率和燃油經濟性。

在車輛自動駕駛中，路徑跟蹤控制是核心技術之一［2］，而鉸接車輛模型復雜、非線性強，其路徑跟蹤控制相比一般乘用車更加困難，也更具挑戰性。目前，車輛路徑跟蹤控制技術已得到廣泛研究，典型控制方法如模糊比例-積分-微分（PID）控制、線性二次調節控制、前饋-反饋控制、模型預測控制（model predictive control，MPC）等。Hayakawa等設計了PID路徑跟蹤控制器［3］，該方法結構簡單，不依賴于精確的車輛模型，但無法對車輛未來狀態進行預測，且對于非線性動力學的控制效果欠佳。Levinson等將線性二次調節器應用于路徑跟蹤控制［4］，提高了路徑跟蹤精度，但該方法難以顯式滿足車輛系統約束，在較復雜環境中難以達到理想的控制效果。Kritayakirana等針對賽車的路徑跟蹤問題設計了前饋-反饋控制器［5］，同樣也有較高的跟蹤精度，但其依賴于系統可逆性，應用范圍有限。

近年來，MPC技術在車輛路徑跟蹤控制中得到了廣泛應用。該技術能夠將未來路徑信息融入控制設計中，對速度和路徑曲率的變化有良好的魯棒性，且可實現多目標優化、顯式處理狀態變量約束及執行器飽和約束［6］，在車輛路徑跟蹤方面具有很大優勢。基于MPC進行鉸接車輛路徑跟蹤控制，可充分考慮其實際物理特性，顯式處理前后車體間最大夾角、最大轉向力矩等約束，提升控制方法實用性。

基于MPC的鉸接車輛路徑跟蹤控制研究尚不多見。Nayl等首先將基于運動學的MPC應用于鉸接車輛的路徑跟蹤問題［7］，但這種基于運動學模型的方法難以在路徑曲率連續變化、低附著路面、高速行駛等復雜條件下保障良好的控制性能。竇鳳謙將基于動力學的MPC應用于鉸接車輛的路徑跟蹤控制［8］，取得了良好的控制效果，但該方法在模型線性化過程中作了較多簡化，且未對線性化誤差進行補償，因此在較高車速、低附著和曲率連續變化情況下難以保證跟蹤精度。為解決上述問題，本文中設計了一種新的基于DMPC（dynamic MPC）的路徑跟蹤控制方法，該方法對線性化誤差進行補償，從而得到一種改進動力學模型，并根據狀態軌跡和實際狀態量的偏差設計DMPC算法，以期提高路徑跟蹤控制精度。Nayl等設計了誤差預瞄模型用以預測跟蹤誤差，但該方法基于預測時域內路徑曲率不變的假設和前后車體間夾角為小角度的假設，實際的變化曲率較大及前后車體間夾角會導致跟蹤精度的降低。為此，本文中直接根據路徑函數生成參考路徑點而非采用近似預瞄誤差方程，提出了考慮多點預瞄誤差的控制目標，以提高控制精度。

本文中首先構建鉸接車輛路徑跟蹤動力學模型，而后設計DMPC控制器，最后通過Matlab/Simulink與Adams聯合仿真驗證所提出路徑跟蹤控制算法的性能。

1 研究方法

本節介紹預測模型構建和控制器設計方法。首先構建鉸接車非線性車輛動力學模型，采用基于狀態軌跡的線性化方法將其線性化，再近似離散化得到預測模型；而后基于預測模型，根據狀態軌跡和真實狀態變量的偏差設計約束條件、路徑多點預瞄和目標函數，進而給出MPC優化問題。

1.1 預測模型

1.1.1 鉸接車輛動力學模型

鉸接車輛質量、尺寸參數如圖1（a）所示，將鉸接車輛液壓轉向系統等效為作用在前車和后車上的轉向力矩Tn，鉸接車輛受力情況如圖1（b）所示。其中，x1、y1為前車局部坐標系坐標軸，Xw1、Xw2為前車輪胎受到的縱向力，Yw1、Yw2為前車輪胎受到的橫向力，ω1、ω2為前、后車橫擺角速度，T1、T2為前、后車在Tn作用下質心處受到的等效力矩，u1、v1分別為前車的縱向、橫向速度，φ為前后車在鉸接點處的夾角，φ?為前后車夾角加速度，Xw3、Xw4分別為后車輪胎所受到的縱向力，Yw3、Yw4分別為后車輪胎所受到的橫向力，u2、v2分別為后車的縱向、橫向速度。

圖1 鉸接車輛動力學模型

如圖1（b）所示，由達朗貝爾原理：

式中：Y1=Yw1+Yw2；Y2=Yw3+Yw4；X2=Xw3+Xw4；ta為半個輪距。

在路徑跟蹤過程中，兩側輪胎受力可近似為相等，且輪胎力可用近似的線性描述：

式中：Fs、Fα分別為輪胎縱向力和橫向力；Cs、Cα分別為輪胎縱向剛度和側偏剛度；s為輪胎縱向滑移率；α為輪胎側偏角。

S2為后輪縱向滑移率；α1、α2分別為前、后輪側偏角；Cs2為后輪縱向滑移剛度；Cα1、Cα2為前、后輪側偏剛度；；u1為縱向速度，為定值，可控制鉸接車輛的轉向。

1.1.2 模型線性化

由于跟蹤的目標為路徑的X、Y坐標及路徑的航向角，需要對式（6）模型進行擴維處理，使得車輛X、Y坐標及車輛航向角成為狀態變量，選擇χ=作為狀態變量，u=Tn為系統輸入控制量，其中X、Y為前車參考點全局坐標，θ為前車航向角，可得適用于模型預測控制的鉸接車輛動力學模型：

式中：f=M-1F；

采用基于狀態軌跡的線性化方法將鉸接車輛動力學模型線性化［9］，該方法優點是不需要提供每一時刻的狀態變量參考值。假設在某一采樣時刻t，車輛狀態變量為χ0，輸入控制量為u0，假設鉸接車輛在預測時域內的控制輸入量為u0不變，得到一條狀態軌跡?，對前文推導的式（7）非線性模型在狀態軌跡點進行泰勒展開，并忽略高階項：

將式（8）重新整理，可得以χ作為狀態變量、以u作為控制量的表達式：

1.1.3 模型離散化

取采樣時間T，對鉸接車輛動力學模型進行近似離散化［6］，取狀態變量χ，控制量u，則k+1時刻狀態變量χ(k+1)及用于表示目標函數需要的輸出η(k)的表達式為

1.2 MPC控制器設計

1.2.1 約束條件

根據實際工況賦予以下約束：

式（12）表示鉸接點處液壓轉向系統輸入的轉向力矩極值約束；式（13）表示鉸接點處輸入的轉向力矩增量極值約束，其中ε≥0為松弛因子［6］，對控制量增量約束進行松弛，其能夠對求解可行域進行擴展，避免在突然轉彎等極限工況下無法在硬約束上下界內求得可行解，保證可行解存在。

1.2.2 路徑多點預瞄

為消除基于預測時域內曲率不變假設和前后車體間夾角為小角度假設的誤差模型產生的跟蹤誤差，提高路徑跟蹤精度，本文中直接根據期望路徑函數生成路徑點，由預測時域內期望路點的狀態軌跡與預測的車輛行駛狀態軌跡之差獲得跟蹤誤差，設計考慮路徑多點預瞄誤差的目標函數。此外，基于文獻［6］中方法，為綜合考慮前方路徑轉向特性、提高鉸接車輛轉彎時的跟蹤精度，計當前時刻 為t，以[t，t+N?T]時域內路點位置及[t+k?T，t+(k+N)?T]時域內路點航向角為車輛期望狀態軌跡，如圖2所示，N為預測時域長度，各預瞄點間的距離為v?T。

圖2 跟蹤誤差示意圖

將預瞄誤差分為位置偏差ed與航向角偏差eh，其表達式為

式中：xr(t)為t時刻預瞄點的x坐標；x(t)為t時刻車輛的x坐標；yr(t)為t時刻預瞄點的y坐標；y(t)為t時刻車輛的y坐標；θr(t)為t時刻預瞄點的航向角；θ(t)為t時刻的車輛航向角。

1.2.3 MPC優化問題

為實現鉸接車輛的精確跟蹤，設置以下優化目標函數：

式中：ΔU(t)=[Δu(t)，Δu(t+1)，…，Δu(t+Np)]；Δη(t+i|t)=η(t+i|t)-ηref(t+i|t)，表示車輛的跟蹤誤差，可分解為如1.2.2節中的位置偏差ed和航向角偏差eh；ηref(t+i|t)為t+i時刻的系統輸出量參考值，由該時刻參考路徑的x坐標、y坐標、前車航向角組成；Δu(t+i|t)為在t時刻預測的t+i時刻的系統控制量增量；ε為1.2.1節中的松弛因子，為找到偏離硬約束最小的可行解，防止約束上下界被松弛因子過分放大，將松弛因子加入目標函數中進行限制；Q、R、ρ為權重矩陣。

設定新的狀態量［6］：

可得預測時域內所有輸出量的表達式為

將目標函數轉變為標準二次型，在每個控制周期求解上述優化問題，得到最優控制增量序列：

取最優序列的第1項作為當前時刻的鉸接車輛控制量增量。如此進行滾動時域優化控制至跟蹤結束。

2 仿真分析

為驗證所設計控制器的性能，建立Adams鉸接車輛模型與Matlab/Simulink進行聯合仿真，如圖3所示，Adams模型各參數如表1所示，各參數含義見圖1，輪胎選用UA輪胎，路面選用二維平路面。

圖3 Adams鉸接車輛模型

表1 Adams模型參數

仿真環境中設置螺旋線、左轉、雙圓3種仿真工況。鉸接車輛初始X、Y坐標、前車航向角、前后車鉸接點處夾角都為0，由于鉸接車輛在不同前后車夾角、路徑曲率等情況具有不同的速度上限，本文中選取縱向速度為3 m/s，可涵蓋大部分轉向情況速度上限。且每一種工況都將本文中提出的DMPC算法與文獻［8］中提出的MPC算法進行比較。由于本文中只考慮鉸接車輛的橫向控制，故將用于對比的MPC控制器的輸出量稍作調整，假設預測時域內車輛縱向速度不變，只對橫向位置進行控制。仿真均使用i5?7200U處理器，采用內點法在Matlab中使用quadprog求解器進行MPC優化問題求解。

仿真中，設采樣時間為0.05 s，兩控制器參數均調節至最優，其中DMPC權重矩陣Q=5×1010I，R=I，ρ=1000I，預測時域Np=11，k=10。控制量Tn的約束設為-100 kN?m≤u(k+t|t)≤100 kN?m，控制 量 增 量 的 約 束 設 為-20 kN?m≤Δu(k+t|t)≤20 kN?m。輪胎縱向滑移率根據仿真測試數據采用估計值0.15，車輛縱向速度變化量在7%以內，本文控制器在每一采樣時間都對車輛最近路點進行搜索并更新，以減小其影響。

2.1 螺旋線工況

圖4為路徑跟蹤結果，可見兩控制器均能控制鉸接車輛跟蹤參考路徑。

圖4 螺旋線工況路徑跟蹤結果

圖5為位置誤差及控制量隨時間的變化。基于DMPC和MPC的控制器產生的最大位置誤差分別為0.079和0.208 m，降低了62.0%，平均位置誤差分別為0.041和0.143 m，下降了71.3%。基于DMPC和MPC的兩控制器在仿真過程中最大控制量分別為32.486和48.594 kN·m，平均控制量分別為9.881和13.474 kN·m，平均控制量增量分別為0.770和1.189 kN·m。

圖5 螺旋線工況位置誤差及控制量變化

2.2 左轉工況

圖6為路徑跟蹤結果，可見兩控制器均能控制鉸接車輛跟蹤參考路徑。

圖6 左轉工況路徑跟蹤結果

圖7為位置誤差及控制量隨時間的變化。基于DMPC和MPC控制器產生的最大位置誤差分別為0.098和0.185 m，降低了47.0%，平均位置誤差分別為0.043和0.094 m，降低了54.3%。基于DMPC和MPC的兩控制器在仿真過程中最大控制量分別為42.515和63.627 kN·m，平均控制量分別為11.599和16.144 kN·m，平均控制量增量分別為0.968和1.502 kN·m。

圖7 左轉工況位置誤差及控制量變化

2.3 雙圓工況

圖8為路徑跟蹤結果，可見兩控制器均能控制鉸接車輛跟蹤參考路徑。

圖8 雙圓工況路徑跟蹤結果

圖9為位置誤差及控制量隨時間的變化。基于DMPC和MPC的控制器產生的最大位置誤差分別為0.088和0.165 m，降低了46.7%，基于DMPC和MPC的控制器產生的平均位置誤差分別為0.043和0.096 m，降低了55.2%。基于DMPC和MPC的兩控制器在仿真過程中最大控制量分別為49.264和76.280 kN·m，平均控制量分別為6.235和8.590 kN·m，平均控制量增量分別為0.417和0.697 kN·m。

圖9 雙圓工況位置跟蹤誤差及控制量變化

2.4 統計結果

為直觀地體現本文中提出算法與對比算法相比在跟蹤精度上的提高，將各工況下兩控制器產生的最大位置誤差與平均位置誤差進行統計，如圖10和圖11所示。

圖10 各工況最大位置誤差

圖11 各工況平均位置誤差

本文中提出的DMPC算法相比于MPC算法在各工況下的最大位置誤差和平均位置誤差均有效降低，說明本文中設計的基于狀態軌跡的動力學模型和考慮路徑多點預瞄誤差的控制目標有效地提高了路徑跟蹤的控制精度。

3 結論

針對鉸接車輛路徑跟蹤控制問題，建立了適用于模型預測控制的鉸接車輛動力學模型，基于狀態軌跡進行線性化，對線性化誤差進行補償。在此基礎上，構建了考慮多點預瞄誤差的控制目標，并設計了基于動力學模型預測控制的算法。Matlab/Simulink和Adams聯合仿真結果顯示，所設計的方法可有效提升控制精度和控制平順性。

本文中假定鉸接車輛縱向速度恒定且線性化誤差在預測時域內不變。未來將進一步考慮變速工況與動態線性化誤差，以提升控制方法的實用性。