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

权利要求书

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增益。

2.根据权利要求1所述的一种履带式拖拉机运动学估计及偏差校准方法，其特征在于，还包括步骤S18，不断重复步骤S13-S17。

3.根据权利要求2所述的一种履带式拖拉机运动学估计及偏差校准方法，其特征在于，还包括步骤S19，利用实际数据进行离线分析调试的过程中，调节观测噪声矩阵、过程噪声矩阵、以及协方差矩阵以达到期望滤波效果，并估计出符合实际的航向角偏差对航向角进行补偿。