基于模糊Stanley模型的农机全田块路径跟踪算法研究

崔冰波 孙 宇 吉 峰 魏新华 朱永云 章少岑

(1.江苏大学农业工程学院， 镇江 212013； 2.江苏大学现代农业装备与技术教育部重点实验室， 镇江 212013)

0 引言

农业机械(简称农机)智能化是无人农场和智慧农业的重要支撑，而自动导航是智能化农机的核心技术[1]。农机自动导航为疫情防控和粮食安全提供了重要保障。路径跟踪算法控制农机沿预定轨迹实现自主行驶，是农机田间无人化自主作业的重要环节[2-3]。

农业机械常用路径跟踪控制方法包括PID控制、纯追踪控制、Stanley控制、神经网络控制和最优控制等[4-6]。BORAH等[7]提出一种基于滚动时域的改进纯追踪控制算法，解决农机高速作业下跟踪效果不理想问题。MOGENS等[8]基于线性时变模型预测控制研究农机在特定约束条件下的路径跟踪控制，取得了较高的跟踪精度。张华强等[9]提出一种基于粒子群优化算法实时确定前视距离的改进纯追踪控制算法，提高了直线跟踪精度。王辉等[10]提出一种基于预瞄追踪模型的农机路径跟踪控制算法，提高了农机复杂路面抗干扰能力。

Stanley模型是一种基于横向跟踪误差的非线性反馈函数，其横向跟踪误差以指数收敛于零[11-12]，但其在农业机械自动导航上的应用报道较少。为提高农业机械全田块自主作业导航精度和稳定性，本文提出一种基于改进Stanley模型的路径跟踪控制算法，采用模糊算法实现控制增益系数的自适应调整，提高农机自动转弯路径跟踪精度，并减小初始误差较大时的上线距离。以移动小车为试验平台，进行联合收获机全田块作业路径跟踪试验，以验证算法的有效性。

1 Stanley路径跟踪算法

如图1所示，Stanley模型前轮期望转角的控制输入由两部分组成，分别为横向偏差与航向偏差

δ(t)=δe(t)+δθ(t)

(1)

式中δ(t)——期望转角

δe(t)——横向偏差引起的期望转角

δθ(t)——航向偏差引起的期望转角

图1 Stanley模型示意图Fig.1 Schematic of Stanley model

单独考虑横向偏差影响，横向偏差越大前轮期望转角越大，假设车辆预期轨迹在距离前轮d(t)处与给定路径上最近点切线相交，根据几何关系得出非线性比例函数

(2)

式中k——增益系数e(t)——横向偏差

v(t)——行驶速度

单独考虑航向偏差的影响，前轮偏角与给定路径切线方向一致，此时前轮期望转角等于车辆航向与最近路径点切线方向之间的夹角

δθ(t)=θe(t)

(3)

式中θe(t)——航向偏差

综合考虑这两方面的影响，得出前轮期望转角函数为

(4)

2 参数自适应方法

常用的参数自适应方法有智能搜索算法、神经网络算法和模糊算法等[13]。智能搜索算法主要包括遗传算法、粒子群优化算法以及差分进化算法[14-16]。神经网络算法根据激励函数的不同可分为BP神经网络、RBF神经网络、PNN神经网络以及GRNN神经网络等[17-20]。相较于其他参数自适应方法，模糊算法具有计算量小、实时性好、鲁棒性强、适合单片机系统等优点[21]，本文采用模糊算法对Stanley模型增益系数进行动态调整。

由式(4)可知，Stanley模型增益系数决定横向偏差对车轮转角影响的权重。增益系数较大可以快速减小横向偏差，使农机快速行驶至规划路径上，但是过大的增益系数会造成直线跟踪的振荡。增益系数较小可以使农机运行平稳，降低算法收敛时的稳态跟踪误差，但初始大横向误差条件下收敛速度较慢。因此针对不同的横向偏差，应选取时变的增益系数，尤其是对初始大误差下上线距离和转弯对行精度要求较高的全田块路径跟踪。除此之外，航向偏差与横向偏差对车轮转角的影响具有方向性。当横向偏差与航向偏差对车轮转角作用方向相同时，可适当减小增益系数，使控制更加平稳。反之，当横向偏差与航向偏差对车轮转角作用方向相反时，可适当增大增益系数，提高算法收敛速度。

考虑农机实际工作情况，以横向偏差和航向偏差为输入变量构建隶属度函数，限定前轮最大转角为35°，初步制定模糊推理规则。以横向偏差最大值和标准差为观测量，进行多次试验，寻找不同误差输入下最优的增益系数，修正模糊推理表。

2.1 隶属度函数构建

如图2所示，将横向偏差的论域确定为[-3 m,3 m]。横向偏差右大、右中、右小、零、左小、左中、左大分别对应NP、NM、NS、ZO、PS、PM、PB。

图2 横向偏差隶属度函数Fig.2 Membership function of lateral deviation

图3 航向偏差隶属度函数Fig.3 Membership function of heading deviation

如图3所示，将航向偏差的论域确定为[-30°,30°]。航向偏差右大、右中、右小、零、左小、左中、左大分别对应NP、NM、NS、ZO、PS、PM、PB。

如图4所示，将增益系数的论域确定为[0, 1.2]。增益系数零、小、中、大分别对应ZO、PS、PM、PB。

图4 增益系数隶属度函数Fig.4 Membership function of gain coefficient

2.2 模糊推理表设计

模糊推理表具体规则为：航向偏差较小时，横向偏差越大，增益系数越大；航向偏差与横向偏差对车轮转角影响方向相同，航向偏差越大，增益系数越小；航向偏差与横向偏差对车轮转角影响方向相反，航向偏差越大，增益系数越大；航向偏差较大时，与横向偏差对前轮转角影响方向相同，选较小增益系数；航向偏差较大时，与横向偏差对前轮转角影响方向相反，选较大增益系数。据此，共总结49条模糊推理规则，并通过试验进行修正，制定如表1所示的模糊推理规则表。

2.3 解模糊化方法确定

解模糊化常用的方法有最大隶属度法、重心法和加权平均法等[22]。重心法在工业上使用广泛，输出平滑，因此选择重心法作为解模糊化的方法，其计算公式为[23]

表1 模糊推理规则Tab.1 Rule base for fuzzy logic control

(5)

式中u0——输出系数m——级数

uk——各级系数μk(uk)——隶属度

3 试验与结果分析

3.1 试验平台搭建

为了验证算法的有效性，搭建移动小车仿真试验平台。基站接收机型号为NovAtel OEM719、流动站接收机型号为OEM718D。双天线RTK模式下定位精度1 cm+1ppm，其中1ppm表示离基站距离每超过1 km定位误差增加1 mm。天线基线长2 m时定向精度0.08°，测量更新率5 Hz。电机驱动器型号为RMDS405，支持30 A电流。编码器型号为欧姆龙E6B2，分辨率可达3 600 p/r(即转一圈输出3 600脉冲)。

移动小车试验平台结构如图5所示，接收机通过天线和电台同时接收卫星信号和基站差分改正数，进行RTK定位解算，实时输出高精度位置与航向信息至导航上位机。上位机通过计算，输出当前位置与规划路径间的横向偏差、航向偏差。下位机根据导航偏差计算出期望转向角，通过CAN总线以频率5 Hz将控制信息发送至电机驱动器。电机驱动器将控制信息转换为电压，并实时接收编码器的反馈，组成闭环控制系统，以频率100 Hz控制电机。

图5 路径跟踪算法验证平台Fig.5 Setup for path tracking system verification1.天线 2.电台 3.接收机 4.编码器 5.电机 6.上位机 7.下位机 8.显示屏

3.2 路径规划方案

为验证算法对全田块农机路径跟踪不同阶段适应性，模拟收获机作业进行全田块路径跟踪试验，其中收获机作业路径采用外圈回字形，内圈AB线辅助导航的混合导航方式，不仅解决无动力换向功能收获机的全田块自动驾驶难题，而且有效提高收获机作业效率，其全田块路径规划方案如图6所示。

图6 联合收获机全田块路径规划示意图Fig.6 Field path planning schematic of combine harvester

收获机按顺序依次进行收获作业，当剩余田块宽度小于收获机自动转弯所需尺寸时，在最后一行直线段向驾驶员发出信号，并自动切换至人工辅助驾驶模式。收获机全田块作业过程分为上线阶段、直线跟踪阶段和曲线跟踪阶段，其中直线跟踪阶段在算法改进前已有较好的跟踪效果，故本次试验重点研究上线阶段以及曲线跟踪阶段。

3.3 试验结果分析

小车直线跟踪阶段平均行驶速度约为2.5 m/s，过渡到曲线跟踪阶段时速度自动降为1 m/s左右。图7为上线阶段行驶轨迹，由图7可知，改进后小车上线速度明显加快，初始横向偏差3 m时上线距离不超过5 m。图8为曲线跟踪阶段小车行驶轨迹，曲线段横向偏差与航向偏差始终处于变化状态。固定增益系数的Stanley模型难以取得良好的路径跟踪效果，采用模糊算法改进后其增益系数根据导航偏差自适应调整，路径跟踪精度明显提高。

图7 大初始偏差上线阶段行驶轨迹Fig.7 Guided trajectory of large initial lateral error

图8 转弯曲线跟踪阶段行驶轨迹Fig.8 Curves of trajectory of turning

图9 模糊Stanley模型横向误差Fig.9 Lateral error of fuzzy-based Stanley model

采用模糊算法改进前后的路径跟踪横向误差如图9所示。直线跟踪时，模糊Stanley模型减小了路径跟踪的上线距离，10 m以后2种方法的稳态误差无明显差异，最大横向误差小于3 cm，标准差小于1 cm。曲线跟踪阶段的稳态误差改善明显，路径跟踪误差最大值由28 cm下降为3 cm，满足转弯对行精度要求。试验过程发现直线路径跟踪存在明显的稳态误差，其可能原因包括转向系统机械间隙和天线基线偏角引起的系统误差。前者在农业机械上影响可以忽略，后者可以通过作业现场标定进一步补偿[24]。

图10 全田块路径跟踪轨迹Fig.10 Field path tracking trajectory

为验证所提模糊Stanley模型的全田块自主作业适应性，基于图6所示的联合收获机全田块作业路径开展试验分析。为改善无人自主收获作业效率，采用混合路径规划实现其全田块导航，其中外圈为回字形路径，内圈采用AB线导航作业。路径跟踪轨迹如图10所示，由图10可知，路径跟踪算法整体跟踪效果良好，具体跟踪误差见表2。采用模糊Stanley模型不仅改善了路径跟踪的上线速度，也显著减小了转弯掉头时曲线路径跟踪误差，满足农业机械全田块路径跟踪的需求。

3.4 联合收获机田间试验

以约翰迪尔C230型轮式联合收获机为试验对象进一步验证算法有效性，在其上安装双天线RTK定位定向系统并进行电控化改造。C230型收获机作业幅宽为5 m，转弯半径约为8 m，其收获作业时导航系统配置与小车试验平台一致，全田块路径规划采用图6所示方案。行驶控制系统包括电动方向盘、转角传感器、电动油门以及卸粮、脱粒相关控制系统等，如图11所示。其中电动方向盘为上海联适EMS2型转向驱动单元，工作电流可达10 A，具有扭矩大、精度高等特点，IP65级防尘防水，适合农机恶劣作业环境。转角传感器型号为北京天海科公司的DWQCAB-V-CH，其线性度为0.02%FS，角分辨率0.022°，转角传感器安装在收获机转向立轴正上方，与转向中心立轴直连。

表2 收获机全田块作业路径跟踪误差Tab.2 Field path tracking error of combine harvester

图12 联合收获机路径跟踪轨迹Fig.12 Path tracking trajectory of combine harvester

联合收获机田间无人驾驶作业结果如图12所示，其中直线段平均作业速度约为0.8 m/s，自动转弯时平均作业速度约为0.6 m/s。受大型联合收获机转弯半径限制和手自动切换控制可靠性影响，目前仅验证了外圈回字形自动转弯收获作业。由试验结果可知，稳定作业时直线和曲线路径跟踪误差均小于15 cm，然而受侧滑和田垄等复杂田间土壤环境影响，收获机最大横向跟踪误差为0.63 m。对比分析移动小车试验结果可知，大型联合收获机双天线安装误差、复杂土壤条件和转向系统时滞对Stanley模型增益系数的适应性有较大影响。

4 结论

(1)为改善农机全田块路径跟踪算法适应性，基于模糊算法提出一种改进Stanley模型的路径跟踪控制算法，实时调整路径跟踪控制增益系数。场地试验表明额定作业速度下车辆直线与曲线最大路径跟踪误差均小于3 cm。

(2)采用移动小车对收获机全田块作业路径进行路径跟踪试验，结果表明本文算法显著改善Stanley模型路径跟踪的适应性，初始横向误差3 m时，上线距离由12 m下降到5 m，满足农业机械全田块自动导航作业要求。

(3)联合收获机田间试验表明，天线安装精度影响路径跟踪稳态误差，且导航系统位姿更新率和稳定性制约着农业机械自主作业的可靠性和作业效率。