葡萄园拖拉机自动导航系统设计与试验

杨舜禾，杨术明，李茂强，王乾

(1.宁夏大学机械工程学院，宁夏 银川 750021；2.北方民族大学机电工程学院，宁夏 银川 750021)

葡萄是一种经济价值较高的水果，但葡萄不同于大田作物，其生产技术流程较为复杂.在葡萄的生长过程中需要伴随起藤、施肥、剪枝、收获、埋藤等一系列作业.其中拖拉机是葡萄园内作业的主要动力来源，但由于驾驶员的驾驶水平存在差异，从而导致了作业生产效率低、农机燃油经济性差等不良现象的产生[1].所以需要大力发展农业机械自动导航技术，以解决上述弊端并顺应精准农业的发展趋势.

由于GPS技术的巨大成功，美国最早展开农业机械卫星导航技术研究，O′Connor等基于约翰迪尔7800型拖拉机开发了一套GPS导航系统[2]，在工厂环境下进行模拟试验，导航精准度可以达到厘米级；Tosaki等研制一种在果园环境使用的鼓风喷雾机器人，采用模糊控制方法引导机器人前行[3]，导航精准度高，但是需要事先埋设电缆；GUEVARA等开发了基于机器视觉与GPS融合的农田作物检测机器人，通过图像处理完成作物行跟踪，使用模糊控制实现导航[4]，用于低矮作物;国内对于农机自动导航技术的研究起步较晚，但近些年来发展非常迅速[5].张志刚等在久保田插秧机上开发了基于DGPS的电子罗盘的导航控制系统[6]，并在水泥路面上进行模拟实验；周俊等研发了基于视觉的农用轮式移动机器人导航系统[7]，但采用绿篱模拟农作物行开展实验；王晨等设计了一种农用车辆视觉导航系统[8]，但视觉系统受光照条件限制.

现阶段的农机自动导航系统大多针对大田作业.大田作业前，通常使用激光平地仪对地面进行平整.但由于埋藤、起藤等作业对葡萄园地面破坏较大，使得拖拉机行走时极易偏航，且葡萄行间两侧有支架，当拖拉机偏离航线时，需要快速修正以免拖拉机碰撞篱架.现有导航系统对葡萄园作业的适应性不强.所以，开发一套适应葡萄园复杂地形的拖拉机自动导航系统，对葡萄产业的发展起到重要的作用.本文针对葡萄园地面坑洼不平特点，设计了一套适用于葡萄园内的拖拉机导航系统，该系统可实现拖拉机在葡萄园导航时快速驶入预定轨迹，并且能快速修正因复杂地形造成拖拉机偏航，使拖拉机回到预定路径.

1 材料与方法

1.1 导航系统总体方案设计

葡萄的种植模式为上架种植，即将葡萄藤固定在葡萄支架上生长.单臂篱架每行设1个架面，行距2.5 m时，架高1.5～1.8 m；行内每间隔4～6 m设一立柱，柱上每隔50 cm拉一道横向铁丝.葡萄种植模式如图1.

1：土壤；2：铁丝；3：葡萄枝藤；4：单臂篱架；5：重物.1：Soil；2：Iron wire；3：Grape vine；4：One-arm hedge；5：Heavy weight.图1 葡萄篱架示意图Figure 1 Schematic diagram of grape hedge

导航方式采用A、B点导航，即在两行葡萄之间确定拖拉机的行驶起点及终点(既A、B点)，将两点的连线作为导航线的基准线.然后，根据葡萄种植的行间距及行数，生成一系列平行于基准线的导航线作为拖拉机的工作路径.导航方式示意图如图2所示.

1：导航基准线；2：单臂篱架；3：铁丝；4：生成预设路径.1：Navigation reference line；2：One-arm fence；3：Iron wire；4：Generate preset path.图2 导航方式示意图Figure 2 Navigation mode diagram

当拖拉机行驶在葡萄行间时，通过拖拉机实时的位置信息与预设工作路径作比较，可计算出拖拉机的横向偏差与航向偏差.导航系统根据偏差计算出拖拉机前轮期望转角并将指令传输给执行机构，执行机构驱动拖拉机前轮转动，从而减小偏差实现自动导航功能，导航系统原理框图如图3所示.

1.2 导航系统硬件设计

葡萄园拖拉机自动导航控制系统主要由拖拉机、GNSS接收机和天线、控制器、转向控制装置、前轮转角检测装置.本文选用昊田454-G型拖拉机，外形尺寸为3 500 mm×1 560 mm×1 965 mm，轴距1 750 mm.GNSS接收机为北斗专用接收电台，可接受差分信号范围30 km，差分定位精度：水平精度(±1 cm+1 mm/km)，垂直精度()±2 cm+1 mm/km).控制器采用ASUS笔记本.考虑葡萄园地面坑洼不平会导致拖拉机颠簸，转向控制装置选用MOOG SD系列伺服电机，伺服电机与方向盘通过1∶2的齿轮传动，而非齿轮直接传动.避免由于颠簸带来的冲击载荷造成啮合齿轮碰撞损坏伺服电机.伺服电机的分辨率为4 000 C/Rev，输出转矩0.28～0.45 N/m,空载最大转速10 400 r/min.前轮转角检测装置选用邦赫BH38角度传感器，分辨率为分辨率0.088°，传输频率1 Hz.试验平台搭建如图4所示.

图3 导航系统原理框图Figure 3 Navigation system block diagram

图4 试验平台Figure 4 Experimental platform

1.3 导航系统软件设计

导航系统软件选择Qt Creator作为导航软件开发环境.Qt Creator是一个用于Qt开发的轻量级跨平台集成开发环境[9-10].具有定性好、运行效率高，实时性好与人机交互性强等特点.并且Qt Creator的模块化程度非常高，可重用性较好，对于用户开发各种软件非常方便.因此，选择Qt Creator作为导航软件开发环境，可以满足导航控制系统的设计功能要求.

1.3.1 总体功能结构 导航控制系统的显示界面是人机交互的窗口，用于显示导航过程各传感器信息及拖拉机行驶状态等.导航系统软件可分为人机交互模块、串口通讯模块、数据处理模块、数据储存模块.导航控制系统可实现的具体功能如图5所示.

1.3.2 人机交互模块 良好的人机交互界面可以使导航系统功能实现更加直观且容易操作.根据导航系统功能的模块化分析，对导航系统软件界面进行设计.系统软件的主界面如图6所示.

1.3.2 串口通讯模块导航平台拥有多个传感器，系统需要保持各个传感器数据相互独立，且进行数据处理时要保证数据同步，所以串口通讯模块采用多线程模式编程[11].本文使用QT5所提供的串口编程模块Qt-SerialPort[12-14].在编写各个传感器数据读写串口时，通过调用QT5提供的QtThread函数创建线程.使用readyRead函数监视是否有数据进入读写缓冲区，并对接收到数据进行校验，判断是否为一个完整数据;为保证线程之间的同步，在对串口数据进行写入与读取时，使用互斥类QMutex对象来访问同一个内存空间.

图5 导航控制系统功能图Figure 5 Functional diagram of the navigation control system

图6 导航控制系统主界面Figure 6 Navigation control system main interface

1.3.3 数据处理模块 串口通讯模块将校验后的完整数据传输给数据处理模块.数据处理模块首先将接收到的GPS数据进行预处理，因为GPS接收到的数据是在大地坐标系下的数据，不能直接用于导航，需要先将大地坐标下的数据转换成地方空间直角坐标系下的数据[15].然后与预设路径参数进行对比得出实时横向偏差及航向偏差，由导航控制算法根据偏差计算出前轮期望转角，主要代码如下：

void GpsThread::llh2enu(double lon,double lat,double h，double heading)

{

……

gps2enu(lon,lat,h,e,n,u);//坐标转换

m_headingerror=calHeadingError(heading,m_targetheading);//计算航向偏差

m_diserror=calDisError(e,n);//计算横向偏差

angle=fuzzy(headingerror,-m_diserror);//计算前轮期望转角

……

}

然后，导航系统根据计算出的前轮转角控制伺服电机顺时或逆时针转动带动方向盘转动，系统同时监测前轮安装的角度传感器，当前轮转动到期望角度时，则停止伺服电机转动，主要代码如下：

void MotorThread::controlmotor()

{

……

m_AngleSensor->GetAngle(angle);//获取当前前轮转角

If(fabs(targetangle-angle)<0.2)//判断前轮是否达到目标转角

{

curTxStr=“S”

m_mutex.lock();

QSerialPort*pTxSerial = m_pCurSerial;

pTxSerial->write(curTxStr.toLocal8Bit());

\发送停止指令

m_mutex.unlock();

}

……

}

1.3.4 数据储存模块 为了实现后期理论验证及误差分析，拖拉机作业的预设路径及车辆行驶轨迹和系统参数信息，都需要进行保存.导航系统通过QT5模块的QFile的setFileName函数创建文本文件形式保存所需的数据.主要代码如下：

void MotorThread::SetSaveFile(const QString &setfilename)

{

……

QString txFileName = setfilename + "motor.csv";//建立文本名称

m_rxfile.setFileName(rxFileName);//建立文本

……

}

1.4 导航系统控制方法

1.4.1 建立模型 建立合理的拖拉机系统模型不仅是设计导航控制算法的前提，也是实现拖拉机导航的基础.建立拖拉机的动力学模型十分复杂，与其动力学特性与车辆轮胎、作业环境及各种系统误差有紧密的关系[16]，但这些数据在拖拉机作业时很难采集.所以本文基于拖拉机的运动学特性建立了拖拉机的运动学模型，并且将拖拉机轮胎视为刚体，不考虑轮胎与地面的侧向滑动，将拖拉机4轮运动学模型简化为2轮运动学模型，如图7所示.

图7 2轮运动学模型Figure 7 2-wheel kinematics model

其中，x，y，o为拖拉机所在坐标系，β为拖拉机的航向角，α为拖拉机前轮转向角，v为拖拉机行驶速度，l为拖拉机的前后轮轮距.

根据2轮简化模型可得出拖拉机的运动学方程：

(1)

在建立拖拉机的运动模型后，需要确定拖拉机的控制算法.模糊控制是一种将专家经验以模糊规则的形式表达的控制策略,通过模糊推理对系统的实时状态做出正确的决策,且无需精准的农机模型[17-19].但葡萄园植保作业造成地面坑洼不平，导致拖拉机驶入预定路径难度较大且当沿预定路径行驶时极易航，传统模糊控制的适应性差，可能导致消除误差响应过慢，使得拖拉机碰撞到葡萄篱架.所以本文采用变论域模糊控制解决此问题.

1.4.2 变论域的模糊控制原理 变论域模糊控制是在模糊控制的基础上，通过被控对象的输入参数对模糊控制器的论域经行实时调整，实现了模糊控制系统的自适应.模糊论域的实时调整，可以提高模糊控制的自适应能力.同时模糊论域的实时调整，降低了初始论域所需的准确性，克服了由于经验不足所设定的初始论域偏差较大，导致控制精度不足的情况[20].

1.4.3 变论域模糊控制器设计 模糊控制的核心是模糊控制器的设计，模糊控制器由模糊化、模糊规则库、模糊推理机和去模糊化4个部分组成.

模糊化是把输入量xi和yi转换成模糊量，Xi=[-E,E]和Yi=[-U,U]分别是xi和yi的取值范围，也称为论域.而变论域是指Xi和Yi可以随着xi和yi的变化而变化.记为：

Xi(xi)=[-αi(xi)E,αi(xi)E]Yi(yi)=[-βi(yi)U,βi(yi)U]

(2)

式中，αi(xi)和βi(yi)(i=1,2,…,n)为输入论域的伸缩因子.本文采用如下的伸缩因子：

(3)

式中，γ∈(0，1)，ki为模糊控制器的输入量，E为初始论域的最大值.

由于模糊控制器的输入论域随着收缩因子φi(ki)变化而变化，模糊控制器的输出论域也需乘上相应的收缩因子ωi(ji)，从而保证模糊控制器的自适应性提高.根据模糊控制规律可知，输出论域变化趋势与输入论域趋势相同，即输出量的收缩因子ωi(ji)与输入量的收缩因子φi(ki)呈线性关系.所以输出量的收缩因子ωi(ji)=φi(ki)+b.

本文模糊控制器的输入变量为拖拉机行驶过程中的横向偏差和航向偏差，输出变量为拖拉机前轮转角.首先，设置输入变量的初始论域.然后，系统根据横向偏差和航向偏差计算收缩因子来调整输入变量的论域.论域随着输入的横向偏差和航向偏差减小而减小.在模糊规则形式不变的前提下，论域收缩相当于增加规则，提高了系统的控制精度[21].当拖拉机偏离预定路径较远时，论域收缩较小，此时系统控制精度不高但是系统响应幅度较大，拖拉机可以快速驶向预定路径.当拖拉机偏离预定路径交近时，论域收缩较大，此时系统控制精度提高，拖拉机行驶更加平稳.

试验平台选用拖拉机的宽度为1.56 m，试验场地葡萄种植行间距为2.5 m，拖拉机在在葡萄行间左右可活动距离为0.47 m，但考虑拖拉机从地头驶入葡萄行间的初始位置不确定，则在可活动距离的基础上，再增加一段距离作为作为拖拉机横向偏差的初始论域范围.规定当拖拉机位于导航线的右侧时横向偏差为正，左侧时为负.则横向偏差的初始论域为[-0.7,0.7](m);量化等级为{-7,-6，-5，-4,-3，-2，-1,0,1,2,3,4,5,6,7}，量化因子为Kd=0.1;试验场地地头宽6 m,拖拉机车身长3.5 m，拖拉机初始航向与葡萄种植方向偏差不会大于45°，否则拖拉机无法正常驶入葡萄行间.规定航向偏差前轮右转时为正，左转时为负，基本论域为[-45，45](°)，量化等级与横向偏差相同，量化因子为 Kh= 0.15.将前轮期望转角作为模糊控制器的输出变量，基本论域为[-15,15](°)，量化因子为 Kh= 3.量化等级与横向偏差相同.

模糊控制规则也称模糊控制算法，是模糊控制器设计的核心.本文根据拖拉机与预设路径位置关系，结合司机驾驶习惯及经验建立了如表1所示的模糊控制规则.

表1 模糊控制规则

其中NB、NM、NS、ZE、PS、PM、PB表示7个模糊子集，分别为负大、负中、负小、零、正小、正中、正大，且7个模糊子集的隶属度函数均采用三角形隶属度函数.D、H、θ分别表示拖拉机的位置偏差、航向偏差和期望前轮转角.

本文采用加权平均法，将模糊控制器输出期望前轮转角的模糊量转化为精确量.

(5)

式中，xi(i=1,2,…,n)为论域X上的元素，μU(xi)为xi所对应模糊集合U的隶属度函数.

1.5 试验验证

为了对导航算法及开发的系统软件进行验证，选取宁夏农林科学院西侧的葡萄种植基地作为试验场地，葡萄园的种植行间距为2.5 m,葡萄枝藤高2 m,种植行数为15行.设定拖拉机的初始横向偏差为10 cm，由于葡萄园复杂地形容易导致拖拉机偏离预定路径，控制系统需要提高控制频率以快速修正偏差，使拖拉机回到预定路径.控制周期选为0.4 s，参照拖拉机在葡萄园内起藤时的作业速度，拖拉机的行驶速度分别选为0.25、0.35 m/s.试验场地如8所示.

图8 试验现场Figure 8 Experimental field

2 结果与分析

拖拉机分别以0.25、0.35 m/s速度的路径跟踪误差曲线图如图9～10所示.

图9 拖拉机行驶速度0.25 m/s误差曲线Figure 9 Tractor travel speed 0.25 m/s error curve

由图9可知，当拖拉机以0.25 m/s的速度行驶在葡萄园中时，导航系统控制拖拉机在行驶3.5 m后，消除初始横向偏差，拖拉机驶入预定轨迹，并控制拖拉机较为稳定的行驶，控制横向偏差在±6 cm范围内，最大横向偏差绝对值为5.36 cm,横向偏差平均值为2.46 cm，均方差为3.04 cm.

图10 拖拉机行驶速度0.35 m/s误差曲线Figure 10 Tractor travel speed 0.35 m/s error curve

由图10可知，拖拉机以0.35 m/s的速度行驶在葡萄园中时，导航系统控制拖拉机在行驶1.5 m后，消除初始横向偏差，拖拉机驶入预定路径，并且当拖拉机遇到坑洼地势偏离预定路径，导航系统控制拖拉机在行驶2.5 m后，消除横向偏差回到预定路径，控制横向偏差在±7 cm范围内，最大横向偏差绝对值为14.68 cm,横向偏差平均值为2.77 cm，均方差为3.28 cm.导航偏差结果统计如表2所示.

表2 导航偏差统计结果

3 结论

本文以葡萄园拖拉机导航控制为研究对象，设计开发了一套导航控制系统，为了解决葡萄园复杂地形造成拖拉机容易偏离预定路径及偏离路径后需快速修正偏差以免碰撞篱架的问题，该系统采用变论域模糊控制方法.并且根据葡萄种植行间距及拖拉机宽度和地头预留位置，设置变论域模糊控制参数.经实验验证，当拖拉机在葡萄园内分别以0.25、0.35 m/s速度沿预设路径行驶时，导航系统控制拖拉机行驶横向位置偏差绝对值的最大值分别为5.36、14.68 cm，平均偏差的绝对值分别为2.46、2.77 cm，均方根偏差值依次为3.04、3.28 cm.当拖拉机遇到坑洼地形出现较大的位置偏差时，导航控制系统能快速、准确地纠正偏差，使拖拉机继续沿预定路径行驶.表明此控制系统基本满足拖拉机在葡萄园内导航作业需求.

分析试验数据可发现导航控制效果随拖拉机辆速度提高而下降，本文后续研究将考虑拖拉机速度对控制效果的影响，对现有的控制算法进行改进，使导航控制系统对速度具有自适应能力，以提高导航控制系统的鲁棒性、精准性和实用性.