CN109724596A - 一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

本发明的有益效果是：

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

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

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

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

5.本发明的定位方法进一步提高导航定位系统时间与空间的覆盖范围，从而较好的实现真正意义上智能车辆的连续导航。

附图说明

图1是本发明定位方法的组合定位系统原理图；

图2是本发明的智能拖拉机农机运动学模型；

图3是本发明定位方法的算法流程图。

具体实施方式

本发明提出一种智能拖拉机定位方法，详细设计了基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法，下面结合附图，对本发明作进一步详细说明，但本发明的保护范围并不限于此。

图1所示本发明定位方法的组合定位系统原理图，该组合系统由GPS和陀螺仪组成，首先通过GPS获得拖拉机行驶过程的经纬度信息，陀螺仪获得车辆行驶的航向角信息，将这些数据输入农机运动学模型中，其次，通过最小二乘法动态估算陀螺仪航向角漂移误差，估算出当前航向角，再代入卡尔曼滤波模型估算当前最优航向角，确定拖拉机准确位置。

图2是本发明的智能拖拉机农机运动学模型，拖拉机的位置(X_n+1，Y_n+1)可以用下式表示：

式中：X_n表示由GPS获得的拖拉机n时刻的经度值；X_n+1表示拖拉机n+1时刻的经度值；Y_n表示由GPS获得的拖拉机n时刻的纬度值；Y_n+1表示拖拉机n+1时刻的纬度值；V_n表示拖拉机行驶速度，单位为m/s；t表示时间；sin表示正弦函数；表示拖拉机的绝对航向角，也就是卡尔曼滤波的最优航向角值；表示拖拉机卡尔曼滤波最优航向角的正弦值；cos表示余弦函数；表示拖拉机卡尔曼滤波最优航向角值的余弦值；φ_n表示最小二乘法航向角值，也是卡尔曼滤波的初始航向角值；D_n表示陀螺仪航向角动态漂移误差；表示陀螺仪采集到的航向角值。

图3所示是本发明定位方法的算法流程图，首先通过单片机读取装在智能拖拉机上的GPS和陀螺仪的数据；其次分析拖拉机运动特点，建立智能拖拉机运动学模型，求解出车辆位置的表达式；然后利用最小二乘法求解陀螺仪动态漂移误差，如下所示。

由最小二乘法计算可得，车辆位置的误差可以通过下式来计算：

I_n表示误差之和，通过最小二乘法表达式表示如下： 表示位置传感器的车身位置向量，即经纬度信息；表示姿态传感器的车身位置向量，即陀螺仪航向角；N为导航点的数目，表示车身位置向量差的模的平方和。

由于I_n是误差之和，在此，在此可以使用最小化I_n来计算适当的D_n：

因此，综合上述公式，可以得到

式中：表示从n-N到n的求和函数；dX_i＝X_i-X_i-1，dY_i＝Y_i-Y_i-1；

cosD_n表示漂移误差的余弦值；sinD_n表示漂移误差的正弦值。

最后，导航系统的航向角漂移误差D_n可以通过下式计算：

求解完陀螺仪动态漂移误差后，继而可通过计算最小二乘法航向角值；然后设计航向角的卡尔曼滤波器，如下所示。

将上述计算的最小二乘法航向角φ_n作为卡尔曼滤波初始状态值，代入卡尔曼滤波模型：

X(k|k-1)＝A·X(k-1|k-1)+B·u(k)

上式中：A、B是系统矩阵，X(k|k-1)是由k-1推断的k时刻的航向角大小；u(k)为对系统控制量，这里为0，T_s表示系统的采样周期。系统的测量值Z(k)表示为Z(k)＝HX(k)+V(k)，V(k)表示测量过程中的噪声。

P(k|k-1)是此刻的误差协方差矩阵，如下式所示：

P(k|k-1)＝A·P(k-1)·A^T+Q

上式中：Q表示陀螺仪系统过程噪声的协方差。

k时刻的的航向角估计量X(k|k)可有下式求得：

X(k|k)＝X(k|k-1)+Kg(k)·(Z(k)-H·X(k|k-1))

上式中：H为观测矩阵，H＝[1 0]，K_g表示卡尔曼增益，可由下式求得：

Kg(k)＝P(k|k-1)·H^T/(H·P(k|k-1)·H^T+R)

式中：R表示陀螺仪测量误差的协方差矩阵，为了使卡尔曼滤波更新，应该通过使用下式协方差方程来更新。

P(k|k)＝(I-Kg(k)·H)·P(k|k-1)

式中：I是单位矩阵，

最后，可由卡尔曼滤波估算出当前最优航向角，代入运动学模型，求解出智能拖拉机当前位置，完成智能拖拉机自主定位方法的整个过程。

应当理解，虽然本说明书是按照各个实施例描述的，但并非每个实施例仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施例的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施例或变更均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法，其特征在于，包括如下步骤：

步骤一：建立智能拖拉机运动学模型；

步骤二：单片机读取分析得到该智能拖拉机GPS采集的经度值、纬度值以及拖拉机当前车速及陀螺仪采集的航向角值；

步骤三：利用单片机分析得到的陀螺仪航向角数据和GPS经度值、纬度值以及车速数据求解GPS的车身位置向量e_pn和陀螺仪的车身位置向量e_φn，通过最小二乘法求解误差之和I_n，利用最小化I_n的方法求解陀螺仪航向角漂移误差D_n；

步骤四：将步骤三中的陀螺仪航向角漂移误差D_n和经过步骤二中单片机处理的陀螺仪航向角值代入公式求出最小二乘法航向角值φ_n；

步骤五：将步骤四中最小二乘法航向角值φ_n作为卡尔曼滤波的初始状态值代入卡尔曼滤波，计算找到卡尔曼滤波最优航向角

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

2.根据权利要求1所述的基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法，其特征在于，步骤一中智能拖拉机运动学模型如下所示：

X_n表示由GPS获得的拖拉机n时刻的经度值；X_n+1表示拖拉机n+1时刻的经度值；Y_n表示由GPS获得的拖拉机n时刻的纬度值；Y_n+1表示拖拉机n+1时刻的纬度值；V_n表示拖拉机行驶速度，单位为m/s；t表示时间；sin表示正弦函数；表示拖拉机的绝对航向角，也就是卡尔曼滤波的最优航向角值；表示拖拉机卡尔曼滤波最优航向角的正弦值；cos表示余弦函数；表示拖拉机卡尔曼滤波最优航向角值的余弦值。

3.根据权利要求1所述的基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法，其特征在于，步骤三中GPS的车身位置向量e_pn可以通过下式来计算：

4.根据权利要求1所述的基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法，其特征在于，步骤三中陀螺仪的车身位置向量e_φn可以通过下式来计算：

φ_n表示n时刻最小二乘法航向角值，也是卡尔曼滤波的初始航向角值；sinφ_n表示拖拉机最小二乘法航向角的正弦值；cosφ_n表示拖拉机最小二乘法航向角的余弦值。

5.根据权利要求1所述的基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法，其特征在于，步骤三中误差之和I_n为表示从n-N到n的求和函数，e_pi-1表示GPS的车身位置向量，e_φi-1表示陀螺仪的车身位置向量，表示车身位置向量差的模的平方和。

6.根据权利要求1所述的基于最小二乘法和卡尔曼滤波的智能园艺电动拖拉机定位方法，其特征在于，步骤三中陀螺仪航向角漂移误差D_n可由下式表示：

tan^-1表示三角函数正切函数的反函数，dX_i＝X_i-X_i-1，dY_i＝Y_i-Y_i-1，

X_i表示由GPS获得的拖拉机i时刻的经度值；X_i-1表示拖拉机i-1时刻的经度值；Y_i表示由GPS获得的拖拉机i时刻的纬度值；Y_i-1表示拖拉机i-1时刻的纬度值；V_i表示拖拉机行驶速度，单位为m/s；sin表示正弦函数；表示陀螺仪i时刻的航向角；表示陀螺仪i-1时刻的航向角；表示陀螺仪i时刻的航向角的正弦值；表示陀螺仪i时刻的航向角的余弦值；表示陀螺仪i-1时刻的航向角的正弦值；表示陀螺仪i-1时刻的航向角的余弦值。