[发明专利]基于双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法

权利要求书

1.基于双天线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接收机横滚角测量值方差统计值。

2.根据权利要求1所述基于双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法，其特征在于，步骤5)中，所述双天线GNSS接收机采用司南K528GNSS接收机，通过串口输出NMEA-0183格式中的$GPTRA数据包，截取$GPTRA数据包中的航向角信息。

3.根据权利要求1所述基于双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法，其特征在于，步骤5)中，设置微型姿态参考系统AHRS通过串口输出x轴MEMS陀螺仪标定角速度数据，以获取拖拉机的横滚角速度。

4.根据权利要求1所述基于双天线GNSS接收机/陀螺仪信息融合的农机横滚角测试方法，其特征在于，利用卡尔曼滤波器进行信息融合的方法为：

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

$\left\{\begin{array}{c}\left[\begin{array}{c}{\mathrm{\θ}}_k\\ b_k\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{\Δ}t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_{k-1}\\ b_{k-1}\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}t\\ 0\end{array}\right]{\mathrm{\ω}}_{k-1}+\left[\begin{array}{c}{\mathrm{\ξ}}_{gk}\mathrm{\Δ}-t\\ 0\end{array}\right]\\ \[{\mathrm{\ψ}}_k\]=\[\begin{array}{cc}1& 0\end{array}\]\left[\begin{array}{c}{\mathrm{\θ}}_k\\ b_k\end{array}\right]+{\mathrm{\ξ}}_{dk}\end{array}\right.$

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

$Q=\left[\begin{array}{cc}q\_double& 0\\ 0& q\_gyro\end{array}\right]$

R＝[r_double]

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

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

预测方程组为：

${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{\hat{u}}_{k-1}---\left(1\right)$

$P_k^{-}={\mathrm{AP}}_{k-1}{A}^T+Q---\left(2\right)$

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

校正方程组为：

$K_k=P_k^{-}{H}^T/({\mathrm{HP}}_k^{-}{H}^T+R)---\left(3\right)$

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})---\left(4\right)$

$P_k=(I-K_k{\mathrm{HP}}_k^{-})P_k^{-}---\left(5\right)$

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