CN109900296A - 一种农机作业行驶速度检测系统及检测方法 - Google Patents

Abstract

本发明公开了一种农机作业行驶速度检测系统及检测方法，该检测系统包括GPS测速模块、霍尔传感器测速模块和单片机测量单元，该检测方法为：GPS测速模块和霍尔传感器测速模块同步测量农机作业行驶速度；对测得的数据进行滑动平均滤波处理；将处理后的GPS测速模块速度数据和霍尔测速数据作差，更新Kalman滤波器的测量值；Kalman滤波器对霍尔传感器测速模块的系统偏差进行实时估计后将系统偏差值叠加到经滑动平均滤波处理后的霍尔测速数据上，得到最终的速度估计值。本发明将GPS测速模块和霍尔传感器测速模块的测量的数据信息进行综合处理，得到农业机械特别是水田农业机械更为精确的作业速度检测。

Description

一种农机作业行驶速度检测系统及检测方法

技术领域

本发明涉及农业机械对地作业速度检测技术领域，尤其涉及一种农机作业行驶速度检测系统及检测方法。

背景技术

农业机械作业过程中的对地速度是重要的基础参数，既为作业档位和油门大小的确定提供直接参考，也为播种量和农药化肥施用量的决策提供定量依据。农机作业速度的实时精确检测，是现代农业机械实现精准作业的重要前提。

农机作业速度的检测主要有驱动轮轮速测量和非接触式对地测量两种方式。驱动轮轮速的测量方式，主要由磁电式轮速传感器、电涡流式轮速传感器和霍尔线性集成式轮速传感器完成驱动轮转速测量，进而通过驱动轮直径换算成对地速度。非接触式对地测量方式，主要有加速度计法、多普勒效应法、风压测量法、光电传感器法、激光测速法和GPS测量法等。考虑到农业机械特别是水田作业机械田间作业条件下的滑转、颠簸、振动等恶劣工况、传感器测量原理与安装方式等影响因素，这些测量方法要么精度不够、要么成本偏高，还不能完全满足现代农业机械对作业速度检测的实时性、精确性和经济性要求。

发明内容

为了克服现有技术存在的缺陷，本发明提供一种农机作业行驶速度检测系统及检测方法，将GPS测速模块和霍尔传感器测速模块的测量数据进行综合处理，得到农业机械特别是水田农业机械更为精确的作业速度检测。

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

一种农机作业行驶速度检测系统，包括测速传感器组合单元和单片机测量单元，所述测速传感器与单片机测量单元连接，所述单片机测量单元用于数据采集和处理，

所述测速传感器组合单元包括GPS测速模块和霍尔传感器测速模块，所述GPS测速模块利用GPS信号测量农机纵向对地速度，所述霍尔传感器测速模块用于测量农机驱动轮转速而转换为农机作业行驶速度，

所述单片机测量单元包括数据采集模块、数据滑动平均处理模块和Kalman滤波器，所述数据采集模块用于采集GPS测速模块和霍尔传感器测速模块的测量数据，所述数据滑动平均处理模块用于对测量数据做滑动平均滤波运算，获得新的滤波结果，所述Kalman滤波器用于对霍尔传感器测速模块的测量数据的偏差进行估计和校正。

作为优选的技术方案，所述霍尔传感器测速模块包括测速转盘、测速转盘支撑座、磁钢、霍尔传感器、霍尔传感器支撑杆，所述测速转盘与农机驱动轮通过链传动方式连接，并安装在测速转盘支撑座上，测速转盘支撑座与霍尔传感器支撑杆连接，霍尔传感器支撑杆上安装霍尔传感器，所述磁钢均匀分布在测速转盘上，所述霍尔传感器的感应面对准磁钢的磁极，测速转盘带动磁钢旋转，霍尔传感器采集脉冲信号。

一种农机作业行驶速度检测方法，具体步骤如下所述：

GPS测速模块测量农机纵向对地速度v_g；

霍尔传感器测速模块测量农机驱动轮转速，并换算得到农机作业速度v_h；

对GPS测得对地速度v_g和霍尔传感器测速模块测得农机作业速度v_h分别进行滑动平均滤波处理；

将滑动平均滤波处理后的对地速度和农机作业速度作差，得到的差值作为Kalman滤波器的测量更新值；

Kalman滤波器对霍尔传感器测速模块的系统偏差进行实时估计；

将Kalman滤波器实时估计后得到的系统偏差值叠加在农机作业速度v_h经滑动平均滤波处理后的数据上，得到最终的速度估计值。

作为优选的技术方案，所述Kalman滤波器的状态量为霍尔传感器测速模块测速的系统偏差估计值，测量量为滑动平均滤波处理后的GPS测速模块速度测速数据和霍尔传感器测速模块测速数据的差值。

作为优选的技术方案，所述GPS测速模块测得的农机对地速度数据的格式为：

$GPVTG,,T,,M,0.00,N,0.00,K*4E，

其中，$GPVTG表示数据包的类型，在$GPVTG后的字段为相对于地理北的航向角，单位为度；T表示地理北，是真北的意思，$GPVTG后的字段为相对于地磁北的航向角，单位为度；M表示地磁北，在M后为GPS对地速度，单位为节；N表示速度单位节，1节的速度为每小时1海里，在N后的字段为GPS对地速度，单位为Km/h；K表示速度单位Km/h；*4E表示该数据包的异或校验和，随数据包内容的变化而变化；

单片机测量单元通过截取$GPVTG数据包中的度量单位为Km/h的对地速度字段，得到农机的GPS对地速度v_g。

作为优选的技术方案，所述霍尔传感器测速模块测得的农机作业速度v_h的计算公式为：

v_h＝3.6πD/ΔT/8，

其中，v_h的单位为Km/h，ΔT为单片机测量单元两个脉冲信号的时间间隔，πD/8表示在时间间隔ΔT内，车轮转过1/8周对应的行驶距离，D为驱动轮直径，单位为m。

作为优选的技术方案，所述对GPS测得对地速度v_g和霍尔传感器测得农机作业速度v_h分别进行滑动平均滤波处理的具体步骤是：把连续取得的N个采样值设为一个队列，队列的长度固定为N，每次采样到一个新数据放入队尾，并去掉原队首的一个数据，把队列中的N个数据进行算术平均运算，获得新的滤波结果。

作为优选的技术方案，所述Kalman滤波器的计算公式具体如下所述：

其中，x_k为k时刻的霍尔测速系统偏差估计值，x_k-1为k-1时刻的霍尔测速系统偏差估计值，为k时刻的霍尔测速系统偏差预测值，P_k为k时刻的系统过程噪声方差估计值，P_k-1为k-1时刻的系统过程噪声方差估计值，为k时刻的系统过程噪声方差预测值，Q为系统过程噪声方差经验值，R为系统测量噪声方差经验值，K_k为k时刻的增益权值，z_k为k时刻滑动平均滤波后的GPS速度数据和霍尔测速数据的差值，是Kalman滤波器在k时刻的测量更新值，

公式(1)和公式(2)构成Kalman滤波器的预测方程组；公式(3)、公式(4)和公式(5)构成Kalman滤波器的校正方程组，

作为优选的技术方案，所述Kalman滤波器对霍尔传感器测速模块的系统偏差进行实时估计的具体步骤如下所述：

将初始状态估计x₀和初始滤波误差方差阵估计P₀引入Kalman滤波器的预测方程组的公式(1)和公式(2)中，

启动Kalman滤波器，由预测方程组中的公式(1)对下一时刻状态进行预测，再由预测方程组中的公式(2)对预测的误差方差进行估计，

预测过程结束，启动Kalman滤波的校正过程，采用公式(3)解算增益矩阵，并结合预测方程组对下一时刻状态的预测结果，采用公式(4)对预测方程组的状态估计进行更新，获得当前最优估计，

采用公式(5)解算当前最优估计的误差方差值。

本发明与现有技术相比，具有如下优点和有益效果：

(1)本发明提出了农业机械作业速度检测方法，将GPS测速模块和霍尔传感器测速模块相结合测量农机作业速度，既适合旱田农业机械，也适合水田农业机械。

(2)本发明采用的Kalman滤波器可根据GPS测速模块测得的速度数据对霍尔传感器测速模块测速的系统偏差进行估计和校正，从而提供更为精确的农业机械作业速度检测值。

(3)本发明既克服了农机驱动轮滑转对霍尔传感器测速的影响，也提高了系统测速精度，同时降低了成本。

附图说明

图1为本发明农机作业行驶速度检测系统组成示意图；

图2为本发明霍尔传感器测速模块的组件安装示意图；

图3为本发明农机作业行驶速度检测方法流程示意图；

图4为本发明Kalman滤波器的数据流向图；

图5为本发明3Km/h速度条件下的不同测速结果对比曲线图；

图6为本发明4Km/h速度条件下的不同测速结果对比曲线图；

图7为本发明5Km/h速度条件下的不同测速结果对比曲线图；

图8为本发明6Km/h速度条件下的不同测速结果对比曲线图。

其中，1-测速转盘支撑座，2-测速转盘，3-磁钢，4-霍尔传感器，5-霍尔传感器支撑杆。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

实施例

本实施例提供一种农机作业行驶速度检测系统，如图1所示，包括测速传感器组合单元，单片机测量单元和便携式计算机，

所述测速传感器组合单元包括GPS测速模块和霍尔传感器测速模块，GPS测速模块利用GPS定位信号测量农机纵向对地速度，霍尔传感器测速模块通过测量农机驱动轮转速进而转换为农机作业速度，

所述单片机测量单元用于数据采集和处理，所述测速传感器与单片机测量单元连接，单片机测量单元通过数据线与便携式计算机连接，所述便携式计算机用于数据保存和数据分析。

如图2所示，所述霍尔传感器测速模块包括测速转盘2、测速转盘支撑座1、磁钢3、霍尔传感器4、霍尔传感器支撑杆5，所述测速转盘2与农机驱动轮通过链传动方式连接，并安装在测速转盘支撑座1上，测速转盘支撑座1与霍尔传感器支撑杆5连接，霍尔传感器支撑杆5上安装霍尔传感器4，所述磁钢3均匀分布在测速转盘2上，所述霍尔传感器4的感应面对准磁钢3的磁极，测速转盘2带动磁钢3旋转，霍尔传感器4采集脉冲信号。

在本实施例中，所述单片机测量单元包括数据采集模块、数据滑动平均处理模块和Kalman滤波器，所述数据采集模块用于采集GPS测速模块和霍尔传感器测速模块的测量数据，所述数据滑动平均处理模块用于对测量数据做滑动平均滤波运算，获得新的滤波结果，所述Kalman滤波器用于对测量数据的偏差值进行估计和校正。

在本实施例中，GPS测速模块的天线安装在农机驾驶室顶部的中心线上，GPS测速模块采用Ublox GPS模块；霍尔传感器测速模块的磁钢固定在驱动轮轮毂或者与驱动轮转速成线性关系的旋转体上，通过测量霍尔传感器脉冲信号的周期，转换成驱动轮转速，并结合驱动轮直径换算成对地速度；单片机测量单元采用STM32单片机测量单元，数据线采用USB-RS232数据传输线与便携计算机建立连接；所述测速传感器组合单元安装在农机后端的台架上，然后用STM32单片机进行动态数据采集和数据处理，其处理频率是1Hz；数据采集模块、数据滑动平均处理模块和Kalman滤波器采用Keil 4编程工具在Windows XP操作系统下开发完成。

本实施例还提供一种农机作业行驶速度检测方法，如图3所示，具体步骤为：

S1：GPS测速模块测量农机纵向对地速度数据，其中GPS测速模块采用Ublox GPS模块，测得农机对地速度信息，输出的NMEA 0183数据包中包含GPVTG类型，该类型数据包的格式如下：

$GPVTG,,T,,M,0.00,N,0.00,K*4E，

其中，$GPVTG表示数据包的类型，在$GPVTG后的字段为相对于地理北的航向角，单位为度；T表示地理北，是真北的意思，$GPVTG后的字段为相对于地磁北的航向角，单位为度；M表示地磁北，在M后为GPS对地速度，单位为节；N表示速度单位节，1节的速度为每小时1海里，在N后的字段为GPS对地速度，单位为Km/h；K表示速度单位Km/h；*4E表示该数据包的异或校验和，随数据包内容的变化而变化，

STM32单片机程序通过截取$GPVTG数据包中的度量单位为Km/h的对地速度字段，解算得出农机的对地速度v_g；

S2：霍尔传感器测速模块测量农机驱动轮转速，并通过驱动轮直径换算得到农机作业速度；

采用链传动方式将后轮驱动轴的旋转运动按1：1的传动比传递到固定在农机底盘的测速转盘上，产生磁场的磁钢均匀固定在测速转盘上，共有8个，如图2所示。选用A44E开关型霍尔传感器，当其磁敏感芯片靠近对准某个磁钢时，传感器输出高电平；当磁敏感芯片远离该磁钢时，传感器输入低电平。当测速转盘随驱动轮旋转时，霍尔传感器输出变周期的脉冲信号。

传感器输出的脉冲信号经信号调理电路处理后，引入STM32单片机的外部中断引脚，利用STM32单片机的定时器测取两个脉冲信号的时间间隔ΔT，在该时间间隔，车轮转过1/8周，对应的行驶距离为πD/8，其中D为驱动轮直径，单位为m。则霍尔传感器测速v_h(单位换算为Km/h)的计算公式为：

v_h＝3.6πD/ΔT/8；

S3：单片机测量单元分别对GPS测速模块和霍尔传感器测速模块测量得到的数据进行滑动平均滤波处理，以滤除测试数据中的高频噪声，

滑动平均滤波的方法是：把连续取得的N个采样值看成一个队列，队列的长度固定为N，每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据(先进先出原则)，把队列中的N个数据进行算术平均运算，获得新的滤波结果。

S4：将滑动平均滤波处理后的GPS测速模块速度数据和霍尔测速数据作差，得到的差值作为Kalman滤波器的测量更新值；

S5：Kalman滤波器对霍尔传感器测速模块的系统偏差进行实时估计；

农业机械在作业过程中，其轮胎与松软或湿滑的农田土壤表面之间会产生滑转，霍尔测速通过轮速转换为对地速度，往往存在一定的偏差，且该偏差呈现缓变的特点。据此建立Kalman滤波器，并递推其计算公式如下：

其中，x_k为k时刻的霍尔测速系统偏差估计值，x_k-1为k-1时刻的霍尔测速系统偏差估计值，为k时刻的霍尔测速系统偏差预测值，P_k为k时刻的系统过程噪声方差估计值，P_k-1为k-1时刻的系统过程噪声方差估计值，为k时刻的系统过程噪声方差预测值，Q为系统过程噪声方差经验值，R为系统测量噪声方差经验值，K_k为k时刻的增益权值，z_k为k时刻滑动平均滤波后的GPS速度数据和霍尔测速数据的差值，是Kalman滤波器在k时刻的测量更新值。

公式(1)和公式(2)构成Kalman滤波器的预测方程组；公式(3)、(4)和(5)构成Kalman滤波器的校正方程组。

如图4所示，Kalman滤波器工作时将初始状态估计x₀和初始滤波误差方差阵P₀估计引入Kalman滤波器的预测方程组中，启动Kalman滤波。先由预测方程组中的公式(1)对下一时刻状态进行预测，再由预测方程组中的公式(2)对该预测的误差方差进行估计。预测过程结束，启动Kalman滤波的校正过程，采用公式(3)先解算增益矩阵，并根据当前测量结果(针对预测方程组对下一时刻状态的预测结果)，采用公式(4)对预测方程组的状态估计进行更新，获得当前最优估计，最后采用公式(5)解算当前最优估计的误差方差值，完成一次滤波过程。

S6：将Kalman滤波器实时估计后得到的系统偏差值叠加在滑动平均滤波后的霍尔传感器测速模块测得的数据上，得到最终的速度融合估计值。

在本实施例中，所述Kalman滤波器的状态量为霍尔传感器测速模块测速的系统偏差估计值，测量量为滑动平均滤波处理后的GPS测速模块测速数据和霍尔传感器测速模块测速数据的差值。

在本实施例中，还提供对测速效果进行实车测试与评估，以下为测试例：

选择水田测试环境，人工驾驶插秧机分别以预定的前进速度3Km/h、4Km/h、5Km/h、6Km/h行走100m，采用人工秒表计时的方法测取插秧机行驶平均速度，同时采用前述作业速度检测系统按1Hz的工作频率对GPS模块和霍尔传感器进行数据采集、处理和保存。

在本测试例中，GPS速度数据和霍尔测速数据的滑动平均滤波的N值均为5；霍尔测速系统偏差估计值x_k的初始值设定为x₀＝0；系统过程噪声方差估计值P_k的初始值设定为P₀＝0；系统过程噪声方差经验值Q＝0.001；系统测量噪声方差经验值R＝0.1。

选择4种速度条件下的实测数据各一组进行分析评估。

利用Matlab软件绘制实测数据和滤波数据曲线图，得到图5、图6、图7、图8。从图中可以看出，滤波后速度曲线较GPS和霍尔速度曲线都更为平滑且更接近与人工秒表测量得到的速度均值。在插秧机起步阶段，Kalman滤波器对加速状态的适应性稍差，滤波速度较参考速度误差较大，但是进入速度稳态之后，滤波速度曲线逐渐接近参考速度并趋于稳定，显示出良好的稳定性和精确性。

根据速度曲线，从中截取稳态阶段的数据进行统计分析，得到数据分析表，如下表1所示：

表1滤波前后测速数据(Km/h)统计结果

从表1可以看出，滤波后速度数据的标准差较GPS和霍尔速度数据都有一定程度的减小，滤波后速度数据的均值较GPS和霍尔速度数据更接近参考速度值。尽管在6Km/h速度条件下测试时，GPS测速均值相比滤波测速更接近于速度参考值，但其标准差比滤波测速数据大1倍，整体性能仍不及滤波测速效果。

上述分析表明，本发明提出的基于GPS和霍尔传感器的融合滤波测速方法在识别霍尔传感器测速的系统偏差方面是有效的；在提高测速精度方面也是可行的。

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

Claims (9)

1.一种农机作业行驶速度检测系统，其特征在于，包括测速传感器组合单元和单片机测量单元，所述测速传感器与单片机测量单元连接，所述单片机测量单元用于数据采集和处理，

所述测速传感器组合单元包括GPS测速模块和霍尔传感器测速模块，所述GPS测速模块利用GPS信号测量农机纵向对地速度，所述霍尔传感器测速模块用于测量农机驱动轮转速而转换为农机作业行驶速度，

所述单片机测量单元包括数据采集模块、数据滑动平均处理模块和Kalman滤波器，所述数据采集模块用于采集GPS测速模块和霍尔传感器测速模块的测量数据，所述数据滑动平均处理模块用于对测量数据做滑动平均滤波运算，获得新的滤波结果，所述Kalman滤波器用于对霍尔传感器测速模块的测量数据的偏差进行估计和校正。

2.根据权利要求1所述农机作业行驶速度检测系统，其特征在于，所述霍尔传感器测速模块包括测速转盘、测速转盘支撑座、磁钢、霍尔传感器、霍尔传感器支撑杆，所述测速转盘与农机驱动轮通过链传动方式连接，并安装在测速转盘支撑座上，测速转盘支撑座与霍尔传感器支撑杆连接，霍尔传感器支撑杆上安装霍尔传感器，所述磁钢均匀分布在测速转盘上，所述霍尔传感器的感应面对准磁钢的磁极，测速转盘带动磁钢旋转，霍尔传感器采集脉冲信号。

3.一种农机作业行驶速度检测方法，其特征在于，具体步骤如下所述：

GPS测速模块测量农机纵向对地速度v_g；

霍尔传感器测速模块测量农机驱动轮转速，并换算得到农机作业速度v_h；

对GPS测得对地速度v_g和霍尔传感器测速模块测得农机作业速度v_h分别进行滑动平均滤波处理；

将滑动平均滤波处理后的对地速度和农机作业速度作差，得到的差值作为Kalman滤波器的测量更新值；

Kalman滤波器对霍尔传感器测速模块的系统偏差进行实时估计；

将Kalman滤波器实时估计后得到的系统偏差值叠加在农机作业速度v_h经滑动平均滤波处理后的数据上，得到最终的速度估计值。

4.根据权利要求3所述的农机作业行驶速度检测方法，其特征在于，所述Kalman滤波器的状态量为霍尔传感器测速模块测速的系统偏差估计值，测量量为滑动平均滤波处理后的GPS测速模块速度测速数据和霍尔传感器测速模块测速数据的差值。

5.根据权利要求3所述的农机作业行驶速度检测方法，其特征在于，所述GPS测速模块测得的农机对地速度数据的格式为：

$GPVTG,,T,,M,0.00,N,0.00,K*4E，

其中，$GPVTG表示数据包的类型，在$GPVTG后的字段为相对于地理北的航向角，单位为度；T表示地理北，是真北的意思，$GPVTG后的字段为相对于地磁北的航向角，单位为度；M表示地磁北，在M后为GPS对地速度，单位为节；N表示速度单位节，1节的速度为每小时1海里，在N后的字段为GPS对地速度，单位为Km/h；K表示速度单位Km/h；*4E表示该数据包的异或校验和，随数据包内容的变化而变化；

单片机测量单元通过截取$GPVTG数据包中的度量单位为Km/h的对地速度字段，得到农机的GPS对地速度v_g。

6.根据权利要求3所述的农机作业行驶速度检测方法，其特征在于，所述霍尔传感器测速模块测得的农机作业速度v_h的计算公式为：

v_h＝3.6πD/ΔT/8，

其中，v_h的单位为Km/h，ΔT为单片机测量单元两个脉冲信号的时间间隔，πD/8表示在时间间隔ΔT内，车轮转过1/8周对应的行驶距离，D为驱动轮直径，单位为m。

7.根据权利要求3所述的农机作业行驶速度检测方法，其特征在于，所述对GPS测得对地速度v_g和霍尔传感器测得农机作业速度v_h分别进行滑动平均滤波处理的具体步骤是：把连续取得的N个采样值设为一个队列，队列的长度固定为N，每次采样到一个新数据放入队尾，并去掉原队首的一个数据，把队列中的N个数据进行算术平均运算，获得新的滤波结果。

8.根据权利要求3所述的农机作业行驶速度检测方法，其特征在于，所述Kalman滤波器的计算公式具体如下所述：

其中，x_k为k时刻的霍尔测速系统偏差估计值，x_k-1为k-1时刻的霍尔测速系统偏差估计值，为k时刻的霍尔测速系统偏差预测值，P_k为k时刻的系统过程噪声方差估计值，P_k-1为k-1时刻的系统过程噪声方差估计值，为k时刻的系统过程噪声方差预测值，Q为系统过程噪声方差经验值，R为系统测量噪声方差经验值，K_k为k时刻的增益权值，z_k为k时刻滑动平均滤波后的GPS速度数据和霍尔测速数据的差值，是Kalman滤波器在k时刻的测量更新值，

公式(1)和公式(2)构成Kalman滤波器的预测方程组；公式(3)、公式(4)和公式(5)构成Kalman滤波器的校正方程组。

9.根据权利要求8所述的农机作业行驶速度检测方法，其特征在于，所述Kalman滤波器对霍尔传感器测速模块的系统偏差进行实时估计的具体步骤如下所述：

将初始状态估计x₀和初始滤波误差方差阵估计P₀引入Kalman滤波器的预测方程组的公式(1)和公式(2)中，

启动Kalman滤波器，由预测方程组中的公式(1)对下一时刻状态进行预测，再由预测方程组中的公式(2)对预测的误差方差进行估计，

预测过程结束，启动Kalman滤波的校正过程，采用公式(3)解算增益矩阵，并结合预测方程组对下一时刻状态的预测结果，采用公式(4)对预测方程组的状态估计进行更新，获得当前最优估计，

采用公式(5)解算当前最优估计的误差方差值。