一种履带式拖拉机运动学估计及偏差校准方法与流程

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

背景技术：

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

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

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

技术实现要素：

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

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

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

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

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

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

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

S3：联立方程求解可得：

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

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

S5：在农机自动驾驶作业过程中，为保证农作物耕种质量，拖拉机设置做匀速直线运动，因此可得：

S6：在实际情况中，由于地面起伏变化、GNSS双天线安装偏差因素引起航向角偏差，导致路径跟踪效果变差，为简化模型，可近似认为航向角偏差为恒定值，则有：

S7：通过S1-S6过程，构建的履带式拖拉机kalman滤波非线性微分方程模型如下：

S8：选取东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角以及航向角偏差δ作为系统状态量，以东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角作为系统观测量，则有：

其中，X为系统估计向量，Z为系统观测向量；

S9：S7中构建的履带式拖拉机kalman滤波模型为非线性模型，采用EKF滤波算法，通过求取雅可比矩阵，对模型进行线性化得到对应的系统线性状态空间方程：

S10：将连续系统离散化得到状态转移矩阵Φ以及观测矩阵H：

S11：选取过程噪声协方差矩阵Q以及观测噪声协方差矩阵R：

S12：初始化状态向量X、协方差矩阵P以及观测状态向量Z：

X(0)＝E[X(0)] (19)；

P(0)＝var[X(0)] (20)；

Z(0)＝Z0 (21)；

其中，X(0)、P(0)、Z(0)分别为状态向量X、协方差矩阵P以及观测状态向量Z的初始值；

S13：Kalman滤波状态一步预测：

S14：计算一步预测协方差矩阵：

P(k+1|k)＝Φ(k+1|k)P(k|k)Φ^T(k+1|k)+Q(k+1) (23)；

S15：计算Kalman增益：

K(k+1)＝P(k+1|k)H^T(k+1)[H(k+1)P(k+1|k)H^T(k+1)+R(k+1)]^-1 (24)；

S16：计算估计值：

S17：更新协方差矩阵：

P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k) (26)；

其中，在公式(22-26)中，k+1表示下一时刻，k表示当前时刻，为系统状态估计量，K为kalman增益。

优选的，还包括步骤S18，不断重复步骤S13-S17。

优选的，还包括步骤S19，利用实际数据进行离线分析调试的过程中，调节观测噪声矩阵、过程噪声矩阵、以及协方差矩阵以达到期望滤波效果，并估计出符合实际的航向角偏差对航向角进行补偿。

与现有技术相比，本发明的有益效果是：

(1)本发明实现的履带式拖拉机运动学估计及偏差校准方法能够快速精确估计出由于地面起伏变化、GNSS天线安装偏差等引起的航向角偏差，从而对航向角进行补偿，提高系统对地面的适应性；

(2)本发明通过采用EKF滤波算法能够对履带式拖拉机自动驾驶控制算法数据源进行滤波处理，减少了数据噪声，降低了外界环境干扰因素及系统噪声对履带式拖拉机自动驾驶系统性能的影响，提升了履带式拖拉机自动驾驶系统控制精度以及系统稳定性；

(3)本发明计算量小，实时性高，相比之下，能将履带式拖拉机自动驾驶性能提升25％左右。

附图说明

图1为履带式拖拉机运动学模型。

图2为kalman滤波估计流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

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

第一步：构建履带式拖拉机运动学模型：

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

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

其中，vl为为左侧履带的行驶速度，vr为为右侧履带的行驶速度，R为转弯半径，b为车体宽度。

第三步：联立方程求解可得：

第四步：由公式(3)及(6)可推出：

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

第五步：在农机自动驾驶作业过程中，为保证农作物耕种质量，拖拉机设置做匀速直线运动，因此可得：

第六步：在实际情况中，由于地面起伏变化、GNSS双天线安装偏差因素，引起航向角偏差，导致路径跟踪效果变差，为简化模型，近似认为航向角偏差为一定值，则有：

本发明的目的则在于通过设计合适的估计方法，能够实时准确地估计出航向角偏差δ，并对航向角进行补偿和校正；

第七步：通过上述过程，本发明构建的履带式拖拉机kalman滤波非线性微分方程模型如下：

第八步：选取东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角以及航向角偏差δ作为系统状态量，以东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角作为系统观测量，则有：

其中，X为系统估计向量，Z为系统观测向量。

第九步：第七步中构建的履带式拖拉机kalman滤波模型为非线性模型，本发明采用EKF滤波算法，通过求取雅可比矩阵，对模型进行线性化得到对应的系统线性状态空间方程：

第十步：将连续系统离散化得到状态转移矩阵Φ以及观测矩阵H：

第十一步：选取过程噪声协方差矩阵Q以及观测噪声协方差矩阵R：

过程噪声协方差矩阵Q：

观测噪声协方差矩阵R的选取：通过采集一段时间内(10～20min)履带式拖拉机自动驾驶系统在静止过程中位姿数据东向位移坐标分量x、北向位移坐标分量y、拖拉机行驶速度v、拖拉机航向角并分别对各组数据求标准差得到系统观测噪声协方差矩阵R，拖拉机在运行过程中的观测噪声协方差矩阵R为静止时的n＝3倍：

值得注意的是，此处选取的Q、R并不为定值，可根据滤波效果进行修改和调整，直至达到满意效果为止。

第十二步：通过采集履带式拖拉机在运动过程中(自动驾驶模式下)的东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角控制量u及车体角速度ω，并将其作为观测向量Z；

第十三步：初始化状态向量X、协方差矩阵P以及观测状态向量Z：

X(0)＝E[X(0)] (19)；

Z(0)＝Z0 (21)；

其中，X(0)、P(0)、Z(0)分别为状态向量X、协方差矩阵P以及观测状态向量Z的初始值，P(0)的大小将直接影响EKF算法的收敛速度；

第十四步：Kalman滤波状态一步预测：

第十五步：计算一步预测协方差矩阵：

P(k+1|k)＝Φ(k+1|k)P(k|k)Φ^T(k+1|k)+Q(k+1) (23)；

第十六步：计算Kalman增益：

K(k+1)＝P(k+1|k)H^T(k+1)[H(k+1)P(k+1|k)H^T(k+1)+R(k+1)]^-1 (24)；

第十七步：计算估计值：

第十八步：更新协方差矩阵：

P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k) (26)；

其中，在公式(22-26)中，k+1表示下一时刻，k表示当前时刻，为系统状态估计量，K为kkalman增益；

第十九步：不断重复步骤第十四-第十八；

本发明利用实际数据进行离线分析调试的过程中，调节观测噪声矩阵R、过程噪声矩阵Q、以及协方差矩阵P(0)以达到期望滤波效果，并估计出符合实际的航向角偏差对航向角进行补偿；

本发明将履带式拖拉机运动学估计及校准方法应用于履带式拖拉机自动驾驶过程，进行在线滤波、数据处理、估计航向角偏差，以达到较好的控制效果；

本发明提出的履带式拖拉机运动学估计及偏差校准方法能够快速精度估计由于地面起伏变化引起的航向角误差，从而提高控制算法对地面起伏变化的适应性，具体包括以下几点：

(1)本发明实现的履带式拖拉机运动学估计及偏差校准方法能够快速精确估计出由于地面起伏变化、GNSS天线安装偏差等引起的航向角偏差，从而对航向角进行补偿，提高系统对地面的适应性；

(2)本发明通过采用EKF滤波算法能够对履带式拖拉机自动驾驶控制算法数据源进行滤波处理，减少了数据噪声，降低了外界环境干扰因素及系统噪声对履带式拖拉机自动驾驶系统性能的影响，提升了履带式拖拉机自动驾驶系统控制精度以及系统稳定性；

(3)本发明计算量小，实时性高，相比之下，能将履带式拖拉机自动驾驶性能提升25％左右。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。