CN106546253B - 一种农田作业机械路径追踪装置及其追踪方法 - Google Patents

Abstract

本发明涉及一种农田作业机械路径追踪装置及追踪方法；包括Android上位机和下位机模块；Android上位机与下位机通过蓝牙模块连接。下位机模块包括单片机、地磁传感器、DGPS模块、蓝牙模块、Zigbee模块、Flash存储器和电源模块；下位机模块接收和存储Android上位机发出的路径信息；本发明以规划好的路径起点作为首个趋近点Q，并驱使农机向Q点靠近。当农机到达路径起点后，再通过邻域画圆寻找农机的第二个趋近点，通过不断的重复运动，实现整个路径追踪。本发明能使农田机械对已规划路径进行精确追踪，大大降低传统模式下耕作人员的劳动强度，提高农业生产效率。

Description

一种农田作业机械路径追踪装置及其追踪方法

(一)技术领域

本发明涉及一种农田作业机械路径追踪装置及其追踪方法，属于农田机械(简称农机)导航控制领域。

(二)背景技术

在目前土地资源匮乏，粮食短缺，耕地面积不断减少的背景下，发展可持续、智能化的现代农业已成为我们国家重要的研究方向。目前人们已研制出各类农用机械，其中包括土壤耕整机械、种植机械、收获机械、植物保护和管理机械等。而要想实现这些农用机械大规模、智能化的自动作业，一种农业机械路径规划和追踪装置及算法是必不可少的。

目前国内外若干科研机构对农机自动导航控制技术和农机路径跟踪算法进行了广泛而深入的研究。提出基于传统的PID控制方法、基于机器视觉的控制方法、模糊控制算法等。其中基于传统的PID控制方法，通过测量农田机械与规划路径的横向误差，通过反馈误差实时调整农机航向角，以实现路径追踪。但这种控制方式存在适应性差，参数整定困难等缺点。基于机器视觉的控制方法通过辨别两侧植株与中间道路，来确定农机前进方向。但这种方法仅能适应少量植物保护和管理机械。模糊控制算法具有一定的智能性，但需要依赖复杂的专家系统，并存在一定的控制误差。

基于以上的需要和遇到的问题，本发明提出了一种新型路径追踪装置及算法。通过在农机邻域画圆，寻找该圆与规划路径的交点，并以该点作为农机的趋近点，驱使农机向其靠拢。通过该装置及算法可以进一步提高农机适应范围，减少农机的跟踪误差，降低农机成本。

(三)发明内容

为减少农田耕作人员的劳动强度，提高农田耕作的效率和质量，本发明提出一种农田作业机械路径追踪装置及追踪方法。相比于传统同类机械，本发明具有控制简便、对复杂路况适应性好、成本低廉等优点。通过本发明，可使农田机械对已规划路径进行精确追踪，大大降低传统模式下耕作人员的劳动强度，提高农业生产效率。

该装置的工作原理如下：首先下位机模块接收和存储Android上位机发出的路径信息，并通过DGPS模块和地磁传感器获取农机当前的位置坐标和航向角。随后下位机模块以农机当前位置为圆心在导航坐标系中画圆并与规划路径交于P1、P2两点。继而下位机模块以P2作为农机的趋近点，控制农机向其靠近。在农机运动过程中，该装置将不断重复以上过程，从而驱动农机沿规划路径行驶。

一种农田作业机械路径追踪装置，包括Android上位机和下位机模块，两者均安装于现有农田作业机械前部。Android上位机与下位机通过蓝牙模块连接。

所述的Android上位机用于获取用户规划的路径信息，获取用户的操作命令，并实时显示农机当前的位置和航向角。

所述的下位机模块包括单片机、地磁传感器、DGPS模块、蓝牙模块、Zigbee模块、Flash存储器和电源模块。下位机模块用于接收Android上位机的路径信息；单片机分别与地磁传感器、DGPS模块、蓝牙模块、Zigbee模块和Flash存储器连接。下位机模块用于获取上位机模块发送的农机的位置坐标及航向角，控制农机沿规划路径运动。

所述的单片机，用于驱动各个传感器、通信模块、存储器，根据农机当前位置坐标、航向角以及规划路径计算农机的前轮转向；运动速度可通过在现有农机驱动轮上加装编码器或测速发电机获得，编码器或测速发电机与单片机连接。

所述的地磁传感器用于获取农机的航向角。所述的地磁传感器选用HMC1022，能将农机周围磁场信息转换为电压信息，再经单片机内置AD转换器转换为数字信息，从而获取农机的航向角。

所述的DGPS模块，用于获取农机的位置坐标。DGPS模块由运动GPS和GPS基准站构成。其中运动GPS安装在现有农业机械上，负责获取农机的基本位置坐标；GPS基准站安装于值班室，用于获取农机位置修正信息。两者配合使用提高位置测量精度。

所述的下位机模块与Android上位机通过所述的蓝牙模块通信。

所述的Zigbee模块用于辅助DGPS模块获取农机位置坐标，运动GPS与GPS基准站之间通过Zigbee模块通信。

所述的Flash存储器，负责存储Android上位机传来的规划路径信息。

所述的电源模块为单片机、各个传感器、通信模块及存储器供电。

本发明还涉及一种农田机械路径追踪方法，步骤以下：

S1、Android上位机接收到用户的启动命令，将规划好的路径信息发送给下位机模块。下位机模块存储规划好的路径信息，并通过GPS模块和地磁传感器获取农机当前所处的位置坐标和航向角。

S2、下位机模块以规划好的路径起点作为首个趋近点Q，并驱使农机向Q点靠近。当农机到达路径起点后，再通过邻域画圆寻找农机的第二个趋近点，当农机到达第二个趋近点再通过邻域画圆寻找第三个趋近点，以此类推，实现整个路径追踪。

具体趋近方法为：首先根据公式(1)计算出农机当前位置A(a1，a2)与趋近点Q(q1,q2)的之间的方位角β，其中：

通过地磁传感器测得农机当前航向角α，求得农机实际航向角与方位角偏差Ф，Ф＝β-α。将偏差Ф输入PID控制器，求得期望农机航向角变化率δ。根据农机运动学模型可知

式中δ---农机航向变化率(单位：rad/s)

ν---农机行驶速度(单位：m/s)

θ---农机前轮转向角(单位：rad)

L---农机轴距(单位：m)

所述A(a1,a2)、Q(q1,q2)为经纬度坐标，a1、q1为经度，a2、q2为纬度，由DGPS模块测得，单位为度。

由此利用公式(3)求得农机前轮期望转向角：

改变农机前轮转向角为θ，经过连续的积分过程，将农机航向角调整至β，从而使农机航向指向趋近点Q。

S3、农机到达趋近点Q后，下位机模块以农机当前位置为圆心、以农机轴距L为半径在导航坐标系中画圆并与规划路径交于P1、P2两点。取P2点为农机新的趋近点Q，驱使农机向其靠近，趋近方法同S3。

S4、依次重复步骤S3，直至农机走完整个规划路径。

本发明具有以下优点：

1、选择领域画圆获取趋近点的方式，相比于传统的求解农机与规划路径横向距离的方法，能够适应更多的复杂路径，进一步提高跟踪精度与跟踪速度。

2、采用地磁传感器HMC1022。其内部自带置位复位线圈，在每次测量前对内部磁阻置位，提高了输出的线性度和准确度；其内部自带的偏转线圈能很好的抵消外界磁场的干扰，减少农机自身磁场对装置的干扰。

3、选择DGPS定位。利用DGPS，从基准站获取校准值，大大提高了定位的精度；其次，由于选用了DGPS，降低了对GPS性能要求，进一步降低成本。

4、采用车载蓄电池供电，由农机自带的发电机为农机供电。将电池由装置内移至装置外，减少了蓄电池磁场对电子指南针的干扰。

(四)附图说明

图1是跟踪算法原理示意图；

图2是主程序执行流程图；

图3为转向控制系统方框图；

图中：地磁传感器反馈环节中“1”代表地磁传感器的增益；

图4为系统功能组件框架图；

(五)具体实施方式

下面结合具体图例说明该装置及算法的实施方案。

图1为跟踪算法原理示意图。

S1、Android上位机接收到用户的启动命令，将规划好的路径信息发送给下位机模块。下位机模块存储规划好的路径信息，并通过GPS模块和地磁传感器获取农机当前所处的位置坐标和航向角。

S2、下位机模块以规划好的路径起点作为首个趋近点Q，并驱使农机向Q点靠近。

具体趋近方法为：首先根据公式(1)计算出农机当前位置A(a1，a2)与趋近点Q(q1,q2)的之间的方位角β，其中：

通过地磁传感器测得农机当前航向角α，求得农机实际航向角与方位角偏差Ф，Ф＝β-α。将偏差Ф输入PID控制器，求得期望农机航向角变化率δ。根据农机运动学模型可知

式中δ---农机航向变化率(单位：rad/s)

ν---农机行驶速度(单位：m/s)

θ---农机前轮转向角(单位：rad)

L---农机轴距(单位：m)

式中A(a1,a2)、Q(q1,q2)为经纬度坐标，a1、q1为经度，a2、q2为纬度，由DGPS模块测得，单位为度。

由此利用公式(3)求得农机前轮期望转向角：

改变农机前轮转向角为θ，经过连续的积分过程，将农机航向角调整至β，从而使农机航向指向趋近点Q。

S3、农机到达趋近点Q后，下位机模块以农机当前位置为圆心、以农机轴距L为半径在导航坐标系中画圆并与规划路径交于P1、P2两点。取P2点(路径数组中位置在后者)为农机新的趋近点Q，驱使农机向其靠近，趋近方法同S3。

S4、依次重复步骤S3，直至农机走完整个规划路径。

图4为系统功能组件框架图。该装置主要由Android上位机和下位机模块构成，其中下位机模块主要由单片机、地磁传感器、DGPS模块、蓝牙模块、zigbee模块、Flash存储器、电源模块组成。

所述的Android上位机基于百度地图而开发，获取用户在地图上规划的路径信息，并实时显示农机当前的位置和航向角。

所述的单片机，选用恩智浦(原飞思卡尔)公司生产的Kinetis K60系列,该型号单片机基于ARM Cortex-M4内核并自带多路串口、AD和FTM组件，能够满足该系统资源要求。另外，K60系列单片机工作温度在-40℃至105℃，能够满足农田作业时的高温环境。

所述的地磁传感器，选用honeywell公司生产的HMC1022磁阻传感器。该地磁传感器原理为磁阻的各向异性，通过搭建惠斯通电桥，将磁场方向信息转换为电压信息，其测量精度可达0.001745rad。不仅如此，HMC1022还自带偏置线圈，用以抵消农机自身的电磁干扰。在这里，地磁传感器主要用于获取农机的航向角。

所述的DGPS模块，由两个U-BLOX公司生产的NEO-6MGPS模块组成。选用DGPS工作模式，可以将精度提高至5cm。且成本较低。

所述的蓝牙模块，选用HC-05系列蓝牙模块，内置2.4GHz天线,

传输距离10m，主要用于下位机模块与Android上位机通信。

所述的Zigbee模块，选用美国Digi公司生产的XBee PRO S2系列Zigbee模块。该模块采用鞭状天线，传输速率为250kbps，室外传输距离可达3.2km。该模块主要用于辅助DGPS模块获取农机位置坐标，负责运动GPS与GPS基准站通信。

所述的Flash存储器，选用华邦电子的W25Q16BVSSIG 16MB存储器。该存储器与单片机之间通过SPI协议通信，主要负责存储Android上位机发送的路径信息。

所述的电源模块采用车载12V蓄电池，通过LM2596 DC-DC变换器变换至5V，再经AMS1117变换至3.3V，为各功能组件分别提供12V、5V和3.3V电源。

该系统主板通过铜柱固定于10*8*4m³的集线盒中。各功能模块通过排针排母固定在主板上。Zigbee模块通过铜柱单独固定于集线盒中，通过排线与主板连接。在集线盒左侧打孔，孔直径为1.2cm，用于引出Zigbee的天线。在集线盒右侧打两个孔，孔直径均为0.5cm，分别负责引入电源线和引出前轮转向信号线。整套控制装置安装于改进的农用拖拉机头部。

本发明的保护范围不限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。

Claims (3)

1.一种农田作业机械路径追踪装置，其特征在于包括Android上位机和下位机模块，两者均安装于现有农田作业机械前部；Android上位机与下位机通过蓝牙模块连接；

所述的Android上位机用于获取用户规划的路径信息，获取用户的操作命令，并实时显示农机当前的位置和航向角；

所述的下位机模块包括单片机、地磁传感器、DGPS模块、蓝牙模块、Zigbee模块、Flash存储器和电源模块；下位机模块用于接收Android上位机的路径信息；单片机分别与地磁传感器、DGPS模块、蓝牙模块、Zigbee模块和Flash存储器连接；下位机模块用于获取上位机模块发送的农机的位置坐标及航向角，控制农机沿规划路径运动；

所述的单片机，用于驱动各个传感器、通信模块和存储器，根据农机当前位置坐标、航向角以及规划路径计算农机的前轮转向；运动速度通过在现有农机驱动轮上加装编码器或测速发电机获得，编码器或测速发电机与单片机连接；

所述的地磁传感器用于获取农机的航向角；

所述的DGPS模块，用于获取农机的位置坐标；DGPS模块由运动GPS和GPS基准站构成；其中运动GPS安装在现有农业机械上，负责获取农机的基本位置坐标；GPS基准站安装于值班室，用于获取农机位置修正信息；两者配合使用提高位置测量精度；

所述的下位机模块与Android上位机通过所述的蓝牙模块通信；

所述的Zigbee模块用于辅助DGPS模块获取农机位置坐标，运动GPS与GPS基准站之间通过Zigbee模块通信；

所述的Flash存储器，负责存储Android上位机传来的规划路径信息；

所述的电源模块为单片机、各个传感器、通信模块及存储器供电。

2.如权利要求1所述的一种农田作业机械路径追踪装置，其特征在于所述的地磁传感器为HMC1022。

3.如权利要求1所述的一种农田作业机械路径追踪装置的追踪方法，其特征在于步骤以下：

S1、Android上位机接收到用户的启动命令，将规划好的路径信息发送给下位机模块；下位机模块存储规划好的路径信息，并通过GPS模块和地磁传感器获取农机当前所处的位置坐标和航向角；

S2、下位机模块以规划好的路径起点作为首个趋近点Q，并驱使农机向Q点靠近；当农机到达路径起点后，再通过邻域画圆寻找农机的第二个趋近点，当农机到达第二个趋近点再通过邻域画圆寻找第三个趋近点，以此类推，实现整个路径追踪；

具体趋近方法为：首先根据公式(1)计算出农机当前位置A(a1，a2)与趋近点Q(q1,q2)的之间的方位角β，其中：

通过地磁传感器测得农机当前航向角α，求得农机实际航向角与方位角偏差Ф，Ф＝β-α；将偏差Ф输入PID控制器，求得期望农机航向角变化率δ；根据农机运动学模型可知

式中δ---农机航向变化率，单位：rad/s

ν---农机行驶速度，单位：m/s

θ---农机前轮转向角，单位：rad

L---农机轴距，单位：m

所述A(a1,a2)、Q(q1,q2)为经纬度坐标，a1、q1为经度，a2、q2为纬度，由DGPS模块测得，单位为度；

利用公式(3)求得农机前轮期望转向角：

改变农机前轮转向角为θ，经过连续的积分过程，将农机航向角调整至β，从而使农机航向指向趋近点Q；

S3、农机到达趋近点Q后，下位机模块以农机当前位置为圆心、以农机轴距L为半径在导航坐标系中画圆并与规划路径交于P1、P2两点；取P2点为农机新的趋近点Q，驱使农机向其靠近，趋近方法同S3；

S4、依次重复步骤S3，直至农机走完整个规划路径。

Images