CN103885076B - 基于gps的农业机械导航的多传感器信息融合方法 - Google Patents

Abstract

本发明公开了一种基于GPS的农业机械导航的多传感器信息融合方法，包括采用导航传感器组合测量得到农业机械在WGS‑84坐标系下的原始纬度、经度、高度坐标和前进速度，并测量得到农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度；将WGS‑84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值；将横摆角速度经积分后转换为航向角度测量值；采用改进的卡尔曼滤波器对获得的横滚角度、俯仰角度、前进速度、前进方向加速度、初步定位位置值和航向角度测量值进行融合处理，获得农业机械定位和航向角度信息的精确估计。本发明方法可形成更连续和稳定的农业机械导航数据，使得航向角度的估计更加精确。

Description

基于GPS的农业机械导航的多传感器信息融合方法

技术领域

本发明涉及一种多传感器信息融合方法，尤其是一种基于GPS的农业机械导航的多传感器信息融合方法，属于农业机械的GPS导航定位技术领域。

背景技术

随着GPS、北斗等卫星导航定位技术、电子技术、机器视觉技术、现代传感测试技术和自动控制技术的不断发展、顺应现代农业生产的发展趋势，基于GPS的农业机械导航技术与装备迅速发展，成为现代智能农业机械与装备的重要组成部分，对现代农业建设提供了良好的技术支持。农业机械自动导航技术在欧美日等发达国际已经实现产业化，产生了良好的经济效益和社会效益。基于GPS的农业机械自动导航技术产品可有效减轻驾驶员的工作负担，避免叠行或者漏行作业，提高作业质量和土地利用率，显著提升农业机械的自动化水平，对我国现代农业的建设将起到有力的推动作用。

基于GPS的农业机械自动导航涉及的主要导航信息包括：定位信息和航向角度信息。已有研究成果主要采用RTK-GPS接收机和惯性测量单元IMU获取上述导航信息，其成本较高。农田表面高度变化大，从而影响GPS天线的定位精度，需要进行倾斜校正；农业机械航向角度信息无法直接精确测量，需要采用最小二乘方法或者卡尔曼滤波方法估计获得。已有研究成果对倾斜校正和卡尔曼滤波都提出了不同解决方法。但存在的问题是：倾斜校正和卡尔曼滤波是各自独立处理的，利用卡尔曼滤波器对航向角度的估计结果对GPS天线进行倾斜校正，容易导致卡尔曼滤波发散。

发明内容

本发明的目的，是为了解决上述现有技术的缺陷，提供一种低成本、高精度的基于GPS的农业机械导航的多传感器信息融合方法，该方法将倾斜校正整合进卡尔曼滤波器中，得到农业机械更加精确和稳定的实时位姿信息。

本发明的目的可以通过采取如下技术方案达到：

基于GPS的农业机械导航的多传感器信息融合方法，其特征在于所述方法包括以下步骤：

1)采用导航传感器组合测量得到农业机械在WGS-84坐标系下的原始纬度、经度、高度坐标和前进速度，并测量得到农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度；

2)将WGS-84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值，将横摆角速度经积分后转换为航向角度测量值；

3)采用改进的卡尔曼滤波器对步骤1)获得的横滚角度、俯仰角度、前进速度和前进方向加速度，以及对步骤2)获得的初步定位位置值和航向角度测量值进行融合处理，获得农业机械定位和航向角度信息的精确估计。

作为一种实施方案，步骤1)所述导航传感器组合包括OEM GPS板卡和微型姿态航向参考系统，其中农业机械在WGS-84坐标系下的原始纬度、经度、高度坐标和前进速度采用OEM GPS板卡测量，农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度采用微型姿态航向参考系统测量。

作为一种实施方案，步骤2)所述将WGS-84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值采用高斯投影变换或切平面法实现。

作为一种实施方案，步骤3)所述卡尔曼滤波器的模型中，状态向量为农业机械导航控制点位置的平面坐标(x_c，y_c)、速度偏差ε_v、加速度偏差ε_a和航向角偏差ε_ψ；测量向量为GPS天线位置的平面坐标(x_g，y_g)。

作为一种实施方案，步骤3)所述卡尔曼滤波器的模型中，状态转移方程是基于位置、速度和加速度的航位推算原理构建的；测量方程是基于车体坐标系和大地导航坐标系的坐标转换公式构建的。

作为一种实施方案，步骤3)所述卡尔曼滤波器的模型中，状态转移矩阵和测量矩阵根据步骤1)获得的农业机械的原始横滚角度、俯仰角度、前进速度、前进方向加速度信息和由横摆角速度累积得到的航向角度，进行实时更新；测量噪声方差矩阵根据OEM GPS板卡的定位质量标识gps_flag自适应调节。

作为一种实施方案，所述方法还包括：

若步骤2)获得的航向角度测量值与步骤3)经过卡尔曼滤波后的航向角度精确估计值的差值超过设定阈值，则采用卡尔曼滤波后的航向角度精确估计值更新横摆角速率的积分初值，以防止积分发散，同时重新初始化卡尔曼滤波器。

本发明相对于现有技术具有如下的有益效果：

1、本发明基于GPS的农业机械导航的多传感器信息融合方法将车体坐标和大地导航坐标的转换公式整合进卡尔曼滤波器中，使卡尔曼滤波器得到改进，既有利于为农业机械导航控制提供更为精确的航向角数据，也有利于增强卡尔曼滤波器的稳定性和适用性。

2、本发明基于GPS的农业机械导航的多传感器信息融合方法采用的改进卡尔曼滤波器可形成更连续和稳定的农业机械导航数据，使得航向角度的估计更加精确。

3、本发明基于GPS的农业机械导航的多传感器信息融合方法采用的改进卡尔曼滤波器，有效整合了多导航传感器信息融合的处理，有利于软件的模块化编程。

附图说明

图1为本发明实施例1的多传感器信息融合方法的流程示意图。

图2为本发明实施例1的卡尔曼滤波器直线跟踪轨迹仿真结果1。

图3为本发明实施例1的卡尔曼滤波器直线跟踪航向角度仿真结果1。

图4为本发明实施例1的卡尔曼滤波器直线跟踪轨迹仿真结果2。

图5为本发明实施例1的卡尔曼滤波器直线跟踪航向角度仿真结果2。

图6为本发明实施例1的卡尔曼滤波器直线跟踪轨迹仿真结果3。

图7为本发明实施例1的卡尔曼滤波器直线跟踪航向角度仿真结果3。

图8为本发明实施例1的卡尔曼滤波器地头转向曲线跟踪轨迹仿真结果。

图9为本发明实施例1的卡尔曼滤波器地头转向曲线跟踪航向角度仿真结果。

具体实施方式

实施例1：

本实施例采用的农业机械为雷沃M904-D轮式拖拉机，其导航系统主要包括：导航传感器组合、嵌入式开发板和显示终端；其中，导航传感器组合包括OEM RTK GPS板卡(Trimble BD970)和微型姿态航向参考系统AHRS(XSENS MTi)，GPS天线安装于拖拉机驾驶室顶部，处于拖拉机的中轴线上，AHRS的传感器坐标系三轴指向与拖拉机车体坐标系三轴指向相同，安装在拖拉机座椅下方；嵌入式开发板与导航传感器组合通过RS232通讯接口连接，用于实现数据采集、数据预处理和信息融合等软件程序的运行；导航传感器组合的数据采集频率为10Hz，数据采集、数据预处理和信息融合软件程序采用ADS1.2编程工具在Windows XP操作系统下开发完成，嵌入式开发板将融合处理后的结果通过RS232通讯接口发送给显示终端实时显示并保存；显示终端的操作系统为WinCE6.0，其软件程序采用Microsoft Visual Studio2005集成开发工具在Windows XP操作系统下开发完成。

如图1所示，本实施例的多传感器信息融合方法，包括以下步骤：

1)采用导航传感器组合测量得到拖拉机在WGS-84坐标系下的原始纬度、经度、高度坐标和前进速度，并测量得到拖拉机的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度；

2)将WGS-84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值，可以采用高斯投影变换或切平面法实现，在本实施例采用高斯投影变换；将横摆角速度经积分后转换为航向角度测量值；

3)采用改进的卡尔曼滤波器对步骤1)获得的横滚角度、俯仰角度、前进速度和前进方向加速度，以及对步骤2)获得的初步定位位置值和航向角度测量值进行融合处理，获得拖拉机定位和航向角度信息的精确估计；

所述卡尔曼滤波器的模型中，状态向量为拖拉机导航控制点位置的平面坐标(x_c，y_c)、速度偏差ε_v、加速度偏差ε_a和航向角偏差ε_ψ；测量向量为GPS天线位置的平面坐标(x_g，y_g)；状态转移方程是基于位置、速度和加速度的航位推算原理构建的；测量方程是基于车体坐标系和大地导航坐标系的坐标转换公式构建的；状态转移矩阵和测量矩阵根据步骤1)获得的拖拉机的原始横滚角度、俯仰角度、前进速度、前进方向加速度信息和由横摆角速度累积得到的航向角度，进行实时更新；测量噪声方差矩阵根据OEM GPS板卡的定位质量标识gps_flag自适应调节；

4)若步骤2)获得的航向角度测量值与步骤3)经过卡尔曼滤波后的航向角度精确估计值的差值超过设定阈值，则采用卡尔曼滤波后的航向角度精确估计值更新横摆角速率的积分初值，以防止积分发散，同时重新初始化卡尔曼滤波器。

具体信息融合方法的处理步骤和公式推导如下：

1、OEM RTK GPS板卡数据采集

BD970GPS板卡通过TTL串口以20Hz的输出频率输出NMEA-0183格式版本的格式数据，利用自主研发的嵌入式数据采集程序从GPGGA数据报中提取拖拉机GPS天线的定位数据，包括纬度、经度和高度；从GPVTG数据报中提取拖拉机前进速度信息。

2、微型姿态航向参考系统数据采集

MTi通过RS232串口以50Hz的输出频率输出固定长度的数据，包括3轴MEMS加速度计标定数据、3轴MEMS陀螺仪标定数据和内置Kalman滤波后的3轴姿态角数据，利用自主研发的嵌入式数据采集程序从MTi数据报中提取拖拉机的横摆角速度、前进方向加速度、横滚角度和俯仰角度数据。

3、GPS定位数据的坐标转换

3.1、WGS-84大地坐标向大地导航坐标系的转换

为了使GPS板卡输出的WGS-84大地坐标定位数据能用于拖拉机的导航控制系统，需要将WGS-84地心大地坐标转换为对应于WGS-84椭球的高斯平面坐标，即需进行高斯投影变换；

采用如下公式：

$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\mathrm{\η}}^2+{4\mathrm{\η}}^4)+\frac{l^6}{720}N\sin B\cos 5B(61-{58t}^2+t^4)$

$y=\mathrm{lN}\cos B+\frac{l^3}{6}N{\cos }^{3}B(1-t^2+{\mathrm{\η}}^2)+\frac{l^5}{120}N{\cos }^{5}B(5-{18t}^2+t^4+{14\mathrm{\η}}^2-{58\mathrm{\η}}^2{t}^2)+500000$

式中，l=L-L₀，L₀为投影带中央子午线经度；卯酉圈曲率半径椭球第一偏心率辅助变量t＝tan B，辅助变量η=e′cos B，椭球第二偏心率a、b分别为参考椭球的长半径、短半径；扁率

X为赤道至纬度为B的平行圈的子午线弧长，其计算公式为：

$X=c{\∫}_0^B(1+e^{\′}2\cos 2B-2/3)\mathrm{dB},$ c为极曲率半径。

本实施例所用投影坐标系的主要参数为：

a、投影方式：Gauss-Kruger

b、中央经线：114.000000(3度带)

c、水平偏移量：500km

d、地理坐标系：GCS_WGS_1984

e、大地参照系：D_WGS_1984

f、参考椭球体：WGS-84

g、椭球长轴：6378137.000000

h、椭球扁率：0.0033528107

3.2、大地导航坐标系下GPS天线位置点向导航控制点的转换

本实施例所使用的大地导航坐标系由WGS-84椭球对应的高斯-克吕格投影坐标平面加上WGS-84地心坐标系中的大地高程构成；所使用的车体坐标系原点位于拖拉机重心位置，三轴指向与MTi传感器坐标系三轴指向相同。导航控制点选择在后轮着地点的连线中点处，主要是考虑到：1)有利于GPS天线倾斜误差的校正；2)有利于GPS天线至导航控制点距离的量取；3)有利于拖拉机自动导航过程中后挂作业机具的位置控制；4)有利于建立更为严密的拖拉机航位推算递推关系式。

假设AHRS测得的拖拉机三轴姿态角横滚角度、俯仰角度和航向角度分别为：φ，θ，ψ，则拖拉机车体坐标系下的空间矢量向大地导航坐标系转换的转换矩阵R_b2n为：

$\begin{array}{c}{R}_{b2n}={R_{\mathrm{\ψ}}}^Z{R_{\mathrm{\θ}}}^Y{R_{\mathrm{\φ}}}^X\\ =\left[\begin{array}{ccc}\cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\θ}& 0& \sin \mathrm{\θ}\\ 0& 1& 0\\ -\sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& -\sin \mathrm{\φ}\\ 0& \sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right]\\ =\begin{array}{}\end{array}\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\\ -\sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& -\sin \mathrm{\φ}\\ 0& \sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right]\\ =\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\cos \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\cos \mathrm{\ψ}\sin \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array}\right]\end{array}$

假设导航控制点到GPS天线的空间矢量在车体坐标系下的表达形式为[a b h]^T，GPS天线在大地导航坐标系下的坐标为[x_g y_g z_g]^T，导航控制点在大地导航坐标系下的坐标为[x_c y_c z_c]^T，则有：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\left[\begin{array}{c}{x}_g\\ y_g\\ z_g\end{array}\right]-R_{b2n}\left[\begin{array}{c}a\\ b\\ h\end{array}\right]---\left(1\right)$

4、卡尔曼滤波器的推导及工作流程

设定k时刻拖拉机本体的真实航向角度为ψ_k′，拖拉机本体纵向方向上的真实前进速度是v_k′，拖拉机本体纵向方向上的真实加速度是a_k′，则

ψ_k′＝ψ_k+ε_ψk+ξ_ψk (2)

v_k′=v_k+ε_vk+ξ_vk (3)

a_k′=a_k+ε_ak+ξ_ak (4)

其中，ψ_k、v_k、a_k分别指航向角度、前进速度和加速度的测量值，ε_ψk、ε_vk、ε_ak分别指航向角度、前进速度和加速度的测量偏差值，ξ_ψk、ξ_vk、ξ_ak分别指航向角度、前进速度和加速度的随机测量误差。

基于牛顿第二定律，建立导航控制点在2D平面坐标系下的运动方程：

x_ck=x_ck-1+v_k′cosθ_k′dt+a_k′cosθ_k′dt²／2.0

y_ck=y_ck-1+v_k′sinθ_k′dt+a_k′sinθ_k′dt²／2.0

将(2)、(3)和(4)式代入上述表达式，得到

x_ck=x_ck-1+v_kcosψ_kdt+ε_vkcosψ_kdt-v_kε_ψksinψ_kdt+a_kcosψ_kdt²／2.0+ε_akcosψ_kdt²／2.0-a_kε_ψksinψ_kdt²／2.0

y_ck=y_ck-1+v_ksinψ_kdt+ε_vksinψ_kdt+v_kε_ψkcosψ_kdt+a_ksinψ_kdt²／2.0+ε_aksinψ_kdt²／2.0+a_kε_ψkcosψ_kdt²／2.0

将上述等式以卡尔曼滤波器状态转移方程的形式表示为：

X_k=A_kX_k-1+b_k+u_k

其中，X_k=[x_ck，y_ck,ε_vk，ε_ak,ε_ψk]，表示状态空间向量；

$A_k=\left[\begin{array}{ccccc}1& 0& \cos {\mathrm{\ψ}}_k\mathrm{dt}& a_k\cos {\mathrm{\ψ}}_k{\mathrm{dt}}^2/2.0& -(v_k\sin {\mathrm{\ψ}}_k\mathrm{dt}+a_k\sin {\mathrm{\ψ}}_k{\mathrm{dt}}^2/2.0)\\ 0& 1& \sin {\mathrm{\ψ}}_k\mathrm{dt}& a_k\sin {\mathrm{\ψ}}_k{\mathrm{dt}}^2/2.0& v_k\cos {\mathrm{\ψ}}_k\mathrm{dt}+a_k\cos {\mathrm{\ψ}}_k{\mathrm{dt}}^2/2.0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\ϵ}}_{\mathrm{vk}}\\ {\mathrm{\ϵ}}_{\mathrm{ak}}\\ {\mathrm{\ϵ}}_{\mathrm{\ψk}}\end{array}\right],$ 是状态转移矩阵，由航向角度、前进速度和加速度的测量值实时更新；

$b_k=\left[\begin{array}{c}{v}_k\cos {\mathrm{\ψ}}_k\mathrm{dt}+a_k\cos {\mathrm{\ψ}}_k{\mathrm{dt}}^2/2.0\\ v_k\sin {\mathrm{\ψ}}_k\mathrm{dt}+a_k\sin {\mathrm{\ψ}}_2{\mathrm{dt}}^2/2.0\\ 0\\ 0\\ 0\end{array}\right],$ u_k=[0，0，ξ_vk，ξ_ak，ξ_ψk]，是状态转移方程的白噪声序列；系统过程噪声协方差矩阵为Q_k，用于表示状态转移方程的误差大小，本发明中Q_k设定为常数矩阵，在仿真和实验过程中整定矩阵参数。

以GPS天线在大地导航坐标系下的定位坐标作为观测向量，结合坐标转换公式(1)，得到卡尔曼滤波器的测量方程如下：

Z_gk=H_kX_k+d_k+V_k

其中， $Z_{\mathrm{gk}}=\left[\begin{array}{c}{x}_{\mathrm{gk}}\\ y_{\mathrm{gk}}\end{array}\right],H_k=\left[\begin{array}{ccccc}1& 0& 0& 0& {\mathrm{\γ}}_{\mathrm{xk}}\\ 0& 1& 0& 0& {\mathrm{\γ}}_{\mathrm{yk}}\end{array}\right],d_k=\left[\begin{array}{c}{\mathrm{\η}}_{\mathrm{xk}}\\ {\mathrm{\η}}_{\mathrm{yk}}\end{array}\right],v_k=\left[\begin{array}{c}{\mathrm{\ξ}}_{\mathrm{gxk}}\\ {\mathrm{\ξ}}_{\mathrm{gyk}}\end{array}\right]$

ξ_gxk，ξ_gyk表示OEM GPS板卡定位在水平面坐标系下的随机定位误差。

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

$R_k=\left[\begin{array}{cc}{r_{\mathrm{xk}}}^2& 0\\ 0& {r_{\mathrm{yk}}}^2\end{array}\right]$

其中，分别为ξ_gxk，ξ_gyk的方差统计值。

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

预测方程组为：

${{\hat{x}}_k}^{-}=A_k{\hat{x}}_k+b_k$

P_k ^-=A_kP_k-1A_k ^T+Q_k-1

校正方程组为：

K_k=P_k ^-H_k ^T(H_kP_k ^-H_k ^T+R_k)^-1

${\hat{x}}_k={{\hat{x}}_k}^{-}+K_k(Z_{\mathrm{gk}}-H_k{{\hat{x}}_k}^{-}-d_k)$

P_k=(I-K_kH_k)P_k ^-

在本实施例中，使用在线自适应调节后的测量噪声方搀R_k，计算校正方程组的增益矩阵，具体自适应调节方法如下：

gps_flag	1	2	4	5
					rx 2	9.0	1.0	0.0004	0.25
ry 2	9.0	1.0	0.0004	0.25

上表中，gps_flag是OEM GPS板卡的定位质量标识，1表示单机定位，2表示伪距差分定位，4表示RTK定位，5表示RTK浮动定位。不同的质量标识对应不同的定位误差经验值。

由于利用横摆角速度积分得到的航向角存在累积误差，需要周期性地初始化。航向角初始化时，卡尔曼滤波器需要一同初始化。

设定时，同时，利用当前状态向量估计值重新初始化卡尔曼滤波器。

卡尔曼滤波器工作的流程为：将初始状态估计X₀和初始滤波误差方差阵P₀估计引入卡尔曼滤波器的预测方程组中，启动卡尔曼滤波。利用当前横滚角、俯仰角和积分横摆角速度得到的航向角度，实时更新卡尔曼滤波器的状态转移矩阵，进行状态向量预测。预测过程结束，启动卡尔曼滤波的校正过程，先对矩阵进行自适应调节，然后引入校正方程组，解算增益矩阵，并根据当前测量结果(针对预测方程组对下一时刻状态的估计结果)，对预测方程组的状态估计进行更新，获得当前最优估计，最后解算当前最优估计的误差协方差矩阵，完成一次滤波过程。

5、对上述信息融合处理效果进行仿真测试

仿真测试数据源来自前期自主研发的GPS自动导航系统的测试数据，田间作业内容为棉花播种，截取三段直线导航和一段人工地头转向数据做仿真用。

设定卡尔曼滤波器的初始条件是：

$P_0=\left[\begin{array}{ccccc}1.0& & & & \\ & 1.0& & & \\ & & 1.0& & \\ & & & 1.0& \\ & & & & 1.0\end{array}\right],R_0=\left[\begin{array}{cc}0.01& \\ & 0.01\end{array}\right],Q_0=\left[\begin{array}{ccccc}0.01& & & & \\ & 0.01& & & \\ & & 0.01& & \\ & & & 0.01& \\ & & & & 0.01\end{array}\right]$

利用卡尔曼滤波器仿真程序读取的第一行文本文件数据对状态空间向量进行初始化。

对于直线跟踪，得到如图2、3、4、5和7所示的仿真结果；对于人工地头转向的曲线跟踪，得到如图8和9所示的仿真结果，所有仿真数据中，gps_flag=4。

从图2、4和6可看出，在直线跟踪过程中，由于地形起伏变化，原始GPS定位点轨迹并非直线，而卡尔曼滤波器处理后获得的导航控制点轨迹呈近似直线状态，表明GPS自动导航系统直线跟踪控制的效果很好，符合实际情况；从图3、5和7可看出，在航向角度的跟踪过程中，陀螺仪累积航向角大约每隔40秒即发散超过5度，但卡尔曼滤波器处理后的航向角则不会发散，反映出GPS自动导航系统直线跟踪控制过程中拖拉机航向角的正常摇摆变化，幅度不超过10度。

从图8和9可看出，在人工地头转向过程中，原始GPS定位点轨迹与卡尔曼滤波器处理后获得的导航控制点轨迹并不重合，这主要是因为在转向过程中，拖拉机上GPS天线位置点和后轮轴中心点的转弯半径有差异，GPS天线位置点的轨迹半径会略大于后轮轴中心点轨迹半径，符合实际情况。

另外，在拖拉机倒车运动状态下，该卡尔曼滤波器仍可精确估计出其运动轨迹和方向，表现出较强的适应性。

以上所述，仅为本发明专利较佳的实施例，但本发明专利的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明专利所公开的范围内，根据本发明专利的技术方案及其发明构思加以等同替换或改变，都属于本发明专利的保护范围。

Claims (4)

1.基于GPS的农业机械导航的多传感器信息融合方法，其特征在于所述方法包括以下步骤：

1)采用导航传感器组合测量得到农业机械在WGS-84坐标系下的原始纬度、经度、高度坐标和前进速度，并测量得到农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度；

所述导航传感器组合包括OEM GPS板卡和微型姿态航向参考系统，其中农业机械在WGS-84坐标系下的原始纬度、经度、高度坐标和前进速度采用OEM GPS板卡测量，农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度采用微型姿态航向参考系统测量；

2)将WGS-84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值，将横摆角速度经积分后转换为航向角度测量值；

3)采用改进的卡尔曼滤波器对步骤1)获得的横滚角度、俯仰角度、前进速度和前进方向加速度，以及对步骤2)获得的初步定位位置值和航向角度测量值进行融合处理，获得农业机械定位和航向角度信息的精确估计；

所述卡尔曼滤波器的模型中，状态向量为农业机械导航控制点位置的平面坐标(x_c，y_c)、速度偏差ε_v、加速度偏差ε_a和航向角偏差ε_ψ；测量向量为GPS天线位置的平面坐标(x_g，y_g)；状态转移方程是基于位置、速度和加速度的航位推算原理构建的；测量方程是基于车体坐标系和大地导航坐标系的坐标转换公式构建的。

2.根据权利要求1所述的基于GPS的农业机械导航的多传感器信息融合方法，其特征在于：步骤2)所述将WGS-84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值采用高斯投影变换或切平面法实现。

3.根据权利要求1所述的基于GPS的农业机械导航的多传感器信息融合方法，其特征在于：步骤3)所述卡尔曼滤波器的模型中，状态转移矩阵和测量矩阵根据步骤1)获得的农业机械的原始横滚角度、俯仰角度、前进速度、前进方向加速度信息和由横摆角速度累积得到的航向角度，进行实时更新；测量噪声方差矩阵根据OEM GPS板卡的定位质量标识gps_flag自适应调节。

4.根据权利要求1所述的基于GPS的农业机械导航的多传感器信息融合方法，其特征在于所述方法还包括：

若步骤2)获得的航向角度测量值与步骤3)经过卡尔曼滤波后的航向角度精确估计值的差值超过设定阈值，则采用卡尔曼滤波后的航向角度精确估计值更新横摆角速率的积分初值，以防止积分发散，同时重新初始化卡尔曼滤波器。