CN101285686A - 一种农业机械导航分级定位的方法和*** - Google Patents

Abstract

本发明公开了一种农业机械导航分级定位的方法，包括获取第一定位参数；利用卡尔曼滤波器对第一定位参数进行融合，获取第二定位参数；获取第三定位参数；利用多传感器自适应加权融合算法将第二定位参数与第三定位参数进行融合，获取目标定位参数；根据目标定位参数对农业机械进行定位。本发明还公开了一种农业机械导航分级定位的***。本发明通过对各测量设备获取的定位参数行多次融合，可平滑DGPS定位数据，有效避免DGPS动态定位的异常结果、有效滤除测试噪声、减小***误差，能够形成连续、稳定、精确的农业机械的位置和航向角度信息，提高了农业机械导航定位的准确性。

Description

一种农业机械导航分级定位的方法和***

技术领域

本发明涉及导航技术领域，特别是涉及一种农业机械导航分级定位的方法和***。

背景技术

导航定位是农业机械导航中的首要问题，导航定位的精度直接影响到农业机械跟踪预定义路径的质量。农业机械导航定位的主要信息包括：位置信息和航向角度信息、定位方法可分为绝对定位方法和相对定位方法。DGPS(Differential Global Positioning System，差分全球定位***)作为典型的绝对定位***，能够全天候为接收机提供绝对位置、速度和方向等信息；但该***需要接收四颗以上卫星才能决定车辆的位置，当受到外界干扰时，定位误差将大大增加。机器视觉属于相对定位方法，具有探测范围宽、信号丰富完整的特点，在提供导航信息的同时还可获得田间作物分布信息，其适应能力强，比较灵活，不需预先设定导航路线，非常适合行间作业的农业机械导航；但当地物标志信息不完整、外界光照条件变化时，都可能导致机器视觉导航定位***的功能失效，并且它只能提供微观尺度的导航定位数据，不适合进行大田内宏观的路径规划及行走方向的引导。电子罗盘、加速度计、陀螺仪和里程计等都是广泛使用的传感器，短时间内具有很高的可信度；但由于随机误差和***误差的存在，测量信号将产生飘移，一段时间后将严重影响导航性能。

可见，单一导航定位方法都存在自身难以克服的缺点，因此在国内外的研究中，多采用组合导航定位方法实现农业机械的高精度定位。目前，常见的组合导航定位方法主要有基于光纤陀螺仪或电子罗盘的DR(Dead Reckoning，航位推算)技术、基于GIS(Geographicinformation system，地理信息***)的地图匹配技术、采用RTK-DGPS(Real Time Kinematic DGPS，实时动态DGPS)和FOG(Fiber-OpticsGyroscopes，光纤陀螺)实现精确定位的技术以及采用低价格低精度GPS(Global Positioning System，全球定位***)融合高精度惯性传感器，通过卡尔曼滤波方法实现农业机械精确定位的技术等等。

在实现本发明过程中，发明人发现现有技术中至少存在如下问题：采用现有的组合导航定位技术时未充分考虑卡尔曼滤波自适应、难以避免滤波发散，从而造成农业机械导航定位的准确性较低，而且成本较高。

发明内容

本发明实施例要解决的问题是提供一种农业机械导航分级定位的方法和***，以克服现有技术中农业机械导航定位的准确性较低的缺陷。

为达到上述目的，本发明实施例的技术方案提供一种农业机械导航分级定位的方法，所述方法包括以下步骤：获取第一定位参数，所述第一定位参数包括通过差分全球定位***DGPS接收机获取的农业机械的第一位置值，通过电子罗盘获取的所述农业机械的第一航向角度值，通过对从加速度计得到的加速度值进行积分而获取的所述农业机械的当前行进速度值；利用卡尔曼滤波器对所述第一位置值、第一航向角度值和当前行进速度值进行融合，获取第二定位参数，所述第二定位参数包括所述农业机械的第二位置值和第二航向角度值；获取第三定位参数，所述第三定位参数包括通过摄像装置获取的所述农业机械的第三位置值和第三航向角度值；利用多传感器自适应加权融合算法将所述第二定位参数与所述第三定位参数进行融合，获取目标定位参数，所述目标定位参数包括所述农业机械的目标位置值和目标航向角度值；根据所述目标定位参数对所述农业机械进行定位。

其中，在所述利用卡尔曼滤波器对第一位置值、第一航向角度值和当前行进速度值进行融合，获取第二定位参数之前，还包括构建卡尔曼滤波器，所述构建卡尔曼滤波器的步骤具体包括：建立基于常速度的卡尔曼滤波器的状态方程和观测方程；根据所述状态方程和观测方程，建立预测方程组和校正方程组；根据所述预测方程组和校正方程组构建卡尔曼滤波器。

其中，所述状态方程为：

$\hat{X}\left(t\right|t-1)=\mathrm{\Φ}(t-1)\hat{X}(t-1|t-1)$

其中，

为t时刻农业机械的运动状态，Ф(t-1)为t-1时刻状态转移矩阵，为t-1时刻农业机械的运动状态；

所述观测方程为：

Z(t)＝H(t)X(t)+W(t)

其中，Z(t)为外部观测向量，H(t)为外部观测向量和状态向量之间的测量矩阵，X(t)为t时刻农业机械的状态向量，W(t)为高斯白噪声序列。

其中，所述预测方程组为：

$\left\{\begin{array}{c}\hat{X}\left(t\right|t-1)=\mathrm{\Φ}(t-1)\hat{X}(t-1|t-1)\\ P\left(t\right|t)=\mathrm{\φ}\left(t\right|t)P(t-1|t-1){\mathrm{\φ}}^T\left(t\right|t)+G(t-1)Q(t-1)G^T(t-1)\end{array}\right.$

其中，

为t时刻农业机械的运动状态，Ф(t-1)为t-1时刻状态转移矩阵，

为t-1时刻农业机械的运动状态，P(t|t)为t时刻滤波误差方差阵，φ(t|t)为t时刻状态转移矩阵，P(t-1|t-1)为t-1时刻滤波误差方差阵，φ^T(t|t)为t时刻状态转移矩阵的转置矩阵，G(t-1)为t-1时刻过程噪声分布矩阵，Q(t-1)为t-1时刻正定协方差矩阵，G^T(t-1)为t-1时刻过程噪声分布矩阵的转置矩阵；

所述校正方程组为：

$\left\{\begin{array}{c}\hat{X}\left(t\right|t)=\hat{X}\left(t\right|t-1)+P\left(t\right|t)H^T\left(t\right)R^{-1}\left(t\right)\·[Z\left(t\right)-H\left(t\right)\hat{X}\left(t\right|t-1)]\\ P^{-1}\left(t\right|t)=P^{-1}\left(t\right|t-1)+H^T\left(t\right)R^{-1}\left(t\right)H\left(t\right)\end{array}\right.$

其中，

为t时刻农业机械的运动状态，为t-1时刻农业机械的运动状态，P(t|t)为t时刻滤波误差方差阵，H(t)为t时刻外部观测向量和状态向量之间的测量矩阵，H^T(t)为t时刻外部观测向量和状态向量之间的测量矩阵的转置矩阵，R^-1(t)为t时刻正定协方差矩阵的逆矩阵，Z(t)为t时刻外部观测向量，P^-1(t|t)为t时刻滤波误差方差阵的逆矩阵，P^-1(t|t-1)为t-1时刻滤波误差方差阵。

其中，在所述卡尔曼滤波器中，利用DGPS接收机得到的第一位置值对所述状态转移矩阵进行实时更新。

其中，所述利用卡尔曼滤波器对第一位置值、第一航向角度值和当前行进速度值进行融合，具体包括：利用所述第一位置值、第一航向角度值和当前行进速度值更新所述预测方程组的矩阵；通过所述预测方程组获取下一时刻的状态估计；利用所述校正方程组对所述下一时刻的状态估计进行校正，获取当前最优估计；根据所述当前最优估计，获取当前最优估计的误差协方差矩阵。

其中，在所述利用多传感器自适应加权融合算法将第二定位参数与第三定位参数进行融合，获取目标定位参数之前，还包括以下步骤：判断所述DGPS接收机和摄像装置是否均能正常工作，如果所述DGPS接收机和摄像装置都不能正常工作，则停止导航；如果所述DGPS接收机和摄像装置中只有一个能正常工作，则判断能正常工作的装置得到的定位参数是否在误差范围内，如果是则采用单一传感器进行导航，否则停止导航；如果所述DGPS接收机和摄像装置都能正常工作，则判断能正常工作的装置得到的定位参数是否在误差范围内，如果所述DGPS接收机和摄像装置得到的定位参数都不在误差范围内，则停止导航；如果所述DGPS接收机和摄像装置得到的定位参数中只有一个在误差范围内，则采用单一传感器进行导航；如果所述DGPS接收机和摄像装置得到的定位参数都在误差范围内，则利用多传感器自适应加权融合算法将所述DGPS接收机得到的第二定位参数和摄像装置得到的第三定位参数进行融合。

其中，所述利用多传感器自适应加权融合算法将第二定位参数与第三定位参数进行融合，具体包括：

根据各测量设备获取的定位参数的方差，获取各测量设备的最优加权因子，所述最优加权因子由公式

$W_p=\frac{1}{{\mathrm{\σ}}_p^2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{1}{{\mathrm{\σ}}_i^2}}$ (p＝1，2，…n)

获取；其中，W_p为最优加权因子，σ²为各测量设备获取的定位参数的方差；

根据公式

$\hat{X}=\frac{{\mathrm{\δ}}_q^2{X}_p+{\mathrm{\δ}}_p^2{X}_q}{{\mathrm{\δ}}_p^2+{\mathrm{\δ}}_q^2}$

获取融合后的定位参数；其中，

为融合后的定位参数，δ_q ²为摄像装置获取的定位参数的方差，X_p为DGPS接收机得到的定位参数值，δ_p ²为DGPS接收机获取的定位参数的方差，X_q为摄像装置得到的定位参数值。

本发明实施例的技术方案还提供了一种农业机械导航分级定位的***，所述***包括：DGPS接收机，用于获取农业机械的第一位置值；电子罗盘，用于获取所述农业机械的第一航向角度值；加速度计，用于获取所述农业机械的加速度值；摄像装置，用于获取所述农业机械的第三位置值和第三航向角度值；目标定位参数获取装置，用于对所述第一位置值、第一航向角度值和当前行进速度值进行融合，获取第二定位参数，并将所述第二定位参数与所述第三定位参数进行融合，获取目标定位参数；定位装置，用于根据所述目标定位参数对所述农业机械进行定位。

其中，所述目标定位参数获取装置包括：卡尔曼滤波器，用于对所述第一位置值、第一航向角度值和当前行进速度值进行融合；多传感器自适应加权融合单元，用于将所述第二定位参数与所述第三定位参数进行融合。

上述技术方案仅是本发明的一个优选技术方案，具有如下优点：本发明实施例通过对各测量设备获取的定位参数行多次融合，可平滑DGPS定位数据，有效避免DGPS动态定位的异常结果、有效滤除测试噪声、减小***误差，能够形成连续、稳定、相对精确的农业机械的位置和航向角度信息，提高了农业机械导航定位的准确性。

附图说明

图1是本发明实施例的一种农业机械导航分级定位的方法的流程图；

图2是本发明实施例的卡尔曼滤波器的工作原理图；

图3是本发明实施例的多传感器自适应加权融合算法的原理示意图；

图4是本发明实施例的多传感器自适应加权融合算法的流程图；

图5是本发明实施例的一种农业机械导航分级定位的***的结构图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

发明实施例的一种农业机械导航分级定位的方法的流程如图1所示，所述方法包括以下步骤：

步骤s101，获取第一定位参数，所述第一定位参数包括通过差分全球定位***DGPS接收机获取的农业机械的第一位置值，通过电子罗盘获取的所述农业机械的第一航向角度值，通过对从加速度计得到的加速度值进行积分而获取的所述农业机械的当前行进速度值。

步骤s102，构建卡尔曼滤波器。首先建立基于常速度的卡尔曼滤波器的状态方程和观测方程，然后根据所述状态方程和观测方程，建立预测方程组和校正方程组，最后根据所述预测方程组和校正方程组构建卡尔曼滤波器。

步骤s103，利用卡尔曼滤波器对所述第一位置值、第一航向角度值和当前行进速度值进行融合，获取第二定位参数，所述第二定位参数包括所述农业机械的第二位置值和第二航向角度值。其中，利用卡尔曼滤波器对第一位置值、第一航向角度值和当前行进速度值进行融合的步骤包括：利用所述第一位置值、第一航向角度值和当前行进速度值更新所述预测方程组的矩阵；通过所述预测方程组获取下一时刻的状态估计；利用所述校正方程组对所述下一时刻的状态估计进行校正，获取当前最优估计；根据所述当前最优估计，获取当前最优估计的误差协方差矩阵。

步骤s104，获取第三定位参数，所述第三定位参数包括通过摄像装置获取的所述农业机械的第三位置值和第三航向角度值。从每一时刻采集的图像中提取导航线，得到导航线的特征参数，即导航线两端点坐标值。导航线的特征参数建立在图像坐标系下，通过坐标转换关系，可以得到导航线特征参数在世界坐标系下的对应位置值。已知导航线方程的两点坐标，根据公式θ＝atan[(x₂-x₁)/(y₂-y₁)]可求得导航线与Y轴夹角，即农机与导航线之间的航向角度值。

步骤s105，利用多传感器自适应加权融合算法将所述第二定位参数与所述第三定位参数进行融合，获取目标定位参数，所述目标定位参数包括所述农业机械的目标位置值和目标航向角度值。在获取目标定位参数之前，还包括：判断所述DGPS接收机和摄像装置是否均能正常工作，如果所述DGPS接收机和摄像装置都不能正常工作，则停止导航；如果所述DGPS接收机和摄像装置中只有一个能正常工作，则判断能正常工作的装置得到的定位参数是否在误差范围内，如果是则采用单一传感器进行导航，否则停止导航；如果所述DGPS接收机和摄像装置都能正常工作，则判断能正常工作的装置得到的定位参数是否在误差范围内，如果所述DGPS接收机和摄像装置得到的定位参数都不在误差范围内，则停止导航；如果所述DGPS接收机和摄像装置得到的定位参数中只有一个在误差范围内，则采用单一传感器进行导航；如果所述DGPS接收机和摄像装置得到的定位参数都在误差范围内，则利用多传感器自适应加权融合算法将所述DGPS接收机得到的第二定位参数和摄像装置得到的第三定位参数进行融合。利用多传感器自适应加权融合算法将第二定位参数与第三定位参数进行融合的步骤包括：根据各测量设备获取的定位参数的方差，获取各测量设备的最优加权因子，所述最优加权因子由公式

$W_p=\frac{1}{{\mathrm{\σ}}_p^2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{1}{{\mathrm{\σ}}_i^2}}$ (p＝1，2，…n)

获取；其中，W_p为最优加权因子，σ²为各测量设备获取的定位参数的方差；根据公式

$\hat{X}=\frac{{\mathrm{\δ}}_q^2{X}_p+{\mathrm{\δ}}_p^2{X}_q}{{\mathrm{\δ}}_p^2+{\mathrm{\δ}}_q^2}$

获取融合后的定位参数；其中，

为融合后的定位参数，δ_q ²为摄像装置获取的定位参数的方差，X_p为DGPS接收机得到的定位参数值，δ_p ²为DGPS接收机获取的定位参数的方差，X_q为摄像装置得到的定位参数值。

步骤s106，根据所述目标定位参数对所述农业机械进行定位。

本实施例所采用的导航传感器主要包括DGPS接收机、摄像装置采用CCD(Charge Coupled Device，电荷耦合器件)摄像头、电子罗盘和加速度计。利用DGPS接收机采集农业机械在WGS-84(WorldGeodetic System-84，1984年世界大地坐标系)下各位置点的坐标，作为初步定位信息；采用电子罗盘测量农业机械的航向角度信息；采用加速度计测其当前时刻的加速度，再通过积分得到当前时刻的速度值。将上述各传感器组合安装在农业机械上，利用数据采集软件***动态采集数据，设定采集频率为1Hz。数据的采集、预处理和信息融合等均由软件***实现。

本发明的融合多传感器信息的农业机械导航分级定位方法通过两级融合，逐步对初步DGPS定位数据、航向角度数据、速度数据以及机器视觉获得的定位数据进行融合，其具体实施步骤如下：

1.采用DGPS接收机测量得到农业机械的初步定位信息：

DGPS***中，采用的是WGS-84坐标系，属于地心坐标系，为使DGPS定位数据能用于导航控制***，需要进行高斯投影变换。

地球椭球上任一点的位置，既可以表示为，又可以表示为。两个坐标系之间的转换公式为：

$\left.\begin{array}{c}X=(N+H)\cos B\cos L\\ Y=(N+H)\cos B\sin L\\ Z=[N(1-e^2)+H]\sin B\end{array}\right\}---\left(1\right)$

式中，N为椭球的卯酉圈曲率半径，a为地球椭球长半径，b为短半径，e为椭球的第一偏心率。

N＝a/W               (2)

W＝(1-e²sin²B)^1/2    (3)

e²＝(a²-b²)/a²       (4)

2.采用卡尔曼滤波器对DGPS得到的农业机械的位置值和电子罗盘得到的航向角度值及通过对加速度计得到的初始值积分得到的当前速度值进行融合：

(1)一般来说，农业机械的作业环境较为平整，工作时要求农业机械的行进速度较为缓慢并且恒定，除地头转弯不会有较大的转向动作。因此，假设农业机械的行进速度恒定且转向角加速度为0，则利用Kalman滤波器便可得到如下的状态空间描述：

X(t+1)＝Ф(t)X(t)+G(t)V(t)    (5)

其中，X(t)为t时刻农业机械的状态向量，且

X(t)＝(x(t)，y(t)，v(t))^T     (6)

其中，x(t)，y(t)为t时刻WGS-84坐标系下农业机械在高斯投影平面上的位置点坐标值，v(t)为t时刻农业机械的行进速度，Ф(t)为状态转移矩阵，G(t)为过程噪声分布矩阵，V(t)为零均值高斯过程白噪声向量，即

E[V(t)]＝0，E[V(u)V^T(j)]＝Q(t)δ_kj                     (7)

设φ(t)为t时刻农业机械的航向角在高斯投影平面坐标下的角度分量，横轴正向为0，逆时针为正，则状态转移矩阵定义为

$\mathrm{\φ}\left(t\right)=\left[\begin{array}{ccc}1& 0& T\cos \left(\mathrm{\θ}(t-1)\right)\\ 0& 1& T\sin \left(\mathrm{\θ}(t-1)\right)\\ 0& 0& 1\end{array}\right]$

可利用上一时刻农业机械的航向角度和前轮转角的实际值对φ(t)矩阵进行实时更新。

(2)Kalman滤波器的基本观测方程：

Z(t)＝H(t)X(t)+W(t)                         (8)

利用基本观测方程，将DGPS接收机得到的农业机械当前位置点坐标(x，y)，速度值v作为外部观测量，则外部观测向量Z(t)＝[x(t)y(t)v(t)]，外部观测向量和状态向量之间的测量矩阵为H(t)，是常数矩阵：

$H\left(t\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

W(t)是均值为0、方差为r_i ²的高斯白噪声序列。测量的噪声方差矩阵R(t)为：

$R\left(t\right)=\left[\begin{array}{ccc}{r}_1^2& 0& 0\\ 0& r_2^2& 0\\ 0& 0& r_3^2\end{array}\right]$

r₁、r₂、r₃分别表示DGPS接收机、电子罗盘、加速度计的测量噪声的标准差。

(3)Kalman滤波器的工作流程：

在Kalman滤波器的状态方程和观测方程的基础上，其递归差分方程组可用如下方程表示：

$\hat{X}\left(t\right|t)=\hat{X}\left(t\right|t-1)+P\left(t\right|t)H^T\left(t\right)R^{-1}\left(t\right)\·[Z\left(t\right)-H\left(t\right)\hat{X}\left(t\right|t-1)]---\left(9\right)$

P^-1(t|t)＝P^-1(t|t-1)+H^T(t)R^-1(t)H(t)              (10)

式(9)，(10)为Kalman滤波器的校正方程组，由它们可以得到

$P^{-1}\left(t\right|t)=P^{-1}\left(t\right|t-1)+\underset{i=1}{\overset{2}{\mathrm{\Σ}}}[P_i^{-1}\left(t\right|t)-P_i^{-1}\left(t\right|t-1)]---\left(11\right)$

$P^{-1}\left(t\right|t)\hat{X}\left(t\right|t)=P^{-1}\left(t\right|t-1)\hat{X}\left(t\right|t-1)+\underset{i=1}{\overset{2}{\mathrm{\Σ}}}[P_i^{-1}\left(t\right|t){\hat{X}}_i\left(t\right|t)-P_i^{-1}\left(t\right|t-1){\hat{X}}_i\left(t\right|t-1)]---\left(12\right)$

为便于算法的实现，对式(11)，(12)做进一步推导，可以得到

$P^{-1}\left(t\right|t)\hat{X}\left(t\right|t-1)=P^{-1}\left(t\right|t-1)\hat{X}\left(t\right|t-1)+\underset{i=1}{\overset{2}{\mathrm{\Σ}}}[P_i^{-1}\left(t\right|t)-P_i^{-1}\left(t\right|t-1)\hat{X}\left(t\right|t-1)]---\left(13\right)$

$\hat{X}\left(t\right|t)=\hat{X}\left(t\right|t-1)+P^{-1}\left(t\right|t)\left\{\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{P}_i^{-1}\left(t\right|t)\right[{\hat{X}}_i\left(t\right|t)-\hat{X}\left(t\right|t-1)]+\underset{i=1}{\overset{2}{\mathrm{\Σ}}}{P}_i^{-1}\left(t\right|t-1)[\hat{X}\left(t\right|t-1)-{\hat{X}}_i\left(t\right|t-1)\left]\right\}---\left(14\right)$

其中，

$\hat{X}\left(t\right|t-1)=\mathrm{\Φ}(t-1)\hat{X}\left(t\right|t-1)---\left(15\right)$

P(t|t)＝φ(t|t)P(t-1|t-1)φ^T(t|t)+G(t-1)Q(t-1)G^T(t-1)(16)

式(15)、(16)为Kalman滤波器的预测方程组。

在上述各方程中，方程(9)为滤波方程，利用测量值Z(t)对预测的***状态估计进行更新。方程(10)为滤波误差方差方程，利用计算出的Kalman滤波器增益对预测的滤波误差方差阵进行更新。方程(15)为Kalman滤波器的状态方程，代入t-1时刻的状态预测值，可对t时刻农业机械的运动状态进行预测。方程(16)为预报误差方差方程，可以对滤波误差方差阵进行预测。方程(14)说明农业机械的状态估计信息融合等价于状态估计与校正的融合结果之和。预测校正包括两部分，其一是各个传感器跟踪与预测差值(跟踪残留误差)的总和，每个差值由逆(预测)协方差加权；其二是整个***预测与各个传感器预测的差值(预测残留误差)的总和，由局部逆(预测)协方差加权。

通常Kalman滤波器的应用中只用一种残留误差——跟踪残留误差来校正状态估计的融合，但本发明采用两种。跟踪残留误差反映了整个***中的不可预测信息所带来的误差，用来校正整个***的状态估计。但由于各传感器的状态估计与整个***的状态估计相互关联，所以跟踪残留误差不能完全反映不可预测信息所带来的误差。因而式(14)采用预测残留误差来校正，显然比式(12)具有更明确的物理意义，且更容易实现。

(4)Kalman滤波器的工作原理：

首先将初始状态估计

和初始滤波误差方差阵P(t-1)估计引入Kalman滤波器的预测方程组进行Kalman滤波。采用DGPS、电子罗盘及加速度计采集到的数据更新预测方程组的矩阵，再由预测方程组对下一时刻状态进行估计。预测过程结束后再利用Kalman滤波器进行校正，将R矩阵引入校正方程组，根据当前测量结果(针对预测方程组对下一时刻状态的估计结果)，对预测方程组的状态估计进行更新，获得当前最优估计，最后解算当前最优估计的误差协方差矩阵，完成一次滤波。Kalman滤波器的工作原理图如图2所示。

3.采用多传感器自适应加权融合算法将机器视觉提供的位置值和航向角度值与初次融合得到相对精确的农业机械的位置和航向角度信息进行再次融合：

不同传感器有各自不同的加权因子，多传感器自适应加权融合算法的中心思想是在总均方误差最小这一最优条件下，根据各个传感器所得到的测量值以自适应的方式寻找各个传感器所对应的最优加权因子，使融合后的值达到最优。

多传感器自适应加权融合算法的原理示意图如图3所示，具体实现步骤如下：

(1)最优加权因子的求取

设n个传感器的方差分别为σ₁ ²，σ₂ ²，…，σ_n ²，所需估计的真值为X，各传感器的测量值分别为X₁，X₂，…，X_n，各测量值相互独立，并且是X的无偏估计；各传感器的加权因子分别为W₁，W₂，…，W_n，融合后的

和加权因子满足：

$\hat{X}=\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{W}_p{X}_p---\left(17\right)$

$\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{W}_p=1---\left(18\right)$

总均方误差为：

${\mathrm{\σ}}^2=E\left[{(X-\hat{X})}^2\right]=E[\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{W_p}^2{(X-X_p)}^2+2\underset{p=q}{\overset{n}{\underset{p=1,q=1}{\mathrm{\Σ}}}}{W}_p{W}_q(X-X_p)(X-X_q)]---\left(19\right)$

因为X₁，X₂，…X_n相互独立，并且为X的无偏估计，所以：

故σ²可以写成：

${\mathrm{\σ}}^2=E\left(\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{W_p}^2{(X-X_p)}^2\right)=\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{W_p}^2{{\mathrm{\δ}}_p}^2---\left(21\right)$

总均方误差σ²是关于各加权因子的多元二次函数，因此，σ²必然存在最小值。该最小值的求取是加权因子满足公式(18)约束条件的多元函数极值的求取。根据多元函数求极值理论，可求出总均方误差最小时对应的加权因子为：

${W_p}^{*}=\frac{1}{{{\mathrm{\σ}}_p}^2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\frac{1}{{\mathrm{\σ}}_i^2}},(p=\mathrm{1,2},n)---\left(22\right)$

此时对应的最小均方误差为：

${{\mathrm{\σ}}_{\min }}^2=\frac{1}{\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\frac{1}{{{\mathrm{\σ}}_p}^2}}---\left(23\right)$

(2)测量方差的加窗估计算法

设DGPS接收机和机器视觉传感器的测量值分别为X_p和X_q，对应的观测误差分别为V_p和V_q，其中V_p和V_q互不相关，且均值为0。X_p和X_q的自协方差函数分别为R_pp和R_qq，互协方差函数为R_pq，第k次采样时R_pp的时间域估计值为R_pp(k)，R_qq的时间域估计值为R_qq(k)，R_pq的时间域估计值为R_pq(k)，则由前N次采样的R_pp，R_qq和R_pq的时间域估计值可以得到如下递推公式：

$R_{\mathrm{pp}}\left(k\right)=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{(X_p\left(i\right)-\mathrm{\μ})}^2---\left(24\right)$

$R_{\mathrm{qq}}\left(k\right)=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{(X_q\left(i\right)-\mathrm{\μ})}^2---\left(25\right)$

$R_{\mathrm{pq}}\left(k\right)=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}(X_p\left(i\right)-\mathrm{\μ})(X_q\left(i\right)-\mathrm{\μ})=R_{\mathrm{qp}}\left(k\right)---\left(26\right)$

随着***长时间运行，***参数会发生缓慢变化，噪声特征也会发生某种程度的缓变，为了在保证估计精度的前提下及时跟踪噪声的变化提出加窗方差估计方法：

当k＜N时，

$R_{\mathrm{pp}}\left(k\right)=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{(X_p\left(i\right)-\mathrm{\μ})}^2---\left(27\right)$

$R_{\mathrm{qq}}\left(k\right)=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{(X_q\left(i\right)-\mathrm{\μ})}^2---\left(28\right)$

$R_{\mathrm{pq}}\left(k\right)=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}(X_p\left(i\right)-\mathrm{\μ})(X_q\left(i\right)-\mathrm{\μ})=R_{\mathrm{qp}}\left(k\right)---\left(29\right)$

当k≥N时，

$R_{\mathrm{pp}}\left(k\right)=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{(X_p\left(i\right)-\mathrm{\μ})}^2---\left(30\right)$

$R_{\mathrm{qq}}\left(k\right)=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{(X_q\left(i\right)-\mathrm{\μ})}^2---\left(31\right)$

$R_{\mathrm{pq}}\left(k\right)=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(X_p\left(i\right)-\mathrm{\μ})(X_q\left(i\right)-\mathrm{\μ})=R_{\mathrm{qp}}\left(k\right)---\left(32\right)$

式中μ为采样数据的平均值，采用上述递推算式时，为实时获取μ，将得到的加权融合后的导航参数做为测量数据的μ。N为移动窗口长度。则DGPS接收机和机器视觉传感器的方差δ_p ²和δ_q ²分别为：

δ_p ²＝R_pp-R_pq    (33)

δ_q ²＝R_qq-R_pq    (34)

由公式(17)和(22)可得到融合后的导航参数

为：

$\hat{X}=\underset{p=1}{\overset{n}{\mathrm{\Σ}}}{W}_p{X}_p=\frac{{{\mathrm{\δ}}_q}^2{X}_p+{{\mathrm{\δ}}_p}^2{X}_q}{{{\mathrm{\δ}}_p}^2+{{\mathrm{\δ}}_q}^2}---\left(35\right)$

此时对应的最小均方误差为：

${{\mathrm{\σ}}_{\min }}^2=\frac{1}{\underset{p=1}{\overset{n}{\mathrm{\Σ}}}\frac{1}{{{\mathrm{\σ}}_p}^2}}=\frac{{{\mathrm{\δ}}_p}^2+{{\mathrm{\δ}}_q}^2}{{{\mathrm{\δ}}_p}^2{{\mathrm{\δ}}_q}^2}---\left(36\right)$

(3)多传感器自适应加权融合算法的实现：

首先判断DGPS接收机和机器视觉传感器是否均能正常工作：

1)如都不能正常工作则停止导航；

2)如只有一个正常工作，则判断其得到的定位参数是否在误差范围内，如果是则选择单一传感器导航方法，如果否则停止导航；

3)如果二者都能正常工作，仍需判断其定位数据是否在误差范围内：如两个定位数据均不在误差范围内则停止导航；如只有一个定位数据值在误差范围内则采用单一传感器导航方式；如两定位数据均在误差范围内，利用自适应加权融合估计的方法进行信息融合以提高定位的精度。

判断机器视觉传感器正常工作的标准是软件能清楚地采集图像，并对图像进行处理；判断DGPS接收机正常工作的标准是其能正常接收卫星信号，并保证能进行差分。利用设定阈值的方法判断定位数据是否满足误差范围，阈值的选择和农业机械的前轮间距有关。本实施例中农业机械的前轮间距为0.78m，因此设定误差范围为[-0.5m，0.5m]。

多传感器信息融合自适应加权融合算法不要求知道传感器测量数据的任何先验知识，只需传感器所提供的测量数据即可以自适应的方式，寻找各自所对应的最优加权因子，计算出融合后的数据，并可保证融合后总均方误差最小，使融合后的估计值达到最优，其算法流程图如图4所示。

4.三种定位方法的定位效果分析：

为验证本发明的融合多传感器信息的农业机械导航分级定位方法的稳定性，通过实验对定位效果进行测试，实验过程如下：

(1)选取一条东西方向的直线作为预定义路径，首先采用GPS4700接收机在预定义直线轨迹的两端点A，B处各进行30分钟静态定位，并对测量数据进行平均，取得两端点的精确定位结果，A点为(444729.280301，4429977.863371)，B点为(444759.627519，4429977.651654)，再利用解析几何方法获得该预定义路径在高斯投影平面坐标系中的几何描述；

(2)由于GPS4700的定位频率为1Hz，机器视觉传感器的定位频率约为10Hz，因此设定两传感器信息的融合频率为1Hz。实时纪录融合后的定位信息，将三种定位方法得到的定位信息进行比较，并通过统计学方法对数据进行分析；

(3)农业机械和预定义路径之间的位置偏差XTE是定位和导航精度的主要评价参数，获取准确的XTE数据是评价***精度的基础。在获取XTE参数的基础上，对XTE进行统计分析。选取XTE的平均值|X_XTE|，最大值|X_XTE|_max和标准差σ_XTE三项数据作为定位精度的评价指标。

实验结果分析：

首先对实验数据进行定性分析，然后通过统计学方法对实验数据进行定量分析。分析结果如表1所示。

表1

	机器视觉(m)	4700(m)	融合(m)
				|XXTE|	0.076999	0.044632	0.004417
|XXTE|max	3.51949	0.172502	0.112359
				σXTE	0.080704	0.034517	0.029885

从表1可看出融合后的统计量平均值，最大值和标准差均为最小。而且，融合算法去除了机器视觉定位中的跳变点，消除了可能出现误差的数据，提高了导航定位的稳定性和精度。同时，融合后的定位数据的波动最小，稳定性最好。

本发明实施例的一种农业机械导航分级定位的***如图5所示，该***包括DGPS接收机51、电子罗盘52、加速度计53、摄像装置54、目标定位参数获取装置55和定位装置56，其中目标定位参数获取装置55分别与DGPS接收机51、电子罗盘52、加速度计53、摄像装置54、定位装置56连接。

DGPS接收机51用于获取农业机械的第一位置值；电子罗盘52用于获取所述农业机械的第一航向角度值；加速度计53用于获取所述农业机械的加速度值；摄像装置54用于获取所述农业机械的第三位置值和第三航向角度值；目标定位参数获取装置55用于对所述第一位置值、第一航向角度值和当前行进速度值进行融合，获取第二定位参数，并将所述第二定位参数与所述第三定位参数进行融合，获取目标定位参数；定位装置56用于根据所述目标定位参数对所述农业机械进行定位。

目标定位参数获取装置55包括卡尔曼滤波器551和多传感器自适应加权融合单元552，其中卡尔曼滤波器551分别与DGPS接收机51、电子罗盘52、加速度计53连接，多传感器自适应加权融合单元552分别与摄像装置54和卡尔曼滤波器551连接。

卡尔曼滤波器551用于对所述第一位置值、第一航向角度值和当前行进速度值进行融合；多传感器自适应加权融合单元552用于将所述第二定位参数与所述第三定位参数进行融合。

本发明采用卡尔曼滤波器融合DGPS得到的农业机械的当前位置值和电子罗盘得到的航向角度值及通过对加速度计得到的初始值积分得到的速度值，可平滑DGPS定位数据，有效避免DGPS动态定位的异常结果。另外，本发明提出采用多传感器自适应加权融合算法将机器视觉提供的位置值和航向角度值与初次融合得到相对精确的农业机械的位置和航向角度信息进行再次融合，获得最优的农业机械位置值和航向角度值。进一步，本发明的农业机械导航分级定位方法，将各个传感器的测量数据进行多次融合，可以有效滤除测试噪声、减小***误差，能够形成连续、稳定、相对精确的农业机械的位置和航向角度信息

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1、 一种农业机械导航分级定位的方法，其特征在于，所述方法包括以下步骤：

获取第一定位参数，所述第一定位参数包括通过差分全球定位***DGPS接收机获取的农业机械的第一位置值，通过电子罗盘获取的所述农业机械的第一航向角度值，通过对从加速度计得到的加速度值进行积分而获取的所述农业机械的当前行进速度值；

利用卡尔曼滤波器对所述第一位置值、第一航向角度值和当前行进速度值进行融合，获取第二定位参数，所述第二定位参数包括所述农业机械的第二位置值和第二航向角度值；

获取第三定位参数，所述第三定位参数包括通过摄像装置获取的所述农业机械的第三位置值和第三航向角度值；

利用多传感器自适应加权融合算法将所述第二定位参数与所述第三定位参数进行融合，获取目标定位参数，所述目标定位参数包括所述农业机械的目标位置值和目标航向角度值；

根据所述目标定位参数对所述农业机械进行定位。

2、 如权利要求1所述农业机械导航分级定位的方法，其特征在于，在所述利用卡尔曼滤波器对第一位置值、第一航向角度值和当前行进速度值进行融合，获取第二定位参数之前，还包括构建卡尔曼滤波器，所述构建卡尔曼滤波器的步骤具体包括：

建立基于常速度的卡尔曼滤波器的状态方程和观测方程；

根据所述状态方程和观测方程，建立预测方程组和校正方程组；

根据所述预测方程组和校正方程组构建卡尔曼滤波器。

3、 如权利要求2所述农业机械导航分级定位的方法，其特征在于，所述状态方程为：

$\hat{X}\left(t\right|t-1)=\mathrm{\Φ}(t-1)\hat{X}(t-1|t-1)$

其中，

为t时刻农业机械的运动状态，Φ(t-1)为t-1时刻状态转移矩阵，

为t-1时刻农业机械的运动状态；

所述观测方程为：

Z(t)＝H(t)X(t)+W(t)

其中，Z(t)为外部观测向量，H(t)为外部观测向量和状态向量之间的测量矩阵，X(t)为t时刻农业机械的状态向量，W(t)为高斯白噪声序列。

4、 如权利要求2所述农业机械导航分级定位的方法，其特征在于，所述预测方程组为：

$\left[\begin{array}{c}\hat{X}\left(t\right|t-1)=\mathrm{\Φ}(t-1)\hat{X}(t-1|t-1)\\ P\left(t\right|t)=\mathrm{\φ}\left(t\right|t)P(t-1|t-1){\mathrm{\φ}}^T\left(t\right|t)+G(t-1)Q(t-1)G^T(t-1)\end{array}\right]$

其中，

为t时刻农业机械的运动状态，Φ(t-1)为t-1时刻状态转移矩阵，

为t-1时刻农业机械的运动状态，P(t|t)为t时刻滤波误差方差阵，φ(t|t)为t时刻状态转移矩阵，P(t-1|t-1)为t-1时刻滤波误差方差阵，φ^T(t|t)为t时刻状态转移矩阵的转置矩阵，G(t-1)为t-1时刻过程噪声分布矩阵，Q(t-1)为t-1时刻正定协方差矩阵，G^T(t-1)为t-1时刻过程噪声分布矩阵的转置矩阵；

所述校正方程组为：

$\left\{\begin{array}{c}\hat{X}\left(t\right|t)=\hat{X}\left(t\right|t-1)+P\left(t\right|t)H^T\left(t\right)R^{-1}\left(t\right)\·[Z\left(t\right)-H\left(t\right)\hat{X}\left(t\right|t-1)]\\ P^{-1}\left(t\right|t)=P^{-1}\left(t\right|t-1)+H^T\left(t\right)R^{-1}\left(t\right)H\left(t\right)\end{array}\right.$

其中，为t时刻农业机械的运动状态，

为t-1时刻农业机械的运动状态，P(t|t)为t时刻滤波误差方差阵，H(t)为t时刻外部观测向量和状态向量之间的测量矩阵，H^T(t)为t时刻外部观测向量和状态向量之间的测量矩阵的转置矩阵，R^-1(t)为t时刻正定协方差矩阵的逆矩阵，Z(t)为t时刻外部观测向量，P^-1(t|t)为t时刻滤波误差方差阵的逆矩阵，P^-1(t|t-1)为t-1时刻滤波误差方差阵。

5、 如权利要求2至4任一项所述农业机械导航分级定位的方法，其特征在于，在所述卡尔曼滤波器中，利用DGPS接收机得到的第一位置值对所述状态转移矩阵进行实时更新。

6、 如权利要求2至4任一项所述农业机械导航分级定位的方法，其特征在于，所述利用卡尔曼滤波器对第一位置值、第一航向角度值和当前行进速度值进行融合，具体包括：

利用所述第一位置值、第一航向角度值和当前行进速度值更新所述预测方程组的矩阵；

通过所述预测方程组获取下一时刻的状态估计；

利用所述校正方程组对所述下一时刻的状态估计进行校正，获取当前最优估计；

根据所述当前最优估计，获取当前最优估计的误差协方差矩阵。

7、 如权利要求1所述农业机械导航分级定位的方法，其特征在于，在所述利用多传感器自适应加权融合算法将第二定位参数与第三定位参数进行融合，获取目标定位参数之前，还包括以下步骤：

判断所述DGPS接收机和摄像装置是否均能正常工作，

如果所述DGPS接收机和摄像装置都不能正常工作，则停止导航；

如果所述DGPS接收机和摄像装置中只有一个能正常工作，则判断能正常工作的装置得到的定位参数是否在误差范围内，如果是则采用单一传感器进行导航，否则停止导航；

如果所述DGPS接收机和摄像装置都能正常工作，则判断能正常工作的装置得到的定位参数是否在误差范围内，

如果所述DGPS接收机和摄像装置得到的定位参数都不在误差范围内，则停止导航；

如果所述DGPS接收机和摄像装置得到的定位参数中只有一个在误差范围内，则采用单一传感器进行导航；

如果所述DGPS接收机和摄像装置得到的定位参数都在误差范围内，则利用多传感器自适应加权融合算法将所述DGPS接收机得到的第二定位参数和摄像装置得到的第三定位参数进行融合。

8、 如权利要求1所述消息的读写方法，其特征在于，所述利用多传感器自适应加权融合算法将第二定位参数与第三定位参数进行融合，具体包括：

根据各测量设备获取的定位参数的方差，获取各测量设备的最优加权因子，所述最优加权因子由公式

$W_p=\frac{1}{{\mathrm{\σ}}_p^2\underset{i=0}{\overset{n}{\mathrm{\Σ}}}\frac{1}{{\mathrm{\σ}}_i^2}}$ (p＝1，2，…n)

获取；其中，W_p为最优加权因子，σ²为各测量设备获取的定位参数的方差；

根据公式

$\hat{X}=\frac{{\mathrm{\δ}}_q^2{X}_p+{\mathrm{\δ}}_p^2{X}_q}{{\mathrm{\δ}}_p^2+{\mathrm{\δ}}_q^2}$

获取融合后的定位参数；其中，为融合后的定位参数，δ_q ²为摄像装置获取的定位参数的方差，X_p为DGPS接收机得到的定位参数值，δ_p ²为DGPS接收机获取的定位参数的方差，X_q为摄像装置得到的定位参数值。

9、 一种农业机械导航分级定位的***，其特征在于，所述***包括：

DGPS接收机，用于获取农业机械的第一位置值；

电子罗盘，用于获取所述农业机械的第一航向角度值；

加速度计，用于获取所述农业机械的加速度值；

摄像装置，用于获取所述农业机械的第三位置值和第三航向角度值；

目标定位参数获取装置，用于对所述第一位置值、第一航向角度值和当前行进速度值进行融合，获取第二定位参数，并将所述第二定位参数与所述第三定位参数进行融合，获取目标定位参数；

定位装置，用于根据所述目标定位参数对所述农业机械进行定位。

10、 如权利要求9所述农业机械导航分级定位的***，其特征在于，所述目标定位参数获取装置包括：

卡尔曼滤波器，用于对所述第一位置值、第一航向角度值和当前行进速度值进行融合；

多传感器自适应加权融合单元，用于将所述第二定位参数与所述第三定位参数进行融合。

Images