智能收获机定位和自适应路径追踪方法

王立辉，石佳晨，王乐刚，焦中元，李耀明，陈 进，徐立章

（1.东南大学 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2.雷沃重工股份有限公司，山东 潍坊 261206；3.江苏大学 现代农业装备与技术教育部重点实验室，江苏 镇江 212013）

0 引言

农业装备的定位与导航技术是实现农业机械车辆路径追踪的基础[1]，目前定位技术主要有全球卫星导航系统（ global navigation satellite system,GNSS）[2]、组合导航技术[3]和机器视觉定位技术[4]。依托先进的定位技术，可以实现大型智能农业机械车辆的路径追踪功能，实现农业机械车辆自动驾驶。文献[5]设计了1 个模糊控制器，根据农机的位置偏差和航向偏差来输出期望转向角，使农机追踪期望路径，并通过仿真验证其有效性。文献[6]设计了1 种基于遗传算法的模糊控制器，能够提升拖拉机自动作业时的动态特性和鲁棒性，提高拖拉机在不同条件下的自适应性。文献[7]基于期望路径的曲率估计理想速度，并根据横向误差和速度动态估计前视距离，能够提升车辆在路径追踪时的精度。

收获机的作业速度越大，位置偏差也越大[8-9]。收获机存在控制延迟是导致这个现象的原因之一，这些控制延迟主要包含导航定位系统延迟和控制系统的延迟等[10]。智能稻麦联合收获机是1 种重载农业机械车辆，它的转向控制系统以非线性、高延迟的电控液压辅助转向系统为主[11]。通过对具体的系统建模，合理地对具体控制模型的参数自适应调整，能够有效减缓控制系统的控制延迟[12]。然而这种办法，对于液压转向控制系统的模型参数获取困难、转向控制系统模型种类繁多的农业收获机的实际应用存在一定困难。模型预测控制（model predictive control, MPC）能够使得收获机的路径追踪控制不依赖于精确的控制模型，抑制控制延迟。但是MPC 算法计算量巨大、计算时间长，会影响收获机路径追踪的实时性[13]。使用分布式MPC、混合式MPC 方法可以大大提高计算速度，但是这种方法需要更多的硬件支持，增加了成本，不利于实际生产应用[14]。

国内外针对拖拉机等中小型农机的定位和路径追踪系统已颇有成就[15-18]，但是针对大型智能联合收获机的定位和路径追踪系统的研究较少。综上所述，针对联合收获机的控制延迟较其他中小型农机更大，控制难度更大的现象，提出1 种适用于大型智能联合收获机的路径追踪方法，有着重要的研究意义。基于运动学模型的纯追踪算法，因其运算量小、实用性强、硬件成本需求低等优点，已广泛应用到了智能农机的辅助驾驶系统和路径追踪系统中[19]。利用预测思想，校正和补偿收获机路径追踪时存在控制延迟导致的位置偏差[20]，可以很好地提升收获机路径追踪的精度。为解决大型智能联合收获机存在控制延迟，导致路径追踪精度下降的问题，本文提出 1 种基于GNSS 的智能联合收获机自适应路径追踪方法：建立联合收获机的运动学模型，根据所建模型推导纯追踪算法。依托双天线GNSS 实时动态差分（real-time kinematic, RTK）技术，获取收获机当前时刻的位置、航向和速度信息，预测未来时刻收获机的位姿信息，校正当前时刻纯追踪算法的横向误差；将速度、航向误差和校正后的横向误差输入模糊控制器，根据速度等信息实时地自适应校正纯追踪算法中的前视距离参数，通过优化后的纯追踪算法，计算期望转向角，控制收获机的转向，实现路径追踪控制。

1 联合收获机路径追踪系统设计

联合收获机路径追踪系统依托雷沃谷神系列大型智能稻麦轮式联合收获机搭建，该系列联合收获机的轴距为3.75 m，转向系统为电液辅助转向系统，前轮为固定轮，后轮为转向轮。路径追踪系统主要由双天线RTK-GNSS 定位模块、液压转向控制模块和工控屏组成。RTK-GNSS 的定位精度已经能达到厘米级，但在实际作业时发现，单天线RTK-GNSS 系统无法获得精确的收获机的航向信息，尤其在收获机作业速度较慢时，测量获得的航向信息和实际的航向信息相差甚大。本文采用了双天线测量法实时获取收获机的航向角信息：将RTK 系统的移动站的接收机天线1 和接收机天线2 分别固定到收获机上，保持其相对位置不发生变化，即可以通过RTK 技术分别测得2 个天线在对应的坐标系中的位置信息，结合2 个天线在收获机上的安装位置，可以直接推算出收获机的精确的航向角。整个系统的架构如图1 所示。双天线RTK-GNSS 采集收获机的速度、位置和航向角信息后，转向控制器STM32F767IG 将收获机的位姿信息发送给工控屏。工控屏基于C++/QT 开发了联合收获机自动导航软件，软件根据路径追踪算法、当前时刻的位姿信息结合期望路径，计算出期望转向角后发送给转向控制器。转向控制器通过液压驱动电路，驱动收获机的液压系统，控制后轮转向，同时转向控制器依托转向角传感器实时监测后轮的转向角，形成闭环反馈控制。

图1 基于GNSS 的路径追踪系统架构

图1 中，PWM(pulse width modulation)为脉冲宽度调制。

2 收获机自适应路径追踪方法

2.1 纯追踪算法

纯追踪算法是1 种基于运动学模型的算法。如图2 所示，根据搭建的路径追踪系统，建立前轮固定、后轮转向的简化的二轮车模型[21]。

图2 简化的后轮转向二轮车模型

在导航坐标系X － O －Y 中：A 为前轮动力轮；B 为后轮转向轮；θws为转向轮的转向角； Lwb为前后轴的轴距；r 为转向半径。转向轮的转向角 θws和转向半径r 的数学关系为

纯追踪算法是模拟人的驾驶行为，选择1 条最优的曲率半径逼近期望路径，实现车辆的路径追踪[22-23]。

如图3 所示，在车体坐标系 Xb－ Ob－Yb中，点（ x(b),y(b)）为在当前的前视距离 Lld下，收获机在期望路径上的预瞄点。

图3 纯追踪算法示意图

根据图3 中可得

转向半径r 和前视距离 Lld的关系为

式中 x(b)在车体坐标系中的值[24]为

式中： dXTE为横向误差，其定义为当前位置到期望路径的距离，符号定义沿着期望路径右侧同向为正，反之为负；eθ 为航向误差，其定义为从当前航向角逆时针转向期望路径角度的角度。

结合式（1），可得期望转向角为

由式（5）可知，纯追踪算法计算期望转向角基于当前时刻的横向误差和航向误差，式中唯一的不确定量为前视距离。

2.2 基于速度的自适应延迟控制校正

收获机在田间自动作业时，由于各种原因，从收获机的控制器发出控制信号到执行机构执行完毕指令存在延迟。传统的纯追踪算法，是以当前时刻的参数计算期望转向角，没有针对系统的滞后性进行校正。优化后的收获机路径追踪算法使用校正后的横向误差计算期望转向角，校正因控制延迟带来的偏差，如图4 所示。

图4 预测位置示意图

在导航坐标系X － O － Y中，点 m( xm, ym)为收获机控制器发出控制信号的位置，当收获机的作业速度为v 时，点 n( xn, yn)为收获机的执行机构执行完毕的点。当其他条件不变时，若收获机以比v 更快的速度 v2行驶，则会行驶到距离更远的点n2( xn2, yn2)处，因控制延迟导致的位置偏差会更大。定义控制器发出信号到执行机构执行完毕的这段时间为控制延迟时间 td，根据速度v 估算控制延迟时间 td的长短，依据点m 预测点n，使用点n 的位置信息校正点m 的位置信息，可以校正收获机因为控制延迟造成的偏差。

假设在控制延迟时间 td内收获机以恒定速度r行驶，且轮胎的转向角没有发生变化，则这段时间内行驶的弧长 lmn为

点m 与点n 的距离为

式中 Δθh为延迟时间 td内航向角的变化量，且有

联立式（7）和式（8），有

在车体坐标系中，点m 相对于点n 的位置关系为

则在导航坐标系中，点m 相对于点n 的位置关系为

结合式（9）～式（11），唯一的不确定量是控制延迟时间 td。收获机转向控制的1 个完成流程为：①收获机接通过RTK-GNSS 获取自身位置、速度和航向信息；②收获机路径追踪控制器通过路径追踪算法，结合期望路径计算期望转向角；③转向控制器通过转向控制算法，将期望转向角转换为电信号输出至转向执行机构上；④转向执行机构根据电信号，执行收获机的转向。

这4 个环节中都存在延迟：①为导航定位系统的延迟，与导航定位系统的性能有关；②、③为控制算法计算的延迟，与算法复杂度和控制器的性能有关；④为转向控制延迟，与收获机的质量、负载与惯性大小、执行机构性能和地面状态等有关。因此，延迟时间 td可以看成由①～③组成的固定延迟时间和由④组成的动态延迟时间组成。而在基于运动学模型的纯追踪算法中，地面状态的变化、收获机负载与惯性的变化等，最终都会反映到速度的变化上，故动态时间与速度v 有关，可以根据速度自适应调整延迟时间 td，克服收获机因为控制延迟导致的路径追踪精度下降的问题。若采用线性关系，则 td和v 的关系可以表示为

式中：vμ 为速度影响系数；tdτ 为固定延迟时间。

根据速度v 估算延迟控制时间 td后，使用预测的未来时刻的位置n 计算横向误差，以替代式（5）中的横向误差，可以克服因滞后性和控制延迟导致的收获机路径追踪的精度下降问题。

2.3 前视距离的自适应调整

由式（5）可知，影响期望路径唯一的不确定量为前视距离，前视距离的大小会影响路径追踪算法的稳定性和精度[25]。若前视距离过小，收获机将会快速逼近期望路径，但是往往带来较大的振荡；若前视距离过大，收获机将会花费较长时间平稳地逼近期望路径。因此，算法应根据速度和收获机其他状态自适应调整前视距离的大小。模糊控制不依赖于具体的模型，能够较好地处理不同的作业条件下收获机前视距离的动态调整的问题，提高收获机的算法自适应性[26]。

本文设计了1 个3 输入、单输出的模糊控制器，将速度、航向误差和校正后的横向误差作为模糊控制器的输入，将前视距离作为模糊控制器的输出。模糊控制器的输入输出的论域档级决定了模糊控制器的性能，过少的档级会使得模糊控制器的控制效果达不到预期情况，过多的档级会使得模糊控制器的计算复杂从而导致计算的实时性下降。根据收获机的实际运行情况，将速度从0～4 m/s 分为3 个论域{L, M, H}，将横向误差从-4～4 m 分为7 个论域{LB, LM, LS, M, RS, RM,RB}、将航向偏差从-180°～180°分为7 个论域{LB,LM, LS, C, RS, RM, RB}、将前视距离从3～9 m分为7 个论域{VN, N, LN, M, LF, F, VF}。同时根据各模糊变量确定其隶属度函数，将隶属度函数离散化，就得到了有限个点上的隶属度。本研究将所有的隶属度函数都定义为梯形函数。

模糊控制器的模糊规则，决定了在不同的输入下前视距离的具体输出。本文设计的规则为：1）当收获机转弯或逼近期望路径，需要满足速度较小、有较大的航向或者和横向误差时，输出较小的前视距离；2）当收获机在高速稳定作业状态，需要满足速度较大、航向误差和横向误差皆较小时，输出较大的前视距离；3）当收获机临近作业结束，需要满足速度较小、航向误差和横向误差皆较小时，输出1 个中等的前视距离；4）当收获机收获机前进速度较大，航向或者横向误差存在条较大时，输出较小的前视距离。

最终模糊控制器会根据输入参数和模糊规则，输出合适的前视距离，将输出的前视距离代入至式（5），可以提高收获机在不同的速度、横向偏差和航向偏差下的路径追踪的精度和稳定性，有效提高路径追踪算法的自适应能力。

3 实验与结果分析

为了验证基于GNSS 的联合收获机路径追踪方法在田地作业中的实际效果，在江苏某农场进行了田间实验。主要设备的安装位置如图5 所示。

图5 主要设备安装位置示意图

将双天线RTK-GNSS 定位系统安装在收获机车体顶部，角度传感器使用非接触式霍尔传感器，安装于转向轮的转向轴上。

实验方法如下：由司机人工驾驶收获机收割田间的小麦，并使用RTK-GNSS 记录下此次路径作为参考路径，由司机人工掉头后，进入自动驾驶的路径追踪阶段，首先控制器根据参考路径规划出期望路径：控制器通过RTK-GNSS 获取当前时刻的位置，读取参考路径上的第1 个点，生成平移向量，将参考路径上的后续点皆按平移向量平移，生成期望路径，随后控制器会根据收获机的位置信息和期望路径，结合本文提出的路径追踪算法计算期望转向角，再将信号发送给转向控制器，由转向控制器完成转向控制，追踪这条期望路径，直至最后1 个点，此次路径追踪结束后，由司机人工掉头，重复上述过程。在路径追踪的过程中，记录下收获机的位置、航向、速度、转向角、横向误差等所有信息，并使用RTK-GNSS 的坐标作为收获机位置的真值。

实验参数设定如下：卫星更新和控制频率皆为5 Hz，转向控制频率为50 Hz，未优化时的前视距离为4 m，综合考虑到算法复杂度、处理器性能、地面状态和液压转向系统的迟滞性，将速度影响系数vμ 设为0.15 s2·m-1，固定延迟时间tdτ 设为0.1 s。

收获机分别在速度为1.0 和2.5 m/s 的条件下，进行了优化前和优化后的路径追踪实验。针对每种情况分别进行3 次实验，其结果如图6 所示。在基于C++/QT 开发的联合收获机自动导航软件上显示，当收获机速度为1.0 m/s 时，使用优化后算法第1 次追踪期望路径时的路径追踪效果，稳定后的横向误差小于3 cm。

图6 路径追踪实验效果

3 次实验横向误差的趋势图分别如图7（a）、图 7（b）、图 7（c）所示。由于期望路径的第1 个点为收获机当前时刻点，所以初始横向误差皆为0 cm，而由于航向误差的存在，收获机会出现短暂的偏离期望路径的情况，随后立刻收敛。优化前的路径跟踪算法，在逼近期望路径时会有较大的振荡，且需要较长时间才能进入平稳行驶的情况，而基于本文提出的改进路径追踪算法，则能够快速且稳定地进入平稳行驶的情况。

3 次实验的横向误差的标准差如表1 所示。

实验结果表明：本研究提出的路径追踪算法的效果优于传统的纯追踪算法，速度在1 和2.5 m/s时，横向误差的标准差都低于2.5 cm。

图7 收获机横向误差与行驶距离的变化趋势

表1 横向误差实验效果

4 结束语

针对农业智能稻麦收获机在田间自动作业时存在控制延迟而降低路径追踪精度的问题，提出1 种基于GNSS 的收获机路径追踪方法。依托收获机建立了运动学模型，推导了纯追踪算法；使用双天线RTK-GNSS 定位系统，获取收获机的当前时刻的速度、位置和姿态信息，估计未来时刻的位置信息，校正当前时刻的位置并重新计算新的横向误差；设计了1 个模糊控制器，控制器将根据速度、航向误差和校正后的横向误差自适应调节纯追踪算法中的前视距离；最后计算期望转向角，同时转向控制器控制着收获机的转向，实现路径追踪。设计了基于GNSS 的收获机路径追踪系统：基于C++/QT 开发了联合收获机自动导航软件，搭建了基于双天线RTK-GNSS 的定位模块和转向控制反馈回路，并进行了实验验证。实验结果表明：收获机的控制延迟影响路径追踪精度的现象有所改善，在不同的速度和状况下都能有效地追踪期望路径；路径追踪的精度有了提高，横向误差的标准差低于2.5 cm。