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

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

【技术实现步骤摘要】
【专利摘要】本专利技术公开了一种基于GPS的农业机械导航的多传感器信息融合方法，包括采用导航传感器组合测量得到农业机械在WGS-84坐标系下的原始纬度、经度、高度坐标和前进速度，并测量得到农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度；将WGS-84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值；将横摆角速度经积分后转换为航向角度测量值；采用改进的卡尔曼滤波器对获得的横滚角度、俯仰角度、前进速度、前进方向加速度、初步定位位置值和航向角度测量值进行融合处理，获得农业机械定位和航向角度信息的精确估计。本专利技术方法可形成更连续和稳定的农业机械导航数据，使得航向角度的估计更加精确。【专利说明】基于GPS的农业机械导航的多传感器信息融合方法
本专利技术涉及一种多传感器信息融合方法，尤其是一种基于GPS的农业机械导航的多传感器信息融合方法，属于农业机械的GPS导航定位
。
技术介绍
随着GPS、北斗等卫星导航定位技术、电子技术、机器视觉技术、现代传感测试技术和自动控制技术的不断发展、顺应现代农业生产的发展趋势，基于GPS的农业机械导航技术与装备迅速发展，成为现代智能农业机械与装备的重要组成部分，对现代农业建设提供了良好的技术支持。农业机械自动导航技术在欧美日等发达国际已经实现产业化，产生了良好的经济效益和社会效益。基于GPS的农业机械自动导航技术产品可有效减轻驾驶员的工作负担，避免叠行或者漏行作业，提高作业质量和土地利用率，显著提升农业机械的自动化水平，对我国现代农业的建设将起到有力的推动作用。基于GPS的农业机械自动导航涉及的主要导航信息包括:定位信息和航向角度信息。已有研究成果主要采用RTK-GPS接收机和惯性测量单元MU获取上述导航信息，其成本较高。农田表面高度变化大，从而影响GPS天线的定位精度，需要进行倾斜校正；农业机械航向角度信息无法直接精确测量，需要采用最小二乘方法或者卡尔曼滤波方法估计获得。已有研究成果对倾斜校正和卡尔曼滤波都提出了不同解决方法。但存在的问题是:倾斜校正和卡尔曼滤波是各自独立处理的，利用卡尔曼滤波器对航向角度的估计结果对GPS天线进行倾斜校正，容易导致卡尔曼滤波发散。
技术实现思路
本专利技术的目的，是为了解决上述现有技术的缺陷，提供一种低成本、高精度的基于GPS的农业机械导航的多传感器信息融合方法，该方法将倾斜校正整合进卡尔曼滤波器中，得到农业机械更加精确和稳定的实时位姿信息。本专利技术的目的可以通过采取如下技术方案达到:基于GPS的农业机械导航的多传感器信息融合方法，其特征在于所述方法包括以下步骤:I)采用导航传感器组合测量得到农业机械在WGS-84坐标系下的原始纬度、经度、高度坐标和前进速度，并测量得到农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度；2)将WGS-84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值，将横摆角速度经积分后转换为航向角度测量值；3)采用改进的卡尔曼滤波器对步骤I)获得的横滚角度、俯仰角度、前进速度和前进方向加速度，以及对步骤2)获得的初步定位位置值和航向角度测量值进行融合处理，获得农业机械定位和航向角度信息的精确估计。作为一种实施方案，步骤I)所述导航传感器组合包括OEM GPS板卡和微型姿态航向参考系统，其中农业机械在WGS-84坐标系下的原始纬度、经度、高度坐标和前进速度采用OEM GPS板卡测量，农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度采用微型姿态航向参考系统测量。作为一种实施方案，步骤2)所述将WGS-84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值采用高斯投影变换或切平面法实现。作为一种实施方案，步骤3)所述卡尔曼滤波器的模型中，状态向量为农业机械导航控制点位置的平面坐标(Xyy。)、速度偏差ε v、加速度偏差ε a和航向角偏差ε ψ ;测量向量为GPS天线位置的平面坐标(xg，yg)。作为一种实施方案，步骤3)所述卡尔曼滤波器的模型中，状态转移方程是基于位置、速度和加速度的航位推算原理构建的；测量方程是基于车体坐标系和大地导航坐标系的坐标转换公式构建的。作为一种实施方案，步骤3)所述卡尔曼滤波器的模型中，状态转移矩阵和测量矩阵根据步骤I)获得的农业机械的原始横滚角度、俯仰角度、前进速度、前进方向加速度信息和由横摆角速度累积得到的航向角度，进行实时更新；测量噪声方差矩阵根据OEM GPS板卡的定位质量标识gps_flag自适应调节。作为一种实施方案，所述方法还包括:若步骤2)获得的航向角度测量值与步骤3)经过卡尔曼滤波后的航向角度精确估计值的差值超过设定阈值，则采用卡尔曼滤波后的航向角度精确估计值更新横摆角速率的积分初值，以防止积分发散，同时重新初始化卡尔曼滤波器。本专利技术相对于现有技术具有如下的有益效果:1、本专利技术基于GPS的农业机械导航的多传感器信息融合方法将车体坐标和大地导航坐标的转换公式整合进卡尔曼滤波器中，使卡尔曼滤波器得到改进，既有利于为农业机械导航控制提供更为精确的航向角数据，也有利于增强卡尔曼滤波器的稳定性和适用性。2、本专利技术基于GPS的农业机械导航的多传感器信息融合方法采用的改进卡尔曼滤波器可形成更连续和稳定的农业机械导航数据，使得航向角度的估计更加精确。3、本专利技术基于GPS的农业机械导航的多传感器信息融合方法采用的改进卡尔曼滤波器，有效整合了多导航传感器信息融合的处理，有利于软件的模块化编程。【专利附图】【附图说明】图1为本专利技术实施例1的多传感器信息融合方法的流程示意图。图2为本专利技术实施例1的卡尔曼滤波器直线跟踪轨迹仿真结果I。图3为本专利技术实施例1的卡尔曼滤波器直线跟踪航向角度仿真结果I。图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，其软件程序采用M本文档来自技高网...

【技术保护点】
基于GPS的农业机械导航的多传感器信息融合方法，其特征在于所述方法包括以下步骤：1)采用导航传感器组合测量得到农业机械在WGS‑84坐标系下的原始纬度、经度、高度坐标和前进速度，并测量得到农业机械的原始横滚角度、俯仰角度、横摆角速度和前进方向加速度；2)将WGS‑84坐标系下的原始纬度、经度和高度坐标转换到NEU大地导航坐标系下得到初步导航定位位置值，将横摆角速度经积分后转换为航向角度测量值；3)采用改进的卡尔曼滤波器对步骤1)获得的横滚角度、俯仰角度、前进速度和前进方向加速度，以及对步骤2)获得的初步定位位置值和航向角度测量值进行融合处理，获得农业机械定位和航向角度信息的精确估计。

【技术特征摘要】

【专利技术属性】
技术研发人员：张智刚，罗锡文，胡炼，单鹏辉，张亚娇，
申请(专利权)人：华南农业大学，
类型：发明
国别省市：广东;44