插秧机导航路径跟踪改进纯追踪算法

李 革 王 宇 郭刘粉 童俊华,2 何 勇

(1.浙江理工大学机械与自动控制学院， 杭州 310018； 2.浙江省种植装备技术重点实验室， 杭州 310018；3.浙江大学生物系统工程与食品科学学院， 杭州 310058)

0 引言

随着智能农业机械的不断发展，自动导航技术成为研究农业领域的主要热点之一[1]。而导航控制技术中的重点和难点是路径跟踪，路径跟踪的效果直接决定导航控制的优劣。因此，开展农业机械导航路径跟踪控制算法的研究对于智能农业机械的设计和开发有着重要的意义[2-5]。

国内外学者对导航控制算法做了许多的研究。NAGASAKAN等[6]在地头以转弯半径为固定值的前提下，根据航向偏差来控制决策插秧机进行路径跟踪，该方法虽然实现了地头转向，但在与直线跟踪衔接处出现了较大的超调量。KISE等[7]利用三次样条函数进行基于最小转弯半径和最大摆角速率的转向路径规划，设计了两种地头转弯控制方法，尽管仿真结果表明在路径跟踪过程中的最大跟踪误差小于0.2 m，但该方法采用三次样条函数对曲线路径进行规划，因此，要实现转向必须对曲线路径进行跟踪，控制难度相对较大。罗锡文等[8]把拖拉机的运动模型看作是简化的二轮车运动模型，设计了直线跟踪导航PID控制器，控制器参数是通过对该系统进行时域阶跃响应仿真分析和田间试验效果综合整定的，不适应曲线路径跟踪。黄沛琛等[9]采用BP神经网络实现动态调整前视距离，提出了一种改进纯追踪模型的农业机械地头转向控制方法，该方法采用神经网络控制，需要大量的高质量训练成本，若仅对有限的农业车辆起始位姿样本进行训练，则具有局限性。李逃昌等[10]提出的模糊自适应纯追踪模型的农业机械路径跟踪方法的模糊控制规则是根据专家经验制定的，其跟踪误差大，很难快速修正。

本文以井关插秧机为研究平台，以约翰迪尔Starfire3000型接收机、GS2630型显示器和ATU200型电动方向盘为主要硬件设备，设计基于GPS技术的改进纯追踪模型的插秧机导航路径跟踪控制算法。通过模型仿真和田间试验相结合的方法，对该导航控制算法进行验证分析。

1 车辆运动学模型

农业车辆为井关PZ60型插秧机，4轮驱动，全长2.96 m，发动机总功率8.3 kW，前轮液压助力转向，无级变速，前轮外径0.6 m，后轮外径0.9 m，轴距1.2 m。

图1 车辆运动学模型Fig.1 Vehicle kinematics model

该车辆模型采用KELLY提出的简化二轮车运动学模型[11]。在WGS- 84的高斯投影平面坐标系下对该简化模型进行运动学分析。如图1所示，δ为前轮转角，以车辆纵向为基准，偏右为正，偏左为负。θ为车辆航向角，即车辆的纵向逆时针旋转到y轴的夹角，取值范围为0°～360°。L为车辆前后两轴间距，对该模型进行运动学分析可得

式中v——车辆纵向速度

x′(t)——车辆在x轴方向分速度

y′(t)——车辆在y轴方向分速度

θ′(t)——车辆角速度

2 导航控制方法设计

2.1 前视距离的动态调整

前视距离是模仿人工驾驶车辆时向前观察的距离，研究表明该距离与车辆速度以及前面路径弯曲程度有关[12-14]。速度越大前视距离越大，弯度越大则前视距离越小，由此可设前视距离为

l=lp+k1v+k2Ω

式中lp——前视距离的基值

k1——速度系数，数值为正

k2——弯度系数，数值为负

Ω——弯度

首先确定预定义路径，将该路径以二维数组的形式存储起来，可表示为(x(n),y(n))。当前车辆的GPS定位点Pr坐标为(x,y)，求取预定义路径上与车辆当前位置最近的点的方法是通过计算预定义路径上的每个点与当前车辆坐标点的距离，距离最小的那个点即为车辆当前位置与预定义路径的最近点Pk坐标为(xk,yk)，由该最近点向后每隔i个点取1个点，再取3个点，由此可得到4个点，分别为Pk、Pk+i、Pk+2i和Pk+3i，其中i为正整数，取值根据前向预测距离而定，如图2所示。用直线依次连接这4个点，Ω表示前面路径的弯度，很显然前面路径越弯曲，Ω越大。由此可以达到动态调整前视距离的目的。最后根据前视距离可得到车辆的目标点。

图2 确定弯度的方法Fig.2 Method of determining camber

2.2 导航偏差的设定

导航偏差分为横向偏差和航向偏差，横向偏差为车辆当前位置距预定义路径最近的垂直距离，以车辆在路径的右边为正，左边为负，该距离可以评价导航效果。航向偏差是车辆当前航向与目标航向的夹角，以车辆当前航向相对于目标航向右偏为正，左偏为负，将该值作为路径跟踪控制算法的输入量。如图3所示，Pr点为当前车辆的位置，以车辆后轴中心点为控制点，A点为在车辆纵向方向上截取适当前视距离所得到的点，Pk+j是预定义路径上距A点最近的点，也就是目标点，则向量PrPk+j为目标方向，φ为航偏角，d为横向偏差。

图3 偏差的确定方法Fig.3 Method of determining deviations

2.3 路径跟踪控制算法的设计

在大量的理论和试验研究的基础上，设计了一种简单有效的路径跟踪控制算法，其基本原理是利用上述得出的航向偏差作为控制器的输入量，建立控制算法的几何模型，理论推导出前轮的转角公式，将前轮转角作为控制器的输出量，另外由于横向偏差是衡量导航效果优劣的量，因此将横向偏差作为观测调节量，当横向偏差过大时，给输出量一个补偿值，使横向偏差快速减小。

具体算法是将预定义路径以二维数组的形式存储于车载工控机(GS2630型显示器)中，由车载GPS接收机实时接收卫星定位信号和基站的差分定位信号，由此可解算出插秧机当前的位置坐标，工控机将该位置坐标与预定义路径数组信息进行比对，通过计算得出距离当前车辆定位点最近的预定义路径上的点，即能够得出横向偏差，再通过上述方法得出合适的前视距离，由前视距离来获得路径跟踪的目标点，从而确定目标方向，将该信息与电子罗盘采集到的当前航向信息进行对比能够得出航向偏差，将航向偏差作为输入量输入路径跟踪算法控制器中，同时将横向偏差作为观测量，最后输出前轮转角。该输出值作为下位机的输入量，计算后得出步进电动机的转角，由步进电动机带动方向盘转动，进而带动转向机构转动，从而达到预计的前轮转角。

如图4所示，Pr为当前车辆的位置点，Pk+j为目标点，R为转弯半径，C为圆心点，φ为航偏角，则在直角三角形BPrC中可得

(1)

由于φ为圆C的弦切角，因此可得

∠PrCPk+j=2φ

(2)

在等腰三角形PrCPk+j中，由正弦定理可得

(3)

式中lPrPk+j——点Pr与点Pk+j间的距离

由式(1)～(3)可得

(4)

从式(4)可以看出，由输入量φ即可求得前轮转角。该角度求出后输入到下位机中，转换成指令，由转向系统完成转向动作，从而实现路径跟踪。另外，式(4)是从几何角度分析得出的，理论上证明可行，但该式不包含横向偏差，因此需要将其进行改进。在设计控制器时[15-19]，将横向偏差作为观测量进行实时监测，如果横向偏差过大，即当横向偏差d≥dr时，则将前轮转角δ乘以一个系数kdr(kdr>0)，由此可以增大前轮转角，使其快速到达目标点，通过仿真和试验得出：当dr=0.02、kdr=1.08时，导航效果最佳。这样就能够使车辆快速减小横向误差，提高导航效果，由于该方法在推导中并未对预定义路径提出要求，因此这种算法既可实现直线路径跟踪，也可实现曲线路径跟踪，适应性较强。

图4 改进纯追踪算法模型Fig.4 Improved pure tracking algorithm model

3 仿真

在Matlab/Simulink环境下，以两轮车运动学模型为控制对象，对上述路径跟踪控制方法建立仿真模型[20]，由于两轮车运动模型是在理想的条件下建立的，即不考虑侧滑因素，因此需要加一个扰动模型，对车辆的位置进行干扰。该模型主要由5部分组成，分别是车辆模型、扰动模型、偏差求解、导航算法和转向系统。将插秧机的速度设定为1 m/s，初始航向偏差为5°，初始横向偏差为0.1 m，初始位置坐标为(0.9,1)，仿真时间为18 s，仿真步长为0.1 s。

通过Matlab/Simulink搭建系统仿真模型，如图5所示。

图5 路径跟踪仿真框图Fig.5 Simulation block diagram of path tracking

预定义路径如图6所示，该路径是井关PZ60型插秧机作业时的标准路径，其转弯半径为0.9 m。

图6 预定义路径Fig.6 Predefined path tracking

从图6可以看出，插秧机在田间作业时行走的路径包括直线段和转弯曲线段，因此要实现其完全自动导航，其控制算法必须满足既能实现直线路径跟踪，又能实现曲线路径跟踪。为了清楚地表达跟踪效果，利用Matlab编写了路径跟踪效果图界面，如图7所示。

图7 导航控制效果图界面Fig.7 Navigation control effect diagram

从图7右侧的偏差波动图可以看出，在跟踪的起始阶段有0.1 m的横向偏差，仿真曲线也出现了一定程度的振荡，但很快便与预定义路径很好的重合，只有在转弯的地方出现了少许偏差。在整个仿真路径跟踪过程中，横向偏差基本维持在5 cm以内，而航向偏差除了在转弯处出现较大的偏差外，基本维持在10°以内。仿真结果表明：该导航控制算法从理论上是可行的。

4 实车试验

4.1 试验与数据采集

试验车为经过改造的井关PZ60型插秧机，GPS接收机采用约翰迪尔生产的GreenStar3000型接收机，该接收机集成了位置传感器、速度传感器和陀螺仪传感器[21]，采样频率为5 Hz，数据通过CAN总线发送到控制器中。控制器使用约翰迪尔公司自主研发的GS2630型控制器，该控制器内集成3种导航模式，分别是：SF1模式，精度为±33 cm；SF2模式，精度为±10 cm；RTK模式，精度为±2 cm。前两种导航模式只需1个GPS接收机即可，而最后一种导航模式需要2个GPS接收机，其中一个为基站。改进纯追踪导航控制算法存储于计算机内，其含有导航控制软件，两者之间通过CAN总线进行数据通讯。自动方向盘采用约翰迪尔公司生产的AutoTrac200型转向套件，将其安装在井关PZ60型插秧机上，通过计算机操作即可实现导航控制。

改装后的插秧机如图8所示。通过CAN记录仪采集数据，实时保存导航过程中产生的数据，主要包括车辆的位置、速度、航向、横向偏差和航向偏差等，以便在试验结束后对导航效果进行分析。

图8 试验插秧机Fig.8 Experimental transplanter

试验在农场水田中进行，有较多的水，泥脚深度约为20 cm。首先人工驾驶插秧机沿标记好的路径慢速行走，利用车载GPS接收机记录路径数据，将该数据修正后作为自动导航的预定义路径。然后将插秧机驾驶到数据记录的起始位置，开启自动导航模式，实现无人驾驶。图9为插秧机在水田中自动导航行驶现场图。

4.2 数据分析

将自动导航时行驶的路径数据和预定义路径数据进行对比，可以得到路径跟踪的误差。图10为插秧机在水田中自动导航时的数据分析图。

对比数据分析得出，路径跟踪的平均误差为0.096 m，最大跟踪误差为0.183 m，最大跟踪误差出现在插秧机转弯过程中，但直线作业段路径跟踪的平均误差为0.058 m，最大跟踪误差为0.135 m，该误差在插秧机作业允许的误差范围内，表明该导航控制算法具有良好的导航效果。综合转弯与直线路径跟踪数据，平均跟踪误差为0.077 m，最大跟踪误差为0.159 m。

图9 插秧机水田自动导航现场图Fig.9 Automatic navigation scene of transplanter in paddy field

图10 插秧机水田作业路径跟踪试验结果Fig.10 Experimental results of paddy field operation path tracking for rice transplanter

5 结论

(1)提出通过动态调整前视距离的方法对插秧机进行目标点的确定，这样能够减小插秧机在导航过程中的横向偏差和航向偏差，有利于提高导航精度。

(2)提出了一种改进纯追踪模型的插秧机导航路径跟踪算法，该控制算法不仅可以进行直线路径跟踪，也可以进行曲线路径跟踪，并能适应车辆速度的变化，实现插秧机在田间的智能控制。其转向灵活，有较好的实时性和鲁棒性。

(3)设计的导航控制算法利用航向偏差实现对直线路径和转弯曲线路径的跟踪，并用横向偏差作为观测量实现对控制量的补偿。试验结果表明：当行驶速度为1 m/s时，平均跟踪误差为0.077 m，最大跟踪误差为0.159 m，满足插秧机自动导航控制的要求。

1 张美娜，吕晓兰，陶建平，等. 农用车辆自主导航控制系统设计与试验[J/OL]. 农业机械学报,2016,47(7):42-47.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160707&flag=1.DOI:10.6041/j.issn.1000-1298.2016.07.007.

ZHANG Meina, LÜ Xiaolan, TAO Jianping, et al. Design and experiment of automatic guidance control system in agricultural vehicle[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2016,47(7):42-47. (in Chinese)

2 REID J F, ZHANG Q, NOGUCHI N, et al. Agriculture automatic guidance research in North America [J]. Computers and Electronics in Agriculture,2000,25(1-2):155-167.

3 TORRI T. Research in autonomous agriculture vehicles in Japan [J]. Computers and Electronics in Agriculture, 2000,25:155-167.

4 张闻宇，丁幼春，王雪玲，等. 基于SVR逆向模型的拖拉机导航纯追踪控制方法[J/OL]. 农业机械学报,2016,47(1):30-35.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160105&flag=1.DOI:10.6041/j.issn.1000-1298.2016.01.005.

ZHANG Wenyu, DING Youchun, WANG Xueling, et al. Purepursuit control method based on SVR inverse-model for tractor navigation [J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016,47(1): 30-35.(in Chinese)

5 田海清，应义斌，张方明. 农业车辆导航系统中自动控制技术的研究进展[J]. 农业机械学报，2005,36(7)：148-152.

TIAN Haiqing, YING Yibin, ZHANG Fangming. Development of automatic control technology for agricultural vehicle guidance[J]. Transactions of the Chinese Society for Agricultural Machinery, 2005,36(7): 148-152.(in Chinese)

6 NAGASAKAN Y, SAITO H, TAMAKI K, et al. An autonomous rice transplanter guided by global positioning system and inertial measurement unit [J]. Journal of Field Robotics, 2009,26(6-7): 537-548.

7 KISE M, NOGUCHI N, ISHII K, et al. Development of the agricultural autonomous tractor with an RTK- GPS and a FOG[C]∥IFAC: Proceedings of the 4th IFAC Symposium on Intelligent Autonomous Vehicles, 2001: 103-106.

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

LUO Xiwen, ZHANG Zhigang, ZHAO Zuoxi, et al. Design of DGPS navigation control system for Dongfanghong X- 804 tractor [J]. Transactions of the CSAE, 2009,25(11): 139-145. (in Chinese)

9 黄沛琛，罗锡文，张智刚. 改进纯追踪模型的农业机械地头转向控制方法[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 Application, 2010,46(21): 216-219. (in Chinese)

10 李逃昌，胡静涛，高雷，等. 基于模糊自适应纯追踪模型的农业机械路径跟踪方法[J/OL]. 农业机械学报,2013,44(1): 205-210.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20130139&flag=1.DOI:10.6041/j.issn.1000-1298.2013.01.039.

LI Taochang, HU Jingtao, GAO Lei, et al. Agricultural machine path tracking method based on fuzzy adaptive pure pursuit model [J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2013,44(1): 205-210. (in Chinese)

11 张智刚，罗锡文，周志艳，等.久保田插秧机的GPS导航控制系统设计[J]. 农业机械学报,2006,37(7): 95-97.

ZHANG Zhigang, LUO Xiwen, ZHOU Zhiyan, et al. Design of GPS navigation control system for rice transplanter [J]. Transactions of the Chinese Society for Agricultural Machinery, 2006,37(7): 95-97. (in Chinese)

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

ZHOU Jianjun, ZHANG Man, WANG Maohua, et al. Path tracking for agricultural vehicles based on fuzzy control[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009,40(4): 151-156. (in Chinese)

13 熊中刚，贺娟，敖邦乾，等. 基于路径弯曲度动态预瞄搜索算法的车辆路径跟踪控制软件系统设计[J]. 江苏农业科学，2015,43(2): 370-373.

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

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. (in Chinese)

15 薛金林,张顺顺.基于激光雷达的农业机器人导航控制研究[J/OL].农业机械学报,2014,45(9):55-60.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20140909&flag=1.DOI:10.6041/j.issn.1000-1298.2014.09.009.

XUE Jinlin,ZHANG Shunshun.Navigation of an agricultural robot based on laser radar[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):55-60.(in Chinese)

16 吕安涛，宋正河，毛恩荣. 拖拉机自动转向最优控制方法的研究[J]. 农业工程学报,2006,22(8): 116 -119.

LÜ Antao, SONG Zhenghe, MAO Enrong. Optimized control method for tractors automatic steering [J]. Transactions of the CSAE, 2006,22(8): 116-119. (in Chinese)

17 丁永前，王致情，林相泽，等. 自主跟随车辆航向控制系统[J/OL].农业机械学报,2015,46(1):8-13.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150102&flag=1.DOI:10.6041/j.issn.1000-1298.2015.01.002.

DING Yongqian,WANG Zhiqing,LIN Xiangze, et al.Heading control system of autonomous following vehicle[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015,46(1):8-13. (in Chinese)

18 赵德安,贾伟宽,张云,等.农业机器人自主导航改进自适应滤波控制器研究[J/OL].农业机械学报,2015,46(5):1-6.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150501&flag=1.DOI:10.6041/j.issn.1000-1298.2015.05.001.

ZHAO Dean,JIA Weikuan,ZHANG Yun, et al.Design of agricultural robot autonomous navigation control based on improved self-adaptive filter[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015,46(5):1-6.(in Chinese)

19 张闻宇,丁幼春,廖庆喜,等.拖拉机液压转向变论域模糊控制器设计与试验[J/OL].农业机械学报,2015,46(3):43-50.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150307&flag=1.DOI:10.6041/j.issn.1000-1298.2015.03.007.

ZHANG Wenyu,DING Youchun,LIAO Qingxi,et al.Variable universe fuzzy controller for tractor hydraulic steering[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015,46(3):43-50.(in Chinese)

20 迟德霞，任文涛，由佳翰，等. 水稻插秧机导航控制器设计与路径追踪仿真研究[J]. 沈阳农业大学学报，2016，47(3)：363-367.

CHI Dexia, REN Wentao,YOU Jiahan, et al.Design on navigation controller and path tracking simulation of rice transplanter [J]. Journal of Shenyang Agricultural Uinversity, 2016, 47(3): 363-367. (in Chinese)

21 张铁民,李辉辉,陈大为,等.多源传感器信息融合的农用小车路径跟踪导航系统[J/OL].农业机械学报,2015,46(3):37-42.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150306&flag=1.DOI：10.6041/j.issn.1000-1298.2015.03.006.

ZHANG Tiemin,LI Huihui,CHEN Dawei, et al.Agricultural vehicle path tracking navigation system based on information fusion of multi source sensor[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015,46(3):37-42. (in Chinese)