基于滑模變結構的農機路徑跟蹤控制算法研究

姜龍騰,遲瑞娟,李青君,黃修煉,熊澤鑫,馬悅琦,朱曉龍

(1.中國農業大學 工學院,北京 100083; 2.上海海事大學 商船學院,上海 201306)

0 引言

農機自動駕駛是實現農業機械現代化必不可少的一環,農機自動駕駛的路徑規劃及路徑跟蹤是研究農機無人駕駛技術的基礎和關鍵[1]。路徑跟蹤的目的是通過跟蹤算法使車輛按照預定路徑軌跡行駛,主要工作模式是通過車載傳感器獲取當前車輛姿態及位置信息,根據創建的車輛運動學模型,通過上位機計算出車輛控制參數,從而保證車輛行駛軌跡與預設軌跡路徑誤差最小化[2-3]。

目前,路徑跟蹤算法有純跟蹤控制算法[4]、PID控制算法[5]、神經網絡[6]、滑模變結構法、機器學習[7]以及前輪反饋控制算法[8]等。對于滑模變結構法的控制算法,許多學者也進行過大量研究。1978年,Young等人首先將滑模變結構控制器應用于機器人的控制中[9]。2004年,王榮本等人采用滑模變結構控制理論設計了控制器,并建立了汽車轉向動力學模型,采用二次型最優設計法搭建了系統的切換超平面,并描述了切換超平面的一些選取方法,經過試驗測試后,車輛具有良好的跟蹤效果[10]。2013年,Guo J H 等人提出了一種橫向自適應的模糊滑模控制方法[11],能夠克服預瞄運動學模型中的參數不確定問題,與單純的滑模控制相比,這種方法有利于提高汽車跟隨過程中的跟蹤精度,且響應時間較短,反應靈敏。

農機與汽車或機器人工作性質不同,其需要克服較為復雜的農田環境因素干擾,同時障礙物的存在會對算法修正參數產生影響[12],故選擇合理的路徑跟蹤算法將提升農機的作業效率、改善農藝水平。為滿足農機行駛的平順性、安全性及控制精度要求,本文采取滑模變結構路徑跟蹤控制算法設計路徑跟蹤控制器,建立汽車運動學模型,并通過Simulink與CarSim進行聯合仿真分析,對比分析基于PID控制算法的預瞄時間自適應模塊的路徑跟蹤效果,以驗證滑模變結構路徑跟蹤控制效果的理想性。

1 車輛的運動學模型

由于農田環境較為復雜,道路較為崎嶇,本文采用二輪車輛模型來分析無人駕駛農機的運動情況。假設汽車始終做平行于地面的運動,速度向前為正,向后為負。車輛的運動學模型如圖1所示。

圖1 車輛的運動學模型Fig.1 Kinematics model of the vehicle

以xoy為坐標系,x為汽車的橫向位置,y為汽車的縱向位置,(xf,yf)為汽車前軸坐標,(xr,yr)為汽車后軸坐標,vf為汽車前軸中心的速度,vr為汽車后軸中心的速度,δf為汽車前輪轉角。圖1中,l為汽車前后軸距,α為汽車的航向偏角[13]。

根據車輛的運動學模型,可以得到

(1)

(2)

(3)

vr=vfcosδf

(4)

將式(1)～式(4)整理化簡可以得到汽車前進和后退的運動學模型,即

(5)

(6)

(7)

2 基于滑模變結構路徑跟蹤控制器設計

經典的PID控制算法雖然應用較廣,技術較為成熟,但由于其對環境適應性較差,導致其跟蹤精度不高。因此,設計了一種滑模變結構控制器應用于無人駕駛農機的軌跡跟蹤,該算法具有對環境適應性好、抗干擾能力強等優點。

2.1 轉向控制策略

滑模變結構控制器采用的轉向控制策略為:將方向盤轉角信號和農機上安裝的各類傳感器所感知的狀態信號輸入到農機的狀態觀測器,從而獲得與農機穩定性相關的數值。將該數值與理想值輸入到控制器中[14],經過上位機計算出的轉角理想值與車載傳感器輸出的實際值產生一個偏差,利用該偏差來控制轉向電機,電機產生一個疊加轉角,該疊加轉角與模擬駕駛員輸入的方向盤轉角之和即為農機行駛過程中總的方向盤轉角,從而使農機行駛更加穩定。轉向控制策略如圖2所示。

圖2 轉向控制策略框圖Fig.2 Block diagram of steering control strategy

農機在行駛過程中,當其轉向輸入后會產生質心側偏角和橫擺角速度等響應,這些響應與轉向輸入符合線性關系,可構建該線性二自由度車輛模型為

(8)

(9)

(10)

其中,δf為前輪轉角;k1為前輪的側偏剛度;k2為后輪的側偏剛度;x=[β,ωd]T為車輛的狀態變量;β為質心側偏角;ωd為橫擺角速度。

(11)

(12)

其中,μ為路面的附著系數。

2.2 滑模變結構控制器設計

根據簡化農機車輛模型與線性化運動學方程,可以得到實際的橫擺角速度與理想的橫擺角速度之差為滑模變結構控制器的控制誤差,公式為

e=ωr-ωd

(13)

其中,ωr為實際的橫擺角速度;ωd為理想的橫擺角速度。

理想的橫擺角速度選擇為

(14)

其中,LH為汽車的預瞄距離,tp定為0.6s。

滑模變結構的切換函數定義為

(15)

其中,λ0為加權系數,其數值為正。

式(15)進行求導可得

(16)

由式(8)可得

(17)

然后,再將式(16)代入式(17),可得

(18)

(19)

為了保證滑模變結構控制器的滑模條件可以不受系統干擾和參數變化的影響,定義滑模控制規律為

δf=δf_equ-k0sgn(s)

(20)

其中,sgn(s)為符號函數;k0為系統的工作點趨近于切換面的運動速率,其數值越大趨近速度越快。因當k0值過大時系統產生的抖動較大,故其數值的選取應滿足如下條件,即

(21)

其中,η0為正實數。

為了消除控制輸入所產生的振動,將符號函數sgn(·)用飽和函數sat(·)進行代替,公式為

(22)

所以,最終滑模變結構控制器的控制規律為

δf=δf_equ-k0sat(s/ε)

(23)

其中,ε為邊界層的寬度。

3 路徑跟蹤控制算法的聯合仿真分析

3.1 基于預瞄時間自適應PID的路徑跟蹤控制仿真分析

3.1.1 Simulink模型的建立

在PID控制算法的基礎上加入了預瞄時間自適應模塊,通過Simulink建立可以輸出一個自適應預瞄時間,其與速度之積為自適應預瞄距離。將自適應預瞄距離作為CarSim的輸入,使車輛仿真過程中能夠獲得最佳預瞄距離,提高其跟蹤精度[15]。

首先,根據PID控制算法的規則建立Simulink模型,如圖3所示。經過多次調試,最終確定比例部分Kp的數值為100,積分部分Ki的數值為15,微分部分Kd的數值為1.5。

圖3 PID模塊的Simulink模型Fig.3 Simulink model of PID module

然后,根據預瞄時間自適應控制規律建立預瞄時間自適應控制器的Simulink模型,如圖4所示。

圖4 預瞄時間自適應控制器的Simulink模型Fig.4 Simulink model of adaptive controller for preview time

圖4中,L為質心的橫向偏差;detaf為車輛的航向角偏差;Vx為車速;tp為自適應預瞄時間。

預瞄時間自適應模塊的整體Simulink模型如圖5所示。

圖5 預瞄時間自適應模塊的Simulink模型Fig.5 Simulink model of adaptive module of preview time

圖5中,RHS_road為道路的曲率;AY_y為汽車行駛時的加速度;ada_tp為自適應預瞄時間;ada_dis為自適應預瞄距離,即自適應預瞄時間與汽車行駛速度之積。

3.1.2 CarSim模型的建立

在CarSim中搭建汽車的整車模型以及道路模型,輸入農機車輛參數,并設置道路圖形,如圖6所示。

圖6 PID算法的CarSim道路圖形Fig.6 CarSim road graph of PID algorithm

然后,設置CarSim整車模型的輸入與輸出。其中,自適應的預瞄距離和方向盤轉角為CarSim的輸入,質心橫向偏差、曲率、縱向車速、加速度、質心側偏角、預瞄點的橫向偏差、實際位置、理想位置為CarSim的輸出。

在CarSim中選擇“Send to Simulink”,進行聯合仿真分析,結果如圖7所示。

圖7 實際路徑與參考路徑對比圖Fig.7 Comparison between actual path and reference path

質心側偏角如圖8所示。由圖8可知:加入預瞄模塊的PID控制算法雖然簡單,但是其本身對環境的變化比較敏感,質心側偏角受轉向影響較大,最大質心側偏角在0.15°左右,抗干擾能力差以及超調量較大,其跟蹤效果并不理想。

圖8 質心側偏角Fig.8 Sideslip Angle of centroid

3.2 基于滑模變結構路徑跟蹤控制算法的聯合仿真分析

3.2.1 Simulink模型的建立

首先,建立滑模變結構控制算法控制誤差的Simulink模型,即理想的橫擺角速度與實際橫擺角速度之差,如圖9所示。

圖9 滑模變結構控制誤差的Simulink模型Fig.9 Simulink model of sliding mode variable structure control error

圖9中,Vx為縱向車速,tp取0.6s,L為預瞄點的預瞄距離,beta為質心側偏角,w_r為實際的橫擺角速度,y為理想的橫擺角速度。

根據滑模變結構控制算法的切換函數及控制規則,建立Simulink模型,如圖10所示。

圖10 滑模變結構控制的切換函數及控制規則的Simulink模型Fig.10 Simulink model of switching function and control rule of sliding mode variable structure control

根據滑模變結構控制的控制規律建立Simulink模型,如圖11所示。

圖11 滑模變結構控制規律的Simulink模型Fig.11 Simulink model of sliding mode variable structure control law

建立汽車的左(右)前輪轉矩的Simulink模型,如圖12所示。

圖12 整車左(右)前輪轉矩的Simulink模型Fig.12 Simulink model of vehicle left (right) front wheel torque

3.2.2 CarSim模型的建立

整車參數的選擇與PID算法相同,道路圖形如圖13所示。

圖13 滑模變結構控制算法的CarSim道路圖形Fig.13 CarSim road graph of sliding mode variable structure control algorithm

方向盤轉角、左前輪轉矩、右前輪轉矩為CarSim整車模型的輸入,縱向車速、橫擺角速度、質心側偏角、預瞄點的預瞄距離、理想的X坐標、理想的Y坐標、實際的X坐標、實際的Y坐標、輪速、橫向偏差為CarSim整車模型的輸出。

進行聯合仿真分析,結果如圖14～16所示。聯合仿真后,得到了兩種算法的實際路徑與參考路徑對比圖。根據曲線可知:基于預瞄時間自適應的 PID路徑跟蹤控制算法的圖像在汽車拐彎時超調量較大,跟蹤效果不理想;基于滑模變結構路徑跟蹤控制算法的路徑圖誤差較小,兩條曲線接近于重合狀態,跟蹤效果較好,車輛轉彎時橫向偏差控制在0.45m之內。因此,滑模變結構路徑跟蹤控制算法相對于PID路徑跟蹤算法的路徑跟蹤精度更高,對路況的適應性更好,更適用于無人駕駛農機車輛的路徑跟蹤控制。

圖14 實際路徑與參考路徑對比圖Fig.14 Comparison between actual path and reference path

圖15 實際路徑與參考路徑對比局部放大圖Fig.15 local enlargement of comparison between actual path and reference path

圖16 橫向偏差圖Fig.16 Transverse deviation diagram

4 結論

1)根據車輛運動學模型與線性化二自由度模型,通過增加控制量極限約束,求解了結構控制器控制規律,建立了基于滑模變結構控制算法路徑跟蹤控制器,使得車輛實際轉角與理想轉角誤差最小化,保證對路徑跟蹤的平穩性。

2)利用Simulink與CarSim建立聯合仿真,對加入預瞄時間自適應模塊的PID控制算法及滑模變結構路徑跟蹤控制算法進行仿真。結果表明:較加入預瞄時間自適應模塊的PID控制算法,滑模變結構跟蹤效果比較理想,實際路徑與期望路徑的誤差較小,橫向偏差控制在0.45m內,具有響應快、抗干擾能力強、操作簡單等優點,是一種適用于農機的路徑跟蹤算法。