CN104502942A - 基于卫星导航和航位推测实现农业机械定位的***及方法 - Google Patents

Abstract

本发明涉及一种基于卫星导航和航位推测实现农业机械定位的***及方法，其中包括卫星导航模块，用以获取所述的农业机械的地理位置信息；航位推测模块，用以采集所述的农业机械的相对移动位移和航向角进行航位推测计算；导航计算机模块，用以结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。采用该种结构的基于卫星导航和航位推测实现农业机械定位的***及方法，实现结合DR算法的实时性和GPS精度高的优点，克服DR算法的积累误差和GPS实施性差的缺陷，具有较高精度和实时性，尤其适用于农业机械辅助驾驶控制***中数据获取和处理中，具有更广泛的应用范围。

Description

基于卫星导航和航位推测实现农业机械定位的***及方法

技术领域

本发明涉及导航定位技术领域，尤其涉及农业机械定位技术领域，具体是指一种基于卫星导航和航位推测实现农业机械定位的***及方法。

背景技术

随着传感器和控制技术的发展和国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械辅助驾驶控制过程中，车体航向角和位置信息是控制算法需要的两大重要因素。目前在导航定位***中，广泛应用的导航方式有GPS(Global PositioningSystem，全球定位***)和DR(Dead Reckoning，航位推测法)两种。

GPS具有全球性、全天候、高精度、实时地提供导航、定位和授时信息，但它要求对卫星有直接的可见性才能发挥其正常功能，当有障碍物或者其他因素影响时，导航卫星的信号质量明显下降，甚至出现导航信息的中断，难以提供可靠、连续、实时地导航定位信息。

航位推算定位是一种常用的自主式导航定位方法，它是根据运动体的运动方向和航向距离的测量，从过去已知的位置来推算当前的位置，或预期将来的位置，从而可以得到一条运动轨迹，以此来引导航行。航位推算导航***的优点是低成本、自主性和隐蔽性好，且短时间内精度较高；其缺点是定位误差会随时间快速积累，定位信息发散，不利于长时间工作，另外它得到的是车辆相对于某一起始点的相对位置。

目前航位推算定位算法主要有两种方式：利用IMU(Inertial Measurement Unit，惯性测量单元)传感器的积分算法和里程计直接计算算法。IMU积分算法虽然能够计算距离，但是由于拖拉机作业时速度基本恒定，积分所获得的速度和位置信息很不准确，所以本发明采用里程计的方式来进行航位推算。

卡尔曼滤波是由R.E.Kalman提出的一种滤波算法，是一种递推线性最小方差估计，它用“状态”表征***的各个物理量、用“状态方程”和“量测方程”来描述***的动力学模型。由于采用单一导航***(GPS或者DR)都有各自的独特性能和局限性，所以本发明采用多卡尔曼滤波信息融合技术把GPS和DR的数据进行综合，使其性能互补、取长补短。

在现有技术中，卡尔曼滤波是一种数据融合方式，它不单单只应用在GPS和DR，它在很多领域都有应用，用好卡尔曼滤波的关键在于滤波参数的确定，如在专利CN200610032110.1中提到了“遗传优化和模糊”确定滤波参数，而在实际工程应用中，此方法往往有些弊端，比方说需要大量的实际数据来训练模型等等，而本发明则是参考传感器本身的性能参数和少量的采样值来确定滤波参数的，而且在实际工程应用中，姿态角对天线的修正是十分必要的，而上述专利CN200610032110.1中没有使用。

发明内容

本发明的目的是克服了上述现有技术的缺点，提供了一种能够实现结合DR算法的实时性和GPS精度高的优点、克服DR算法的积累误差和GPS实施性差的缺陷、具有更广泛应用范围的基于卫星导航和航位推测实现农业机械定位的***及方法。

为了实现上述目的，本发明的基于卫星导航和航位推测实现农业机械定位的***及方法具有如下构成：

该基于卫星导航和航位推测实现农业机械定位的***，其主要特点是，所述的***包括：

卫星导航模块，用以获取所述的农业机械的地理位置信息；

航位推测模块，用以采集所述的农业机械的相对移动位移和航向角进行航位推测计算；

导航计算机模块，用以结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。

较佳地，所述的卫星导航模块为GPS导航模块，所述的航位推测模块包括里程计单元和电子罗盘单元。

本发明还涉及一种通过所述的***基于卫星导航和航位推测实现农业机械定位的方法，其主要特征在于，所述的方法包括以下步骤：

(1)所述的卫星导航模块获取所述的农业机械初始地理位置信息；

(2)所述的航位推测模块采集所述的农业机械的相对移动位移和航向角进行航位推测计算；

(3)所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正；

(4)所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。

较佳地，所述的卫星导航模块获取所述的农业机械初始地理位置信息，具体为：

当所述的农业机械静止时，所述的卫星导航模块获取所述的农业机械初始的地理位置坐标。

更佳地，所述的航位推测模块包括里程计单元和电子罗盘单元，所述的航位推测模块采集所述的农业机械的相对移动位移和航向角进行航位推测计算，包括以下步骤：

(2-1-1)所述的电子罗盘单元采集地磁场信息和车体的翻滚角θ和俯仰角φ；

(2-1-2)所述的电子罗盘单元根据以下公式对采集的数据进行倾角补偿：

Xh₁＝X*cos(φ)+Y*sin(θ)sin(φ)-Z*cos(θ)*sin(φ)

Yh₁＝Y*cos(θ)+Z*sin(θ)；

所述的电子罗盘单元根据以下公式对采集的数据进行圆心修正：

Xh₂＝Xh₁-Xoffset

Yh₂＝Yh₁-Yoffset；

所述的电子罗盘单元根据以下公式对采集的数据进行椭圆修正：

$\left[\begin{array}{c}{\mathrm{Xh}}^{,}\\ \mathrm{Yh}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\α}& \sin \mathrm{\α}\\ -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]\left[\begin{array}{c}{\mathrm{Xh}}_2\\ {\mathrm{Yh}}_2\end{array}\right];$

Xh＝Xh'/σ

所述的电子罗盘单元根据以下公式进行航向角计算：

ψ₁＝-1*atan2(Yh,Xh)

if(ψ₁＜0)

ψ＝ψ₁+2π

if(ψ₁≥0)

ψ＝ψ₁

其中，X、Y、Z为电子输出的三轴磁场强度，θ为翻滚角，φ为俯仰角，Xh、Yh为修正后的磁场强度，ψ为航向角，Xoffset，Yoffset为圆心修正参数，α为椭圆修正参数，σ为由X和Y轴磁场强度最大值和最小值(X_Max-X_Min)/(Y_Max-Y_Min)计算出来的比率。

更进一步地，所述的步骤(1)和(2-1-1)之间，还包括以下步骤：

(2-0-1)所述的农业机械行驶***预设路程后并采集电子罗盘数据和进行硬磁和软磁修正参数计算；

(2-0-2)所述的农业机械继续行驶***预设路程后判断校准是否成功并保存成功的硬磁和软磁修正参数。

更佳地，所述的航位推测模块通过所述的农业机械车体航向角和车体的运行速度获得车体的坐标值，具体为：

所述的航位推测模块根据以下公式计算得到车体的坐标值：

$\mathrm{xk}=x0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}{v}_i*{\sin \mathrm{\ψ}}_i*\mathrm{\Δt};$

$\mathrm{yk}=y0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}{v}_i*{\cos \mathrm{\ψ}}_i*\mathrm{\Δt}$

其中，(x0，y0)农业机械初始的地理位置坐标，(xk，yk)为计算得到的车体的坐标值；v_i为t_i时刻的车体的运行速度，ψ_i为t_i时刻的车体航向角。

较佳地，所述的卫星导航模块为GPS导航模块，所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正，包括以下步骤：

(3-1)所述的卫星导航模块获取所述的农业机械当前地理位置坐标；

(3-2)所述的卫星导航模块根据以下公式对农业机械当前地理位置坐标进行修正：

$\begin{array}{c}\left[\begin{array}{c}{X}_{\mathrm{GPSa}}\\ Y_{\mathrm{GPSa}}\\ Z_{\mathrm{GPSa}}\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{GPSz}}\\ Y_{\mathrm{GPSz}}\\ Z_{\mathrm{GPSz}}\end{array}\right]-\\ \left[\begin{array}{ccc}\cos \mathrm{\ψ}{\cos \mathrm{\θ}}_r& -\sin \mathrm{\ψ}{\cos \mathrm{\φ}}_p+\cos \mathrm{\ψ}{\sin \mathrm{\θ}}_r{\sin \mathrm{\φ}}_p& \sin \mathrm{\ψ}\sin {\mathrm{\φ}}_p+\cos \mathrm{\ψ}{\sin \mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\\ \sin \mathrm{\ψ}{\cos \mathrm{\θ}}_r& \cos \mathrm{\ψ}{\sin \mathrm{\φ}}_p+\sin \mathrm{\ψ}{\sin \mathrm{\θ}}_r\sin {\mathrm{\φ}}_p& -\cos \mathrm{\ψ}{\sin \mathrm{\φ}}_p+\sin \mathrm{\ψ}\sin {\mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\\ -{\sin \mathrm{\θ}}_r& {\cos \mathrm{\θ}}_r{\sin \mathrm{\φ}}_p& {\cos \mathrm{\θ}}_r{\cos \mathrm{\φ}}_p\end{array}\right]\left[\begin{array}{c}a\\ b\\ h\end{array}\right]\end{array};$

其中， $\left[\begin{array}{c}{X}_{\mathrm{GPSa}}\\ Y_{\mathrm{GPSa}}\\ Z_{\mathrm{GPSa}}\end{array}\right]$ 为修正后的定位数据； $\left[\begin{array}{c}{X}_{\mathrm{GPSz}}\\ Y_{\mathrm{GPSz}}\\ Z_{\mathrm{GPSz}}\end{array}\right]$ 为转换到导航坐标系的GPS测量的位置信息；a、b、h分别为GPS天线在机体坐标系中的投影；θ_r为机体翻滚角；φ_p为机体俯仰角；ψ为机体航向角。

较佳地，所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位，包括以下步骤：

(4-1)当农业机械以恒速运动时，取组合导航定位的状态分量为X＝[x_e,v_e,x_n,v_n]^T，其中x_e、y_e分别为车体东向和北向的位置分量；v_e、v_n分别为车体东向和北向的速度分量，T为***周期，进行离散化后得到状态方程和观测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\ΦX}}_{k-1}+{\mathrm{\ΓW}}_k\\ Z_k={\mathrm{HX}}_{k-1}+V_k\end{array}\right.$

其中， $\mathrm{\Φ}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],\mathrm{\Γ}=\left[\begin{array}{cc}T/2& 2\\ 1& 0\\ 0& T/2\\ 0& 1\end{array}\right],H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right],$ V为零均值、协方差阵为R的白噪声，且与W互不相关；W与V在两坐标方向上的加速度相互独立并具有相同的方差σ²，X为***模型的状态向量，Z为观测向量(x，y)位置坐标，W为***模型噪声矩阵，是东向和北向；

(4-2)所述的导航计算机模块输出所述的农业机械定位的位置信息和航向角。

采用了该发明中的基于卫星导航和航位推测实现农业机械定位的***及方法，具有如下有益效果：

(1)本发明采用了里程计能够代替IMU传感器，降低成本和开发难度；

(2)本发明采用的卡尔曼数据融合技术，充分结合了DR算法的实时性和GPS精度高的优点，克服了DR算法的积累误差和GPS实时性差的缺陷，使二者得到了很好的互补；

(3)本发明通过对电子罗盘一系列校准和补偿算法，获得较高精度的车体航向角，为DR算法和后期的控制算法提供一个精确的数据输入；

(4)本发明采用的姿态修正算法可以解决农业机械在作业过程中由于路况复杂而引起的车身倾斜造成的误差；

(5)本发明具有较高精度和实时性，尤其适用于农业机械辅助驾驶控制***中数据获取和处理中，具有更广泛的应用范围。

附图说明

图1为本发明的基于卫星导航和航位推测实现农业机械定位的***的结构示意图。

图2为本发明的基于卫星导航和航位推测实现农业机械定位的方法的DR算法的原理图。

图3为本发明的基于卫星导航和航位推测实现农业机械定位的方法的流程图。

具体实施方式

为了能够更清楚地描述本发明的技术内容，下面结合具体实施例来进行进一步的描述。

本发明针对农业机械辅助驾驶控制***位置信息的获取中GPS数据输出频率低、不连续和采用IMU传感器DR算法存在积分误差等缺陷，设计了一种利用里程计和组合导航等技术的导航定位算法。

该基于卫星导航和航位推测实现农业机械定位的***，其主要特点是，所述的***包括：

卫星导航模块，用以获取所述的农业机械的地理位置信息；

航位推测模块，用以采集所述的农业机械的相对移动位移和航向角进行航位推测计算；

导航计算机模块，用以结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。

在一种较佳的实施方式中，所述的卫星导航模块为GPS导航模块，所述的航位推测模块包括里程计单元和电子罗盘单元。

本发明的基于卫星导航和航位推测实现农业机械定位的方法包括以下步骤：

(1)所述的卫星导航模块获取所述的农业机械初始地理位置信息；

(2)所述的航位推测模块采集所述的农业机械的相对移动位移和航向角进行航位推测计算；

(3)所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正；

(4)所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。

在一种较佳的实施方式中，所述的卫星导航模块获取所述的农业机械初始地理位置信息，具体为：

当所述的农业机械静止时，所述的卫星导航模块获取所述的农业机械初始的地理位置坐标。

在一种较佳的实施方式中，所述的航位推测模块包括里程计单元和电子罗盘单元，所述的航位推测模块采集所述的农业机械的相对移动位移和航向角进行航位推测计算，包括以下步骤：

(2-1)所述的电子罗盘单元采集地磁场信息和车体的翻滚角和俯仰角并通过硬磁、软磁和姿态修正算法计算农业机械车体航向角；

(2-2)所述的里程计单元采集农业机械车体的转速进而获得车体的转速进而获得车体的运行速度；

(2-3)所述的航位推测模块通过所述的农业机械车体航向角和车体的运行速度获得车体的坐标值。

在一种更佳的实施方式中，所述的电子罗盘单元采集地磁场信息和车体的翻滚角和俯仰角并通过硬磁、软磁和姿态修正算法计算农业机械车体航向角，包括以下步骤：

(2-1-1)所述的电子罗盘单元采集地磁场信息和车体的翻滚角θ和俯仰角φ；

(2-1-2)所述的电子罗盘单元根据以下公式对采集的数据进行倾角补偿：

Xh＝X*cos(φ)+Y*sin(θ)sin(φ)-Z*cos(θ)*sin(φ)

Yh＝Y*cos(θ)+Z*sin(θ)；

所述的电子罗盘单元根据以下公式对采集的数据进行圆心修正：

Xh＝Xh-Xoffset

Yh＝Yh-Yoffset；

所述的电子罗盘单元根据以下公式对采集的数据进行椭圆修正：

$\left[\begin{array}{c}\mathrm{Xh}\\ \mathrm{Yh}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\α}& \sin \mathrm{\α}\\ -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]\left[\begin{array}{c}\mathrm{Xh}\\ \mathrm{Yh}\end{array}\right];$

Xh＝Xh/σ

所述的电子罗盘单元根据以下公式进行航向角计算：

ψ＝-1*atan2(Yh,Xh)

if(ψ＜0)；

ψ＝ψ+2π

其中，X、Y、Z为电子输出的三轴磁场强度，θ为翻滚角，φ为俯仰角，Xh、Yh为修正后的磁场强度，ψ为航向角，Xoffset，Yoffset为圆心修正参数，α为椭圆修正参数。

在一种更进一步的实施方式中，所述的步骤(1)和(2-1-1)之间，还包括以下步骤：

(2-0-1)所述的农业机械行驶***预设路程后并采集电子罗盘数据和进行硬磁和软磁修正参数计算；

(2-0-2)所述的农业机械继续行驶***预设路程后判断校准是否成功并保存成功的硬磁和软磁修正参数。

在一种更佳的实施方式中，所述的航位推测模块通过所述的农业机械车体航向角和车体的运行速度获得车体的坐标值，具体为：

所述的航位推测模块根据以下公式计算得到车体的坐标值：

$\mathrm{xk}=x0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}{v}_i*{\sin \mathrm{\ψ}}_i*\mathrm{\Δt};$

$\mathrm{yk}=y0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}{v}_i*{\cos \mathrm{\ψ}}_i*\mathrm{\Δt}$

其中，(x0，y0)农业机械初始的地理位置坐标，(xk，yk)为计算得到的车体的坐标值；v_i为t_i时刻的车体的运行速度，ψ_i为t_i时刻的车体航向角。

在一种较佳的实施方式中，所述的卫星导航模块为GPS导航模块，所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正，包括以下步骤：

(3-1)所述的卫星导航模块获取所述的农业机械当前地理位置坐标；

(3-2)所述的卫星导航模块根据以下公式对农业机械当前地理位置坐标进行修正：

$\begin{array}{c}\left[\begin{array}{c}{X}_{\mathrm{GPSa}}\\ Y_{\mathrm{GPSa}}\\ Z_{\mathrm{GPSa}}\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{GPSz}}\\ Y_{\mathrm{GPSz}}\\ Z_{\mathrm{GPSz}}\end{array}\right]-\\ \left[\begin{array}{ccc}\cos \mathrm{\ψ}{\cos \mathrm{\θ}}_r& -\sin \mathrm{\ψ}{\cos \mathrm{\φ}}_p+\cos \mathrm{\ψ}{\sin \mathrm{\θ}}_r{\sin \mathrm{\φ}}_p& \sin \mathrm{\ψ}\sin {\mathrm{\φ}}_p+\cos \mathrm{\ψ}{\sin \mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\\ \sin \mathrm{\ψ}{\cos \mathrm{\θ}}_r& \cos \mathrm{\ψ}{\sin \mathrm{\φ}}_p+\sin \mathrm{\ψ}{\sin \mathrm{\θ}}_r\sin {\mathrm{\φ}}_p& -\cos \mathrm{\ψ}{\sin \mathrm{\φ}}_p+\sin \mathrm{\ψ}\sin {\mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\\ -{\sin \mathrm{\θ}}_r& {\cos \mathrm{\θ}}_r{\sin \mathrm{\φ}}_p& {\cos \mathrm{\θ}}_r{\cos \mathrm{\φ}}_p\end{array}\right]\left[\begin{array}{c}a\\ b\\ h\end{array}\right]\end{array};$

其中， $\left[\begin{array}{c}{X}_{\mathrm{GPSa}}\\ Y_{\mathrm{GPSa}}\\ Z_{\mathrm{GPSa}}\end{array}\right]$ 为修正后的定位数据； $\left[\begin{array}{c}{X}_{\mathrm{GPSz}}\\ Y_{\mathrm{GPSz}}\\ Z_{\mathrm{GPSz}}\end{array}\right]$ 为转换到导航坐标系的GPS测量的位置信息；a、b、h分别为GPS天线在机体坐标系中的投影；θ_r为机体翻滚角；φ_p为机体俯仰角；ψ为机体航向角。

在一种较佳的实施方式中，所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位，包括以下步骤：

(4-1)当农业机械以恒速运动时，取组合导航定位的状态分量为X＝[x_e,v_e,x_n,v_n]^T，其中x_e、y_e分别为车体东向和北向的位置分量；v_e、v_n分别为车体东向和北向的速度分量，T为***周期，进行离散化后得到状态方程和观测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\ΦX}}_{k-1}+{\mathrm{\ΓW}}_k\\ Z_k={\mathrm{HX}}_{k-1}+V_k\end{array}\right.$

其中， $\mathrm{\Φ}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],\mathrm{\Γ}=\left[\begin{array}{cc}T/2& 2\\ 1& 0\\ 0& T/2\\ 0& 1\end{array}\right],H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right],$ V为零均值、协方差阵为R的白噪声，且与W互不相关；W与V在两坐标方向上的加速度相互独立并具有相同的方差σ²，，X为***模型的状态向量，Z为观测向量(x，y)位置坐标，W为***模型噪声矩阵，是东向和北向。

(4-2)所述的导航计算机模块输出所述的农业机械定位的位置信息和航向角。

下面以一个具体实施例来进一步介绍本发明：

本发明是通过以下方法实现的，见附图1：

本发明包括电子罗盘单元、里程计单元、GPS模块和导航计算机模块等。电子罗盘单元和里程计单元共同组成了航位测算模块。

1、电子罗盘单元主要采集地磁场信息和车体的翻滚角和俯仰角，然后通过硬磁、软磁和姿态修正等算法计算农业机械车体航向角，从而利用此角度完成DR计算；

2、里程计单元采集农业机械车体(主要是车轮)的转速，从而通过转换获得车体的运行速度，通过采样频率计算车体的相对移动位移，然后结合车体航向角完成DR算法；

3、GPS导航模块实时获取农业机械所处的初始地理位置信息；

4、导航计算机模块主要实现车体航向计算、数据处理和卡尔曼数据融合算法；

结合附图2，对本发明中的DR航位推算算法原理做简要说明：

在农业机械作业的过程中，由于地球面积远大于作业面积，所以其运动可视为二维平面上的运动。建立局部平面坐标系，以x为局部平面坐标系的正东方向，y为局部平面坐标系的正北方向。假设知道车体初始位置坐标为(x0，y0)，已知在t0时刻的航向角为ψ₀，通过实时的测量车辆的行驶距离和航向角度的变化，即可以推算出车辆在每个时刻的二维坐标。设航向角为ψ₀时，经过Δt＝tl-t0时间内航行距离S0，则在tl时刻车辆的坐标值为：

x1＝x0+S0*sinψ₀

y1＝y0+S0*cosψ₀   (公式1)

将公式1递推，即得t2时刻车辆的坐标值：

x2＝x1+S1*cosψ₁＝y0+S0*cosψ₀+S1*cosψ₁

y2＝y1+S1*cosψ₁＝y0+S0*cosψ₀+S1*cosψ₁   (公式2)

则依次递推可得到坐标的一般方程：

$\mathrm{xk}=x0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}\mathrm{Si}*{\sin \mathrm{\ψ}}_i$

$\mathrm{yk}=y0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}\mathrm{Si}*{\cos \mathrm{\ψ}}_i$    (公式3)

当采样周期Δt恒定且采样频率足够高时，则可近似认为在采样周期内车辆的加速度为零，车辆做匀速运动，则在tk时刻的坐标值为：

$\mathrm{xk}=x0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}{v}_i*{\sin \mathrm{\ψ}}_i*\mathrm{\Δt}$

$\mathrm{yk}=y0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}{v}_i*{\cos \mathrm{\ψ}}_i*\mathrm{\Δt}$    (公式4)

式中，v_i为t_i时刻车体的作业速度。

结合附图3，对本发明中的DR算法和Kalman数据融合算法做简要说明：

第一步，首先农业机械静止，获取GPS数据，由于在DR算法(公式4)中需要初始位置信息(x0，y0)，作为DR航位推算算法数值积分的初值；

第二步，农业机械行驶一圈半，采集电子罗盘数据，进行硬磁和软磁修正参数计算，然后行驶半圈，进行判断校准是否成功，保存修正参数，包括Xoffset，Yoffset，α，σ；

第三步，电子罗盘数据采集，经过椭圆修正，圆心修正以及倾角补偿，然后计算航向角：

Xh＝X*cos(φ)+Y*sin(θ)sin(φ)-Z*cos(θ)*sin(φ)

Yh＝Y*cos(θ)+Z*sin(θ)   (公式5)

Xh＝Xh-Xoffset

Yh＝Yh-Yoffset   (公式6)

$\left[\begin{array}{c}\mathrm{Xh}\\ \mathrm{Yh}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\α}& \sin \mathrm{\α}\\ -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]\left[\begin{array}{c}\mathrm{Xh}\\ \mathrm{Yh}\end{array}\right]$    (公式7)

Xh＝Xh/σ

ψ＝-1*atan2(Yh,Xh)

if(ψ＜0)   (公式8)

ψ＝ψ+2π

其中，X,Y，Z为电子输出的三轴磁场强度，θ为翻滚角，φ为俯仰角，Xh，Yh为修正后的磁场强度，ψ为航向角，Xoffset，Yoffset为圆心修正参数，α为软磁(椭圆)修正参数，此参数为一个数值常量，即X和Y轴磁场强度最大值和最小值(xMax-xMin)/(yMax-yMin)计算出来的比率，用于对X轴磁场强度值的修正，即电子罗盘数据修正中的椭圆修正，公式5为倾角补偿，公式6为圆心修正，公式7为椭圆修正，公式8为航向角计算。

第四步，里程计数据采集，通过公式4和步骤3计算的航向角计算位置坐标；

第五步，获取GPS数据，由于GPS接收天线的安装位置高出农业机械一定距离，当在不平整的路面行驶过程中会出现倾斜和俯仰，会影响GPS的定位精度，应予以补偿或修正。 $\begin{array}{c}\left[\begin{array}{c}{X}_{\mathrm{GPSa}}\\ Y_{\mathrm{GPSa}}\\ Z_{\mathrm{GPSa}}\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{GPSz}}\\ Y_{\mathrm{GPSz}}\\ Z_{\mathrm{GPSz}}\end{array}\right]-\\ \left[\begin{array}{ccc}\cos \mathrm{\ψ}{\cos \mathrm{\θ}}_r& -\sin \mathrm{\ψ}{\cos \mathrm{\φ}}_p+\cos \mathrm{\ψ}{\sin \mathrm{\θ}}_r{\sin \mathrm{\φ}}_p& \sin \mathrm{\ψ}\sin {\mathrm{\φ}}_p+\cos \mathrm{\ψ}{\sin \mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\\ \sin \mathrm{\ψ}{\cos \mathrm{\θ}}_r& \cos \mathrm{\ψ}{\sin \mathrm{\φ}}_p+\sin \mathrm{\ψ}{\sin \mathrm{\θ}}_r\sin {\mathrm{\φ}}_p& -\cos \mathrm{\ψ}{\sin \mathrm{\φ}}_p+\sin \mathrm{\ψ}\sin {\mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\\ -{\sin \mathrm{\θ}}_r& {\cos \mathrm{\θ}}_r{\sin \mathrm{\φ}}_p& {\cos \mathrm{\θ}}_r{\cos \mathrm{\φ}}_p\end{array}\right]\left[\begin{array}{c}a\\ b\\ h\end{array}\right]\end{array};$    (公式9)

式中， $\left[\begin{array}{c}{X}_{\mathrm{GPSa}}\\ Y_{\mathrm{GPSa}}\\ Z_{\mathrm{GPSa}}\end{array}\right]$ 为修正后的定位数据； $\left[\begin{array}{c}{X}_{\mathrm{GPSz}}\\ Y_{\mathrm{GPSz}}\\ Z_{\mathrm{GPSz}}\end{array}\right]$ 为转换到导航坐标系的GPS测量的位置信息；a、b、h分别为GPS天线在机体坐标系中的投影；θ_r为机体翻滚角；φ_p为机体俯仰角；ψ为机体航向角。

第六步，卡尔曼数据融合。

在GPS/DR车载组合导航定位***中,选取合适的数学模型极其重要,因为它将直接影响导航数据滤波的效果，并将最终影响到整个组合导航定位***的定位精度。本发明采用恒速(CV)模型。

当农业机械以恒速运动时，取组合导航定位***的状态变量为X＝[x_e,v_e,x_n,v_n]^T，其中x_e、x_n分别为车体东向和北向的位置分量；v_e、v_n分别为车体东向和北向的速度分量，***采用周期为T，对连续***进行离散化，即可得到GPS/DR组合导航定位***的状态方程和观测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\ΦX}}_{k-1}+{\mathrm{\ΓW}}_k\\ Z_k={\mathrm{HX}}_{k-1}+V_k\end{array}\right.$    (公式10)

式中， $\mathrm{\Φ}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],\mathrm{\Γ}=\left[\begin{array}{cc}T/2& 2\\ 1& 0\\ 0& T/2\\ 0& 1\end{array}\right],H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right],$

***噪声平均值为零、方差阵为Q的高斯随机噪声序列，观测噪声

【V为零均值、协方差阵为R的白噪声，且与W互不相关；W与V在两坐标方向上的加速度相互独立并具有相同的方差σ²。

第七步，重复第三、四、五、六步即可进行循环导航坐标计算。

采用了该发明中的基于卫星导航和航位推测实现农业机械定位的***及方法，具有如下有益效果：

(1)本发明采用了里程计能够代替IMU传感器，降低成本和开发难度；

(2)本发明采用的卡尔曼数据融合技术，充分结合了DR算法的实时性和GPS精度高的优点，克服了DR算法的积累误差和GPS实时性差的缺陷，使二者得到了很好的互补；

(3)本发明通过对电子罗盘一系列校准和补偿算法，获得较高精度的车体航向角，为DR算法和后期的控制算法提供一个精确的数据输入；

(4)本发明采用的姿态修正算法可以解决农业机械在作业过程中由于路况复杂而引起的车身倾斜造成的误差；

(5)本发明具有较高精度和实时性，尤其适用于农业机械辅助驾驶控制***中数据获取和处理中，具有更广泛的应用范围。

在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。

Claims (10)

1.一种基于卫星导航和航位推测实现农业机械定位的***，其特征在于，所述的***包括：

卫星导航模块，用以获取所述的农业机械的地理位置信息；

航位推测模块，用以采集所述的农业机械的相对移动位移和航向角进行航位推测计算；

导航计算机模块，用以结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。

2.根据权利要求1所述的基于卫星导航和航位推测实现农业机械定位的***，其特征在于，所述的卫星导航模块为GPS导航模块，所述的航位推测模块包括里程计单元和电子罗盘单元。

3.一种根据权利要求1所述的***基于卫星导航和航位推测实现农业机械定位的方法，其特征在于，所述的方法包括以下步骤：

(1)所述的卫星导航模块获取所述的农业机械初始地理位置信息；

(2)所述的航位推测模块采集所述的农业机械的相对移动位移和航向角进行航位推测计算；

(3)所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正；

(4)所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。

4.根据权利要求3所述的基于卫星导航和航位推测实现农业机械定位的方法，其特征在于，所述的卫星导航模块获取所述的农业机械初始地理位置信息，具体为：

当所述的农业机械静止时，所述的卫星导航模块获取所述的农业机械初始的地理位置坐标。

5.根据权利要求3所述的基于卫星导航和航位推测实现农业机械定位的方法，其特征在于，所述的航位推测模块包括里程计单元和电子罗盘单元，所述的航位推测模块采集所述的农业机械的相对移动位移和航向角进行航位推测计算，包括以下步骤：

(2-1)所述的电子罗盘单元采集地磁场信息和车体的翻滚角和俯仰角并通过硬磁、软磁和姿态修正算法计算农业机械车体航向角；

(2-2)所述的里程计单元采集农业机械车体的转速进而获得车体的转速进而获得车体的运行速度；

(2-3)所述的航位推测模块通过所述的农业机械车体航向角和车体的运行速度获得车体的坐标值。

6.根据权利要求5所述的基于卫星导航和航位推测实现农业机械定位的方法，其特征在于，所述的电子罗盘单元采集地磁场信息和车体的翻滚角和俯仰角并通过硬磁、软磁和姿态修正算法计算农业机械车体航向角，包括以下步骤：

(2-1-1)所述的电子罗盘单元采集地磁场信息和车体的翻滚角θ和俯仰角φ；

(2-1-2)所述的电子罗盘单元根据以下公式对采集的数据进行倾角补偿：

Xh₁＝X*cos(φ)+Y*sin(θ)sin(φ)-Z*cos(θ)*sin(φ)

                                     ；

Yh₁＝Y*cos(θ)+Z*sin(θ)

所述的电子罗盘单元根据以下公式对采集的数据进行圆心修正：

Xh₂＝Xh₁-Xoffset

                               ；

Yh₂＝Yh₁-Yoffset

所述的电子罗盘单元根据以下公式对采集的数据进行椭圆修正：

$\left[\begin{array}{c}X{h}^{,}\\ \mathrm{Yh}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\α}& \sin \mathrm{\α}\\ -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]\left[\begin{array}{c}X{h}_2\\ X{h}_2\end{array}\right];$

Xh＝Xh’/σ

所述的电子罗盘单元根据以下公式进行航向角计算：

ψ₁＝-1*a tan2(Yh,Xh)

if(ψ₁＜0)

ψ＝ψ₁+2π

if(ψ₁≥0)

ψ＝ψ₁

其中，X、Y、Z为电子输出的三轴磁场强度，θ为翻滚角，φ为俯仰角，Xh、Yh为修正后的磁场强度，ψ为航向角，Xoffset，Yoffset为圆心修正参数，α为椭圆修正参数，σ为由X和Y轴磁场强度最大值和最小值(X_Max-X_Min)/(Y_Max-Y_Min)计算出来的比率。

7.根据权利要求6所述的基于卫星导航和航位推测实现农业机械定位的方法，其特征在于，所述的步骤(1)和(2-1-1)之间，还包括以下步骤：

(2-0-1)所述的农业机械行驶***预设路程后并采集电子罗盘数据和进行硬磁和软磁修正参数计算；

(2-0-2)所述的农业机械继续行驶***预设路程后判断校准是否成功并保存成功的硬磁和软磁修正参数。

8.根据权利要求5所述的基于卫星导航和航位推测实现农业机械定位的方法，其特征在于，所述的航位推测模块通过所述的农业机械车体航向角和车体的运行速度获得车体的坐标值，具体为：

所述的航位推测模块根据以下公式计算得到车体的坐标值：

$\mathrm{xk}=x0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}{v}_i*\sin {\mathrm{\ψ}}_i*\mathrm{\Δt}$

                        ；

$\mathrm{yk}=y0+\underset{i=0}{\overset{k-1}{\mathrm{\Σ}}}{v}_i*\sin {\mathrm{\ψ}}_i*\mathrm{\Δt}$

其中，(x0，y0)农业机械初始的地理位置坐标，(xk，yk)为计算得到的车体的坐标值；v_i为t_i时刻的车体的运行速度，ψ_i为t_i时刻的车体航向角。

9.根据权利要求3所述的基于卫星导航和航位推测实现农业机械定位的方法，其特征在于，所述的卫星导航模块为GPS导航模块，所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正，包括以下步骤：

(3-1)所述的卫星导航模块获取所述的农业机械当前地理位置坐标；

(3-2)所述的卫星导航模块根据以下公式对农业机械当前地理位置坐标进行修正：

$\begin{array}{c}\left[\begin{array}{c}{X}_{\mathrm{GPSa}}\\ Y_{\mathrm{GPSa}}\\ Z_{\mathrm{GPSa}}\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{GPSz}}\\ Y_{\mathrm{GPSz}}\\ Z_{\mathrm{GPSz}}\end{array}\right]-\\ \left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos {\mathrm{\θ}}_r& -\sin \mathrm{\ψ}\cos {\mathrm{\φ}}_p+\cos \mathrm{\ψ}\sin {\mathrm{\θ}}_r\sin {\mathrm{\φ}}_p& \sin \mathrm{\ψ}\sin {\mathrm{\φ}}_p+\cos \mathrm{\ψ}\sin {\mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\\ \sin \mathrm{\ψ}\cos {\mathrm{\θ}}_r& \cos \mathrm{\ψ}\sin {\mathrm{\φ}}_p+\sin \mathrm{\ψ}\sin {\mathrm{\θ}}_r\sin {\mathrm{\φ}}_p& -\cos \mathrm{\ψ}\sin {\mathrm{\φ}}_p+\sin \mathrm{\ψ}\sin {\mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\\ -\sin {\mathrm{\θ}}_r& \cos {\mathrm{\θ}}_r\sin {\mathrm{\φ}}_p& \cos {\mathrm{\θ}}_r\cos {\mathrm{\φ}}_p\end{array}\right]\left[\begin{array}{c}a\\ b\\ h\end{array}\right]\end{array};$

其中， $\left[\begin{array}{c}{X}_{\mathrm{GPSa}}\\ Y_{\mathrm{GPSa}}\\ Z_{\mathrm{GPSa}}\end{array}\right]$ 为修正后的定位数据； $\left[\begin{array}{c}{X}_{\mathrm{GPSz}}\\ Y_{\mathrm{GPSz}}\\ Z_{\mathrm{GPSz}}\end{array}\right]$ 为转换到导航坐标系的GPS测量的位置信息；a、b、h分别为GPS天线在机体坐标系中的投影；θ_r为机体翻滚角；φ_p为机体俯仰角；ψ为机体航向角。

10.根据权利要求3所述的基于卫星导航和航位推测实现农业机械定位的方法，其特征在于，所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位，包括以下步骤：

(4-1)当农业机械以恒速运动时，取组合导航定位的状态分量为X＝[x_e,v_e,x_n,v_n]^T，其中x_e、y_e分别为车体东向和北向的位置分量；v_e、v_n分别为车体东向和北向的速度分量，T为***周期，进行离散化后得到状态方程和观测方程为：

$\left\{\begin{array}{c}{X}_k=\mathrm{\Φ}{X}_{k-1}+\mathrm{\Γ}{W}_k\\ Z_k=H{X}_{k-1}+V_k\end{array}\right.$

其中， $\mathrm{\Φ}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],\mathrm{\Γ}=\left[\begin{array}{cc}T/2& 2\\ 1& 0\\ 0& T/2\\ 0& 1\end{array}\right],H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right],$ V为零均值、协方差阵为R的白噪声，且与W互不相关；W与V在两坐标方向上的加速度相互独立并具有相同的方差σ²，X为***模型的状态向量，Z为观测向量(x，y)位置坐标，W为***模型噪声矩阵，是东向和北向；

(4-2)所述的导航计算机模块输出所述的农业机械定位的位置信息和航向角。