专利名称：平地机的水平控制系统及其控制方法
技术领域：
本发明涉及平地机的水平控制系统及其控制方法。
背景技术：
精细平整的水田可以保证农田水层深浅一致，节省农业用水；可以提高农 药、化肥和除草剂的使用效率，减少农药、化肥和除草剂的使用量，进而减少 生产成本和对环境的污染；可以增加农作物产量，促进农业可持续发展。世界
各国的经验证明，激光平地技术是农业生产中一项重要的节本增效措施。 由于水田犁底层高低不平，水田激光机在水田平地作业中，不但要保证平
地妒高低可调，而且要保证平地4产水平可控，使平地伊在工作过程中始终保持
在与激光束平面平行的一个平面上。
发明内容
针对现有技术的缺点，本发明的目的是提供一种能准确计算出平地伊水平 倾角的平地机的水平控制系统及其控制方法。
为实现上述目的，本发明的技术方案为 一种平地机的水平控制系统，包 括平地伊、控制平地4产水平位置的水平调节液压系统及控制水平调节液压系统 工作的水平控制系统，平地铲上设有感应平地铲水平倾角信号及倾角变化率信 号的传感器系统，该传感器系统将倾^H言息传输至水平控制器进4亍处理。
该传感器系统包括用于感应水平倾角信号的加速度传感器及感应倾角变化 率信号的角速度传感器。
该加速度传感器安装于平地伊转轴上。
该角速度传感器为陀螺仪传感器。
该水平控制系统包括处理感应信号的微控制器及将微控制器输出信号驱动 放大的驱动电路。该水平调节液压系统包括连接至平地4产的双作用油缸，双作用油缸通过电 磁阀连接至水平控制系统。
另外本发明还提供了 一种平地机的水平控制方法，其包括以下步骤
a、 传感器系统感应平地伊的水平倾角信号及倾角变化率信号；
b、 水平控制系统采集感应信号，并对感应信号进行融合处理，发出脉沖宽 度调制信号至水平调节B系统；
c 、水平调节液压系统根据发出的信号来控制平地伊的水平度。 步骤a中，传感器系统包括加速度传感器及角速度传感器，加速度传感器 用于感应水平倾角信号，角速度传感器用于感应倾角变化率信号，步骤c中， 该水平调节液压系统包括双作用油缸，水平控制系统将信号发送至一电磁阀， 由电^兹阀来控制双作用油缸的伸缩度，并通过双作用油缸作用于平地4产来调节 平地铲的水平度。
在步骤b中，对该水平倾角信号及倾角变化率信号的处理还包括以下步骤 (1)检查水平倾角信号及倾角变化率信号是否改变，是则执行步骤(2 )，否 则结束；
(2 )计算两种传感器测得的水平倾角信号COacc及倾角变化率信号(Ogyr。，并将 C0acx及(Ogyr。之差绝对值与设定阀值相比，若小于阀值，则按水平倾角信号CO acc 计算平地4产倾角，若大于阀值，则按设定倾角加上COgyr。xT，求出平地4产倾角，
其中T为樣吏控制器的工作周期；
(3) 根据平地l产的倾角变化刷新脉沖宽度调制信号的占空比；
(4) 驱动电,收脉沖宽度调制信号，进行放大并输出至电磁阀。 在步骤(1)中，微控制器采集水平倾角信号及倾角变化率信号的周期为IOT。 与现有技术相比，本发明采用加速度传感器及角速度传感器来感应平地铲的
水平倾角信号，并采用微控制器对感应信号进行融合计算得出倾角，最后通过 水平调节液压系统调节平地伊，使其始终保持水平。由于采用了加速度传感器
及角速度传感器相结合的测量方法，能准确地测出平地机在横向摆动、转弯、水田犁底层不平所造成的上下颠簸而产生的加速度分量，从而准确地计算出平 地伊的倾角。
另夕卜，本发明的方法通过融合计算出两种传感器测得的水平倾角信号COacc及 倾角变化率信号 W gyro ， 将CO acc与CO gyro之差的纟色对值与i殳定阈 <直相比,判断并
选择倾角的计算方法，并计算出平地妒倾角的大小，再通过倾角的大小刷新PWM 信号的占空比，最后通过PWM来控制水平调节液压系统，达到调节平地4产的目 的。
附固说明
图l为本发明的平地才几结构示意图； 图2为加速度传感器与角速度传感器的安装示意图； 图3为加速度传感器测量平地伊水平倾角的原理图； 图4为水平控制系统的工作原理图； 图5为水平控制系统的结构示意图； 图6为水平调节絲系统的电磁阀的控制逻辑原理图； 图7为水平控制系统的控制程序流程图。
具体实施例方式
下面结合附图对本发明作进一步的详细说明。
参阅
图1， 一种平地机的水平控制系统，包括平地伊1及控制平地4产1水平 位置的水平调节液压系统及用于控制水平调节液压系统工作的水平控制系统2 ， 平地铲上设有感应平地4产水平倾角信号及倾角变化率信号的感应系统，该感应 系统将感应信号传输至水平控制系统2进行处理。
该感应系统包括用于感应水平倾角信号的加速度传感器4及感应倾角变化 率信号的角速度传感器5。如图2所示，该加速度传感器4安装于平地伊1转轴 上，才艮据平地4产1转轴所在的平面定出加速度传感器4的测量轴x轴和y轴(与
传感器固连)，其x轴和y轴的加速度分别为ax与ay，可用下式计算平地4产1的水平倾角oc ，其原理可用图3来说明。重力加速度g在测量轴的分量ax与ay (由传感器测出)，其大小取决于测量轴水平倾斜角a:
ax = g x s in a
ay = g x cos oc
才艮据上述二式，通过传感器输出可以计算出平地铲1水平倾角a:
a=arctg (ax/ay)
式中ax是传感器测出的加速度值。上式成立的前提条件是a,完全由g引起, 即平地4产1的运动不在x轴产生加速度，例如平地4产1静止、匀速直线运动等 时即属于这种情况，否则当平地机引起的横向摆动及平地机在转弯时引起的甩 尾在加速度传感器4的测量轴上产生的加速度分量，或者由于水田犁底层不平 引起平地机上下颠簸而在加速度传感器4的测量轴上产生的加速度分量等时， 单纯使用加速度传感器4和上述公式并不能准确测量平地伊1水平倾角，为此 必须引入角速度传感器5对平地伊1的水平倾角进行测定。本实施例中，该角 速度传感器5为一陀螺仪传感器。利用角速度传感器5测得的倾角变化率co对 时间T的积分，即在上次的倾角勤出上每次加上co x T得到当前的平地伊1的实 际倾角。
如图4及图5所示，该水平控制系统2包括处理感应信号的孩t控制器及将 微控制器输出信号驱动放大的驱动电路。其中，该微控制器为ARM7内核的32 位微处理器，其内集成快速存储器，分别具有256KB的Flash ROM和64KB的 SRAM，无需扩展存储器，且该微控制器还内置8通道10位A/D转换器，为提高 控制精度,用软件方式提高转换精度至13位。此外它内有PWM信号发生器，及 RS232、 SPI、 UART等多种通信接口。该水平控制系统2还包括提供3. 3V、 5V、 12V等各种不同电压的电源电路，该电源电路连接至外部的蓄电池或其他电源设 备。
该水平调节液压系统包括连接至平地伊l的双作用油缸3，双作用油釭3通过电磁阀连接至水平控制系统2。本发明中，由上述两个传感器分别测出平地伊 1的水平倾角及水平倾角变化率，再才艮据融合控制算法计算输出脉冲宽度调制信 号，即PWM控制信号的占空比，利用电石兹阀，控制双作用油缸3动作调整平地 伊l，直至平地伊l调整至水平状态。电磁阀使用PWM脉冲控制，可有效地起到 流量控制效果，实现双作用油缸3行程的平滑模拟控制，从而使控制过程平稳。
水平控制系统2还设有两个用于调节平地妒1与感应系统安装时的水平误 差的拾睫与两个用于指示升降电磁阀动作情况的LED指示灯。由于感应系统安 装时产生的误差，感应系统测定的平地4产1水平倾角与平地4产1实际水平倾角 难免存在一个固定偏差值，M该值调整好后写入系统的存贮器中。而LED指 示灯则在相应电磁阀动作时点亮。
另外，本发明还提供了一种平地才几的水平控制方法，其包括以下步骤
a、 加速度传感器感应平地伊的水平倾角信号，角速度传感器感应倾角变化 率信号；
b、 微控制器采集感应信号，并对感应信号进行融合处理，发出脉冲宽度调 制信号至驱动电路；
c、 电磁阀根据驱动电路发出的信号来控制双作用油缸的伸缩度；
d、 通过双作用油缸作用于平地伊来调节平地伊的水平度。
在步骤b中，对该水平倾角信号及倾角变化率信号的处理还包括以下步骤
(1) 检查水平倾角信号及倾角变化率信号是否改变，是则执行步骤(2)， 否则结束；
(2) 计算两种传感器测得的水平倾角信号C0acc及倾角变化率信号COgyr。，为
判断平地4产是否受到干扰，分别求出两个传感器测得的水平倾角信号COacc及倾
角变化率信号CO gyr。，求出二者之差的绝对值，由该值决定平地伊受到干扰与否， 若小于设定的阈值则认为未受千扰，否则出现千扰，由此可决定使用加速度传
感器还是角速度传感器来测量平地伊倾角，若平地4产未受干扰，则按水平倾角信号COacc计算平地铲倾角，若平地铲受到干扰，则按上一次平地铲的倾角加上 COgyr。xT,求出平地铲倾角，其中T为微控制器的工作周期；
(3 )根据平地伊的倾角变化刷新脉冲宽度调制信号的占空比； (4)驱动电5^#收脉沖宽度调制信号，进4亍;汶大并输出至电》兹阀。 步骤(1 )中，微控制器采集水平倾角信号及倾角变化率信号的周期为IOT。 孩i:控制器的工作周期T由定时器设定为10ms,采集感应信号的周期即为100ms， 这样设定可实现对信号采样及对采集的信号进行滑动滤波，同时较大刷新周期 内才计算平地4产倾角变化率co，可保证结果比较准确，因为计算时间太短的话 则变化量太小。
微控制器的控制程序主要是基于定时器中断的中断服务程序，如图7所示， 其流程如下
微控制器先采集加速度传感器与角速度传感器的输出，通过超采样提高分 辨率并进行软件滤波，得到加速度传感器与角速度传感器的当前值；
PWM占空比是否刷新，若是，则执行下一步骤，若否，则定时器中断服务程
序结束；
分别计算两种传感器测得的平地妒的水平倾角及倾角变化率C0acc、 COgyr。； 00 acx与(0 gyr。之差绝对值是否小于设定阀值，若是则按水平倾角信号(0 acc
计算平地伊倾角，若否则按上一次平地4产的倾角加上cogyr。 x T,求出平地伊倾角；
检测平地4产的倾角是否在增加，若是则选择上升死区，若否则选择下 降死区，定义两个电i兹阀均不动作的平地4产倾角范围为死区，则滑动死区 保证平地4产接近水平位置时根据倾角变化的方向有不同的死区，进一步提 高控制过程的稳定性；
按控制算法、死区设置要求计算并刷新输出占空比；
定时器中断服务程序结束。
图6是水平调节絲系统的电磁阀的控制逻辑原理图。为了补偿安装中加速度传感器的X轴与平地伊之间平行度误差，可以通过键盘微调设置要 求的平地妒倾角00)(实为加速度传感器本身倾角，该角对应平地妒水平位
置)。设定值a。一般来说不为0，除非安装平行度误差为0,即x轴严格平
行于平地4产。图6中4黄坐标为平地4产倾角(测量得到)，下部分则是电》兹 岡工作区间分析图，上部分是相应区间电^f兹阀PWM驱动信号占空比大小。
以a。为分界点，左边倾角误差为负，为双作用油缸上升电》兹阀工作区，而
CO)右侧区间倾角误差为正，为下降电i兹阀工作区。上升与下降电i兹阀动作 互锁，即不能同时动作。为提高控制稳定性，减少平地伊处于水平位置附 近时电/兹阀频繁动作，设计上采取如下措施， 一是在a。两侧附近(亦即平 地伊水平位置附近)设置滑动死区，死区内两电磁阀不工作(对应占空比 为0)， 二是在水平位置(co))附近时根据误差大小减小PWM占空比。死区 设置上，每个电磁阀均加有施密特特性即动作滞后区间H。以对上升电f兹 阀为例，倾角由左边大的负误差区上升向ao靠近时，电磁阀一直工作直到 ca处停止，此后仅当误差又变负(倾角由ao向右变化)且倾角达到a" a4<a0， 二者之差为H)时，电磁阀才开始动作。同理图中m与co定义了下降电磁
阀动作滞后区间H。 ca、 a2、 a3、与cu形成了两个不重叠的死区DZO与DZl, 它们对应平地铲倾角不同的变化趋势，本设计称为滑动死区，其具体位置 与co)有关。见图6, DZO为基本死区，它以ao为中心对称设置，上升死区 DZO与下降死区DZ1分别为DZO与右侧下降电磁阀滞后H或左侧上升电磁 阀滞后H合成的结果。平地妒工作时，如果倾角是由左到右变化(上升)， 则死区为上升死区DZO，具体说来就是，倾角误差较负(靠左)时上升油
缸动作直到倾角变为oa,此时ii^上升死区DZO, DZO —直维持到％才结束，此时误差已偏正，下降油缸开始动作。同理倾角由右向左变化(下降)
则死区为下降死区DZ1。具体程序上可用一个全局变量平地伊倾角变化趋 势，由它决定使用那个死区。提高控制稳定性采取的另一措施是对水平控 制电磁阀使用PWM控制，见图6上部分所示。图中表示出平地伊不同水平 倾角下PWM占空比变化，误差小时(接进水平位置)使用小占空比以减小 冲击，本设计中采用简单线性规律，即占空比随误差(可经控制算法校正 后的误差，图中直接应用误差本身，相当于比例控制算法)大小线性变化， 死区参数决定后，由占空比上下限Dmax与Dmin即可决定任意倾角下输出 的占空比D,其中Dmax与Dmin由试验决定，本设计中分别为75 %与30%。
权利要求
1. 一种平地机的水平控制系统，包括平地铲、控制平地铲水平位置的水平调节液压系统及控制水平调节液压系统工作的水平控制系统，其特征在于平地铲上设有感应平地铲水平倾角信号及倾角变化率信号的传感器系统，该传感器系统将信号传输至水平控制器进行处理。
2、 根据权利要求l所述的平地机的水平控制系统，其特征在于该传感器 系统包括用于感应水平倾角信号的加速度传感器及感应倾角变化率信号的角速 度传感器。
3、 根据权利要求2所述的平地机的水平控制系统，其特征在于该加速度 传感器安装于平地伊转轴上。
4、 根据权利要求2所述的平地机的水平控制系统，其特征在于该角速度 传感器为一陀螺仪传感器。
5、 根据权利要求1所述的平地机的水平控制系统，其特征在于该水平控 制系统包括处理感应信号的微控制器及将微控制器输出信号驱动放大的驱动电 路。
6、 根据权利要求1至5任一项所述的平地机的水平控制系统，其特征在于 该7K平调节液压系统包括连接至平地妒的双作用油缸，双作用油缸通过电磁阀连接至水平控制系统。
7、 根据权利要求1所述平地机的水平控制系统的控制方法，其包括以下步骤a、 传感器系统感应平地伊的水平倾角信号及倾角变化率信号；b、 水平控制系统采集感应信号，并对感应信号进行融合处理，发出脉冲宽度调制信号至水平调节g系统；c 、水平调节液压系统才艮据发出的信号来控制平地4产的水平度。
8、 根据权利要求7所述的平地机的水平控制系统的控制方法，其特征在于 步骤a中，传感器系统包括加速度传感器及角速度传感器，加速度传感器用于感应水平倾角信号，角速度传感器用于感应倾角变化率信号，步骤C中，该水 平调节液压系统包括双作用油缸，水平控制系统将信号发送至一电i兹阀，由电 f兹阀来控制双作用油缸的伸缩度，并通过双作用油缸作用于平地妒来调节平地 铲的水平度。
9、根据权利要求8所述的平地机的水平控制系统的控制方法，其特征在于 对该水平倾角信号及倾角变化率信号的处理还包括以下步骤(1)采样并检查水平倾角信号及倾角变化率信号是否改变，是则执行步骤 (2)，否则结束；(2 )计算两种传感器测得的水平倾角信号00 acc及倾角变化率信号co gyr。，并将 C0,及COgyr。之差绝对值与设定阀值相比，若小于阀值，则按水平倾角信号COacc 计算平地铲倾角，若大于阔值，则按设定倾角加上(Ogyr。xT，求出平地4产倾角，其中T为微控制器的工作周期；(3 )根据平地铲的倾角变化刷新脉冲宽度调制信号的占空比；(4)驱动电,收脉沖宽度调制信号，进4亍;故大并输出至电i兹阀。
10、根据权利要求9所述的平地机的水平控制系统的控制方法，其特征在于步骤(1)中，孩史控制器采集水平倾角信号及倾角变化率信号的周期为IOT。
全文摘要
本发明公开了一种平地机的水平控制系统，包括平地铲、控制平地铲水平位置的液压系统及控制该液压系统工作的水平控制系统。平地铲上设有感应平地铲水平倾角及倾角变化率信号的传感器系统，将信号传输至水平控制系统进行处理；传感器系统包括用于感应水平倾角信号的加速度传感器及感应倾角变化率信号的角速度传感器。另外，本发明还公开了一种平地机的水平控制方法。本发明能准确地计算出平地铲的水平倾角。
文档编号E02F3/76GK101280570SQ200810028328
公开日2008年10月8日 申请日期2008年5月27日 优先权日2008年5月27日
发明者庆 李, 李就好, 王在满, 罗锡文, 赵祚喜 申请人:华南农业大学