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

摘要

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

主权项

1.一种基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法，其特征在于，包括如下步骤：步骤一：建立智能拖拉机运动学模型；步骤二：单片机读取分析得到该智能拖拉机GPS采集的经度值、纬度值以及拖拉机当前车速及陀螺仪采集的航向角值；步骤三：利用单片机分析得到的陀螺仪航向角数据和GPS经度值、纬度值以及车速数据求解GPS的车身位置向量e_pn和陀螺仪的车身位置向量e_φn，通过最小二乘法求解误差之和I_n，利用最小化I_n的方法求解陀螺仪航向角漂移误差D_n；步骤四：将步骤三中的陀螺仪航向角漂移误差D_n和经过步骤二中单片机处理的陀螺仪航向角值代入公式求出最小二乘法航向角值φ_n；步骤五：将步骤四中最小二乘法航向角值φ_n作为卡尔曼滤波的初始状态值代入卡尔曼滤波，计算找到卡尔曼滤波最优航向角步骤六：将步骤五中卡尔曼滤波最优航向角代入步骤一的拖拉机运动学模型，求出该智能拖拉机的当前位置坐标