CN101082493B - 一种农业机械导航的组合定位方法 - Google Patents

Abstract

本发明公开了一种农业机械导航的组合定位方法。该方法首先采用伪距差分GPS传感器测量得到农业机械的初步定位位置值；然后采用电子罗盘和微机械陀螺传感器对农业机械的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值；再采用自适应卡尔曼滤波器对包括初步定位位置值和航向角度值的多源信息进行再次融合，获得农业机械定位和航向角度信息的精确估计。本发明可为导航控制提供更为精确和可靠的航向角度估计数据，定位精度可控制在亚米级范围内，而且可以形成连续、稳定的农业机械导航数据。

Description

一种农业机械导航的组合定位方法

技术领域

本发明涉及导航定位技术，特别涉及一种农业机械导航的组合定位方法。

背景技术

精细农业包括定位处方农作和农情信息自动采集，其中定位处方农作要求农业机械可以按照预先规划好的路径在田间行走，准确到达目的地并完成既定作业任务。精确导航是实现农业机械自主行走的关键技术之一，其定位精度直接影响农业机械进行路径自动跟踪的质量。因此，提高导航定位的精度，是改善农业机械路径跟踪质量的首要问题。

农业机械导航定位的主要信息包括：位置信息和航向角度信息。在以GPS为主的导航定位技术方面，已有研究成果主要采用RTK-DGPS和FOG实现农业机械的精确定位，其成本较高。也有一些研究成果采用低价格低精度GPS融合高精度惯性传感器，通过卡尔曼滤波方法实现农业机械的精确定位；目前这类研究还较少，而且现有的研究忽视卡尔曼滤波的自适应问题，难以避免滤波发散问题。

发明内容

本发明的目的在于克服现有技术中存在的缺点，提供一种高精度的农业机械导航的组合定位方法，将多个导航传感器信息经过融合处理后，得到农业机械更为精确的当前位姿信息。

本发明一种农业机械导航的组合定位方法，通过下述步骤实现：(1)首先采用伪距差分GPS传感器测量得到农业机械的初步定位位置值；(2)然后采用电子罗盘和微机械陀螺传感器对农业机械的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值；(3)再采用自适应卡尔曼滤波器对包括初步定位位置值和航向角度值的多源信息进行再次融合，获得农业机械定位和航向角度信息的精确估计。

步骤2中，所述推算得到最优的航向角度值的具体步骤如下：首先电子罗盘传感器对农业机械的航向角度进行测量，微机械陀螺传感器对农业机械的横向角速率进行测量，经积分后转换为航向角度测量值；然后采用基于协方差函数的加窗估计算法，在线估计电子罗盘和微机械陀螺的测量方差；再根据上述在线估计的测量方差，采用自适应加权融合估计算法以自适应的方式推算出两个传感器所对应的最优加权因子，最后推算得到最优的航向角度值。

步骤2中，采用最优估计值周期性地更新微机械陀螺传感器横向角速率的积分初值，以防止积分发散。

步骤3中，所述对多源信息进行再次融合的具体步骤如下：首先利用航位推算的基本原理建立常速度条件下的卡尔曼滤波器的状态方程和观测方程；然后基于新息的理论协方差阵概念，设计一种测量噪声协方差矩阵R(k)的自适应调节方法；最后，在卡尔曼滤波器状态方程和观测方程的基础上，结合测量噪声协方差矩阵R(k)的在线调节方法，建立预测方程组和校正方程组，构建卡尔曼滤波器，实现农业机械定位和航向角度信息的精确估计。

步骤3中，所述的常速度卡尔曼滤波器模型中，状态空间和测量向量均为GPS接收机的位置数据(x，y)和速度数据v，航向角度估计数据对状态转移矩阵进行实时更新。

步骤3中，所述R矩阵的自适应调节中，主要采用新息的实际协方差阵Pr(k)与理论协方差矩阵Cr(k)进行对比的方法，在线决策R矩阵的调节量。

所述伪距差分GPS传感器是指差分全球定位系统(DGPS)接收机。所述电子罗盘是指磁航向传感器电子罗盘。所述微机械陀螺传感器是指基于MEMS技术的低成本微机械陀螺。

本发明与现有技术相比具有如下优点和效果：

(1)本发明提出了农业机械航向角度的自适应加权融合估计算法，通过对电子罗盘和微机械陀螺信息的自适应加权融合处理，可为导航控制提供更为精确和可靠的航向角度估计数据。

(2)本发明采用自适应卡尔曼滤波器可抑制发散，平滑DGPS定位数据，有效避免DGPS动态定位的异常结果，定位精度可控制在亚米级范围内。

(3)本发明的导航组合定位方式，将各个传感器的测量数据进行多次融合，既平滑又可以滤除测试噪声，形成了连续、稳定的农业机械导航数据。

附图说明

图1为农业机械导航的组合定位系统组成示意图。

图2为卡尔曼滤波器的数据流向图。

图3为完全DGPS条件下Kalman滤波器直线跟踪仿真结果。

图4为由DGPS到GPS条件下Kalman滤波器直线跟踪仿真结果。

图5为完全DGPS条件下Kalman滤波器曲线跟踪仿真结果。

图6为由DGPS到GPS条件下Kalman滤波器曲线跟踪仿真结果。

具体实施方式

下面结合实施例对本发明做进一步详细的描述，但本发明的实施方式不限于此。

本实施例采用的农业机械为久保田插秧机，其导航定位系统主要包括：导航传感器组合、便携计算机和USB-RS232数据传输线等。导航传感器组合包括伪距差分GPS、电子罗盘和微机械陀螺。伪距差分GPS可以采集插秧机在WGS-84坐标系下的定位坐标，作为初步定位信息；电子罗盘测量其航向角度信息(以正北方向为0，逆时针方向为正)；微机械陀螺测量其横向角速率(逆时针方向为正)。便携计算机实现数据采集、数据预处理和信息融合定位软件系统的运行。便携计算机与导航传感器组合通过多根USB-RS232数据传输线建立连接。将上述多个传感器组合安装在插秧机后端的台架上，然后用数据采集软件系统进行动态数据采集，采集频率是1Hz。数据采集、数据预处理和信息融合定位软件系统采用Visual C++编程工具在Windows XP操作系统下开发完成。导航定位系统结构框图如图1。

所述插秧机导航定位系统所获得的导航数据以伪距差分GPS定位数据为主，电子罗盘和微机械陀螺的航向角度数据为辅。为了获得插秧机准确的位置及航向估计，本发明通过两级融合的方法，逐次对原始航向角度数据和GPS定位数据进行融合。

插秧机的具体组合定位方法：(1)首先采用伪距差分GPS传感器测量得到插秧机的初步定位位置值；(2)然后采用电子罗盘和微机械陀螺传感器对插秧机的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值，实现一级融合；(3)再采用自适应卡尔曼滤波器对包括初步定位位置值和航向角度值的多源信息进行二级融合，获得插秧机定位和航向角度信息的精确估计。

具体步骤包括：

1、采用伪距差分GPS传感器测量得到插秧机的初步定位位置值；

GPS接收机输出的经纬度坐标是WGS-84地心大地坐标。为了使DGPS定位数据能用于导航控制系统，需要将WGS-84地心大地坐标转换为对应于WGS-84椭球的高斯平面坐标，即需进行高斯投影变换。

本发明采用如下公式：

$x=X+\frac{l^2}{2}N\sin B\cos B+\frac{l^2}{24}N\sin B{\cos }^{3}B(5-t^2+9{\mathrm{\η}}^2+4{\mathrm{\η}}^4)+\frac{l^6}{720}N\sin B\cos 5B(61-58t^2+t^4)$

$y=\mathrm{lN}\cos B+\frac{l^3}{6}N{\cos }^{3}B(1-t^2+{\mathrm{\η}}^2)+\frac{l^5}{120}N{\cos }^{5}B(5-18t^2+t^4+14{\mathrm{\η}}^2-58{\mathrm{\η}}^2{t}^2\mathrm{})+500000$

式中，l＝L-L₀，L₀为投影带中央子午线经度。卯酉圈曲率半径 $N=\frac{a}{\sqrt{1-e^2{\sin }^{2}B}},$ 椭球第一偏心率 $e=2\∂-{\∂}^2,$ 辅助变量t＝tanB，辅助变量η＝e′cosB，椭球第二偏心率 $e\′=\sqrt{a^2/b^2-1},$ a、b分别为参考椭球的长、短半径，扁率 $\∂=(a-b)/a,$ X为赤道至纬度为B的平行圈的子午线弧长，其计算公式为：

$X=c{\∫}_0^B(1+e\′2\cos 2B-2/3)\mathrm{dB}$

c为极曲率半径。

本发明所用投影坐标系的主要参数为：

1)投影方式：Gauss-Kruger

2)中央经线：114.000000(3度带)

3)水平偏移量：500km

4)地理坐标系：GCS_WGS_1984

5)大地参照系：D_WGS_1984

6)参考椭球体：WGS_1984

7)椭球长轴：6378137.000000

8)椭球扁率：0.0033528107

高斯投影变换后的WGS-84椭球的高斯平面坐标可作为插秧机的初步定位位置值。

2、采用电子罗盘和微机械陀螺传感器对插秧机的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值，实现一级融合；

设定电子罗盘和微机械陀螺的航向角度测量值分别为X_p和X_q，对应的观测误差分别为：V_p和V_q，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)。可通过如下步骤，完成一级融合：

1)前N次采样时的R_pp、R_qq和R_pq的时间域估计值可由下述递推公式获得：

当k<N时，

$R_{\mathrm{pp}}\left(k\right)=\frac{k-1}{k}{R}_{\mathrm{pp}}(k-1)+\frac{1}{k}(X_p-\mathrm{\&mu;})(X_p-\mathrm{\&mu;})$

$R_{\mathrm{qq}}\left(k\right)=\frac{k-1}{k}{R}_{\mathrm{qq}}(k-1)+\frac{1}{k}(X_q-\mathrm{\&mu;})(X_q-\mathrm{\&mu;})$

$R_{\mathrm{pq}}\left(k\right)=R_{\mathrm{qp}}\left(k\right)=\frac{k-1}{k}{R}_{\mathrm{pq}}(k-1)+\frac{1}{k}(X_p-\mathrm{\&mu;})(X_q-\mathrm{\&mu;})$

当k>N时，

$R_{\mathrm{pp}}\left(k\right)=\frac{N-1}{N}{R}_{\mathrm{pp}}(k-1)+\frac{1}{N}(X_p-\mathrm{\&mu;})(X_p-\mathrm{\&mu;})$

$R_{\mathrm{qq}}\left(k\right)=\frac{N-1}{N}{R}_{\mathrm{qq}}(k-1)+\frac{1}{N}(X_q-\mathrm{\&mu;})(X_q-\mathrm{\&mu;})$

$R_{\mathrm{pq}}\left(k\right)=R_{\mathrm{qp}}\left(k\right)=\frac{N-1}{N}{R}_{\mathrm{pq}}(N-1)+\frac{1}{N}(X_p-\mathrm{\&mu;})(X_q-\mathrm{\&mu;})$

式中μ为采样数据的均值，在采用上述递推算式时，为了实时获取μ，将后续加权融合算法中的航向角度一步预测值作为测量数据的μ。N为移动窗口长度。

2)由下式可以获得电子罗盘和微机械陀螺的测量方差δ_p ²、δ_q ²：

δ_p ²＝R_pp-R_pq

δ_q ²＝R_qq-R_pq

3)融合后的航向角度数据

可由下式获得：

$\hat{X}=\frac{X_p}{{{\mathrm{\&delta;}}_p}^2(\frac{1}{{{\mathrm{\&delta;}}_q}^2}+\frac{1}{{{\mathrm{\&delta;}}_q}^2})}+\frac{X_q}{{{\mathrm{\&delta;}}_q}^2(\frac{1}{{{\mathrm{\&delta;}}_p}^2}+\frac{1}{{{\mathrm{\&delta;}}_q}^2})}=\frac{{{\mathrm{\&delta;}}_q}^2{X}_p+{{\mathrm{\&delta;}}_p}^2{X}_p}{{{\mathrm{\&delta;}}_p}^2+{{\mathrm{\&delta;}}_q}^2}$

3、采用自适应卡尔曼滤波器对包括初步定位位置值和航向角度值的多源信息进行二级融合，获得插秧机定位和航向角度信息的精确估计。

首先依据航位推算原理建立自适应卡尔曼滤波器状态转移方程和观测方程。

θ_k为插秧机方位角，即插秧机纵向方向与x轴正向之间的夹角；v为插秧机在纵向方向上的前进速度；(x_k，y_k)为插秧机在k时刻的坐标，(x_k+1，y_k+1)为插秧机在k+1时刻的坐标；T为采样周期。采用下述递推关系式推算插秧机的航位：

x_k+1＝x_k+Tvcosθ_k

y_k+1＝y_k+Tvsinθ_k

根据卡尔曼滤波器的基本状态转移方程X(k+1)＝φ(k)X(k)+W(k)，定义状态空间向量X(k)＝[x(k)y(k)v(k)]，其中x(k)，y(k)为WGS-84坐标系下高斯投影平面坐标的东向和北向坐标，v(k)为插秧机纵向方向上的行进速度，θ(k)为插秧机纵向方向在高斯投影平面坐标下的角度分量，横轴正向为0，逆时针为正。

状态转移矩阵定义为：

$\mathrm{\&Phi;}\left(k\right)=\left[\begin{array}{ccc}1& 0& T\cos \left(\mathrm{\&theta;}(k-1)\right)\\ 0& 1& T\sin \left(\mathrm{\&theta;}(k-1)\right)\\ 0& 0& 1\end{array}\right]$

采用上一时刻航向角度和转向角度的测量值的一级融合结果，利用经坐标转换后的航向角度估计值对Φ(k)矩阵进行实时更新。W(k)为离散时间白噪声序列，系统过程的噪声协方差矩阵为Q(k)。Q(k)设定为常数矩阵，在仿真和实验过程中整定矩阵参数。

通过伪距差分GPS传感器测量得到插秧机的位置(x，y)以及前进速度V_b。外部测量向量为z(k)＝[x(k)y(k)v(k)]，测量的噪声方差阵R为：

$R\left(k\right)=\left[\begin{array}{ccc}{r_1}^2& & \\ & {r_2}^2& \\ & & {r_3}^2\end{array}\right]$

其中，r₁、r₂、r₃分别表示GPS接收机x向、y向定位以及速度测量噪声。

然后采用线性离散Kalman滤波器的递归差分方程进行预测和校正：

预测方程组为：

$\hat{x}\left(k\right|k-1)=\mathrm{\&Phi;}(k-1)\hat{x}(k-1)$

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

校正方程组为：

K_k＝P(k|k-1)H^T(HP(k|k-1)H^T+R(k))^-1

$\hat{x}\left(k\right)=\hat{x}\left(k\right|k-1)+K_k(Z\left(k\right)-H\hat{x}\left(k\right|k-1\left)\right)$

P(k)＝(I-K_kH)P(k|k-1)

在本发明中，使用在线自适应调节后的测量噪声方差阵R，计算校正方程组的增益矩阵。具体自适应调节方法如下：

采用下述方程式来推算残差或新息，即k时刻传感器的实际观测值与预测值之间的差值： $r\left(k\right)=z\left(k\right)-H\left(k\right)\hat{x}\left(k\right|k-1)$

上述残差或新息的实际协方差公式为：

P_r(k)＝H(k)[Φ(k-1)P(k-1)Φ^T(k-1)+Q(k-1)]H^T(k)+R(k)

上述残差或新息的理论协方差公式为：

$\mathrm{Cr}\left(k\right)=\frac{1}{N}\underset{i=i0}{\overset{k}{\mathrm{\&Sigma;}}}{r}_i{r}_i^T,i_0=k-N+1$

其中N为估计窗的大小。

如果上述推算出来的新息的实际协方差阵Pr(k)与理论协方差阵Ct(k)的计算结果近似相等，则说明当前测量噪声协方差矩阵R(k)的估计是准确的。如果Pr(k)与Ct(k)的计算结果相差较大，则认为当前R(k)的估计结果需要根据其差别进行修正，若Pr(k)偏大，表明当前R(k)偏大，应适当减小，若Pr(k)偏小，表明当前R(k)偏小，应适当增大。

因此，采用下述公式对R(k)进行自适应调整：

$R\left(k\right)=\left\{\begin{array}{cc}R\left(k\right)+\mathrm{\&lambda;R}\left(k\right)& -\mathrm{TrL}\&le;\mathrm{DOM}\left(k\right)\&le;\mathrm{TrL}\\ R\left(k\right)& -\mathrm{TrL}\&le;\mathrm{DOM}\left(k\right)\&le;\mathrm{TrL}\\ R\left(k\right)-\mathrm{\&lambda;R}\left(k\right)& \mathrm{DOM}\left(k\right)>\mathrm{TrL}\end{array}\right.$

上式中，λ是测量噪声协方差矩阵R(k)的调节系数，TrL是Pr(k)与Cr(k)差别程度的界定常数。定义DOM(k)＝Tr(Pr(k)-Cr(k))，通过两个协方差阵的差值矩阵的迹来表示其差别程度。

Kalman滤波器工作的数据流向图如图2所示。将初始状态估计X和初始滤波误差方差阵P估计引入Kalman滤波器的预测方程组中，启动Kalman滤波。先由自适应加权融合算法对航向角度信息进行估计，并用估计结果更新预测方程组的矩阵，再由预测方程组对下一时刻状态进行估计。预测过程结束，启动Kalman滤波的校正过程，先对R矩阵进行自适应调节，然后引入校正方程组，解算增益矩阵，并根据当前测量结果(针对预测方程组对下一时刻状态的估计结果)，对预测方程组的状态估计进行更新，获得当前最优估计，最后解算当前最优估计的误差协方差矩阵，完成一次滤波过程。

测试例：对本发明方位的定位效果进行仿真测试：

采用人工驾驶插秧机以恒定的前进速度在预定的各种路线上精确跟踪行走。分别对其中的一条直线和一条圆曲线进行了多次数据采集。

选择4组数据进行滤波器仿真。这4组数据中，两组在曲线上，两组在直线上。曲线上的两组数据中，一组是完全DGPS定位的数据，一组是先有DGPS定位的数据，再有GPS定位的数据(DGPS差分信号中断的情况下)。直线上的两组数据亦是如此。

设滤波器的初始条件是：

$P\left(0\right)=\left[\begin{array}{ccc}4.0& & \\ & 4.0& \\ & & 0.5\end{array}\right]$

$R\left(0\right)=\left[\begin{array}{ccc}0.8& & \\ & 0.8& \\ & & 0.01\end{array}\right]$

$Q\left(0\right)=\left[\begin{array}{ccc}0.03& & \\ & 0.03& \\ & & 0.002\end{array}\right]$

N＝15，TrL＝1.1，λ＝0.1，RESIDUAL＝0.8，利用Kalman滤波器仿真程序读取的第一行文本文件数据对状态空间向量进行初始化。

对于直线跟踪，得到如图3，4所示的仿真结果。对于曲线跟踪，得到如图5，6所示的仿真结果。在完全DGPS条件下，从图3、4可看出，滤波器对AB处原始DGPS定位的跳变起到了较好的平滑作用，定位精度有所改善；在由DGPS转变为GPS定位的情况下，在图5、6中的AB处，滤波器依靠航位推算定位，避免了DGPS的异常定位数据，但航位推算的累积误差随时间逐渐增大。这主要是由于GPS定位误差比较大，滤波器的一致性判据始终不能满足，长时间处于航位推算模式造成的。

横向跟踪误差(XTE)参数指标是当前DGPS测量定位点到预定义路线的距离。仿真实验以该参数指标评价Kalman滤波效果。从Kalman滤波仿真曲线和滤波前后横向跟踪误差的统计结果(表1)可以看出，Kalman滤波器对定位结果具有一定的平滑和稳定作用，XTE误差得到减小。无论直线跟踪还是圆曲线跟踪，在GPS差分信号丢失的情况下，可有效避免GPS动态定位的异常结果，最大XTE误差和平均XTE误差大大减小，提高了DGPS定位的质量。在完全DGPS定位情况下，该Kalman滤波器对DGPS定位精度可控制在亚米级范围内，最大XTE误差和平均XTE误差在一定程度上得到减小。

表1 滤波前后横向跟踪误差(XTE)统计结果。

Claims (5)

1.一种农业机械导航的组合定位方法，其特征在于包括下述步骤：

(1)首先采用伪距差分GPS传感器测量得到农业机械的初步定位位置值；

(2)然后采用电子罗盘和微机械陀螺传感器对农业机械的航向角度进行测量，并通过自适应加权融合，推算得到最优的航向角度值；

(3)再采用自适应卡尔曼滤波器对包括初步定位位置值和最优的航向角度值的多源信息进行再次融合，获得农业机械定位和航向角度信息的精确估计。

2.根据权利要求1所述的农业机械导航的组合定位方法，其特征在于：所述推算得到最优的航向角度值是：首先电子罗盘传感器对农业机械的航向角度进行测量，微机械陀螺传感器对农业机械的横向角速率进行测量，经积分后转换为航向角度测量值；然后采用基于协方差函数的加窗估计算法，在线估计电子罗盘和微机械陀螺传感器的测量方差；再根据上述在线估计的测量方差，采用自适应加权融合估计算法以自适应的方式推算出两个传感器所对应的最优加权因子，最后推算得到最优的航向角度值。

3.根据权利要求1所述的农业机械导航的组合定位方法，其特征在于：所述对多源信息进行再次融合的是：首先利用航位推算的基本原理建立常速度条件下的卡尔曼滤波器的状态方程和观测方程；然后基于新息的理论协方差阵概念，设计一种测量噪声协方差矩阵R(k)的自适应调节方法；最后，在卡尔曼滤波器状态方程和观测方程的基础上，结合测量噪声协方差矩阵R(k)的在线调节方法，建立预测方程组和校正方程组，构建卡尔曼滤波器，实现农业机械定位和航向角度信息的精确估计。

4.根据权利要求1或3所述的农业机械导航组合定位方法，其特征在于：所述的卡尔曼滤波器中，状态空间和测量向量均为伪距差分GPS传感器的位置数据(x，y)和速度数据v，最优的航向角度值对状态转移矩阵进行实时更新。

5.根据权利要求3所述的农业机械导航的组合定位方法，其特征在于：所述测量噪声协方差矩阵R(k)自适应调节方法，是采用新息的实际协方差阵Pr(k)与理论协方差矩阵Cr(k)进行对比的方法，在线决策R(k)矩阵的调节量。

Images