基于DGPS与双闭环控制的拖拉机自动导航系统

黎永键 赵祚喜 黄培奎 关 伟 吴晓鹏

(1.广东农工商职业技术学院机电系， 广州 510507；2.华南农业大学南方农业机械与装备关键技术教育部重点实验室， 广州 510642)

基于DGPS与双闭环控制的拖拉机自动导航系统

黎永键1赵祚喜2黄培奎2关 伟1吴晓鹏2

(1.广东农工商职业技术学院机电系， 广州 510507；2.华南农业大学南方农业机械与装备关键技术教育部重点实验室， 广州 510642)

以东方红X-804型拖拉机为平台，设计了一种基于RTK-DGPS定位和双闭环转向控制相结合的自动导航系统，研究提高农业机械导航控制精度的方法。阐述了导航系统整体设计方案，以RTK-DGPS和AHRS500GA分别提供位置信息和辅助修正信息实现准确定位，以电控液压转向系统实现转向控制。分析了整体控制的策略，建立了路径跟踪的传递函数模型，阐述了双闭环转向控制算法的建立过程，以及控制器的硬件实现。试验结果表明：GPS定位数据经过校正后，平均偏差降低至0.031 m；双闭环控制算法提高了自动转向系统性能，稳态时方波信号以及正弦波信号的跟踪误差平均值为0.40°；在拖拉机田间作业跟踪过程中，路径跟踪误差平均值不超过0.019 m，转向轮偏角跟踪误差平均值为0.43°，标准差不超过0.041 m。

拖拉机； 自动导航； 转向系统； 跟踪误差； 载波相位差分全球定位系统； 双闭环控制

引言

农业机械自动导航技术是现代农业机械装备的重要支持技术之一。实现农业机械的智能化导航，可以有效降低劳动强度，提高田间作业精准度[1]。近年来，国内外对于农用车辆路径跟踪控制技术进行了大量研究，主要采用全球定位系统(Global positioning system，GPS)技术与惯性导航技术相结合的方法[2]。ZHANG等[3]利用载波相位差分全球定位系统(Real time kinematic differential global positioning system，RTK-DGPS)、磁罗盘传感器和惯性传感器，将Kalman滤波融合处理后的拖拉机定位精度控制在0.1 m内。罗锡文等[4]采用RTK-DGPS定位技术，研究东方红X-804型拖拉机的自动导航系统，设计了直线跟踪的比例-微分控制(Proportion-differentiation，PD)导航控制器,试验结果表明：在拖拉机行进速度为0.8 m/s时，直线跟踪最大误差控制在0.15 m以内，平均跟踪误差控制在0.03 m以内。该系统具有良好的适应性，但是存在以下问题：①田间作业环境复杂，系统误差随时间积累，导致GPS定位准确度下降。②电控液压系统具有非线性特征，转向控制采用单一的位置传感器或者角度传感器测量值作为控制反馈量，容易出现控制超调现象。

本文在东方红X-804型拖拉机上开发基于RTK-DGPS与双闭环转向控制相结合的自动导航系统，实现东方红拖拉机较高精度的速度控制和转向控制，减少控制超调，以期提高农业车辆田间导航控制精度。

1 自动导航系统整体设计

自动导航系统在东方红X-804型拖拉机上进行设计，拖拉机基本参数如表1所示[5]。

1.1 自动导航系统整体设计方案

根据研究现状，表2首先列出了原有控制方案。对比前人的研究，本文选择的方案有以下改进：①建立RTK-GPS与姿态航向参考系统(Attitude and heading reference system，AHRS)组合的定位系统。②转向控制系统的传感部分采用无接触磁阻传感器KMA199和惯性传感器ADIS16300组合。③改进控制电路，控制芯片采用AT91SAM9261。 ④双闭环控制方法。

表1 东方红X-804型拖拉机参数Tab.1 Main parameters of Dongfanghong X-804 tractor

表2 自动导航系统设计方案Tab.2 Design of automatic navigation system

1.2 组合定位方法

农业车辆在田间作业行驶时，由于地面凹凸不平导致车体倾斜而存在较大的横滚角、俯仰角，车载GPS天线端与车体中心不重合。因此，定位数据实际上是天线的位置，而不是车体中心的位置，定位偏差较大[6]。基于以上考虑，必须通过姿态角校正原始接收数据的方法以提高定位准确度。

本文设计了Trimble5700型RTK-DGPS(Trimble 公司)与AHRS500GA型AHRS(Crossbow公司)组合的定位方法。Trimble5700 型RTK-DGPS的动态测量水平定位精度为±10 mm+10-6RMS，垂直定位精度可达±20 mm+10-6RMS[7]。AHRS500GA测量精度为：俯仰与横滚0.03°RMS，航向0.2°RMS[8]。在定位算法中，RTK-DGPS为主要导航方式，用于测量拖拉机的位置和速度信息；AHRS500GA提供车体姿态和航向等信息以修正位置信息。坐标转换关系为[9]

(1)

其中

(2)

由式(1)、(2)可知，利用GPS测得大地坐标系下的位置信息，通过AHRS500GA准确测得φ、θ、ψ的值，代入式(2)求出方向余弦矩阵，再代入式(1)即可计算出车体中心的真实GPS位置信息。

1.3 电液控转向系统

为实现自动转向，设计并搭建了电液控转向系统，结构如图1所示。在东方红拖拉机原机械转向系统的基础上并联1台由步进电动机驱动的液压转向器，并安装换向电磁阀、比例阀、溢流阀作为执行元件。工作过程如下：当驾驶员选择自动模式，转向控制器驱动换向电磁阀工作从而实现油路后切换，以控制前轮转向；以脉宽控制的方式通过电控比例阀控制流量以调整转向速度，使前轮平稳地转向目标角度[10]。溢流阀用于防止油压过载。

图1 东方红X-804型电液控转向控制系统Fig.1 Electro-hydraulic steering system of Dongfanghong X-804 tractor1.油箱 2.手动切换阀 3.转向器 4.方向盘 5.油管 6.转向油缸 7.转向前轮 8.手动切换阀 9.三位四通电磁阀 10.比例阀 11.溢流阀

图2为转向系统的运动学分析。转向时液压油从油管进入转向油缸，推动活塞使转向轮动作。相关物理量的表示规定如下：Fw是转向过程中作用在转向轮的外力，SA是活塞横截面积，PQ是负载压降，BQ是活塞的粘性阻尼系数，m是负载质量，QL是进入油缸的液压油量，yh是活塞移动距离。

图2 油缸运动学分析Fig.2 Kinematics analysis of cylinder1.油管 2.转向油缸 3.油箱 4.转向前轮 5.活塞横截面

以活塞为研究对象，根据牛顿第二定律建立力的平衡方程为

(3)

转向轮转角δ与活塞移动距离yh关系为[11]

δ=kδyh

(4)

转向轮的角速率ω与油缸活塞移动距离yh属于一阶导数的关系[12]

(5)

比例阀的开度φp与输入电流OI的关系为

φp=kφOI

(6)

式中kδ、kω、kφ——比例系数

文献[13]对液压转向系统的传递函数进行了计算，结果为二阶函数，但前提是Fw约为0。在实际的转向过程中，存在电控液压阀的执行动作延时，并且转向轮与地面有较大的摩擦力。因此，本文使用1个二阶惯性环节和1个延时环节表示，并建立相应的传递函数Gω(s)和传递函数Gδ(s)

(7)

(8)

式中U(s)——系统控制输入δ(s)——转向角度控制输出ω(s)——角速度控制输出s——拉氏变换的复数τ——时间函数的延迟Ka、Kb、Kc——系统传递函数参数

以比例阀电流为输入信号，转向角速率为输出，利用Matlab系统辨识工具箱进行参数估计，可得：Ka=0.422 8，Kb=6.952 4，Kc=3.790 2。

1.4 CAN通信系统

以控制局域网(Controller area net，CAN)作为导航系统的通信网络，设计主控制器节点和功能节点实现拖拉机自动导航[14]。主控制器负责处理RTK-GPS位置信息、AHRS500GA姿态信息，以及根据当前的转向角、转向角速度和拖拉机机具升降信息，发出控制指令，按照预先设定的路线规划行驶及转向。功能节点包括转向控制节点、油门控制节点、制动控制节点、角速度测量以及机具升降控制节点。

2 导航控制器的设计

自动导航系统的控制内容包括路径跟踪及转向控制，系统整体控制策略如图3所示。由GPS测量拖拉机当前位置的坐标，由AHRS500GA测量航向角。以横向偏差和航向角测量值为输入量1，由控制器1决策出目标航向角，并且计算出航向角的偏差。以航向角偏差和当前转向轮转角为输入量2，由控制器2决策出目标转向角。

图3 整体控制策略框图Fig.3 Control block diagram of whole system

2.1 路径跟踪的控制学模型

路径跟踪的主要功能是实现拖拉机按照预先规划的路径行驶，将误差控制在尽可能小的范围内。将拖拉机行走实时GPS定位坐标点到当前作业横坐标的距离定义为横向跟踪误差。本文设计的路径跟踪方法如下：首先计算出拖拉机实时位置的横向偏差量并测量实时航向角，利用控制器决策目标航向角，然后通过转向控制系统决策目标转向角，最后通过转向执行机构控制拖拉机行驶路线，从而实现路径跟踪功能。

文献[15]研究了拖拉机的运动学模型，方程为

传递函数为

(9)

式中x、y——位置坐标L——拖拉机轴距Vx——拖拉机横向行驶速度Vy——拖拉机纵向行驶速度

2.2 双闭环转向控制算法设计

结合运动学模型与转向系统模型，可得出导航系统的传递函数

(10)

在控制系统中，为消除结构不稳定，引入PID控制算法。PID控制由比例、积分、微分3个环节构成，其传递函数表达式为[16]

式中KP、KI、KD——比例系数、积分系数、微分系数

TI——积分时间

TD——微分时间

比例环节作用是成比例反映控制系统的偏差信号，以减少偏差；积分环节的作用是消除静差，提高系统的无差度；微分环节作用是反映偏差信号的变化趋势，引入一个有效的早期修正信号。

由于电液控转向系统的非线性特性[17]，在自动转向过程中存在较大的控制超调量，导致转向轮的转角无法准确达到目标值，从而产生震荡现象。因此，必须设计相应的控制算法抑制超调量。相关研究表明，双闭环控制方法较好地解决了控制超调大的问题[18-20]。本文设计了基于转向轮的转角和角速率的双闭环控制系统：以转向角度控制为外环，通过角度传感器测量转向轮实时转角δR与目标角度δT进行对比，由控制算法决策出目标角速率；以角速率控制为内环，通过陀螺仪测量实时角速率ωR与目标角速率ωT进行对比，由控制算法决策出比例阀输入电流的目标值。

(1)角度控制

设eδ为转角误差，则有

eδ=δT-δR

(11)

控制目标的角速率是ωT，由PID算法可得到

ωT=KδPeδ+KδIeiδ+KδDedδ

(12)

其中

eiδ=eiδ_b+eδts

(13)

edδ=(eδ-eδ_b)/ts

(14)

式中KδP、KδI、KδD——角度控制算法的系数eδ_b——上一时刻的角度误差eiδ——角度误差的积分部分eiδ_b——上一时刻的角度误差积分部分edδ——角度误差的微分部分ts——采样时间

(2)角速率控制

设eω为角速率误差，控制目标的比例阀电流是OT，由PID算法可得

OT=KωPeω+KωIeiω+KωDedω

(15)

其中

eiω=eiω_b+eωts

(16)

edω=(eω-eω_b)/ts

(17)

式中KωP、KωI、KωD——角速率控制算法系数eω_b——上一时刻的角速率误差eiω——角速率误差的积分部分eiω_b——上一时刻的角速率误差积分部分edω——角速率误差的微分部分

2.3 基于Simulink控制参数估计

以Simulink为平台建立仿真模型，通过仿真得到系统传递函数的未知参数。根据传递函数式，使用1个二阶惯性环节加上1个系统延时，分别建立系统外环、内环的仿真模型。运行仿真程序，图4是典型的仿真结果。

图4 三角波信号的跟踪仿真结果Fig.4 Triangle wave signal tracking simulation results

图4a是角度跟踪结果，输入信号为周期10 s的三角波信号，输出信号准确完成了目标角度跟踪，延时为0.1 s。图4b是对应的角速度跟踪仿真结果，三角波信号对应的角速率跟踪信号也很好地跟踪上目标角速率。以上跟踪效果达到要求，其中重要的原因是跟踪目标信号的变化率不大，表明：稳态下的双闭环控制效果理想。经过反复参数调整，得到角度控制外环的参数为：KδP=1.05，KδI=0.023，KδD=0.015；角速度控制内环参数为：KωP=103.70，KωI=8.35，KωD=1.80。

2.4 地头转向的路径规划方法

文献[4]通过试验证明，由于东方红X-804型拖拉机转向半径为5.3 m，在地头转向的过程中占空间较大，导致对行行驶困难。因此，本文采用跨行地头转向的方法进行转弯时的路径规划，如图5所示。当前作业行记为i，当拖拉机行驶至该行终点时，不进入i+1行，而进入n+j行(j>1，本文取j=7，其计算见3.3节)。采用跨行地头转向的依据是：拖拉机可以在较大的运动空间内通过两次接近90°的转向动作和一段短距离的直线行走即可完成地头转向，以解决对行困难的问题。

图5 跨行地头转向的路径规划Fig.5 Path planning of headland turning through spanning line

在地头转向控制过程中，航向角是关键参数之一。AHRS500GA提供实际的航向角信息，控制系统将航向角测量值与目标航向角进行对比。目标航向角ψT计算式为[15]

ψT=arctan((yk-yk-N)/(xk-xk-N))×180/π

(18)

式中N为常数，其大小取决于拖拉机行驶的速度，通过大量的田间测试，本文的N取10。

地头转向的控制过程如下：设定距离阈值Ep和航向角度阈值Eθ。当拖拉机沿直线行驶至每行地头转向点附近时，车体质心距离该行节点小于Ep，主控制器发送转向行走速度指令和机具提升指令，执行地头转向动作。转向控制系统使前轮以最大转向角向目标航向转向；当实时航向角与目标航向之间的差值小于Eθ，即停止转向动作并切换回直线跟踪模式。按照相同的方法，拖拉机进入第i+7作业行。如此循环，完成16作业行的行驶。Ep和Eθ的取值由拖拉机行驶速度、转向控制参数决定。通过反复的田间试验调节，本文的Ep取1.5 m，Eθ取25°。

2.5 导航控制系统的硬件实现

图6 导航控制系统硬件框图及安装示意图Fig.6 Structural diagram and installation sketch of navigation system1.转向轮 2.KMA199 3.ADIS16300 4.转向控制系统 5.转向桥 6.转向节 7.转向中心轴

设计的系统硬件框图如图6a所示，主要部分包括：导航控制器AT91SAM9261、电源电路、传感器部分、转向控制系统等。IO是上位机控制指令，IF是角度反馈信息，IL是LMS8962控制信号，ax是载体绕X轴转动的加速度，ADC是模拟/数字转换器，DAC是数字/模拟转换器，SPI是串行外设接口控制器，I/O是输入输出口。图6b是传感器安装示意图。

2.5.1 传感器部分

(1)角度传感器KMA199

KMA199是一种非接触式的磁阻效应传感器，其测量原理：安装该传感器的载体运动时产生旋转的磁场，作用在传感器上引起磁阻的改变，磁阻值的变化量正比于转角大小，利用数学关系即可换算得到角度值[21]。将KMA199传感器安装在拖拉机转向节上以测量前轮转向角。

(2)惯性测量单元ADIS16300

ADIS16300是4自由度惯性测量模块，内部集成1个数字陀螺仪和3轴式加速度计，数字控制采样速率可达819.2 SPS[22]。在惯性导航领域，ADIS16300主要用于检测运动载体的姿态角、角速率和加速度等信息。本文以该传感器测量转向轮的角速率。

2.5.2 控制器选择

(1)导航控制器：采用ARM9E 32位嵌入式微处理器AT91SAM9261作为导航控制器(上位机)。该处理器运算速度可达200 MIPS，并集成了64 MB SDRAM[23]。基于AT91SAM9261的导航控制器可完成GPS、AHRS、各传感器的信号采集与处理，并控制执行机构工作。

(2)转向控制芯片：采用ARMv7架构的Cortex-M3内核微处理器LMS8962作为转向控制器芯片。该芯片内外设主要包括数模转换模块ADC、CAN总线控制器、 SPI总线控制器，运算能力强大且功耗低[24]。

3 试验与结果分析

3.1 GPS定位校正试验

为验证GPS倾斜校正的设计效果，进行拖拉机定位信息校正试验。

在平整路面上，首先用直尺划出1条20 m长的直线，然后用厚度为10 cm、边长为35 cm方砖按照直线的轨迹铺成道路，方砖之间保持8 cm的距离。试验过程中，驾驶东方红X-804型拖拉机沿着直线轨迹通过方砖，其中左侧车轮压在方砖上。由于砖块之间的空隙，车体行驶过程中有剧烈的抖动。同时启动本文设计的组合导航系统，收集RTK-DGPS与AHRS500GA信号，并利用式(1)、(2)进行校正，数据保存在SD卡内。

图7是1次典型试验结果。图7校正前由于车辆抖动导致GPS天线倾斜，其表现为定位数据偏离实际的中心直线，统计数据表明校正前最大偏差为20 cm,平均偏差为7.2 cm；图7校正后GPS数据与规划直线表现出较好的一致性，最大偏差为12 cm，平均偏差为3.1 cm。可知校正公式对定位数据起到平滑作用，能更准确反映车体质心位置。误差来源包括人工驾驶技术、铺设路面测量误差，以及车身振动等[25]。

图7 不平路面的直线行走试验结果Fig.7 Straight line driving test result on uneven road

3.2 双闭环转向控制算法试验

为验证设计的双闭环PID控制算法能有效地使转向执行机构按控制系统指令达到目标角度，进行转向试验。

上位机以10 Hz频率发送控制指令信号，通过CAN通信网络向转向控制器传输控制指令，下位机的执行频率同样为10 Hz。运行本文设计的双闭环控制算法，控制信号追踪结果如图8所示。

(1)从图8a可以看出，对于方波信号，跟踪角度与目标信号有良好一致性。图8b是跟踪误差曲线，可知角度变化较大时，出现较大的震荡；稳态时，误差基本在0°附近。统计结果表明：平均误差为0.40°，最大误差为0.60°。最大跟踪时间为1.6 s，平均跟踪时间为1.3 s。以上分析可知，转向系统的超调量得到较好抑制。

(2)从图8c、8d可知，正弦信号的跟踪值与目标值呈现较好的一致性。统计表明：跟踪误差平均值为0.40°，平均跟踪时间为0.15 s。因此，跟踪过程比较平稳，超调控制较好。

图8 转向信号响应与对应误差Fig.8 Steering signal tracking and error

图9 田间试验测试结果Fig.9 Results of field test on tractor

3.3 田间试验

为验证拖拉机自动导航系统的实际工作精度，进行田间试验。

试验在华南农业大学的一块试验田中进行，规划出如图9所示的行驶路径。路径设定方法如下：①首先确定长方形试验田(测量宽度为40 m，长度为75 m)的4个顶点为地头节点，分别记为A、B、C、D，使用GPS装置测量各顶点的位置坐标。②以AD构成的边为基准线，划定总数若干平行作业线，行与行之间的距离均为2.5 m。将第1作业行定位于北端，拖拉机在该行的行驶方向为自东向西，在该行的终点处拖拉机跨7行进入下一作业行并按照自西向东行驶。以同样的方法循环作业，规划的作业行数目为16行。③计算所有平行作业线与边界的交点，作为每行的节点，跨行数目j=(16-1)/2，取j=7。④按照文中2.3节所介绍的地头转向规划方法，编制作业顺序。将相应作业线节点存储进2维数组，供导航控制软件调用。

驾驶员首先将拖拉机驾驶至首行，车头对准首行路径方向，启动自动导航系统，拖拉机处于自动行驶状态。拖拉机从第一作业行起点开始行驶，导航控制系统实时控制拖拉机按照规划路径行驶至终点，在每行的终点执行地头转向。在行驶过程中，由上位机记录下RTK-GPS、AHRS500GA以及转向系统的数据，导航控制系统工作并储存数据。田间作业结束后，从控制系统导出以下主要参数：GPS定位数据、转向轮偏角期望值和测量值、姿态信息,在PC平台上进行试验结果分析。

图9是拖拉机在田间作业时的定位追踪与转向角跟踪情况。图9a给出了目标转向角度与转向轮实际转角关系。由导航控制器决策并发出目标转向角度的控制指令，由 KMA199测量转向角度的实际值。从图可知，在-5°～35°范围内转向执行机构按照控制指令动作，跟踪误差平均值为0.43°，控制在合理范围内。

图9b是拖拉机田间行驶实际轨迹与规划路径比较。在获取试验数据的基础上，对误差进行统计分析，以横向跟踪误差(记为W)平均值Wavg、最大值Wmax以及标准差σW作为路径跟踪效果的评价指标，计算式为[26]

对试验数据进行统计分析(表3)，结果表明：所有行的平均跟踪误差均不大于0.020 m，标准差不超过0.041 m。除第7、12、14、16行之外，其他所有行的最大跟踪误差不超过0.088 m。与文献[4]试验结果进行对比，平均误差、各作业行的最大跟踪误差均减少。可知，双闭环PID转向控制方法能有效提高导航控制精度和稳定性。观察采样数据发现，所有行的最大跟踪误差出现在起始位置，其原因是：从地头转向结束，开始直线行驶的过渡期间，由于田间路面颠簸、车体振动而产生相对较大的误差。因此，将来的工作需改进地头转向与直线行驶的衔接问题。

表3 试验跟踪误差统计结果Tab.3 Statistical results of experimental offset error m

4 结论

(1) GPS倾斜校正试验表明，基于RTK-DGPS与AHRS组合可有效平滑GPS定位数据，更准确反映车体实际位置。

(2)转向控制算法试验表明，转向轮能按照转向控制系统发出的指令达到目标角度和角速度，稳态跟踪误差不超过0.6°，试验效果良好。

(3)田间行走试验证明，拖拉机能按照预先规划的路径自动转向和行驶。路径跟踪误差平均值不超过0.019 m，标准差不超过0.041 m，角度跟踪误差平均值为0.43°。本文设计的导航系统能满足拖拉机田间导航控制的要求。

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

2 张智刚. 插秧机的DGPS自动导航控制系统研究[D]. 广州：华南农业大学，2006. ZHANG Zhigang. Research of DGPS navigation control system for rice transplanter[D]. Guangzhou: South China Agricultural University, 2006. (in Chinese)

3 ZHANG Q, REID J F, NOBORU N. Agricultural vehicle navigation using multiple guidance sensors [C]∥Proceedings of the International Conference on Field and Service Robotics, 1999:293-298.

4 罗锡文，张智刚，赵祚喜,等.东方红X-804拖拉机的DGPS自动导航控制系统[J].农业工程学报，2009，25(11)：139-143. LUO Xiwen, ZHANG Zhigang, ZHAO Zuoxi, et al. Design of DGPS navigation control for Dongfanghong X-804 tractor[J]. Transactions of the CSAE, 2009, 25(11): 139-143.(in Chinese)

5 吴晓鹏，赵祚喜，张智刚，等.东方红拖拉机自动转向系统设计[J].农业机械学报，2009，40(增刊)：1-5. WU Xiaopeng, ZHAO Zuoxi, ZHANG Zhigang, et al. Development of automatic steering control system based on Dongfanghong tractor[J].Transactions of the Chinese Society for Agricultural Machinery, 2009,40(Supp.):1-5. (in Chinese)

6 籍颖，刘兆祥，刘刚，等.基于卡尔曼滤波农用车辆导航定位方法[J].农业机械学报，2009，40(9)：13-17. JI Ying, LIU Zhaoxiang, LIU Gang, et al. Positions research of vehicle navigation system based on kalman filter[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009，40(9): 13-17. (in Chinese)

7 张智刚，罗锡文，赵祚喜，等.基于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)

8 Crossbow Technology, Inc．AHRS500GA-Series User's Manual：Revision B[M]. New York: McGraw Hill, 2007:21-25．

9 秦永元.惯性导航[M]．北京:科学出版社，2014：243-259．

10 陈斌. 东方红X-804拖拉机自动转向控制系统研究与设计[D].广州：华南农业大学，2009. CHEN Bin. Research and design of steering control system for Dongfanghong X-804 tractor [D]. Guangzhou: South China Agricultural University, 2009. (in Chinese)

11 NOGUCHI N, WILL J, REID J, et al. Development of a master-slave robot system for farm operation[J].Computer and Electronics in Agriculture，2004，44(1)：1-19.

12 吴晓鹏.东方红拖拉机转向伺服控制系统设计[D].广州：华南农业大学，2011. WU Xiaopeng. Steering control system design for Dongfanghong X-804 tractor[D]. Guangzhou: South China Agricultural University, 2011. (in Chinese)

13 GUO Linsong, HE Yong, ZHANG Qin, et al. Real-time tractor position estimation system using Kalman filter[J]. Transactions of the CSAE,2002,18(5)：96-101.

14 Bosch CAN User's Guide [EB/OL]. http:∥www.xhl.com.cn/sjsc/sjscdetail.asp?sid=25,2007.

15 HAN S, ZHANG Q, NOBORU N. Kalman filtering of DGPS position for a parallel tracking application [J].Transactions of the ASAE, 2002, 45(3):553-559.

16 GREWAL M S, WEILL L R, ANDREWS A P. Global positioning systems, inertial navigation, and integration[M]. 2nd ed. New York: John Wiley & Sons，Inc. , 2007: 316-381．

17 KURODA S K. Discriminatively trained unscented Kalman filter for mobile robot localization[J]. Journal of Advanced Research in Mechanical Engineering, 2010, 1(3):153-161.

18 李勇，于韶辉.电液比例阀的双闭环控制技术[J].微特电机，2005，33(6)：35-36. LI Yong, YU Shaohui. Double feedback control of a proportional valve[J]. Small & Special Electrical Machines, 2005, 33(6):35-36. (in Chinese)

19 魏万迎，殷国富，田大庆.电动助力转向系统双闭环模糊PID控制系统分析[J].制造业自动化，2010,32(14):41-42. WEI Wanying, YIN Guofu, TIAN Daqing. Analysis on eps machitrical fuzzy dual closed-loop control system [J]. Manufacturing Automation, 2010, 32(14):41-42. (in Chinese)

20 胡炼，罗锡文，张霖，等.1PJ-4.0型激光平地机设计与试验[J/OL].农业机械学报，2014，45(4):146-150. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20140423&journal_id=jcsam. DOI:10.6041/j.issn.1000-1298.2014.04.023. HU Lian, LUO Xiwen, ZHANG Lin, et al. Development of 1PJ-4.0 laser leveler installed on a wheeled tractor for paddy field[J/OL].Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(4): 146-150. (in Chinese)

21 NXP INC. KMA199E product data sheet [DB/OL]. (2014-05-15)[2016-02-28]. http:∥www.nxp.com/acrobat_download/datasheets/ KMA199E_1.pdf, 2008.

22 Analog Devices Inc.ADIS16300 Datasheet [DB/OL]. (2013-04)[2016-01-28]. http:∥ www.analog.com/static/imported-files/data_sheets/ADIS16300.pdf.

23 FARRELL J A. Aided navigation：GPS with high rate sensors[M].New York: McGraw Hill，2006:35-60.

24 Luminary Micro, Inc. LM3S8962 microcontroller data sheet[DB/OL]. [EB/OL]. (2012-1-19) [2016-1-22]. http:∥www.luminarymicro.com.

25 GUO Linsong. Development of a low-cost navigation system for autonomous off-road vehicle [D].Urbana-Champaign: University of Illions, 2003.

26 周俊，张鹏，宋百华.农业机械导航中的GPS定位误差分析与建模[J].农业机械学报，2010，41(4)：189-192. ZHOU Jun, ZHANG Peng, SONG Baihua. Analysis and modeling of GPS positioning error for navigation of agricultural machinery[J]. Transactions of the Chinese Society for Agricultural Machinery,2010，41(4)：189-192. (in Chinese)

Automatic Navigation System of Tractor Based on DGPS and Double Closed-loop Steering Control

LI Yongjian1ZHAO Zuoxi2HUANG Peikui2GUAN Wei1WU Xiaopeng2

(1.DepartmentofMechanicalandElectricalEngineering,GuangdongAIBPolytechnicCollege,Guangzhou510507,China2.KeyLaboratoryofKeyTechnologyonAgriculturalMachineandEquipment,MinistryofEducation,SouthChinaAgriculturalUniversity,Guangzhou510642,China)

An agricultural automatic navigation system was designed on Dongfanghong X-804 tractor to improve the navigation control of agricultural machinery by using RTK-DGPS and double closed-loop steering control. The make-up of the whole navigation system and working principle were presented, among which their main features were: RTK-DGPS could offer positioning data, including heading, roll and pitch parameters, which were acquired from AHRS500GA, and the electro-hydraulic steering controller was developed for automatic steering control. Then, the system control strategy was analyzed and the control transfer function model was developed for trajectory tracking, with a double closed-loop control algorithm for steering system designed according to characteristic of the system nonlinear. The implementation description on an ARM9E-based embedded control system was provided in terms of electronics hardware design. Tests were conducted to examine the navigation system, including a straight line driving test on uneven road, which was to verify the effectiveness of the correction model. The test results showed that the proposed positioning and orientation evaluation algorithm could eliminate the effects of uneven field condition on GPS positioning and the average error of GPS positioning was reduced to 0.43°. Then a test of steering control system was carried out to verify the performance of double closed-loop control algorithm. Test results showed that the steering control system solved the control overshoot well and the average error was 0.40°. Finally, the field test results showed that the performance of automatic navigation system was improved, with average route tacking error was less than 0.019 m, average steering angle tracking error was 0.43° and standard deviation was less than 0.041 m. The field test results indicated that the proposed positioning evaluation algorithm and double closed-loop steering control algorithm on uneven field were appropriated to Dongfanghong X-804 tractor.

tractor; automatic navigation; steering system; tracking error; real time kinematic differential global positioning system; double closed-loop control

10.6041/j.issn.1000-1298.2017.02.002

2016-02-25

2016-09-01

国家自然科学基金项目(61175081)、国家重点研发计划项目(2016YFD0700101)和农业部948计划重点项目(2011-G32)

黎永键(1983—)，男，讲师，主要从事农业电气化与自动化研究，E-mail： leeeyong@qq.com

赵祚喜(1968—)，男，教授，博士生导师，主要从事农业机械与装备自动控制设计研究，E-mail： zhao_zuoxi@hotmail.com

S11+2

A

1000-1298(2017)02-0011-09