[发明专利]单天线GNSS辅助陀螺仪的农机转向角测量方法及装置

权利要求书

1.单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于：

基于农机航向角变化率和作业速度，计算出农机期望转向角；

由k时刻的期望转向角和陀螺仪原始量测计算的转向角进行卡尔曼滤波更新，估计出陀螺仪测量偏置b_g,k和k时刻转向角测量误差δθ_k；

以滤波周期Δt内陀螺仪输出角速率ω_k减去农机航向角变化率并积分，可得补偿后陀螺仪计算的转向角；利用补偿后陀螺仪计算的转向角和所述δθ_k，得到k时刻农机转向角；最终实现农机左、右转向角的同步测量输出。

2.根据权利要求1所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述陀螺仪输出角速率ω_k由陀螺仪敏感角速率减去陀螺仪测量偏置得到。

3.根据权利要求1所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述农机期望转向角的计算公式为：

其中：L表示农机轴距，表示t_n时刻农机航向角变化率，V(t_n)表示t_n时刻农机的速度。

4.根据权利要求3所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述t_n时刻农机的速度的计算公式为：

其中：为未知矩阵的估计值，t₀为初始时刻。

5.根据权利要求4所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述未知矩阵的估计值满足：

其中：N为位置序列的点数，为未知矩阵的估计值，Δ_n＝t_n-t₀为时刻t_n至初始时刻t₀的时间间隔，P表示轨迹方程。

6.根据权利要求5所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述轨迹方程由GNSS定位天线输出的N+1个位置点序列(P₀…P_N)构造而成。

7.根据权利要求1所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，由k时刻的期望转向角和陀螺仪原始量测计算的转向角进行卡尔曼滤波更新，具体为：以与的差值为量测，构造更新量测方程：z_k＝H_kx_k+v_k，其中为卡尔曼滤波器输入量测数据，v_k为转向角测量误差量测模型的零均值高斯噪声，H_k为量测矩阵。

8.一种实现权利要求1-7任一项所述的单天线GNSS辅助陀螺仪的农机转角测量方法的测量装置，其特征在于，包括：GNSS定位天线、定位接收机、ARM1微控制器、ARM2微控制器、DSP、FPGA以及两个单轴陀螺仪；

所述GNSS定位天线与定位接收机进行连接，GNSS定位天线和定位接收机提供农机位置数据；

所述两个单轴陀螺仪分别安装在农机左前轮和右前轮转动部件上；

所述ARM1微控制器采集两个单轴陀螺仪的角速率量测并送入FPGA中，所述ARM2微控制器采集定位接收机输出的位置信息并送入FPGA中；

所述FPGA实现陀螺仪和定位接收机数据的同步；

所述DSP基于FPGA，在线估计陀螺测量偏置与转向角误差，进而实现农机前轮左、右转向角的同时测量输出。