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

说明书

本发明提供了一种单天线GNSS辅助陀螺仪的农机转向角测量方法及装置，测量方法包括：基于农机航向角变化率和作业速度，计算出农机期望转向角；由k时刻的期望转向角和陀螺仪原始量测计算的转向角进行卡尔曼滤波更新，估计出陀螺仪测量偏置b_g,k和k时刻转向角测量误差δθ_k；以滤波周期Δt内陀螺仪输出角速率ω_k减去农机航向角变化率并积分，可得补偿后陀螺仪计算的转向角；利用补偿后陀螺仪计算的转向角和所述δθ_k，得到k时刻农机转向角；最终实现农机左、右转向角的同步测量输出。本发明能以任意更新率输出农机航向角及航向角速率，补偿由GNSS量测延迟引入的转向角计算时滞。

技术领域

本发明属于智能农业机械技术领域，具体涉及一种单天线GNSS辅助陀螺仪的农机转角测量方法及测量装置。

背景技术

农业机械转向角度测量是农机自动驾驶系统的重要组成部分，其测量精度和可靠性对行驶控制的稳定性影响显著。传统的基于接触式连杆机构测量转角的方法，受传感器安装误差影响较大，且水田深泥脚作业时容易导致支撑轴与连杆间几何位置形变，产生角度测量误差。近年来随着微惯性技术的发展，基于陀螺仪的非接触式转角测量方法受到广泛关注，其具有安装简单、维护方便等特点。繆存孝等人在“基于双GNSS天线及单陀螺的车轮转角测量系统”(农业机械学报，48(9):17-23,2017)中，利用角速率陀螺积分角度和车辆运动学模型计算的转向角度差值，实现陀螺仪偏置和角度计算误差的补偿。上述方法不适用于单天线GNSS配置下农机转角测量，且其直接基于GNSS速度量测进行理论转角计算，更新率低且受制于GNSS信号传输稳定性。

发明内容

有鉴于此，本发明提供一种单天线GNSS辅助陀螺仪的农机转角测量方法及测量装置。

本发明是通过以下技术手段实现上述技术目的的。

单天线GNSS辅助陀螺仪的农机转角测量方法，具体为：

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

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

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

进一步的技术方案，所述陀螺仪输出角速率ω_k由陀螺仪敏感角速率减去陀螺仪测量偏置得到。

进一步的技术方案，所述农机期望转向角的计算公式为：

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

更进一步的技术方案，所述t_n时刻农机的速度的计算公式为：

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

更进一步的技术方案，所述未知矩阵的估计值满足：

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