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

摘要

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

主权项

1.一种履带式拖拉机运动学估计及偏差校准方法，其特征在于，包括以下步骤，S1，构建履带式拖拉机运动学模型：其中，x为履带式拖拉机东向位移坐标分量，y为北向位移坐标分量，v为履带式拖拉机行驶速度，为履带式拖拉机航向角，ω为履带式拖拉机车体角速度；S2：由于履带式拖拉机在进行转向时，左轮、右轮及质心处角速度相等，故可推导出：其中，v_l为为左侧履带的行驶速度，v_r为为右侧履带的行驶速度，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)＝Z₀                                         (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增益。