农机自动驾驶系统方案剖析

张 军，李丙春

（喀什大学计算机科学与技术学院，新疆 喀什 844006）

0 引言

上个世纪，农业活动实现了机械化，农业机械代替了大部分繁重枯燥的人力工作。进入本世纪，农业活动向智能化方向快速发展，农机驾驶员的工作需要智能化的解决方案，农机自动驾驶项目的主要目的就是解放广大农机驾驶员，实现农机的智能化工作。

1 农机自动驾驶系统概述

目前，业内农机自动驾驶有两种解决方案：一是电动方向盘控制自动转向；二是改装油路，利用电磁换向阀和比例阀控制自动转向。本文中农机自动驾驶系统采用第二种方案，系统主要包括两套软件和若干硬件。

1.1 控制软件

控制系统包括数据采集、计算和车辆控制。农机在田间行驶时的实时数据包括实时位置、实时速度和车辆实时姿态，车辆姿态包括俯仰角、横滚角和偏航角。各个传感器将采集到的实时数据通过CAN总线传递给控制软件，控制软件比较得到的实时数据和预先规划行驶线路计算得出农机发生实际偏移的距离，而后通过控制液压电磁阀调整农机行驶方向，使其回到正确的工作路线上。

1.2 显示器软件

驾驶人员通过该软件预先设定当前农机的尺寸数据和天线位置。在工作前，设定使用农具的尺寸以及农具和农机之间的距离，根据实地作业需要设定农机工作线路及工作模式。在工作时将实时工作数据传递给控制器，控制器根据目标数据和农机实时数据调整运行路线。图1为设计好的农机自动驾驶系统。

图1 农机自动驾驶系统整体方案

1.3 工作系统

系统包括显示器软件、控制器软件、北斗天线（包括电台）、液压阀和角度传感器。显示器软件的作用是设定农机、农具参数，设定耕作工作路线以及显示实际工作状态。北斗天线接收北斗卫星的卫星信号并将卫星数据传入控制箱，控制箱中北斗芯片解析卫星信号得到卫星数据，同时结合差分基准站的实时误差数据比较实际位置与预先规划路线，得出偏离值，差分定位计算模块结合这两个数据进行差分定位计算，得到当前实际位置，定位误差可达厘米级。控制箱包含电子陀螺仪，持续输出农机的横滚角和俯仰角，计算模块结合偏离值和农机实时姿态和速度计算得出液压电磁阀进行转向数据控制并向液压电磁阀发出指令。液压电磁阀收到指令后通过控制液压油入口大小来实施车轮转向。车轮转向的实时角度会通过角度传感器反馈给计算模块。计算模块根据车轮实时转向角度来调节转向控制数据，直至拖拉机回到预先规划好的路线上。

2 农机自动驾驶系统要解决的问题

要实现以上功能，系统必须解决下面问题：（1）农具实时位置和实时速度的获取；（2）农机姿态数据的获取；（3）农机转向的精确控制；（4）农机方向的自动调整。

3 问题解决

3.1 农具实时位置和实时速度的获取

实时位置的获取依赖北斗定位系统（BDS），北斗定位系统结构如图2。

图2 北斗卫星定位系统结构

北斗系统包括3部分：空间部分、控制部分和用户部分。空间部分截至2020年7月，共有55颗导航卫星，其主要作用是发送用于导航定位的卫星信号。卫星的分布保证在任一时刻、任一地点高度角15°以上都能够观测到4颗以上的卫星。控制部分由主控站、监控站和注入站组成。

主控站：从各个监控站收集卫星数据，计算出卫星的星历和时钟修正参数等，并通过注入站注入卫星，向卫星发布指令控制卫星，在卫星出现故障时调度备用卫星。

监控站：接收卫星信号，检测卫星运行状态，收集天气数据并将这些信息传送给主控站。

注入站：将主控站计算的卫星星历及时钟修正参数等注入卫星。

用户部分包含北斗接收机及相关设备，能够捕获到按一定卫星高度截止角所选择的待测卫星的信号，并跟踪这些卫星的运行，对所接收到的卫星信号进行变换、放大和处理以便测量出卫星信号从卫星到接收机天线的传播时间，解译出卫星所发送的导航电文，实时计算出测站的三维位置甚至三维速度和时间。

实时定位原理：通过4颗已知位置的卫星来确定接收器的位置。

如图3，每个卫星不间断向外发射信息，信息内容包括卫星的实时坐标x、y、z和发射时间Ts，假设位于地面上的接收机在Te接收到卫星信号，接收机上信号的接收时间与卫星信号数据的时间差是信号传输的时间T=Te-Ts，信号传播速度V=C=3×108m/s，以卫星1为例，得出传输距离d1=（Te-Ts）×V，得到方程d12=（x1-x0）2+（y1-y0）2+（z1-z0）2，其中未知数为x0，y0，z0，即接收机的位置。同理，可列出卫星2和卫星3的方程，联立得到三元方程组，可以解出接收机位置x0，y0，z0。然而考虑到地面接收机时间存在误差，会影响距离结果，故将地面接收时间也设为未知数t0，重新列方程（t1-t0）2×V2=（x1-x0）2+（y1-y0）2+（z1-z0）2。方程中包含4个未知数x0，y0，z0，t0，因此，需要四元方程组方可求解4个未知数，故至少需要接收到四个卫星信号数据才可以进行地面定位。

图3 卫星系统定位原理

由于电离层和对流层的折射及反射等因素影响，会导致测距不准，即图4中的四个卫星信号圆球很难交汇于一点，因此需要消除这些影响带来的误差，业内采用RTK技术来减小误差。

图4 理想情况下卫星信号交于一点

实时动态差分技术（RTK）原理：基准站用一个固定坐标来做参考，以后基站每通过接收卫星算得一个坐标就跟固定坐标作比对得到差值，然后将这个差值发送给移动站，移动站用卫星接收到的坐标再减去基站发过来的差值，就得到了改正后的坐标，如图5。

图5 实时动态差分

系统装备了电台和接收机，采用RTK技术进行定位，实现了系统要求的高精度定位，可测量出连续位置的精确坐标以及位置间的时间间隔并通过计算得出农机的实时速度。

3.2 农机姿态数据的获取

利用MEMS陀螺仪能够获取农机的姿态数据。陀螺仪的原理是一个旋转物体的旋转轴所指的方向在不受外力影响时保持不变。传统的陀螺仪是一个不停转动的物体，其转轴的指向不随承载它的支架旋转而变化。如图6，陀螺仪转动轴在空中始终处于竖直方向，当飞机在空中姿态变化时，会带动陀螺仪的框架一起同步转动（左右摆动，俯仰摆动、横滚摆动），测出框架和陀螺仪转轴的各个角度，就是当前飞机在空间中的各个角度。由于测量过程中会存在少量误差，若长期使用陀螺仪测量误差会累计变大。因此在使用过程中通常与卫星定位一起，通过卫星定位定期消除陀螺仪的误差。目前在工业上应用的主要是MEMS陀螺仪，本系统采用MEMS陀螺仪。

图6 陀螺仪测量姿态

3.3 农机转向的精确控制

为了实现农机转向的精确控制，对农机进行改造，采用电液方式进行转向控制，电控液压转向系统工作原理如下。

图7 电控液压转向系统原理

电控液压转向系统在原有油路的基础上并联安装集成液压阀（由比例阀、三位四通电磁换向阀和安全阀组成，如途中虚线部分所示），实现农机的自动换向。系统工作时，根据控制器指令，三位四通电磁换向阀实现油路方向的切换控制，达到控制前轮左右转向的效果，比例阀实现油路流量的控制，达到控制转向速度快慢的效果。比例阀前端安装安全阀，保证系统工作可靠度。

自动转向控制的原理是：发动机启动后，转向液压泵开始工作，控制器监测农机前轮转向信息并发出控制指令，控制集成液压阀的开度和方向；液压油经过比例阀和电磁阀时改变方向和流量，进入转向油缸，油缸活塞在油压作用下移动，实现拖拉机前轮向目标角度转向。

3.4 农机方向的自动调整

农机在工作过程中行驶路线发生偏移时应遵循如下调整原则：

（1）农机应以小角度长距离平缓转向返回预设路线，否则容易造成过调，出现农机以Z字形在规划路线左右反复摆动的情况。

（2）农机工作时的角度误差控制在1°～2°，距离误差控制在8cm内，即误差在该范围内均认为正常工作，无需纠偏，避免因系统过敏导致反复调整，造成液压转向装置过热过压，影响系统使用寿命。

（3）农机的转向角有上限，纠偏时的转向角不可超过此角度，否则无法执行。

3.4.1 理想的纠偏路线

理想的纠偏路线如图8（a）（平行规划路线），车辆以S型曲线切线进入规划路线。纠偏路线设计为两个大小相同的相切圆，农机位置位于上圆的最下位置，入线位置位于下圆的最上位置。调整过程中会出现8（b）（朝向规划路线），可看作图8（a）的后半部分。控制思路如上。当农机背离规划线路时，如图8（c），纠偏路线依然为S型曲线，具体位置根据车辆当前角度与切线位置相同确定。

图8 纠偏路线

3.4.2 纠偏控制算法

农机的路线纠偏采用模糊自适应PID控制算法，因农机在田地工作会受到泥土阻力和坡度影响，因此在传统PID算法的基础上添加人类经验调整PID参数，使参数自适应于不同的农田环境。

3.4.2.1 PID控制器算法

PID控制算法原理：整体上采用基于误差生成消除误差的策略。首先，控制系统通过比较对象反馈值与目标值的大小得到系统偏差值，偏差值经由控制器中比例项（P）、积分项（I）和微分项（D）的计算后，各项相加得到系统控制量并作为对象输入量用于对被控对象进行调整，如图9。

图9 PID控制器算法

PID的公式为增量式和位置式，本系统选择增量型PID控制算法。增量式PID控制算法如下：

增量型PID控制输出仅与当前偏差值及上两次的偏差值相关，无需累计偏差值，输出的是控制量的增量，在执行上简单有效。公式中各项的作用如下：

（1）P参数：Kpe（k），比例项，为比例系数和当前时刻偏差之积。在偏差较大时能及时减小偏差。

（2）i参数：积分项，为积分系数与历史偏差累计之积，消除了静态误差。

静态误差是指单独采用P参数时当偏差非常小的时候，一些客观因素（如阻力）会导致系统无法进行调节到目标值，最终导致输出结果与目标值有一定距离的情况，这种情况可通过累计偏差来处理，因为累计偏差会趋向一个固定的数值。

（3）D参数：Kd[e（k）-e（k-1）]，微分项，为微分系数与当前偏差变化率之积。其主要作用为抑制超调、减少震荡、让系统快速收敛。体现在与P项进行对抗，根据偏差变化情况进行控制。

3.4.2.2 模糊自适应PID控制器算法

控制原理：操作人员在长期操作过程中积累了一定经验知识，但经验不易精确定量表示，因此运用模糊数学的基本理论和方法，用模糊集表示规则的条件和操作，将这些模糊控制规则以及有关信息（如评价指标、初始PID参数等）作为知识存入表中，随后根据控制系统的实际响应情况运用模糊推理自动实现对PID参数的最佳调整。模糊自适应PID是在PID算法的基础上通过计算当前系统误差e和偏差变化率ec，利用模糊规则进行模糊推理，查询模糊矩阵表进行参数调整，原理如图10。

图10 PID控制器算法

角度传感器回传当前角度值Ak，控制系统计算得到error=Ak-Ak-1，ec=de/dt=error/T，作为模糊控制器的输入变量。模糊控制器的输出为PID控制器的参数修正量，PID调节器根据获取到的调节量对参数Kp、Ki、Kd进行调整，结合当前的误差error和ec，PID控制器输出控制电流I，电流I改变电磁比例阀的开口大小，从而控制农机转向角度大小。

模糊控制器设计：（1）确定输入输出变量的论域。将变量量化设为7个等级：分别是负大（NB）、负中（NM），负小（NS）、零（ZO）、正小（PS）、正中（PM）、正大（PB）。取所有变量的量化论域为[-6，6]，转向角偏差的基本论域为[-30，30]，量化因子为0.2，转向角偏差变化率ec的基本论域为[-30，30]，量化因子为 0.2，ΔKp的基本论域为[-30，30]，量化因子为 0.2，ΔKi的基本论域为[-6，6]，量化因子为 1，ΔKd的基本论域为[-3，-3]，量化因子为2。（2）隶属函数采用高斯隶属函数。（3）确定模糊控制规则。模糊控制规则是模糊控制器的的核心，根据实践经验编写，形式如表1。根据角度偏差e和偏差变化率ec的值对参数进行调整，每个参数都可根据实际情况定制控制表格。

表1 PID模糊调整规则

4 总结

作者在2014年～2015年期间曾参与农机自动驾驶系统的研发工作，系统研发成功后在东北和新疆各地进行测试调整，测试结果同国外优秀产品相比功能上基本相当。该系统在直线的稳定性上稍有不足，原因为未考虑平缓小角度，以切线方式入线以及PID算法造成过调问题，文中已经对入线策略进行了详细改进阐述，并对控制算法进行了优化，采用模糊自适应PID控制器算法，后期将继续研究改进方案算法，使产品达到国外同类产品效果。