自动导航是智能农机装备的核心技术之一。农机自动导航作业技术可有效提高农机的作业效率和质量，避免漏行叠行作业，以及降低驾驶员的劳动强度^[1-2]。目前，自动导航农机在我国新疆、黑龙江等地已实现产业化与规模化生产，但在实际应用中也存在一些问题：一是农机自动导航系统需要依赖高精度GNSS定位技术，其复杂的地基增强差分通讯链路增加了系统复杂度和运营成本；二是导航控制算法对不同农机作业速度的适应性不好，随着速度的增加，导航精度会逐渐下降^[3-4]。

星基增强精密单点定位技术可单机作业，灵活机动，作业不受通讯距离的限制^[5-7]。它集成了标准单点定位和差分定位的优点，克服了各自的缺点^[8-11]，改变了只能使用双差定位模式才能达到较高定位精度的现状，较传统的差分定位技术具有显著的技术优势^[12-16]。近年来，国内有些公司和科研机构开始提供星基增强精密单点定位技术的商业化服务，定位性能不断提高，服务范围不断扩大。本研究尝试采用星基增强精密单点定位技术开发农机自动导航系统，以减少系统对地基增强差分通讯链路的依赖。与此同时，设计基于位速卡尔曼滤波的导航信息处理算法和预瞄跟随PID路径跟踪控制算法，通过对不同农机作业速度下的前视距离与PID参数进行试验整定，获得控制参数整定表，并在自动导航拖拉机上进行不同行驶速度下的性能验证，以期为提高系统对不同速度的自适应能力提供参考。

1 系统总体结构

本研究以华南农业大学研究的农机自动导航系统为基础，以配套高花轮胎的雷沃TX1204拖拉机为平台^[17-21]，基于精简、通用、稳定可靠的设计思想，融合导航信息获取和路径跟踪控制算法，开发基于星基增强精密单点定位的拖拉机自动导航系统。导航系统由GNSS天线、合众思壮V28星基增强定位板卡、导航控制器、显示终端、轮角/压力传感器、电控液压转向系统等组成，如图1所示。导航控制器是农机导航系统的核心，主要功能是：1)根据显示终端输入的A₁、A₂点信息和作业幅宽规划作业路径；2)通过GNSS天线、星基增强定位板卡以及轮角/压力传感器获取定位信息以及前轮转向角度信息，之后对接收的各传感器信息进行处理；3)利用路径跟踪控制算法决策期望轮角并输出转向控制信号。电控液压转向系统是农机的转向执行机构，其作用是将转向控制信号转换成农机转向轮的转向速度和角度，进而控制农机按照预定作业路线精确跟踪行走。显示终端是导航系统的人机交互终端，通过CAN总线与导航控制器进行通信，实现导航系统的状态显示、系统调试、作业设置和系统操控等人机交互功能。

2 导航信息处理方法 2.1 GRS80大地坐标向大地导航坐标系的转换

为了使GNSS星基增强定位板卡输出的GRS80大地坐标定位数据能用于拖拉机的导航控制系统，需要采用高斯投影变换公式，将GRS80地心大地坐标转换为对应于GRS80椭球的高斯投影平面坐标。

采用如下公式：

$ \begin{split} x =& X + \frac{{{l^2}}}{2}N\sin B\cos B + \frac{{{l^4}}}{{24}}N\sin B({\cos ^3}B)(5 - {t^2} +\\ &9{\eta ^2} + 4{\eta ^4}) + \frac{{{l^6}}}{{720}}N\sin B(\cos 5B)(61 - 58{t^2} + {t^4})，\end{split} $

(1)

$ \begin{split} y = & lN\cos B + \frac{{{l^3}}}{6}N({\cos ^3}B)(1 - {t^2} + {\eta ^2}) +\\ & \frac{{{l^5}}}{{120}}N({\cos ^5}B)(5 - 18{t^2} + {t^4} +\\ &14{\eta ^2} - 58{\eta ^2}{t^2}) + 500\;000，\end{split} $

(2)

式中， $ l = L - {L_0} $ ， $ {L_0} $ 为投影带中央子午线经度，B为纬度，卯酉圈曲率半径 $ N = \dfrac{a}{{\sqrt {1 - {e^2}{{\sin }^2}B} }} $ ，椭球第一偏心率 $ e = 2\partial - $ $ {\partial ^2} $ ，辅助变量 $ t = \tan B $ ，辅助变量 $ \eta = $ $ e'\cos B $ ，椭球第二偏心率 $ e' = \sqrt {{a^2}/{b^2} - 1} $ ，a、b 分别为参考椭球的长、短半径；扁率 $ \partial = (a - b)/a $ 。X为赤道至纬度为B的平行圈的子午线弧长，其计算公式为：

$ X=c{\displaystyle {\int }_{0}^{B}(1+e'2\mathrm{cos}2B-2/3){\rm{d}}B}，$

(3)

式中，c为极曲率半径。

所用投影坐标系的主要参数为：投影方式：Gauss-Kruger；中央经线：114.000000(3度带)；水平偏移量：500 km；地理坐标系：GCS_GRS_1980；大地参照系：D_GRS_198参考椭球体：GRS80；椭球长轴：6378137.000000；椭球扁率：0.0033528107。

2.2 GNSS天线位置点向导航控制点的转换

本文所使用的大地导航坐标系由GRS80椭球对应的高斯−克吕格投影坐标平面加上GRS80地心坐标系中的大地高程构成，其中 ${x_n}$ 轴指向地理东向， ${y_{\rm{n}}}$ 轴指向地理北向， ${z_{\rm{n}}}$ 轴指向天空方向，与 ${x_{\rm{n}}}{y_{\rm{n}}}$ 轴构成右手握手坐标系，坐标原点 ${O_{\rm{n}}}$ 设置在拖拉机作业区域的适当位置；所使用的车体坐标系原点 ${O_{\rm{b}}}$ 位于拖拉机重心位置，三轴指向分别为： ${X_{\rm{b}}}$ 轴指向拖拉机机头方向， ${Z_{\rm{b}}}$ 轴指向拖拉机驾驶室顶棚方向。将导航控制点 ${P}$ 选择在拖拉机2个后轮着地点的连线中点处，有利于建立更为严密的拖拉机航位推算递推关系式；有利于GNSS天线定位点 $P'$ 至导航控制点 ${P}$ 距离的量取；有利于拖拉机自动导航过程中后挂作业机具的位置控制；有利于GNSS天线倾斜误差的校正。

假设导航系统测得的拖拉机三轴姿态角航向角度、横滚角度和俯仰角度分别为 $\psi $ 、 $\varphi $ 和 $\theta $ ，则拖拉机车体坐标系下的空间矢量向大地导航坐标系转换的转换矩阵 ${R_{{\rm{b}}{\rm{n}}}}$ 为：

$ \begin{split} {R_{{\rm{bn}}}} = & {R_\psi }^Z{R_\theta }^Y{R_\varphi }^X \\ = & \left[ {\begin{array}{*{20}{c}} {\cos \psi }&{ - \sin \psi }&0 \\ {\sin \psi }&{\cos \psi }&0 \\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\cos \theta }&0&{\sin \theta } \\ 0&1&0 \\ { - \sin \theta }&0&{\cos \theta } \end{array}} \right]\left[ {\begin{array}{*{20}{c}} 1&0&0 \\ 0&{\cos \varphi }&{ - \sin \varphi } \\ 0&{\sin \varphi }&{\cos \varphi } \end{array}} \right] \\ = & \left[ {\begin{array}{*{20}{c}} {\cos \psi \cos \theta }&{ - \sin \psi }&{\cos \psi \sin \theta } \\ {\sin \psi \cos \theta }&{\cos \psi }&{\sin \psi \sin \theta } \\ { - \sin \theta }&0&{\cos \theta } \end{array}} \right]\left[ {\begin{array}{*{20}{c}} 1&0&0 \\ 0&{\cos \varphi }&{ - \sin \varphi } \\ 0&{\sin \varphi }&{\cos \varphi } \end{array}} \right] \\ = & \left[ {\begin{array}{*{20}{c}} {\cos \psi \cos \theta }&{ - \sin \psi \cos \varphi + \cos \psi \sin \theta \sin \varphi }&{\sin \psi \sin \varphi + \cos \psi \sin \theta \cos \varphi } \\ {\sin \psi \cos \theta }&{\cos \psi \cos \varphi + \sin \psi \sin \theta \sin \varphi }&{ - \cos \psi \sin \varphi + \sin \psi \sin \theta \cos \varphi } \\ { - \sin \theta }&{\cos \theta \sin \varphi }&{\cos \theta \cos \varphi } \end{array}} \right]。\end{split} $

(4)

假设GNSS天线定位点 $P'$ 到导航控制点 ${P}$ 的空间矢量在车体坐标系下的表达形式为[a b h]^T，GNSS天线定位点 $P'$ 在大地导航坐标系下的坐标为[x' y' z']^T，导航控制点 $ {P} $ 在大地导航坐标系下的坐标为[x y z]^T，则有：

$ \left[ {\begin{array}{*{20}{c}} {{x}} \\ {{y}} \\ {{z}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{x'}} \\ {{y'}} \\ {{z'}} \end{array}} \right] - {R_{{\rm{bn}}}}\left[ {\begin{array}{*{20}{c}} a \\ b \\ h \end{array}} \right]。$

(5)

2.3 位速卡尔曼滤波器设计

为保证作业质量，拖拉机田间作业过程中大多采用往复直线作业的形式。针对这种情况，本文提出基于位置和速度的拖拉机运动学模型，来适配拖拉机田间直线行驶作业的实际过程。

基于航位推算原理，建立拖拉机导航控制点 ${P}$ 在2D平面坐标系下的运动方程：

$ {x_{k}} = {x_{k - 1}} + {v_{xk}}\Delta t ，$

(6)

$ {y_{k}} = {y_{k - 1}} + {v_{yk}}\Delta t，$

(7)

式中， ${x_{k}}$ 、 ${y_{k}}$ 为k时刻拖拉机导航控制点的高斯投影平面坐标， ${v_{xk}}$ 、 ${v_{yk}}$ 为k时刻的导航控制点在高斯投影平面x、y轴向的前进速度，Δt为航位推算的时间间隔。Δt与GNSS定位数据的更新频率 ${f'}$ 有关：

$ \Delta t = 1/{f'} 。$

(8)

星基增强精密单点定位可能存在异常跳点，会对导航系统产生干扰。为了提高导航系统的工作稳定性，本文基于上述航位推算原理设计了位速卡尔曼滤波器来对星基增强精密单点定位进行滤波处理。卡尔曼滤波器的设计基础是状态转移方程和观测方程。考虑到拖拉机田间匀速行驶作业的实际特点，本文假设 ${v}_{xk}$ 、 ${v}_{yk}$ 的稳态是缓变的。将前述(6)、(7)式以卡尔曼滤波器状态转移方程的形式表示为：

$ {{\boldsymbol{X}}_k} = {{\boldsymbol{A}}_k}{{\boldsymbol{X}}_{k - 1}} + {{\boldsymbol{u}}_k} ，$

(9)

式中， ${{\boldsymbol{X}}_k} = \left[ {{x_{k}},{v_{xk}},{y_{k}},{v_{yk}}} \right]$ ，表示状态空间向量；

$ \boldsymbol{A}_k = \left[ {\begin{array}{*{20}{c}} 1&1&0&0 \\ 0&1&0&0 \\ 0&0&1&1 \\ 0&0&0&1 \end{array}} \right]$ 是k时刻状态转移矩阵，根据前述分析是个常值矩阵；

${{\boldsymbol{u}}_k}$ 是状态转移方程的白噪声；系统过程噪声协方差矩阵为 ${Q_k}$ ，用于表示状态转移方程的误差大小，本文中 ${Q_k}$ 设定为常数矩阵，在仿真和试验过程中整定矩阵参数。

以经式(5)转换得到的拖拉机导航控制点定位坐标作为观测向量，得到卡尔曼滤波器的测量方程如下：

$ {{\boldsymbol{Z}}'_{k}} = {{\boldsymbol{H}}_k}{{\boldsymbol{X}}_k} + {{\boldsymbol{w}}_k}，$

(10)

式中， ${{\boldsymbol{Z}}'_{k}} = \left[ \begin{array}{l} {x'_{k}} \\ {y'_{k}} \\ \end{array} \right]$ ， ${x'_{k}}$ 、 ${y'_{k}}$ 为拖拉机导航控制点的k时刻定位坐标， ${{\boldsymbol{H}}_k} = \left[ {\begin{array}{*{20}{c}} 1&0&0&0 \\ 0&0&1&0 \end{array}} \right]$ ，为卡尔曼滤波器k时刻的测量矩阵， ${{\boldsymbol{w}}_k} = \left[ \begin{array}{l} {\xi' _{xk}} \\ {\xi' _{yk}} \\ \end{array} \right]$ ， ${\xi' _{xk}}$ 、 ${\xi' _{yk}}$ 表示OEM GNSS板卡定位在水平面坐标系下的随机定位误差。

测量向量的噪声方差矩阵为：

$ {{\boldsymbol{R}}_k} = \left[ {\begin{array}{*{20}{c}} {{r_{x{k}}^2}}&0 \\ 0&{{r_{y{k}}^2}} \end{array}} \right]，$

(11)

式中， ${r_{x{k}}^2}$ 、 ${r_{y{k}}^2}$ 分别为 ${\xi' _{xk}}$ 、 ${\xi' _{yk}}$ 的方差统计值。

综合上述推导，采用线性离散卡尔曼滤波器的递归差分方程进行状态向量预测和测量向量校正：

预测方程组为：

$ {\widehat {\boldsymbol{x}}_k}^ - = {{\boldsymbol{A}}_k}{\widehat {\boldsymbol{x}}_k} + {{\boldsymbol{u}}_k}，$

(12)

$ {{\boldsymbol{P}}_k} = {{\boldsymbol{A}}_k}{{\boldsymbol{P}}_{k - 1}}{{\boldsymbol{A}}_k}^{\rm{T}} + {{\boldsymbol{Q}}_{k - 1}}，$

(13)

式中， ${\widehat {\boldsymbol{x}}_k}^ - $ 代表k时刻的预测结果， ${\widehat {\boldsymbol{x}}_k}$ 代表k时刻的最优估计结果， ${{\boldsymbol{A}}_k}$ 代表k时刻的状态转移矩阵， ${{\boldsymbol{A}}_k}^{\rm{T}}$ 代表 ${{\boldsymbol{A}}_k}$ 的转置， ${{\boldsymbol{\widehat P}}_k}^-$ 为对应 ${\widehat {\boldsymbol{x}}_k}^ - $ 在k时刻的系统过程噪声方差预测值， ${{\boldsymbol{P}}_{k - 1}}$ 为对应 ${\widehat {\boldsymbol{x}}_k}$ 在k−1时刻的系统过程噪声方差预测值， ${{\boldsymbol{Q}}_{k - 1}}$ 为k−1时刻的系统过程协方差。

校正方程组为：

$ {{\boldsymbol{K}}_k} = {\widehat {\boldsymbol{P}}_k}^ - {{\boldsymbol{H}}_k}^{\rm{T}}{({{\boldsymbol{H}}_k}{\widehat {\boldsymbol{P}}_k}^ - {{\boldsymbol{H}}_k}^{\rm{T}} + {{\boldsymbol{R}}_k})^{ - 1}}, $

(14)

$ {\widehat {\boldsymbol{x}}_k} = {\widehat {\boldsymbol{x}}_k}^ - + {{\boldsymbol{K}}_k}({{\boldsymbol{Z}}'_{k}} - {{\boldsymbol{H}}_k}{\widehat {\boldsymbol{x}}_k}^ - ), $

(15)

$ {{\boldsymbol{P}}_k} = ({\boldsymbol{E}} - {{\boldsymbol{K}}_k}{{\boldsymbol{H}}_k}){\widehat {\boldsymbol{P}}_k}^ -, $

(16)

式中， ${{\boldsymbol{K}}_k}$ 为k时刻的卡尔曼滤波增益， ${{\boldsymbol{H}}_k}^{\rm{T}}$ 为 ${{\boldsymbol{H}}_k}$ 的转置， ${\widehat {\boldsymbol{x}}_k}$ 为k时刻卡尔曼滤波器最优估计值， ${{\boldsymbol{Z}}'_{k}}$ 为k时刻卡尔曼滤波器的测量更新值， ${{\boldsymbol{P}}_k}$ 为k时刻卡尔曼滤波器的系统过程噪声方差估计值，E为单位矩阵。

3 预瞄跟随路径跟踪控制算法设计 3.1 预瞄跟随PID路径跟踪控制模型

本文设计的预瞄跟随PID路径跟踪控制模型可用图2描述：直线A₁A₂为作业规划行直线；点 ${P_1}$ 为拖拉机的当前导航控制点， ${P_2}$ 为拖拉机前进方向上的预瞄点， ${P_1}{P_2}$ 为预瞄跟随的前视距离d；过点 ${P_1}$ 向A₁A₂做垂线，垂线的距离即为拖拉机在大地坐标系下的位置偏差 $P{\rm{e}}$ ；预瞄点 ${P_2}$ 向A₁A₂做垂线，垂线的距离为拖拉机的预瞄点位置偏差 $P{{\rm{e}}_1}$ ；设 $\alpha $ 为拖拉机当前航向与目标航向的偏差角，称为航向偏差。预瞄跟随PID路径跟踪控制算法步骤如下：

1)获取拖拉机当前导航控制点 ${P_1}$ 的导航平面坐标。

2)计算拖拉机导航控制点到规划路径A₁A₂上位置偏差 $P{\rm{e}}$ ，则拖拉机预瞄位置点处的位置偏差 $P{{\rm{e}}_1}$ 可计算为：

$ P{{\rm{e}}_1}=P{\rm{e}}\pm d \mathrm{sin}\alpha 。$

(17)

3)设 $ \Delta \psi $ 为拖拉机转向轮角控制量，则基于位置偏差 $P{e_1}$ 的PID控制算法如式(18)所示：

$ \Delta \psi = {K_{\rm{P}}} P{\rm{e}}_1 + K_{\rm{t}} V \tan \alpha + {K_{\rm{I}}} \int\limits_0^t {P{\rm{e}}(t){\rm{d}}t}，$

(18)

式中， $ {K_{\rm{P}}} $ 为位置误差系数， $ {K_{\rm{t}}} $ 为航向误差系数， $ {K_{\rm{I}}} $ 为位置误差积分系数， $ V $ 为行驶速度。

3.2 模型控制参数的试验整定方法

由于算法简单、鲁棒性好和可靠性高，PID控制策略被广泛应用于工业过程控制，尤其适用于可建立精确数学模型的确定性控制系统。比例控制P可快速、及时、按比例调节偏差，提高控制灵敏度^[22]，在保持系统稳定的前提下，P值越大，精度越高。积分控制I能消除偏差，提高控制精度、改善稳态性能。微分控制D是一种超前控制，能调节系统速度、减小超调量、提高稳定性。驾驶员在驾驶农机作业时常常使用对中杆，对中杆与预定路线偏差大时，驾驶员对转向轮角进行大角度控制，以快速校正预瞄航向偏差跟踪预瞄路径；当农机方向和预瞄方向偏差较小时，驾驶员对转向轮角进行小角度控制，以稳定地跟踪预瞄路径^[23-26]。在路径跟踪控制过程中，农机的行驶速度越高，其需要设置的前视距离越大，也就是要瞄的更远些。根据上述经验和规律，需合理选择前视距离和PID控制器参数才能迅速、准确、平稳地消除偏差，达到较好的控制效果。

本文制定的模型控制参数的试验整定方法为：在某一恒定前进速度条件下，首先将P固定为一个经验适中值，前视距离由大向小调(步长1 m)，调到系统微微震荡时停止，将前视距离+1作为此前进速度条件下前视距离的寻优结果；然后，将前视距离固定为寻优值，将P从那个适中值由小向大调(步长5)，调到系统微微震荡时停止，当前的P值即为寻优结果；再次，将前视距离和P值固定为寻优值，将D从0由小向大调(步长1)，调到系统不震荡时停止，当前的D值即为寻优结果。对I的寻优整定，遵循低速大些、高速小些的调节原则，保证在能够快速纠偏的前提下不引起震荡即可。整定后的参数如表1所示。

4 试验测试 4.1 测试方法

采用和芯星通UM482-BOX2 RTK定位终端输出数据作为参考量，搭建拖拉机自动导航测试系统。自动导航系统测试的原理图如图3所示。

本文使用1拖2功分器将GNSS双天线分别接入V28星基增强定位板卡和UM482-BOX2 RTK定位终端。导航控制器将拖拉机路径跟踪结果加上GNSS定位时间戳以10 Hz的输出频率从串口输出到笔记本以供保存。导航控制器的输出参数主要包括：星基增强定位的相关数据、A₁和A₂点、位置偏差、航向偏差、期望轮角、反馈轮角等。与此同时，UM482-BOX2 RTK定位终端通过内置4G网络模块接入千寻知寸厘米级差分服务数据，将带有GNSS定位时间戳的NMEA GPGGA报文以10 Hz的输出频率从串口输出到笔记本以供保存。笔记本同时接收2路数据输入，以GNSS定位的时间戳保持数据同步，以UM482-BOX2 RTK定位终端的参考数据以及A₁和A₂点数据为依据，通过自主开发的Matlab程序和Python程序来测算导航控制精度指标。

具体测试过程为：在平坦地面，启动拖拉机自动导航系统，按照速度为3、5、7 km/h 3个档位开展系统性能测试^[27]。每次测试执行如下流程：首先是完成上线过程，再按规定的速度继续沿着A₁A₂线一个方向自动导航，直线行驶测试段长度不小于80 m，然后掉头，重新上线，并沿着相邻行的相反方向自动导航至少80 m回到开始位置，最后依据和芯星通UM482 RTK接收机提供的记录数据计算相关参数，进而测试系统的直线跟踪误差和邻接行误差，如图4所示。

4.2 试验结果

用Python编程语言开发原始数据对照测试的程序。最后将测算的数据以文本文件的方式导入Matlab程序中，绘制出自动导航性能测试结果如图5所示。

4.3 测试结果分析

以和芯星通UM482 RTK接收机提供的记录数据为参考，对速度3、5、7 km/h 3个档位的路径跟踪数据进行统计分析，结果如表2所示。

由表2统计路径跟踪的误差分析可知，农机自动导航系统在直线跟踪方面，3、5、7 km/h的平均误差分别为−0.0007564、−0.0009170和−0.0011575 m，直线跟踪误差大小随着行驶速度的增加而略微增大，表明行驶速度对农机自动导航系统的直线跟踪性能的影响不明显；农机自动导航系统在邻接行误差方面，3、5、7 km/h的平均误差分别为0.0006187、0.0007249和0.0007950 m，表明不同的行驶速度对农机自动导航系统邻接行跟踪性能的影响不明显。上述分析表明，预瞄跟随PID参数的整定有利于提高系统对不同速度的适应性。

由不同速度下各个指标的平均值可知，在直线跟踪误差方面，平均误差为−0.0009436 m，标准差为0.02452 m，最大误差绝对值为0.08472 m；在邻接行误差方面，平均误差为0.0007128 m，标准差为0.02986 m，最大误差绝对值为0.15444 m。结果表明，农机自动导航作业直线平均误差和邻接行平均误差都在1 mm左右，标准差小于3 cm，这一精度可满足大部分农机自动导航作业需求，星基增强精密单点定位技术可以用于农机自动导航系统的产品研发和推广应用。

5 结论

本文设计了基于星基增强精密单点定位的农机自动导航系统，以和芯星通地基增强RTK接收机为参考，按3、5、7 km/h 3个速度档位开展了系统测试工作。测试结果表明，国产星基增强定位技术已趋于成熟，将国产星基增强定位技术用于农机自动导航是可行的，其路径跟踪控制精度可满足大部分农机作业需求。另外，本文设计的预瞄跟随PID路径跟踪控制模型和提出的不同速度条件下PID参数与前视距离的整定方法，为提高系统对不同速度的自适应能力提供了基础。

对于农机自动导航系统来说，星基增强精密单点定位精度是影响其直线跟踪精度和对行精度的重要因素，因而优化精密单点定位的算法性能，提升其定位稳定性，是提升星基增强自动导航系统精度的重要措施。