CN104206063B - 一种作业机具自动调平系统及其控制方法 - Google Patents

Abstract

本发明公开了一种作业机具自动调平系统及其控制方法，所述系统包括倾角检测传感器模块、位移检测传感器、执行模块以及自动调平控制模块，所述执行模块包括作业机具、旋转机构以及伸缩机构，所述作业机具通过旋转机构与农业机械车体连接；所述伸缩机构相对于作业机具倾斜设置，且伸缩的一端与作业机具连接；所述倾角检测传感器模块设置在农业机械车体上，用于检测农业机械车体的倾斜运动信号；所述位移检测传感器设置在伸缩机构上，用于检测伸缩机构所在调节位置的直线位移；所述倾角检测传感器模块、位移检测传感器和伸缩机构分别与自动调平控制模块连接。本发明及方法可以保证作业机具的精准、平稳控制，以达到精确作业的目的。

Description

一种作业机具自动调平系统及其控制方法

技术领域

本发明涉及一种自动调平系统，尤其是一种作业机具自动调平系统及其控制方法，属于智能农业机械领域。

背景技术

现代农业机械大都要求作业机具保持与作业对象面平行。目前市场上的大多数农业机械仍为传统的仿形作业方式，只有少量农业机械可实现作业机具高度方向的控制，但还不能做到水平控制。然而，在实际作业中，由于田块不平整、泥脚深浅不一等因素，容易导致左右轮不在同一水平位置，升降仿形装置只能控制作业机具的高低，不能控制整机在左右水平方向的平衡，导致作业达不到预定要求。如大多水田犁底层高低不平，水田平整作业时，激光控制系统仅能保证平地铲高低可调，而不能保证平地铲水平可控，导致泥面平整度难以满足种植要求。

为了实现手扶插秧机水平控制，中国专利号为200820133989.3的实用新型专利公开了一种插秧机自动平衡装置，应用万有引力及天平杠杆原理，传感器重锤自动找平衡，控制液压系统带动相关部件实现插秧机体左右水平，保证秧苗栽植深度一致。为保证挂车水平，中国专利号为201220293588.0的实用新型专利公开了一种挂车水平角度补偿机构，通过调节气囊的充气量来调节角度，使车体保持水平。为实现平地机的水平控制，中国专利号为200810028328.9的发明专利公开了一种平地机的水平控制系统及其控制方法，在平地铲上安装有感应平地铲水平倾角及倾角变化率信号的传感器系统以计算出平地铲的水平倾角，从而控制平地铲水平，实际应用中这种方式受平地铲运动影响导致难以准确检测平地铲倾斜角度，并存在传感器系统安装要求高，传感系统和控制系统易进水问题。

发明内容

本发明的目的是为了解决上述现有技术的缺陷，提供了一种作业机具自动调平系统，该系统适用于现代农业机械，且结构简单，可以保证作业机具的精准、平稳控制，以达到精确作业。

本发明的另一目的在于提供一种上述作业机具自动调平系统的自动调平控制方法。

本发明的目的可以通过采取如下技术方案达到：

一种作业机具自动调平系统，包括倾角检测传感器模块、位移检测传感器、自动调平控制模块以及执行模块，所述执行模块包括作业机具、旋转机构以及伸缩机构，所述作业机具通过旋转机构与农业机械车体连接；所述伸缩机构相对于作业机具倾斜设置，且伸缩的一端与作业机具铰接，通过伸长或缩短驱动作业机具旋转，以调节作业机具相对于农业机械车体的倾斜角度；所述倾角检测传感器模块设置在农业机械车体上，用于检测农业机械车体的倾斜运动信号；所述位移检测传感器设置在伸缩机构上，用于检测伸缩机构所在调节位置的直线位移；所述倾角检测传感器模块、位移检测传感器和伸缩机构分别与自动调平控制模块连接。

作为一种优选方案，所述倾角检测传感器模块包括加速度传感器和角速度传感器，所述加速度传感器和角速度传感器检测到的农业机械车体的倾斜运动信号通过Kalman融合算法计算出农业机械车体的实时倾斜角度。

作为一种优选方案，所述自动调平控制模块包括运算控制器、驱动单元以及人机交互单元，所述驱动单元和人机交互单元分别与运算控制器连接；所述运算控制器用于根据农业机械车体的实时倾斜角度、作业机具和车体结构的几何模型、位移检测传感器数据和通过人机交互单元设定的作业机具相对于农业机械车体的目标倾斜角度，通过驱动单元控制伸缩机构伸长或缩短，以调节作业机具相对于农业机械车体的倾斜角度。

作为一种优选方案，所述作业机具与旋转机构铰接。

作为一种优选方案，所述伸缩机构为液压油缸或电动缸。

本发明的另一目的可以通过采取如下技术方案达到：

S1、倾角检测传感器模块的加速度传感器和角速度传感器检测农业机械车体的倾斜运动信号，并传输给自动调平控制模块的运算控制器；

S2、运算控制器根据农业机械车体的倾斜运动信号，通过Kalman融合算法计算出农业机械车体的实时倾斜角度；

S3、运算控制器读取当前位移检测传感器的信号，并转化为伸缩机构的位移量；

S4、运算控制器根据作业机具和农业机械车体结构的几何模型，以及伸缩机构当前的位移量，通过建立伸缩机构的位移x变化与作业机具相对于农业机械车体的倾斜角度θ变化的关系θ＝f(x)，计算作业机具相对于农业机械车体的倾斜角度；

S5、运算控制器根据农业机械车体的倾斜角度和作业机具相对于农业机械车体的倾斜角度的差值，结合结构的几何模型，将角度的差值代入θ＝f(x)，计算出作业机具调平所需的伸缩机构的控制目标位移值；

S6、运算控制器将控制信号发送给驱动单元，通过驱动单元使伸缩机构运动到目标位移，实现对作业机具的调平控制。

作为一种优选方案，所述伸缩机构的位移x变化与作业机具相对于农业机械车体的倾斜角度θ变化的关系如下：

其中，∠α和∠β分别为作业机具与旋转机构铰接处的两个固定角度，a和b分别为作业机具与旋转机构的铰接点到伸缩机构两端的距离，x₀为伸缩机构最短长度。

本发明相对于现有技术具有如下的有益效果：

1、本发明的作业机具自动调平系统，利用倾角检测传感器模块检测农业机械车体(一般为农用拖拉机)的姿态角度(横滚角和俯仰角)，利用伸缩机构的固定几何关系，以伸缩机构的位移为变量，建立伸缩机构的位移x变化与作业机具相对于农业机械车体的倾斜角度θ变化的关系θ＝f(x)，自动调平控制模块根据所需的作业机具相对于农业机械车体的倾斜角度θ控制伸缩机构的位移x，保证了作业机具的精准、平稳控制，以达到精确作业的目的。

2、本发明的作业机具自动调平系统中的倾角检测传感器模块可以安装于农业机械车体上，农业机械车体的姿态变化较作业机具状态变化缓慢、平稳，因此更便捷、精度更高。

附图说明

图1为本发明的作业机具自动调平系统结构示意图。

图2为本发明的作业机具自动调平系统的控制原理框图。

图3为Kalman融合算法的流程图。

图4为本发明的作业机具自动调平系统简化结构几何模型。

图5为本发明的作业机具自动调平系统应用在水田平地机上的结构示意图。

其中，1-倾角检测传感器模块，2-位移检测传感器，3-自动调平控制模块，4-作业机具，5-旋转机构，6-伸缩机构，7-农业机械车体，8-拖拉机，9-平地铲，10-平地铲连接机构，11-液压油缸，12-铰接处。

具体实施方式

实施例1：

如图1和图2所示，本实施例的作业机具自动调平系统包括倾角检测传感器模块1、位移检测传感器2、自动调平控制模块3以及执行模块，所述执行模块包括作业机具4、旋转机构5以及伸缩机构6，所述倾角检测传感器模块1、位移检测传感器2和伸缩机构6分别与自动调平控制模块3连接，所述伸缩机构6可以为液压油缸或电动缸；其中：

所述倾角检测传感器模块1包括加速度传感器和角速度传感器，其设置在农业机械车体7上，用于检测农业机械车体7的倾斜运动信号，检测的运动信号通过Kalman融合算法计算出农业机械车体7的实时倾斜角度；所述位移检测传感器2设置在伸缩机构6上，用于检测伸缩机构6所在调节位置的直线位移；

所述作业机具4通过旋转机构5与农业机械车体7连接，所述伸缩机构6相对于作业机具4倾斜设置，且伸缩的一端与作业机具4连接，本实施例中另一端与农业机械车体7连接，伸缩机构6通过伸长或缩短驱动作业机具4旋转，以调节作业机具4相对于农业机械车体7的倾斜角度；

所述自动调平控制模块3包括运算控制器(型号为TMS320F28069)、驱动单元以及人机交互单元，所述驱动单元和人机交互单元分别与运算控制器连接；所述运算控制器用于根据农业机械车体7的实时倾斜角度、作业机具4和农业机械车体7结构的几何模型、位移检测传感器2数据和通过人机交互单元设定的作业机具4相对于农业机械车体7的目标倾斜角度，通过驱动单元控制伸缩机构6伸长或缩短，以调节作业机具4相对于农业机械车体7的倾斜角度。

本实施例的作业机具自动调平系统的自动调平控制方法，包括以下步骤：

S1、倾角检测传感器模块1的加速度传感器和角速度传感器检测农业机械车体7的运动信号，并传输给自动调平控制模块3的运算控制器；

S2、运算控制器根据农业机械车体7的运动信号，通过Kalman融合算法计算出车体的倾斜角度；Kalman融合算法如图3所示，方程如下：

预测状态： ${\hat{X}}_{K|K-1}={\mathrm{\φ}}_{K|K-1}{\hat{X}}_{K-1};$

预测估计协方差矩阵： $P_{K|K-1}={\mathrm{\φ}}_{K|K-1}{P}_{K-1}{\mathrm{\φ}}_{K|K-1}^T+Q_{K-1};$

更新的状态估计： ${\hat{X}}_K={\hat{X}}_{K|K-1}+K_K(Z_K-H_K{\hat{X}}_{K|K-1});$

更新的最优卡尔曼增益： $K_K=P_{K|K-1}{H}_K^T{(H_K{P}_{K|K-1}{H}_K^T+R_K)}^{-1};$

更新的协方差估计：P_k＝(I-K_KH_K)P_K|K-1。

S3、运算控制器读取当前位移检测传感器2的信号，并转化为伸缩机构6的位移量；

S4、运算控制器根据作业机具4和农业机械车体7结构的几何模型，如图4所示，以及伸缩机构6当前的位移量，建立伸缩机构6的位移x变化与作业机具4相对于农业机械车体7的倾斜角度θ变化的关系，如下：

其中，∠α和∠β分别为作业机具4与旋转机构5铰接处12的两个固定角度，a和b分别为作业机具4与旋转机构5的铰接点到伸缩机构6两端的距离，x₀为伸缩机构6最短长度；通过θ＝f(x)计算作业机具4相对于农业机械车体7的倾斜角度；

S5、运算控制器根据农业机械车体7的倾斜角度和作业机具4相对于农业机械车体7的倾斜角度的差值，结合结构的几何模型，将角度的差值代入θ＝f(x)，计算出作业机具4调平所需的伸缩机构6的控制目标位移值；

S6、运算控制器将控制信号发送给驱动单元，通过驱动单元使伸缩机构6运动到目标位移，实现对作业机具4的调平控制。

实施例2：

本实施例是一种应用实例，将作业机具自动调平系统应用在水田平地机上，构成水田平地机的平地铲自动调平系统；本实施例如图5所示，农业机械车体为拖拉机8，作业机具为平地铲9，旋转机构为平地铲连接机构10，伸缩机构为液压油缸11，驱动单元为电控液压换向阀。

如图5所示，所述平地铲9与平地铲连接机构10铰接，液压油缸11相对于平地铲9和平地铲连接机构10倾斜设置，且伸缩的一端与平地铲9铰接，另一端与平地铲连接机构10铰接，平地铲9通过平地铲连接机构10与拖拉机8连接组成水田平地机；在液压油缸11不调节时，平地铲9和平地铲连接机构10与拖拉机8是刚体连接，当拖拉机8受左右轮限深不一时，平地铲9将随拖拉机8车身出现倾斜，液压油缸11伸长或缩短使平地铲9绕铰链结构旋转实现调节相对于拖拉机8的倾斜角度；倾角检测传感器模块1安装于拖拉机8上，用于检测拖拉机8受左右轮限深不一时产生的倾斜角度；位移检测传感器2安装于液压油缸11上，用于检测自动调平油缸10的伸长量。

本实施例的自动调平控制方法过程如下：

当平地机在水田平整作业时，由于水田犁底层不平，导致拖拉机8的右轮陷深比左轮深，出现拖拉机8和平地铲9向右倾斜，如图5所示；此时若不对平地铲9进行调平控制，水田平地机将难以达到精确平整的目的，安装于拖拉机8上的倾角检测传感器模块1检测到倾斜角度并传输至自动调平控制模块3，自动调平控制模块3根据平地铲9、平地铲连接机构10和液压油缸11结构的几何模型，以及位移检测传感器2当前的位移量计算出当前平地铲9相对于拖拉机8的倾斜角度，再结合检测到的拖拉机8的倾斜角度，得到两个倾斜角度之间的差值，然后计算出平地铲9调平所需的液压油缸11的控制目标位移值，向电控液压换向阀发出驱动信号，控制调平油缸10伸长或缩短，以调节平地铲9保持水平，实现对平地铲9瞬间调平控制，达到水田精确平整的目的。

以上所述，仅为本发明专利较佳的实施例，但本发明专利的保护范围并不局限于此，如实施例2中的液压油缸用电动缸替换，任何熟悉本技术领域的技术人员在本发明专利所公开的范围内，根据本发明专利的技术方案及其发明构思加以等同替换或改变，都属于本发明专利的保护范围。

Claims (7)

1.一种作业机具自动调平系统，其特征在于：包括倾角检测传感器模块、位移检测传感器、自动调平控制模块以及执行模块，所述执行模块包括作业机具、旋转机构以及伸缩机构，所述作业机具通过旋转机构与农业机械车体连接；所述伸缩机构相对于作业机具倾斜设置，且伸缩的一端与作业机具铰接，通过伸长或缩短驱动作业机具旋转，以调节作业机具相对于农业机械车体的倾斜角度；所述倾角检测传感器模块设置在农业机械车体上，用于检测农业机械车体的倾斜运动信号；所述位移检测传感器设置在伸缩机构上，用于检测伸缩机构所在调节位置的直线位移；所述倾角检测传感器模块、位移检测传感器和伸缩机构分别与自动调平控制模块连接。

2.根据权利要求1所述的一种作业机具自动调平系统，其特征在于：所述倾角检测传感器模块包括加速度传感器和角速度传感器，所述加速度传感器和角速度传感器检测到的农业机械车体的运动信号通过Kalman融合算法计算出农业机械车体的实时倾斜角度。

3.根据权利要求1所述的一种作业机具自动调平系统，其特征在于：所述自动调平控制模块包括运算控制器、驱动单元以及人机交互单元，所述驱动单元和人机交互单元分别与运算控制器连接；所述运算控制器用于根据农业机械车体的实时倾斜角度、作业机具和车体结构的几何模型、位移检测传感器数据和通过人机交互单元设定的作业机具相对于农业机械车体的目标倾斜角度，通过驱动单元控制伸缩机构伸长或缩短，以调节作业机具相对于农业机械车体的倾斜角度。

4.根据权利要求1所述的一种作业机具自动调平系统，其特征在于：所述作业机具与旋转机构铰接。

5.根据权利要求1所述的一种作业机具自动调平系统，其特征在于：所述伸缩机构为液压油缸或电动缸。

6.一种作业机具自动调平系统的自动调平控制方法，其特征在于包括以下步骤：

s1、倾角检测传感器模块的加速度传感器和角速度传感器检测农业机械车体的倾斜运动信号，并传输给自动调平控制模块的运算控制器；

S2、运算控制器根据农业机械车体的倾斜运动信号，通过Kalman融合算法计算出农业机械车体的实时倾斜角度；

S3、运算控制器读取当前位移检测传感器的信号，并转化为伸缩机构的位移量；

s4、运算控制器根据作业机具和农业机械车体结构的几何模型，以及伸缩机构当前的位移量，通过建立伸缩机构的位移x变化与作业机具相对于农业机械车体的倾斜角度θ变化的关系θ＝f(x)，计算作业机具相对于农业机械车体的倾斜角度；

S5、运算控制器根据农业机械车体的倾斜角度和作业机具相对于农业机械车体的倾斜角度的差值，结合结构的几何模型，将角度的差值代入θ＝f(x)，计算出作业机具调平所需的伸缩机构的控制目标位移值；

S6、运算控制器将控制信号发送给驱动单元，通过驱动单元使伸缩机构运动到目标位移，实现对作业机具的调平控制。

7.根据权利要求6所述的一种作业机具自动调平系统的自动调平控制方法，其特征在于：所述伸缩机构的位移x变化与作业机具相对于农业机械车体的倾斜角度θ变化的关系如下：

其中，∠α和∠β分别为作业机具与旋转机构铰接处的两个固定角度，a和b分别为作业机具与旋转机构的铰接点到伸缩机构两端的距离，x₀为伸缩机构最短长度。