基于DGPS定位与双闭环转向控制的农业自动导航系统

黎永键，赵祚喜，高俊文，吴晓鹏，关伟

（1. 广东农工商职业技术学院，广东 广州 510507； 2. 华南农业大学工程学院，广东 广州 510642）

基于DGPS定位与双闭环转向控制的农业自动导航系统

黎永键1，赵祚喜2*，高俊文1，吴晓鹏2，关伟1

（1. 广东农工商职业技术学院，广东 广州 510507； 2. 华南农业大学工程学院，广东 广州 510642）

以东方红X-804拖拉机为平台，开发了一种基于RTK-DGPS定位和双闭环转向控制相组合的农业自动导航系统。系统主要包括RTK-DGPS接收机、姿态航向参考系统（AHRS）、转向控制器、电控液压转向装置和转向角检测传感器。设计了Kalman滤波器对定位数据进行平滑处理，同时实现航向角的校正。为实现自动转向，在拖拉机原有手动控制系统基础上加上电控比例液压阀，并设计电控单元。然后，推导了转向系统的数学模型，通过Matlab仿真工具箱得到传递函数的参数，设计了双闭环转向控制算法。最后，进行了算法验证试验和田间试验，结果表明，双闭环控制方法较好抑制了稳态时的震荡现象，方波信号的角度跟踪稳态时最大误差0.60°，平均误差0.40°，平均延时为0.20 s；设计的Kalman滤波器有助于提高定位系统的精度，横向跟踪误差不超过0.09 m，转向角度平均跟踪误差为0.43°，延时0.25 s。

农业自动导航；载波相位差分全球定位系统；姿态航向参考系统；卡尔曼滤波；双闭环控制

黎永键， 赵祚喜， 高俊文， 吴晓鹏， 关伟. 基于DGPS定位与双闭环转向控制的农业自动导航系统［J］. 农业现代化研究，2016， 37（2）： 387-394.

Li Y J， Zhao Z X， Gao J W， Wu X P， Guan W. Agricultural automatic navigation system based on DGPS positioning and double closed-loop steering control［J］.

Research of Agricultural Modernization， 2016， 37（2）： 387-394.

农业自动导航需要解决准确定位和自动转向控制的问题［1］。为提高农业导航定位精度，农业智能车辆田间导航定位需要测量载体位置坐标、航向角以及姿态角等导航参数，其测量值应尽量准确［2］。大量研究工作表明，Kalman滤波能有效解决惯性导航系统误差随时间积累的问题［3］。赵祚喜［4］等设计了基于GPS的组合惯性导航系统，利用“位置—速度—姿态”运动学模型建立了Kalman滤波器，使导航定位精度提高到0.05 m以内。但是，试验证明Kalman滤波的使用有以下局限性：研究物体的运动过程能够线性化表示，模型误差和传感器测量误差能够用高斯白噪声模型化；不满足以上条件，则不能提供最优估计［5］。因此，实现农业机械自动导航不仅需要Kalman滤波提供准确姿态信息，还需要对非线性的转向过程进行控制。

农业导航系统采用的控制方法一般为线性模型控制法、模糊控制法和最优控制等方法［6］。周建军等［7］提出了一种基于遗传算法的自适应模糊控制方法，将拖拉机自动导航的跟踪精度控制在10 cm以内。罗锡文等［8］设计了基于PID的农业自动导航控制器，采用跨行地头转向控制方式，直线跟踪的最大误差小于0.15 m。以上研究主要使用了PID控制算法，存在的问题是控制算法单一，通常只用位置传感器或者角速度传感器作为反馈信号，容易出现控制超调现象。近年来，双闭环控制方法在农业自动导航得到了应用。吴晓鹏［9］设计了双闭环PID控制算法，实现拖拉机自动转向控制，其信号跟踪误差最大不超过1.1°。

本文在东方红X-804拖拉机上开发了基于载波相位差分GPS（RTK-DGPS）与双闭环转向控制的自动导航系统。设计了Kalman滤波器对定位数据进行平滑，从而消除航向角偏差。对原有机械系统进行了电控液压系统改造，将拖拉机运动与转向模型相结合，设计双闭环转向控制系统实现拖拉机田间地头自动转弯。通过以上方法提高农业车辆田间导航控制精度。

1 自动导航控制系统结构

1.1 GPS与AHRS组合的导航定位系统

自动导航控制系统在东方红X-804拖拉机上进行设计，其主要组成部分包括全球定位系统（GPS）、姿态航向参考系统（AHRS）、转向控制系统、转向执行机构等。吴晓鹏和赵祚喜［10］介绍了拖拉机基本参数。GPS 设备是Trimble 公司的Trimble5700 型，包括GPS 天线与流动站接收机，支持RTK-DGPS定位，动态测量水平定位精度为±10 mm +1 ppm RMS，垂直定位精度可达±20 mm +1 ppm RMS［11］。AHRS使用由美国Crossbow公司生产高精度姿态航向参考系统AHRS500GA，其基本参数为：26位数字输出，测量信号包括姿态角、三轴角速度、三轴角速度、温度等，测量精度分别为：航向0.2° RMS，俯仰与横滚0.03° RMS［12］。

1.2 转向控制系统结构

为了实现东方红X-804田间自动导航作业，陈斌［13］对其转向系统进行了改造。在原机械式转向系统基础上并联一台由步进电机直接驱动的全液压转向器，执行机构包括电控液压阀、换挡电磁阀和溢流阀（图1）。系统工作过程为：当选择自动控制模式时，系统切换阀油路，由手动控制油路切换为自动控制油路。使用电控液压阀控制流量以调整转向速度，换向电磁阀控制油路切换以控制前轮转向，溢流阀作用是防止油压过载。

图1 东方红X-804电液控转向系统Fig. 1 Electro-hydraulic controlled steering system of Dongfanghong X-804 tractor

东方红拖拉机转向控制由安装在转向桥上的转向控制器实现，采用ADIS16300（美国ADI公司生成的MEMS惯性测量集成模块）和KMA199 （NXP公司生成的非接触磁阻效应传感器）联合测量。图2是转向控制系统的传感器部分安装示意图，传感器安装方法为：以转向轮转向轴为轴心，ADIS16300安装在与KMA199同轴的平面上，分别测量以转向轴为轴心的转向角速度和转向角。

图2 转向控制系统安装示意图Fig. 2 Layout of steering control system on tractor

1.3 自动导航控制系统的硬件实现

本文设计的导航系统硬件主要部分包括电源电路、传感器、换向阀驱动电路、比例阀驱动电路、SD卡存储电路等，上位机使用导航控制器AT91SAM9261（图3）。

1）KMA199。KMA199是一种非接触式的磁阻效应传感器，其基本原理：当有磁场作用于传感器时，磁阻就会发生改变，根据磁阻值的变化由数学关系即可推算出转角大小［14］。

图3 导航控制系统硬件框图Fig. 3 Structural diagram of the navigation control system

2）ADIS16300。ADIS16300是美国ADI公司生产的四自由度惯性检测系统，其内部集成了数字角速度传感器、三轴加速度传感器以及温度传感器，上电后以采样频率819.2 SPS测量载体角速度和三轴加速度，同时提供横滚角和俯仰角计算值［15］。

3）CAN总线控制器。CAN即控制总路线区域网络，具有结构简单、传输距离短、传输效率快、各节点实现自由通信的特点［16］。考虑到组合导航系统设计要求和拖拉机工作过程，选用CAN通讯网络作为导航控制网络。

4）主控芯片LMS8962。LMS8962是ARM7v7架构的Cortex-M3内核微处理器，具有运算能力强大、低功耗的特点，最高运算速度可达 70 MIPS［17］。从性能上看，LMS8962可作为本文转向控制系统的主控芯片。

2 Kalman滤波与双闭环转向控制算法

2.1 GPS与AHRS组合的导航定位校正方法

研究表明，农业车辆在凹凸不平的田地里行驶过程中，由于车体发生倾斜，GPS天线端与车体中心不重合，导致GPS定位出现较大偏差［18］。本文使用RTK-DGPS为主要导航手段，AHRS500GA辅助修正的方法对农业拖拉机导航参数进行测量。

对于坐标系，为表示方便，在三维位置矢量的右上方用符号表示该坐标系属性，其中大地坐标系（n），载体坐标系（b）。根据坐标转换关系，有以下式子成立：

式中：Xa、Ya、Za为真实的质心位置，Xg、Yg、Zg为GPS测得的天线位置。在大地坐标系中，（1）式右端第一项由GPS测得；在载体坐标系下，（1）式右端第二项由车体几何关系，测量获得。Cbn是方向余弦矩阵，作用是将定位误差投影到高斯平面坐标系下，该矩阵计算方法为［19］：

式中：φ、θ、ψ分别为农用车辆的横滚角、俯仰角和航向角。如果准确测得φ、θ、ψ的值，代入（2）式求出方向余弦矩阵，再代入式（1）即可车辆真实质心位置。

2.2 Kalman滤波器设计

在组合导航系统里，拖拉机的铁质材料对于AHRS500GA磁强计测量产生一定的影响，从而导致航向角测量值的偏移误差。设计Kalman滤波器可解决以上问题［20］。

Guo［21］介绍了应用于导航控制的拖拉机运动学模型。假设vk为车体速度，δk是时间τ内的转向轮偏转角，xk、yk表示K时刻位置坐标，T为采样周期，△ψk为航向偏移误差，由航位推算方法为：

将式（3）、式（4）进行三角函数展开，则：

由此设定车体前进速度为vk，vkx和vky分别是△ψk和△ψk+90°方向上投影，即vkx=vkcos△ψk，vky=vksin△k。Kalman滤波器的状态变量为：

1）设wk为输入噪声，系统状态方程为：

式中：wk是输入噪声，φk是系统矩阵，由下式计算：

2）观测方程为：

式中：Zk= ［xckyck］，vk是观测噪声，Hk是观测矩阵，由下式计算：

3）将线性离散Kalman滤波器的递归差分构成预测方程：

4）滤波方程为：

增益方程为：

滤波误差方差方程为：

Qk为输入噪声协方差矩阵，Rk为观测噪声协方差矩阵，通过对试验数据进行多元统计获得。经过多次反复试验，Qk和Rk分别取：

2.3 转向系统模型的建立

根据转向系统工作原理，将系统进行简化，主要部分包括油管、转向油缸、油箱和转向轮等（图4）。

图4 油缸运动学分析Fig. 4 Kinematics analysis of cylinder

对油缸运动进行动态分析，转向时液压油油管进入转向油缸，通过活塞驱动转向轮转动，转向过程中作用在转向轮上的外力为Ft。以活塞为研究对象，设A为油缸活塞横截面积，pL为负载压降，B、m分别为活塞和负载的粘性阻尼系数、质量，Q是流入转向油缸的液压油量，y是转向油缸活塞移动距离，则根据牛顿第二定律，平衡时上述变量满足关系式：

转向轮转角（δ）与油缸活塞位移（y）关系为：

转向轮转向角速度（ω）与油缸活塞位移（y）一阶导数关系为：

比例阀开度（x）与输入电流大小（u）关系为：

式中：kδ、kω、ku为对应式子比例系数。

在不考虑Ft情况下，对式（12）-（15）进行拉氏变换并联解得到系统传递函数，结果为二阶函数［22］。考虑到电控液压阀存在动作延时，并且转向轮等处存在摩擦，系统的传递函数改为转向角速度ω（s）与系统输入U（s）之间的传递函数Gω（s）、转向角度Φ（s）与系统输入U（s）之间的传递函数Gφ（s）。使用一个二阶惯性环节和一个延时环节表示为：

式中：Kt、Kb、Kc表示传递函数的未知参数，其确定方法为：首先进行开环阶跃响应，输入比例阀控制电流的阶跃信号，输出为转向角速率。然后通过Matlab系统辨识工具箱对转向系统进行辨识，得：Kt=0.405 8；Kb=6.945 8；Kc=3.788 1。

2.4 转向控制器的设计

考虑到转向系统的非线性特点，对传统PID算法进行改进，设计了基于双闭环控制方法的转向控制器（图5）。基本思路是：以转向角度控制为外环基础上，加入一个角速度控制的内环。外环是通过角度传感器测量转向轮实时转角（θR）与目标角度（θT）进行对比，由控制器1实现角度控制；内环是通过陀螺仪测量实时角速度（ωR）与目标角速度（ωT）进行对比，由控制器2实现速度控制。加入内环控制的目的是使得转向车轮能较平稳地以较低速度到达目标位置［23］。

1）外环转向角度控制。设eθ为转角误差，由PID算法可得到下式：

式中：KθP、KθI、KθD是外环角度PID控制的比例、积分和微分部分的系数，eθ_b为上一次的角度误差，eiθ为角度误差的积分，eiθ_b为上一次的角度误差积分，edθ为角度误差的微分，tS为采样时间。

图5 双闭环控制结构框图Fig. 5 Block diagram of double closed-loop control

2）内环角速度控制。系统输出量是由速度内环决策，即电控比例阀的输入电流量OI，ω为当前车轮转向的角速度。设eω为角速度误差，由PID算法可得：

式中：KωP、KωI、KωD是内环角速率PID控制的比例、积分和微分部分的系数，eω_b为上一次的角速率误差，eiω为角速率误差的积分，eiω_b为上一次的角速率误差积分，edω为角速率误差的微分，tS为内环采样时间。

联立式（18）-（23），即可得到输出量OI。在本文控制系统中，转向轮的角度和角速度分别通过角度传感器KMA199和ADIS16300陀螺仪测量得到。

3）参数估计方法。以Simulink为平台建立仿真模型，经过仿真得到系统传递函数的未知参数［24］。根据传递函数式，使用一个二阶惯性环节加上一个系统的延时，分别建立系统外环、内环的仿真模型。主要应用模块为：采用波形信号发生器产生关于角速率的目标方波信号；MATLAB Function在此次实现PID控制算法的实现；系统延时模块Transport Delay，延时时间常数设置τ=0.2 s。经过反复参数调整，得到角度外环的PID控制参数为：KθP=1.01，KθI=0.020，KθD=0.012。角速度内环的PID控制参数为：KωP=103.50，KωI=8.29，KωD=1.75。

3 验证试验与结果分析

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

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

试验方法：上位机以10 Hz频率发送控制指令方波信号，该信号为5º或10º阶跃信号，通过CAN通讯网络向转向控制器传输控制指令，下位机的执行频率同样10 Hz。运行本文设计的双闭环PID控制算法，考虑到之前的试验结果有较大的震荡超调现象，加入微分控制环节，观测控制信号追踪结果。图6为一次典型方波跟踪试验结果。

1）从图6（a）可以看出，对于5º或10º阶跃信号，跟踪角度与方波信号有良好一致性，较好抑制了超调振荡现象。图6（b）是方波信号跟踪误差，可知角度跟踪值未达到稳态时，出现类似于三角波的较大误差曲线，说明有明显震荡；当跟踪值达到稳态时，误差在0º附近波动，平均误差值为0.40º，最大误差值0.60º。最大跟踪时间1.6 s，最小跟踪时间1.0 s，平均跟踪时间为1.3 s，平均延时0.2 s。可见，稳态的振荡有较好的抑制。

2）图6（c）是图6（a）在方波信号跟踪时的角速率跟踪情况。由图6（d）可以看出，跟踪角速率与目标角速率曲线有良好一致性，统计数据表明稳态时平均误差1.25 °/s，延时时间平均值为0.2 s。振荡较大时存在角速率跟踪超调现象，最大误差达到5 °/s。

图6 方波信号响应与跟踪误差Fig. 6 Square signal response and tracking error

3）综上所述，当转向角度与目标角度相差较大时，应控制车速为较大值；当转向角度接近目标角度时，速度应降低，以一个较小的速度平缓达到目标角度。但是在换向和进入稳态前角速率控制出现了较大的超调现象，角速率控制尚存不足。

3.2 田间验证试验

试验目的：验证本文设计Kalman滤波器与双闭环转向控制系统在拖拉机实际工作过程中能有效提高导航控制精度，拖拉机能按照预先规划的路径自动转向和行驶。

试验方法：试验在华南农业大学的一块试验田中进行，实验的内容包括无人驾驶拖拉机直线行驶和地头转向。按以下步骤进行路径规划：

1）首先长方形的试验田四个顶点作为定点，分别记为A、B、C、D，使用RTK-DGPS测定每个点的坐标。

2）以AD构成的边为基准线，划定总数15条平行作业线，行与行之间的距离相同，由拖拉机作业间距决定。

3）按照地头转向的设计要求，编制作业顺序。直线跟踪过程中，拖拉机的机械速度设定为0.5 m/s；地头转向过程中，拖拉机机械速度设定为0.2 m/s。RTK-DGPS与AHRS500GA组合系统实时提供拖拉机的位置和姿态信息，经过Kalman滤波获得高精度的导航定位数据。拖拉机的直线行走和地头转向使用了双闭环PID控制，以尽量按预定轨迹行驶。在田间作业过程中，由工控机记录下GPS以及转向系统的数据，CAN总线实现数据通信。按此方法拖拉机进行多次田间行走，启实时记录并储存数据。

将拖拉机行走实时GPS定位坐标点到当前作业行坐标的距离定义为横向跟踪误差（LTE）。在获取LTE参数的基础上，对LTE进行统计分析，以LTE平均值LTEavg、最大值LTEmax以及标准差σLTE作为路径跟踪效果的评价指标，计算方法为：

试验结果分析：图7是本文设计方法下东方红拖拉机在田间作业时的定位追踪与转向角跟踪情况。图7（a）反映了目标转向角度（Target angle）与转向轮实际转角（KMA Angle）关系，其中Target angle控制指令由上位机发送，KMA Angle则是KMA199传感器测量结果。从图中可知，转向系统在-5°至25°转角范围内转向动作执行情况良好，平均跟踪误差为0.43º，误差值控制在合理范围内。

图7 拖拉机田间试验结果Fig.7 Result of the field test on tractor

图7（b）是拖拉机田间行驶实际轨迹与规划路径比较。对实验数据进行统计分析（表1），路径跟踪的精度指标和不采用Kalman滤波处理相比，直线跟踪时，分别由0.044 m、0.136 m和0.056 m减小到0.020 m、0.090 m和0.020 m；地头转向跟踪时，m、0.082 m和0.115 m。可知，Kalman滤波算法和分别由0.041 m、0.088 m和0.019 m减小到0.018 双闭环PID转向控制方法能有效提高导航控制精度和稳定性。但仍存在有问题，如田头转弯过程出现一定的控制延滞，平均延时为0.25 s；在转向角度较大的区域，跟踪误差较大。

表1 试验跟踪误差统计结果Table 1 Results of the experimental tracking error

4 结论

本文提出基于RTK-DGPS定位与双闭环转向控制的农业自动导航系统，设计了适用于农业机械田间作业的Kalman滤波方法以及转向控制器。测试结果表明，转向轮能按照转向控制系统发出的指令达到目标角度和角速度，稳态跟踪误差不超过1°。拖拉机能按照预先规划的路径自动转向和行驶，直线行驶的横向跟踪误差不超过0.09 m，地头转向的横向跟踪误差不超过0.088 m。有效提高GPS定位精度。

采用基于ARM7v7架构的Cortex-M3内核微处理器LMS8962实现了以上滤波算法与控制算法，研制了导航控制器，在精细农业的实践中有良好的推广前景。

［1］ 蒋浩然， 陈军， 王虎， 等. 移动机器人自动导航技术研究进展［J］.西北农林科技大学学报（自然科学版）， 2011， 39（12）： 207-209. Jiang H R， Chen J， Wang H， et al. Research progress of automatic navigation technology for orchard mobile robot［J］. Journal of Northwest A & F University （Natural Science Edition）， 2011，39（12）： 207-209.

［2］ 胡炼， 罗锡文， 张霖， 等. 1PJ-4.0型激光平地机设计与试验［J］.农业机械学报， 2014， 45（4）： 146-150. Hu L， Luo X W， Zhang L， et al. Development of 1PJ-4.0 laser leveler installed on a wheeled tractor for paddy field［J］. Transactions of the Chinese Society for Agricultural Machinery，2014， 45（4）： 146-150.

［3］ 秦永元. 惯性导航［M］ . 北京： 科学出版社， 2014： 243-259. Qin Y Y. Inertial Navigation［M］. Beijing： Science Press， 2014：243-259.

［4］ 黎永键， 赵祚喜， 高俊文. 基于GPS/SINS组合的农业导航定位系统设计与研究［J］. 农机化研究， 2014， 36（3）： 16-22. Li Y J， Zhao Z X， Gao J W. Design and research of agricultural navigation positioning system based on GPS and SINS［J］. Journal of Agricultural Mechanization Research， 2014， 36（3）： 16-22.

［5］ 邓自立， 王欣， 高媛. 建模与估计［M］. 北京： 科学出版社，2008： 126-135. Deng Z L， Wang X， Gao Y. Modeling and Estimation［M］. Beijing：Science Press， 2008： 126-135.

［6］ 刘豹， 唐万生. 现代控制理论［M］. 北京： 机械工业出版社，2013： 188-193. Liu B， Tang W S. Modern Control Theory［M］. Beijing： China Machine Press， 2013： 188-193.

［7］ 周建军，张漫，刘刚， 等. 基于模糊控制的农用车辆路线跟踪［J］. 农业机械学报， 2009， 40（4）： 151-155. Zhou J J， Zhang M， Liu G， et al. Path tracking for agricultural vehicle based on fuzzy control［J］. Transactions of the Chinese Society for Agricultural Machinery， 2009， 40（4）： 151-155.

［8］ 罗锡文， 张智刚， 赵祚喜， 等. 东方红X-804拖拉机的DGPS自动导航控制系统［J］. 农业工程学报， 2009， 25（11）： 139-143. Luo X W， Zhang Z G， Zhao Z X， et al. Design of DGPS navigation control for Dongfanghong X-804 tractor［J］. Transactions of the Chinese Society of Agricultural Engineering， 2009， 25（11）： 139-143.

［9］ 吴晓鹏. 东方红拖拉机转向伺服控制系统设计［D］. 广州： 华南农业大学， 2011. Wu X P. Steering control system design for Dongfanghong X-804 tractor［D］. Guangzhou： South China Agricultural University，2011.

［10］ 吴晓鹏， 赵祚喜. 东方红拖拉机自动转向系统设计［J］. 农业机械学报， 2009（S1）： 1-5. Wu X P，Zhao Z X. Development of automatic steering control system based on Dongfanghong tractor［J］. Transactions of the Chinese Society for Agricultural Machinery， 2009（S1）： 1-5.

［11］ 张智刚. 插秧机的DGPS 自动导航控制系统研究［D］. 广州： 华南农业大学， 2006. Zhang Z G. Research of DGPS navigation control system for rice transplanter［D］. Guangzhou： South China Agricultural University， 2006.

［12］ Crossbow Technology， Inc. AHRS500GA-Series User's Manual：Revision B［M］. New York： McGraw Hill， 2007： 21-25.

［13］ 陈斌. 东方红X-804拖拉机自动转向控制系统研究与设计［D］.广州： 华南农业大学， 2009. Chen B. Research and design of steering control system for Dongfanghong X-804 tractor［D］. Guangzhou： South China Agricultural University， 2009.

［14］ NXP， Inc. KMA199E product data sheet［DB/OL］. （2014-11-01）［2015-04-01］. http：//www.nxp.com/acrobat_download/datasheets/ KMA199E_1.pdf.

［15］ Analog Devices， Inc. ADIS16300 Datasheet［DB/OL］. （2009-04-01）［2015-01-28］. http：//www.analog.com/static/imported-files/ data_sheets/ADIS16300.pdf.

［16］ Bosch， Inc. Bosch CAN user's guide［EB/OL］. （2013-09-01）［2015-02-01］. http：//www.xhl.com.cn/sjsc/sjscdetail.asp？sid=25.

［17］ Luminary Micro， Inc. LM3S8962 microcontroller datasheet［DB/ OL］. （2010-01-19）［2015-01-22］. http：//www.luminarymicro.com.

［18］ 赵汝祺， 赵祚喜， 赵汝准. 基于卡尔曼滤波器的车辆定位试验［J］. 农业工程学报， 2012， 28（18）： 49-54. Zhao Y Q， Zhao Z X， Zhao Y Z. Research and experiment on vehicle localization based on polynomial Kalman filter［J］. Transactions of the Chinese Society of Agricultural Engineering，2012， 28（18）： 49-54.

［19］ 孙冬梅， 田曾山， 韩宁军. 捷联惯导系统中四元素法求解姿态角仿真模拟［J］. 弹箭与制导学报， 2009， 29（1）： 51-60. Sun D M， Tian Z S， Han L J. Simulation on quaternion calculate attitude angle of the strapdown inertial navigation system［J］. Journal of Projectiles， Rockets， Missiles and Guidance， 2009，29（1）： 51-60.

［20］ Lu J J， Zhou J， Lu X D， et al. The measurement correction of MEMS gyroscope based interlaced Kalman filter［C］. Advanced Materials Research， 2011： 1083-1088.

［21］ Guo L S. Development of a low-cost navigation system for autonomous off-road vehicle［D］. Urbana-Champaign： University of Illinois， 2003.

［22］ Gordon M. Robot Builder's Sourcebook［M］. New York： McGraw Hill eBook， 2003： 577-581.

［23］ Sakai A， Kuroda Y. Discriminatively trained unscented Kalman filter for mobile robot localization［J］. Journal of Advanced Research in Mechanical Engineering， 2010， 1（3）： 153-161.

［24］ Jay A F. Aided Navigation： GPS with High Rate Sensors［M］. New York： McGraw Hill， 2006： 35-60.

（责任编辑：童成立）

Agricultural automatic navigation system based on DGPS positioning and double closed-loop steering control

LI Yong-jian1， Zhao Zuo-xi2， GAO Jun-wen1， WU Xiao-peng2， GUAN Wei1

（1. Guangdong AIB Polytechnic College， Guangzhou， Guangdong 510507， China； 2. College of Engineering， South China Agricultural University， Guangzhou， Guangdong 510642， China）

An agricultural automatic navigation system based on real time kinematic differential global positioning system （RTK-DGPS） and double closed-loop steering control was developed on Dongfanghong X-804 tractor. The system includes RTK-DGPS receiver， attitude and heading reference system （AHRS）， steering controller， electrohydraulic steering actuator and encoder sensor for steering angle testing. One Kalman filter was designed to smooth the collected GPS positioning data and evaluate the bias error of the heading angle. In order to realize automatic steering， an electro-hydraulic proportional valve was added to the original manually operated steering system and the electronic control unit was designed. Then the mathematical model of steering system was derived， using Matlab System Identification Toolbox to estimate transfer function parameters， and a double closed-loop control algorithm for steering was designed. Finally， experimental results of square signal tracking test and field test are presented， showing that the double closed-loop control solved the control overshoot well and the maximum error is 0.60°， average error 0.40°， average delay 0.20 s. The designed Kalman filter is helpful to improve the accuracy of positioning system and the maximum lateral tracking error is less than 0.09 m， average steering angle tracking error 0.43°， average delay 0.25 s.

agricultural automatic navigation； real time kinematic differential global positioning system （RTK-DGPS）；attitude and heading reference system （AHRS）； Kalman filter； double closed-loop control

National Natural Science Foundation of China （61175081）； 948 Key Project of Ministry of Agriculture （2011-G32）； Doctoral Project （20114404110003）.

ZHAO Zuo-xi， E-mail： zhao_zuoxi@scau.edu.cn.

26 May， 2015； Accepted 20 October， 2015

S219.02

A

1000-0275（2016）02-0387-08

10.13872/j.1000-0275.2015.0186

国家自然科学基金项目（61175081）；农业部948计划重点项目（2011-G32）；博士点基金项目（20114404110003）。

黎永键（1983-），男，广东清远人，硕士，讲师，主要从事农业电气化与自动化研究，E-mail： liyj@gdaib.edu.cn；

赵祚喜（1968-），男，湖南慈利人，博士，教授，主要从事农业机械导航技术研究，E-mail： zhao_zuoxi@scau.edu.cn。

2015-05-26，接受日期：2015-10-20