基于双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法与流程

本发明涉及农业机械姿态测量领域，特别涉及一种基于双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法。

背景技术：

农业机械自动导航是精准农业技术体系中的一项核心关键技术，广泛应用于耕作、播种、施肥、喷药、收获等农业生产过程。农机的自动导航控制主要是对农机底部的横向位置偏差进行控制，即控制农机跟踪事先规划好的作业路径，使其与路径之间的横向位置偏差保持在一定的精度范围之内，从而满足农业生产的需要。在农机自动导航路径跟踪控制方法中，农机底盘与规划作业路径之间的横向位置偏差主要受GNSS接收机定位精度和倾斜校正中农机横滚角精度的影响。载波相位差分的GNSS接收机符合定位精度的要求，因此获取农机高精度的横滚角信息是获取精确横向位置偏差的关键，而获取精确的位置偏差是农机控制稳定和精确的关键。

传统的高精度测姿是利用惯导来实现的，通过陀螺和加速度计感应运动过程中的角速度以及线加速度，通过积分和推算方法获得运动载体的姿态角。其设备复杂、价格昂贵、维修困难、长期精度差。近年来由于GNSS载波相位测量技术迅猛发展，利用GNSS来测量运动载体的航向和姿态信息已经成为导航领域的又一新的热点。双天线测姿技术是指在载体上合理安置两个GNSS天线，通过天线间的载波相位差分，求解出两天线的基线矢量，从而求解出载体的二维姿态信息。为了降低接收机硬件成本，一类采用时钟同步技术的OEM双天线GNSS接收机开始出现，并逐渐被运用于高精度的姿态测量领域。2008年以后，一体双天线接收机开始被广泛关注和认可，双天线接收机开始大批量进入市场。利用一体双天线接收机测量运动载体的姿态信息，具有稳定性好、成本低廉、精度高、实时性好、无需初始校正和无时间累积误差等优点，但存在倾斜角信息白噪声较大的问题。相对于双天线GNSS而言，陀螺仪输出数据平稳，短时间稳定性强。利用单轴陀螺仪角速度和双天线倾斜角度信息融合的方法测试农机横滚角，对于农机获取精确、高可靠性的横滚角具有十分重要的意义。

技术实现要素：

本发明的主要目的在于克服现有技术中双天线GNSS接收机输出的农机横滚角白噪声大，无法满足农机倾斜校正的精度要求，提供一种基于双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法。

为了达到上述目的，本发明采用以下技术方案：

本发明基于双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法，包括下述步骤：

1)为农机建立车体坐标系，车体坐标系Oxy定义为以车体质心位置为原点O，横轴x指向车体前进方向，纵轴y与横轴x垂直指向车体前进方向的左侧；

2)双天线固定支架为长度不低于1.3m的矩形钢管，安装在车体质心正上方的拖拉机驾驶室顶部，安装原则为钢管平行于车体坐标系y轴，关于原点O对称；

3)将GNSS主天线安装在双天线固定支架的右端，从天线安装在固定支架的左端，主从天线关于车体坐标系的x轴对称；

4)陀螺仪安装在农机质心正上方拖拉机座椅下方，旋转轴指向车体坐标系x轴方向；

5)设置双天线GNSS接收机输出NMEA-0183标准格式中$GPTRA数据包，截取$GPTRA数据包中的倾斜角ψ；利用陀螺仪获取横滚角速度ω；

6)采用卡尔曼滤波模型对步骤5)获取的横滚角度和横滚角速度进行融合处理，获得农机横滚角度最优估计值；

7)步骤6)中所述卡尔曼滤波器的模型中，状态向量为农业机械的横滚角θ和陀螺仪的零偏b二维向量；测量量为双天线GNSS接收机获取的横滚角ψ一维标量；

8)系统过程噪声协方差矩阵Q为用于表示状态转移方程的误差大小，设定为常数矩阵，在仿真和试验过程中整定矩阵参数，测量噪声方差R为双天线GNSS接收机横滚角测量值方差统计值。

作为优选的技术方案，步骤5)中，所述双天线GNSS接收机采用司南K528GNSS接收机，通过串口输出NMEA-0183格式中的$GPTRA数据包，截取$GPTRA数据包中的航向角信息。

作为优选的技术方案，步骤5)中，设置微型姿态参考系统AHRS通过串口输出x轴MEMS陀螺仪标定角速度数据，以获取拖拉机的横滚角速度。

作为优选的技术方案，利用卡尔曼滤波器进行信息融合的方法为：

首先建立卡尔曼滤波器的状态方程和测量方程，横滚角和横滚角速度存在导数关系，系统倾斜真实角度θ用来做一个状态向量，在该系统中，采用双天线GNSS接收机横滚角测量值估计出陀螺仪零偏b，以此偏差作为状态向量得到相应的状态方程和观测方程：

其次，通过系统过程噪声协方差阵Q以及测量误差的协方差矩阵R，对卡尔曼滤波器进行校正，Q与R矩阵的形式如下：

R＝[r_double]

q_double和q_gyro分别是双天线GNSS接收机和陀螺仪测量的协方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，值越小表明信任程度越高；r_double是双天线GNSS接收机横滚角测量噪声的方差统计值；

最后，采用线性离散卡尔曼滤波器的递归差分方程进行状态向量预测和测量向量校正：

预测方程组为：

式中，是k时刻的预测结果，是k-1时刻的最优结果，是对应的协方差，A^T表示A的转置矩阵，Q是系统的过程协方差；

校正方程组为：

K_k为时刻k时的卡尔曼增益矩阵，是时刻k的最优化估算值。

作为优选的技术方案，为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值，需更新k状态下的协方差P_k，I为单位阵，当系统进入k+1状态时,P_k就是式子(2)的P_k-1，(3)、(4)、(5)式为卡尔曼滤波器状态更新方程，计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果。

作为优选的技术方案，步骤7)中，所述卡尔曼滤波模型中，状态转移方程是基于角度、角速度的计算关系推算原理构建的，ω为横滚角速度控制输入，由陀螺仪获得的加速度值实时更新。

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

1、本发明采用卡尔曼滤波方法对来自双天线GNSS接收机和陀螺仪的信号进行融合，解决由于双天线GNSS接收机横滚角测量噪声大不易直接用于农机姿态校正的问题。

2本发明双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法可形成更连续和稳定的农业机械横滚角数据，使得农机导航的控制更加稳定准确。

3本发明双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法相比于加速度计与陀螺仪数据融合，双天线GNSS横滚角测试方法比加速度计横滚角测试方法动态性能好。

4本发明双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法可以代替昂贵的高精度姿态传感器测量农机横滚角，节省农机自动导航系统成本。

附图说明

图1为本发明实施例农机横滚角测试方法流程图；

图2为本发明实施例设备安装结构示意图；

图3为X轴陀螺仪原始数据累积横滚角；

图4是陀螺仪零偏最优估计值；

图5是滤波前后横滚角对比图。

具体实施方式

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

实施例

本实施例采用农业机械为雷沃M904-D轮式拖拉机，采用的设备或装置主要包括：司南K528双天线GNSS接收机、双天线固定支架和微型姿态参考系统AHRS。如图2所示为本发明实施例的设备安装结构示意图，为轮式拖拉机1建立车体坐标系，车体坐标系Oxy定义为以车体质心位置为原点O，横轴x指向车体前进方向，纵轴y与横轴垂直指向车体前进方向的左侧。双天线固定支架2为长度不低于1.3m的矩形钢管，安装在车体质心正上方的拖拉机驾驶室顶部，安装原则为钢管平行于车体坐标系y轴，关于原点O对称。GNSS主天线3安装在双天线固定支架的右端(车体前进方向为前)，从天线4安装在固定支架的左端，主从天线关于车体坐标系的x轴对称。设置双天线接收机输出NMEA-0183标准格式中$GPTRA数据包。截取$GPTRA数据包中的倾斜角ψ。截取微型姿态参考系统AHRS获取的横滚角速度ω。采用卡尔曼滤波器对获取的横滚角度ψ和横滚角速度ω进行融合处理，获得农机横滚角度精确估计值。所述卡尔曼滤波器的模型中，状态向量为农业机械的横滚角θ和陀螺仪的零偏b；状态转移方程是基于角度、角速度的计算关系推算原理构建的。测量值为双天线接收机获取的横滚角ψ。测量矩阵是基于预测状态向量与观测值之间关系构建的。

如图1所示，本实施例双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法，具体处理步骤和公式推导如下：

1、接收机数据采集：设置司南K528GNSS接收机通过串口以10Hz的输出频率输出NMEA-0183格式中的$GPTRA数据包，利用XCOM V2.0串口助手接收$GPTRA数据，打开串口助手时间戳功能，即在每包$GPTRA数据后面加上获取该包数据的时间信息，数据采集完成后保存至txt文本文档。

2、姿态传感器数据采集：设置AHRS通过串口以20Hz的输出频率输出X轴MEMS陀螺仪角速度标定数据，利用XCOM V2.0串口助手接收AHRS输出数据，打开串口助手时间戳功能，即在每包数据后面加上获取该包数据的时间信息。姿态传感器和接收机的数据采集同时进行，数据采集完成后保存至txt文本文档。

3、根据双天GNSS接收机和AHRS数据包获取时间信息，利用Microsoft excel工作表的数据筛选功能截取出双天线GNSS接收机k时刻的横滚角ψ_k和陀螺仪对应时刻的横滚角速度ω_k。

4、根据步骤3获取的k时刻的横滚角ψ_k和横滚角速度ω_k，卡尔曼滤波器数据融合过程：

5、首先建立卡尔曼滤波器的状态方程和测量方程。横滚角和横滚角速度存在导数关系，系统倾斜真实角度θ用来做状态向量一个分量。在该系统中，采用双天线GNSS接收机测量得到的横滚角估计出陀螺仪零偏b，以陀螺仪零偏作为状态向量的另一个分量，得到相应的状态方程和观测方程：

6、系统过程噪声协方差阵Q以及测量误差的协方差矩阵R，对卡尔曼滤波器进行校正。Q与R矩阵的形式如下：

R＝[r_double]

7、q_double和q_gyro分别是双天线GNSS接收机和陀螺仪测量的协方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，值越小表明信任程度越高。r_double是双天线GNSS接收机横滚角测量噪声的方差统计值。

8、综上所述，采用线性离散卡尔曼滤波器的递归差分方程进行状态向量预测和测量向量校正：

预测方程组为：

式中,是k时刻的预测结果，是k-1时刻的最优结果，是对应的协方差。A^T表示A的转置矩阵，Q是系统的过程协方差。式子(1)、(2)即对系统的状态更新。

校正方程组为：

K_k为时刻k时的卡尔曼增益矩阵，是时刻k的最优化估算值。为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值，我们还要更新k状态下的协方差P_k。I为单位阵。当系统进入k+1状态时,P_k就是式子(2)的P_k-1，(3)、(4)、(5)式为卡尔曼滤波器状态更新方程。计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为下一次计算的先验估计，这样，周而复始、循环反复地运算下去直至找到最优的结果。

对上述信息融合方法进行仿真测试，设定卡尔曼滤波的初始化条件是：仿真数据来自自主研发的雷沃M904-D拖拉机双天线GNSS导航控制系统，设定卡尔曼滤波器的初始条件是：

图3是x轴陀螺仪原始数据累积横滚角，可以看出误差随时间累积越来越大。图4是本信息融合方法对陀螺仪零偏的最优估计值，可以看出零偏估计值迅速收敛稳定在某个值附近。图5是滤波前后横滚角对比图，线1是双天线GNSS接收机输出的原始横滚角，线2是双天线GNSS接收机/陀螺仪信息融合后的农机横滚角，线3是陀螺仪经过零偏校正后累积的横滚角。从图5中可看出，双天线GNSS接收机输出的横滚角趋势正确，但白噪声较大。陀螺仪累积的横滚角数据平稳，短时间稳定性强，但长期精度差。采用卡尔曼滤波方法对双天线GNSS接收机/陀螺仪获取的信息进行融合处理，可形成更连续、稳定、准确的农机横滚角。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。