The invention discloses an intelligent tractor positioning method based on least squares method and Kalman filter, which relates to the field of intelligent machinery. Firstly, longitude and latitude data of tractor actual driving, current speed data are obtained by GPS, and heading angle data of tractor driving are obtained by gyroscope; secondly, gyroscope heading angle drift error is dynamically calculated by least squares method, and current heading angle drift error is estimated. The heading angle is used as the initial value of Kalman filter state. Finally, it is substituted into Kalman filter model to estimate the optimal heading angle, and the optimal heading angle is substituted into the tractor kinematics model to determine the accurate position of the tractor. The positioning method improves the positioning accuracy of tractors, overcomes the shortage of single module positioning accuracy, improves the positioning effect of tractors, and further improves the time and space coverage of navigation and positioning system, so as to better realize the continuous navigation of intelligent tractors in real sense.
【技术实现步骤摘要】
一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法
本专利技术涉及拖拉机自动驾驶领域,具体涉及到一种基于最小二乘法和卡尔曼滤波的智能拖拉机自主定位方法。
技术介绍
随着精准农业和智能农机的发展,传统的农业车辆已经无法满足作业要求,农业车辆的精确定位问题是智能农机发展的关键技术问题之一,并大大制约着智能农业车辆的发展。因此,设计出一种高精度低成本的智能农业车辆定位方法对于智能农业的发展具有重大的意义。为了解决智能农业车辆定位精度不足、定位困难的问题,本专利提出一种陀螺仪和GPS组合的定位方法,通过GPS获得拖拉机实际行驶的经纬度信息,陀螺仪获得拖拉机行驶的航向角,经过最小二乘法和卡尔曼滤波处理,计算出最优航向角,从而确定拖拉机准确位置。
技术实现思路
针对上述问题,提出一种智能拖拉机定位方法,采用陀螺仪和GPS组合的定位,通过GPS获得拖拉机实际行驶的经纬度值、陀螺仪获得拖拉机行驶的航向角,经过最小二乘法和卡尔曼滤波处理,估算最优航向角,从而确定拖拉机准确位置。本专利技术是通过如下技术方案得以实现的:一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法,包括农机运动学建模、最小二乘法处理和卡尔曼滤波处理过程,具体有如下步骤:第一步:读取该智能拖拉机GPS传感器的经度值、纬度值以及拖拉机当前车速,读取该智能拖拉机陀螺仪传感器的航向角值;第二步:该智能拖拉机工作在二维平面,且认为智能拖拉机的左右对称,建立智能拖拉机的两轮运动学模型;第三步:利用陀螺仪得到的航向角数据和GPS的经度值、纬度值以及车速数据求解和表示位置传感器的车身位置向量,即经纬度信息;表示姿态传感器的车 ...
【技术保护点】
1.一种基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法,其特征在于,包括如下步骤:步骤一:建立智能拖拉机运动学模型;步骤二:单片机读取分析得到该智能拖拉机GPS采集的经度值、纬度值以及拖拉机当前车速及陀螺仪采集的航向角值;步骤三:利用单片机分析得到的陀螺仪航向角数据和GPS经度值、纬度值以及车速数据求解GPS的车身位置向量epn和陀螺仪的车身位置向量eφn,通过最小二乘法求解误差之和In,利用最小化In的方法求解陀螺仪航向角漂移误差Dn;步骤四:将步骤三中的陀螺仪航向角漂移误差Dn和经过步骤二中单片机处理的陀螺仪航向角值代入公式
【技术特征摘要】
1.一种基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法,其特征在于,包括如下步骤:步骤一:建立智能拖拉机运动学模型;步骤二:单片机读取分析得到该智能拖拉机GPS采集的经度值、纬度值以及拖拉机当前车速及陀螺仪采集的航向角值;步骤三:利用单片机分析得到的陀螺仪航向角数据和GPS经度值、纬度值以及车速数据求解GPS的车身位置向量epn和陀螺仪的车身位置向量eφn,通过最小二乘法求解误差之和In,利用最小化In的方法求解陀螺仪航向角漂移误差Dn;步骤四:将步骤三中的陀螺仪航向角漂移误差Dn和经过步骤二中单片机处理的陀螺仪航向角值代入公式求出最小二乘法航向角值φn;步骤五:将步骤四中最小二乘法航向角值φn作为卡尔曼滤波的初始状态值代入卡尔曼滤波,计算找到卡尔曼滤波最优航向角步骤六:将步骤五中卡尔曼滤波最优航向角代入步骤一的拖拉机运动学模型,求出该智能拖拉机的当前位置坐标2.根据权利要求1所述的基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法,其特征在于,步骤一中智能拖拉机运动学模型如下所示:Xn表示由GPS获得的拖拉机n时刻的经度值;Xn+1表示拖拉机n+1时刻的经度值;Yn表示由GPS获得的拖拉机n时刻的纬度值;Yn+1表示拖拉机n+1时刻的纬度值;Vn表示拖拉机行驶速度,单位为m/s;t表示时间;sin表示正弦函数;表示拖拉机的绝对航向角,也就是卡尔曼滤波的最优航向角值;表示拖拉机卡尔曼滤波最优航向角的正弦值;cos表示余弦函数;表示拖拉机卡尔曼滤波最优航向角值的余弦值。3.根据权利要求1所...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。