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

楊福威 孟 紅 朱 強(qiáng)

1 引言

精確而穩(wěn)定的軌跡跟蹤控制是實(shí)現(xiàn)無人車輛智能化和實(shí)用化的關(guān)鍵。國內(nèi)外學(xué)者在軌跡跟蹤控制方面做了許多研究，提出了PID控制算法、純跟蹤控制算法、模糊自適應(yīng)控制算法、模型預(yù)測控制算法等。但車輛高速運(yùn)動時(shí)軌跡跟蹤的復(fù)雜性和路面條件的波動性等問題導(dǎo)致軌跡跟蹤控制一直是目前的技術(shù)難點(diǎn)［1～7］。

上述控制方法大多需要建立精確的數(shù)學(xué)模型，但車輛是一個(gè)復(fù)雜的非線性系統(tǒng)，在工程應(yīng)用中往往需要對模型進(jìn)行簡化，而高精度的控制方法還需要考慮各種不確定因素，這些都會導(dǎo)致控制問題的復(fù)雜化。

模型預(yù)測控制算法是一類滾動求解帶約束優(yōu)化問題的控制方法。模型預(yù)測控制具有預(yù)測模型、滾動優(yōu)化和反饋校正的優(yōu)點(diǎn)，能夠克服復(fù)雜環(huán)境下的跟蹤誤差和隨機(jī)擾動，能夠及時(shí)彌補(bǔ)模型失配以及外界干擾引起的不確定性，適用于數(shù)學(xué)模型建立不精確且存在約束條件的系統(tǒng)［8～15］。

本文首先建立了履帶車輛在打滑條件下的運(yùn)動學(xué)模型和基于地面力學(xué)和剪切變形理論的非線性動力學(xué)模型?；趦煞N模型設(shè)計(jì)線性時(shí)變預(yù)測控制器，將無人車輛的軌跡跟蹤問題轉(zhuǎn)化為標(biāo)準(zhǔn)的二次規(guī)劃問題。最后在MATLAB/Simulink中建立車輛動力學(xué)模型和預(yù)測控制器，驗(yàn)證了控制算法的準(zhǔn)確性和魯棒性。

2 運(yùn)動學(xué)模型

首先建立全局坐標(biāo)系XOY和固定在車輛上的局部坐標(biāo)系xoy，局部坐標(biāo)系原點(diǎn)為無人車輛質(zhì)心位置。將無人車輛運(yùn)動模型簡化為二維模型，選取左、右兩側(cè)轉(zhuǎn)速為控制量。運(yùn)動學(xué)模型如圖1所示。

圖1 履帶車輛運(yùn)動學(xué)模型

無人車輛質(zhì)心運(yùn)動方程為

由式（1）可知，通過控制兩側(cè)主動輪的轉(zhuǎn)速，即可得到無人車輛相應(yīng)的速度和轉(zhuǎn)角。在考慮打滑率和打滑角的情況下，運(yùn)動學(xué)模型為

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

3 履帶車輛動力學(xué)模型

為了方便對無人車輛進(jìn)行動力學(xué)分析，現(xiàn)作如下假設(shè)：

1）車輛在水平硬路面上進(jìn)行穩(wěn)態(tài)轉(zhuǎn)向運(yùn)動，運(yùn)動過程中所受的剪切力與剪切位移有關(guān)；

2）直駛與轉(zhuǎn)向過程的行駛阻力系數(shù)相同；

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

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

3.1 轉(zhuǎn)向慣性力

圖2為車輛瞬態(tài)轉(zhuǎn)向受力情況。圖中ICR為瞬態(tài)轉(zhuǎn)向中心，cx和cy分別為質(zhì)心偏移的橫向與縱向距離，CR為質(zhì)心運(yùn)動軌跡的旋轉(zhuǎn)中心。

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

圖2 履帶車輛瞬態(tài)轉(zhuǎn)向受力情況

3.2 履帶-地面相互作用力

3.2.1 履帶-地面法向作用力

轉(zhuǎn)向時(shí)慣性力的橫向分量Fcy使得高速側(cè)履帶接地段的法向負(fù)荷減小，低速側(cè)履帶接地段的法向負(fù)荷增加。分別表示如下：

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

3.2.2 履帶-地面剪切力

當(dāng)履帶車輛在堅(jiān)硬路面下運(yùn)動時(shí)，履帶接地段的剪切力與剪切位移有關(guān)，忽略土壤的黏著系數(shù)和內(nèi)聚摩擦因數(shù)等參數(shù)的影響，可以表示為

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

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

式中，b為履帶板寬度。

3.2.3 履帶-地面剪切位移

履帶接地段任意一點(diǎn)（縱向坐標(biāo)為x）到最前端的距離為

則該點(diǎn)運(yùn)動到最前端所需時(shí)間為

式中，T為一個(gè)仿真步長，N為主動輪轉(zhuǎn)速為0時(shí)經(jīng)歷的迭代次數(shù)。

高速側(cè)履帶（縱向坐標(biāo)為x2）和低速側(cè)履帶（縱向坐標(biāo)為x1）上某點(diǎn)瞬時(shí)滑動速度在縱向與橫向方向的分量為

則該點(diǎn)處剪切位移的縱向與橫向分量為

合成的剪切位移為

3.3 轉(zhuǎn)向牽引力與制動力

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

車輛穩(wěn)態(tài)轉(zhuǎn)向時(shí)，高速側(cè)縱向方向與橫向方向的剪切力表示為剪切力dF2、dF1在兩個(gè)方向上的分量沿履帶接地長度上的積分。

3.4 轉(zhuǎn)向驅(qū)動力矩和阻力矩

驅(qū)動力矩由兩側(cè)履帶縱向力分量繞車輛質(zhì)心產(chǎn)生，高、低速側(cè)履帶的驅(qū)動力矩分別表示為

式中，W 為車體寬度；左轉(zhuǎn)向時(shí)，i=1；右轉(zhuǎn)向時(shí)，i=-1。

阻力矩由兩側(cè)履帶橫向力分量繞車輛質(zhì)心產(chǎn)生，高、低速側(cè)履帶的轉(zhuǎn)向阻動力矩分別表示為

3.5 滾動阻力

高、低速側(cè)履帶滾動阻力表示為

4 模型預(yù)測控制

4.1 線性預(yù)測模型的建立

與非線性模型預(yù)測控制相比，線性模型預(yù)測控制具有計(jì)算量小，求解時(shí)間短等優(yōu)點(diǎn)，更加適用于無人車輛進(jìn)行實(shí)時(shí)控制。建立的非線性系統(tǒng)為

將上面的連續(xù)方程進(jìn)行離散化，得到

4.2 基于運(yùn)動學(xué)模型的MPC算法

1）預(yù)測方程

d～k，t為系統(tǒng)離散線性化后產(chǎn)生的誤差，設(shè)定系統(tǒng)的預(yù)測時(shí)域?yàn)镹p，控制時(shí)域?yàn)镹c，則在預(yù)測時(shí)域內(nèi)未來一段時(shí)間的輸出為

其中：

由式（23）可知，預(yù)測時(shí)域內(nèi)輸出量可以通過當(dāng)前時(shí)刻狀態(tài)量ξ(t | t)、預(yù)測時(shí)域內(nèi)未知的控制增量ΔU(t)得到，需要進(jìn)行預(yù)測和滾動優(yōu)化。

2）優(yōu)化求解

要求得控制增量ΔU(t)，必須建立合適的優(yōu)化目標(biāo)函數(shù)，如下式

式中，Q和R為權(quán)重矩陣。第一項(xiàng)反應(yīng)系統(tǒng)對參考軌跡的跟蹤能力，第二項(xiàng)反應(yīng)系統(tǒng)對控制量變化的約束，第三項(xiàng)是松弛因子，當(dāng)目標(biāo)函數(shù)在控制周期內(nèi)無最優(yōu)解時(shí)，系統(tǒng)會以次優(yōu)解代替。經(jīng)過相應(yīng)的矩陣運(yùn)算，目標(biāo)函數(shù)可以轉(zhuǎn)化為

其中：

3）約束條件

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

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

建立如下約束條件

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

4）仿真模型建立

履帶無人車輛的參數(shù)如表1所示。

表1 履帶無人車輛參數(shù)表

在Matlab/Simulink中建立車輛的動力學(xué)模型和控制器，其中，基于運(yùn)動學(xué)模型的MPC控制器基于S函數(shù)編寫。S-Function的輸入量為位置狀態(tài)量[X ，Y，φ]T，由動力學(xué)模型計(jì)算給出。輸出量為兩側(cè)轉(zhuǎn)速[ωL，ωR]T，通過MPC控制器的二次規(guī)劃函數(shù)求最優(yōu)值得到，輸出給動力學(xué)模型，如此實(shí)現(xiàn)了閉環(huán)控制。

4.3 基于動力學(xué)模型的MPC算法

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

5 計(jì)算機(jī)仿真與結(jié)果分析

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

在車輛低速狀態(tài)下，可以認(rèn)為車輛不存在打滑，基于式（2）建立預(yù)測模型。基于兩種模型建立控制器，進(jìn)行仿真。在高速狀態(tài)下，基于動力學(xué)模型（31）建立MPC算法的控制模型，仿真結(jié)果如下。

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

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

圖4 低速時(shí)基于運(yùn)動學(xué)模型和動力學(xué)模型的兩側(cè)轉(zhuǎn)速變化曲線

圖5 低速時(shí)狀態(tài)量誤差變化曲線

從圖4可以看出，仿真時(shí)兩側(cè)轉(zhuǎn)速變化比較平滑，這主要是因?yàn)閷D(zhuǎn)速增量進(jìn)行了約束。動力學(xué)控制器兩側(cè)轉(zhuǎn)速較高，主要是考慮了路面作用力和車輛打滑的影響，所以需要增大主動輪轉(zhuǎn)速來克服打滑的影響。運(yùn)動學(xué)控制器的計(jì)算時(shí)間短，實(shí)時(shí)性好，低速時(shí)可以基于運(yùn)動學(xué)模型建立控制器進(jìn)行軌跡跟蹤。

下面是車輛高速運(yùn)動時(shí)仿真結(jié)果。

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

圖7 兩側(cè)轉(zhuǎn)速變化曲線

圖8 高速時(shí)狀態(tài)量誤差變化曲線

圖9 縱向與側(cè)向車速變化曲線

圖10 縱向與側(cè)向加速度變化曲線

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

圖12 滑移率變化曲線

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

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

6 結(jié)語

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

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

3）MPC算法具有滾動優(yōu)化和反饋校正的優(yōu)點(diǎn)，通過設(shè)定約束條件，可以使得控制量平穩(wěn)變化，增加車輛的穩(wěn)定性。

［1］Fang H，F(xiàn)an 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］李逃昌，胡靜濤，高雷，等.基于模糊自適應(yīng)純追蹤模型的農(nóng)業(yè)機(jī)械路徑跟蹤方法［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，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ī)器人動力學(xué)模型及其反饋控制［J］.清華大學(xué)學(xué)報(bào)（自然科學(xué)版），2006，46（08）：1377-1380.

［7］李力，鄒興龍.海底機(jī)器人自動跟蹤預(yù)定開采路徑控制［J］.機(jī)械工程學(xué)報(bào)，2007，43（1）：152-157.

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

［9］龔建偉，姜巖，徐威.無人駕駛車輛模型預(yù)測控制［M］.北京：北京理工大學(xué)出版社，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的車輛穩(wěn)定性控制研究［J］.汽車工程，2016，38（3）：308-317.

［16］王紅巖，王欽龍，芮強(qiáng)等.高速履帶車輛轉(zhuǎn)向過程分析與試驗(yàn)驗(yàn)證［J］.機(jī)械工程學(xué)報(bào)，2014，50（16）：162-172.