基于MPC的履帶車步進行駛控制器設計

宋秋杰,熊樹生

(浙江大學,浙江 杭州 310012)

0 引言

機場跑道建設是機場建設的重中之重,基于滑模攤鋪的混凝土施工技術多應用于機場停機坪等大面積混凝土攤鋪工程,可以在保證施工質量、效率的同時控制成本,近年來逐漸成為機場道路攤鋪的主流方案[1]。由于滑模攤鋪特殊的施工方式,盡管攤鋪機能夠快速高質量完成攤鋪作業,其鋪設的道面仍需要人工進行抹平抹光、拉毛養護等后處理工作,如圖1所示。

圖1 滑模攤鋪及其人工后處理

我國滑模攤鋪技術引入較晚,缺乏高素質的攤鋪后處理技術工人,往往需要投入大量人力完成后處理工作,人工處理速度慢,道面后處理質量參差不齊,提高了施工成本的同時極大地影響了道路攤鋪效率。

針對上述問題,亟需一種自動化后處理作業車,代替人工進行道面后處理工作。本文基于該智能車工作的重難點,對其行駛控制器進行理論分析與開發設計,實現后處理智能車自動步進行駛功能,為項目開發奠定理論基礎。

1 智能車動力學建模

根據施工要求,本文研究對象的結構示意圖如圖2所示。

圖2 攤鋪后處理智能車結構示意圖

如圖所示,該車為履帶車,由于履帶結構的特殊性,其在正常行駛的過程中必然伴隨著較為明顯的滑移滑轉現象,因此該車的運動學模型示意圖如圖3所示[2]。

圖3 履帶車運動學示意圖

圖中,考慮滑移滑轉時,Or為轉向中心,v1r為低速側履帶實際速度,v2r為高速側履帶實際速度,vr為履帶車實際速度,ωr為車輛實際橫擺角速度,Rr為車輛實際轉向半徑,B為左右履帶間距。不考慮滑移滑轉時,ω為車輛橫擺角速度,v1為低速側履帶速度,v2為高速側履帶速度,R為履帶車轉向半徑。由圖可知,相較于無滑移滑轉的情況,實際履帶車低速側履帶速度增加,高速側履帶速度減小,車輛轉向半徑增大。根據運動學關系,可以得到:

(1)

考慮滑移滑轉的動力學示意圖如圖4所示。圖中,O1、O2分別為左右履帶的瞬時轉動中心,A1、A2分別為左右履帶的轉向極(即瞬時轉動中心距履帶中心的距離),P(x1,y1)、Q(x2,y2)分別為左右履帶上的一點,vp、vq分別為P、Q點相對地面的滑動速度,vpx和vpy分別為P點滑動速度在x軸和y軸上的分量,vqx和vqy分別為Q點滑動速度在x軸和y軸上的分量,Fp和Fq分別為P、Q點受到的滑動摩擦力,θ1和θ2分別為vp、vq和各自履帶中軸線的夾角,F1、F2分別為左右履帶受到的縱向滑動摩擦力,f1和f2分別為左右履帶受到的行駛阻力,是由履帶車機械結構決定的。M1和M2分別為左右履帶受到的行駛阻力矩。

圖4 履帶車動力學示意圖

根據幾何關系結合力學分析可得:

(2)

由于履帶受力均勻且忽略履帶寬度的影響,因此有:

(3)

式中,G為車輛重量,φ為地面摩擦因數。

根據運動學原理有:

(4)

對低速側履帶中心點取矩,根據力與力矩平衡條件可以得到:

(5)

式中,f為行駛阻力系數。聯立上述公式,解超越方程即可求得未知量A1和A2。根據運動學關系可得:

(6)

在左右履帶理論線速度v1和v2已知的情況下,根據已求得的A1、A2,結合公式(5)和公式(6)即可求得ωr和Rr。至此,履帶車動力學模型求解完成。

(7)

(8)

2 四次多項式步進軌跡規劃

2.1 軌跡規劃坐標轉換

四次多項式軌跡曲線表達式如下:

(9)

式中,S為前方車道線上某點相對攝像頭所在直線的橫向偏移距離,Sy為車道線上某點距攝像頭的縱向距離。K1～K5為三次多項式系數。對于沿該多項式行駛的車輛,其橫向加速度與縱向距離的對應關系為:

(10)

步進軌跡需要保證沿該軌跡行駛的履帶車不會與待施工路面發生碰撞,履帶車接地部分僅有兩側履帶,因此在碰撞分析時僅需考慮履帶與路面的接觸情況。為便于分析,將履帶簡化為長為L,寬為b的長方形,兩側履帶中心距為B,左右履帶內側頂點分別為M、N、P、Q,如圖5所示。

圖5 履帶簡化與參數定義

設沿軌跡行駛的履帶車幾何中心點的坐標為Q(x,y),履帶車橫擺角為φ,如圖6所示,則四個頂點的坐標可以通過幾何關系求得。

圖6 軌跡上某點的坐標轉換

M點坐標為:

(11)

N點坐標為:

(12)

P點坐標為:

(13)

Q點坐標為:

(14)

2.2 軌跡規劃碰撞約束

為使履帶車行駛時不會與待施工路面產生碰撞,需要對履帶頂點位置做出限制,設待施工路面寬度為D,安全距離為i,如圖7所示。

圖7 碰撞約束

(15)

2.3 軌跡規劃優化函數設計

為保證步進的整個過程中履帶車行駛平滑穩定且步進結束時平穩停車,定義關于車輛最大曲率、最大急動度及終點曲率的代價函數[3]:

J=aρ2+bj2+cσ2

(16)

其中,ρ為軌跡最大曲率,j為軌跡最大急動度,σ為軌跡終點曲率,a、b、c為權重系數。

最大曲率、最大急動度的求解會在每一次迭代中再求解一次參數極值,若采用優化算法進行迭代求解,理論較為復雜且算力要求較高。為滿足工程需求,將最大曲率和最大急動度用插值代替,即:

(17)

式中t代表采樣時刻。由于四次多項式曲線是連續且平滑的,因此只要采樣頻率足夠,且每項插值均滿足約束條件,則插值優化函數可以保證整個曲線接近最優曲線。

在整個步進過程中,定義φ(t)為采樣時刻t的車輛運動狀態,S為整個步進過程中的運動狀態合集。即:

φ(t)= [ρt,jt,yt,φt]T?t∈[1,Nend]

(18)

S=[φ(1),φ(2),…φ(Nend)]

(19)

用lr(t)、hr(t)分別表示在采樣時刻t,車輛的運動狀態約束,并用LR和HR分別表示整個步進過程中的上下邊界條件。即:

(20)

(21)

綜上所述,四次多項式軌跡規劃優化函數為:

s.t.LR≤S≤HR

(22)

至此,四次多項式軌跡規劃轉化為求解上述優化問題,通過設置合適的權重系數,利用MATLAB提供的fmincon函數,采用默認的內點法或有效集法即可求得優化后的四次多項式軌跡。

3 MPC軌跡跟蹤控制器設計

3.1 MPC基本原理

模型預測控制機理如圖8所示[4],包含三個重要環節:預測模型、滾動優化、反饋矯正。基于本文討論的軌跡跟蹤控制問題,三個環節的內容可以概括為:

圖8 MPC基本原理

1)預測模型:即車輛動力學模型。

2)滾動優化:在下一時刻,需要再次測量該時刻的狀態量,并將其作為初始條件重新進行優化求解。

3)反饋矯正:狀態量與參考量之間的差值作為反饋信息,構成整體的閉環控制。

3.2 非線性MPC理論模型

根據車輛運動學規律,履帶車在步進過程中滿足:

(23)

根據以上公式,取狀態量:χ=[x,y,φ]T,控制量:u=[v,ω]T。對于履帶車控制系統,任意時刻的狀態量和控制量均滿足:

(24)

某一時刻,在任一點(χr,ur)處,根據泰勒定理,保留一階項,忽略高階項得:

(25)

根據公式(23)、(24)和(25),狀態量誤差的變化量為:

(26)

定義離散時間間隔為T,對公式(26)進行前向歐拉離散化可得:

(27)

在非線性MPC控制算法中,采用目標函數:

(28)

該目標函數中的第一項反映了該控制算法對目標軌跡的跟蹤能力,第二項反映了該控制算法中控制量變化的平穩性,第三項ρ為權重,ε為松弛因子,避免出現優化問題無解的情況。

3.3 線性MPC理論模型

(29)

其中,Nc=2為控制量個數,Nx=3為χ的狀態量個數,η為狀態量輸出方程。在滿足控制精度的情況下,嘗試簡化計算,做出如下假設:

對控制周期內任一時刻下的狀態方程有,

Ak=A,k∈[1,…,t+N-1]

Bk=B,k∈[1,…,t+N-1]

(30)

在該假設條件下,對公式(29)進行遞歸計算:

(31)

歸納總結得到狀態量ξ的輸出方程為:

(32)

由上式可以看出,根據當前時刻的狀態量ξ(k)和控制時域內的控制增量ΔU即可求得預測時域內的狀態量輸出Y,且這種映射關系是線性的,即通過定義新的狀態量將非線性系統線性化。

類似地,定義優化目標函數:

(33)

仿真及實驗的理想控制效果為車輛運行軌跡和目標軌跡重合,因此Yref=[0,0,…,0]T。結合式(32)和式(33),優化函數可以改寫為:

J(k)=ΔUT(ΘTQΘ+R)ΔU+2ETQΘΔU+ρε2+ETQE

(34)

將ΔU作為參數,則上述目標函數轉變為quadprog二次規劃問題,將優化后的結果序列ΔU中的第一個元素作用于被控對象,在下一時刻重復上述步驟,即可實現模型預測控制。由于二次規劃問題已有較為完備高效的求解方法,因此該模型計算速度較快,在實際應用中可以結合PID輔助控制,使車輛盡量在縱向上實現恒速行駛。

3.4 控制器MATLAB仿真驗證

在MATLAB中對上述兩種模型進行仿真驗證,待跟蹤的軌跡由軌跡規劃給出。仿真時,對于同一條參考軌跡,為比較兩種方法的控制效果,在同樣仿真參數下進行驗證,具體數值見表1。非線性模型預測仿真中,目標函數權重設置為:R=Q=100INp;線性模型預測仿真中,目標函數權重設置為:R=Q=100INp,PID控制器參數為:Kp=0.5,Ki=0.08,Kd=0.01。

表1 模型預測控制仿真參數

結合動力學模型約束條件及履帶車本身機械結構、穩定性要求,對模型預測控制的控制量、控制量增量及狀態量進行約束,邊界條件見表2。

表2 模型預測控制仿真約束

設定履帶與待施工路面間的最小安全距離為0.2 m,步進距離為6 m,則履帶車在終點的位置目標狀態為[xend,yend,φend]=[6,0,0],理想狀態下履帶車縱向速度恒定為1 m。假設車輛初始位姿為[x0,y0,φ0]=[0,0.5,0],取優化函數中曲率及急動度的采樣步長均為0.1 m,優化函數的權重系數分別為:at=bt=1(t∈[1,60]),ct=100,優化算法采用內點法。四次多項式軌跡規劃輸出結果為:

y=-0.0012x4+0.0185x3-0.0833x2+0.5

(35)

終點處曲率為1.3×10-4,橫擺角為1.4×10-12,履帶車幾何中心縱坐標為2.12×10-12,軌跡最大曲率為0.16,可見該軌跡規劃滿足軌跡平穩及終點運動狀態要求。

在上述參考軌跡及約束條件下,在MATLAB中對非線性模型預測控制、線性模型預測控制與PID控制耦合模型分別進行仿真,得到圖9所示結果。

圖9 模型預測結果圖

從結果圖可以看出,在理想條件下,非線性模型預測控制和線性模型預測與PID耦合控制均能對給定軌跡進行較精確的跟蹤。

為進一步分析兩種算法的優劣,對整個仿真過程中的狀態量及其偏差進行分析,繪制出運動學參數相對于縱向位置的變化圖,如圖10所示。

圖10 模型預測結果分析圖

上述兩種工況下,線性模型預測的線速度曲線起初對軌跡速度的跟蹤效果較差,但隨著仿真的進行,其線速度偏差逐漸回落,最后趨于平穩,其后半程偏差均穩定在0.01 m/s內,這是由于在控制算法中加入了PID橫向速度跟蹤控制器,其對線速度和角速度的跟蹤性能遠強于非線性模型預測控制器。

值得注意的是,線性模型預測控制器的縱向位置偏差均較高,但總體跟蹤效果仍良好,這說明該控制算法對橫向位置的跟蹤有延遲性,但隨著時間的推移,控制算法能較快彌補,且接近終點處該算法的各項參數均更加穩定,說明該算法在終點處控制效果更好。兩種算法的橫向位置偏差均遠小于0.2 m,說明軌跡跟蹤滿足碰撞極限約束。

4 結論

本文針對道面攤鋪后處理智能車,設計了一種基于MPC的步進行駛控制器,相較于人工步進行駛,該控制器基于履帶車動力學模型、四次多項式軌跡規劃及MPC控制算法實現了自動的步進行駛,提高了行駛的安全性及終點的平穩性。在MATLAB中對上述控制器進行仿真驗證,結果表明,本文設計的控制器滿足功能要求,對提升道面后處理施工效率具有重要意義。