吳帆， 蔣蘋， 胡文武， 羅亞輝， 金生

(湖南農(nóng)業(yè)大學機電工程學院，湖南 長沙 410128)

近年來，隨著傳感器的精度不斷提高，自動調(diào)平控制系統(tǒng)已經(jīng)逐漸被應用于農(nóng)業(yè)機械領域[1-4]。目前，國內(nèi)市場上絕大部分耕整地[5]類農(nóng)業(yè)機械為農(nóng)業(yè)機具非自動調(diào)平的拖拉機，不能保證其機具在農(nóng)田作業(yè)的時候保持實時水平狀態(tài)。而平整的農(nóng)田不僅可以很大程度上降低其用水率，并且能夠提升土壤中肥料的利用率。因此，農(nóng)業(yè)機具自動調(diào)平控制[6-8]在作物生產(chǎn)過程中顯得尤為重要。胡煉等[9]采用傾角傳感器和直線位移傳感器與控制技術，通過電磁換向閥來控制機具的水平角度，平均絕對誤差0.40°；彭凱等[10]設計了旋耕機三點懸掛[11]調(diào)平系統(tǒng)，通過復合數(shù)字濾波算法[12]對傳感器信號進行處理，濾波后數(shù)據(jù)變得平滑，誤差值在0.1°左右，能夠有效地抗干擾及波動。丁為民等[13]將調(diào)平控制系統(tǒng)用于犁旋一體機上，實現(xiàn)了犁旋一體機的自動調(diào)平，相比較需手動調(diào)平的機具，耕深一致性和土地平整性有大幅度提高。這些自動調(diào)平控制系統(tǒng)主要都通過頻繁控制電磁換向閥來控制油缸伸縮量，容易對液壓系統(tǒng)造成損害。作者以1GDZ-150型履帶拖拉機為平臺，研制并加裝了一套自動調(diào)平控制系統(tǒng)。采取單缸設定不動點調(diào)平策略，通過自主研制的傾角傳感器實時檢測機具的傾斜角度變化，將卡爾曼濾波后數(shù)據(jù)反饋回控制器，根據(jù)其數(shù)據(jù)采用電機驅(qū)動全液壓轉(zhuǎn)向機控制油缸的伸縮，以滿足機具田間作業(yè)需求。

1 自動調(diào)平控制系統(tǒng)整體結(jié)構及工作原理

自動調(diào)平控制系統(tǒng)主要由拖拉機、控制系統(tǒng)、電機、驅(qū)動模塊、液壓驅(qū)動系統(tǒng)、執(zhí)行機構、傾角傳感器等組成。該系統(tǒng)整體如圖1所示。田間作業(yè)時，農(nóng)業(yè)機具上的傳感器將實時采集的機具的水平度發(fā)送給主控制器，收到角度后，判斷是否處于預定角度的范圍。當角度超出設定值時，主控制器發(fā)信號給電機控制器，帶動電機正反轉(zhuǎn)來控制全液壓

轉(zhuǎn)向機進出油，控制其油缸的行程，從而保持農(nóng)業(yè)機具的水平。機具調(diào)至預先設定范圍以內(nèi)，開始下個周期的調(diào)平，實現(xiàn)農(nóng)業(yè)機具調(diào)平的閉環(huán)控制，保持機具水平度始終在預設范圍之內(nèi)。當傾斜的角度越大時，電機轉(zhuǎn)速越快，調(diào)平油缸伸縮量越大，形成比例對應關系。

1.拖拉機；2.主控制器；3.電機控制器；4.電機；5.閥塊及電磁閥；6.液壓轉(zhuǎn)向機；7.舉升油缸；8.調(diào)平油缸；9.農(nóng)業(yè)機具；10.傾角傳感器。

1.tractor; 2.main controller; 3.motor controller; 4.motor; 5.valve block and solenoid valve; 6.hydraulic steering gear; 7.lifting cylinder; 8.leveling cylinder; 9.agricultural machinery; 10.inclination sensor.

圖1 自動調(diào)平控制系統(tǒng)
Fig.1 Automatic leveling control system

牽引式作業(yè)機具簡化為調(diào)平機構示意圖，如圖2所示。A、C為化簡后的調(diào)平油缸，B、D為調(diào)平支撐桿的固定端點。其中，AC為油缸處于50 mm行程初始位置，AC1為油缸縮短后長度，AC2為油缸伸長后長度，θ為牽引式作業(yè)機具相較地面的傾角。調(diào)平支撐桿兩端球頭固定，調(diào)平油缸伸縮相應行程帶動牽引式作業(yè)機具調(diào)節(jié)相較于地面的傾角。

圖2 調(diào)平機構示意圖

油缸行程L±L0(mm)與牽引式作業(yè)機具相較地面的角度θ(°)的關系如下：

(1)

式中：CD為油缸固定端點至調(diào)平支撐桿固定端點的長度，cm；θ為∠C1DC或∠C2DC，°；L為調(diào)平油缸處于50 mm行程位置時長度，mm；L0為調(diào)平油缸伸縮行程長度，mm。

2 機具水平度檢測系統(tǒng)設計

機具水平角度檢測系統(tǒng)主要包括硬件和軟件算法兩部分，如圖3所示。WT901C模塊輸出溫度、陀螺儀、加速度計等實時數(shù)據(jù)至主控芯片STC15W4K48S4，再進行軟件算法處理，包括初始化、數(shù)據(jù)平均值處理、卡爾曼濾波、角度估計，最后輸出其角度值。

圖3 機具水平度檢測系統(tǒng)

2.1 硬件設計

水平度檢測系統(tǒng)硬件主要由主控芯片STC15W4K48S4、WT901C模塊、電源電路、485串口通信電路等組成。

2.1.1 核心處理器 根據(jù)設計要求，本文所選取的主控芯片為STC公司的STC15W4K48S4，該芯片的工作頻率范圍為5 MHz至28 MHz ，完全滿足角度運算的處理要求；具有48字節(jié)的片F(xiàn)lash、4組串口通信端口、8通道高速AD、6通道高精度PWM、2通道CCP、SPI高速同步串行通信端口與低功耗設計等特點[14]。

2.1.2 傾角模塊 傾角模塊的精度是測量水平度的關鍵，本文選取的傾角模塊為WT901C，最高輸出數(shù)據(jù)速率可達200 Hz，加速度量程±16 g，穩(wěn)定性0.01 g，對應分辨率6.1e-5g，角速度量程±2 000°·s-1，穩(wěn)定性0.05°·s-1，對應分辨率7.6e-3°·s-1，角度量程±180°[15]。

傾角傳感器使用RS485串口與主控制器通信，波特率115 200 k·s-1。

2.2 軟件算法

田間作業(yè)過程中，機體因結(jié)構原因震動劇烈、田間路面復雜多變以及傳感器自身精度不高，極易對傾角傳感器數(shù)據(jù)造成較大干擾。而卡爾曼濾波算法[16-17]能夠在噪聲污染的信號中，采取遞推方式，濾除靜態(tài)與動態(tài)過程中姿態(tài)測量系統(tǒng)的干擾信號，以獲取更加精確的信號值。將離散系統(tǒng)的陀螺儀常值與傾角實際值的偏差b為狀態(tài)向量，其過程方程與測量方程如下：

X(K)=AX(K-1)+BU(K-1)+W(K-1)

(2)

Z(K)=HX(K)+V(K)

(3)

式中：X(K)為K時刻下的狀態(tài)矩陣；U(K-1)為K-1時刻下對系統(tǒng)的控制量；Z(K)為K時刻下的測量值；W(K-1)為K-1時刻過程的噪聲；V(K)為K時刻測量的噪聲；A、B與H分別為系統(tǒng)參數(shù)值和測量系統(tǒng)的參數(shù)值。

其迭代計算公式如下：

驗算其預測狀態(tài)：

X(K|K-1)=AX(K-1|K-1)+BU(K)

(4)

姿態(tài)測量系統(tǒng)K時刻對應的誤差協(xié)方差：

P(K|K-1)=AP(K-1|K-1)AT+Q

(5)

計算其卡爾曼增益矩陣：

Kg(K)=P(K-1|K-1)HT/(HP(K|K-1)HT+R

(6)

式中：Kg為濾波增益；R為測量噪聲協(xié)方差，根據(jù)經(jīng)驗確定為0.5。

修正其預測的偏差：

X(K|K)=X(K|K-1)+Kg(K)(Z(K)-HX(K|K-1)

(7)

更新誤差協(xié)方差矩陣：

P(K|K)=(I-Kg(K)H)P(K|K-1)

(8)

式中：I為單位矩陣。

3 試驗與分析

試驗于2019-06-30在湖南農(nóng)業(yè)大學車輛實訓中心進行，試驗車輛為1GDZ-150型履帶式拖拉機，試驗機具為旋耕機，采用自主研制傾角傳感器測量機具的傾斜角，通過自制遙控器(無線模塊為RFM96)與自制控制器進行無線通訊[18-19]。

3.1 靜態(tài)試驗

將傾角傳感器放置水平面上靜置，分別采集卡爾曼濾波和未卡爾曼濾波的傾角數(shù)據(jù)，數(shù)據(jù)采集頻率為50 Hz，數(shù)據(jù)通過RS232串口以文本形式轉(zhuǎn)存至計算機，通過MATLAB進行處理，如圖4所示。

圖4 靜止時濾波效果對比

旋耕機放置水平臺上，傾角傳感器底部分別安裝塑料、泡沫、海綿，將傳感器安裝在旋耕機較為平整的平面上，讀取傾角傳感器數(shù)據(jù)，將其誤差校準為0°，使旋耕機和傾角傳感器成為一體。再將調(diào)平支撐桿調(diào)至油缸實際長度加上其一半行程，將旋耕機調(diào)至水平，使之上下調(diào)平行程基本一致。

拖拉機啟動后，開啟旋耕刀，將調(diào)平油缸分別調(diào)至50 mm處14 s，分別采集底部墊有塑料、泡沫、海綿的卡爾曼濾波與未濾波傾角數(shù)據(jù)，通過MATLAB進行處理，如圖5所示。

圖5 卡爾曼濾波對比

試驗結(jié)果表明，當傾角傳感器靜止時，未濾波的數(shù)據(jù)波動較小，誤差較低，在±0.2°范圍以內(nèi)波動，而使用卡爾曼濾波的數(shù)據(jù)更加精確，最大波動不超過±0.05°。當開啟旋耕刀之后，機具開始頻繁抖動，底部墊有海綿的未濾波數(shù)據(jù)在±4°范圍波動，而墊有塑料或泡沫的未濾波數(shù)據(jù)波動范圍±6°，偶爾出現(xiàn)大跳動，最大波動超過±10°。墊有海綿卡爾曼濾波后數(shù)據(jù)波動在0.1°左右，最大波動不超過0.3°，相比墊有塑料、泡沫濾波后數(shù)據(jù)波動在0.35°左右，最大波動超過0.8°，有較為明顯提高。傾角數(shù)據(jù)波動過大極易對拖拉機作業(yè)過程中自動調(diào)平控制系統(tǒng)調(diào)平精度造成干擾，且調(diào)平過程中傳感器數(shù)據(jù)跳動過大致電機頻繁工作，容易燒毀電機，而底部墊有海綿卡爾曼濾波后的數(shù)據(jù)變得平滑，最大波動不超過0.2°能達到控制精度要求。如表1所示，傾角傳感器靜止時，未濾波數(shù)據(jù)最大誤差0.175°，誤差平均值0.07°，均方根誤差≤0.049°，濾波后數(shù)據(jù)最大誤差0.05°，誤差平均值0.01°，均方根誤差≤0.014°。當開啟旋耕刀后，墊有海綿的濾波后數(shù)據(jù)最大誤差0.3°，誤差平均值0.12°，均方根誤差≤0.023°，相比墊有塑料或泡沫穩(wěn)定性有一定提高，說明在傳感器底部墊有海綿能夠消除部分干擾和抖動的影響。

表1 靜態(tài)傾角測試結(jié)果

3.2 動態(tài)試驗

動態(tài)試驗采用融合卡爾曼濾波算法傾角傳感器的拖拉機進行連續(xù)過障試驗，行程為590 cm路面上180 cm和450 cm分別設置障礙，障礙物長22 cm、寬18 cm，分別為單邊履帶連續(xù)過障和雙邊履帶連續(xù)過障(由于過于危險，雙邊履帶20 cm只過1次障礙)，單邊履帶分為左右兩邊，其中S1和S2分別為過10 cm障礙物(車身傾斜約4°左右，機具傾斜3°左右)和過20 cm障礙物(車身傾斜約8°左右，機具傾斜7°左右)，每組試驗4次。采用串口助手實時采集濾波后的實時傾角，通過MATLAB進行數(shù)據(jù)處理。

表2為所示，4種不同條件動態(tài)試驗下的傾角數(shù)據(jù)結(jié)果。動態(tài)試驗過程中采集了傾角實時變化數(shù)據(jù)，圖6為S1和S2每種試驗中的一組傾角實時變化圖。

表2 單邊履帶動態(tài)傾角測試結(jié)果Table 2 Inclination angle of dynamic continuous obstacle crossing

由圖6與表2可知，在拖拉機行駛過程未遇障礙情況下，機具相比水平角度0°，最大誤差0.86°，誤差平均值0.264°，均方根誤差≤0.267°。當拖拉機單邊履帶遇到10 cm障礙物的情況下，機具相比傾斜角度3°，最大誤差0.62°，誤差平均值0.375°，均方根誤差≤0.163°。越過10 cm障礙物之后，車體產(chǎn)生沖擊造成機具震動，機具相比水平角度0°，最大誤差1°，機具在±0.5°范圍左右抖動。當拖拉機單邊履帶遇到20 cm障礙物的情況下，機具相比傾斜角度7°，最大誤差1.09°，誤差平均值0.587°，均方根誤差≤0.33°。越過20 cm障礙物之后，車身產(chǎn)生較大沖擊，機具因慣性反向傾斜及震動，機具相比水平角度0°，最大誤差超過2°，經(jīng)過2 s的調(diào)整，機具在±0.7°范圍左右抖動。當雙邊履帶同時遇到障礙物時，車身整體向后傾斜，對機具采集的傾斜角產(chǎn)生一定影響，最大誤差超過2°，越過障礙物之后產(chǎn)生劇烈震蕩，對機具的傾角采集造成較大影響。在跨越20 cm以下障礙物時，單邊履帶過障的障礙物高度越高，其機具傾角度采集的差誤越高。雙邊履帶過障的傾角度采集誤差明顯大于單邊履帶，因車體向后產(chǎn)生大幅度傾斜且過障產(chǎn)生過大震動與沖擊，導致較大誤差。

圖6 傾角的實時變化

4 結(jié)論

本研究設計了農(nóng)業(yè)機具自動調(diào)平控制系統(tǒng)，陀螺儀與加速度計結(jié)合卡爾曼濾波模型精確測量機具的實時傾斜角度，通過電機控制全液壓轉(zhuǎn)向機調(diào)節(jié)油缸的行程。靜態(tài)試驗中傾角傳感器靜止時，未濾波數(shù)據(jù)最大誤差0.175°，誤差平均值0.07°，均方根誤差≤0.049°，濾波后數(shù)據(jù)最大誤差0.05°，誤差平均值0.01°，均方根誤差≤0.014°。當開啟旋耕刀后，墊有海綿濾波后的數(shù)據(jù)最大誤差0.3°，誤差平均值0.12°，均方根誤差≤0.023°，相比墊有塑料或泡沫穩(wěn)定性有一定提高，說明在傳感器底部墊有海綿能夠消除部分干擾和抖動的影響。動態(tài)試驗表明，拖拉機行駛過程未遇障礙情況下，機具相比水平角度0°，最大誤差0.86°，誤差平均值0.264°，均方根誤差≤0.267°。當單邊履帶遇到10 cm障礙時，最大誤差0.62°，誤差平均值0.375°，均方根誤差≤0.163°。遇到20 cm障礙時，最大誤差1.09°，誤差平均值0.587°，均方根誤差≤0.33°。在跨越20 cm以下障礙物時，拖拉機單邊履帶遇到的障礙物越高，其誤差值越大。當單邊履帶越過障礙物時，會對車身產(chǎn)生沖擊與震蕩，對傾角度數(shù)據(jù)的采集造成一定影響。當雙邊履帶同時遇到障礙物時，車身整體向后傾斜，對機具采集的傾斜角產(chǎn)生一定影響，最大誤差超過2°，越過障礙物之后產(chǎn)生劇烈震蕩，對機具的傾角采集造成較大影響，雙邊履帶過障的傾角度采集誤差明顯大于單邊履帶。