发明内容
本发明的目的是克服了上述现有技术的缺点,提供了一种能够实现结合DR算法的实时性和GPS精度高的优点、克服DR算法的积累误差和GPS实施性差的缺陷、具有更广泛应用范围的基于卫星导航和航位推测实现农业机械定位的***及方法。
为了实现上述目的,本发明的基于卫星导航和航位推测实现农业机械定位的***及方法具有如下构成:
该基于卫星导航和航位推测实现农业机械定位的***,其主要特点是,所述的***包括:
卫星导航模块,用以获取所述的农业机械的地理位置信息;
航位推测模块,用以采集所述的农业机械的相对移动位移和航向角进行航位推测计算;
导航计算机模块,用以结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。
较佳地,所述的卫星导航模块为GPS导航模块,所述的航位推测模块包括里程计单元和电子罗盘单元。
本发明还涉及一种通过所述的***基于卫星导航和航位推测实现农业机械定位的方法,其主要特征在于,所述的方法包括以下步骤:
(1)所述的卫星导航模块获取所述的农业机械初始地理位置信息;
(2)所述的航位推测模块采集所述的农业机械的相对移动位移和航向角进行航位推测计算;
(3)所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正;
(4)所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位。
较佳地,所述的卫星导航模块获取所述的农业机械初始地理位置信息,具体为:
当所述的农业机械静止时,所述的卫星导航模块获取所述的农业机械初始的地理位置坐标。
更佳地,所述的航位推测模块包括里程计单元和电子罗盘单元,所述的航位推测模块采集所述的农业机械的相对移动位移和航向角进行航位推测计算,包括以下步骤:
(2-1-1)所述的电子罗盘单元采集地磁场信息和车体的翻滚角θ和俯仰角φ;
(2-1-2)所述的电子罗盘单元根据以下公式对采集的数据进行倾角补偿:
Xh1=X*cos(φ)+Y*sin(θ)sin(φ)-Z*cos(θ)*sin(φ)
Yh1=Y*cos(θ)+Z*sin(θ);
所述的电子罗盘单元根据以下公式对采集的数据进行圆心修正:
Xh2=Xh1-Xoffset
Yh2=Yh1-Yoffset;
所述的电子罗盘单元根据以下公式对采集的数据进行椭圆修正:
Xh=Xh'/σ
所述的电子罗盘单元根据以下公式进行航向角计算:
ψ1=-1*atan2(Yh,Xh)
if(ψ1<0)
ψ=ψ1+2π
if(ψ1≥0)
ψ=ψ1
其中,X、Y、Z为电子输出的三轴磁场强度,θ为翻滚角,φ为俯仰角,Xh、Yh为修正后的磁场强度,ψ为航向角,Xoffset,Yoffset为圆心修正参数,α为椭圆修正参数,σ为由X和Y轴磁场强度最大值和最小值(XMax-XMin)/(YMax-YMin)计算出来的比率。
更进一步地,所述的步骤(1)和(2-1-1)之间,还包括以下步骤:
(2-0-1)所述的农业机械行驶***预设路程后并采集电子罗盘数据和进行硬磁和软磁修正参数计算;
(2-0-2)所述的农业机械继续行驶***预设路程后判断校准是否成功并保存成功的硬磁和软磁修正参数。
更佳地,所述的航位推测模块通过所述的农业机械车体航向角和车体的运行速度获得车体的坐标值,具体为:
所述的航位推测模块根据以下公式计算得到车体的坐标值:
其中,(x0,y0)农业机械初始的地理位置坐标,(xk,yk)为计算得到的车体的坐标值;vi为ti时刻的车体的运行速度,ψi为ti时刻的车体航向角。
较佳地,所述的卫星导航模块为GPS导航模块,所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正,包括以下步骤:
(3-1)所述的卫星导航模块获取所述的农业机械当前地理位置坐标;
(3-2)所述的卫星导航模块根据以下公式对农业机械当前地理位置坐标进行修正:
其中, 为修正后的定位数据; 为转换到导航坐标系的GPS测量的位置信息;a、b、h分别为GPS天线在机体坐标系中的投影;θr为机体翻滚角;φp为机体俯仰角;ψ为机体航向角。
较佳地,所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位,包括以下步骤:
(4-1)当农业机械以恒速运动时,取组合导航定位的状态分量为X=[xe,ve,xn,vn]T,其中xe、ye分别为车体东向和北向的位置分量;ve、vn分别为车体东向和北向的速度分量,T为***周期,进行离散化后得到状态方程和观测方程为:
其中, V为零均值、协方差阵为R的白噪声,且与W互不相关;W与V在两坐标方向上的加速度相互独立并具有相同的方差σ2,X为***模型的状态向量,Z为观测向量(x,y)位置坐标,W为***模型噪声矩阵,是东向和北向;
(4-2)所述的导航计算机模块输出所述的农业机械定位的位置信息和航向角。
采用了该发明中的基于卫星导航和航位推测实现农业机械定位的***及方法,具有如下有益效果:
(1)本发明采用了里程计能够代替IMU传感器,降低成本和开发难度;
(2)本发明采用的卡尔曼数据融合技术,充分结合了DR算法的实时性和GPS精度高的优点,克服了DR算法的积累误差和GPS实时性差的缺陷,使二者得到了很好的互补;
(3)本发明通过对电子罗盘一系列校准和补偿算法,获得较高精度的车体航向角,为DR算法和后期的控制算法提供一个精确的数据输入;
(4)本发明采用的姿态修正算法可以解决农业机械在作业过程中由于路况复杂而引起的车身倾斜造成的误差;
(5)本发明具有较高精度和实时性,尤其适用于农业机械辅助驾驶控制***中数据获取和处理中,具有更广泛的应用范围。
具体实施方式
为了能够更清楚地描述本发明的技术内容,下面结合具体实施例来进行进一步的描述。
本发明针对农业机械辅助驾驶控制***位置信息的获取中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;
所述的电子罗盘单元根据以下公式对采集的数据进行椭圆修正:
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)所述的农业机械继续行驶***预设路程后判断校准是否成功并保存成功的硬磁和软磁修正参数。
在一种更佳的实施方式中,所述的航位推测模块通过所述的农业机械车体航向角和车体的运行速度获得车体的坐标值,具体为:
所述的航位推测模块根据以下公式计算得到车体的坐标值:
其中,(x0,y0)农业机械初始的地理位置坐标,(xk,yk)为计算得到的车体的坐标值;vi为ti时刻的车体的运行速度,ψi为ti时刻的车体航向角。
在一种较佳的实施方式中,所述的卫星导航模块为GPS导航模块,所述的卫星导航模块获取所述的农业机械当前地理位置坐标并进行修正,包括以下步骤:
(3-1)所述的卫星导航模块获取所述的农业机械当前地理位置坐标;
(3-2)所述的卫星导航模块根据以下公式对农业机械当前地理位置坐标进行修正:
其中, 为修正后的定位数据; 为转换到导航坐标系的GPS测量的位置信息;a、b、h分别为GPS天线在机体坐标系中的投影;θr为机体翻滚角;φp为机体俯仰角;ψ为机体航向角。
在一种较佳的实施方式中,所述的导航计算机模块结合所述的卫星导航模块、导航计算机模块采用卡尔曼数据融合算法对所述的农业机械进行定位,包括以下步骤:
(4-1)当农业机械以恒速运动时,取组合导航定位的状态分量为X=[xe,ve,xn,vn]T,其中xe、ye分别为车体东向和北向的位置分量;ve、vn分别为车体东向和北向的速度分量,T为***周期,进行离散化后得到状态方程和观测方程为:
其中, V为零均值、协方差阵为R的白噪声,且与W互不相关;W与V在两坐标方向上的加速度相互独立并具有相同的方差σ2,,X为***模型的状态向量,Z为观测向量(x,y)位置坐标,W为***模型噪声矩阵,是东向和北向。
(4-2)所述的导航计算机模块输出所述的农业机械定位的位置信息和航向角。
下面以一个具体实施例来进一步介绍本发明:
本发明是通过以下方法实现的,见附图1:
本发明包括电子罗盘单元、里程计单元、GPS模块和导航计算机模块等。电子罗盘单元和里程计单元共同组成了航位测算模块。
1、电子罗盘单元主要采集地磁场信息和车体的翻滚角和俯仰角,然后通过硬磁、软磁和姿态修正等算法计算农业机械车体航向角,从而利用此角度完成DR计算;
2、里程计单元采集农业机械车体(主要是车轮)的转速,从而通过转换获得车体的运行速度,通过采样频率计算车体的相对移动位移,然后结合车体航向角完成DR算法;
3、GPS导航模块实时获取农业机械所处的初始地理位置信息;
4、导航计算机模块主要实现车体航向计算、数据处理和卡尔曼数据融合算法;
结合附图2,对本发明中的DR航位推算算法原理做简要说明:
在农业机械作业的过程中,由于地球面积远大于作业面积,所以其运动可视为二维平面上的运动。建立局部平面坐标系,以x为局部平面坐标系的正东方向,y为局部平面坐标系的正北方向。假设知道车体初始位置坐标为(x0,y0),已知在t0时刻的航向角为ψ0,通过实时的测量车辆的行驶距离和航向角度的变化,即可以推算出车辆在每个时刻的二维坐标。设航向角为ψ0时,经过Δt=tl-t0时间内航行距离S0,则在tl时刻车辆的坐标值为:
x1=x0+S0*sinψ0
y1=y0+S0*cosψ0 (公式1)
将公式1递推,即得t2时刻车辆的坐标值:
x2=x1+S1*cosψ1=y0+S0*cosψ0+S1*cosψ1
y2=y1+S1*cosψ1=y0+S0*cosψ0+S1*cosψ1 (公式2)
则依次递推可得到坐标的一般方程:
(公式3)
当采样周期Δt恒定且采样频率足够高时,则可近似认为在采样周期内车辆的加速度为零,车辆做匀速运动,则在tk时刻的坐标值为:
(公式4)
式中,vi为ti时刻车体的作业速度。
结合附图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)
(公式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的定位精度,应予以补偿或修正。 (公式9)
式中, 为修正后的定位数据; 为转换到导航坐标系的GPS测量的位置信息;a、b、h分别为GPS天线在机体坐标系中的投影;θr为机体翻滚角;φp为机体俯仰角;ψ为机体航向角。
第六步,卡尔曼数据融合。
在GPS/DR车载组合导航定位***中,选取合适的数学模型极其重要,因为它将直接影响导航数据滤波的效果,并将最终影响到整个组合导航定位***的定位精度。本发明采用恒速(CV)模型。
当农业机械以恒速运动时,取组合导航定位***的状态变量为X=[xe,ve,xn,vn]T,其中xe、xn分别为车体东向和北向的位置分量;ve、vn分别为车体东向和北向的速度分量,***采用周期为T,对连续***进行离散化,即可得到GPS/DR组合导航定位***的状态方程和观测方程为:
(公式10)
式中,
***噪声平均值为零、方差阵为Q的高斯随机噪声序列,观测噪声
【V为零均值、协方差阵为R的白噪声,且与W互不相关;W与V在两坐标方向上的加速度相互独立并具有相同的方差σ2。
第七步,重复第三、四、五、六步即可进行循环导航坐标计算。
采用了该发明中的基于卫星导航和航位推测实现农业机械定位的***及方法,具有如下有益效果:
(1)本发明采用了里程计能够代替IMU传感器,降低成本和开发难度;
(2)本发明采用的卡尔曼数据融合技术,充分结合了DR算法的实时性和GPS精度高的优点,克服了DR算法的积累误差和GPS实时性差的缺陷,使二者得到了很好的互补;
(3)本发明通过对电子罗盘一系列校准和补偿算法,获得较高精度的车体航向角,为DR算法和后期的控制算法提供一个精确的数据输入;
(4)本发明采用的姿态修正算法可以解决农业机械在作业过程中由于路况复杂而引起的车身倾斜造成的误差;
(5)本发明具有较高精度和实时性,尤其适用于农业机械辅助驾驶控制***中数据获取和处理中,具有更广泛的应用范围。
在此说明书中,本发明已参照其特定的实施例作了描述。但是,很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此,说明书和附图应被认为是说明性的而非限制性的。