CN103914066B - 农机自动导航控制器及其控制方法 - Google Patents

Abstract

本发明公开一种农机自动导航控制器及其控制方法，该控制器分为两层，下层为控制单元，上层为扩展单元；其中，控制单元主要由基于32位ARM9内核的处理器模块、存储器模块、CAN通信模块、电源管理模块组成；扩展单元主要由第1~6通用智能节点模块和GPS模块组成，通用智能节点模块和GPS模块通过插槽与扩展单元的底板连接；控制单元与扩展单元之间通过排线相互连接；本发明方法主要是结合嵌入式操作系统，控制单元采用多任务、事件触发的设计思想及基于模糊自适应纯追踪模型的路径跟踪算法，实时地计算出农机跟踪田间路径所需要的期望车轮转向角，使实际车轮转向角与期望车轮转向角保持一致，以实现农机田间自动导航控制。

Description

农机自动导航控制器及其控制方法

技术领域

本发明涉及一种农机自动导航控制器及其控制方法，该自动导航控制器主要用于轮式农机的自动导航控制，属于农机自动化及智能化领域。

背景技术

在精准农业中，智能化农机的自动导航控制精度是保证精准农业实施效果的重要因素。由于传统的人工驾驶农机的方式已经不能满足现代精准农业对农机作业的效率和精度的要求，所以出现了农机的辅助驾驶系统，例如，光靶辅助驾驶系统。该系统的出现提高了农机的工作效率和精准程度，但是农机辅助驾驶系统并不能使农机驾驶员摆脱长时间单调、劳累的重复驾驶工作和并不能使驾驶员有充足的时间去监控和操作农机具，所以农机辅助驾驶系统也不能从根本上提高农机的作业效率和精度以及减轻驾驶员的负担。为此，近年来，农机的自动导航控制技术得到了人们的重视。国外关于农机自动导航控制的研究起步较早，并取得了一些可观的成果，而国内关于农机的自动导航控制研究尚处于起步阶段，但也出现了一些研究成果。

然而，现有的农机自动导航控制器具有通用性不强、硬件结构不紧凑、软件可扩展性差和通信接口不规范等缺点。

发明内容

针对现有的农机自动导航控制器通用性不强、硬件结构不紧凑、软件可扩展性差和通信接口不规范缺点，本发明提供一种通过位姿传感器所获得的信息，实时地计算出农机跟踪预定义的田间路径所需要的期望车轮转向角，并控制转向执行设备运行，使实际车轮转向角与期望的车轮转向角保持一致，以实现农机的田间自动导航控制。

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

一种农机自动导航控制器，该自动导航控制器是基于ARM的嵌入式设备，导航控制器结构上分为两层，下层为控制单元，上层为扩展单元；其中，控制单元主要由基于32位ARM9内核的处理器模块、存储器模块、CAN通信模块、电源管理模块组成；扩展单元主要由第1~6通用智能节点模块和GPS模块组成，所述通用智能节点模块和GPS模块通过插槽与扩展单元的底板连接；控制单元与扩展单元之间通过CAN总线相互连接。

所述CAN通信模块由外扩的CAN控制器和CAN驱动器组成，CAN驱动器连接至CAN总线上，控制单元通过该模块以一种自定义的数据格式与扩展单元及与导航控制器连接的设备通信。

所述CAN控制器，采用芯片SJA1000T；CAN驱动器采用芯片CTM8251T。

所述的自定义的数据格式是指，利用ISO11783的私有广播报文对农机自动导航控制系统中的各种设备和信号定义导航专用通信报文。

所述的扩展单元集成有高精度GPS模块和通用智能节点模块，GPS模块定位精度可以达到2cm，该扩展单元主要完成GPS数据采集与处理、姿态传感器数据采集与处理、车轮转角传感器数据采集与处理以及转向控制任务。

所述扩展单元的每个通用智能节点模块均包括32位Cortex-M3处理器，采用单片机LM3S5749、串口RS232、半双工串口RS485、CAN驱动器及达林顿驱动器，用于与控制单元或外部传感器通讯和控制电机运行。

一种农机自动导航控制器的控制方法，该方法包括以下步骤：

S1、建立并执行启动任务；在启动任务中主要完成板级初始化工作并建立信息获取与解析任务、导航控制任务、开启导航任务和停止导航任务，然后挂起启动任务；执行系统调度程序，调用运行信息获取与解析任务；

S2、信息获取与解析任务从CAN总线上接收导航路径信息并保存导航路径信息；

S3、当信息获取与解析任务从CAN总线上接收到开启导航控制命令，释放开启导航控制信号量，当得到信号量控制单元执行开启导航任务，主要是对路径跟踪算法中的参数初始化、向扩展单元发送开启数据采集命令和唤醒导航控制任务；

S4、当信息获取与解析任务从CAN总线上接收到扩展单元采集的农机位姿信息为航向信息或位置信息时，对其信息保存并释放航向信息或位置信息信号量，触发导航控制任务，该任务调用基于模糊自适应纯追踪模型的路径跟踪算法计算出期望车轮转向角；

S5、导航控制任务将期望转向角通过CAN总线发送到扩展单元，由扩展单元通过转向控制算法计算出转向控制量，通过转向控制量控制外部执行设备包括电机运行，以实现期望的车轮转向角与第3个通用智能节点模块3C采集到的实际车轮转向角达到一致；

S6、当信息获取与解析任务未从CAN总线上接收到停止导航命令时，重复步骤S4~S5。

S7、当信息获取与解析任务从CAN总线上接收到停止导航命令时，释放停止导航控制信号量，控制单元执行停止导航任务，向扩展单元发送停止数据采集命令，挂起导航控制任务，导航结束。

所述信息获取与解析任务是整个程序的信息服务任务包括农机位姿信息、命令信息、导航路径信息，主要进行CAN总线上各种报文的获取、解析和根据解析结果释放相应的信号量。

所述步骤Ｓ4中的导航控制任务主要利用路径跟踪算法程序实现，导航控制任务主要根据信息获取与解析任务提供的当前农机位姿信息为位置信息和航向信息，调用路径跟踪算法算出期望车轮转向角，并发送到CAN总线上。

本发明与现有技术相比有益效果及优点：

1、本发明的自动导航控制器采用CAN通信模块使通信接口规范；通用智能节点模块和GPS模块通过插槽与扩展单元的底板连接使硬件集成度高，由于采用通用智能节点模块，通过不同需要可以设计相应功能的程序，实现功能扩展。

2、本发明的方法的主要是结合现有嵌入式操作系统，控制单元采用多任务、事件触发的程序设计思想，程序设计简单、维护方便、功能扩展性强；由于采用基于模糊自适应纯追踪模型的路径跟踪算法使农机导航控制精度大大提高。

附图说明

图1为自动导航控制器整体结构示意图；

图2为自动导航控制器控制单元方框图；

图3为自动导航控制器扩展单元中通用智能节点组成方框图；

图4为自动导航控制器软件主程序流程图；

图5为自动导航控制器信息获取与解析任务流程图；

图6为自动导航控制器开启导航任务流程图；

图7为自动导航控制器导航控制任务流程图；

图8为自动导航控制器停止导航任务流程图。

具体实施方式

以下结合附图和实施例对本发明方案进一步详细描述：

如图1所示，是一种农机自动导航控制器，该自动导航控制器是基于ARM的嵌入式设备，导航控制器结构上分为两层，下层为控制单元，上层为扩展单元；其中，控制单元主要由基于32位ARM9内核的处理器模块、存储器模块、CAN通信模块、电源管理模块组成；扩展单元主要由第1~6通用智能节点模块3_A~3_F和GPS模块4组成，所述通用智能节点模块和GPS模块通过插槽与扩展单元的底板连接；控制单元与扩展单元之间通过排线1相互连接。

该自动导航控制器由控制单元和扩展单元两部分组成。下层为控制单元，上层为扩展单元。在两个单元之间采用铜柱5相互固定，采用排线1包括CAN总线、电源线作为两个单元之间信息交互的物理通道；扩展单元上的6个通用智能节点模块3_A~3_F通过插槽与扩展单元的底板2连接，其中第1~4通用智能节点模块3_A~3_D通用智能节点模块，采用相应软件分别实现GPS数据采集与处理、姿态传感器数据采集与处理、车轮转角传感器数据采集与处理以及转向控制功能，第5~6通用智能节点模块3_E~3_F作为备用；控制单元与扩展单元之间通过CAN总线进行通信。

如图1所示，扩展单元集成有GPS模块，该模块选用NovAtel的OEMV-2GPS模块，其定位精度可以达到2cm，是一款兼容OEM4-G2L的换代产品；在该实施例中，高精度GPS模块集成在扩展单元的底板2上，为自动导航控制器提供农机的位置信息，该信息在自动导航控制器内通过串口RS232传送到扩展单元的第1通用智能节点模块上，通过该模块对GPS位置信息采集、处理及转换后发送到CAN总线上供自动导航控制器内的路径跟踪算法及自动导航控制器外的其他设备使用。

如图2所示，所述控制单元中的基于32位ARM9内核的处理器模块、存储器模块组成ARM最小系统；CAN通信模块通过数据总线ＤＢ与ARM最小系统连接；电源管理模块为ARM最小系统提供3.3Ｖ电源。

控制单元的处理器是基于32位ARM9内核的芯片LPC3250，可工作在高于200MHz的CPU频率下；该芯片LPC3250带有矢量浮点协处理器（VFP），VFP协处理器在标量模式下能将典型的计算速度提高四至五倍，在优化矢量模式下则提高的更多；为了实现低功耗，芯片LPC3250采用先进的技术来优化内在功率，并使用增强型的软件控制结构使基于功率管理的应用得到优化；该芯片LPC3250还具有一个完整的存储器管理单元（MMU）。

本发明外扩的存储器模块由32M NAND FLASH、2M NOR FLASH和64M SDRAM组成，以提供支持嵌入式操作系统多程序设计所需的虚拟存储器功能。

所述CAN通信模块由外扩的CAN控制器和CAN驱动器组成，CAN驱动器连接至CAN总线上，控制单元通过该模块以一种自定义的数据格式与扩展单元及与导航控制器连接的设备通信。

所述CAN控制器，采用芯片SJA1000T；CAN驱动器采用芯片CTM8251T。

如图2所示，由于芯片LPC3250的内部没有CAN控制器，为了实现CAN通讯功能，所以控制单元利用LPC3250的数据总线DB、与地址总线和片选信号线配合，外扩CAN控制器为芯片SJA1000T，CAN控制器和CAN驱动器采用芯片CTM8251T，构成CAN通信模块；其中，芯片SJA1000T是一款独立的CAN控制器，它增加了一种新的PeliCAN工作模式，这种模式支持具有很多新特性的CAN2.0B协议；其中，芯片CTM8251T是一款带隔离的通用CAN收发器芯片，可以连接任何一款CAN协议控制器，实现CAN节点的收发与隔离功能，该芯片CTM8251T内部集成了所有必需的CAN隔离及CAN收、发器件，主要功能是将CAN控制器的逻辑电平转换为CAN总线的差分电平并且具有DC2500V的隔离功能，该芯片符合ISO11898标准，因此它可以和其他遵从ISO11898标准的CAN收发器产品互操作。

如图2所示，电源管理模块是采用芯片SP7656，它是一款PWM控制模式的异步DC-DC降压型稳压器；芯片SP7656提供的4.5V~29V的宽范围输入电压；提供最大3A的持续输出电流也为自动导航控制器的全速工作提供了保证。

所述的自定义的数据格式是指，利用ISO11783的私有广播报文对农机自动导航控制系统中的各种设备和信号定义导航专用通信报文。

所述的扩展单元集成有高精度GPS模块和通用智能节点模块，GPS模块定位精度可以达到2cm，该扩展单元主要完成GPS数据采集与处理、姿态传感器数据采集与处理、车轮转角传感器数据采集与处理以及转向控制任务。

所述扩展单元的每个通用智能节点模块均包括32位Cortex-M3处理器，采用单片机LM3S5749、串口RS232、半双工串口RS485、CAN驱动器及达林顿驱动器，用于与控制单元或外部传感器通讯和控制电机运行。

如图3所示，扩展单元中的每个通用智能节点模块均由单片机LM3S5749、电源模块采用芯片SP7656、CAN驱动器采用芯片CTM8251T、串口RS232采用芯片MAX3232E、半双工串口RS485采用芯片MAX3485和达林顿驱动器采用芯片ULN2003A组成；其中，单片机LM3S5749是基于32位Cortex-M3内核的单片机，该单片机可以工作在最高50MHz的频率下；由于LM3S5749内部集成有完整的CAN控制器，支持CAN2.0A/B协议，所以该实施例在通用智能节点模块电路中只扩展一个CAN驱动器芯片CTM8251T；其中，电源管理模块同控制单元的电源管理模块同样，采用芯片SP7656；另外，为了驱动电机，在通用智能节点模块中还扩展了一个达林顿驱动器ULN2003A。

该农机自动导航控制器的工作原理：

农机自动导航控制器接通电源后，在农机的自动导航控制过程中，自动导航控制器根据位姿传感器采集到的位置和航向信息，实时地计算出控制农机转向执行设备的控制量，使农机的实际的车轮转向角与期望的车轮转向角达到一致，从而实现农机自动导航控制的目的。

一种农机自动导航控制器的控制方法，该方法包括以下步骤：

S1、建立启动任务；在启动任务中主要完成板级初始化工作并建立信息获取与解析任务、导航控制任务、开启导航任务和停止导航任务，然后挂起启动任务；执行系统调度程序，调用运行信息获取与解析任务；

S2、信息获取与解析任务从CAN总线上接收并保存导航路径信息；

S3、当信息获取与解析任务从CAN总线上接收到开启导航控制命令，释放开启导航控制信号量，当得到信号量控制单元执行开启导航任务，主要是对路径跟踪算法中的参数初始化、向扩展单元发送开启数据采集命令和唤醒导航控制任务；

S4、当信息获取与解析任务从CAN总线上接收到扩展单元采集的农机位姿信息为航向信息或位置信息时，对其信息保存并释放航向信息或位置信息信号量，触发导航控制任务，该任务调用基于模糊自适应纯追踪模型的路径跟踪算法计算出期望车轮转向角；

S5、导航控制任务将期望转向角通过CAN总线发送到扩展单元，由扩展单元通过转向控制算法计算出转向控制量，通过转向控制量控制外部执行设备包括电机运行，以实现期望的车轮转向角与第3个通用智能节点模块3C采集到的实际车轮转向角达到一致；

S6、当信息获取与解析任务未从CAN总线上接收到停止导航命令时，重复步骤S4~S5。

S7、当信息获取与解析任务从CAN总线上接收到停止导航命令时，释放停止导航控制信号量，控制单元执行停止导航任务，向扩展单元发送停止数据采集命令，挂起导航控制任务，导航结束。

所述信息获取与解析任务是整个程序的信息服务任务包括农机位姿信息、命令信息、导航路径信息，主要进行CAN总线上各种报文的获取、解析和根据解析结果释放相应的信号量。

所述步骤Ｓ4中的导航控制任务主要利用路径跟踪算法程序实现，导航控制任务主要根据信息获取与解析任务提供的当前农机位姿信息为位置信息和航向信息，调用路径跟踪算法算出期望车轮转向角，并发送到CAN总线上。

如图4所示，为自动导航控制器主程序流程图。当自动导航控制器上电后，控制单元内的启动程序开始运行。事先在主函数建立启动任务，在启动任务中主要进行板级初始化工作以及建立信息获取与解析任务、导航控制任务、开启导航任务和停止导航任务等任务，在完成上述任务和初始化后，启动任务将会被挂起，并执行信息获取与解析任务（如图5所示）。

当自动导航控制器从CAN总线上获得开启导航控制命令后，执行开启导航任务，对路径跟踪算法进行参数初始化、向扩展单元发送开启数据采集命令（如图6所示）。S4中当控制单元获得需要的农机位姿数据后，触发导航控制任务，调用路径跟踪算法程序计算期望车轮转向角并发送到CAN总线上，本实施例中的路径跟踪算法采用基于模糊自适应纯追踪模型的路径跟踪算法（如图7所示）。S5中第2通用智能节点模块3_B通过CAN总线获取期望的车轮转向角命令并控制转向执行设备运转，从而达到农机自动导航控制的目的。

当自动导航控制器从CAN总线上获得停止导航控制命令时，在嵌入式操作系统调度程序的管理下，执行停止导航任务，该任务的主要作用是关闭导航控制任务，使导航控制任务处于睡眠状态，不进行控制量的计算和输出并向扩展单元发送停止数据采集命令，自动导航控制结束（如图8所示）。

当信息获取与解析任务未接收到停止导航命令时，重复步骤S4~S5。

当系统程序处于非法状态时包括未接收到导航路径信息或农机位姿信息或开启导航控制命令时，程序自动进入错误处理程序，对系统重新复位。

以上参照附图公开了本发明的实施例。本发明不限于此实施例，在不脱离本发明范围对该实施例的可能的多种改变和修改，仍属于本发明所要保护的范围。

Claims (3)

1.一种农机自动导航控制器，其特征在于：该自动导航控制器是基于ARM的嵌入式设备，在结构上分为两层，下层为控制单元，上层为扩展单元；其中，控制单元主要由基于32位ARM9内核的处理器模块、存储器模块、CAN通信模块、电源管理模块组成；扩展单元主要由第1～6通用智能节点模块3_A～3_F和GPS模块(4)组成，所述通用智能节点模块和GPS模块通过插槽与扩展单元的底板(2)连接；控制单元与扩展单元之间通过排线(1)相互连接；

所述CAN通信模块由外扩的CAN控制器和CAN驱动器组成，CAN驱动器连接至排线(1)上，控制单元通过该模块以一种自定义的数据格式与扩展单元及与导航控制器连接的设备通信；

所述的自定义的数据格式是指，利用ISO11783的私有广播报文对农机自动导航控制系统中的各种设备和信号定义导航专用通信报文；

所述的扩展单元集成有高精度GPS模块和通用智能节点模块，GPS模块定位精度可以达到2cm，该扩展单元主要完成GPS数据采集与处理、姿态传感器数据采集与处理、车轮转角传感器数据采集与处理以及转向控制任务；

所述扩展单元的每个通用智能节点模块均包括32位Cortex-M3处理器，采用单片机LM3S5749、串口RS232、半双工串口RS485、CAN驱动器及达林顿驱动器，用于与控制单元或外部传感器通讯和控制电机运行。

2.根据权利要求1所述的农机自动导航控制器，其特征在于：所述CAN控制器，采用芯片SJA1000T；CAN驱动器采用芯片CTM8251T。

3.一种根据权利要求1所述的农机自动导航控制器的控制方法，其特征在于：该方法包括以下步骤：

S1、建立并执行启动任务；在启动任务中主要完成板级初始化工作并建立信息获取与解析任务、导航控制任务、开启导航任务和停止导航任务，然后挂起启动任务；执行系统调度程序，调用运行信息获取与解析任务；

S2、信息获取与解析任务从CAN总线上接收导航路径信息并保存导航路径信息；

S3、当信息获取与解析任务从CAN总线上接收到开启导航控制命令，释放开启导航控制信号量，当得到信号量控制单元执行开启导航任务，主要是对路径跟踪算法中的参数初始化、向扩展单元发送开启数据采集命令和唤醒导航控制任务；

S4、当信息获取与解析任务从CAN总线上接收到扩展单元采集的农机位姿信息为航向信息或位置信息时，对其信息保存并释放航向信息或位置信息信号量，触发导航控制任务，该任务调用基于模糊自适应纯追踪模型的路径跟踪算法计算出期望车轮转向角；

S5、导航控制任务将期望转向角通过排线(1)发送到扩展单元，由扩展单元通过转向控制算法计算出转向控制量，通过转向控制量控制外部执行设备中的电机运行，以实现期望的车轮转向角与第3个通用智能节点模块3_C采集到的实际车轮转向角达到一致；

S6、当信息获取与解析任务未从CAN总线上接收到停止导航命令时，重复步骤S4～S5；

S7、当信息获取与解析任务从CAN总线上接收到停止导航命令时，释放停止导航控制信号量，控制单元执行停止导航任务，向扩展单元发送停止数据采集命令，挂起导航控制任务，导航结束；

所述信息获取与解析任务是整个程序的信息服务任务，包括农机位姿信息、命令信息、导航路径信息，信息获取与解析任务主要进行CAN总线上各种报文的获取、解析和根据解析结果释放相应的信号量；

所述步骤S4中的导航控制任务主要利用路径跟踪算法程序实现，导航控制任务主要根据信息获取与解析任务提供的当前农机位姿信息为位置信息和航向信息，调用路径跟踪算法算出期望车轮转向角，并发送到CAN总线上。