[发明专利]一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法

说明书

本发明公开了一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法，涉及智能机械领域，首先通过GPS获取拖拉机实际行驶的经纬度数据、当前车速数据，陀螺仪获得拖拉机行驶的航向角数据；其次，通过最小二乘法动态计算陀螺仪航向角漂移误差，估算出当前航向角并作为卡尔曼滤波器状态初值；最后，将其代入卡尔曼滤波模型估算最优航向角，将最优航向角代入拖拉机运动学模型确定拖拉机准确位置。该定位方法较好的提高了拖拉机的定位精度，克服了单一模块定位精度不足的问题，较好的提高了拖拉机的定位效果，也进一步提高导航定位系统时间与空间的覆盖范围，从而较好的实现真正意义上智能拖拉机的连续导航。

技术领域

本发明涉及拖拉机自动驾驶领域，具体涉及到一种基于最小二乘法和卡尔曼滤波的智能拖拉机自主定位方法。

背景技术

随着精准农业和智能农机的发展，传统的农业车辆已经无法满足作业要求，农业车辆的精确定位问题是智能农机发展的关键技术问题之一，并大大制约着智能农业车辆的发展。因此，设计出一种高精度低成本的智能农业车辆定位方法对于智能农业的发展具有重大的意义。

为了解决智能农业车辆定位精度不足、定位困难的问题，本专利提出一种陀螺仪和GPS组合的定位方法，通过GPS获得拖拉机实际行驶的经纬度信息，陀螺仪获得拖拉机行驶的航向角，经过最小二乘法和卡尔曼滤波处理，计算出最优航向角，从而确定拖拉机准确位置。

发明内容

针对上述问题，提出一种智能拖拉机定位方法，采用陀螺仪和GPS组合的定位，通过GPS获得拖拉机实际行驶的经纬度值、陀螺仪获得拖拉机行驶的航向角，经过最小二乘法和卡尔曼滤波处理，估算最优航向角，从而确定拖拉机准确位置。

本发明是通过如下技术方案得以实现的：

一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法，包括农机运动学建模、最小二乘法处理和卡尔曼滤波处理过程，具体有如下步骤：

第一步：读取该智能拖拉机GPS传感器的经度值、纬度值以及拖拉机当前车速，读取该智能拖拉机陀螺仪传感器的航向角值；

第二步：该智能拖拉机工作在二维平面，且认为智能拖拉机的左右对称，建立智能拖拉机的两轮运动学模型；

第三步：利用陀螺仪得到的航向角数据和GPS的经度值、纬度值以及车速数据求解和表示位置传感器的车身位置向量，即经纬度信息；表示姿态传感器的车身位置向量，通过最小二乘法求解误差之和关系表达式I_n，最小化误差之和I_n求解陀螺仪航向角的动态修正漂移误差D_n的表达式；

第四步：将上述陀螺仪航向角动态漂移误差表达式D_n和陀螺仪传感器采集的航向角初始值代入公式求出最小二乘法航向角的值φ_n；

第五步：设计航向角的卡尔曼滤波器，初始输入值为陀螺仪航向角量测值和最小二乘法的航向角状态值，计算找到卡尔曼滤波最优航向角

第六步：将卡尔曼滤波最优估计航向角值代入拖拉机运动学模型，求出该智能拖拉机的当前位置坐标

本发明的有益效果是：

1.本发明的定位方法采用GPS和陀螺仪组合定位，既减小了陀螺仪自身的累积偏差，也在一定程度上提高了GPS的定位精度；

2.本发明的定位方法采用动态修正陀螺仪的漂移误差，定位精度高，响应快；

3.本发明的定位方法采用最小二乘法求解陀螺仪的漂移误差表达式，扩大了最小二乘法的使用范围；

4.本发明的定位方法较好的降低了智能车辆导航定位系统的成本，具有一定的实用价值；