吴帆， 蒋蘋， 胡文武， 罗亚辉， 金生

(湖南农业大学机电工程学院，湖南 长沙 410128)

近年来，随着传感器的精度不断提高，自动调平控制系统已经逐渐被应用于农业机械领域[1-4]。目前，国内市场上绝大部分耕整地[5]类农业机械为农业机具非自动调平的拖拉机，不能保证其机具在农田作业的时候保持实时水平状态。而平整的农田不仅可以很大程度上降低其用水率，并且能够提升土壤中肥料的利用率。因此，农业机具自动调平控制[6-8]在作物生产过程中显得尤为重要。胡炼等[9]采用倾角传感器和直线位移传感器与控制技术，通过电磁换向阀来控制机具的水平角度，平均绝对误差0.40°；彭凯等[10]设计了旋耕机三点悬挂[11]调平系统，通过复合数字滤波算法[12]对传感器信号进行处理，滤波后数据变得平滑，误差值在0.1°左右，能够有效地抗干扰及波动。丁为民等[13]将调平控制系统用于犁旋一体机上，实现了犁旋一体机的自动调平，相比较需手动调平的机具，耕深一致性和土地平整性有大幅度提高。这些自动调平控制系统主要都通过频繁控制电磁换向阀来控制油缸伸缩量，容易对液压系统造成损害。作者以1GDZ-150型履带拖拉机为平台，研制并加装了一套自动调平控制系统。采取单缸设定不动点调平策略，通过自主研制的倾角传感器实时检测机具的倾斜角度变化，将卡尔曼滤波后数据反馈回控制器，根据其数据采用电机驱动全液压转向机控制油缸的伸缩，以满足机具田间作业需求。

1 自动调平控制系统整体结构及工作原理

自动调平控制系统主要由拖拉机、控制系统、电机、驱动模块、液压驱动系统、执行机构、倾角传感器等组成。该系统整体如图1所示。田间作业时，农业机具上的传感器将实时采集的机具的水平度发送给主控制器，收到角度后，判断是否处于预定角度的范围。当角度超出设定值时，主控制器发信号给电机控制器，带动电机正反转来控制全液压

转向机进出油，控制其油缸的行程，从而保持农业机具的水平。机具调至预先设定范围以内，开始下个周期的调平，实现农业机具调平的闭环控制，保持机具水平度始终在预设范围之内。当倾斜的角度越大时，电机转速越快，调平油缸伸缩量越大，形成比例对应关系。

1.拖拉机；2.主控制器；3.电机控制器；4.电机；5.阀块及电磁阀；6.液压转向机；7.举升油缸；8.调平油缸；9.农业机具；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 自动调平控制系统
Fig.1 Automatic leveling control system

牵引式作业机具简化为调平机构示意图，如图2所示。A、C为化简后的调平油缸，B、D为调平支撑杆的固定端点。其中，AC为油缸处于50 mm行程初始位置，AC1为油缸缩短后长度，AC2为油缸伸长后长度，θ为牵引式作业机具相较地面的倾角。调平支撑杆两端球头固定，调平油缸伸缩相应行程带动牵引式作业机具调节相较于地面的倾角。

图2 调平机构示意图

油缸行程L±L0(mm)与牵引式作业机具相较地面的角度θ(°)的关系如下：

(1)

式中：CD为油缸固定端点至调平支撑杆固定端点的长度，cm；θ为∠C1DC或∠C2DC，°；L为调平油缸处于50 mm行程位置时长度，mm；L0为调平油缸伸缩行程长度，mm。

2 机具水平度检测系统设计

机具水平角度检测系统主要包括硬件和软件算法两部分，如图3所示。WT901C模块输出温度、陀螺仪、加速度计等实时数据至主控芯片STC15W4K48S4，再进行软件算法处理，包括初始化、数据平均值处理、卡尔曼滤波、角度估计，最后输出其角度值。

图3 机具水平度检测系统

2.1 硬件设计

水平度检测系统硬件主要由主控芯片STC15W4K48S4、WT901C模块、电源电路、485串口通信电路等组成。

2.1.1 核心处理器 根据设计要求，本文所选取的主控芯片为STC公司的STC15W4K48S4，该芯片的工作频率范围为5 MHz至28 MHz ，完全满足角度运算的处理要求；具有48字节的片Flash、4组串口通信端口、8通道高速AD、6通道高精度PWM、2通道CCP、SPI高速同步串行通信端口与低功耗设计等特点[14]。

2.1.2 倾角模块 倾角模块的精度是测量水平度的关键，本文选取的倾角模块为WT901C，最高输出数据速率可达200 Hz，加速度量程±16 g，稳定性0.01 g，对应分辨率6.1e-5g，角速度量程±2 000°·s-1，稳定性0.05°·s-1，对应分辨率7.6e-3°·s-1，角度量程±180°[15]。

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

2.2 软件算法

田间作业过程中，机体因结构原因震动剧烈、田间路面复杂多变以及传感器自身精度不高，极易对倾角传感器数据造成较大干扰。而卡尔曼滤波算法[16-17]能够在噪声污染的信号中，采取递推方式，滤除静态与动态过程中姿态测量系统的干扰信号，以获取更加精确的信号值。将离散系统的陀螺仪常值与倾角实际值的偏差b为状态向量，其过程方程与测量方程如下：

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

(2)

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

(3)

式中：X(K)为K时刻下的状态矩阵；U(K-1)为K-1时刻下对系统的控制量；Z(K)为K时刻下的测量值；W(K-1)为K-1时刻过程的噪声；V(K)为K时刻测量的噪声；A、B与H分别为系统参数值和测量系统的参数值。

其迭代计算公式如下：

验算其预测状态：

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

(4)

姿态测量系统K时刻对应的误差协方差：

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为测量噪声协方差，根据经验确定为0.5。

修正其预测的偏差：

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

(7)

更新误差协方差矩阵：

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

(8)

式中：I为单位矩阵。

3 试验与分析

试验于2019-06-30在湖南农业大学车辆实训中心进行，试验车辆为1GDZ-150型履带式拖拉机，试验机具为旋耕机，采用自主研制倾角传感器测量机具的倾斜角，通过自制遥控器(无线模块为RFM96)与自制控制器进行无线通讯[18-19]。

3.1 静态试验

将倾角传感器放置水平面上静置，分别采集卡尔曼滤波和未卡尔曼滤波的倾角数据，数据采集频率为50 Hz，数据通过RS232串口以文本形式转存至计算机，通过MATLAB进行处理，如图4所示。

图4 静止时滤波效果对比

旋耕机放置水平台上，倾角传感器底部分别安装塑料、泡沫、海绵，将传感器安装在旋耕机较为平整的平面上，读取倾角传感器数据，将其误差校准为0°，使旋耕机和倾角传感器成为一体。再将调平支撑杆调至油缸实际长度加上其一半行程，将旋耕机调至水平，使之上下调平行程基本一致。

拖拉机启动后，开启旋耕刀，将调平油缸分别调至50 mm处14 s，分别采集底部垫有塑料、泡沫、海绵的卡尔曼滤波与未滤波倾角数据，通过MATLAB进行处理，如图5所示。

图5 卡尔曼滤波对比

试验结果表明，当倾角传感器静止时，未滤波的数据波动较小，误差较低，在±0.2°范围以内波动，而使用卡尔曼滤波的数据更加精确，最大波动不超过±0.05°。当开启旋耕刀之后，机具开始频繁抖动，底部垫有海绵的未滤波数据在±4°范围波动，而垫有塑料或泡沫的未滤波数据波动范围±6°，偶尔出现大跳动，最大波动超过±10°。垫有海绵卡尔曼滤波后数据波动在0.1°左右，最大波动不超过0.3°，相比垫有塑料、泡沫滤波后数据波动在0.35°左右，最大波动超过0.8°，有较为明显提高。倾角数据波动过大极易对拖拉机作业过程中自动调平控制系统调平精度造成干扰，且调平过程中传感器数据跳动过大致电机频繁工作，容易烧毁电机，而底部垫有海绵卡尔曼滤波后的数据变得平滑，最大波动不超过0.2°能达到控制精度要求。如表1所示，倾角传感器静止时，未滤波数据最大误差0.175°，误差平均值0.07°，均方根误差≤0.049°，滤波后数据最大误差0.05°，误差平均值0.01°，均方根误差≤0.014°。当开启旋耕刀后，垫有海绵的滤波后数据最大误差0.3°，误差平均值0.12°，均方根误差≤0.023°，相比垫有塑料或泡沫稳定性有一定提高，说明在传感器底部垫有海绵能够消除部分干扰和抖动的影响。

表1 静态倾角测试结果

3.2 动态试验

动态试验采用融合卡尔曼滤波算法倾角传感器的拖拉机进行连续过障试验，行程为590 cm路面上180 cm和450 cm分别设置障碍，障碍物长22 cm、宽18 cm，分别为单边履带连续过障和双边履带连续过障(由于过于危险，双边履带20 cm只过1次障碍)，单边履带分为左右两边，其中S1和S2分别为过10 cm障碍物(车身倾斜约4°左右，机具倾斜3°左右)和过20 cm障碍物(车身倾斜约8°左右，机具倾斜7°左右)，每组试验4次。采用串口助手实时采集滤波后的实时倾角，通过MATLAB进行数据处理。

表2为所示，4种不同条件动态试验下的倾角数据结果。动态试验过程中采集了倾角实时变化数据，图6为S1和S2每种试验中的一组倾角实时变化图。

表2 单边履带动态倾角测试结果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障碍物之后，车体产生冲击造成机具震动，机具相比水平角度0°，最大误差1°，机具在±0.5°范围左右抖动。当拖拉机单边履带遇到20 cm障碍物的情况下，机具相比倾斜角度7°，最大误差1.09°，误差平均值0.587°，均方根误差≤0.33°。越过20 cm障碍物之后，车身产生较大冲击，机具因惯性反向倾斜及震动，机具相比水平角度0°，最大误差超过2°，经过2 s的调整，机具在±0.7°范围左右抖动。当双边履带同时遇到障碍物时，车身整体向后倾斜，对机具采集的倾斜角产生一定影响，最大误差超过2°，越过障碍物之后产生剧烈震荡，对机具的倾角采集造成较大影响。在跨越20 cm以下障碍物时，单边履带过障的障碍物高度越高，其机具倾角度采集的差误越高。双边履带过障的倾角度采集误差明显大于单边履带，因车体向后产生大幅度倾斜且过障产生过大震动与冲击，导致较大误差。

图6 倾角的实时变化

4 结论

本研究设计了农业机具自动调平控制系统，陀螺仪与加速度计结合卡尔曼滤波模型精确测量机具的实时倾斜角度，通过电机控制全液压转向机调节油缸的行程。静态试验中倾角传感器静止时，未滤波数据最大误差0.175°，误差平均值0.07°，均方根误差≤0.049°，滤波后数据最大误差0.05°，误差平均值0.01°，均方根误差≤0.014°。当开启旋耕刀后，垫有海绵滤波后的数据最大误差0.3°，误差平均值0.12°，均方根误差≤0.023°，相比垫有塑料或泡沫稳定性有一定提高，说明在传感器底部垫有海绵能够消除部分干扰和抖动的影响。动态试验表明，拖拉机行驶过程未遇障碍情况下，机具相比水平角度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以下障碍物时，拖拉机单边履带遇到的障碍物越高，其误差值越大。当单边履带越过障碍物时，会对车身产生冲击与震荡，对倾角度数据的采集造成一定影响。当双边履带同时遇到障碍物时，车身整体向后倾斜，对机具采集的倾斜角产生一定影响，最大误差超过2°，越过障碍物之后产生剧烈震荡，对机具的倾角采集造成较大影响，双边履带过障的倾角度采集误差明显大于单边履带。