农业机械自动导航技术已经得到较为普遍的应用^[1-3]，前轮转角测量是农业机械自动导航的关键技术之一^[4]。轮角测量传感器的测量精度和安装可靠性是直接影响自动导航控制性能的重要因素^[5]。目前，自动导航农机的轮角传感器主要有连杆式直接安装的测量方式和安装较为简洁的间接测量方式^[6]。连杆式直接安装的角度传感器可直接测量农机的转向轮角，常采用的传感器有电位计^[7]、电阻式角度传感器^[8-10]、绝对式编码器^[11-13]、霍尔式传感器^[14-15]等。这类传感器一般成本较低，但是在实际生产中存在安装较麻烦、需准确标定等问题，且连杆易松动或折断^[16]。间接测量常采用陀螺仪及MEMS陀螺仪^[17-18]、磁敏传感器^[19-20]等非接触式传感器测量车轮的相对转动角速率或角度，通过信息处理的方式获得车轮转向角。

本文提出基于GNSS航向微分和MEMS陀螺仪的转向轮角测量方法，通过MEMS陀螺仪测量车身和车轮的合转动速率以及对GNSS天线输出的航向和速度信息进行处理，设计自适应卡尔曼滤波器进行信息的融合和校正，提高轮角测量的精度，并在自动导航拖拉机上试验进行验证。

1 材料与方法 1.1 试验材料

如图1a所示，选取安装农业机械导航系统的雷沃欧豹M904–D型拖拉机为试验研究平台，农机导航系统采用华南农业大学和雷沃重工股份有限公司联合研制的AGCS–I系统，其定位定向模块采用上海司南导航技术股份有限公司生产的K728GNSS板卡，RTK定向更新频率为10 Hz，航向角测量精度为0.2°/R(R为双天线基线长，1.6 m)。试验选用美国BEI公司生产的DUNCAN 9360型霍尔式角度传感器作为轮角测量的参考对象，如图1b所示安装于农机左前轮轮轴上，DUNCAN 9360型角度传感器的测量精度为 ± 0.6%，输出频率最低响应为1 kHz。选用日本Silicon sensing公司生产的CRS03−02型硅性模拟量输出的陀螺仪测量车轮转动合角速率，安装于农机左前轮转向轴，如图1c所示，CRS03–02陀螺仪启动时间小于0.2 s，分辨率小于 ± 0.1°/s/g，非线性度小于 ± 0.5%，带宽为10 Hz(−3 dB)。

1.2 基于GNSS航向微分和MEMS陀螺仪的轮角测量方法

如图2所示，获取GNSS双天线测量车身航向微分，设计自适应卡尔曼滤波器提高双GNSS天线航向微分的精度，获得车身航向微分；采用MEMS陀螺仪测量农机转向轮的转动合速率，导航控制器采集MEMS陀螺仪数据与GNSS双天线测量的车身航向作差值，滤除车轮MEMS测得的车身航向微分，获得纯车轮转动的速率，然后进行积分获得车轮转向角度。

由于MEMS陀螺仪存在零偏、随机漂移等噪声，测得的车轮转向角速率被噪声污染，且积分过程中MEMS陀螺仪的噪声误差会随时间累积，严重影响转向轮角测量结果的准确性，因此以农机运动学方程计算转向轮角观测值( $\alpha $ )。

$ \alpha =\arctan (\overset{\centerdot }{\mathop{\phi }}\,L/v), $

(1)

式中， ${\mathop{\phi }} $ 为车身航向微分， $v$ 为拖拉机后轮轴中心速度， $L$ 为拖拉机轴距。

以式(1)作为观测方程设计自适应卡尔曼滤波器，滤除MEMS陀螺仪的输出信号与航向差值积分过程的噪声干扰，对角度积分过程进行负反馈误差校正，获得车轮转向角的精准测量结果。

1.3 自适应卡尔曼滤波器

双天线GNSS板卡航向输出频率为10 Hz，直接对航向微分处理得到的航向角速率噪声大，不能直接作为车身航向角速率参与卡尔曼滤波器运算，因此本文设计了一种自适应卡尔曼滤波器，利用实时观测数据进行滤波的同时，通过不断改变模型参数或噪声统计特性进行实时估计和修正，实现滤波器的在线改进，缩小实际滤波误差，改善卡尔曼滤波器的动态性能，该方法能将系统辨识与滤波估计有效融为一体。

假设农机自动导航直线行驶时车身的角加速度(a)为0，且存在一个正态分布的随机噪声，在直线导航状态下可以利用该假设对航向角速率进行卡尔曼滤波，令：

$ {{\overset{\centerdot }{\mathop{\phi }}\,}_{k}}={{\overset{\centerdot }{\mathop{\phi }}\,}_{k-1}}+a\Delta t+w, $

(2)

式中， ${{\overset{\centerdot }{\mathop{\phi }}\,}_{k}}$ 为当前时刻农机车身角速率， ${{\overset{\centerdot }{\mathop{\phi }}\,}_{k-1}}$ 为上一时刻农机车身角速率， $a = 0$ ， $\Delta t $ 表示上一时刻与当前时刻的时间间隔， $w$ 为高斯白噪声。

卡尔曼滤波器的状态变量即为农机车身角速率，无控制输入，故状态转移矩阵 ${{A}} = 1$ ；包含噪声的观测值是状态变量的直接体现，故测量矩阵 ${{H}} = 1$ 。建立该卡尔曼滤波器的状态方程和观测方程分别如式(3)和(4)所示：

状态方程：

${X_k} = {{A}}{X_{k - 1}} + w,$

(3)

观测方程：

${Z_k} = {{H}}{X_k} + v,$

(4)

式中， ${X_k}$ 为k时刻农机车身角速率， ${X_{k - 1}}$ 为k−1时刻农机车身角速率， ${Z_k}$ 为k时刻双GNSS航向微分后的车身转动速率观测值， $v$ 为高斯白噪声。

根据滤波器迭代原理，建立卡尔曼滤波器时间更新方程式(5)和(6)，测量更新方程式(7)、(8)和(9)。

${\mathop x\limits^{\wedge}} _k^ - = {\mathop x\limits^{\wedge}} _{k - 1}^{},$

(5)

${P_k}^ - = {P_{k - 1}} + {{Q}},$

(6)

${K_k} = P_k^ - /{(P_k^ - + {{R}})^{}},$

(7)

${{\mathop x\limits^{\wedge}} _k} = {\mathop x\limits^{\wedge}} _k^ - + {K_k}({Z_k} - {\mathop x\limits^{\wedge}} _k^ - ),$

(8)

${P_k} = (I - {K_k})P_k^ - {\text{，}}$

(9)

式(5)~(9)中， ${\mathop x\limits^{\wedge}} _k^ - $ 为k时刻先验状态估计值， ${{\mathop x\limits^{\wedge}} _{k-1}} $ 为k−1时刻后验状态估计值， $P_k^ - $ 为k时刻先验估计误差的协方差估计值， $P_{k-1} $ 为k−1时刻后验估计误差的协方差估计值，Q为过程激励噪声协方差矩阵，K_k为卡尔曼增益，R为观测噪声协方差矩阵， ${{\mathop x\limits^{\wedge}} _k} $ 为k时刻计算的后验状态估计值， ${\mathop x\limits^{\wedge}} _k^ - $ 为k时刻先验状态估计值，Z_k为k时刻前轮转向角度的观测值，P_k为k时刻后验估计误差的协方差估计值。

由于农机曲线行驶时车身角加速度不能近似为0，卡尔曼滤波器在农机曲线行驶时模型失配。本文提出了基于农机横向跟踪偏差进行自适应调节卡尔曼滤波器的过程激励噪声协方差矩阵Q，卡尔曼滤波器判断农机为上线过程时，可通过增大Q值改变卡尔曼滤波器的权重值解决模型失配的问题。

${{Q}} = \left\{ {\begin{array}{*{20}{l}} {\dfrac{{\left| {{\rm{p}}{{\rm{e}}_{{\rm{now}}}}} \right|}}{{\left| {{\rm{p}}{{\rm{e}}_{\max }}} \right| - \left| {{\rm{p}}{{\rm{e}}_{\min }}} \right|}}({q_{\max }} - {q_{\min }}) + {q_{\min }}}&{0.1 < \left| {{\rm{p}}{{\rm{e}}_{{\rm{now}}}}} \right| < 3}\\ {{q_{\min }}}&{\left| {{\rm{p}}{{\rm{e}}_{{\rm{now}}}}} \right| < 0.1} \end{array}} \right.,$

(10)

式中， ${\rm{p}}{{\rm{e}}_{{\rm{now}}}}$ 为当前农机的横向跟踪偏差， ${\rm{p}}{{\rm{e}}_{\max }}$ 、 ${\rm{p}}{{\rm{e}}_{\min }}$ 分别为最大上线距离和最小上线距离， ${q_{\max }}$ 和 ${q_{\min }}$ 分别为自适应卡尔曼滤波器过程激励噪声协方差矩阵Q初始化的最大值和最小值。

1.4 试验方法

为验证本文设计的GNSS航向微分和MEMS陀螺仪转向轮角测量方法的准确性，分别设计了如下试验。

1.4.1 自适应卡尔曼滤波器性能验证试验

以安装于拖拉机车身的MEMS陀螺仪测量的车身角速率作为参照，对比检验双天线航向微分采用自适应卡尔曼滤波前后的效果。在华南农业大学增城基地的水泥路面自动导航驾驶拖拉机直线行驶、人工驾驶拖拉机曲线行驶，分别采集后轮轴中心速度、MEMS陀螺输出角速率、双天线航向角等原始数据进行仿真分析，对原始数据和滤波效果进行评价。自适应卡尔曼滤波器初始化参数为 $P(0) = 100$ ， ${{Q}}(0) = 0.005$ ， ${{Q}}$ 自适应公式为：

${{Q}} =\! \left\{ \!\!\!\!\begin{array}{l} \dfrac{{0.295(| {{\rm{pe}}} | - 0.1)}}{{2.9}}+ 0.005\begin{array}{*{20}{c}} {}&{0.1 < | {{\rm{pe}}} |} \end{array} \!\!\!\!\!<\! 3 \\ 0.005\begin{array}{*{20}{c}} {}&{| {{\rm{pe}}} | < 0.1} \end{array} \\ \end{array} \right.\!\!\!\!{\text{。}}\!\!\!\!\!$

(11)

${{R}} = 0.05$ ，初始化轮角值为0°，令农机后轮轴中心速度大于0.2 ${\rm{m/s }}$ 时，自适应卡尔曼滤波器工作。

1.4.2 对比试验

分别以GNSS双天线航向微分和MEMS陀螺仪测量的轮角和DUNCAN 9360型角度传感器测量的轮角作为导航系统的轮角反馈，选取拖拉机偏离目标航线1.5和2.5 m作为上线距离，拖拉机上线速度为2.88 km/h，进行对比试验。试验时，卡尔曼滤波器初始化参数与验证试验的相同，初始化轮角值为0°。试验采集MEMS陀螺仪测量轮角的观测值、卡尔曼滤波值和连杆式轮角传感器获得的角度基准值，比较上线过程卡尔曼滤波器收敛速度和轮角测量的准确性和动态响应性能。

1.4.3 田间直线导航应用试验

采用“1.4.1”所述卡尔曼滤波器初始化参数，以4 km/h行进速度进行田间直线导航试验。试验采集并统计对比前轮转向角度测量值和横向位置偏差，通过评价拖拉机导航行驶作业性能，间接考察拖拉机双天线GNSS航向微分和MEMS陀螺仪转向轮角测量性能。

2 结果与分析 2.1 自适应卡尔曼滤波器性能验证试验

双天线航向微分计算值、车身陀螺测量值和自适应卡尔曼滤波值的验证试验结果如图3所示。从图3可以看出，自适应卡尔曼滤波值基本与MEMS陀螺仪测量值吻合，在直线行驶和转弯部分均得到了较好的滤波效果。以车身MEMS陀螺仪测量值为参考，对滤波前和后的3组采样数据进行统计分析，结果如表1所示。

以车身陀螺仪测量值作为参考，直线行驶时，滤波前最大误差为5.40 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，平均绝对误差最大为0.94 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，均方根误差最大为1.21 ${{\rm{^{\circ} }} / {\rm{s}}}$ ；自适应卡尔曼滤波器滤波后最大误差为4.14 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，平均绝对误差最大为0.66 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，均方根误差最大为0.85 ${{\rm{^{\circ} }} / {\rm{s}}}$ 。3次直线行驶数据角速率平均绝对误差均值由滤波前的0.91 ${{\rm{^{\circ} }} / {\rm{s}}}$ 降低为滤波后的0.59 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，均方根误差均值由滤波前的1.16 ${{\rm{^{\circ} }} / {\rm{s}}}$ 降低为滤波后的0.76 ${{\rm{^{\circ} }} / {\rm{s}}}$ 。

曲线行驶时，滤波前最大误差为18.00 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，平均绝对误差最大为1.69 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，均方根误差最大为2.75 ${{\rm{^{\circ} }} / {\rm{s}}}$ ；自适应卡尔曼滤波器滤波后最大误差为15.69 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，平均绝对误差最大为1.45 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，均方根误差最大为2.34 ${{\rm{^{\circ} }} / {\rm{s}}}$ 。3次曲线行驶数据角速率平均误差均值由滤波前的1.57 ${{\rm{^{\circ} }} / {\rm{s}}}$ 降低为滤波后的1.38 ${{\rm{^{\circ} }} / {\rm{s}}}$ ，均方根误差均值由滤波前的2.61 ${{\rm{^{\circ} }} / {\rm{s}}}$ 降低为滤波后的2.19 ${{\rm{^{\circ} }} / {\rm{s}}}$ 。

自适应卡尔曼滤波器有效去除了直线行驶和曲线行驶时GNSS双天线航向微分计算的车身角速率值噪声，平滑了车身角速率值，降低了平均误差和均方根误差，滤波效果明显。验证试验结果表明，设计的自适应卡尔曼滤波器对车身航向角速率滤波效果良好，满足测量精度要求，可作为信息源替代车身MEMS陀螺仪的输出值参与前轮转向角度的计算。

2.2 对比试验

对比试验结果如图4所示。从图4可知，卡尔曼滤波器初始化的轮角为0°，真实角度值分别为−1.0°和6.2°，滤波值逼近真实值时间分别为1.4和1.1 s；前轮转向角度的观测值噪声大，影响滤波效果。以滤波逼近真实值时刻为开始，滤波前和滤波后角度值与连杆式角度传感器输出差值误差统计信息如表2所示，上线距离为2.5 m时，平均绝对误差由2.66°降低至1.13°，均方根误差由3.48°降低至0.90°；上线距离为1.5 m时，平均绝对误差由1.98°降低至0.87°，均方根误差由2.89°降低至0.68°。滤波后误差大幅下降，表明自适应卡尔曼滤波器滤波效果较好。

图5为采用GNSS航向微分和MEMS陀螺仪测量转向轮角导航时2.5 m 和1.5 m上线距离下拖拉机的上线时间，图6为采用角度传感器导航时2.5和1.5 m上线距离下拖拉机的上线时间。在1.5和2.5 m的上线距离和相同速度下，采用传统角度传感器时上线时间分别为15.0 s和14.2 s，采用GNSS航向微分和MEMS陀螺仪测量轮角时上线时间分别为29.4和23.5 s，较采用传统角度传感器时的上线时间长，动态响应性能稍逊。主要原因是模型失配导致角度测量误差较大，上线超调量较大，但采用的自适应卡尔曼滤波器收敛较快，且未出现发散和震荡，上线时间在可接受范围内，能满足导航作业的基本需求。

2.3 田间直线导航应用试验

图7为4 km/h速度下前轮转向角度滤波值、观测值和角度传感器测量的基准值对比试验结果，由图7可知GNSS航向微分和MEMS陀螺仪测量的轮角和角度传感器测量值的基准角度趋势一致，测量结果稳定，设计的自适应卡尔曼滤波器能很快收敛，并滤除杂波。以连杆式轮角传感器测量结果为基准，对采用卡尔曼滤波前后GNSS航向微分和MEMS陀螺仪的测量结果进行统计分析可得，滤波前平均绝对误差为1.07°，均方根误差为2.12 °；滤波后平均绝对误差为0.44 °，均方根误差为0.87 °。应用试验结果表明自适应卡尔曼滤波器效果良好，能够替代角度传感器用于农业机械导航。

图8为4 km/h速度下采用GNSS航向微分和MEMS陀螺仪测量值作为轮角反馈进行直线导航时采集的拖拉机横向位置偏差。从导航路径跟踪效果看，最大横向位置偏差为5.6 cm，平均偏差为1.8 cm，横向位置偏差均方根为2.2 cm，极差为10.5 cm，跟踪精度基本能满足拖拉机旱地作业要求。试验结果表明，GNSS航向微分和MEMS陀螺仪测量轮角方法基本可满足拖拉机自动导航作业场景，且成本相对较低，具有一定的应用前景。

3 结论

本文分析了基于GNSS航向微分和MEMS陀螺仪的农机轮角测量方法，设计了自适应卡尔曼滤波器抑制GNSS双天线航向微分测量噪声，提高了农机轮角测量的精度和稳定性。

设计了自适应卡尔曼滤波器性能验证试验，对获取的双天线GNSS航向微分和MEMS陀螺仪测量的车身角速率以及自适应卡尔曼滤波后的车身转动速率进行比较，验证试验结果表明，卡尔曼滤波器在直线行驶和曲线行驶时对双天线航向微分计算的车身角速率值的噪声进行了有效去除且平滑了车身角速率值，降低了平均绝对误差和均方根误差，滤波效果良好，满足测量精度要求。

GNSS航向微分和MEMS陀螺仪测量值和角度传感器测量值对比试验结果表明，该方法转向轮角测量精度与传统轮角传感器测量结果相当，2.5 和1.5 m上线距离均方根误差分别为0.90°和0.68°，基本满足农机自动导航轮角测量需求；动态响应性能比传统角度传感器性能稍逊，分别采用本文方法和轮角传感器进行导航，拖拉机偏离目标航线2.5 m时上线时间分别为29.4 和15.0 s，偏离目标航线1.5 m时上线时间分别为23.5 和14.2 s，较采用传统角度传感器时上线时间长。

4 km/h速度直线导航应用试验结果表明，GNSS航向微分和MEMS陀螺仪测量的轮角与角度传感器测量的轮角平均绝对误差为0.44°，导航路径跟踪横向位置偏差均方根为2.2 cm，满足拖拉机旱地作业要求。

3组试验结果表明，本文提出的GNSS航向微分和MEMS陀螺仪测量方法性能与连杆式轮角传感器相当，且成本相对较低，能够替代角度传感器用于农业机械较低速导航，具有一定的应用前景。

由于转向轮上MEMS陀螺仪存在零偏未校正，GNSS航向微分和MEMS陀螺仪测量的轮角值存在初始化角度值不准确的缺陷，后续研究应进行MEMS陀螺仪零偏校正。由于GNSS板卡更新频率低，在高速导航作业时GNSS航向微分值噪声较大，前轮转向角度测量系统稳定性会变差，农机高速导航作业时轮角测量精度和稳定性有待验证。