CN109901602A - 一种菱形四轮中耕机主动调平***及方法 - Google Patents

Abstract

本发明涉及一种菱形四轮中耕机主动调平***，包括传感器、控制器和执行器；其中传感器包括用于实时测量菱形四轮中耕机行驶加速度的三轴加速度传感器、用于分别实时测量菱形四轮中耕机行驶角速度的陀螺仪Ⅰ和陀螺仪Ⅱ；三轴加速度传感器、陀螺仪Ⅰ和陀螺仪Ⅱ分别与控制器电连接；控制器与执行器电连接。该***提高菱形四轮中耕机的自动化程度；有效改善其作业条件，减小作业时的侧翻安全隐患；提高菱形四轮中耕机作业时的稳定性和舒适性。还涉及一种菱形四轮中耕机主动调平方法。在调平过程中采集角速度信号计算菱形四轮中耕机的姿态角Ⅱ做负反馈判断，提高调平精确性。

Description

一种菱形四轮中耕机主动调平***及方法

技术领域

本发明涉及农业机械技术控制领域，尤其涉及一种菱形四轮中耕机主动调平***及方法。

背景技术

目前，在诸如甘蔗、玉米等高杆作物机械化植保技术过程中，中耕培土、除草、施肥是生产过程中的重要环节。菱形四轮中耕机不仅具有可一次性完成松土、除草、培土和施肥作业等通用中耕机具有的特点，又因为其独特的机械结构更加适用于甘蔗、玉米等高杆作物，而成为高杆作物机械化管理的主要发展方向。但是目前，国内菱形四轮中耕机的作业自动化水平较低，同时也存在诸如作业过程中操作难度较大、作业质量较差以及容易倾翻等问题。

发明内容

针对现有技术中存在的技术问题，本发明的目的是：提供一种菱形四轮中耕机主动调平***，该***提高菱形四轮中耕机的自动化程度；有效改善其作业条件，减小作业时的侧翻安全隐患；提高菱形四轮中耕机作业时的稳定性和舒适性。

本发明的另一目的是提供一种菱形四轮中耕机主动调平方法，该方法在调平过程中采集角速度信号计算菱形四轮中耕机的姿态角Ⅱ做负反馈判断，提高调平精确性。

为了达到上述目的，本发明采用如下技术方案：

一种菱形四轮中耕机主动调平***，包括传感器、控制器和执行器；其中传感器包括用于实时测量菱形四轮中耕机行驶加速度的三轴加速度传感器、用于分别实时测量菱形四轮中耕机行驶角速度的陀螺仪Ⅰ和陀螺仪Ⅱ；三轴加速度传感器、陀螺仪Ⅰ和陀螺仪Ⅱ分别与控制器电连接；控制器与执行器电连接。采用陀螺仪Ⅰ和陀螺仪Ⅱ分别测量角速度是由于三轴加速度传感器噪声大，陀螺仪精度高、噪声小但有积累误差。因此，陀螺仪Ⅰ目的是与三轴加速度传感器数据融合，得到新的更准确的数据。采用陀螺仪Ⅱ因为只需要瞬时值对比，没有积累误差，只需要陀螺仪Ⅱ数据就足够了，可以减少运算量提高效率。

优选地，控制器包括A/D、D/A转换器和单片机；执行器包括电磁阀和液压油缸；通过单片机控制电磁阀的通断使液压油缸上下运动，实现调平。

优选地，单片机预先设定了预设角值。

优选地，传感器放置在菱形四轮中耕机的质心位置，液压油缸分别安装于菱形四轮中耕机两侧的悬架上。

优选地，通过卡尔曼滤波器将加速度和陀螺仪Ⅰ测量的角速度的数据进行补偿和信息整合得到姿态角Ⅰ。

优选地，通过陀螺仪Ⅱ测量的角速度的数据计算得到姿态角Ⅱ做负反馈判断。

一种菱形四轮中耕机主动调平方法，预先在单片机设定预设角值，调平方法包括以下步骤：(1)利用三轴加速度传感器、陀螺仪分别实时采集菱形四轮中耕机的加速度和角速度值；通过卡尔曼滤波器将加速度和陀螺仪Ⅰ测量的角速度的数据进行补偿和信息整合得到姿态角Ⅰ；(2)比较姿态角Ⅰ与预设角，若姿态角Ⅰ小于预设角，则跳转至步骤(1)；若姿态角大于等于预设角，则进入步骤(3)；(3)将步骤(1)得到的姿态角Ⅰ结合菱形四轮中耕机悬架的几何关系解算得到车身被调至水平时各个悬架液压缸的伸缩位移量；(4)将步骤(3)计算得到的液压缸伸缩位移量，结合菱形四轮中耕机的液压***参数，计算得到各电磁阀的信号输出量，控制电磁阀的通断时间；(5)根据陀螺仪Ⅱ测量的角速度信号计算得到菱形四轮中耕机的姿态角Ⅱ并与预设角相比；若姿态角Ⅱ小于预设角则跳转至(1)进行循环，若姿态角Ⅱ大于等于预设角则停止调平。

优选地，步骤(1)中，利用控制器读取三轴加速度传感器和陀螺仪Ⅰ所采集的数据值，由三轴加速度传感器测得的加速度值计算得出菱形四轮中耕机初始姿态角；采用卡尔曼滤波方法，对由三轴加速度传感器计算得出的初始姿态角和陀螺仪Ⅰ的角速度信号建立特征模型和姿态角度测量***，对两组数据进行补偿及信息融合，得到姿态角Ⅰ。

优选地，步骤(2)中，单片机比较姿态角Ⅰ和预设角并输出控制信号。

优选地，步骤(3)中，电磁阀接收单片机发出的控制信号改变通断时间，从而控制液压油缸的举升量程，以达到调平目的。

本发明的原理是：本发明的主动调平***通过卡尔曼滤波器将加速度和角速度的数据进行补偿和信息整合，得到更加准确的菱形四轮中耕机的姿态角Ⅰ。姿态角Ⅰ信号再通过解算得到液压油缸的举升量程，通过电磁阀的通断来使中耕机液压油缸上下运动。调平过程中采集角速度信号计算菱形四轮中耕机的姿态角Ⅱ做负反馈判断，提高调平精确性。

总的说来，本发明具有如下优点：

1.该***提高菱形四轮中耕机的自动化程度；有效改善其作业条件，减小作业时的侧翻安全隐患；提高菱形四轮中耕机作业时的稳定性和舒适性。从而推动我国农业机械生产自动化、智能化的发展进程。

2.由三轴加速度传感器解算出的初始姿态角作为卡尔曼滤波器的观测值，以此建立卡尔曼滤波器的观测方程；由于初始姿态角与陀螺仪Ⅰ测得的角速度存在导数关系，以俯仰角、滚转角以及对应的角速度偏移为状态量，建立卡尔曼滤波器的状态方程。

3.调平过程中采集角速度信号计算菱形四轮中耕机的姿态角Ⅱ做负反馈判断，提高调平精确性。

附图说明

图1为菱形四轮中耕机主动调平方法的流程图。

图2为菱形四轮中耕机主动调平***的示意图。

图3为调平过程时三轴加速度传感器状态示意图。

图4为菱形四轮中耕机主动调平***的安装位置示意图。

图5为菱形四轮中耕机调平前后的示意图，其中左侧为调平前，右侧为调平后。

图中的标号和对应的零部件名称为：1为传感器(包括三轴加速度传感器、陀螺仪Ⅰ和陀螺仪Ⅱ)；2为控制器；3为电磁阀；4为液压油缸。图3中X、Y、Z表示坐标轴，x、y表示水平面xy，α：俯仰角，pitch；β：滚转角，roll；g表示重力加速度方向。

具体实施方式

下面结合实施例及附图，对本发明做进一步详细的说明，但本发明的实施方式不限于此。

一种菱形四轮中耕机主动调平***，包括传感器、控制器和执行器。其中传感器包括三轴加速度传感器、陀螺仪Ⅰ和陀螺仪Ⅱ；控制器包括A/D、D/A转换器和单片机；执行器包括电磁阀和液压油缸。三轴加速度传感器、陀螺仪Ⅰ和陀螺仪Ⅱ分别与单片机电连接；单片机与执行器电连接，发出控制信号控制电池阀的通断时间。单片机预先设定了预设角值。

三轴加速度传感器能实时采集菱形四轮中耕机的加速度大小，陀螺仪能实时采集菱形四轮中耕机的角速度大小；控制器能读取三轴加速度传感器、陀螺仪Ⅰ所测得的加速度和角速度大小进而计算出菱形四轮中耕机的姿态角Ⅰ，并将计算得到的姿态角Ⅰ和预设角相比较，以判断是否需要进行调平；进行判断后，控制器结合菱形四轮中耕机的几何结构及其液压***参数，解算出液压缸所需的伸缩位移量及电磁阀通断时间；进而对中耕机进行调平。控制器能读取陀螺仪Ⅱ所测得的角速度大小进而计算出菱形四轮中耕机的姿态角Ⅱ，利用姿态角Ⅱ配合调平。

如图1所示，一种菱形四轮中耕机主动调平***的调平方法详细内容如下：

步骤1：通过三轴加速度传感器实时采集菱形四轮中耕机的加速度值，根据其三轴加速度值算出菱形四轮中耕机的初始姿态角；具体计算方法如下：如图3所示，假设三轴加速度传感器的X轴与水平面xy之间的夹角为α(称为俯仰角)，Y轴与水平面间的夹角为β(称为滚转角)，Z轴与重力方向夹角为γ。重力加速度在XYZ三个轴上的投影即为三个轴传感器的读数，因此可计算出：

α＝arcsin(a_x/g)

β＝arcsin(a_y/g)

γ＝arccos(a_z/g)

根据三个轴加速度的矢量和等于重力加速度，即：

即可得俯仰角α滚转角β分别为：

式中：a_x、a_y、a_z分别为三个轴的加速度的矢量，g为重力加速度。

步骤2：由于陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差；三轴加速传感器所测加速度值虽然没有累积误差，但是它包含的噪声太多(因为待测物运动时会产生加速度，电机运行时振动会产生加速度等)；所以，单独使用三轴加速度传感器或陀螺仪所测得值计算姿态角误差较大，需要互补。因此由三轴加速度传感器和陀螺仪Ⅰ建立姿态角度测量***，建立其特征模型，采用卡尔曼滤波方法，对来自三轴加速度传感器和陀螺仪Ⅰ的信号进行融合,解决以上问题得到更准确的姿态角(即为姿态角Ⅰ)。已知卡尔曼滤波的***状态方程为：

x_k＝Ax_k-1+Bu_k-1+w_k-1

观测方程为：

z_k＝Hx_k+v_k

其中w_k-1是服从高斯分布的预测过程噪声w_k-1～N(0，Q)，v_k是服从高斯分布的观测噪声v_k～N(0，R)。

由于倾角和倾角角速度存在导数关系,我们以俯仰角(pitch、滚转角(roll)以及对应的角速度偏移为状态量：以三轴加速度传感器经计算得到的初始姿态角为观测值，则得到如下模型：

状态方程：

观测方程：

通过卡尔曼滤波器时间更新方程：

和卡尔曼滤波器状态更新方程：

进行循环迭代可以得到最后的姿态角Ⅰ。

步骤3：通过步骤2算出经卡尔曼滤波器修正、融合后的姿态角Ⅰ；结合菱形四轮中耕机的几何结构与液压***的参数进行姿态角Ⅰ的解算，计算出每个液压油缸的伸缩位移量，再通过控制电磁阀的通断时间实现液压油缸的伸缩。

步骤4：由于陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，且目前只需要采集瞬时中耕机的姿态角Ⅱ值做负反馈判断，因此通过陀螺仪Ⅱ实时采集菱形四轮中耕机的角速度值，通过对其角速度值的积分可以得到倾角：设有一角度函数θ，其微分方程为：

θ′＝f(ω，t)

f(ω，t)即为角速度ω对时间t的相关函数。从微积分学中得知微分方程的定义可以被近似为：

重新整理形式，可得：

θ_k＝θ_k-1+h*f(ω，t)

即可得到中耕机的姿态角Ⅱ。

步骤5：根据步骤4所计算出的菱形四轮中耕机姿态角Ⅱ，与预设角进行比较，若小于预设角则退出；若大于等于预设角则跳至步骤1进行循环。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种菱形四轮中耕机主动调平***，其特征在于：包括传感器、控制器和执行器；其中传感器包括用于实时测量菱形四轮中耕机行驶加速度的三轴加速度传感器、用于分别实时测量菱形四轮中耕机行驶角速度的陀螺仪Ⅰ和陀螺仪Ⅱ；三轴加速度传感器、陀螺仪Ⅰ和陀螺仪Ⅱ分别与控制器电连接；控制器与执行器电连接。

2.按照权利要求1所述的菱形四轮中耕机主动调平***，其特征在于：控制器包括A/D、D/A转换器和单片机；执行器包括电磁阀和液压油缸；通过单片机控制电磁阀的通断使液压油缸上下运动，实现调平。

3.按照权利要求2所述的菱形四轮中耕机主动调平***，其特征在于：单片机预先设定了预设角值。

4.按照权利要求1所述的菱形四轮中耕机主动调平***，其特征在于：传感器放置在菱形四轮中耕机的质心位置，液压油缸分别安装于菱形四轮中耕机两侧的悬架上。

5.按照权利要求1所述的菱形四轮中耕机主动调平***，其特征在于：通过卡尔曼滤波器将加速度和陀螺仪Ⅰ测量的角速度的数据进行补偿和信息整合得到姿态角Ⅰ。

6.按照权利要求1所述的菱形四轮中耕机主动调平***，其特征在于：通过陀螺仪Ⅱ测量的角速度的数据计算得到姿态角Ⅱ做负反馈判断。

7.一种菱形四轮中耕机主动调平方法，其特征在于，预先在单片机设定预设角值，调平方法包括以下步骤：

(1)利用三轴加速度传感器、陀螺仪分别实时采集菱形四轮中耕机的加速度和角速度值；通过卡尔曼滤波器将加速度和陀螺仪Ⅰ测量的角速度的数据进行补偿和信息整合得到姿态角Ⅰ；

(2)比较姿态角Ⅰ与预设角，若姿态角Ⅰ小于预设角，则跳转至步骤(1)；若姿态角大于等于预设角，则进入步骤(3)；

(3)将步骤(1)得到的姿态角Ⅰ结合菱形四轮中耕机悬架的几何关系解算得到车身被调至水平时各个悬架液压缸的伸缩位移量；

(4)将步骤(3)计算得到的液压缸伸缩位移量，结合菱形四轮中耕机的液压***参数，计算得到各电磁阀的信号输出量，控制电磁阀的通断时间；

(5)根据陀螺仪Ⅱ测量的角速度信号计算得到菱形四轮中耕机的姿态角Ⅱ并与预设角相比；若姿态角Ⅱ小于预设角则跳转至(1)进行循环，若姿态角Ⅱ大于等于预设角则停止调平。

8.按照权利要求7所述的菱形四轮中耕机主动调平方法，其特征在于：步骤(1)中，利用控制器读取三轴加速度传感器和陀螺仪Ⅰ所采集的数据值，由三轴加速度传感器测得的加速度值计算得出菱形四轮中耕机初始姿态角；采用卡尔曼滤波方法，对由三轴加速度传感器计算得出的初始姿态角和陀螺仪Ⅰ的角速度信号建立特征模型和姿态角度测量***，对两组数据进行补偿及信息融合，得到姿态角Ⅰ。

9.按照权利要求7所述的菱形四轮中耕机主动调平方法，其特征在于：步骤(2)中，单片机比较姿态角Ⅰ和预设角并输出控制信号。

10.按照权利要求9所述的菱形四轮中耕机主动调平方法，其特征在于：步骤(3)中，电磁阀接收单片机发出的控制信号改变通断时间，从而控制液压油缸的举升量程，以达到调平目的。