基于模型預測控制的履帶式無人平臺軌跡跟蹤控制算法研究?

楊福威 孟 紅 朱 強

1 引言

精確而穩定的軌跡跟蹤控制是實現無人車輛智能化和實用化的關鍵。國內外學者在軌跡跟蹤控制方面做了許多研究，提出了PID控制算法、純跟蹤控制算法、模糊自適應控制算法、模型預測控制算法等。但車輛高速運動時軌跡跟蹤的復雜性和路面條件的波動性等問題導致軌跡跟蹤控制一直是目前的技術難點［1～7］。

上述控制方法大多需要建立精確的數學模型，但車輛是一個復雜的非線性系統，在工程應用中往往需要對模型進行簡化，而高精度的控制方法還需要考慮各種不確定因素，這些都會導致控制問題的復雜化。

模型預測控制算法是一類滾動求解帶約束優化問題的控制方法。模型預測控制具有預測模型、滾動優化和反饋校正的優點，能夠克服復雜環境下的跟蹤誤差和隨機擾動，能夠及時彌補模型失配以及外界干擾引起的不確定性，適用于數學模型建立不精確且存在約束條件的系統［8～15］。

本文首先建立了履帶車輛在打滑條件下的運動學模型和基于地面力學和剪切變形理論的非線性動力學模型?；趦煞N模型設計線性時變預測控制器，將無人車輛的軌跡跟蹤問題轉化為標準的二次規劃問題。最后在MATLAB/Simulink中建立車輛動力學模型和預測控制器，驗證了控制算法的準確性和魯棒性。

2 運動學模型

首先建立全局坐標系XOY和固定在車輛上的局部坐標系xoy，局部坐標系原點為無人車輛質心位置。將無人車輛運動模型簡化為二維模型，選取左、右兩側轉速為控制量。運動學模型如圖1所示。

圖1 履帶車輛運動學模型

無人車輛質心運動方程為

由式（1）可知，通過控制兩側主動輪的轉速，即可得到無人車輛相應的速度和轉角。在考慮打滑率和打滑角的情況下，運動學模型為

其中iL、iR分別為兩側履帶的打滑率，α為車輛速度方向與縱向方向的夾角，計算公式為

3 履帶車輛動力學模型

為了方便對無人車輛進行動力學分析，現作如下假設：

1）車輛在水平硬路面上進行穩態轉向運動，運動過程中所受的剪切力與剪切位移有關；

2）直駛與轉向過程的行駛阻力系數相同；

3）考慮無人車輛高速轉向時離心力的縱向分量與橫向分量的作用［16］。

4）將履帶視為不可拉伸的柔性帶子，不考慮履帶張力對接地壓力的影響。

3.1 轉向慣性力

圖2為車輛瞬態轉向受力情況。圖中ICR為瞬態轉向中心，cx和cy分別為質心偏移的橫向與縱向距離，CR為質心運動軌跡的旋轉中心。

車輛在縱向方向和橫向方向的慣性力為

圖2 履帶車輛瞬態轉向受力情況

3.2 履帶-地面相互作用力

3.2.1 履帶-地面法向作用力

轉向時慣性力的橫向分量Fcy使得高速側履帶接地段的法向負荷減小，低速側履帶接地段的法向負荷增加。分別表示如下：

履帶接地段壓力分布與地面狀況相關，松軟地面下載荷均勻分布，堅硬地面下載荷主要集中在負重輪正下方，類似于集中載荷，高、低速側接地段壓力分布為

3.2.2 履帶-地面剪切力

當履帶車輛在堅硬路面下運動時，履帶接地段的剪切力與剪切位移有關，忽略土壤的黏著系數和內聚摩擦因數等參數的影響，可以表示為

式中，μ為履帶與地面之間的摩擦系數；j為履帶接地段剪切位移；K為剪切模量。

兩側履帶接地段剪切力與滑動速度相反，單位面積下的剪切力可表示為

式中，b為履帶板寬度。

3.2.3 履帶-地面剪切位移

履帶接地段任意一點（縱向坐標為x）到最前端的距離為

則該點運動到最前端所需時間為

式中，T為一個仿真步長，N為主動輪轉速為0時經歷的迭代次數。

高速側履帶（縱向坐標為x2）和低速側履帶（縱向坐標為x1）上某點瞬時滑動速度在縱向與橫向方向的分量為

則該點處剪切位移的縱向與橫向分量為

合成的剪切位移為

3.3 轉向牽引力與制動力

令δ2和δ1分別為高速側和低速側履帶滑動速度與縱向方向的夾角，則有

車輛穩態轉向時，高速側縱向方向與橫向方向的剪切力表示為剪切力dF2、dF1在兩個方向上的分量沿履帶接地長度上的積分。

3.4 轉向驅動力矩和阻力矩

驅動力矩由兩側履帶縱向力分量繞車輛質心產生，高、低速側履帶的驅動力矩分別表示為

式中，W 為車體寬度；左轉向時，i=1；右轉向時，i=-1。

阻力矩由兩側履帶橫向力分量繞車輛質心產生，高、低速側履帶的轉向阻動力矩分別表示為

3.5 滾動阻力

高、低速側履帶滾動阻力表示為

4 模型預測控制

4.1 線性預測模型的建立

與非線性模型預測控制相比，線性模型預測控制具有計算量小，求解時間短等優點，更加適用于無人車輛進行實時控制。建立的非線性系統為

將上面的連續方程進行離散化，得到

4.2 基于運動學模型的MPC算法

1）預測方程

d～k，t為系統離散線性化后產生的誤差，設定系統的預測時域為Np，控制時域為Nc，則在預測時域內未來一段時間的輸出為

其中：

由式（23）可知，預測時域內輸出量可以通過當前時刻狀態量ξ(t | t)、預測時域內未知的控制增量ΔU(t)得到，需要進行預測和滾動優化。

2）優化求解

要求得控制增量ΔU(t)，必須建立合適的優化目標函數，如下式

式中，Q和R為權重矩陣。第一項反應系統對參考軌跡的跟蹤能力，第二項反應系統對控制量變化的約束，第三項是松弛因子，當目標函數在控制周期內無最優解時，系統會以次優解代替。經過相應的矩陣運算，目標函數可以轉化為

其中：

3）約束條件

這里需要考慮控制量約束、控制增量約束和輸出量約束?？紤]如下矩陣

式中，m與控制量的維度相同，?為克羅內克積，u(k -1)為上一時刻的控制量。

建立如下約束條件

每一個控制周期內在約束條件下完成對式（26）的求解后，可得到控制增量，取該序列的第一個元素為控制輸入量，可得到當前時刻的控制量為

4）仿真模型建立

履帶無人車輛的參數如表1所示。

表1 履帶無人車輛參數表

在Matlab/Simulink中建立車輛的動力學模型和控制器，其中，基于運動學模型的MPC控制器基于S函數編寫。S-Function的輸入量為位置狀態量[X ，Y，φ]T，由動力學模型計算給出。輸出量為兩側轉速[ωL，ωR]T，通過MPC控制器的二次規劃函數求最優值得到，輸出給動力學模型，如此實現了閉環控制。

4.3 基于動力學模型的MPC算法

基于上述動力學模型建立MPC控制器。設定控制量約束、控制增量約束和輸出量約束，如式（30）所示，采用有效集法進行求解。

5 計算機仿真與結果分析

這里設計了低速和高速時兩種仿真軌跡。仿真軌跡1：0～20s為直線；20s～60s為圓形；60s～90s為雙移線，初始誤差：Y=3m。期望速度為3m/s。仿真軌跡2：0～15s跟蹤直線，15s～35s跟蹤圓弧，35s～50s跟蹤直線，50s～70s跟蹤圓弧。初始誤差：Y=10m。期望速度為8m/s。

在車輛低速狀態下，可以認為車輛不存在打滑，基于式（2）建立預測模型?；趦煞N模型建立控制器，進行仿真。在高速狀態下，基于動力學模型（31）建立MPC算法的控制模型，仿真結果如下。

從圖3和圖5可以看出，兩種控制器在t=10s時就能跟蹤上期望軌跡，車輛跟蹤圓形軌跡誤差較大，原因在于車輛前進方向不斷改變，車輛難以預測未來一段時間的狀態變化，這可以通過增加預測時域來減小誤差，但是相應會降低運算速度，影響實時性。直線和雙移線航向角變化平緩，車輛能準確跟蹤，X方向最大誤差不超過4m，Y方向最大誤差不超過2m。注意，此處的誤差是軌跡誤差，與路徑誤差不同。從圖3可以看出，路徑誤差最大不超過0.1m。

圖3 低速仿真軌跡變化曲線

圖4 低速時基于運動學模型和動力學模型的兩側轉速變化曲線

圖5 低速時狀態量誤差變化曲線

從圖4可以看出，仿真時兩側轉速變化比較平滑，這主要是因為對轉速增量進行了約束。動力學控制器兩側轉速較高，主要是考慮了路面作用力和車輛打滑的影響，所以需要增大主動輪轉速來克服打滑的影響。運動學控制器的計算時間短，實時性好，低速時可以基于運動學模型建立控制器進行軌跡跟蹤。

下面是車輛高速運動時仿真結果。

圖6 高速仿真軌跡變化曲線

圖7 兩側轉速變化曲線

圖8 高速時狀態量誤差變化曲線

圖9 縱向與側向車速變化曲線

圖10 縱向與側向加速度變化曲線

圖11 橫擺角速度與角加速度變化曲線

圖12 滑移率變化曲線

從圖7和圖8可以看出，基于運動學模型建立的控制器跟蹤效果較差，在40s以前會在期望軌跡附近來回擺動，X和Y方向的誤差在10m左右，嚴重偏離預定軌跡。40s后才能沿著期望軌跡行駛，在實際過程中，難以保證車輛的行駛穩定性和軌跡可控性?；趧恿W模型建立的控制器在較大初始誤差條件下也能跟蹤上期望軌跡，X和Y方向的軌跡誤差不超過5m，在給定軌跡下能穩定行駛，不會出現跑偏和震蕩現象。從圖9～11中可看出，車輛在直駛過程中會出現一定的抖振現象，但幅度較小，經計算，穩定直駛時縱向車速變化幅度不超過1km/h，橫向車速變化幅度不超過0.6km/h。如果減小采樣頻率或者地面阻力系數較小時，抖振現象會有所減弱。從圖12可以看出，車輛在起步時出現了嚴重的打滑現象，這是因為初始誤差設計較大，車輛驅動力矩大，由于慣性作用使得車輛產生了劇烈的打滑。

仿真過程中，基于運動學模型建立的控制器比基于動力學模型的跟蹤誤差大，車輛軌跡偏離較為嚴重。原因在于基于運動學模型的控制器未曾考慮地面作用力、轉向離心力和滑移對車輛產生的影響，而在高速時這些參數的變化明顯，使得運動學仿真模型與車輛真實情況差距很大。動力學模型較好地考慮了這些參數的影響，在仿真實驗時能準確跟蹤上期望軌跡，說明堅硬土路上的動力學模型建立的準確，控制算法具有良好的收斂性和穩定性。

6 結語

1）基于運動學和動力學模型建立了兩種MPC控制器。Matalb仿真實驗驗證了算法的準確性和魯棒性。低速時軌跡誤差在3m左右，高速時基于動力學模型建立的控制器跟蹤誤差不超過5m。由于運動學模型沒有考慮打滑率和地面作用力的影響，在高速行駛時不能夠準確反映車輛的運動情況，需要基于動力學模型構建。

2）由于受打滑的影響，車輛在低速直駛過程中會出現抖振現象，可以通過增加計算精度或者在MPC算法約束條件中降低控制增量的變化率來削弱這種現象；

3）MPC算法具有滾動優化和反饋校正的優點，通過設定約束條件，可以使得控制量平穩變化，增加車輛的穩定性。

［1］Fang H，Fan R，Thuilot B，et al.Trajectory tracking con?trol of farm vehicles in presence of sliding［J］.Robotics and Autonomous Systems，2006，54（10）：828-839.

［2］Marino R，Scalzi S，Netto M.Nested PID steering control for lane keeping in autonomous vehicles［J］.Control Engi?neering Practice，2011，19（12）：1459-1469.

［3］Marcel SG，Arben C，Lesolliec G.Longitudinal Con trol for an All-Electric Vehicle［C］//Proceedings of IEEE In?ternational Conference on Electric Vehicle， 2012：357-375.

［4］李逃昌，胡靜濤，高雷，等.基于模糊自適應純追蹤模型的農業機械路徑跟蹤方法［J］.農業機械學報，2013，44（1）：205-210.

［5］ Thomas Thueer，Roland Siegwart.Mobility evaluation of wheeled all-terrain robots［J］.Journal of Robotics and Autonomous Systems，2010，58（5），508-519.

［6］李巖，楊向東，陳懇.履帶式移動機器人動力學模型及其反饋控制［J］.清華大學學報（自然科學版），2006，46（08）：1377-1380.

［7］李力，鄒興龍.海底機器人自動跟蹤預定開采路徑控制［J］.機械工程學報，2007，43（1）：152-157.

［8］孫銀健.基于模型預測控制的無人車輛軌跡跟蹤控制算法研究［D］.北京：北京理工大學，2015：50-60.

［9］龔建偉，姜巖，徐威.無人駕駛車輛模型預測控制［M］.北京：北京理工大學出版社，2014：72-105.

［10］Falcone P，Borrelli F，Asgari J，et al.Predictive active steering control for autonomous vehicle systems［J］.Con?trol Systems Technology，IEEE Transactions on，2007，15（3）：566-580.

［11］Falcone P，Eric Tseng H，Borrelli F，et al.MPC-based yaw and lateral stabilisation via active front steering and braking［J］.Vehicle System Dynamics，2008，46（S1）：611-628.

［12］ Falcone P，Borrelli F，Asgari J，et al.A model predic tive control approach for combined braking and steering in autonomous vehicles［C］//Control&Automation，2007.MED'07.Mediterranean Conference on.IEEE，2007：1-6.

［13］Falcone P，Borrelli F，Tseng H E，et al.Linear timevary?ing model predictive control and its application to active steering systems：Stability analysis and experimental val?idation［J］.International journal of robust and nonlinear control，2008，18（8）：862-875.

［14］Falcone P，Tufo M，Borrelli F，et al.A linear time var ying model predictive control approach to the integrated vehicle dynamics control problem in autonomous systems［C］//Decision and Control，2007 46th IEEE Conference on.IEEE，2007：2980-2985.

［15］陳杰，李亮，宋健.基于LTV-MPC的車輛穩定性控制研究［J］.汽車工程，2016，38（3）：308-317.

［16］王紅巖，王欽龍，芮強等.高速履帶車輛轉向過程分析與試驗驗證［J］.機械工程學報，2014，50（16）：162-172.