基于预瞄追踪模型的农机导航路径跟踪控制方法

王 辉，王桂民，罗锡文，张智刚，高 阳，何 杰，岳斌斌



基于预瞄追踪模型的农机导航路径跟踪控制方法

王 辉1，王桂民2※，罗锡文1，张智刚1，高 阳2，何 杰1，岳斌斌1

（1. 华南农业大学南方农业机械与装备关键技术教育部重点实验室，广州 510642；2. 雷沃重工股份有限公司，潍坊 261206）

农机导航系统的上线性能和复杂路面抗干扰能力影响着农田作业的质量和效率，为提高农机导航系统的上线速度、上线稳定性和对复杂路面的适应性，提出了一种预瞄追踪模型的农机导航路径跟踪控制方法。该方法实质是对农机运动学模型方法的改进，针对农机运动学模型小角度线性化算法中近似条件的缺点，采用预瞄追踪辅助直线引导农机快速稳定跟踪规划路径。该文参考农机运动学模型极点最优配置算法证明过程，分3步证明了该控制方法的可行性，并通过仿真和试验验证了该方法的有效性。仿真结果显示在不同的初始位置偏差和航向偏差条件下该方法都可以迅速消除偏差以稳定跟踪规划路径，位置偏差校正曲线平滑且超调量微小，说明预瞄追踪模型方法对提高农机导航系统的上线性能和抗干扰能力是有效的。田间试验结果：在初始航向偏差为0，初始位置偏差分别为0.5、1、1.5 m条件下，上线时间分别为6.8、8.2、9.4 s，上线距离分别为6.73、8.11、9.33 m，超调量分别为5.2 、7.0 、8.5 cm；颠簸不平旱地路面直线路径跟踪的最大误差不超过4.23 cm，误差绝对值的平均值为1 cm，标准差为1.25 cm。数据表明采用该文提出的控制方法具有良好的上线和直线路径跟踪效果，满足农业机械的导航作业要求。

农业机械；模型；控制；上线；颠簸路面；路径跟踪

0 引 言

农业机械自动导航是精准农业技术体系中核心技术之一，广泛应用于耕作、播种、施肥、喷药、收获等农业生产过程[1-2]。农田作业导航系统应用中，地头的对行上线是一种较为频繁且困难的操作，路面颠簸不平现象时常发生，为提高作业质量和效率，要求农机自动导航系统上线快速稳定、直线跟踪精度高以及对颠簸复杂路面抗干扰性好。影响农机自动导航系统工作性能的两项关键技术是：位置姿态信息的获取和路径跟踪控制方法[1]。在农机位置姿态信息精度足够的情况下，路径跟踪控制方法的选择和参数整定是提高农机自动导航系统精度和稳定性的关键。

国内外众多学者对路径跟踪控制方法进行了研究，包括基于农机运动学模型的路径跟踪控制方法[3-10]、基于农机动力学模型的路径跟踪控制方法[11-17]和与模型无关的路径跟踪控制方法[18-26]。基于运动学模型的方法主要是对模型进行小角度线性化逼近，在常速假设条件下进行控制器算法设计，这样不但引进了线性化误差造成系统的稳定性变差，而且在速度变化时控制器的鲁棒性也变差。基于动力学模型的控制方法虽然考虑了农机的动力学特征，但是动力学模型参数很难在线实时辨识。在与模型无关的控制方法中，纯追踪方法前视距离的在线自适应问题至今仍没有很好解决，尤其是变速条件下的前视距离在线自适应。PID控制是一种消除误差的控制策略，应用广泛，但是控制参数整定困难，需要一定的经验和大量的参数整定试验。智能方法虽然具有仿人智能和非线性映射能力，但是其设计需要一定的经验知识和复杂的学习训练过程。

针对基于农机运动学模型控制方法中小角度线性化的缺点以及大田作业对农机自动导航系统的实际要求，本文提出一种预瞄追踪模型的路径跟踪控制方法。参考农机运动学模型小角度线性化极点最优配置算法的证明过程，将本文方法的路径跟踪过程分成3个阶段进行证明。通过仿真和试验，验证了该算法的有效性，可以满足农业机械大田作业要求。

1 导航工作原理及控制参数作用分析

农机的自动导航控制主要是对农机进行横向位置和航向偏差控制，控制农机跟踪预先设置好的作业路径，使其与设置路径之间的横向偏差和航向偏差保持在一定的精度范围之内，从而满足农业生产的需要。

现代农机自动导航系统一般由检测单元、控制单元、执行单元及监控单元四部分构成。检测单元负责实时、精准地获取农机当前的位置与姿态信息；控制单元负责根据农机当前的位姿信息和目标路径信息，按照某种控制策略计算出转向角度的期望值；执行单元的作用是驱动农机转向轮由当前值转动至轮角期望值；监控单元是导航系统的人机交互界面，主要功能是导航状态监视，系统参数设定和导航控制任务管理[1]。

控制单元是导航系统的核心，路径跟踪控制方法的选择和参数整定是提高农机自动导航精度和稳定性的关键。假设基于位置偏差和航向偏差设计期望轮角的二元线性控制律

式中θ为期望车轮转角，rad；P为车体与期望路径（当前要跟踪的规划路径）的位置偏差，m；ψ为车体与期望路径的航向偏差，rad；为位置偏差系数；为航向偏差系数；1为位置偏差轮角决策量，rad；2为航向偏差轮角决策量，rad。

式（1）至式（3）中，输入是农业机械与期望路径的位置偏差P和航向偏差ψ，输出是农业机械的轮角。通过控制轮式拖拉机的轮角修正农业机械与设定路线的位置偏差和航向偏差。期望轮角的决策量可分为两部分：位置偏差轮角决策量1和航向偏差轮角决策量2。位置偏差轮角决策量的作用是使农机沿着规划路径快速上线纠偏，其中位置偏差轮角决策量的权重越大，系统纠偏速度越快，但是可能会产生较大的控制震荡，相当于二阶系统的欠阻尼阶跃响应；航向偏差轮角决策量的作用是使农机沿着平行于规划路径的直线行走，具有预测的作用，有益于农机的稳定性，航向偏差轮角决策量的权重越大，系统纠偏速度慢，但是系统稳定性增强。相当于二阶系统的过阻尼阶跃响应[26]。当位置偏差较大时，应增大位置偏差的轮角决策量权重以保证农机快速地跟踪至设定路径，但是过大的位置偏差轮角决策量权重会造成农机上线或直线跟踪时震荡严重甚至发散。当航向偏差较大时，应增大航向偏差轮角决策量权重以保证农机快速的跟踪目标航向，但是过大的航向偏差轮角决策量权重会造成系统纠偏性能疲软导致系统在直线路径跟踪时出现大S型弯。

综上所述，农机处于不同的位置偏差或航向偏差时，合理配置位置偏差轮角决策量和航向偏差轮角决策量的权重，可有效提高农机位置偏差和航向偏差的校正速度和稳定性。实际农机导航应用中的上线问题可归结为初始位置偏差较大的直线路径跟踪问题；颠簸复杂农田路面造成农机出现甩头摆尾滑移等问题可归结为不确定因素的农机位置偏差或航向偏差瞬间变大问题。合理配置位置偏差轮角决策量和航向偏差轮角决策量的权重，可以提高农机自动导航系统的上线速度、上线稳定性和对复杂路面的适应性。

2 基于农机运动学模型的路径跟踪方法

2.1 农机运动学模型小角度线性化方法

农机运动学模型是导航控制算法设计的基础，本研究采用的雷沃M904-D拖拉机是一种前轮转向、四轮驱动的轮式农业机械。如不考虑拖拉机行走时的侧偏和地面状况等因素的影响时，基于拖拉机二轮转向的特点，可将其简化为二轮车模型，如图1所示。在导航平面坐标系下得到农机运动学微分方程，如式（4）～（5）所示。

注：Pe为车体与期望路径的位置偏差，m；ψe为车体与期望路径的航向偏差角，rad；R为车体后轮中心转弯半径，m；V为车体速度，m·s-1；θ为车轮转角，rad；L为车体轴距，m。

式中为时间，s。

直线路径跟踪中车轮转角和航向偏差ψ为小角度值（一般≤5°），故可对方程进行线性化，得到二轮转向拖拉机运动学微分方程的状态空间矩阵形式

根据方程（6）可设计全状态反馈的二元线性直线路径跟踪控制律

式中1为航向偏差反馈系数，2为位置偏差反馈系数。

将式（7）带入式（6）中即可得到闭环控制系统的状态空间方程

利用极点配置思想，将此系统的极点放置在一个理想的位置以获得最佳的系统响应。为了求出极点，可利用该系统的特征方程为0，即令

式中为拉氏变换的复频域，为二阶单位矩阵。

计算行列式得出

假设系统所需的理想极点为，则有[27]

比较式（10）和（11）2个等式，令相等的幂次方项系数相等，可得

试验证明，典型二轮转向农机车辆中，当=−1时，系统表现出良好的响应效果。因此，可得到二轮转向车辆直线路径跟踪的转向角计算公式[27]如下

实际应用中定义车辆位置偏差、航向偏差相对跟踪路径左偏值为负，右偏值为正；车辆轮角相对轮角零位左偏值为正，右偏值为负。基于此，可将式（14）改写成

2.2 影响农机运动学模型方法控制效果因素分析

从式（15）可以看出，对基于农机运动学模型的微分方程式经过小角度线性化和最优闭环极点配置后，最终得到关于位置偏差和航向偏差的二元线性控制律，位置偏差系数和航向偏差系数与速度有关。推导过程中对航向偏差ψ和车轮转角进行了小角度线性化，因此此控制律只有在位置偏差、航向偏差以及轮角摆动都很小的理想直线路径跟踪控制过程才能取得良好的控制效果。大田作业导航系统应用中，由于地头转弯调头对行上线时，人工摆放初始位置与规划行偏差较大，颠簸复杂农田路面造成农机出现甩头摆尾滑动等现象，从而导致农机位置或航向偏差瞬间变大等问题，农机不可能总是处于较为理想的直线路径跟踪控制过程中。农田实际应用环境迫切要求研究提高农机自动导航系统上线速度、上线稳定性以及对复杂路面适应性的路径跟踪控制算法。

3 预瞄追踪模型路径跟踪方法

3.1 预瞄追踪模型方法描述

预瞄追踪模型根据农机驾驶员在进行路径跟踪时的转向操作规律建立。一个合格的驾驶员对农机操作发出的控制转向指令都是遵循一定原则的。这个原则的主要目标是使农机的运动尽可能地与设定的轨迹一致[28-30]。在直线跟踪时，驾驶员预瞄农机前方路径规划直线上的一点，通过控制车轮转角跟踪该点。当农机方向和预瞄方向偏差较大时，驾驶员对车轮转角进行大角度控制，以快速校正预瞄航向偏差跟踪预瞄路径；当农机方向和预瞄方向偏差较小时，驾驶员对车轮转角进行小角度控制，以稳定地跟踪预瞄路径。

根据上述思想设计控制律如下：期望车轮转角为农机方向和预瞄方向偏差的PID控制，得到预瞄追踪模型。具体描述如下：如图2所示，建立导航平面坐标系，定义：点1为农机中心点；直线为作业规划行直线，ψ为线在导航平面坐标系下的航向角度，称为目标航向；规定点2为作业规划行直线上农机追踪的一个目标点，称作预瞄追踪点；直线12称作预瞄追踪直线，ψ为12线在导航平面坐标系下的航向角度，称作预瞄航向角；过点1向做垂线，垂足为，1之间的距离即为位置偏差P，位置偏差为有符号数，本文规定农机在期望路径前进方向右侧时的位置偏差为正（P>0），农机在期望路径左侧时的位置偏差为负（P<0）；2之间的距离称作前视距离；为车轮转角，为有符号数，轮角相对零位角度值右偏值为负（<0），轮角相对零位角度值左偏值为正（>0）；ψ为车体在导航平面坐标系下的当前航向角；ψ为农机当前航向ψ和目标航向ψ之间的偏差，称作航向偏差角；φ为农机当前航向ψ和预瞄航向ψ之间的偏差，称作预瞄航向偏差角。设计控制律，令农机的车轮转角为预瞄航向偏差φ的倍，称为控制增益。则有

由式（16）～（19）得

注：ψt为目标航向角，rad；ψc为车体航向角，rad；ψa为预瞄航向角，rad；φe为预瞄航向偏差角，rad；d为前视距离，m；P1为农机中心点；P2为预瞄追踪点；A、B为规划作业行1上的两点。

3.2 预瞄追踪模型控制方法可行性分析

预瞄航向ψ是车体中心位置点1和规划作业行直线上农机预瞄追踪点2的连线方向，控制律设计车轮转角为预瞄航向偏差φ的PID控制，其控制规则的作用是使农机在行驶过程中通过控制车轮转角从而沿预瞄追踪直线12行驶。在行驶过程中，一方面，农机通过控制车轮转角不断纠正车身方向，使车体沿12方向行驶，在消除初始导航阶段的较大预瞄航向偏差φ之后，农机车体航向ψ和预瞄航向ψ保持大体一致；另一方面，农机中心点1不断移动，沿规划作业行方向行驶的分速度使农机渐渐靠近规划作业行上线（当P<5 cm，ψ<3°时，认为农机上线）。上线时，车体航向ψ、预瞄航向ψ和目标航向ψ基本重合，控制量接近零。以上分析说明，农机的上线过程是一个渐进修正农机车体航向ψ和位置偏差P靠近作业规划行的过程，上线时位置偏差P、航向偏差ψ和车轮转角都接近为0，不会产生大超调和震荡。

上线后，农机进入规划作业行直线的路径跟踪阶段，控制规则仍然通过控制车轮转角跟踪预瞄直线12行驶，而此时预瞄直线12和作业规划行直线基本重合，说明此控制律可以通过控制农机跟踪预瞄直线12从而进行规划作业行直线的路径跟踪。

3.3 农机预瞄追踪模型的证明

将农机的路径跟踪过程分成3个阶段，第一阶段：消除初始导航阶段的大预瞄航向偏差；第二阶段：农机沿预瞄方向行驶渐进上线；第三阶段：沿作业规划行的直线路径跟踪。

第一阶段，是一个大角度的航向跟踪问题，张智刚等设计了目标航向跟踪的PID校正环节，并进行了仿真，90°航向跟踪过程中，转向控制系统动态响应的上升时间大约在4 s以内[31-32]。说明基于航向偏差的PID控制方法对运动中的农机具有较好的大角度航向跟踪能力。

第二阶段，农机沿着预瞄追踪直线12行驶，由于此阶段预瞄航向偏差角φ和车轮转角为小角度，将预瞄追踪直线12作为辅助直线，本阶段可看做是农机对预瞄追踪直线12的直线跟踪。采用农机运动学模型小角度线性化极点最优配置推导方法进行推导，此时农机车轮转角可引用二轮转向车辆直线路径跟踪的期望转向角计算式（15），此时的航向偏差为农机车体当前航向ψ和预瞄航向ψ的偏差即预瞄航向偏差φ，位置偏差为农机中心点1到预瞄跟踪直线12的距离P=0（因为点1在直线12上）。第二阶段农机的最优车轮转角计算公式为

由式（17）、（18）、（19）可知

由式（21）、（22）可得

对比预瞄追踪模型车轮转角决策式（20）和式（23）可知，取控制增益

此时两式相同，第二阶段农机预瞄追踪模型公式得证。

第三阶段，农机上线对行后，进行直线段的路径跟踪。此时位置偏差很小，故有

结合第二阶段中的取值，此时可将预瞄追踪模型车轮转角决策式（20）简化为

将式（26）与农机运动学模型小角度线性化公式（15）对比可知，取前视距离的大小等于2时，此时两式相同，从而说明预瞄追踪模型算法在直线跟踪中有良好的路径跟踪效果。

综上，可得出当控制增益2，前视距离的值等于2时，农机预瞄追踪模型算法可在农机的上线和直线路径跟踪阶段得到良好的控制效果。

4 运动学和预瞄两种方法的比较

比较运动学模型极点最优算法计算式（15）和预瞄追踪模型算法计算式（23），两式的不同之处在于位置偏差轮角决策量不同，运动学模型公式中位置偏差轮角决策量1M

预瞄追踪模型公式中位置偏差轮角决策量1A

1M随位置偏差的增大呈线性增加，1A随位置偏差的增大呈反正切函数增加，如图3所示。

图3 两种算法位置偏差轮角决策量变化趋势

位置偏差轮角决策量随位置偏差的增大呈线性增加显然是不合适的，因为位置偏差增大到一定程度时，航向偏差轮角决策量将失去作用（因航向偏差变动范围为（−p/2，p/2）），导致农机大初始位置偏差路径跟踪时的失稳或失控。预瞄追踪模型中，航向偏差轮角决策量范围和位置偏差轮角决策量范围均为（−p/2，p/2），这有益于位置偏差轮角决策量和航向偏差轮角决策量保持合适的权重，不论农机相对规划路径处于什么样的位姿，导航决策出的轮角始终有益于农机跟踪预瞄直线12，从而快速稳定追踪目标直线。

农田实际导航应用中的上线问题可归结为初始位置偏差较大的直线跟踪问题，颠簸复杂农田路面造成农机出现甩头摆尾滑动等问题可归结为不确定因素的农机位置偏差或航向偏差瞬间变大问题。以上2个问题均难以避免和预测，提高路径跟踪速度和稳定性是解决以上2个问题的途径之一。

根据上述分析，预瞄追踪模型方法不论农机相对规划路径处于何种位姿，导航决策出的轮角始终有益于农机跟踪预瞄直线，从而快速稳定追踪目标直线。因此，采用预瞄追踪模型算法可提高农机自动导航系统的上线速度、上线稳定性和对复杂路面的适应性。

5 预瞄追踪模型的仿真研究

为了验证预瞄追踪模型在上线和复杂路面直线路径跟踪中的有效性，建立了Matlab/Simulink环境下预瞄追踪模型的路径跟踪仿真模型。

实际中，上线时的初始位置偏差一般在1.5 m之内，颠簸复杂路面直线路径跟踪时的位置偏差波动范围在±0.15 m范围之内，航向偏差波动在±15°以内。

仿真中：设定速度=1 m/s，根据本文3.3章节描述，应取前视距离=2 m，经过仿真，前视距离=2 m时，系统上线和直线跟踪稳定。但为充分挖掘此算法的优势，提高导航系统的上线速度和偏差校正速度的同时不失稳定性，经过反复仿真和试验，适当减小了前视距离，选取前视距离=1.5 m，此时，偏差校正能力强且系统稳定。最大车轮转角max=25°。雷沃M904-D拖拉机轴距=2.4 m，根据式（23）设置控制增益=4.8。图4为初始航向偏差0、初始位置偏差0.5～1.5 m条件下的上线仿真路径跟踪误差变化曲线图，表1列出初始航向偏差为0（因用户进行导航上线操作时，一般会把农机车辆摆放在平行或朝向规划作业行的位置）、初始位置偏差为0.5～1.5 m条件下的上线性能参数，表2列出复杂路面直线跟踪的仿真参数统计表。（农机与规划路径的位置偏差P<0.05 m，航向偏差<3°时，称农机进入上线直线路径跟踪状态。）

图4 上线路径跟踪仿真位置偏差变化曲线

表1 前视距离为1.5 m时上线性能仿真数据统计

注：初始航向偏差为0，初始位置偏差为0.5～1.5 m。

Note: Initial yaw deviation is 0, and the initial position deviation is 0.5-1.5 m.

表2 复杂路面直线跟踪性能仿真数据统计

由图4和表1可知，基于预瞄追踪模型方法的上线路径跟踪，随着初始位置偏差增加，上线时间和上线距离增长。但从总体上看，在不同的初始位置偏差条件下都可以迅速消除横向偏差，且无超调和震荡，上线曲线变化平缓，说明预瞄追踪模型算法可有效提高农机的上线速度和稳定性。

由表2可知，基于预瞄追踪模型方法的颠簸路面直线跟踪，在出现小位置偏差、大航向偏差情况下，由于农机很短时间内来不及调整车身方向，可能会出现小幅超调。但从总体上看，在不同的初始位置偏差和航向偏差条件下都能够迅速消除横向位置偏差和航向偏差，进而稳定地跟踪目标直线，说明预瞄追踪模型方法在一定程度上可以提高农机对复杂农田的适应性。

6 试验验证及结果讨论

6.1 验证平台

为了对提出的预瞄追踪模型路径跟踪控制算法进行验证，以雷沃M904-D拖拉机作为试验农机，并在其上面安装自主研发的双天线GNSS农机自动导航控制平台，该平台由位姿传感器、导航控制器、转向执行装置、轮角传感器和监控显示器构成。其中，位置传感器姿传感器采用上海司南卫星导航公司的双天线板卡 K728，定位精度10 mm+1×10-6，航向精度0.2°/（为双天线基线长）。主天线安装在车辆驾驶室顶部的右端，从天线安装在车辆驾驶室顶部的左端（以拖拉机前进方向为正前方）。姿态传感器采用 XSENS 公司MTi-30惯性传感器，横滚、俯仰角测量精度为0.3°。惯性传感器坐标系三轴指向与拖拉机车体坐标系三轴指向相同，安装在拖拉机座椅下方。导航控制器和监控显示器采用雷沃重工股份有限公司的AGCS-II辅助自动驾驶系统配套设备。转向执行装置主件采用 EATON-KDG4V 型比例换向阀，通过加装液压管路与拖拉机转向油路并联，构成电-液转向控制机构。轮角传感器采用BEI公司的9902120CWHT 型霍尔角度传感器，12位AD采样精度，安装在左前轮的转向柱上，通过连杆将传感器转轴与前桥固连。图5为双天线GNSS农机自动导航控制平台系统结构图，图6为装备农机自动导航系统的雷沃M904-D拖拉机。

图5 农机自动导航系统结构

图6 装备农机自动导航系统的M904-D拖拉机

6.2 基于本文方法的农机上线试验

以拖拉机四轮中心投影到地面上的点为定位点，在前视距离=1.5 m，速度=1 m/s，初始航向偏差0的条件下，进行了0.5～1.5 m不同初始位置偏差条件下的上线试验，试验在华南农业大学增城教学科研基地水泥路面进行。

试验过程为：在试验路面上通过高精度GNSS采集、两点，将由其确定的直线作为农机跟踪的目标路径。启动农机并开启导航系统，通过导航系统的整体行偏移功能，将农机平移至设定的初始位置偏差位置，清除整体行偏移数值，则农机在路径一端的初始位置偏差处上线。当位置偏差稳定在0.03 m内时，通过人机交互界面发送停止导航命令，结束一次上线试验。在初始航向偏差为0、初始位置偏差为1 m时的路径跟踪效果如图7所示，不同初始位置偏差条件下上线试验数据统计结果如表3所示。

图7 1 m初始位置偏差路径跟踪效果

表3 不同初始位置偏差上线性能数据统计

从图7和表3可以看出，田间测试结果与图4和表1的仿真结果趋势一致，但由于转向执行滞后等原因，效果稍差于仿真结果，上线时间误差不超过1.17 s，上线距离误差不超过1.12 m，这样的误差是可以接受的。对预瞄追踪模型路径跟踪算法上线试验的上线时间、上线距离和超调量进行统计分析，3个参数代表了农机上线性能。由表3可知，随着初始位置偏差增加，上线时间、上线距离、超调量呈增大趋势。但从总体来看，基于预瞄追踪模型的路径跟踪在不同的初始位置偏差条件下都可以迅速消除横向偏差，超调量较小、震荡也较小、上线曲线变化相对平缓，同时具有良好的动态特性。表明预瞄追踪模型算法对提高农机导航系统的上线速度和稳定性是有效的。

6.3 基于本文方法不同路况的直线路径跟踪试验

为了验证本文方法对复杂农田的鲁棒性和适应性较好，分别在水泥路面和玉米秋收后颠簸不平的旱地路面2种路况环境下进行了直线路径跟踪试验，试验在新疆额敏县团结农场进行。试验中，设定前视距离=1.5 m，速度=1 m/s。

试验过程为：在试验地块上采集、两点，将由其确定的直线作为农机跟踪的目标路径。然后在路径线附近启动农机，并启动导航控制系统，进行自动路径跟踪控制，当农机接近路径的另一端时，通过人机界面发送停止导航命令，结束一次直线路径跟踪试验。在水泥路面和颠簸旱地路面的直线路径跟踪效果如图8所示，试验数据统计结果如表4所示。

试验数据分析：对预瞄追踪模型直线路径跟踪的横向位置偏差均值、偏差绝对值的极值、偏差绝对值的均值和标准差进行了统计分析。偏差的均值反映了导航系统的系统误差，偏差绝对值的极值和偏差绝对值的均值反映了导航控制的精度，标准差反映了导航控制的稳定性。

由表4可知，预瞄追踪模型路径跟踪控制算法在水泥路面上时，导航控制偏差的极值为2.66 cm，偏差绝对值的平均值为0.54 cm，标准差为0.67 cm。在颠簸旱地环境下，导航控制偏差的极值为4.23 cm，偏差绝对值的平均值为1 cm，标准差为1.25 cm。本文算法在水泥路面上表现出良好的控制效果，控制精度高，稳定性好；随地况条件的下降，控制效果下降，但本文算法依然表现出良好的路况适应性，在颠簸旱地路面导航控制精度依然较高，稳定性良好。基于预瞄追踪模型算法的直线路径跟踪精度高，对复杂农田地况的鲁棒性和适应性较好。考虑到颠簸地面不平对GNSS定位精度造成一定的影响，这样的跟踪精度和稳定性是可以接受的，满足农业机械的作业要求。

图8 不同路面环境下直线路径跟踪效果

表4 不同路面环境下横向位置偏差统计

7 结 论

针对农机运动学模型极点最优配置算法中，小角度线性化近似条件对较大初始位置偏差、较大初始航向偏差路径跟踪控制效果的影响，提出了一种预瞄追踪模型的路径跟踪控制算法。经证明该方法可在不同的较大初始位置偏差和航向偏差条件下，快速稳定跟踪规划路径，提高农机自动导航系统的上线速度、上线稳定性和对复杂路面的适应性。

仿真分析了预瞄追踪模型算法的上线和颠簸路面直线路径跟踪性能。数据显示：初始航向偏差为0 时，在不同的初始位置偏差条件下该方法都可以迅速消除横向偏差，上线曲线平缓无超调，表明本文提出的方法具有快速稳定的上线性能；在初始航向偏差−15°～15°范围，初始位置偏差0.15 m范围内，上线时间、上线距离和超调量均较小，表明该方法对于颠簸复杂路面的直线跟踪具有很好的鲁棒性和适应性。

进行了实车的上线和直线路径跟踪试验，在初始航向偏差为0，初始位置偏差分别为0.5、1、1.5 m条件下，上线时间分别为6.8、8.2、9.4 s，上线距离分别为6.73、8.11、9.33 m，超调量分别为5.2、7.0、8.5 cm；颠簸不平旱地路面直线路径跟踪的最大误差为4.23 cm，误差绝对值的平均值为1 cm，标准差为1.25 cm。试验结果表明本文提出的控制方法具有较好的上线和直线路径跟踪效果，根据经验可满足农业机械的导航作业要求。

[1] 胡静涛，高雷，白晓平，等. 农业机械自动导航技术研究进展[J]. 农业工程学报，2015，31(10)：1－10.

Hu Jingtao, Gao Lei, Bai Xiaoping, et al. Review of research on automatic guidance of agricultural vehicles[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(10): 1－10. (in Chinese with English abstract)

[2] 姬长英，周俊. 农业机械导航技术发展分析[J]. 农业机械学报，2014，45(9)：44－54.

Ji Changying, Zhou Jun. Current situation of navigation technologies for agricultural machinery[J]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(9): 44－54. (in Chinese with English abstract)

[3] O’Connor M L. Carrier-phase Differential GPS for Automatic Control of Land Vehicles[D]. Stanford: Stanford University, 1997.

[4] 陈军，朱忠祥，鸟巢谅，等. 拖拉机沿曲线路径的跟踪控制[J]. 农业工程学报，2006，22(11)：108－111.

Chen Jun, Zhu Zhongxiang, Torisu R, et al. On-tracking control of tractor running along curved path[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2006, 22(11): 108－111. (in Chinese with English abstract)

[5] Zhu Zhongxiang, Chen Jun, Yoshida Toyofumi, et al. Path tracking control of autonomous agricultural mobile robots[J]. Journal of Zhejiang University-Science A, 2007, 8(10): 1596－1603.

[6] 罗锡文，张智刚，赵祚喜，等. 东方红X-804拖拉机的 DGPS 自动导航控制系统[J]. 农业工程学报，2009，25(11)：139－145.

Luo Xiwen, Zhang Zhigang, Zhao Zuoxi, et al. Design of DGPS navigation control system for Dong fang hong X-804 tractor[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2009, 25(11): 139－145. (in Chinese with English abstract)

[7] 刘兆朋，张智刚，罗锡文，等.雷沃ZP9500高地隙喷雾机的GNSS自动导航作业系统设计[J]. 农业工程学报，2018，34(1)：15－21.

Liu Zhaopeng, Zhang Zhigang, Luo Xiwen, et al. Design of automatic navigation operation system for Lovol ZP9500 high clearance boom sprayer based on GNSS[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2018, 34(1): 15－21. (in Chinese with English abstract)

[8] 李逃昌，胡静涛，高雷，等. 一种与行驶速度无关的农机路径跟踪方法[J]. 农业机械学报，2014，45(2)：59－65.

Li Taochang, Hu Jingtao, Gao Lei, et al. Agricultural machine path tracking method irrelevant to travel speed[J]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(2): 59－65. (in Chinese with English abstract)

[9] 白晓平，胡静涛，高雷，等. 农机导航自校正模型控制方法研究[J]. 农业机械学报，2015，46(2)：1－7.

Bai Xiaoping, Hu Jingtao, Gao Lei, et al. Self-tuning model control method for farm machine navigation[J]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(2): 1－7. (in Chinese with English abstract)

[10] 陈军，鸟巢谅，朱忠祥，等. 拖拉机在牧草地上自动引导行走的控制[J]. 农业机械学报，2005，36(7)：104－107.

Chen Jun, Torisu R, Zhu Zhongxiang, et al. Study on automatic guidance for tractor on grassland[J]. Transactions of the Chinese Society for Agricultural Machinery, 2005, 36(7): 104－107. (in Chinese with English abstract)

[11] Bevly D M, Gerdes J C, Parkinson B W. A new yaw dynamic model for improved high speed control of a farm tractor[J]. ASME Journal of Dynamic Systems, Measurement, and Control, 2002, 124(4): 659－667.

[12] Bevly D M. High Speed, Dead Reckoning, and Towed Implement Control for Automatically Steered Farm Tractors Using GPS [D] .Stanford: Stanford University, 2001.

[13] Eaton R, Pota H, Katupitiya J. Path tracking control of agricultural tractors with compensation for steering dynamics[C]//Proceedings of the 48th IEEE Conference on Decision and Control. Shanghai, P.R. China, 2009: 7357－7362.

[14] Eaton R, Katupitiya H, Pota H, et al. Robust sliding mode control of an agricultural tractor under the influence of slip[C]//Proceedings of 2009 IEEE/ASME International Conference on Advanced Intelligent Mechatronics: 1873－1878.

[15] Zhang Q, Qiu H. A dynamic path search algorithm for tractor automatic navigation [J]. Transactions of the ASAE, 2004, 47(2): 639－646.

[16] Qiu H. Navigation Control for Autonomous Tractor Guidance[D]. Urbana-Champaign: University of Illinois at Urbana-Champaign, 2002.

[17] 冯雷. 基于GPS和传感技术的农用车辆自动导航系统的研究[D]. 杭州：浙江大学，2004.

Feng Lei. Study on Navigation System for Off-Road Vehicle Guidance based on GPS and Sensor Technology[D]. Hangzhou: Zhejiang University, 2004. (in Chinese with English abstract)

[18] 孙木楠，孙怀江. 纯追踪模型研究[J]. 机器人，2001,23(增刊1)：36－39.

Sun Munan, Sun Huaijiang. A study on pure prusuit model[J]. Robot, 2001,23(Supp.1) : 36－39.

[19] 张智刚，罗锡文，赵祚喜，等. 基于Kalman滤波和纯追踪模型的农业机械导航控制[J]. 农业机械学报，2009，40(增刊)：6－12.

Zhang Zhigang, Luo Xiwen, Zhao Zuoxi, et al. Trajectory tracking control method based on Kalman filter and pure pursuit model for agricultural vehicle[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009, 40(Supp.): 6－12. (in Chinese with English abstract)

[20] 黄沛琛，罗锡文，张智刚，改进纯追踪模型的农业机械地头转向控制方法[J]. 计算机工程与应用，2010，46(21)：216－219.

Huang Peichen, Luo Xiwen, Zhang Zhigang. Control method of headland turning based on improved pure pursuit model for agricultural machine[J]. Computer Engineering and Applications, 2010, 46(21): 216－219. (in Chinese with English abstract)

[21] 连世江，陈军，贾海政，等. 基于模糊控制的拖拉机转向跟踪控制研究[J]. 西北农林科技大学学报：自然科学版，2009，37(9)：224－228.

Lian Shijiang, Chen Jun, Jia Haizheng, et al. Heading following control of tractor based on fuzzy control[J]. Journal of Northwest A＆F University: Natural Science Edition, 2009, 37(9): 224－228. (in Chinese with English abstract)

[22] 周建军，张漫，汪懋华，等. 基于模糊控制的农用车辆路线跟踪[J]. 农业机械学报，2009，40(4)：151－156.

Zhou Jianjun, Zhang Man, Wang Maohua, et al. Path tracking for agricultural vehicle based on fuzzy control[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009, 40(4): 151－156. (in Chinese with English abstract)

[23] 陈军，朱忠祥，鸟巢谅，等. 基于神经网络的农用车辆自动跟踪控制[J]. 农业机械学报，2007，38(5)：131－133.

Chen Jun, Zhu Zhongxiang, Torisu R, et al. Automatic on-tracking control of farm vehicle based on neural network[J]. Transactions of the Chinese Society for Agricultural Machinery, 2007, 38(5): 131－133. (in Chinese with English abstract)

[24] 白晓鸽，陈军，朱磊，等. 基于神经网络的拖拉机自动导航系统[J]. 农机化研究，2010，32(4)：75－77.

Bai Xiaoge, Chen Jun, Zhu Lei, et al. Automatic navigation control system of tractor based on neural network[J]. Journal of Agricultural Mechanization Research, 2010,32(4): 75－77. (in Chinese with English abstract)

[25] 刘兆祥，刘刚，籍颖，等. 基于自适应模糊控制的拖拉机自动导航系统[J]. 农业机械学报，2010，41(11)：148－152，162.

Liu Zhaoxiang, Liu Gang, Ji Ying, et al. Autonomous navigation system for agricultural tractor based on self-adapted fuzzy control[J]. Transactions of the Chinese Society for Agricultural Machinery, 2010, 41(11): 148－152, 162. (in Chinese with English abstract)

[26] 李逃昌，胡静涛，高雷，等. 基于模糊自适应纯追踪模型的农业机械路径跟踪方法[J]. 农业机械学报，2013，44(1)：205－210.

Li Taochang, Hu Jingtao, Gao Lei, et al. Agricultural machine path tracking method based on fuzzy adaptive pure pursuit model[J]. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(1): 205－210. (in Chinese with English abstract)

[27] John W, Peake, San F. Method and Apparatus for Automatic Vehicle Guidance Using Continuous 2-D Poly-Point Path: US 813145B2[P]. 2012-05-06.

[28] 郭孔辉. 驾驶员－汽车闭环系统操纵运动预瞄最优曲率模型[J]. 汽车工程，1984，6(3)：1－16.

Guo Konghui. Driver-vehicle closed-loop simulation of handling by “preview optimal curvature method”[J]. Automotive Engineering, 1984, 6(3): 1－16. (in Chinese with English abstract)

[29] 高振海. 汽车方向预瞄式自适应PD控制算法[J]. 机械工程学报，2004，40(5)：101－105.

Gao Zhenhai. Vehicle direction preview adaptive PD control algorithm[J]. Chinese Journal of Mechnical Engneering, 2004, 40(5): 101－105. (in Chinese with English abstract)

[30] 明廷友. 智能汽车的轨迹跟随控制研究[D]. 长春：吉林大学，2016.

Ming Tingyou. Research on Trajectory Tracking Control for Intelligent Vehicle[D]. Changchun: Jilin University, 2016. (in Chinese with English abstract)

[31] 张智刚，罗锡文，李俊岭. 轮式农业机械自动转向控制系统研究[J]. 农业工程学报，2005，21(11)：77－80.

Zhang Zhigang, Luo Xiwen, Li Junling. Automatic steering control system of wheeled model farming machinery[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2005, 21(11): 77－80. (in Chinese with English abstract)

[32] 张智刚. 插秧机的DGPS自动导航控制系统研究[D]. 广州：华南农业大学，2006.

Zhang Zhigang. Automatic Guidance System Based on DGPS for Rice Transplanter[D]. Guangzhou: South China Agricultural University.

Path tracking control method of agricultural machine navigation based on aiming pursuit model

Wang Hui1, Wang Guimin2※, Luo Xiwen1, Zhang Zhigang1, Gao Yang2, He Jie1, Yue Binbin1

(1.,510642; 2..,,261206,)

Agricultural machine automatic navigation is one of the key technologies in precision agriculture technology system, the in-depth study is important in scientific research, application and social values. In this paper, we investigated the navigation problems in the application of the farm work, including slow on-line speed, bad on-line stability, and poor adaptability of bumpy complex road surface. These problems can be summarized as the speed and stability problems of track tracking in the case of large position deviation or large course deviation. Through the analysis of the work principle and control parameters function of the navigation, a conclusion is made that the correct speed and stability of the position deviation and course deviation of agricultural machinery can be improved by allocating the weight of the position deviation wheel angle decision quantity and course deviation wheel angle decision quantity reasonably. Then in this paper, we developed the navigation control algorithm based on the agricultural machine kinematics model and the pole optimal configuration theory. Because of the small angle linearization of course deviation angle and wheel angleduring the deducing, the control law can achieve good control effect only in the ideal straight path tracking control with small position deviation, course deviation, and wheel angle. Based on this, a path tracking control method of aiming pursuit model for agricultural machine navigation was proposed aiming at improving the on-line speed, stability and adaptability to complex road surface of the automatic navigation system of agricultural machine. In this method, we selected a tracking target point on the planning path of agricultural machine ahead, and tracked the target point by controlling the steering wheel angle. The direction of the agricultural machine vehicle center point to the target point was called as the aiming course. The desired steering angle would be larger when the deviation was larger between the course of agricultural machine and the aiming course, with a rapid correction of aiming course deviation to achieve the goal of fast tracking the target path. On the other hand, the desired steering angle would be smaller when the deviation was smaller between the course of agricultural machine and the aiming course, with a stable tracking to aiming path to achieve the goal of stable tracking the target path. In this model, the steering wheel angle was designed to be K times of the aiming course deviation and K was called as control gain. The length of the projection of the agricultural machine vehicle center to the target point vector on the planning path was named as the preview distance. The control gain and the preview distance were two important parameters that affected the control effect of the model. In this paper, there were three steps to prove the feasibility of the method. The control gain K and the preview distance were set up by referring to the result of the pole optimal configuration method based on the kinematic model of agricultural machine. By comparing two methods formula, the position deviation wheel angle decision quantity had a linear relationship with the position deviation in the kinematics model method and the position deviation wheel angle decision quantity had an inverse tangent function relationship with the position deviation. The inverse tangent function relation was more beneficial to maintain proper weight of position deviation wheel angle decision quantity and course deviation wheel angle decision quantity that would make the path tracking control of agricultural machinery navigation more rapid and stable. Simulation analysis results of aiming pursuit model algorithm in different position and different course deviation showed that the proposed method had a fast and stable path tracking performance and good robustness and adaptability to the navigation path tracking. The test results of agricultural machine showed that the control method proposed in this paper had a good effect in the rapid responsibility and line tracking performance. In the case of 0.5, 1, 1.5 m initial position error, the on-line time was 6.8, 8.2, 9.4 s, respectively, the corresponding travelling distance was 6.73, 8.11 and 9.33 m, respectively and the corresponding overshoot was 5.2, 7.0, 8.5 cm, respectively. The maximum error of straight-line path tracking for bumpy uneven field was not more than 4.23 cm, the mean value of the absolute value of the error was 1 cm, and the standard deviation was 1.25 cm, which satisfied the operation requirements of agricultural machine.

agricultural machinery; models; control; on-line; bumpy pavement; path tracking

王 辉，王桂民，罗锡文，张智刚，高 阳，何 杰，岳斌斌. 基于预瞄追踪模型的农机导航路径跟踪控制方法[J]. 农业工程学报，2019，35(4)：11－19. doi：10.11975/j.issn.1002-6819.2019.04.002 http://www.tcsae.org

Wang Hui, Wang Guimin, Luo Xiwen, Zhang Zhigang, Gao Yang, He Jie, Yue Binbin. Path tracking control method of agricultural machine navigation based on aiming pursuit model[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(4): 11－19. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2019.04.002 http://www.tcsae.org

2019-01-27

2019-02-10

国家重点研发计划项目（2017YFD0700400）；广东省科技计划项目（2016B020205003）

王 辉，博士生，主要从事农业机械自动导航技术研究。 Email：scau_wh@163.com。中国农业工程学会会员：06025802。

王桂民，高级工程师，主要从事农业装备的研究。 Email：wangguimin@lovol.com

10.11975/j.issn.1002-6819.2019.04.002

TP273; S24

A

1002-6819(2019)-04-0011-09