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

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

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

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

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

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

1.拖拉機(jī)；2.主控制器；3.電機(jī)控制器；4.電機(jī)；5.閥塊及電磁閥；6.液壓轉(zhuǎn)向機(jī)；7.舉升油缸；8.調(diào)平油缸；9.農(nóng)業(yè)機(jī)具；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è)機(jī)具簡化為調(diào)平機(jī)構(gòu)示意圖，如圖2所示。A、C為化簡后的調(diào)平油缸，B、D為調(diào)平支撐桿的固定端點。其中，AC為油缸處于50 mm行程初始位置，AC1為油缸縮短后長度，AC2為油缸伸長后長度，θ為牽引式作業(yè)機(jī)具相較地面的傾角。調(diào)平支撐桿兩端球頭固定，調(diào)平油缸伸縮相應(yīng)行程帶動牽引式作業(yè)機(jī)具調(diào)節(jié)相較于地面的傾角。

圖2 調(diào)平機(jī)構(gòu)示意圖

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

(1)

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

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

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

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

2.1 硬件設(shè)計

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

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

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

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

2.2 軟件算法

田間作業(yè)過程中，機(jī)體因結(jié)構(gòu)原因震動劇烈、田間路面復(fù)雜多變以及傳感器自身精度不高，極易對傾角傳感器數(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ù)值。

其迭代計算公式如下：

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

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

(4)

姿態(tài)測量系統(tǒng)K時刻對應(yīng)的誤差協(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。

修正其預(yù)測的偏差：

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è)大學(xué)車輛實訓(xùn)中心進(jìn)行，試驗車輛為1GDZ-150型履帶式拖拉機(jī)，試驗機(jī)具為旋耕機(jī)，采用自主研制傾角傳感器測量機(jī)具的傾斜角，通過自制遙控器(無線模塊為RFM96)與自制控制器進(jìn)行無線通訊[18-19]。

3.1 靜態(tài)試驗

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

圖4 靜止時濾波效果對比

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

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

圖5 卡爾曼濾波對比

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

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

3.2 動態(tài)試驗

動態(tài)試驗采用融合卡爾曼濾波算法傾角傳感器的拖拉機(jī)進(jìn)行連續(xù)過障試驗，行程為590 cm路面上180 cm和450 cm分別設(shè)置障礙，障礙物長22 cm、寬18 cm，分別為單邊履帶連續(xù)過障和雙邊履帶連續(xù)過障(由于過于危險，雙邊履帶20 cm只過1次障礙)，單邊履帶分為左右兩邊，其中S1和S2分別為過10 cm障礙物(車身傾斜約4°左右，機(jī)具傾斜3°左右)和過20 cm障礙物(車身傾斜約8°左右，機(jī)具傾斜7°左右)，每組試驗4次。采用串口助手實時采集濾波后的實時傾角，通過MATLAB進(jìn)行數(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可知，在拖拉機(jī)行駛過程未遇障礙情況下，機(jī)具相比水平角度0°，最大誤差0.86°，誤差平均值0.264°，均方根誤差≤0.267°。當(dāng)拖拉機(jī)單邊履帶遇到10 cm障礙物的情況下，機(jī)具相比傾斜角度3°，最大誤差0.62°，誤差平均值0.375°，均方根誤差≤0.163°。越過10 cm障礙物之后，車體產(chǎn)生沖擊造成機(jī)具震動，機(jī)具相比水平角度0°，最大誤差1°，機(jī)具在±0.5°范圍左右抖動。當(dāng)拖拉機(jī)單邊履帶遇到20 cm障礙物的情況下，機(jī)具相比傾斜角度7°，最大誤差1.09°，誤差平均值0.587°，均方根誤差≤0.33°。越過20 cm障礙物之后，車身產(chǎn)生較大沖擊，機(jī)具因慣性反向傾斜及震動，機(jī)具相比水平角度0°，最大誤差超過2°，經(jīng)過2 s的調(diào)整，機(jī)具在±0.7°范圍左右抖動。當(dāng)雙邊履帶同時遇到障礙物時，車身整體向后傾斜，對機(jī)具采集的傾斜角產(chǎn)生一定影響，最大誤差超過2°，越過障礙物之后產(chǎn)生劇烈震蕩，對機(jī)具的傾角采集造成較大影響。在跨越20 cm以下障礙物時，單邊履帶過障的障礙物高度越高，其機(jī)具傾角度采集的差誤越高。雙邊履帶過障的傾角度采集誤差明顯大于單邊履帶，因車體向后產(chǎn)生大幅度傾斜且過障產(chǎn)生過大震動與沖擊，導(dǎo)致較大誤差。

圖6 傾角的實時變化

4 結(jié)論

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