基于虚拟雷达模型的履带拖拉机导航路径跟踪控制算法

刘志杰 王小乐 任志刚 毛文菊 杨福增

(1.西北农林科技大学机械与电子工程学院， 陕西杨凌 712100；2.农业农村部北方农业装备科学观测试验站， 陕西杨凌 712100；3.陕西省苹果重点实验室， 陕西杨凌 712100)

0 引言

我国传统果园具有种植密度大、作业通道低矮狭窄等特点，普通拖拉机难以进入进行作业，而小型履带拖拉机具有体积小、地形适应能力强、转向灵活等优点，应用较为广泛[1]，但仍存在操作人员劳动强度大、工作环境恶劣等问题。在劳动力短缺、人工成本不断提高的情况下，只有发展自动化、无人化的果园作业机械才能保证水果产业的可持续发展[2]。导航技术是农机自动化的关键技术之一[3-6]，路径跟踪作为导航控制系统的核心技术，对提高作业质量和效率起决定性作用[7-10]。因此，研究小型履带拖拉机路径跟踪算法对提高果园生产自动化和智能化具有重要意义。

国内外学者针对轮式车辆的导航路径跟踪控制算法进行了广泛而卓有成效的研究[7,9,11-17]。履带车辆与轮式车辆的转向原理不同，轮式车辆的路径跟踪算法难以直接应用于履带车辆，HUANG等[18]针对差速驱动履带机器人设计了一种前馈PID控制算法，提高了路径跟踪精度；贾全等[19]提出了一种基于航向预估模型的控制算法，在一定程度上降低了履带拖拉机滑移对控制精度的影响；付拓等[20]提出了一种基于履带转速与虚拟驱动速度和虚拟转向角转换模型的控制算法，为导航控制系统构建了完整的反馈控制。

上述路径跟踪算法基于反馈控制的思路，以路径偏差作为控制输入，实现了履带车辆路径跟踪，但未考虑路径的变化趋势对车辆控制的影响，从而导致控制精度不一定能满足作业要求。本文以单边制动转向小型履带拖拉机为试验平台，提出一种基于虚拟雷达模型的导航路径跟踪控制算法，以虚拟雷达图描述车辆与路径的相对位置关系，经深度神经网络生成驾驶指令，从而控制拖拉机行驶，并进行仿真验证和果园实车试验，验证该算法的有效性。

1 材料与方法

1.1 试验平台

为研究基于虚拟雷达模型的履带拖拉机导航路径跟踪控制算法性能，以小型履带拖拉机(1KFL-30型，富平县悦达机械制造有限公司)为基础搭建了试验平台，如图1所示。拖拉机在行驶过程中，可通过电控液压系统进行转向控制，当拖拉机需要转向时，电磁阀启动转向液压缸，对应一侧的牙嵌离合器分离，切断动力输出并制动实现差速转向；当拖拉机停车时，两侧离合器同时分离切断动力并制动实现停车。拖拉机主要技术参数如表1所示。

表1 履带拖拉机主要技术参数

导航控制系统结构如图2所示，主要由导航定位设备、上位控制系统和下位控制系统组成。导航定位设备为XN422型组合导航系统(西安北方捷瑞光电科技有限公司)，该设备由基站、移动站和数据链路3部分组成，能够以100 Hz的频率提供精度为0.03 m的位置信息和精度为0.1°的航向信息。下位控制系统部署在拖拉机上，主要功能包括：①对导航定位设备提供的拖拉机位姿信息进行转发。②接收上位控制系统指令或遥控指令。③控制液压系统实现拖拉机行驶。上位控制系统以便携式计算机为硬件平台，主要功能包括：①为仿真和试验过程提供可视化界面。②路径规划。③人工驾驶拖拉机模型产生神经网络训练数据。④记录仿真和试验数据。上位和下位控制系统之间采用无线数传电台(E34-DTU型，成都亿佰特电子科技有限公司)进行通信。

1.2 履带拖拉机运动学模型

履带车辆的转向方式为差速转向，依靠行进时两侧履带的不同转速来改变车辆的行驶方向。本研究所使用的履带拖拉机只能实现单边制动转向，即转向时内侧履带速度为0，外侧速度保持不变，其转弯半径为固定值。

为简化运动学分析过程，假设：①拖拉机仅在水平面上运动。②两侧履带完全一样且忽略履带宽度对行驶轨迹的影响。③整机质心和几何中心重合。④两侧履带在运动过程中不发生侧滑。

履带拖拉机运动学模型如图3所示，O′为拖拉机质心位置。根据运动学原理，履带拖拉机的运动学方程为

(1)

式中 (x,y,θ)——拖拉机位姿

B——拖拉机轨距宽度

vL——拖拉机左侧履带速度

vR——拖拉机右侧履带速度

当vL=vR≠0时，左右两侧履带速度相等，拖拉机直线行驶；当vL=0或vR=0时，即左侧或右侧履带速度为0时，拖拉机向左或向右转向。

1.3 虚拟雷达模型描述

目前农业机械导航路径跟踪控制一般根据车辆当前位置相对于规划路径的横向和航向偏差，按照某种控制策略计算车辆的期望转向角，使车辆沿预定路线行驶[4,6]。但履带车辆没有轮式车辆的前轮转向角概念[20]，因此轮式车辆的导航路径跟踪控制算法难以直接应用于履带车辆；同时，横向和航向偏差虽然可以反映车辆相对于规划路径的偏差情况，但其计算过程较为复杂且无法反映路径的走向变化趋势。

本研究针对现有路径跟踪算法的不足提出一种基于虚拟雷达模型的导航路径跟踪控制算法。该算法借鉴人类驾驶经验，结合雷达扫描原理和图像识别的思路，使用虚拟雷达图描述拖拉机与路径的相对位置关系，运用深度神经网络对虚拟雷达图进行识别并生成对应的拖拉机行驶操纵指令以实现导航路径跟踪控制。

1.3.1虚拟雷达模型原理

驾驶员驾驶车辆匀速沿着车道行驶时(图4)，其用来进行转向操作的参照物并不是车道的几何中线，而是左右两条车道边线，通过判断前方视野区的车道边线与车辆的相对位置以及车道的形状和走向变化趋势(图4中的曲线AB和DC)来转动方向盘从而控制车辆沿着车道行驶。

通过对人类驾驶经验的借鉴，本研究将农机的规划路径进行偏移，形成虚拟路径边界AB和DC，拖拉机在自动驾驶过程中不再以横向和航向偏差为控制输入，而是以拖拉机“看到”的封闭图形ABCD的形状来控制拖拉机转向，ABCD组成的封闭图形不仅可以反映自身和路径的相对位置而且可以提供路径和走向趋势信息。如何让拖拉机“看到”虚拟路径边界成为本算法实现的核心问题之一。真实无线电雷达的工作原理是通过向目标发射电磁波信号，然后接收目标反射回来的信号(目标回波)，进而获得目标的轮廓、位置信息[21]。如图5所示，lmax表示雷达最大探测距离，l表示目标距离，θ表示目标方向。借鉴这一原理，本研究通过算法模拟雷达扫描目标的过程，实现拖拉机“看到”虚拟路径边界组成的封闭图形ABCD。

为了便于实现所提出的算法，对虚拟雷达模型做出如下约定：

(1)虚拟雷达只是通过算法模拟雷达扫描目标的过程，不涉及真实世界的雷达。

(2)为降低控制算法的复杂度，虚拟雷达具有有限的扫描分辨率和有限的扫描距离。

(3)虚拟雷达只对虚拟路径边界有探测作用。

1.3.2虚拟雷达图生成

虚拟雷达对虚拟路径边界的扫描可以反映出拖拉机的路径偏移情况和路径的走向变化，将虚拟雷达对虚拟边界的扫描结果定义为虚拟雷达图。为了方便结合雷达扫描原理，对人类控制车辆转向的原理做了进一步拓展，图4中封闭图像不再仅限于视野前方，而是围绕拖拉机一周，如图6所示；具体生成虚拟雷达图的流程如图7所示。

如图6所示，设参考路径的直线部分起点为点S(xs,ys)，终点为E(xe,ye)，虚拟路径宽度为d，则参考路径可描述为

A0x+B0y+C0=0

(2)

路径边界方程为

A1x+B1y+C1=0

(3)

A2x+B2y+C2=0

(4)

其中

A0=A1=A2=ys-ye

式中A0、A1、A2、B0、B1、B2、C0、C1、C2——直线方程系数

(5)

式中i——虚拟雷达探测距离个数

j——两侧虚拟边界，取1或2

当Ajcos(θ+ir)+Bjsin(θ+ir)或探测距离l(i)<0以及探测距离超过虚拟雷达的最大探测距离lmax时，l(i)=lmax，否则雷达的探测距离l(i)=min(l1,l2)，其中l1、l2分别表示到两侧虚拟边界的距离，最终对探测距离l进行归一化即可得到虚拟雷达的探测结果。

以直线路径为例，典型的虚拟雷达探测图对应的车辆和规划路径的相对位置关系如图8和图9所示，图8中拖拉机的横向偏差均小于虚拟路径边界，图9中的横向偏差已超出虚拟路径边界，图中绿色图形为虚拟雷达探测到的图像，其由4条线段构成，其中直线段为探测到的虚拟路径边界，圆弧段为雷达的最大探测边界。

1.4 虚拟雷达模型导航路径跟踪控制算法实现

1.4.1基本思路

导航路径跟踪控制的实质是建立车辆和规划路径的偏差与行驶指令的映射关系[22]。分析目前传统果园广泛使用的小型履带拖拉机的功能和作业特点，其前行时其操作指令仅有直行、左转、右转和停车等4种，所要建立的导航路径跟踪控制算法的任务就是将4种操作指令与虚拟雷达图建立起映射关系。随着计算机性能不断提升，深度学习在诸如图像分类、智能驾驶等领域有着越来越广泛的应用[23]，同时Google、Microsoft、百度等公司纷纷研发并开源深度学习框架，进一步减少了使用深度神经网络的学习成本，加快了深度学习在各个领域的应用推广。

本研究通过利用深度神经网络对输入的虚拟雷达图进行识别并输出对应的行驶操作指令。算法整体结构如图10所示，输入为虚拟雷达图，输出为拖拉机4种备选转向指令(TL为左转、GS为直行、TR为右转、ST为停车)的概率，选取其中概率最大的备选转向指令为拖拉机的转向控制指令。

本研究使用Python 3.5和Google公司的开源深度学习程序框架TensorFlow(GPU版本)搭建神经网络，运行环境为Windows 10系统。具体为使用keras中的Sequential模型搭建网络，网络结构如图11所示，包括输入层、2个全连接隐藏层和1个输出层；其中输入层有360个神经元(由虚拟雷达分辨率决定)，2个隐藏层分别有700个和500个神经元，激活函数为线性整流函数(Rectified linear unit, ReLU)，表达式为

(6)

输出层有4个神经元，分别对应拖拉机行驶的4种行驶状态(左转、右转、直行和停车)，激活函数为Softmax函数，表达式为

(7)

式中yj——输入到Softmax之前的每个单元的值

yk——输入到Softmax之前的第k个单元的值

Softmax(y)k——第k个单元经过Softmax函数处理后的概率分布

Softmax回归本身可作为一个学习算法来优化分类结果，而TensorFlow中去掉了Softmax回归的一些参数，使它仅作为输出层的一个额外处理层，将神经网络的输出变为一个概率分布输出。

1.4.2训练数据准备

训练数据的获取算法是通过人工在仿真平台驾驶履带拖拉机(基于运动学模型式(1))沿规划路径行驶，记录人的操作指令，并与该时刻的虚拟雷达图配对存储，以此作为神经网络的训练数据。

在仿真平台获取训练数据时，规划路径为多段折线路径，拖拉机的横向偏差控制在[-1.000 m，1.000 m]，航向偏差控制在[-60°，60°]，共采集数据32 000组，包括5 055组直行、13 560组左转、13 385组右转；考虑到实际试验过程中可能出现定位漂移，导致虚拟雷达扫描出现图9c的情况，因此人为加入2 000组停车指令；将数据按照8∶1∶1分为训练集、验证集和测试集。

基于草原丝路文物资源的乡村旅游产业开发缺乏统一部署，具有文物资源地区各行其是，未用系统眼光看待问题，解决问题，开发模式分散。无论是从横向上看还是从纵向上看，乡村旅游产业与相关产业融合度都比较低，导致相关产品附加值低，宣传力度不够。产品部门间协作不强，产业链条较短，产业关联效果不显著。产业处于起步阶段，发展机制仍需完善。

1.4.3网络训练

网络训练过程中，初始学习率设为0.000 1，选用TensorFlow提供的Adam自适应学习率优化器[24]对训练过程中的学习率进行优化，Batch size为10，迭代次数为180，Dropout为0.02，损失函数为稀疏分类交叉熵函数(Sparse categorical cross entropy)，公式为

(8)

式中M——类别数量

yc——指示变量，若该类和样本的类别相同则为1，否则为0

pc——观测样本属于类别c的预测概率

训练所用计算机硬件配置为中央处理器(CPU)i5-8300H，主频2.3 GHz；GTX-1050Ti显卡(GPU)；16 GB内存，DDR4，2666 MHz。训练过程中训练集和验证集损失函数变化如图12a所示，训练集和验证集的精度变化如图12b所示，网络在测试集精度为0.990 8。

2 仿真与试验结果分析

2.1 仿真验证

仿真规划路径为U形折线路径，长20 m，宽4 m(路径总长约42.83 m)，路径起始点为(0，-0.5 m)，航向角为180°，拖拉机运动学模型初始横向偏差为0.5 m，航向角为180°。分别对2个速度0.36、0.75 m/s进行仿真验证，仿真过程中上位控制系统通信、显示、数据记录等功能正常，仿真结果如图14所示。当仿真速度为0.36 m/s时，路径跟踪最大横向偏差0.103 m，平均横向偏差0.01 m，标准差0.01 m；当仿真速度为0.75 m/s时，路径跟踪最大横向偏差0.108 m，平均横向偏差0.016 m，标准差0.016 m。仿真结果表明，基于虚拟雷达模型的导航路径跟踪控制算法在原理上可行，可以进行导航路径跟踪控制，并且具有较高的路径跟踪精度，能够满足实际作业需求。

2.2 实车试验

为进一步验证本文路径跟踪算法的有效性，以目前广泛使用的模糊控制算法为对比进行了实车试验。模糊控制算法以横向偏差和航向偏差作为输入，期望转向角作为输出；横向偏差论域为[-1.200 m，1.200 m]，航向偏差和期望转向角论域均为[-60°，60°]，规定左侧横向偏差为负、右侧为正，航向偏差和期望转向角逆时针为正、顺时针为负；输入输出变量均划分为7 个模糊集，采用三角隶属度函数对变量进行模糊化，重心法进行反模糊化。对于单边制动履带拖拉机没有期望转向角的问题，参照文献[25-26]对控制输出的期望角进行处理，当期望转向角绝对值小于3°时对应直行指令，其余情况根据期望转向角的正、负情况对应左、右转指令。

实车试验场地为西北农林科技大学国家柿种质资源圃，果树行距为2.4 m，试验现场如图15所示。考虑到拖拉机在果园作业过程中需要进行直线跟踪和调头换行，因此规划路径设计为20.0 m×2.4 m的U形折线路径，试验速度设定为0.36、0.75 m/s，为保证试验数据有效性，每组试验重复3次(对试验结果中的横向和航向偏差均取绝对值)。路径跟踪试验效果如图16～19所示，重复试验的数据统计如表2所示。

表2 路径跟踪试验数据

由图16～19可知，在速度相同的条件下，基于虚拟雷达模型的路径跟踪算法与基于模糊控制的路径跟踪算法相比，试验路径全程的整体横向偏差更小；在转向路径和换行后直线路径的起始部分有更好的路径跟踪精度和行驶稳定性。

对比2种控制算法的直线路径部分(U形路径0～20 m)跟踪精度，结果如表3所示，在速度0.36 m/s时，模糊控制在最大横向偏差控制和拖拉机行驶稳定性方面(标准差)好于基于虚拟雷达模型的路径跟踪算法，但新算法平均横向偏差降低了8.51%；在速度0.75 m/s时，本研究所提出算法在横向偏差、行驶稳定性等方面均优于模糊控制，最大横向偏差、平均横向偏差和标准差分别降低了7.10%、5.55%、10.26%。

表3 直线路径跟踪精度对比

对比2种导航路径跟踪控制算法的U形路径跟踪精度，结果如表4所示，新算法的所有路径跟踪指标均优于模糊控制，最大横向偏差最高降低了36.33%，平均横向偏差最高降低了27.91%，标准差最高降低了28.00%。表明本文提出的算法在U形路径的整体路径跟踪性能明显好于基于模糊控制的跟踪算法。

表4 U形路径跟踪精度对比

对比2种算法U形路径和直线路径的跟踪精度，结果如表5所示，模糊控制的U形路径和直线路径跟踪精度指标相差较大，特别是最大横向偏差，速度0.36 m/s和0.75 m/s的U形路径分别比直线路径高56.14%和63.94%，而研究所提算法的相关指标均比较相近，最大差异出现在0.75 m/s时的最大横向偏差，U形路径比直线路径仅高出12.35%。表明新算法在转向时的偏差控制能力优于模糊控制，有较好的转向路径适应性。

表5 U形和直线路径跟踪精度对比

3 结论

(1)为提高传统果园广泛使用的小型履带式拖拉机导航路径跟踪控制精度和行驶稳定性，提出了一种基于虚拟雷达模型的导航路径跟踪控制算法，该算法简化了现有路径跟踪控制算法的输入计算过程，并且不依赖于复杂控制理论和数学模型，易于应用。

(2)以果园作业过程中典型U形路径为例，对所提出的跟踪控制算法进行了仿真验证。当速度为0.36 m/s时，路径跟踪最大横向偏差为0.103 m；当速度为0.75 m/s时，最大横向偏差为0.108 m。说明该算法在原理上可以实现导航路径跟踪控制，且具有较高的控制精度，可以满足果园导航作业需求。

(3)在速度为0.36 m/s和0.75 m/s下采用基于虚拟雷达模型的控制算法和目前广泛使用的模糊控制算法进行了实车对比试验。结果表明：与模糊控制算法相比，本文算法最大横向偏差分别减小了15.73%和36.33%，平均横向偏差分别减小了27.91%和19.05%，标准差分别减少了21.88%和28.00%。说明该算法的整体性能优于基于模糊控制的路径跟踪算法，有效提高了导航路径跟踪精度和履带拖拉机自动驾驶的稳定性，对果园典型作业路径有更好的适应性。