基于改进纯追踪模型的农机路径跟踪算法研究

张华强 王国栋 吕云飞 秦昌礼 刘 林 宫金良

(1.山东理工大学机械工程学院， 淄博 255049； 2.山东理工大学生态无人农场研究院， 淄博 255049)

0 引言

农业机械(以下简称农机)自动导航控制技术已经成为解放生产力、实现农业自动化的重要技术。2017年英国哈珀亚当斯大学建立了全球首家应用自动拖拉机进行播种、喷洒的无人农场，美国爱荷华州超级农场的智能拖拉机已经能够完成种植、喷洒农药的全部工作。我国正处在传统农业向现代农业转型的重要时期，农业科技对我国农业未来发展起着至关重要的作用[1]。农机路径跟踪控制是农业装备自动导航技术的关键，我国研究人员对路径跟踪方法进行了研究，主要方法包括PID控制方法、模糊控制方法、A*控制方法、纯追踪模型等[2]。文献[3]采用RTK-DGPS定位技术设计了直线跟踪的PID导航控制系统，在农机行进速度为0.8 m/s时，最大横向误差小于0.15 m，平均误差小于0.03 m。文献[4]采用模糊控制方法，路径跟踪最大误差小于0.1 m，最小误差小于0.05 m。PID控制方法的缺点是参数整定困难，需要进行大量的试验；模糊控制方法进行直线行驶的精度在很大程度上依赖于专家的经验和知识以及操作员的操作模式，因而精度有限。

纯追踪模型是一种模拟人驾驶行为的几何追踪模型，通过计算曲率将车辆从初始位置移动到目标位置，纯追踪模型的核心是确定一个合适的前视距离。文献[5]通过试验证明，取轮距为前视距离时能得到较好的跟踪效果，但得到的精度有限。文献[6]通过BP神经网络动态调整能够获得更加合适的前视距离，且具有较高的鲁棒性，但此方法需要大量的优质样本，并且容易陷入局部最优。

为了提升农机作业时直线行驶的精度，本文提出一种通过PSO算法动态确定纯追踪模型前视距离的路径跟踪算法，进而得到更高精度的跟踪轨迹。以东方红1104-C型拖拉机为试验平台构建农机自动导航系统，进行农田播种试验，以验证算法的有效性。

1 直线行驶路径跟踪方法设计

在自动导航控制系统中，为了对农机直线行驶路径进行精确跟踪，构建了路径跟踪控制器，其由卡尔曼滤波模块、PSO模块、纯追踪模型模块组成，如图1所示。图中ξd为经过卡尔曼滤波模块的横向误差、ξθ为经过卡尔曼滤波模块的航向角误差；Ld为PSO算法自适应实时确定的前视距离。

图1 路径跟踪系统原理图Fig.1 Path following system

1.1 运动学建模

农机在路径跟踪的作业过程中，符合低速行驶、小转向角度微调行驶路径特点，假设轮胎与地面不产生横向力，轮胎不产生横偏角，则车辆遵循阿克曼转向模型[7]。阿克曼转向几何示意图如图2所示，图中δ1为外轮转向角、δ2为内轮转向角、L为轴距、r为转向半径、b为轮距、δ为前轮平均转向角。

图2 阿克曼转向几何示意图Fig.2 Ackerman steering geometry diagram1.前轮外轮 2.前轮内轮

对图2中几何关系分析，内外轮转向角关系为

cotδ2-cotδ1=b/L

(1)

前轮平均转向角为

δ≈L/r

(2)

农机通常以二轮模型进行运动学分析，即假设农机在平滑路面行驶，轮胎与地面只产生纵向压力，可将农机模型简化为二轮模型。轮胎为刚性轮，不产生侧向滑动，二轮模型实际是简化后的阿克曼模型。二轮模型示意图如图3所示。图中，A(xs,ys)表示前轮轴心坐标，B(xd,yd)表示后轮轴心坐标，α为农机航向角，δs为前轮转向角，vs和vd分别表示前、后轮速度。

图3 二轮模型示意图Fig.3 Schematic of two-round model

由二轮模型可得前轮转向角为

δs=arctan(L/R)

(3)

根据转向角与后轮线性速度关系可得

(4)

根据前后轮几何约束可得农机运动学模型为

(5)

1.2 改进纯追踪模型构建

1.2.1卡尔曼滤波器设计

由于农田地面凹凸不平、定位系统自身精度不足等原因，系统的定位误差精度必然会受到影响。为得到精度更高的农机横向误差和航向误差，本文设计了卡尔曼滤波对定位误差进行平滑处理。

农机在作业过程中通常以低速行进，设农机行驶过程中恒速且加速度为零。卡尔曼滤波器的状态向量为Xt=(xt,yt,vt)，其中xt和yt为t时刻农机的平面位置，vt为农机行驶速度[8]。

对连续系统进行离散处理后状态向量为

Xt+1=φtXt+wt

(6)

式中φt——系统状态转移矩阵

wt——离散时间白噪声

设θt为t时刻农机航向角、Δθt为t时刻农机航向偏移误差、T为采样周期，则卡尔曼滤波器[9]的状态转移矩阵为

(7)

离散处理后的观测方程为

Zt=HtXt+Vt

(8)

式中Zt——t时刻系统的观测向量

Ht——观测矩阵

Vt——高斯白噪声

通过前期仿真试验，并将数据进行多元统计分析，观测噪声协方差Rt[10]确定为0.05，过程激励噪声协方差Qt确定为0.01。设计的卡尔曼滤波器能够最大化地降低其他噪声信号对真实信号的干扰[11]，保证了农机当前位姿信息的可靠性。

1.2.2纯追踪算法建模

纯追踪模型是一种几何追踪模型，按照人的驾驶习惯，通过计算跟踪农机当前位置到目标位置的曲率进行工作，并能够依据目标点距离对当前误差进行动态调整[12-14]。纯追踪模型示意图如图4所示。图中，C(xg,yg)表示目标点坐标，2α为到达目标点所转过的航向角，k为农机转弯曲率，d为当前位置距目标路径的横向误差，θ为航向误差，m为预设路径。

图4 纯追踪模型示意图Fig.4 Schematic of pure tracking model

根据图4，由正弦定理得

Ld/sinα=2R

(9)

根据目标点横纵坐标表达式及勾股定理得

(10)

由图4目标点横坐标与横向误差几何关系得

(11)

将式(3)、(9)～(11)联立求得前轮转向角关系式为

(12)

式(12)中，横向误差d和航向误差θ由DGPS定位信息计算后通过卡尔曼滤波平滑处理后得到。只有前视距离Ld未知，下面将设计一种改进的PSO算法，根据农机实时误差信息对前视距离Ld进行动态调整[15-16]。

2 前视距离确定方法

由式(12)可知，当前视距离Ld较大时，农机以较小曲率k向预定轨迹跟踪路径，但系统响应较慢；当前视距离Ld较小时，农机以较大曲率k向预定轨迹跟踪路径，但容易产生欠阻尼响应，出现控制振荡。

PSO算法[17]是一种群体智能算法，假设每个微粒在n维空间中是没有质量和体积的个体，并以一定的速度在约束空间下飞行。粒子群在n维空间进行飞行搜索时，Xi=(xi1,xi2,…,xin)为微粒个体i的当前位置，Vi=(vi1,vi2,…,vin)为微粒个体i当前的速度。

PSO算法的基本进化公式为

vij(s+1)=ωvij(s)+c1r1j(s)(pij(s)-xij(s))+
c2r2j(s)(pgj(s)-xij(s))

(13)

xij(s+1)=xij(s)+vij(s+1)

(14)

式中j——微粒个体的维数

s——迭代次数

ω——惯性权重系数

pi——微粒个体所经历的最好位置

pg——所有微粒群体所经历的最好位置

c1、c2——步长

r1、r2——(0,1)之间的随机数

惯性权重系数ω具有平衡粒子群全局和局部搜索的能力，能够减小粒子群算法对最大速度的需要。当惯性权重系数ω较大时，粒子群全局搜索能力较强[18]；当惯性权重系数ω较小时，粒子群局部搜索能力较强。目前粒子群搜索方法主要采用惯性权重系数ω随着进化而线性减小的方法，但由于粒子群算法早期收敛的速度较快[19]，所以应尽快使粒子群进行局部搜索，因此惯性权重系数ω线性减小的方法求解效率较差。

2.1 PSO算法惯性权重系数的改进

为尽快使粒子群进行局部搜索，应加速惯性权重系数ω的递减速度，故本文设计了一种惯性权重系数ω从ωmax到ωmin以凹函数递减的方法

ω=e-s/smax

(15)

式中smax——最大迭代次数

根据式(15)得到不同迭代次数的凹函数和线性函数的变化对比，如图5所示。

图5 不同线性曲线权重系数ω变化对比Fig.5 Comparison of weight coefficients ω of different linear curves

由图5可知，当迭代次数s增大时，凹函数比线性函数的惯性权重系数降低更快，能够使PSO算法更快地进入局部搜索，使粒子群具有更好的局部搜索能力，且能够获得更好的求解效率。

通过多次对比试验，当惯性权重系数ω采用凹函数递减时，在迭代260次左右，粒子群便能获得最优解；而当惯性权重系数采用线性函数递减时，当迭代400次左右时，粒子群才能获得最优解。因此凹函数递减方法能够较大幅度提高粒子群算法计算的效率，减少计算时间。

2.2 适应度函数设计

适应度函数[20]是粒子群算法的关键，合适的适应度函数能够使粒子快速找到最优解。均方根误差是衡量实际值与真值之间误差的重要依据，本文采用横向误差d与航向误差θ的均方根之和作为粒子群寻优的主要参考。

横向误差d的评价参数为

(16)

航向误差θ的评价参数为

(17)

由式(16)、(17)为基础设计适应度函数

F=λF1+(1-λ)F2

(18)

式中N——工作时长M——采样时刻

λ——权重

d(t)、θ(t)——t时刻的横向误差和航向误差

所设计的适应度函数F能够根据横向误差d，调整横向误差d和航向误差θ的决策权重，具有良好的适应性。

2.3 权重函数设计

在基于PSO算法的纯追踪模型中，当横向误差d较大时，前视距离Ld主要通过横向误差d进行决策；当横向误差d较小时，前视距离Ld主要通过航向误差θ进行决策，故设计权重为[21]

λ=loga((d+1)/U) (U≥(dmax+1)/a)

(19)

式中a——权重函数的底数，a>1

U——调整参数

dmax——最大横向误差

根据式(19)得到权重示意图如图6所示。

图6 权重示意图Fig.6 Schematic of weight coefficient

由图6可知，当横向误差d增大时，权重加快增大，即横向误差d在适应度函数F中比重快速增大，航向误差θ比重减小，当横向误差增大到0.25 m时，权重λ为0.65，即适应度函数为F=0.65F1+0.35F2，适应度函数F主要通过横向误差d进行决策。

2.4 PSO算法参数设置及流程

在PSO算法中，最大迭代次数t为300，粒子群个数为50，步长c1、c2均为1，随机数r1、r2均为0.9，权重系数最大值ωmax为1、最小值ωmin为1/e，前视距离Ld范围为[1 m,7 m]，粒子速度v范围为[0.7 m/s,1.5 m/s]。PSO算法流程如图7所示。

图7 PSO算法流程图Fig.7 PSO algorithm flow chart

将PSO算法中惯性权重系数进行改进，设计了凹函数式(15)使粒子群能够尽快进行局部搜索，提高了PSO算法的求解效率；设计了具有权重调整的适应度函数式(18)，能够使粒子群根据横向误差和航向误差调节决策权重[22]，以便能够获得更加合适的前视距离。

本文通过上述改进PSO算法确定前视距离，步骤如下：

(2)计算每个粒子的适应度。根据农机的实时位姿信息，通过所设计的适应度函数式(18)确定每个粒子，即每个前视距离的适应度。

(3)比较每个粒子与个体最优位置pi适应度，若更好，则将该粒子作为个体最优位置。

(4)比较每个粒子与全局最优位置pg适应度，若更好，则将该粒子作为全局最优位置。

(5)根据PSO算法的基本进化公式(13)、(14)，以及本文所设计的凹函数式(15)对粒子的位置和速度进行进化。

(6)在达到最大迭代次数前，重复步骤(2)～(5)；达到最大迭代次数后，停止运算，输出全局最优位置，即最优前视距离。

3 试验与结果分析

3.1 试验平台搭建

通过前期仿真试验，本文所设计的基于改进纯追踪模型的农机路径跟踪算法具有良好的适应性，能够以较高精度进行直线行驶。为了进一步验证该方法有效性，以东方红1104-C型拖拉机为试验平台构建农机试验系统，如图8所示，并于山东省淄博市朱台镇试验田进行了实地验证。

图8 播种试验系统平台Fig.8 Seeding test system platform1.GNSS天线 2.电动方向盘 3.自动驾驶控制器 4.角度传感器 5.播种机

东方红1104-C型拖拉机参数见表1。

表1 东方红1104-C型拖拉机参数Tab.1 Dongfanghong 1104-C tractor parameters

试验平台系统组成如图9所示，主要由平台端、自动驾驶端、控制执行端构成[23]。其中自动驾驶端的核心是基于改进纯追踪模型的路径跟踪控制器[24]。

图9 试验平台系统组成图Fig.9 Composition chart of test platform system

图10 试验平台系统结构示意图Fig.10 Schematic of test platform system

路径跟踪过程首先由平台端(RTK-DGPS)采集边界信息数据生成路径，随后指令下发给自动驾驶端[25]，自动驾驶端解析路径文件及控制文件，根据位置判断下发控制指令，通过CAN总线控制执行端，执行端完成启动、停止、加速、转弯、农具升降。试验平台系统结构如图10所示。

3.2 前视距离对比试验

设置起始点A坐标为(20 m,13 m)，农机行驶速度为0.7 m/s，试验行驶长度为70 m，行距为2.5 m，采用鱼尾型调头方式，设定当农机位于预设轨迹北侧时，横向误差d>0；位于南侧时，横向误差d<0。试验路径规划如图11所示。

图11 试验路径规划Fig.11 Test path planning

在相同的试验路径下，对比固定前视距离和基于改进的纯追踪模型动态调整前视距离的方法进行对比，设定初始固定前视距离为轮距2.0 m。

图12 固定前视距离的行驶轨迹Fig.12 Driving trajectory with fixed-looking distance

图13 固定前视距离的横向误差Fig.13 Lateral deviation with fixed-looking distance

图14 动态调整前视距离的行驶轨迹Fig.14 Dynamic adjustment of driving trajectory of forward-looking distance

图15 动态调整前视距离的横向误差Fig.15 Dynamic adjustment of lateral deviation of forward-looking distance

图12、13表示在固定前视距离下的行驶轨迹和横向误差变化；图14、15表示动态前视距离下的行驶轨迹和横向误差变化。对比试验结果，在相同规划路径下，农机上线后，固定前视距离的行驶轨迹横向误差波动较大，最大横向误差为0.15 m，行驶距离大于20 m以后，最大横向误差为0.05 m；而动态前视距离行驶轨迹横向误差变化较小，最大横向误差为0.09 m，当行驶距离大于20 m以后，最大横向误差为0.02 m。动态前视距离变化如图16所示。

图16 动态前视距离变化曲线Fig.16 Dynamic forward-looking distance change

通过对比试验可以确定：基于改进的纯追踪模型动态调整前视距离的方法，在直线跟踪的效果以及初始调整的距离上明显优于固定前视距离的方法。

3.3 试验结果分析

在改进纯追踪模型的农机路径跟踪算法下，以农机试验平台进行播种试验，对农机行驶的6条轨迹的横向误差进行统计分析，统计结果如表2所示。

表2 横向跟踪误差统计结果Tab.2 Statistical results of horizontal tracking error m

根据统计结果，除第1、3行外，其他行最大横向误差不大于0.02 m，这说明本文所设计系统方法具有良好的稳定性和精度。第1行最大误差0.09 m，是由于农机在前5 m时初始对准产生的；第3行最大误差-0.025 m，超过0.02 m，是因为田间作业不确定因素较多，有的轨迹路线上会出现凹坑、碎石等。由于第1行初始校准误差较大，统计分析意义不大，所以只分析第2行及以后各行。不同轨迹横向误差示意图如图17所示。

图17 不同轨迹横向误差变化曲线Fig.17 Lateral deviation of different trajectories

由图17可知，除部分轨迹因田间不可避免因素产生较大误差，其余轨迹均能保证在预定轨迹0.02 m范围内波动，动态调整前视距离能够使横向误差快速消除，精度大幅提高。

4 结论

(1)针对农机直线行驶的精度问题，提出了一种改进纯追踪模型的农机路径跟踪算法，基于PSO粒子群算法改进纯追踪模型，根据农机实时位姿信息动态调整前视距离。采用该方法的试验结果表明：当行驶距离超过5 m、前进速度为0.7 m/s时，农机行驶的最大横向误差为0.02 m，与常规方法相比，基于PSO算法的改进纯追踪模型能够提高直线行驶的精度。

(2)设计了以东方红1104-C型拖拉机为试验平台的农机自动导航系统，并进行农田播种试验。针对信号噪声影响GPS导航精度问题，设计了卡尔曼滤波器，保证了农机位姿信息的精度。

(3)改进的PSO算法参数能够使粒子群尽快进行局部搜索，减少运算时间，优于权重系数ω线性减小的方法。