基于BDS的果园施药机自动导航控制系统

熊 斌 张俊雄 曲 峰 范智棋 王大帅 李 伟

(中国农业大学工学院， 北京 100083)

基于BDS的果园施药机自动导航控制系统

熊 斌 张俊雄 曲 峰 范智棋 王大帅 李 伟

(中国农业大学工学院， 北京 100083)

根据果园施药机自动导航喷药作业需求，设计了一种基于北斗卫星导航系统(BDS)的施药机自动导航控制系统。该系统主要包括RTK-BDS接收机、导航控制器、转向控制器、电控气动转向系统和三轴电子罗盘。其中转向控制器、电控气动转向系统和三轴电子罗盘构成转向角闭环控制回路，该回路可根据导航控制器发送的施药机期望转向角实现转向角的随动控制。将施药机的运动学模型和纯追踪模型相结合，设计了施药机直线跟踪导航控制器，输入为位置坐标和方位角，输出为期望转向角。针对果园地形特点对施药机进行了导航路径规划，并在果园进行现场试验。试验结果表明：所设计的导航控制系统，在施药机行进速度为2 km/h时，直线跟踪最大误差不大于0.13 m，平均跟踪误差不大于0.03 m，能满足果园自动导航作业精度要求。

果园； 施药机； 自动导航； 控制系统； 北斗卫星导航系统

引言

我国作为水果生产大国，但果园种植、管理以及果实采收等工序机械化程度低、劳动强度大、成本高。果园机械的智能化导航，可以减轻果农劳动强度，提高果园作业效率和质量，对水果生产具有重要的研究意义和实际应用价值[1-4]。随着差分式卫星定位技术的不断发展，卫星定位精度不断提高，载波相位差分卫星定位精度可达厘米级，越来越多学者已着力研究卫星定位技术在农业机械自动导航中的应用[5-12]。

日本国家农业研究中心利用DGPS(Differential global positioning system)和卡尔曼滤波器对耕地用拖拉机进行导航试验，测量结果表明直线位置误差为0.1 m,转弯误差则为0.12 m[13]。YOSHISADA等[14]将RTK-DGPS和光纤陀螺应用于水田插秧机的直线跟踪导航，插秧机在正常作业速度条件下，最大跟踪误差小于0.12 m，均方根误差小于0.055 m。斯坦福大学的MICHAEL O’ Connor等为约翰迪尔7800型拖拉机开发了一套GPS导航系统，在拖拉机驾驶室的上方安装了4个单通道的GPS传感器，接收器以10 Hz的频率进行姿态测量，航向响应小于1°，直线跟踪标准偏差小于2.5 cm[15]。ZHANG等[16]运用RTK-GPS接收器和光纤陀螺仪，探索了拖拉机自动导航中的动态路径搜索算法来满足农业作业的需求。张智刚等[17]将计算机技术、传感器技术、GPS技术和数据通信技术等集成和融合，在久保田插秧机上开发了基于DGPS和电子罗盘的导航控制系统，提出了一种利用航向跟踪实现路径跟踪的控制方法。罗锡文等[18]将RTK-DGPS定位技术应用于东方红X-804型拖拉机自动导航，设计开发了可电控的液压转向闭环控制回路，将拖拉机运动模型和液压转向控制模型相结合，设计了直线跟踪的PID导航控制器。周建军等[19]为提高农机车载GPS和DR组合导航系统定位的精度，将模糊逻辑和卡尔曼滤波相结合，研究了模糊自适应卡尔曼滤波算法，在线修正系统量测噪声协方差阵，在改装农机车上试验表明：系统使用模糊自适应卡尔曼滤波后，x方向和y方向平均误差分别为0.13 m和0.20 m。郭娜等[20]以洋马VP6型高速插秧机为试验平台，对主变速操作、插植操作机构和变速机构进行电控改造，在此基础上设计了基于GPS导航的田间作业控制系统。

北斗卫星导航系统是中国自主研发的全球卫星导航系统，可在全球范围内全天候、全天时为用户提供高精度、高可靠定位导航服务，精度最高可达厘米级。本文采用RTK-BDS定位技术，研究果园喷药机自动导航系统。设计可电控的气动转向闭环控制回路，并进行闭环测试。将喷药机运动模型和纯追踪模型结合，设计直线跟踪的导航控制器，通过果园试验验证控制系统的精确性和可靠性。

1 自动导航系统

1.1 系统组成

施药机自动导航系统由RTK-BDS定位传感器、导航控制器、电控气动转向系统、转向控制器、三轴电子罗盘和施药机本体等组成，如图1所示。

图1 施药机自动导航系统构成图Fig.1 Structure sketch of spraying robot navigation system1.车载电台天线 2.工控机 3.气泵 4.前小盘天线 5.柱塞泵 6.药箱 7.喷头 8.后小盘天线 9.自走式底盘车 10.控制箱 11.气动三联件

施药机由履带自动式底盘和喷雾系统组成，属于手动挡汽油机，速度范围为0.5～6.1 km/h，两履带中心距为1 m，长度为1.9 m，通过拉动转向离合切断左轮或者右轮脱离运动力主轴，实现履带车差速转向。定位传感器采用华测公司N71J型BDS接收机系统，包括BDS天线、基准站接收机、移动站接收机和数传电台组成。该系统采用RTK-BDS定位，水平定位精度可达0.008 m+10-8RMS，垂直定位精度可达0.015 m+10-8RMS，动态延迟小于25 ms。三轴电子罗盘采用PNI公司TCM5型倾角补偿3轴电子罗盘，倾斜角精度为0.3°RMS，俯仰角精度为0.2°RMS，横滚角精度为0.5°RMS，分辨率为0.1°。

1.2 气动转向闭环控制系统

气动转向闭环控制系统由气动转向回路、转向机构、转向控制器和三轴电子罗盘组成，属于闭环系统。

转向机构主要由2个气缸、钢丝绳和左右转向离合器组成，如图2所示。气缸收缩时，拉动钢丝绳，钢丝绳拉动转向离合器，从而控制左或右轮脱离运动力主轴，使该轮停转，实现施药机差速转动。气缸伸出时，转向离合器在弹簧作用下复位，使左或右轮与运动主轴啮合，继续转动。气缸拉动左转向离合器时，施药机左转；反之，施药机右转；气缸同时拉动左右转向离合器时，施药机停止运动。

图2 转向机构构成图Fig.2 Structure sketch of steering mechanism1.弹簧 2.右转向离合 3.钢丝绳 4.右转向气缸 5.左转向气缸 6.施药机机架 7.左转向离合

气路系统回路图如图3所示，左右换向阀采用二位五通电磁阀，分别控制左右转向气缸动作，从而控制喷药机转弯方向。比例阀实现流量控制，用于控制履带转速,继而控制施药机转向速度快慢。气泵选用直流双缸充气泵，长时间连续工作会导致严重发热甚至烧坏，故选用电接点压力表和储气罐控制气泵间歇性工作,电接点压力表压力上下限分别设置为0.55 MPa和0.2 MPa，储气罐容量为6 L，转向气缸正常工作要求系统压力大于0.15 MPa，当系统压力低于0.2 MPa时气泵开启工作，当系统压力高于0.55 MPa时气泵停止工作。气泵停止工作期间系统压力大于0.2 MPa，满足转向气缸正常工作要求，因此可满足施药机持续作业模式。溢流阀起安全保护作用。

图3 气动系统回路图Fig.3 Circuit diagram of pneumatic system1.空气过滤器 2.储气罐 3.消声器 4.溢流阀 5.气泵 6.电接点压力表 7.单向阀 8.减压阀 9.油雾器 10.右换向阀 11.右电气比例阀 12.右转向气缸 13.左电气比例阀 14.左换向阀 15.左转向气缸

自动转向原理是施药机沿着预先设定路径前进，导航控制器由建立纯路径跟踪控制算法计算出实时期望转向角发送给转向控制器，然后转向控制器输出控制信号，通过控制换向阀和比例阀来控制转向气缸动作，实现施药机转向闭环控制。

转向控制器在转向控制过程中使用PID控制算法，如图4所示，θ1为上位机发送的期望转向角，θ为施药机实际转向角，θ2为电子罗盘测得的转向角，e为输入期望转向角和测得的转向角之间的偏差。转向控制器通过分析e，设计控制算法和程序，控制比例换向阀动作，实现电控自动转向。

图4 自动转向闭环控制系统总体结构框图Fig.4 Structure diagram of closed loop steering control system

考虑到施药机在果园作业时大部分时间都是直线跟踪行走，转向角一般比较小，本文通过一个±10°的阶跃响应测试，测得施药机在2 km/s行进速度下气动转向闭环回路的动态响应性能，阶跃响应测试结果如图5所示。由阶跃测试结果可知，平均超调量为15.3%，平均峰值时间为0.54 s，转向闭环控制回路视为二阶系统。

图5 气动转向系统的动态响应Fig.5 Dynamic response of pneumatic steering system

1.3 RS232通信

自动导航系统采用RS232进行通信。转向控制器采用ATmega128单片机，实现航偏角数据采集和转向控制。导航控制器采用台达工控机，实现BDS定位数据采集、处理和转向控制指令决策。系统总体结构如图6所示。

图6 自动导航系统总体结构框图Fig.6 Structure diagram of automatic navigation system

1.4 软件运行流程

在工控机上用LabView搭建人机交互界面，界面用于接收BDS数据和电子罗盘数据，根据路径跟踪控制算法计算出期望转向角并发送给转向控制器。转向控制器将接收的期望转向角与测得实际转向角比较，根据偏差的大小进行PID控制。定位传感器发送BDS数据频率为10 Hz，电子罗盘发送数据频率为20 Hz，工控机发送期望转向角频率为10 Hz。施药机导航作业软件运行流程如图7所示。

图7 软件运行流程图Fig.7 Flow chart of software operation

2 导航控制器设计

2.1 喷药机运动模型

施药机底盘是履带式的，转向时内侧转向离合器彻底分离，牵引负荷完全由外侧履带承担。如图8所示，履带车不带负荷在水平地面上以转弯半径R绕轴线O作低速稳定转向，OT是轴线O在水平地面上的投影，C点为履带车坐标控制点，由于低速离心力可忽略不计，设履带车角速度为ω，方位角为ψ，方位角定义是北极方向沿顺时针方向与施药机纵轴线前进方向夹角，由运动几何关系可得

(1)

(2)

式中ψ1——t时刻施药机的方位角ψ2——t+Δt时刻施药机的方位角θ——从t时刻到t+Δt时刻施药机转向角变化值

R——施药机转弯半径

L——两履带中心距

由式(1)关系得到，转向角变化值等于方位角变化值，因此可以通过检测方位角来实现转向角的精准控制。由式(2)可知履带车的转弯半径是一个固定值，通过测量两履带中心距L为1 m，可以计算出R为0.5 m。由喷药机运动模型可知，喷药机以固定转弯半径进行转向，可以用检测方位角来对转向角实现精确控制。

图8 履带车转向运动简图Fig.8 Steering motion diagram of tracked vehicle

2.2 纯追踪模型

纯追踪模型是一种基于几何原理的计算方法，用来计算车辆在运动中从当前位置到目标点走过的弧形轨迹。车辆在以固定角度转弯时，所走过的轨迹为圆周上的一段弧线，施药机整个行走轨迹可以看成由无数段圆弧组成，只要确定了不同时间段轨迹弧线的圆心和半径就可以得到行驶的整个轨迹，这样就可以通过控制施药机转向角来改变行驶轨迹，使车辆沿着预期路径前进。如图9所示，施药机沿着设定轨迹直线AB导航，实际运动轨迹由各弧线段组成，由当前点C(XC,YC) 和方位角ψC可确定当前弧线段的圆心O(XO,YO)，由转弯半径为0.5 m可求出目标点P(XP,YP)，因此Δψ可以求出，再根据式(1)可求出期望方位角ψ2，即期望转向角值。

图9 纯追踪模型Fig.9 Model of look-ahead dynamic path search

3 导航路径规划

针对果园地形特点及种植模式，结合施药机转弯半径小的特点，设计施药机器人导航路径，如图10所示，带箭头轨迹线代表施药机预设导航路径，点0为初始位置，点1～5为地头转弯点，点A为施药机器人导航作业时任意一点，点A到预设轨迹的偏距为D，D用于衡量直线跟踪误差，D0为施药机器人作业时不碰到果树的最大安全距离，施药机器人正常导航作业时D需小于D0。施药机在直线段沿着预设路径导航，到地头转弯点时转弯约90°。

图10 导航路径规划图Fig.10 Sketch of navigation path planning

4 果园导航试验

为检验施药机性能和作业效果，在北京市通州区国际种业园樱桃果园内进行导航试验，如图11所示。樱桃果园种植行间距为5 m，垄长70 m，考虑到樱桃枝干伸出部分，施药机器人垄间实际可行走区域宽度约为2.5 m，施药机器人整体宽度尺寸为1.8 m，因此，施药机器人不碰到果树的最大安全距离为0.3 m。选取樱桃果园连续3垄地作为试验地，试验前用采点仪器采集规划路径上点0～5的坐标，计算各导航直线段方程，沿着3垄地完成导航，记作1次试验，施药机前进速度为1、2、3、4 km/h各做5次试验，并记录试验数据。

图11 施药机器人导航试验现场Fig.11 Navigation test site of spraying robot

施药机器人在不同前进速度下，5次试验都能在不撞到果树的情况下完成导航作业，取前进速度为2 km/h时第1次试验的导航坐标点数据进行处理，可得到施药机导航轨迹图，如图12所示。

图12 施药机导航轨迹Fig.12 Navigation track of spraying machine

对不同前进速度下施药机行走轨迹上的横向跟踪误差进行统计分析，统计结果如表1所示。横向跟踪误差定义为实时BDS定位坐标点到当前作业行的距离。

从统计结果可以看出，施药机前进越快，最大直线跟踪误差越大。前进速度为2 km/h时为最优速度，转向速度较快，直线跟踪误差较小，此时最大直线跟踪误差不超过0.13 m，平均跟踪误差不超过0.03 m，这表明所设计的导航控制系统有较高的控制精度和稳定性。施药机能沿着预设路径进行直线跟踪和地头自动转弯作业，表明该路径规划适用于施药机导航作业。

表1 横向跟踪误差统计结果Tab.1 Statistical results of offset error

5 结论

(1)使用RTK-BDS接收机实时提供的导航定位数据，采用纯追踪的路径跟踪算法，运用PID转向控制方法，实现了施药机直线跟踪导航和地头转向控制。

(2)以履带自走式底盘车为施药机移动载体，根据履带车转向原理设计了电控气动转向控制装置，通过闭环控制实现精准转弯，气动装置提高了转向机构的响应速度。

(3)果园导航试验表明，施药机器人可有效实现果园精准直线导航，并可自主完成地头自动换垄，在前进速度为2 km/h时，直线跟踪的最大误差不大于0.13 m，平均跟踪误差不大于0.03 m，满足果园施药作业要求。

1 顾家冰,丁为民,邱威,等.果园变量施药机械及施药技术研究现状与趋势[J].果树学报,2014,31(6):1154-1157. GU Jiabing, DING Weimin, QIU Wei, et al. Current research situation and development trend of equipment and technology for orchard spraying[J]. Journal of Fruit Science, 2014,31(6):1154-1157. (in Chinese)

2 张海锋,许林云.果园喷雾机发展现状及展望[J].中国农机化学报,2014,35(3):112-118. ZHANG Haifeng, XU Linyun. Summary of research status on orchard sprayer[J]. Journal of Chinese Agricultural Mechanization,2013,35(3):112-118. (in Chinese)

3 常有宏,吕晓兰,蔺经,等.我国果园机械化现状与发展思路[J].中国农机化学报,2013,34(6):21-26. CHANG Youhong, LV Xiaolan, LIN Jing, et al. Present state and thinking about development of orchard mechanization in China[J]. Journal of Chinese Agricultural Mechanizaiton, 2013,34(6):21-26. (in Chinese)

4 慕军营,戚树腾,陈军,等. 自动导航系统在农业中的应用及果园适用性分析[J].农机化研究,2014(7):6-12. MU Junying, QI Shuteng, CHEN Jun, et al. Application of automatic navigztion system in agricultural machinery and the analysis of applicability in orchard[J]. Journal of Agricultural Mechanization Research, 2014(7):6-12. (in Chinese)

5 姬长英,周俊.农业机械导航技术发展分析[J/OL].农业机械学报,2014,45(9):44-54. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20140908&flag=1.DOI=10.6041/j.issn.1000-1298.2014.09.008. JI Changying,ZHOU Jun. Current situation of navigation technologies for agricultural machinery [J/OL].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):44-54. (in Chinese)

6 李建平,林妙玲.自动导航技术在农业工程中的应用研究进展[J].农业工程学报,2006,22(9):232-236. LI Jianping,LIN Miaoling. Research progress of automatic guidance technologies applied in agricultural engineering [J].Transactions of the CSAE,2006,22(9):232-236． (in Chinese)

7 胡静涛,高雷,白晓平,等.农业机械自动导航技术研究进展[J].农业工程学报,2015,31(10):1-10. HU Jingtao,GAO Lei,BAI Xiaoping,et al. Review of research on automatic guidance of agricultural vehicles[J]. Transactions of the CSAE,2015,31(10):1-10. (in Chinese)

8 伟利国,张权,颜华,等.XDNZ630型水稻插秧机GPS自动导航系统[J].农业机械学报,2011,42(7):186-190. WEI Liguo,ZHANG Quan,YAN Hua,et al. GPS automatic navigation system design for XDNZ630 rice transplanter [J]. Transactions of the Chinese Society for Agricultural Machinery,2011,42(7):186-190. (in Chinese)

9 周建军,王秀,张睿,等.农机车载GPS和DR组合导航系统定位方法[J/OL].农业机械学报,2012,43(增刊):262-265.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=2012s54&flag=1.DOI:10.6041/j.issn.1000-1298.2012.S0.054. ZHOU Jianjun,WANG Xiu,ZHANG Rui,et al. GPS/DR integrated navigation positioning method for agricultural machinery[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2012,43(Supp.):262-265. (in Chinese)

10 纪朝凤,刘刚,周建军,等.基于CAN总线的农业车辆自动导航控制系统[J].农业机械学报,2009,40(增刊):28-32. JI Chaofeng,LIU Gang,ZHOU Jianjun,et al. System of agricultural vehicles based on CAN bus[J].Transactions of the Chinese Society for Agricultural Machinery,2009,40(Supp.):28-32. (in Chinese)

11 周建军,郑文刚,李素基,等.基于ISO 11783的拖拉机导航控制系统设计与试[J].农业机械学报,2010,41(4):184-188. ZHOU Jianjun,ZHENG Wengang,LI Suji,et al. Navigation control system for tractor based on ISO 11783[J]. Transactions of the Chinese Society for Agricultural Machinery,2010,41(4):184-188. (in Chinese)

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

13 TORII T. Research in autonomous agriculture vehicles in Japan [J].Computers and Electronics in Agriculture,2000,25(1-2):133-153.

14 YOSHISADA N,UMEDA N,KANETAI Y,et al. Autonomous guidance for rice transplanting using global positioning and gyroscopes [J].Computers and Electronics in Agriculture,2004,43(3):223-234.

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

16 ZHANG Q,QIU H. A dynamic path search algorithm for tractor automatic navigation [J].Transactions of the ASAE, 2004,47(2):639-646.

17 张智刚,罗锡文,周志艳,等.久保田插秧机的GPS导航控制系统设计[J].农业机械学报,2006,37(7):95-97,82. 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,82. (in Chinese)

18 罗锡文,张智刚,赵祚喜,等.东方红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)

19 周建军,王秀,张睿,等.农机车载GPS和DR组合导航系统定位方法[J/OL].农业机械学报,2012,43(增刊):262-265. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=2012s54&flag=1.DOI:10.6041/j.issn.1000-1298.2012.S0.054. ZHOU Jianjun,WANG Xiu,ZHANG Rui,et al. GPS /DR integrated navigation positioning method for agricultural Machinery [J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(Supp.):262-265. (in Chinese)

20 郭娜,胡静涛,王鹤.基于GPS导航的插秧机作业控制系统[J/OL].农业机械学报,2013,44(1):200-204. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20130138&flag=1.DOI:10.6041/j.issn.1000-1298.2013.01.038. GUO Na,HU Jingtao,WANG He. Intelligent operation control system for rice transplanter based on GPS navigation [J/OL]. Transactions of the Chinese Society for Agricultural Machinery,2013,44(1):200-204. (in Chinese)

Navigation Control System for Orchard Spraying Machine Based on Beidou Navigation Satellite System

XIONG Bin ZHANG Junxiong QU Feng FAN Zhiqi WANG Dashuai LI Wei

(CollegeofEngineering，ChinaAgriculturalUniversity，Beijing100083，China)

According to the demand of automatic navigation of orchard spraying machine, a navigation control system for spraying robot based on Beidou navigation satellite system(BDS)was designed. The system included RTK-BDS receiver, navigation controller, steering controller, electro pneumatic steering system, three dimensional electronic compass. The closed control loop of steering angle was composed of steering controller, electro pneumatic steering system, three dimensional electronic compass. The closed loop could realize the servo control of steering angle according to control command from navigation controller. The kinematics and dynamics models of spraying machine were analyzed and a look-ahead dynamic path search model was established. The input of the look-ahead dynamic path search model was lateral tracking error and the output was steering angle from navigation controller. Navigation path planning on the spraying machine was designed according to the characteristics of the orchard, and field tests were carried out in the orchard. The tests were done at different traveling speeds, which showed the offset error became bigger with the increase of traveling speed and the traveling speed of 2 km/h was the optimal speed for quick steering speed and small offset error. The field tests showed that the maximum offset error was no more than 0.13 m and the average offset error was no more than 0.03 m for straight tracking at the traveling speed of 2 km/h. The field tests indicated that the navigation control method was appropriated to orchard spraying machine.

orchard； spraying machine; automatic guidance; control system; Beidou navigation satellite system

10.6041/j.issn.1000-1298.2017.02.006

2016-09-24

2016-11-27

国家高技术研究发展计划(863计划)项目(2012AA041507)和“十二五”国家科技支撑计划项目(2015BAD23B02)

熊斌(1989—)，男，博士生，主要从事智能农业机械装备研究，E-mail： 815149730@qq.com

张俊雄(1979—)，男，副教授，博士生导师，主要从事机械制造、机器视觉及农业机器人技术研究，E-mail： cau2007@cau.edu.cn

S219.3; TP242

A

1000-1298(2017)02-0045-06