小型履带式油菜播种机导航免疫PID控制器设计

丁幼春，何志博，夏中州，彭靖叶，吴太晖

（1. 华中农业大学工学院，武汉 430070； 2. 农业农村部长江中下游农业装备重点实验室，武汉 430070）

0 引 言

长江中下游地区是中国冬油菜主要产区，但受前茬作物收获农时、土壤重板结和小田块等因素影响，油菜机械化播种水平低下，播种机智能化水平有待提高[1-2]。2018年国务院印发的《关于加快推进农业机械化和农机装备产业转型升级的指导意见》提出到2025年油菜机械化种植水平达到 50%。播种机自主导航是播种机智能化的重要方向，它能有效提高劳动生产率、降低作业成本、促进农机智能化的发展[3-7]。在相同条件下，相对于轮式车辆，履带式车辆对单位压力地面约为轮式车辆的33%、滑转率约为轮式车辆的 42%、滚动阻力系数约为轮式车辆的 70%，滚动阻力系数随土壤湿度加大而增大，且轮式车辆增加更敏感[8-10]。另外，履带式播种机具有越野通过性能好、克服障碍能力强、可进行零半径转向等特点，能更好地适应于土壤黏湿、小田块的长江中下游地区的地面环境[11-13]。导航控制器的设计是实现自主导航的关键技术，因此，设计一种小型履带式油菜播种机导航控制器对于提升长江中下游地区的作业质量、作业效率以及播种机智能化水平具有重要意义。

国外学者针对导航控制器开展了广泛的研究，取得了丰富的成果。Urrea等[14-15]针对轮式农业机器人分别设计了自适应 PID控制器和基于模糊逻辑的路径跟踪控制器并将它们应用于农作物移动机器人的自主路径跟踪。Zakaria等[16-17]以轮式车辆为平台分别研究了基于方向盘角度、车辆横摆率的控制器和基于制导原理的直线目标跟踪控制器，并证明了所提出的控制器的有效性。Marino等[18]设计了一种无人驾驶车的导航控制器，该控制器采用嵌套 PID方法，对路径曲率的变化有较强的鲁棒性。Takai等[19]采用HST（静液传动系统）实现履带式拖拉机转向控制，导航控制误差的均方根小于 0.05 m。近年来国内学者也进行了一系列研究，多以 PID控制为核心[20-23]，通过和其他控制算法的结合，改善 PID控制的快速性和稳定性。

丁幼春等[24-26]在轮式液压车辆上对控制器进行了相关研究，以田间信息采集平台为基础提出了一种运动控制器，通过模糊控制器横向纠偏和 PID纵向定速，稳定跟踪后最大横向偏距在0.31 m以内；以轮式小麦联合收获机为对象，运用改装的电控液压转向方式，提出了一种速度自适应导航控制器，采用双闭环控制结构和PD控制，在长约200 m的大田间跟踪误差变化范围为18 cm。贾全等[27]以NF-752型履带式拖拉机为研究对象，采用液压转向方式，设计了一种基于航向预估模型的路径跟踪控制方法，跟踪误差优于10 cm。孟庆宽等[28-29]通过粒子群算法优化加权因子，提出了改进的模糊控制器，结果表明该控制器可以快速消除横向偏差，超调量小，提高了系统控制性能。当前农机导航主要以轮式车辆为研究对象，通过对液压转向系统改装实现电控液压转向进而进行导航作业，多应用于田块较大的平原地区，该方式具有转向响应时间较快、转向控制较为精准等特点，但不可避免的增加了改装难度和成本。针对履带式车辆或播种机的研究和应用较少。

针对适应于长江中下游地区稻茬田土壤黏湿、小田块的轻简化播种机智能化问题，本文以小型履带式播种机为基础，利用电磁铁对其转向系统进行电控改装，采用高精度北斗定位模块和电子罗盘获取履带式播种机的位置和航向信息作为导航控制器的输入，设计了一种基于免疫 PID的小型履带式播种机导航控制器，并进行了对比仿真、路面和田间试验。

1 小型履带式油菜播种机导航系统总体设计

1.1 小型履带式油菜播种机组成

小型履带式油菜播种机由履带底盘、汽油机、电动推杆和播种机组成。汽油机（168FA型，重庆农业机械有限公司）通过皮带轮传动为履带底盘提供动力；播种机通过 3点悬挂挂接于履带底盘尾部，播种机幅宽为1.5 m，行距为0.3 m。选用课题组研制的“一器六行”离心式排种器作为排种装置[30]。排种器正常播种转速为90～180 r/min，总排量为 36～108 g/min（6行排种），其总排量稳定性变异系数不大于14.8%，各行排量一致性变异系数不大于 16.4%，型孔无堵塞现象。选用雨田5D60-12DC型（雨田电机有限公司）直流减速电机作为离心排种盘的驱动电机，转速范围为0～330 r/min。工作时，油菜种子在离心力的作用下经导种管落入到双圆盘开沟器所形成的种床表面，覆土后完成播种。控制器通过电动推杆（HF-150-24型，温州恵丰电气）控制播种机的升降来调节播种深度，根据播种机行驶速度调节排种盘转速，从而实现排种量的自动调节。小型履带式油菜播种机及导航系统组成如图1所示。

1.播种机 2.电动推杆 3.控制箱 4.三维电子罗盘 5.汽油机 6.北斗定位模块 7.转向机构 8.履带底盘1.Seeder 2.Electric push rod 3.Control box 4.Three-dimensional electronic compass 5.Gasoline engine 6.Beidou positioning module 7.Steering mechanism 8.Track chassis

1.2 导航控制系统结构组成

导航控制系统结构框图如图 2所示，由控制器、传感器、转向机构和驱动系统组成。控制器采用MSP430F149单片机作为主控制器，LCD12864显示导航参数，按键模块进行参数设置，输入控制电路通过主控制器发出的指令控制 2路光耦隔离固态继电器的通断，从而实现对转向机构的控制。转向机构由电磁铁和“离合器拨叉”构成。传感器由三维电子罗盘（SCM345-485型，无锡迈科传感科技有限公司，精度为0.1°）和高精度北斗定位模块（M300型，上海司南卫星导航技术股份有限公司，精度为 2.0 cm）组成，电子罗盘实时采集航向信息传送给主控制器；高精度北斗模块由差分基准站、移动站、DTU设备组成（CM8350P型，厦门才茂通信科技有限公司），差分基准站把BDS观测数据上传到Internet网络，移动站中的DTU设备（内含SIM卡）通过GPRS进入Internet网络，下载实时差分数据，结合移动站BDS接收机的观测值进行数据处理，实时地解算出移动站的坐标，并将解算出的信息传送给主控制器。驱动系统由电源模块、降压模块和输出驱动电路组成，为导航控制系统各元件供电，驱动转向机构运行。

图2 导航控制系统结构框图Fig.2 Structure block diagram of navigation control system

1.3 小型履带式油菜播种机电控转向改装

本文中履带式油菜播种机配套动力由履带式运输机改装而成（7B-320A，重庆威马农业机械有限公司），图3为履带底盘转向分析示意图，履带底盘的传动系变速箱的两侧各有一个转向离合器。转向离合器靠摩擦表面的摩擦力传递转矩，当分离某一侧的转向离合器时，就可以切断该侧驱动轮所传递的转矩使两边履带形成速度差，进而实现履带播种机的转向。在驾驶过程中，一般使用操纵杆拉动“离合器拨叉”，使转向离合器与变速箱分离控制履带车辆的转向。

图3 履带底盘转向分析示意图Fig.3 Schematic diagram of track chassis steering analysis

为了实现履带式油菜播种机的自主导航作业，对履带底盘的转向机构进行了电控改装。根据履带式播种机的转向特性，采用电磁铁作为拉动“离合器拨叉”的执行装置（代替人为控制操纵杆），构成了“电磁铁-离合器拨叉”转向机构。对离合器拨叉进行拉力测试，最终选用 TAU80120型电磁铁，24V直流供电、保持力为250 N、行程为50 mm，满足“分离拨叉”要求。采用24V/2路光耦隔离固态继电器控制电磁铁的通断，导航中通过改变单片机输出的PWM（pulse-width modulation）占空比控制继电器的通电长短，进而控制履带式播种机的转角进行纠偏。还可以利用遥控器控制继电器，便于改装后机具的转移和导航时进行必要的干预。

电磁铁上电后拉动离合器拨叉使转向离合器分离需要一定的时间，称为转向机构响应时间。采用单片机定时器A的PWM增计数模式定时1 s（即PWM的周期为1 s），通过调节 PWM 占空比测试转向机构响应时间，测得占空比高于0.29时，转向离合器刚好能实现分离，当占空比低于0.29时，转向离合器不能实现分离，由此获得转向机构响应时间为290 ms。

2 履带式油菜播种机运动学模型与转向角传递函数

2.1 履带式油菜播种机运动学模型

本文中履带式油菜播种机转向时一侧速度为零，另一侧速度保持不变，即以一侧履带上的某一点为圆心转动。因此转向角速度等于两侧速度之差除以履带底盘的宽度b。当速度不变时，履带式油菜播种机的转向是通过作用时间控制的，转向角度与作用时间成正比，比例系数为转向角速度。假设：1）履带式油菜播种机的质心和几何中心重合；2）转动过程中履带式油菜播种机的转向角速度和阻力系数不变；3）忽略外侧履带接地段滑转和内侧履带接地段滑移。

根据运动学原理，履带式播种机在高斯平面坐标系建立运动学方程式如下

式中 v为履带式播种机速度，m/s；v1、v2表示左侧、右侧履带的卷绕速度，m/s；θ为履带式播种机航向偏差，(°)；b为左右履带距离，m。以此方程作为简化的播种机运动学模型，如图4所示。

2.2 转向角传递函数

为了实现对履带式播种机的直线跟踪控制，需要获得履带式播种机的转向角的传递函数。播种机左右转向时一侧履带运动，另一侧履带停止，播种机围绕停止转动的履带上的某一点o旋转，角速度为ω由运动学方程求出。小型履带式油菜播种机简化转向过程如图5所示，履带式播种机绕o点转动，从a-o位置转到c-o位置，转向角度为δ。由圆心角定理可知：δ(t)=ω(t-t0)，其中t0为转向机构响应（滞后）时间。因此，播种机的转向角度大小与作用时间长短成正比，系数为角速度。进行拉式变换得到传递函数为：

式中ω为角速度，rad/s；s是传递函数中特有的变量；t0为向机构响应（滞后）时间，s。

图4 小型履带式油菜播种机运动学模型示意图Fig.4 Kinematics model diagram of small crawler-type rape seeder

图5 小型履带式油菜播种机转向角度示意图Fig.5 Diagram of steering angle of small crawler-type rape seeder

3 小型履带式油菜播种机导航控制器设计

图 6为小型履带式油菜播种机导航控制器整体设计流程。首先，人为设定要跟踪的直线路径的起点和终点的平面坐标，主控制器自动计算目标航向、起点和终点的距离。导航控制系统利用北斗定位模块和电子罗盘采集播种机的位置信息和航向信息。为减小动态定位误差，利用加权平均算法对最新 2次采集到的经纬度坐标进行滤波，得到当前位置。通过点到直线距离公式计算当前位置到设定航线的距离偏差d。以滤波后的距离偏差d作为输入，利用目标航向修正公式计算目标航向。再以航向偏差θ（修正后的目标航向和电子罗盘实时采集的航向之差）作为免疫PID控制器的输入，免疫PID控制器决策出占空比作为输出 u作用于转向机构，从而实现对履带式播种机转向的控制。

图6 小型履带式油菜播种机导航控制器整体设计Fig.6 Overall design of navigation controller for small crawler-type rape seeder

3.1 距离偏差和航向偏差

播种机直线跟踪时，在导航前需确定目标导航直线，记为导航线AB。北斗定位坐标经过高斯投影变换后，得到平面坐标A(x1,y1)，B(x2,y2)。AB的直线方程为

式中a，b，c为直线方程常数。

播种机当前位置为O(x,y)，当前位置到预定航向的距离即为距离偏差，可以通过点到直线的距离公式求解。

航向角从0～360°变化，正北方向为0，沿顺时针变化。若AB导航线的目标航向角为α，有

由此可得导航线的目标航向角α，结合电子罗盘实时测得的航向角β，两者差值即为航向偏差θ，单位为(°)。规定播种机偏左时航向偏差为正，播种机偏右时航向偏差为负。

3.2 导航策略

根据当前播种机的位置判断是否对设定航向进行修正，设计如图7所示的导航策略。

图 7中，D为播种机当前位置到设定航线的距离，dmin为设定的最小纠偏距离。当|D|≤dmin时，不对设定航向进行修正，播种机沿着设定目标航向小幅度纠偏；当距离偏差|D|＞dmin时，对设定目标航向进行修正，使播种机沿着新目标航向快速缩短距离偏差。新目标航向与原设定目标航线在行驶前方相交，因此播种机能够朝着设定目标航线行驶，不断缩小距离偏差，当|D|≤dmin时，目标航向重新变为原设定目标航向。其中，航向修正量与距离偏差正相关，距离偏差越大，航向修正量越大，纠偏幅度越大。航向修正量x计算公式为式（5），根据播种机作业环境和防止播种机原地打转，设定x的最大修正量为xmax，对应求出最大纠偏距离Dmax，即D≥Dmax时，航向修正量都为xmax。

图7 小型履带式油菜播种机导航策略Fig.7 Navigation strategy of small crawler-type rape seeder

式中λ为航向修正系数，θmin为设定的最小纠偏航向，当航向偏差|θ|偏小于等于θmin时，不对航向进行纠偏。修正后的目标航向为

航向偏差为正时播种机偏左，航向偏差为负时播种机偏右。

一般情况下能够通过两种方式进行集成，一种是通过库类配置集成SDK，另一种是通过Gradle集成SDK。相较于通过Eclipse或Android Studio导入第三类库进行集成的方式，Gradle抛弃了基于XML的各种配置，转而基于Apache Ant和Apache Maven概念进行项目自动化构建，使用更加简洁。同时，Gradle支持局部构建，适用于任何结构的工程，能够实现灵活轻松的迁移，能够帮助使用者更简便地实现在一个APP中集成多种功能。

3.3 免疫PID控制器设计

当被控对象受到较大干扰且具有大惯性时，传统PID控制效果不好，其抗干扰能力较差。通过研究免疫系统T细胞和B细胞免疫反馈机理[31]，提出了一种非线性的免疫PID控制器，该控制器结构如图8所示，其输入为航向偏差，输出为转向控制量。本文履带车的转向控制量为占空比，通过占空比来控制转向的角度大小。

图8 免疫PID控制器结构Fig.8 Structure diagram of immune PID controller

假设第k代抗原的数量为ε(k)，在受到抗原刺激后，增强细胞TH产生的输出为TH(k)，抑制细胞TS对B细胞的影响为TS(k)。因此，B细胞接收到的总刺激S(k)为

式中k1是刺激因子，k2是抑制因子；ΔS(k)为B细胞受到总刺激S(k)的变化量。若以抗原的数量ε(k)作为给定值的偏差e(k)，本文中可替换为航向偏差θ。S(k)作为免疫PID控制器的输出u(k)，可替换为输出的PWM占空比，不同的占空比改变的转角不同，Δu(k)为输出变化率，则存在以下控制规律

非线性函数(,)fuuΔ采用模糊控制器逼近：每个输入变量被2个模糊集合模糊化，分别是P（正）和N（负），其离散论域取值分别为+3和-3；输出变量被3个模糊集合模糊化，分别是P（正）、Z（零）和N（负）。隶属度函数的定义域为R∈（-3，3），模糊控制器可以采用以下4条规则：1）If u(k) is P and Δu(k) is P, then f(u(k),Δu(k)) is N；2）If u(k) is P and Δu(k) is N, then f(u(k), Δu(k))is Z；3）If u(k) is N and Δu(k) is P, then f(u(k), Δu(k)) is Z；4）If u(k) is N and Δu(k) is N, then f(u(k), Δu(k) is P。

依据模糊规则进行模糊推理，采用最大隶属度平均法进行解模糊化，得到模糊控制器的输出f(u, Δu)。

式中 ki，kd，kp为 PID 初始系数，ki′=ki/kp，kd′=kd/kp。

3.4 免疫PID控制器仿真

3.4.1 转向控制仿真

小型履带式油菜播种机在0.5 m/s行驶速度下的传递函数为

为了验证免疫 PID控制器的转向控制性能，在 10°阶跃信号作用下，对履带车分别运用所设计的 PID 控制器和免疫 PID控制器进行转向控制仿真[32]，结果如图 9所示。控制器的参数采用试凑法取得，不断地调整 kp，ki，kd这3个参数，选择一组能够使系统的调节时间和超调量尽量小的参数作为控制器的 PID参数。其中，PID控制器的kp=5，ki=2，kd=0.9。免疫PID控制器的kp=5，ki=2，kd=0.9，η=0.8。在t=25 s时加入一个5°的信号干扰，持续2 s，测试系统的抗干扰能力。仿真结果表明，在相同参数下，所设计的免疫 PID 控制器与传统 PID 控制器相比，具有超调量更小、调整时间更短、收敛速度更快和抗干扰能力强等特点。

图9 PID控制器和免疫PID控制器对比Fig.9 Comparison of PID controller and immune PID controller

3.4.2 直线路径跟踪仿真

为了验证免疫 PID控制器的直线跟踪效果，进行了直线跟踪仿真。设定将要跟踪的直线为y=x，小型履带式油菜播种机在xoy坐标系中的初始位置为(0,0)，播种机速度为0.5 m/s，采样时间为0.1 s，初始航向偏差为45°。根据前文建立的传递函数和免疫 PID控制器，进行履带车直线跟踪仿真，如图10所示。结果表明：免疫PID直线跟踪控制器能够跟踪直线路径，具有超调量小、稳态误差小等特点。

图10 免疫PID控制器直线跟踪仿真Fig.10 Linear tracking simulation of immune PID controller

4 导航控制器试验

4.1 北斗定位模块精度标定

试验前对北斗定位模块定位精度进行标定：将小型履带式油菜播种机启动后挂至空挡，天线静止放置在履带式播种机上，测量北斗定位模块在播种机启动时的定位精度。北斗定位模块通过RS232串口连接到电脑，利用Compass Receiver Utility 软件采集北斗定位数据，再利用Matlab对经纬度数据进行高斯投影处理，得到平面坐标并绘图。测量定位模块静止放置60 s输出的定位数据，输出频率为2.5 Hz并求出圆概率定位精度R，如图11所示。由图11可知，R=2.0 cm，即振动时司南M300圆概率定位精度为2.0 cm。

图11 北斗定位圆概率误差定位精度Fig.11 Probability error location accuracy of BeiDou location circle

4.2 路面直线跟踪试验

为了对导航控制器的控制效果进行验证和评估，在华中农业大学南荟路进行了路面试验，路面平整、无遮挡。试验材料：小型履带式油菜播种机 1辆、笔记本电脑 1台、导航箱、控制箱、粉笔、卷尺、红线、装有墨水的水瓶。在小型履带式油菜播种机的中轴正下方位置固定一个装满墨水的水瓶，播种机行驶过程中水滴落下，通过墨迹记录播种机的运动轨迹。用卷尺每隔0.2 m测量墨迹到目标直线的距离，以此记录距离偏差的变化。设定相同的初始距离偏差和航向偏差，以0.50 m/s的速度行驶，分别搭载免疫PID控制器、常规PID控制器进行对比。试验表明：常规PID控制器的平均绝对偏差为6.1 cm、最大跟踪偏差为16.0 cm，稳态调节时间（稳定到15.0 cm内）为11.9 s，免疫PID控制器的平均绝对偏差为4.2 cm、最大跟踪偏差12.0 cm，调节时间为9.2 s，其平均跟踪偏差、最大跟踪偏差和调节时间都比常规PID控制器小（图12）。

图12 路面直线跟踪试验Fig.12 Linear tracking test in pavement

4.3 田间直线跟踪试验

为了进一步验证导航控制器的直线跟踪效果，2018年9月15日在华中农业大学试验田开展了田间直线跟踪试验，导航距离为30 m。采用五点取样法测得田间土壤含水率平均值为 20.1%，土壤坚实度平均值为543 kPa。以0.50 m/s的速度行驶，分别搭载免疫PID控制器、常规PID控制器。试验中常规PID控制器和免疫PID控制器的基本参数相同，试验平台均为履带式播种机。设定最小纠偏航向θmin=5°，最大修正量为xmax=20°，dmin设定最小纠偏距离dmin=5.0 cm。

试验步骤：1）选取田块边界两端点A、B，分别在A、B两点采集20组数据并转换为平面坐标求平均值，记A、B为直线跟踪的起点和终点；2）调节初始距离偏差和初始航向偏差，使用电动推杆将播种机架向下降至播种深度；3）启动履带式播种机，开始直线跟踪；4）记录履带式播种机的速度，并利用放置在车顶的北斗天线记录车辆位置信息，工作频率为2.5 Hz。根据试验效果，调节PID参数。重复步骤1）～4），进行多次试验。表1为常规PID与免疫PID控制效果对比，田间试验表明，当播种机速度为0.50 m/s时，免疫PID导航控制器直线跟踪平均绝对偏差为5.8 cm，最大偏差不超过15.2 cm，能够较好地满足播种机导航作业要求，为履带式播种机的自主导航提供了技术参考。

表1 常规PID与免疫PID控制效果对比Table 1 Comparison of control effect between conventional PID and immune PID

如表1所示，免疫PID控制器的最大跟踪偏差和平均绝对偏差均比常规PID控制器小。假定

式中M为导航精度提高百分比（符号为正表示精度提高，符号为负表示导航精度降低）；Savg为常规PID控制平均值；Mavg为免疫 PID控制平均值。分别以最大跟踪偏差和平均跟踪偏差作为衡量导航精度指标，由式（13）得出：免疫PID控制相对于常规PID控制，导航精度分别提高了18.1%、32.6%。试验表明：在相同参数配置条件下免疫PID控制相对于常规PID控制降低了最大跟踪偏差和平均绝对偏差。免疫 PID的直线跟踪偏差变化曲线和播种效果如图13。

田间试验与路面试验相比最大跟踪偏差和平均跟踪偏差有所增大，主要是由于田间土地的平整度和坚实度较低，履带底盘与接触面的抓着力减少，导致纠偏时不能及时转向。在相同条件和参数下，免疫 PID的试验效果相较与常规PID有所提升，免疫PID控制器模仿人体免疫机制，当受到较大干扰时能够迅速收敛并恢复原位，具有调节时间短、稳态误差小和抗干扰能力较强。针对长江中下游土壤黏湿、小田块的特点，采用免疫 PID控制器具有较好的适应性。另外，试验过程中设定较低速度0.5 m/s，主要是由履带式播种机转向机构响应时间 290 ms决定的，即控制器的输出 290 ms后才能实现纠偏，在此段时间播种的偏差又进一步增加，所以随着速度增大播种机的跟踪偏差会不断增大，为了提升较高速度下的导航效果，有必要进一步提升转向控制的响应灵敏度。采用电子罗盘和北斗定位模块组合导航的优点有：当卫星受到遮挡后，依然能够保证航向的准确性；受北斗定位误差的影响，当履带式播种机以 0.5m/s的速度行驶时，利用北斗定位信息间接计算航向的准确性得不到保证；三维电子罗盘能实时测得履带式播种的俯仰角、横滚角，便于进一步分析履带式播种机姿态。

图13 田间直线跟踪试验Fig.13 Linear tracking test in field

5 结 论

针对适应于长江中下游地区稻茬田土壤黏湿、小田块的轻简化播种机智能化问题，设计了一种基于免疫PID的小型履带式油菜播种机导航控制器，并进行了仿真和试验。

1）根据履带播种机的转向特性，采用电磁铁作为拉动“离合器拨叉”的执行装置，对其转向系统进行了改装。建立了履带式油菜播种机运动学模型和转向角传递函数。

2）以小型履带式油菜播种机为基础，采用高精度北斗定位模块和电子罗盘进行组合导航，获取履带式油菜播种机的位置和航向信息作为导航控制器的输入，设计了一种基于免疫 PID的小型履带式油菜播种机导航控制器。

3）路面试验表明：当播种机速度为0.50 m/s时，导航控制器直线跟踪的平均绝对偏差为 4.2 cm，最大跟踪偏差为11.9 cm。田间试验表明：当播种机速度为0.50 m/s时，导航控制器直线跟踪平均绝对偏差为 5.8 cm，最大偏差不超过15.2 cm。

后续还将进一步开展曲线跟踪和路径规划研究和试验，以更好地满足播种机导航作业要求。