張培培,楊自棟,趙相君
(浙江農(nóng)林大學(xué)工程學(xué)院,杭州市,311300)
拖拉機可以與附裝的、懸掛的或牽引的農(nóng)機具一起完成起壟、播種、噴藥、收獲等大部分田間作業(yè)??萍疾吭凇皵?shù)字農(nóng)業(yè)”建設(shè)戰(zhàn)略研討會上提出,以“精準(zhǔn)農(nóng)業(yè)”和“智能農(nóng)業(yè)”為切入點,組織實施數(shù)字化農(nóng)業(yè)科技行動,實施精準(zhǔn)農(nóng)業(yè),廣泛應(yīng)用智能農(nóng)業(yè)機械,提高資源利用率和經(jīng)濟效益[1]。因此拖拉機自動駕駛技術(shù)已成為農(nóng)業(yè)裝備智能化研究熱點之一[2-6],其研究方向主要分為車輛行駛軌跡規(guī)劃和車輛軌跡跟蹤控制兩個方面。
隨著控制技術(shù)的發(fā)展,國內(nèi)外眾多學(xué)者對拖拉機路徑跟蹤進行了建模并提出了相應(yīng)的控制策略。應(yīng)用較多的有PID控制[7]、模糊控制[8]、滑模控制[9]、模型預(yù)測控制(Model Predictive Control,MPC)[10]、非線性模型預(yù)測控制(Nonlinear Model Predictive Control,NMPC)[11]等。
拖拉機是與農(nóng)機具或掛車完成田間作業(yè),因此近十年來眾多學(xué)者開始對拖拉機牽引農(nóng)機具或者掛車系統(tǒng)進行自動駕駛的路徑跟蹤控制進行研究[11-15]。Backman等[11]建立了基于拖拉機—牽引式農(nóng)機具運動學(xué)模型的NMPC路徑跟蹤系統(tǒng),設(shè)計的NMPC控制器能夠保證拖拉機和農(nóng)機具在牽引速度12 km/h時的橫向誤差最大誤差為10 cm。Kayacan等[12]建立了拖拉機掛車機組的縱向和橫向的非線性動力學(xué)模型,設(shè)計的PID控制器使掛車機組有效地跟蹤曲線路徑。Cariou等[13]研究了農(nóng)用車輛—牽引式農(nóng)機具在地頭轉(zhuǎn)向和速度自動駕駛控制,并設(shè)計了基于運動學(xué)模型前進和倒退的路徑跟蹤控制器,系統(tǒng)以0.5 m/s速度倒車沿期望路徑行駛80 m時,農(nóng)機具的后軸中心的橫向誤差在±20 cm。Karkee等[14]研究了拖拉機單軸牽引農(nóng)機具系統(tǒng),分別建立了運動學(xué)模型、動力學(xué)模型和高度仿真模型,試驗證明當(dāng)牽引速度小于4.5 m/s和輸入頻率小于1 rad/s運動學(xué)模型能較好地反映車輛行駛時的運動學(xué)特性,并設(shè)計了基于拖拉機牽引農(nóng)機具運動學(xué)模型的閉環(huán)LQR控制器跟蹤期望路徑。
本文針對拖拉機—牽引式農(nóng)機具系統(tǒng)在運動軌跡假設(shè)的基礎(chǔ)上建立運動學(xué)模型。該系統(tǒng)是本身存在一定運動約束的一類欠驅(qū)動系統(tǒng),既獨立控制輸入少于自由度的系統(tǒng),且具有非線性,因此為簡化穩(wěn)定跟蹤控制器的驗算和求解,本文應(yīng)用準(zhǔn)確線性化方法對建立的模型線性化,最后采用滑模變結(jié)構(gòu)算法來實現(xiàn)對拖拉機—農(nóng)機具系統(tǒng)的跟蹤控制,并通過仿真手段分析和驗證了控制算法的有效性和實時性。
為簡化研究問題,作如下假設(shè):車輛只作平行于地面的平面運動;車輛速度較低,車輪沒有側(cè)滑;拖拉機和牽引式農(nóng)機具鉸接點在拖拉機的后軸中心。這樣,拖拉機—牽引式農(nóng)機具系統(tǒng)就可以簡化為如圖1所示模型。
拖拉機—牽引式農(nóng)機具的直線行駛運動軌跡跟蹤模型如圖1所示,(XOY)為卡迪爾坐標(biāo)系,使農(nóng)機具跟蹤X軸,X軸為期望路徑,即控制目標(biāo)為y2→0,θ→0,γ→0。
圖1 拖拉機—農(nóng)機具直線跟蹤運動模型Fig. 1 Line path tracking model of tractor-trailer system
根據(jù)圖1模型建立拖拉機—農(nóng)機具系統(tǒng)的直線運動學(xué)微分方程
(1)
(2)
(3)
(4)
(5)
以P點橫坐標(biāo)x2為新的時間量標(biāo),新量標(biāo)x2與時間t之間的關(guān)系
(6)
式中:V1——拖拉機后軸中心速度。
式(1)~式(5)可以改寫為
(7)
(8)
(9)
則拖拉機—農(nóng)機具的線性化狀態(tài)方程
(10)
因此可以為該線性系統(tǒng)設(shè)計一個負反饋線性控制器u作為新系統(tǒng)輸入
(11)
根據(jù)式(11),拖拉機前輪轉(zhuǎn)角α如式(12)。
(12)
式(10)、式(11)表明,設(shè)計合理的負反饋控制器u會使系統(tǒng)快速穩(wěn)定,當(dāng)x2不斷增大時,ξ→0,根據(jù)式(8)得到y(tǒng)2→0,θ→0,γ→0即車輛直線運動時會跟蹤X軸。式(12)表明,前輪轉(zhuǎn)角α由u、θ,γ決定。
圖2為拖拉機—農(nóng)機具系統(tǒng)圓弧軌跡示意圖,農(nóng)機具車軸中心的期望路徑的半徑為R,農(nóng)機具車軸中心的實際位置用極坐標(biāo)(r,φ)表示,用以跟蹤期望路徑,極坐標(biāo)的圓心為期望路徑的圓心O,O1為拖拉機的瞬時回轉(zhuǎn)中心,O2為農(nóng)機具的瞬時回轉(zhuǎn)中心。
圖2 拖拉機—農(nóng)機具圓弧跟蹤運動模型Fig. 2 Circle path tracking model of tractor-trailer system
以期望路徑的弧長l=Rφ作為新的時間量標(biāo),以(r-R)→0,θ→定值,(γ-φ)→90°為控制目標(biāo)。根據(jù)圖2模型建立新量標(biāo)l與時間t的關(guān)系和拖拉機—農(nóng)機具的圓弧運動學(xué)微分方程
(13)
(14)
將式(14)通過坐標(biāo)變換進行準(zhǔn)確的線性化,得到與式(10)類似的系統(tǒng)線性化模型。
(15)
(16)
根據(jù)式(16)可以求出拖拉機前輪轉(zhuǎn)角α如式(17)。
(17)
設(shè)計合理的負反饋控制器u會使系統(tǒng)快速穩(wěn)定,當(dāng)新的時間量標(biāo)l不斷增加,ξ→0,r快速收斂到R。式(17)表明,極坐標(biāo)系下拖拉機—農(nóng)機具系統(tǒng)圓弧軌跡跟蹤時拖拉機前輪轉(zhuǎn)角α由u、r、θ和γ-φ決定。
拖拉機—牽引式農(nóng)機具系統(tǒng)具有明顯的非線性,前面將該系統(tǒng)進行準(zhǔn)確的線性化為反饋線性控制提供了基礎(chǔ)。本文采用滑模變結(jié)構(gòu)算法來實現(xiàn)對該線性化模型的跟蹤控制。
設(shè)有線性系統(tǒng)
(18)
A、B——系數(shù)矩陣;
v——輸出矩陣控制函數(shù)。
滑模面設(shè)計為
(19)
n——狀態(tài)變量的維數(shù)。
根據(jù)本文建立的拖拉機—農(nóng)機具的模型是一個三階系統(tǒng),即n=3。
滑??刂瓢吔\動和滑模運動兩個過程。為了保證快速趨近同時削弱抖振,本文采用指數(shù)趨近律的方法和準(zhǔn)滑動模態(tài)控制。
指數(shù)趨近律
(20)
式中:ε——等速趨近速率,ε>0;
k——指數(shù)趨近項系數(shù),k>0。
ε和k決定了趨近切換面時的速度。
(21)
式中:δ——很小的正常數(shù)。
(22)
根據(jù)Lyapunov穩(wěn)定性理論,系統(tǒng)是穩(wěn)定的,結(jié)合式(18)~式(21)得到系統(tǒng)的變結(jié)構(gòu)滑??刂坡?/p>
(23)
合理地選擇切換函數(shù)矩陣C可以保證滑模變結(jié)構(gòu)控制系統(tǒng)具有良好的動態(tài)特性,C的選取方法主要有極點配置法、二次型最優(yōu)等,Ackermann公式可以簡單有效地進行極點配置[18],故本文基于Ackermann公式進行極點配置選取C。線性系統(tǒng)(18)是一個閉環(huán)三階系統(tǒng),具有3個都位于左半開復(fù)平面的極點,其中2個共軛主導(dǎo)極點λ1、λ2由系統(tǒng)的參數(shù)決定,為使第3個實數(shù)極點λ3對系統(tǒng)的影響小,應(yīng)使其遠離兩個主極點。
根據(jù)文獻[12]確定系統(tǒng)的固有頻率和阻尼為ωn= 2.26 Hz,ζ=0.70,則主導(dǎo)極點
(24)
第3個極點λ3遠離2個主導(dǎo)極點,取λ3=-10。
根據(jù)Ackermann公式
(25)
其中:f(A)=(A-Iλ1)(A-Iλ2)(A-Iλ3)。
根據(jù)式(25)可以求出c1=40.3,c2=32.0,c3=12.8,取ε=0.5,k=3,δ=0.01,則系統(tǒng)的滑模控制律為
(26)
一輛自制的青山牌輪轂電動拖拉機和小型半掛車組作為仿真試驗用車,拖拉機的軸距L1=1.4 m,鉸接點到半掛車軸的距離L2=1.2 m。
根據(jù)上述滑模變結(jié)構(gòu)控制器的設(shè)計,搭建仿真模塊并在Matlab/Simulink環(huán)境下分別進行拖拉機—農(nóng)機具系統(tǒng)的直線和圓弧路徑跟蹤仿真,假設(shè)運動速度穩(wěn)定在1 m/s。設(shè)置初始時刻為t0=0,末端時刻t1=30 s,求解算法采用龍格—庫塔數(shù)值解法,設(shè)定模型仿真步長為0.01 s。
系統(tǒng)的直線軌跡跟蹤初始條件如下:y2=0.5 m,θ=0,γ=π/6,仿真試驗結(jié)果如圖3所示。
(a) 狀態(tài)變量隨時間變化歷程
(b) 前輪轉(zhuǎn)角隨時間變化歷程
(c) 農(nóng)機具與拖拉機夾角隨時間變化歷程
(d) 農(nóng)機具與X軸夾角隨時間變化歷程圖3 直線跟蹤仿真結(jié)果Fig. 3 Straight-path-tracking simulation results
線性化系統(tǒng)的狀態(tài)變量ξ經(jīng)過約3 s 后均收斂到0,農(nóng)機具的后軸中心P點的Y坐標(biāo)y2=ξ1從初始位置0.5 m增大到約0.67 m后迅速收斂于期望路徑X軸;前輪轉(zhuǎn)角α經(jīng)過約5 s收斂到0°;農(nóng)機具與拖拉機夾角θ在(-60.26°,22.24°)波動后快速收斂到0°;農(nóng)機具與X軸夾角γ在(-26.38°,30.00°)波動后約5 s后收斂到0°,說明農(nóng)機具可以跟隨拖拉機沿著期望路徑行駛,因此設(shè)計的控制器可對直線軌跡進行跟蹤。
拖拉機—農(nóng)機具系統(tǒng)的圓弧軌跡跟蹤初始條件如下:r-R=1 m,θ=π/18,γ=π/2,φ=π/10。參考軌跡圓的半徑依次設(shè)計為5 m、10 m和20 m,仿真試驗結(jié)果如圖4所示。
(a) 狀態(tài)變量隨時間變化歷程
(b) 前輪轉(zhuǎn)角隨時間變化歷程
(c) 農(nóng)機具與拖拉機的夾角隨時間變化歷程
(d) (γ-φ)隨時間變化歷程圖4 圓弧軌跡跟蹤仿真結(jié)果Fig. 4 Curved-path-following simulation results
從圖4(a)中看出,線性化系統(tǒng)的狀態(tài)變量ξ經(jīng)過約3 s收斂到0,農(nóng)機具的車軸中心P點的實際半徑與期望半徑差(r-R)=ξ1從初始值1 m快速收斂到0。圖4(b)、圖4(c)和圖4(d)可以看出,R依次為5 m、10 m 和20 m 時,前輪轉(zhuǎn)角α經(jīng)過約7 s分別快速收斂到期望值16.14°、8.97°和5.09°;農(nóng)機具與拖拉機的夾角θ分別收斂到13.47°、6.81°和3.40°;(γ-φ)均快速收斂于90°,說明農(nóng)機具的縱向中心軸線與期望軌跡圓弧快速相切,這些表明農(nóng)機具的回轉(zhuǎn)中心O2收斂于期望路徑的圓心O,由此可見設(shè)計的控制器可對曲線軌跡進行跟蹤。
1) 應(yīng)用非線性相關(guān)理論對欠驅(qū)動拖拉機—牽引式農(nóng)機具系統(tǒng)動態(tài)規(guī)律進行了分析,以此建立了拖拉機—牽引式農(nóng)機具系統(tǒng)的直線和圓弧行駛運動軌跡跟蹤模型,為達到簡化運動的目的,應(yīng)用準(zhǔn)確線性化方法對建立的非線性系統(tǒng)模型線性化。
2) 采用滑模變結(jié)構(gòu)算法設(shè)計了拖拉機—牽引式農(nóng)機具系統(tǒng)的跟蹤控制算法。最后利用Matlab/Simulink對系統(tǒng)仿真試驗,結(jié)果表明:設(shè)計的控制器可以使線性化系統(tǒng)的狀態(tài)變量ξ經(jīng)過約3 s收斂到0,在直線跟蹤時,農(nóng)機具從初始位置y2=0.5 m經(jīng)過約3 s跟蹤到期望路徑X軸,農(nóng)機具與拖拉機夾角及農(nóng)機具角度偏移量均在5 s左右收斂到0°;在跟蹤半徑為5 m、10 m 和20 m的圓弧軌跡時,農(nóng)機具車軸中心從偏離期望圓弧半徑1 m的初始位置經(jīng)過約3 s可以跟蹤到期望圓弧,農(nóng)機具與拖拉機夾角均可快速收斂到定值,同時農(nóng)機具的回轉(zhuǎn)中心O2收斂于期望路徑的圓心O,因此農(nóng)機具的后軸中心P點能迅速跟蹤不同期望路徑,驗證了控制算法具有良好的道路跟蹤精度和動態(tài)性。