本发明属于农业装备自动控制领域，具体涉及悬挂式农具的水平控制。
背景技术
悬挂式农具利用拖拉机提供动力或支撑平台，在农业生产中被大量使用，例如悬挂式旋耕机、耙平机、根茎收获机等。在南方水田种植地区，犁底层一般是不平坦的，并且存在一些排水沟，当拖拉机的一侧轮胎落入排水沟或低洼点时，悬挂式农具会随之倾斜，对耙平机而言，就失去了耙平的目的，对旋耕机或根茎收获机而言，由于单侧入土太深，不仅影响了作业的质量和效率，而且使农具的内部构件产生严重的受力不均现象，例如根茎收获机的较深入土侧连杆机构中的拉杆受力会严重超过设计载荷，极易使该拉杆断裂而中断作业，因此有必要为悬挂式农具配备水平自动控制装置而使它时刻处于水平姿态。
悬挂式农具水平自动控制技术已经有所研究及发明，例如胡炼在《农机具自动水平控制系统设计与试验》(《农业工程学报》,2015，vol.31(8):(15～20))利用陀螺仪与位移传感器获取信息，通过控制电磁换向阀实现农机具水平控制。再如魏新华在《喷杆式喷雾机喷杆高度及平衡在线调控系统》(农业机械学报，2015，vol.46(8):(66～71))利用超声波测距，通过plc控制比例换向阀组实现喷杆高度及平衡的调节。又例如陈君梅在《水田激光平地机非线性水平控制系统》(《农业机械学报》，2014，vol.45(7)：(79～84))利用非线性pid控制算法实现机具在非目标位置时的快速反应，同时能减少超调和振荡，具有快速性和稳定性。上述论文的被控目标自身无振动，姿态测量系统受噪声或机械振动的干扰少，测量姿态的传感器直接安装在农具上，而根茎收获机是依据鄂式曲柄连杆机构的运动机理，收获机以5～8hz的频率挖掘土壤中的根茎，其振幅为10～20mm，这种强烈的振动使得直接进行机具的姿态测量变得困难。发明专利《前置式拖拉机水平自动控制装置》(中国专利，201510843200.8)提出了一种间接测量农具姿态的方法，它把倾角传感器安装在拖拉机的机体上，再采用角位移传感器采集倾斜提升油缸的位移量，上位机结合这两个传感器估算农具的倾斜而发出信号控制提升油缸动作而使农具保持水平。然而受性价比因素考虑，现有的倾角传感器一般是基于mems陀螺仪、依靠一些姿态估计算法获得倾角，其横滚角的静态测量精度可达0.01°，当拖拉机满油门作业时，其自身的小幅机械振动仍不可避免，其测量精度可能降低到0.5°，极大地影响水平控制的效果。
采用rtk-gps的卫星导航系统已经大量地在拖拉机上获得推广应用，伴随我国的北斗导航系统的高速发展，装备高精度卫星定位系统逐渐将成为农机发展的趋势之一。近年来利用双天线构成的卫星定位接收机在驾考领域获得大量推广应用，它在厘米级定位的基础上，能提供精确的航向角和俯仰角，外界的机械振动对这两个角度值的影响很小。如果把两只天线一左一右分别摆放在拖拉机驾驶室顶部，卫星定位接收机就能够实时输出拖拉机的横滚角，其精确度达到0.08°/2m，略低于倾角传感器的静态输出精度，但此横滚角不受机械振动的影响，因此，使用倾角传感器与卫星导航系统中的卫星定位接收机两种传感器，共同测量拖拉机车身的横滚角，通过多传感器融合算法，将提高悬挂式农具的姿态估计的精度、水平自动控制的效果。
技术实现要素：
为了解决悬挂式农具的水平自动控制装置中倾角传感器受拖拉机自身的机械振动而降低l测量精度的问题，本发明提供一种融合倾角传感器和卫星定位接收机的悬挂式农具水平自动控制装置。
本发明解决其技术问题所采用的技术方案是：悬挂式农具水平自动控制装置由上位机、摇杆手柄、ecu、卫星定位接收机、卫星天线、倾角传感器、拉绳位移传感器、活塞式钢丝拉线、电磁阀和倾斜提升油缸组成，卫星定位接收机以oem718d板卡为核心构建，倾角传感器选择mems类型的wt61板卡。上位机固定在驾驶员座位的右前方、方便驾驶员伸手操作的位置，摇杆手柄的底座固定在驾驶员座位的右方、方便驾驶员伸手操作摇杆的位置，卫星定位接收机与ecu叠放，固定在驾驶员座位左侧的地板上，2只卫星天线固定在驾驶室的天线支架的左右两侧，且与车身纵轴线成90度，电磁阀固定在拖拉机的传动箱后上部位的阀支架上，倾斜提升油缸替代拖拉机左侧的悬挂提升杆，拉绳位移传感器固定在阀支架的下方，该传感器的拉线与活塞式钢丝拉线的上端出线固结，活塞式钢丝拉线的上部节头固定在阀支架的侧面，它的下部活塞节与倾斜提升油缸的缸体平行，活塞节的上部节头与倾斜提升油缸的缸体铰接，活塞节的下部节头与倾斜提升油缸的伸缩杆的头部的横凸出支架铰接，活塞节与倾斜提升油缸同伸同缩，这样可以把倾斜提升油缸的位移通过活塞式钢丝拉线传递到拉绳位移传感器。
上位机的can端口通过can总线与ecu的总线板卡的can端口电连接，总线板卡通过总线插座与gpsgyro板卡和电磁阀板卡电连接。gpsgyro板卡的com2口与卫星定位接收机的com1口电连接，电磁阀板卡的an0端口与拉绳位移传感器电连接，电磁阀板卡的out端口与电磁阀的电磁线圈电连接。
ecu由机箱、航空插座、防水接头、防水盒、总线板卡、电磁阀板卡、gpsgyro板卡、接线端子排、24v电源、硅胶减震圈组成。倾角传感器wt61板卡是直接焊接在gpsgyro板卡上的，总线板卡、电磁阀板卡和gpsgyro板卡全部密封在防水盒之内，总线板卡的一只外接端子与防水盒外部的接线端子排和can总线电连接，总线板卡上平行排列的4只插座，为其他板卡供电和联通can总线。接线端子排与机箱的12v端子电连接，再与24v电源的输入端、总线板卡的外接端子电连接，24v电源的输出端与机箱的24v端子电连接。防水盒与机箱的底板之间由硅胶减震圈间隔，使得wt61板卡不再受拖拉机的振动而影响横滚角的测量精度，机箱与拖拉机之间通过螺丝固定。
gpsgyro板卡由pic18f25k80、wt61板卡、mcp2551电路和max232电路组成。pic18f25k80具备两只异步通信端口，第一只异步通信端口的tx1和rx1与wt61板卡的异步通信端口rx和tx电连接，采集横滚角数据；第二只异步通信端口的tx2和rx2通过max232电路构成com2端口，它与卫星定位接收机的rs232串口电连接，也采集横滚角数据。
电磁阀板卡由mcp2551电路、mcp6242电路、pic18f25k80、aod4184电路和lvt817电路组成。拉绳位移传感器的模拟量输出端、mcp6242电路的输入端、mcp6242电路放大器输出端和pic18f25k80的ra0引脚依次电连接。摇杆手柄的信号输出端、lvt817电路的光耦阴极端、lvt817电路的光耦射极端和pic18f25k80的rb0、rb1引脚依次电连接。pic18f25k80的两只pwm引脚分别与两路aod4184电路的栅极输入端、aod4184电路的漏极端和电磁阀的升、降电磁线圈的导线依次电连接。pic18f25k80的canrx、cantx引脚与mcp2551电路电连接，使得电磁阀板卡可接收上位机的指令去控制倾斜提升油缸，也可采集拉绳位移传感器的模拟量，发送给上位机。
gpsgyro板卡采集两种传感器的横滚角。ecu上电后，pic18f25k80的ra、rb、rc端口、两个uart异步串口通信、can通信和定时器依次进行寄存器的初始化，构建软元件时间继电器t0，开启定时器和异步串口通信中断功能。程序按照循环扫描的方式运行。当pic18f25k80的rx1收完1帧数据时，表明收到了wt61板卡的数据，则提取出绕y轴的角速率和横滚角，同时启动未开启的的时间继电器t0；当pic18f25k80的rx2收完gpgga的数据帧时，提取出卫星定位接收机的定位状态的字符，同时关闭时间继电器t0；当接收完heading帧数据时，提取出横滚角，pic18f25k80通过mcp2551电路把这些数据发送给上位机。如果时间继电器t0的常开触点闭合，说明卫星定位接收机没有输出数据，pic18f25k80通过mcp2551电路只把wt61板卡的数据发送给上位机，同时关闭时间继电器t0。
电磁阀板卡对悬挂式农具进行直接的测量与控制。ecu上电后，pic18f25k80的ra、rb、rc端口、ad变换、pwm输出、can通信和定时器依次进行寄存器的初始化，构建软元件时间继电器t0和t1，开启定时器和can通信中断功能。程序按照循环扫描的方式运行。当pic18f25k80的rb0引脚发生电平由低跳高的变化时，表明摇杆手柄发生了压下的动作，启动pwm1和t0，接通电磁阀左侧的线圈，进而控制倾斜提升油缸伸出动作，使悬挂式农具的左端下降；当pic18f25k80的rb1引脚发生电平由低跳高的变化时，表明摇杆手柄发生了抬升的动作，启动pwm2和t1，接通电磁阀右侧的线圈，进而控制倾斜提升油缸缩回动作，使悬挂式农具的左端上升；当pic18f25k80发生can中断时，程序对收到的指令进行判别，如果是下降悬挂式农具的指令，则从指令中取出占空比设置值和定时时长，修改pwm1的寄存器参数，启动pwm1；如果是抬升悬挂式农具的指令，则从指令中取出占空比设置值和定时时长，修改pwm2的寄存器参数，启动pwm2；如果是采集拉绳位移传感器的指令，则启动ad转换，把转换后的数字量通过mcp2551电路发送给上位机；当t0的常开触点闭合，则关闭pwm1和t0，使悬挂式农具停止运动；当t1的常开触点闭合，则关闭pwm2和t1，使悬挂式农具停止运动。
上位机对悬挂式农具进行总控制。系统上电之后，系统上电之后，读入水平控制的kp、ki、和kd参数，显示机器的主界面，然后启动can通信线程、控制线程。can通信线程一直处于等待接收数据状态，如果有卫星定位接收机的数据，则解析出定位状态和横滚角；如果有wt61板卡的数据，则解析出绕y轴的角速率和横滚角，然后通知控制线程；如果有拉绳位移传感器的数据，则需计算出在此数值下造成的悬挂式农具的高低差1，然后通知控制线程。在控制线程中，一旦获知横滚角已经采集到，立即发送采集拉绳位移传感器的指令给电磁阀板卡，并由wt61板卡产生的绕y轴的角速率、横滚角和卫星定位接收机产生的横滚角构成观测量，由车身的绕y轴的角速率和横滚角构成状态量，进行kalman滤波器的递推计算，状态方程和观测方程如下：
xk+1＝φkxk+wk(1)
zk＝hkxk+vk(2)
其中，状态变量为
xk＝[ωk,γk]t(3)
状态转移矩阵是
观察量和观测矩阵是
通过先期的实验，计算出状态变量的协方差矩阵qk和观测量的协方差矩阵rk。在控制线程的初始化阶段设置这些参数，并在实时运行时，依据卫星定位接收机的状态——固定解、浮点解、单点解，更新协方差矩阵rk的卫星定位接收机的横滚角的方差值。因此，通过kalman滤波器的运算，可以实时估计出当前横滚角，计算出此横滚角值所产生的悬挂式农具左右两端的高低差2。等电磁阀板卡发送回拉绳位移传感器的位移量数据和高低差1的数据时，控制线程综合这两个高低差的数据，产生悬挂式农具的左右两端的高低差。如果此高低差的绝对值不超出1.0cm，则不需要控制，不发送控制指令。如果此高低差的绝对值超过6.0cm，则控制指令中pwm的占空比直接设定为99％，pwm波的持续时间为200ms；如果高低差的绝对值不在上述范围内，则运行pid控制算法，产生pwm波的持续时间和升降控制方向，pwm的占空比设定为70％，控制线程把这些参数发送给电磁阀发板卡，由它控制电磁阀动作，对悬挂式农具进行水平自动控制。
本发明的有益效果是，即使拖拉机作业时车身发送较为严重的机械振动，水平自动控制装置对车身的横滚姿态和悬挂农具左右两端的高低差的估计更加可靠、精确，悬挂农具的保持水平姿态控制能力更强。
附图说明
下面结合附图和实施例对本发明进一步说明。
图1是本发明的在轮式拖拉机上应用的一个实施例的正视图。
图2是该实施例的俯视图。
图3是本发明的电子部件的电连接原理图。
图4是ecu的结构的一个实施例。
图5是gpsgyro板卡的电路接线的一个实施例。
图6是电磁阀板卡的电路接线的一个实施例。
图7是gpsgyro板卡的软件流程图。
图8是电磁阀板卡的软件流程图。
图9是上位机的软件流程图。
图中，1.卫星天线，2.天线支架，3.拖拉机，4.上位机，5.摇杆手柄，6.电磁阀，7.阀支架，8.拉绳位移传感器，9.活塞式钢丝拉线，10.倾斜提升油缸，11.卫星定位接收机，12.ecu，13.总线板卡，14.电磁阀板卡，15.gpsgyro板卡，16.机箱，17.航空插座，18.防水接头，19.防水盒，20.wt61板卡，21.接线端子排，22.24v电源，23.硅胶减震圈，24.mcp2551电路，25.pic18f25k80，26.max232电路，27.mcp2551电路，28.mcp6242电路29.pic18f25k80，30.aod4184电路，31.lvt817电路
在图1和图2的实施例中，上位机(4)固定在驾驶员座位的右前方、方便驾驶员伸手操作的位置，摇杆手柄(5)的底座固定在驾驶员座位的右方、方便驾驶员伸手操作摇杆的位置，卫星定位接收机(11)与ecu(12)叠放，固定在驾驶员座位左侧的地板上，2只卫星天线(1)固定在驾驶室的天线支架(2)的左右两侧，且与车身纵轴线成90度，电磁阀(6)固定在拖拉机(3)的传动箱后上部位的阀支架(7)上，倾斜提升油缸(10)替代拖拉机(3)左侧的悬挂提升杆，拉绳位移传感器(8)固定在阀支架(7)的下方，该传感器(10)的拉线与活塞式钢丝拉线(9)的上端出线固结，活塞式钢丝拉线(9)的上部节头固定在阀支架(7)的侧面，它(5)的下部活塞节与倾斜提升油缸(10)的缸体平行，活塞节的上部节头与倾斜提升油缸(10)的缸体铰接，活塞节的下部节头与倾斜提升油缸(10)的伸缩杆的头部的横凸出支架铰接，活塞节与倾斜提升油缸(10)同伸同缩，这样可以把倾斜提升油缸(10)的位移通过活塞式钢丝拉线(9)传递到拉绳位移传感器(8)。
在图3中，上位机(4)的can端口通过can总线与ecu(12)的总线板卡(13)的can端口电连接，总线板卡(13)通过总线插座与gpsgyro板卡(15)和电磁阀板卡(14)电连接。gpsgyro板卡(15)的com2口与卫星定位接收机(11)的com1口电连接，电磁阀板卡(14)的an0端口与拉绳位移传感器(8)电连接，电磁阀板卡(14)的out端口与电磁阀(6)的电磁线圈电连接。
图4是ecu的一个实施例，ecu由机箱(16)、航空插座(17)、防水接头(18)、防水盒(19)、总线板卡(13)、电磁阀板卡(14)、gpsgyro板卡(15)、接线端子排(21)、24v电源(22)、硅胶减震圈(23)组成。倾角传感器wt61板卡(20)是直接焊接在gpsgyro板卡(15)上的，总线板卡(13)、电磁阀板卡(14)和gpsgyro板卡(15)全部密封在防水盒(19)之内，总线板卡(13)的一只外接端子与防水盒(19)外部的接线端子排(21)和can总线电连接，总线板卡(13)上平行排列的4只插座，为其他板卡(14，15)供电和联通can总线。接线端子排(21)与机箱(16)的12v端子电连接，再与24v电源(22)的输入端、总线板卡(13)的外接端子电连接，24v电源(22)的输出端与机箱(16)的24v端子电连接。防水盒(19)与机箱(16)的底板之间由硅胶减震圈(23)间隔，使得wt61板卡(20)不再受拖拉机(3)的振动而影响横滚角的测量精度，机箱(16)与拖拉机(3)之间通过螺丝固定。
在图5的实施例中，gpsgyro板卡(15)由pic18f25k80(22)、wt61板卡(20)、mcp2551电路(24)和max232电路(26)组成。pic18f25k80(22)具备两只异步通信端口，第一只异步通信端口的tx1和rx1与wt61板卡(20)的异步通信端口rx和tx电连接，采集横滚角数据；第二只异步通信端口的tx2和rx2通过max232电路(26)构成com2端口，它与卫星定位接收机的rs232串口电连接，也采集横滚角数据。
在图6的实施例中，电磁阀板卡(14)由mcp2551电路(27)、mcp6242电路(28)、pic18f25k80(29)、aod4184电路(30)和lvt817电路(31)组成。拉绳位移传感器(8)的模拟量输出端、mcp6242电路(28)的输入端、mcp6242电路(28)放大器输出端和pic18f25k80(29)的ra0引脚依次电连接。摇杆手柄(5)的信号输出端、lvt817电路(31)的光耦阴极端、lvt817电路(31)的光耦射极端和pic18f25k80(29)的rb0、rb1引脚依次电连接。pic18f25k80(29)的两只pwm引脚分别与两路aod4184电路(30)的栅极输入端、aod4184电路(30)的漏极端和电磁阀(6)的升、降电磁线圈的导线依次电连接。pic18f25k80(29)的canrx、cantx引脚与mcp2551电路(20)电连接，使得电磁阀板卡(14)可接收上位机(4)的指令去控制倾斜提升油缸(10)，也可采集拉绳位移传感器(8)的模拟量，发送给上位机(4)。
在图7的gpsgyro板卡(15)软件流程图中，ecu(12)上电后，pic18f25k80(22)的ra、rb、rc端口、两个uart异步串口通信、can通信和定时器依次进行寄存器的初始化，构建软元件时间继电器t0，开启定时器和异步串口通信中断功能。程序按照循环扫描的方式运行。当pic18f25k80(22)的rx1收完1帧数据时，表明收到了wt61板卡(20)的数据，则提取出绕y轴的角速率和横滚角，同时启动未开启的的时间继电器t0；当pic18f25k80(22)的rx2收完gpgga的数据帧时，提取出卫星定位接收机(11)的定位状态的字符，同时关闭时间继电器t0；当接收完heading帧数据时，提取出横滚角，pic18f25k80(22)通过mcp2551电路(24)把这些数据发送给上位机(4)。如果时间继电器t0的常开触点闭合，说明卫星定位接收机(11)没有输出数据，pic18f25k80(22)通过mcp2551电路(24)只把wt61板卡的数据发送给上位机(4)，同时关闭时间继电器t0。
在图8的gpsgyro板卡(14)软件流程图中，ecu(12)上电后，pic18f25k80(29)的ra、rb、rc端口、ad变换、pwm输出、can通信和定时器依次进行寄存器的初始化，构建软元件时间继电器t0和t1，开启定时器和can通信中断功能。程序按照循环扫描的方式运行。当pic18f25k80(29)的rb0引脚发生电平由低跳高的变化时，表明摇杆手柄(5)发生了压下的动作，启动pwm1和t0，接通电磁阀(6)左侧的线圈，进而控制倾斜提升油缸(10)伸出动作，使悬挂式农具的左端下降；当pic18f25k80(29)的rb1引脚发生电平由低跳高的变化时，表明摇杆手柄(5)发生了抬升的动作，启动pwm2和t1，接通电磁阀(6)右侧的线圈，进而控制倾斜提升油缸(10)缩回动作，使悬挂式农具的左端上升；当pic18f25k80(29)发生can中断时，程序对收到的指令进行判别，如果是下降悬挂式农具的指令，则从指令中取出占空比设置值和定时时长，修改pwm1的寄存器参数，启动pwm1；如果是抬升悬挂式农具的指令，则从指令中取出占空比设置值和定时时长，修改pwm2的寄存器参数，启动pwm2；如果是采集拉绳位移传感器(8)的指令，则启动ad转换，把转换后的数字量通过mcp2551电路(27)发送给上位机(4)；当t0的常开触点闭合，则关闭pwm1和t0，使悬挂式农具停止运动；当t1的常开触点闭合，则关闭pwm2和t1，使悬挂式农具停止运动。
在图9的上位机(4)软件流程图中，系统上电之后，读入水平控制的kp、ki、和kd参数，显示机器的主界面，然后启动can通信线程、控制线程。can通信线程一直处于等待接收数据状态，如果有卫星定位接收机(11)的数据，则解析出定位状态和横滚角；如果有wt61板卡(20)的数据，则解析出绕y轴的角速率和横滚角，然后通知控制线程；如果有拉绳位移传感器(8)的数据，则需计算出在此数值下造成的悬挂式农具的高低差1，然后通知控制线程。在控制线程中，一旦获知横滚角已经采集到，立即发送采集拉绳位移传感器(8)的指令给电磁阀板卡(14)，并由wt61板卡(20)产生的绕y轴的角速率、横滚角和卫星定位接收机(11)产生的横滚角构成观测量，依据卫星定位接收机(11)的定位状态更新协方差矩阵rk，由车身的绕y轴的角速率和横滚角构成状态量，进行kalman滤波器的递推计算，获得当前横滚角的估计值，计算出此横滚角值所产生的悬挂式农具左右两端的高低差2。等电磁阀板卡(14)发送回拉绳位移传感器(8)的位移量数据和高低差1的数据时，控制线程综合这两个高低差的数据，产生悬挂式农具的左右两端的高低差。如果此高低差的绝对值不超出1.0cm，则不需要控制，不发送控制指令。如果此高低差的绝对值超过6.0cm，则控制指令中pwm的占空比直接设定为99％，pwm波的持续时间为200ms；如果高低差的绝对值不在上述范围内，则运行pid控制算法，产生pwm波的持续时间和升降控制方向，pwm的占空比设定为70％，控制线程把这些参数发送给电磁阀发板卡(14)，由它控制电磁阀(6)动作，对悬挂式农具进行水平自动控制。