[发明专利]一种履带式拖拉机运动学估计及偏差校准方法

说明书

本发明公开了一种履带式拖拉机运动学估计及偏差校准方法，属于农机自动驾驶技术领域，现提出如下方案，其包括以下步骤，构建履带式拖拉机运动学模型，在实际情况中，由于地面起伏变化、GNSS双天线安装偏差因素，引起航向角偏差，导致路径跟踪效果变差，为简化模型，可近似认为航向角偏差为一定值，选取东向位移坐标分量、北向位移坐标分量、拖拉机速度、北向位移坐标分量、拖拉机速度、拖拉机航向角作为系统观测量，构建的履带式拖拉机kalman滤波模型为非线性模型。本发明能够快速精确估计出由于地面起伏变化、GNSS天线安装偏差等引起的航向角偏差，从而对航向角进行补偿，提高系统对地面的适应性。

技术领域

本发明涉及农机自动驾驶技术领域，尤其涉及一种履带式拖拉机运动学估计及偏差校准方法。

背景技术

随着GNSS高精度卫星导航定位技术、自动驾驶以及信息技术的飞速发展，现代农业正逐步向数字农业、精准农业的方向发展。拖拉机作为一种常用的农业装备，其信息化、智能化程度对精准农业的发展具有重要意义。

按照行走方式可将拖拉机分为轮式拖拉机及履带式拖拉机。与轮式拖拉机相比，履带式拖拉机具有接触面大、接地比压小、牵引附着性能好、不易打滑等优点，更适合在条件相对比较恶劣的环境中作业，如雪地、山坡、泥泞、草地、高原等，有效地填补了轮式拖拉机的不足。然而在履带式拖拉机自动驾驶作业过程中，由于实际作业环境中存在着大量的不可预测的干扰，如耕种农田地面的复杂状况、运动学模型不精确、GNSS天线安装角度偏差、GPS信号遮挡及反射、系统噪声以及外界环境噪声干扰等因素，致使拖拉机位置姿态相关信息出现异常等，严重影响履带式拖拉机自动驾驶控制精度，不仅造成农户作业强度及经济成本增加，也大大降低了农业耕作效率和土地利用率。

本发明针对履带式拖拉机自动驾驶控制技术中存在的问题，提出了一种履带式拖拉机运动学估计及偏差校准方法，以实现履带式拖拉机位置姿态信息的精确估计，增强系统对外界环境的适应性以及抗干扰性，提高履带式拖拉机自动驾驶系统作业精度。本发明的目的则在于通过设计合适的估计方法，能够实时准确地估计出航向角偏差，并对航向角进行补偿和校正，以提高系统对环境的适应性。

发明内容

本发明的目的是针对履带式拖拉机在实际作业环境中由于受多种因素干扰而引起的控制效果差、作业精度低的问题，本发明提出一种抗干扰因素的履带式拖拉机运动学估计及偏差校准方法，该算法能够快速精度估计出由于地面起伏变化、GNSS双天线安装偏差等因素引起的航向角误差，并对航向角进行补偿，从而提高履带式拖拉机自动驾驶控制算法对地面等各种环境干扰因素的适应性。

为了实现上述目的，本发明采用了如下技术方案：

一种履带式拖拉机运动学估计及偏差校准方法，包括以下步骤，

S1，构建履带式拖拉机运动学模型：

其中，x为履带式拖拉机东向位移坐标分量，y为北向位移坐标分量，v为履带式拖拉机行驶速度，为履带式拖拉机航向角，ω为履带式拖拉机车体角速度；

S2：由于履带式拖拉机在进行转向时，左轮、右轮及质心处角速度相等，故可推导出：

其中，v_l为为左侧履带的行驶速度，v_r为为右侧履带的行驶速度，R为转弯半径，b为车体宽度；

S3：联立方程求解可得：

S4：由公式(3)及(6)可推出：

式中，u为左右履带速度差，即为控制量；