一种农业机械导航的组合定位方法技术

本发明专利技术公开了一种农业机械导航的组合定位方法。该方法首先采用伪距差分ＧＰＳ传感器测量得到农业机械的初步定位位置值；然后采用电子罗盘和微机械陀螺传感器对农业机械的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值；再采用自适应卡尔曼滤波器对包括初步定位位置值和航向角度值的多源信息进行再次融合，获得农业机械定位和航向角度信息的精确估计。本发明专利技术可为导航控制提供更为精确和可靠的航向角度估计数据，定位精度可控制在亚米级范围内，而且可以形成连续、稳定的农业机械导航数据。

【技术实现步骤摘要】

本专利技术涉及导航定位技术，特别涉及。
技术介绍
精细农业包括定位处方农作和农情信息自动采集，其中定位处方农作要求农业机械可以按照预先规划好的路径在田间行走，准确到达目的地并完成既定作业任务。精确导航是实现农业机械自主行走的关键技术之一，其定位精度直接影响农业机械进行路径自动跟踪的质量。因此，提高导航定位的精度，是改善农业机械路径跟踪质量的首要问题。农业机械导航定位的主要信息包括位置信息和航向角度信息。在以GPS为主的导航定位技术方面，已有研究成果主要采用RTK-DGPS和FOG实现农业机械的精确定位，其成本较高。也有一些研究成果采用低价格低精度GPS融合高精度惯性传感器，通过卡尔曼滤波方法实现农业机械的精确定位；目前这类研究还较少，而且现有的研究忽视卡尔曼滤波的自适应问题，难以避免滤波发散问题。
技术实现思路
本专利技术的目的在于克服现有技术中存在的缺点，提供一种高精度的农业机械导航的组合定位方法，将多个导航传感器信息经过融合处理后，得到农业机械更为精确的当前位姿信息。本专利技术，通过下述步骤实现(1)首先采用伪距差分GPS传感器测量得到农业机械的初步定位位置值；(2)然后采用电子罗盘和微机械陀螺传感器对农业机械的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值；(3)再采用自适应卡尔曼滤波器对包括初步定位位置值和航向角度值的多源信息进行再次融合，获得农业机械定位和航向角度信息的精确估计。步骤2中，所述推算得到最优的航向角度值的具体步骤如下首先电子罗盘传感器对农业机械的航向角度进行测量，微机械陀螺传感器对农业机械的横向角速率进行测量，经积分后转换为航向角度测量值；然后采用基于协方差函数的加窗估计算法，在线估计电子罗盘和微机械陀螺的测量方差；再根据上述在线估计的测量方差，采用自适应加权融合估计算法以自适应的方式推算出两个传感器所对应的最优加权因子，最后推算得到最优的航向角度值。步骤2中，采用最优估计值周期性地更新微机械陀螺传感器横向角速率的积分初值，以防止积分发散。步骤3中，所述对多源信息进行再次融合的具体步骤如下首先利用航位推算的基本原理建立常速度条件下的卡尔曼滤波器的状态方程和观测方程；然后基于新息的理论协方差阵概念，设计一种测量噪声协方差矩阵R(k)的自适应调节方法；最后，在卡尔曼滤波器状态方程和观测方程的基础上，结合测量噪声协方差矩阵R(k)的在线调节方法，建立预测方程组和校正方程组，构建卡尔曼滤波器，实现农业机械定位和航向角度信息的精确估计。步骤3中，所述的常速度卡尔曼滤波器模型中，状态空间和测量向量均为GPS接收机的位置数据(x，y)和速度数据v，航向角度估计数据对状态转移矩阵进行实时更新。步骤3中，所述R矩阵的自适应调节中，主要采用新息的实际协方差阵Pr(k)与理论协方差矩阵Cr(k)进行对比的方法，在线决策R矩阵的调节量。所述伪距差分GPS传感器是指差分全球定位系统(DGPS)接收机。所述电子罗盘是指磁航向传感器电子罗盘。所述微机械陀螺传感器是指基于MEMS技术的低成本微机械陀螺。本专利技术与现有技术相比具有如下优点和效果(1)本专利技术提出了农业机械航向角度的自适应加权融合估计算法，通过对电子罗盘和微机械陀螺信息的自适应加权融合处理，可为导航控制提供更为精确和可靠的航向角度估计数据。(2)本专利技术采用自适应卡尔曼滤波器可抑制发散，平滑DGPS定位数据，有效避免DGPS动态定位的异常结果，定位精度可控制在亚米级范围内。(3)本专利技术的导航组合定位方式，将各个传感器的测量数据进行多次融合，既平滑又可以滤除测试噪声，形成了连续、稳定的农业机械导航数据。附图说明图1为农业机械导航的组合定位系统组成示意图。图2为卡尔曼滤波器的数据流向图。图3为完全DGPS条件下Kalman滤波器直线跟踪仿真结果。图4为由DGPS到GPS条件下Kalman滤波器直线跟踪仿真结果。图5为完全DGPS条件下Kalman滤波器曲线跟踪仿真结果。图6为由DGPS到GPS条件下Kalman滤波器曲线跟踪仿真结果。具体实施例方式下面结合实施例对本专利技术做进一步详细的描述，但本专利技术的实施方式不限于此。本实施例采用的农业机械为久保田插秧机，其导航定位系统主要包括导航传感器组合、便携计算机和USB-RS232数据传输线等。导航传感器组合包括伪距差分GPS、电子罗盘和微机械陀螺。伪距差分GPS可以采集插秧机在WGS-84坐标系下的定位坐标，作为初步定位信息；电子罗盘测量其航向角度信息(以正北方向为0，逆时针方向为正)；微机械陀螺测量其横向角速率(逆时针方向为正)。便携计算机实现数据采集、数据预处理和信息融合定位软件系统的运行。便携计算机与导航传感器组合通过多根USB-RS232数据传输线建立连接。将上述多个传感器组合安装在插秧机后端的台架上，然后用数据采集软件系统进行动态数据采集，采集频率是1Hz。数据采集、数据预处理和信息融合定位软件系统采用Visual C++编程工具在Windows XP操作系统下开发完成。导航定位系统结构框图如图1。所述插秧机导航定位系统所获得的导航数据以伪距差分GPS定位数据为主，电子罗盘和微机械陀螺的航向角度数据为辅。为了获得插秧机准确的位置及航向估计，本专利技术通过两级融合的方法，逐次对原始航向角度数据和GPS定位数据进行融合。插秧机的具体组合定位方法(1)首先采用伪距差分GPS传感器测量得到插秧机的初步定位位置值；(2)然后采用电子罗盘和微机械陀螺传感器对插秧机的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值，实现一级融合；(3)再采用自适应卡尔曼滤波器对包括初步定位位置值和航向角度值的多源信息进行二级融合，获得插秧机定位和航向角度信息的精确估计。具体步骤包括1、采用伪距差分GPS传感器测量得到插秧机的初步定位位置值；GPS接收机输出的经纬度坐标是WGS-84地心大地坐标。为了使DGPS定位数据能用于导航控制系统，需要将WGS-84地心大地坐标转换为对应于WGS-84椭球的高斯平面坐标，即需进行高斯投影变换。本专利技术采用如下公式x=X+l22NsinBcosB+l424NsinBcos3B(5-t2+9η2+4η4)+l6720NsinBcos5B(61-]]>58t2+t4)]]>y=lNcosB+l36Ncos3B(1-t2+η2)+l5120Ncos5B(5-18t2+t4+14η2-58η2t2)]]>+500000]]>式中，l＝L-L0，L0为投影带中央子午线经度。卯酉圈曲率半径N=a1-e2sin2B,]]>椭球第一偏心率e＝2-2，辅助变量t＝tanB，辅助变量η＝e′cosB，椭球第二偏心率e′=a2/b2-1,]]>a、b分别为参考椭球的长、短半径，扁率＝(a-b)/a，X为赤道至纬度为B的平行圈的子午线弧长，其计算公式为X=c∫0B(1+e′2cos2B-2/3)dB]]>c为极曲率半径。本专利技术所用投影坐标系的主要参数为1)投影方式Gauss-Kruger2)中央经线11本文档来自技高网...

【技术保护点】
一种农业机械导航的组合定位方法，其特征在于包括下述步骤：（１）首先采用伪距差分ＧＰＳ传感器测量得到农业机械的初步定位位置值；（２）然后采用电子罗盘和微机械陀螺传感器对农业机械的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值；（３）再采用自适应卡尔曼滤波器对包括初步定位位置值和航向角度值的多源信息进行再次融合，获得农业机械定位和航向角度信息的精确估计。

【技术特征摘要】

【专利技术属性】
技术研发人员：罗锡文，张智刚，周志艳，赵祚喜，
申请(专利权)人：华南农业大学，
类型：发明
国别省市：81[中国|广州]