农业机械导航技术作为精准农业技术体系中的一项关键技术，能有效提高农业机械田间作业效率和作业质量，降低操作人员劳动强度^[1-3]。在无导航设备的情况下，由于受农机操作人员的工作经验和作业环境的影响，农业机械田间作业易产生重叠和遗漏区域，导致作业成本高、作业效率低，甚至影响农田环境安全^[4-5]。因此，研制农业机械导航系统对于提高农机作业效率和质量、促进我国精准农业的发展具有重要经济价值和社会意义。

近年来，国内外专家学者对农业机械导航系统进行了大量的研究和试验。O′Connor等^[6]、Baio等^[7]、罗锡文等^[8]和刘兆朋等^[9]基于GNSS开发了农业机械自动导航作业系统，并分别在拖拉机、甘蔗收获机、插秧机和喷雾机上开展相应田间试验，系统直线路径平均跟踪偏差均小于3 cm。研究^[10-12]等利用机器视觉识别作物行，提取导航基准线，测量农机与作物行的相对位置偏差，进而控制农机实现自动作业。自动导航系统定位精度高，能够实现自动作业，可有效降低农机操作人员的劳动强度，提高作业精度和作业效率，已应用于耕作、播种、喷药、施肥和收获等生产试验过程^[13]。但自动导航系统成本较高、回收周期长、对农民经营要求较高，并且国内现有老旧农机设备多数不易实现自动化改造。

我国南方地区大多农户经营的田块小且分散，农业整体利润水平低，加之种植规模小，难以接受价格昂贵的农机设备^[14]，我国南方地区多数农户仍较难接受农机自动导航系统。作为一种成本低、易操作且通用性强的产品，辅助导航系统^[15-16]是提高农机作业效率、降低农机操作人员劳动强度的有效方式。本文基于超宽带(Ultra wide band，UWB)技术设计一种低成本的农业机械田间辅助导航系统，无需改装机器，直接安装在作业机械上工作，可实现田间环境中农业机械的精确定位、作业路径规划和作业偏差计算，通过友好人机交互界面为驾驶员提供辅助导航作业信息。该系统适用于南方中小型田块中各种农业机械的田间辅助导航作业，可有效降低农机操作人员的劳动强度，提高农机作业效率。

1 辅助导航系统设计开发 1.1 系统组成

农业机械辅助导航系统由UWB定位系统和终端处理器2部分组成(图1)。UWB定位系统包括1个UWB定位标签和4个UWB定位基站；其中，UWB定位基站固定于定位区域边缘作为定位参考点，由UWB无线传感器、移动电源和基站安装支架组成；UWB定位标签为1个UWB无线传感器，安装于作业机械上并与终端处理器相连，通过与定位基站进行无线通信，获得与各定位基站间的距离；UWB无线传感器内置姿态传感器，用于实时测量作业机械的姿态信息。终端处理器根据测量的距离信息和姿态信息解算出作业机械的准确位置坐标，实时规划作业路径，计算作业偏差，并通过可视化界面实时显示。

整个系统组成结构简单，不需要对机器进行任何改装，可直接安装于不同作业机械上工作，成本低(约0.4万元)，且通用性强。

UWB无线传感器选用Nooploop空循环公司的LinkTrack P定位模块，模块供电电压5 V，最远通信距离为600 m，数据更新频率为1~200 Hz，直线测距精度<10 cm，可通过终端上位机设定工作类型(定位基站或定位标签)。UWB无线传感器内置InvenSense公司的姿态传感器MPU6500，集成了加速度计、陀螺仪和数字运动处理器DMP，三轴欧拉角(俯仰、横滚和航向角)的静态测量精度为0.1°，数据输出频率为1~200 Hz。终端处理器选用中柏科技有限公司的EZpad 7平板电脑，处理器为Intel Cherry Trail Z8350。

1.2 位置解算算法

三维空间中实现1个点的定位一般至少需要4个已知参考点，通过增加定位参考点的数量可以提高定位精度，但定位成本也随之增加^[17]。考虑农业装备的经济和实用性，本系统选用4个UWB定位基站作为系统定位参考点。对于任意四边形的定位区域，将4个UWB定位基站分别布置于定位区域的4个角落，各基站垂直地面安装于相同高度的安装支架上。如图2所示，按作业过程中作业行递增方向作为定位坐标系X轴正方向的原则进行基站布置，基站A₀为定位坐标系原点，基站A₀和A₃的连线为X轴，过原点垂直于水平面竖直向上为Z轴，根据右手定则过原点垂直于XOZ平面为Y轴建立空间直角坐标系作为田块定位坐标系。

各基站间通过无线通信互发数据包测量距离，通过上位机软件进行基站标定，计算各基站的位置坐标，计算公式如式(1)所示：

$\left\{ \begin{array}{l} ({x_0},{y_0}) = (0,0)\\ ({x_1},{y_1}) =\\ \left[ {\dfrac{{d_{03}^2 + d_{01}^2 - d_{13}^2}}{{2{d_{03}}}},\sqrt {{d^2_{01}} - {{\left( {\dfrac{{d_{03}^2 + d_{01}^2 - d_{13}^2}}{{2{d_{03}}}}} \right)}^2}} } \right]\\ ({x_2},{y_2}) = \\ \left[ {\dfrac{{d_{03}^2 + d_{02}^2 - d_{13}^2}}{{2{d_{03}}}},\sqrt {{d^2_{02}} - {{\left( {\dfrac{{d_{03}^2 + d_{02}^2 - d_{13}^2}}{{2{d_{03}}}}} \right)}^2}} } \right]\\ ({x_3},{y_3}) = ({d_{03}},0) \end{array} \right.,$

(1)

式中，(x_i，y_i)为基站A_i的二维位置坐标，d_ij为基站A_i和基站A_j间的直线距离，i，j=0，1，2，3。

将UWB定位基站安装在同一高度，安装高度大于定位标签在田间作业过程中的最大高度，定位基站所在平面为Z轴的零平面。利用基于时间到达差的TWR双向双边测距法测量UWB定位标签与UWB定位基站间的距离，根据定位标签与各基站的距离以及各基站的位置坐标，得到空间4点的定位方程组，如式(2)所示：

$\left\{ {\begin{array}{*{20}{l}} {{{\left( {x - {x_0}} \right)}^2} + {{\left( {y - {y_0}} \right)}^2} + {{\textit{z}}^2} = d_0^2}\\ {{{\left( {x - {x_1}} \right)}^2} + {{\left( {y - {y_1}} \right)}^2} + {{\textit{z}}^2} = d_1^2}\\ {{{\left( {x - {x_2}} \right)}^2} + {{\left( {y - {y_2}} \right)}^2} + {{\textit{z}}^2} = d_2^2}\\ {{{\left( {x - {x_3}} \right)}^2} + {{\left( {y - {y_3}} \right)}^2} + {{\textit{z}}^2} = d_3^2} \end{array}} \right.,$

(2)

式中，(x，y，z)为定位标签的三维坐标，(x_i，y_i， ${\textit{z}}$ _i)为定位基站A_i的三维坐标，d_i为定位标签到基站A_i的直线距离，i=0，1，2，3。

通过加减消元法，消去z坐标得到矩阵方程，如式(3)所示：

$ \left[\begin{array}{ll} 2\left(x_{0}-x_{1}\right) & 2\left(y_{0}-y_{1}\right) \\ 2\left(x_{0}-x_{2}\right) & 2\left(y_{0}-y_{2}\right) \\ 2\left(x_{0}-x_{3}\right) & 2\left(y_{0}-y_{3}\right) \end{array}\right]\left[\begin{array}{l} x \\ y \end{array}\right]=\left[\begin{array}{l} \lambda_{1} \\ \lambda_{2} \\ \lambda_{3} \end{array}\right] \text{，} $

(3)

式中， $\lambda_{i}=d_{i}^{2}-d_{0}^{2}+x_{0}^{2}-x_{i}^{2}+y_{0}^{2}-y_{i}^{2}, \quad i=1, \quad 2, \quad 3$ 。

矩阵方程(3)为超定方程，利用最小二乘法解得定位标签的二维坐标，计算公式如式(4)所示：

$ \left[\begin{array}{l} x \\ y \end{array}\right]=\left({\boldsymbol{A}}^{{\rm{T}}} {\boldsymbol{A}}\right)^{-1} {\boldsymbol{A}}^{{\rm{T}}} {{{\boldsymbol{\beta}}}} \text{，} $

(4)

式中，A为矩阵方程(3)的系数矩阵，β为矩阵方程(3)的常数项列矩阵。

将解得的二维坐标代入原方程组(2)中，默认z坐标为负，解得各方程z坐标并取均值，得到UWB定位标签的三维位置坐标。

1.3 位置校正算法

超宽带脉冲信号传播过程中，非视距现象是影响定位精度下降的主要原因之一^[18-19]。为了避免非视距现象对定位精度的影响，将UWB定位标签安装在作业机械顶部，以保证UWB定位标签与定位基站间通信良好。UWB定位标签安装位置高，作业机械在作业过程中车身倾斜时，导致测量的UWB定位标签位置中心投影与车身真实位置中心投影不重合，所测量的坐标和真实位置存在误差，因此必须对车身倾斜引起的位置误差进行校正^[20-21]。大多数作物行都是平行直线，农业机械的作业路径也是平行直线，作业质量仅要求平面内作业行的直线度。因此，本文仅对作业机械车身定位的横向和纵向误差进行分析，对垂直地面的Z轴误差不予分析。

单纯的车身横滚导致车身出现横向位置偏差，单纯的车身俯仰会导致纵向位置偏差，单纯的航向偏差不会对定位产生太大影响^[22]。如图3a所示，传统的校正方法是假设车身绕重心旋转，利用欧拉角和传感器安装位置来估计位置误差，但由于拖拉机的重心难以估计，且拖拉机在田间作业时的姿态变化并不是单纯围绕车身重心旋转^[23]。实际上，拖拉机在田间环境中的车身倾斜是由于轮胎的下陷和滑移导致的，因此车身的倾斜可以看作是以地面与轮胎接触点为轴心旋转引起的，如图3b所示。

如图4所示，作业机械在田间环境中车轮不同程度的下陷和滑移，导致车身产生横滚角 $\theta_{{\rm{r}}}$ (图4a)和俯仰角 $\theta_{{\rm{p}}}$ (图4b)，测量的车身位置坐标分别存在横向定位误差 $\Delta x $ 和纵向定位误差 $\Delta y$ ，近似将 $ \Delta x $ 和 $ \Delta y$ 直线所在平面看作地平面，分别对测量的位置坐标进行校正，校正公式如式(5)所示：

$ \left\{\begin{array}{l} x_{{\rm{r}}}=x^{\prime}-h \sin \theta_{{\rm{r}}} \\ y_{{\rm{r}}}=y^{\prime}-h \sin \theta_{{\rm{p}}} \end{array}\right. \text{，} $

(5)

式中，x_r为校正后作业机械的横坐标， $ x^{\prime} $ 为校正前UWB定位标签测量的横坐标，y_r为校正后作业机械的纵坐标， $y^{\prime}$ 为校正前UWB定位标签测量的纵坐标，h为UWB定位标签距离地面的安装高度。

利用姿态传感器MPU6500测量车身姿态，通过预试验测试得姿态传感器的三轴欧拉角静态测量最大误差小于0.5°，当传感器安装高度为3 m时，传感器测量误差引入的最大定位误差为2.62 cm，相对于系统定位误差此误差较小，在可接受范围内。

1.4 路径规划算法

根据农业机械田间往返式直线作业特点，设计一种基于AB线的路径规划算法，实现田间作业的实时路径规划，减少农机作业过程中的重复作业和遗漏作业区域，提高农机作业效率和作业质量。首行作业时，农业操作人员沿作业区域边缘自主完成首行作业。作业过程中，系统从该行作业起始A点开始采集作业轨迹，到该行结束B点停止采集。

由于受到外部环境、传感器内部构造和随机误差等多种因素的影响，传感器测得的数据往往具有一定的不确定性，而系统采集的异常数据将直接影响试验结果。常见的检测异常值方法包括3σ原则、狄克逊准则等，这些方法都是假定测量数据呈正态分布的，而实际传感器采集的数据分布往往是复杂而未知的。这些方法判断异常值的标准都是通过计算采集数据的均值和标准差来实现的，均值和标准差均受数据中异常值的影响，而异常值难以准确判断。本文采用分位数法判断采集数据中的异常值。将采集的数据按从小到大排列成数组X：[x₁，x₂，…，x_n]，分别找出下四分位数Q₁，中位数Q₂和上四分位数Q₃，各数值分布如图5所示。中位数Q₂为数组X的中位数，下四分位数Q₁为数组X中[x₁，Q₂]的中位数，Q₃为数组X中[Q₂，x_n]的中位数，异常值上下边界点的计算如式(6)所示：

$\left\{\begin{array}{l} F_{{\rm{U}}}=Q_{3}+\dfrac{\gamma }{2}\left(Q_{3}-Q_{1}\right) \\ F_{{\rm{L}}}=Q_{{\rm{1}}}-\dfrac{\gamma }{2}\left(Q_{3}-Q_{1}\right) \end{array}\right.,$

(6)

式中，F_U上边缘异常值分界点，F_L为下边缘异常值分界点， $\gamma $ 为判断系数。

经验表明，当r=3时，批量处理数据时具有较高的鲁棒性^[24]，因此本文确定异常值上下边界时r取值为3。

利用分位数法剔除采集数据中的异常数据，通过最小二乘法拟合出该作业行的直线作业轨迹l₁，如式(7)所示，将拟合作业轨迹沿作业行递增方向平移1个作业幅宽d，得到下一作业行的规划作业路径l₂。作业过程中，根据规划的作业路径和当前的位置坐标，实时计算出当前作业偏差。

$ \left\{\begin{array}{l} l_{1}: y=k x+b \\ l_{2}: y=k(x+d)+b \end{array}\right.,$

(7)

式中， $k = \frac{{\sum\limits_{i = 1}^n {\left( {{x_i} - \bar x} \right)} \left( {{y_i} - \bar y} \right)}}{{\sum\limits_{i = 1}^n {{{\left( {{x_i} - \bar x} \right)}^2}} }},\quad b = \bar y - k\bar x$ ；n为本次作业行采集数据点的个数， $ \bar y $ 为采集数据点纵坐标的平均值， $ \bar x $ 为采集数据点横坐标的平均值，x_i、y_i分别为采集点的横坐标和纵坐标。

1.5 辅助导航系统设计

辅助导航系统在Visual Studio 2012开发环境下，利用C#面向对象编程语言进行软件开发，实现串口通信、位置与姿态数据的采集与处理，作业路径规划，实时位置偏差计算和数据储存等功能，系统操作界面如图6所示。

辅助导航系统工作时，首先与UWB定位系统和姿态传感器建立串口通信，根据接收的距离信息和姿态信息实时解算出作业机械的准确位置；然后系统根据作业轨迹实时规划作业路径、计算作业偏差，并通过可视化界面显示；最后系统对作业的轨迹进行实时记录，并实现作业数据的储存，系统工作流程如图7所示。

2 田间试验与结果

为验证本文算法的有效性，分别开展系统田间定位精度测试试验和辅助导航系统作业性能测试试验。

2.1 系统田间定位精度测试试验

为验证位置解算算法和位置校正算法的有效性，进行田间环境的定位测试试验。试验在广东省肇庆市农业科学研究所沙浦农场进行，定位区域为长约93.6 m、宽约94.4 m的四边形水田，试验条件为微风，室外温度约30 ℃。首先，将UWB定位基站等高度固定安装于定位区域的4个角落，基站安装高度为3.6 m，建立定位坐标系，各基站坐标分别为A₀(0，0)、A₁(1.408，95.58)、A₂(95.25，94.97)、A₃(93.39，0)，单位为m。辅助导航系统搭载于井关PZ60插秧机上，UWB定位标签安装于插秧机顶部对称轴线上，安装高度为2.5 m，如图8所示。

在定位水田区域内布置3条直线，插秧机分别以0.5、1.0和1.5 m/s 沿着规划直线行驶，记录实际跟踪轨迹，并将采集的实际跟踪轨迹和规划直线进行比较，剔除采集数据中的异常值，计算横向偏差。校正前、后实际跟踪轨迹离散点和规划路径如图9所示。

不同速度下校正前、后实际跟踪轨迹与规划路径的横向偏差统计结果如表1所示，插秧机分别以0.5、1.0和1.5 m/s的速度沿规划直线行驶，校正前最大横向偏差分别为21.10、33.83和45.10 cm，平均横向偏差分别为8.01、13.81和17.30 cm，标准差分别为4.94、8.57和12.14 cm；校正后最大横向偏差分别为15.93、16.90和17.97 cm，平均横向偏差分别为6.15、6.52和6.61 cm，标准差分别为4.06、4.30和4.66 cm。试验结果表明，当插秧机行驶速度大于1.0 m/s时，校正后平均横向偏差降低了52.79%，最大横向偏差降低了50.04%，平均标准差降低了49.82%，位置校正算法显著提升了系统田间动态定位精度。

另外，随着插秧机行驶速度加快，系统定位精度略微下降，其原因是行驶速度提高加剧了插秧机的震动，增大了姿态角测量误差，车身姿态变得更加复杂以致解算出的位置精度下降。田间动态定位试验结果表明，在复杂的田间环境中，本研究提出的位置校正算法能有效提高定位精度，提高系统定位稳定性，能够较好地满足农业机械田间实时定位和位置监测的需求。

2.2 辅助导航系统作业性能测试试验

2021年3月30日，在广东省肇庆市农业科学研究所沙浦农场进行插秧辅助导航试验。插秧试验田为长约50 m、宽约45 m的不规则四边形田块，试验条件为微风，田间温度约为33 ℃，湿度为56%。将UWB定位基站等高度固定安装于试验田块的4个角落，基站安装高度为3.6 m，建立定位坐标系，各基站坐标分别为A₀(0,0)、A₁(−4.404，45.158)、A₂(36.218，50.511)、A₃(43.201，0)，单位为m。如图8所示，在井关PZ60插秧机上搭载辅助导航系统，插秧机作业幅宽为1.8 m，UWB定位标签安装高度为2.5 m。插秧过程中，插秧机行进速度为1.0 m/s，不打开插秧划线杆，驾驶员不参考已作业完苗带和侧标杆，仅根据辅助导航系统提示实时调整作业方向进行作业，作业跟踪轨迹如图10所示。

各行作业轨迹分别与自身拟合直线和规划路径的横向偏差计算结果如表2所示，利用辅助导航系统进行插秧作业，各行作业轨迹与自身拟合直线的平均横向偏差为5.90 cm，最大横向偏差为19.27 cm，标准差为3.64 cm；作业行与规划直线路径的平均横向偏差为6.98 cm，最大横向偏差为31.50 cm，标准差为4.95 cm。第一行作业轨迹与自身拟合直线平均偏差较大是因为第一行是由驾驶员按照插秧区域边界和自身驾驶经验完成作业，导致第1行作业轨迹直线度较差。由于作业田块中存在泥脚较深区域，插秧机在此区域作业时车身滑移和倾斜较严重，导致第6行作业轨迹与规划路径的横向偏差较大。

试验结果表明，利用辅助导航系统进行辅助作业，各行作业轨迹与自身拟合直线横向偏差较小，作业行直线度较好；各行作业轨迹与规划直线路径存在一定的行距偏差。利用辅助导航系统进行作业总体效果较好，基本满足农业生产需求，在一定程度上降低了操作人员的劳动强度、提高了劳动生产率、降低了农业机械作业效果对操作人员熟练度的依赖。

3 结论

基于超宽带(UWB)技术设计的农业机械辅助导航系统，适用于南方中小型田块中各种农业机械的田间辅助导航作业，可在一定程度上降低农机操作人员的劳动强度、提高作业效率、降低农业机械作业效果对操作人员熟练度的依赖。该系统不需要改装机器，可直接安装于不同农业机械，低成本实现农业机械田间辅助导航作业。

开展田间环境中系统定位试验，低成本实现田间环境中农业机械的精确定位。水田环境中，插秧机分别以0.5、1.0和1.5 m/s的速度沿着规划路径行驶，系统平均横向定位偏差分别为6.15、6.52和6.61 cm，标准差分别为4.06、4.30和4.66 cm。当插秧机速度大于1.0 m/s时，通过位置校正算法，校正后平均横向定位偏差降低了52.79%，最大横向定位偏差降低了50.04%，平均标准差降低了49.82%，有效提高了系统田间定位精度和稳定性，可实现田间环境下农业机械的精确定位。

进行了搭载辅助导航系统田间机插秧试验，实现插秧机田间辅助导航对行作业。搭载辅助导航系统进行田间插秧试验，试验结果表明，各行作业轨迹与自身拟合直线平均横向偏差为5.90 cm，标准差为3.64 cm；各行作业轨迹与规划直线路径的平均横向偏差为6.98 cm，标准差为4.95 cm；搭载辅助导航系统作业，各行作业轨迹直线度较好，与规划路径的横向偏差较小，基本满足南方中小型田块中农业机械田间作业要求，有效地降低了操作人员的劳动强度、提高了工作效率。