CN110007667B - 一种履带拖拉机及其路径跟踪控制方法和系统 - Google Patents

Abstract

本发明公开了一种履带拖拉机及其路径跟踪控制方法和系统，首先通过优选一种电控装置改造履带拖拉机手动转向机构，实现电控转向；接着利用主驱动轮半径及其转速计算两侧履带理论速度，结合履带拖拉机滑移特性建立了滑移率与履带理论速度的关系，进而得到航向预估模型；其次采用增量式PID控制算法设计了底层控制器，利用采样周期和航向预估模型计算得到航向变化预估量；然后把拖拉机当前航向和航向变化预估量之和作为航向反馈量，把期望航向与航向反馈量差值作为底层控制器输入计算得到控制输出；最后利用控制输出控制电控装置动作，实现履带拖拉机转向以跟踪期望路径。

Description

一种履带拖拉机及其路径跟踪控制方法和系统

技术领域

本发明涉及车辆自动驾驶技术领域，尤其涉及一种履带拖拉机及其路径跟踪控制方法和系统。

背景技术

随着农村土地规模化经营迅速发展、农业劳动力大量转移，农业领域对智能化农机装备的需求愈加迫切。相比轮式拖拉机，履带拖拉机具有接地比压低、转向灵活等特点，广泛适用于南方水田作业，对其自动驾驶技术进行研究有助于提高南方水田作业效率，符合我国当前农业发展需要。

航向跟踪算法是一种常用的路径跟踪控制方法。在该算法中，位置偏差和航向偏差最终都可统一转化为航向偏差，准确的航向跟踪是实现路径跟踪的前提条件。履带拖拉机在水田环境作业时，其车身航向与其纵向速度、两侧履带速度、旋转中心位置、履带的滑移系数等诸多因素有关，常规方法中在建立车辆控制模型时只考虑了期望航向和实际航向的偏差，而未能包含其它因素的影响，难以达到满意的控制效果。特别当纵向速度发生较大变化或拖拉机发生滑移现象时，跟踪误差会明显增大，甚至会出现大幅度震荡。因此，在履带拖拉机路径跟踪过程中，履带拖拉机控制模型应该能对纵向速度、滑移工况等影响因素有一定的自适应能力。

另外，在实际航向控制过程中，控制器根据期望航向和履带拖拉机的实际航向计算得到航向偏差，再计算控制量。执行机构执行该控制量需要经过一个采样周期，此时履带拖拉机的实际航向已经发生了变化，即控制输出会有一个采样周期的滞后，这也会对路径跟踪精度产生较大影响。

发明内容

为解决上述问题，在现有研究基础上，考虑履带滑移特性建立了拖拉机航向预估控制模型，提出了一种履带拖拉机及其路径跟踪控制方法和系统。

具体地说，本发明公开了一种路径跟踪控制方法，包括：利用拖拉机主驱动轮半径及其转速计算两侧履带的理论速度，结合履带拖拉机滑移特性建立滑移率与该理论速度，建立航向预估模型；根据采样周期和该航向预估模型计算得到航向变化预估量；把拖拉机当前航向和该航向变化预估量之和作为航向反馈量，底层控制器根据期望航向与该航向反馈量，计算得到控制输出，控制输出控制电控装置动作，实现履带拖拉机转向以跟踪期望路径。

该路径跟踪控制方法，其中包括以下步骤：

步骤1，通过定位系统对拖拉机进行测速，得到该拖拉机的移动速度；

步骤2，根据该拖拉机履带与地面的接触特性、该移动速度以及该拖拉机两侧履带的理论速度，得到该拖拉机两侧履带的滑移率；

步骤3，根据该滑移率、该拖拉机两侧履带的理论速度和该拖拉机的履带间距，获得该拖拉机的航向变化率表达式；

步骤4，根据该航向变化率表达式和采样周期，计算该拖拉机的航向变化预估量；

步骤5，将该航向变化预估量和该拖拉机的当前航向之和作为航向反馈量，根据该航向反馈量和期望航向，控制该拖拉机的舵机角度，以使该拖拉机跟踪期望路径。

该路径跟踪控制方法，其中该步骤2还包括：根据该拖拉机两侧驱动轮的转速和尺寸，得到该履带拖拉机两侧履带的该理论速度：

v_r＝w_r·r

v_l＝w_l·r

式中，v_l和v_r分别为左右两侧履带的理论速度，w_r为右侧驱动轮转速，w_l为左侧驱动轮转速，r为驱动轮半径。

该路径跟踪控制方法，其中该步骤2还包括，通过联立以下公式确定该滑移率：

式中n为可调参数，v为该移动速度，v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率。

该路径跟踪控制方法，其中该步骤5还包括：将该航向反馈量和期望航向输入至底层控制器，该底层控制器根据该该航向反馈量和期望航向，生成调整控制量，以控制该拖拉机的舵机角度；

其中该底层控制器采用增量式PID控制算法，通过以下公式生成该调整控制量：

Δμ＝μ(k)-μ(k-1)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]

式中K_p，K_i和K_d分别为比例、积分和微分系数，μ(k)表示第k个采样时刻的控制量，e(k)表示第k个采样时刻的航向输入偏差，k为自然数，Δμ为该调整控制量。

该路径跟踪控制方法，其中该步骤3还包括，通过以下公式确定该航向变化率表达式：

为航向变化率，v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率，d为履带间距。

该路径跟踪控制方法，其中步骤4中的该航向变化预估量通过以下公式确定：

式中v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率，d为履带间距，T为采样周期，Δθ为该航向变化预估量。

本发明还提出了一种路径跟踪控制系统，其中包括以下模块：

测速模块，用于对拖拉机进行测速，得到该拖拉机的移动速度；

滑移率计算模块，用于根据该拖拉机履带与地面的接触特性、该移动速度以及该拖拉机两侧履带的理论速度，得到该拖拉机两侧履带的滑移率；

航向变化率表达模块，用于根据该滑移率、该拖拉机两侧履带的理论速度和该拖拉机的履带间距，获得该拖拉机的航向变化率表达式；

控制模块，用于根据该航向变化率表达式和采样周期，计算该拖拉机的航向变化预估量，将该航向变化预估量和该拖拉机的当前航向之和作为航向反馈量，根据该航向反馈量和期望航向，控制该拖拉机的舵机角度，以使该拖拉机跟踪期望路径。

该路径跟踪控制系统，其中该滑移率计算模块还包括：根据该拖拉机两侧驱动轮的转速和尺寸，得到该履带拖拉机两侧履带的该理论速度：

v_r＝w_r·r；v_l＝w_l·r

式中，v_l和v_r分别为左右两侧履带的理论速度，w_r为右侧驱动轮转速，w_l为左侧驱动轮转速，r为驱动轮半径。

该路径跟踪控制系统，其中该滑移率计算模块还包括，通过联立以下公式确定该滑移率：

式中n为可调参数，v为该移动速度，v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率。

该路径跟踪控制系统，其中该控制模块还包括：将该航向反馈量和期望航向输入至底层控制器，该底层控制器根据该该航向反馈量和期望航向，生成调整控制量，以控制该拖拉机的舵机角度；

其中该底层控制器采用增量式PID控制算法，通过以下公式生成该调整控制量：Δμ＝μ(k)-μ(k-1)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]，式中K_p，K_i和K_d分别为比例、积分和微分系数，μ(k)表示第k个采样时刻的控制量，e(k)表示第k个采样时刻的航向输入偏差，k为自然数，Δμ为该调整控制量。

本发明还提出了一种采用上述任意一种路径跟踪控制系统的拖拉机。

本发明对车身纵向速度、滑移工况等影响因素有一定的自适应能力，有利于提高履带拖拉机自动驾驶系统在水田作业条件下的路径跟踪控制精度。

附图说明

图1是本发明履带拖拉机转向机构电控改造示意图；

图2是本发明履带拖拉机运动学模型；

图3是本发明基于航向预估模型的路径跟踪算法控制框图；

图4是本发明路径跟踪算法的步骤框图；

图5是本发明拖拉机结构示意图。

附图标记：

1：液压差速阀； 2：大扭矩舵机； 3：机械连杆；

4：拖拉机上层结构； 5：路径跟踪控制系统； 6：拖拉机履带；

7：地面。

具体实施方式

本发明的目的是提供一种路径跟踪控制方法，以解决履带拖拉机自动驾驶系统在水田作业条件下路径跟踪误差较大的问题，包括：利用拖拉机主驱动轮半径及其转速计算两侧履带的理论速度，结合履带拖拉机滑移特性建立滑移率与该理论速度，建立航向预估模型；根据采样周期和该航向预估模型计算得到航向变化预估量；把拖拉机当前航向和该航向变化预估量之和作为航向反馈量，底层控制器根据期望航向与该航向反馈量，计算得到控制输出，控制输出控制电控装置动作，实现履带拖拉机转向以跟踪期望路径。

具体来说，如图4所示，本发明提出了一种基于滑移率估算的履带拖拉机航向预估控制方法，包括以下步骤：

步骤1，通过定位系统对拖拉机进行测速，得到该拖拉机的移动速度；

步骤2，根据该拖拉机履带与地面的接触特性、该移动速度以及该拖拉机两侧履带的理论速度，得到该拖拉机两侧履带的滑移率；

步骤3，根据该滑移率、该拖拉机两侧履带的理论速度和该拖拉机的履带间距，获得该拖拉机的航向变化率表达式；

步骤4，根据该航向变化率表达式和采样周期，计算该拖拉机的航向变化预估量；

步骤5，将该航向变化预估量和该拖拉机的当前航向之和作为航向反馈量，根据该航向反馈量和期望航向，控制该拖拉机的舵机角度，以使该拖拉机跟踪期望路径。

该路径跟踪控制方法，其中该步骤2还包括：根据该拖拉机两侧驱动轮的转速和尺寸，得到该履带拖拉机两侧履带的该理论速度：

v_r＝w_r·r

v_l＝w_l·r

式中，v_l和v_r分别为左右两侧履带的理论速度，w_r为右侧驱动轮转速，w_l为左侧驱动轮转速，r为驱动轮半径。

该路径跟踪控制方法，其中该步骤2还包括，通过联立以下公式确定该滑移率：

式中n为可调参数，v为该移动速度，v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率。

该路径跟踪控制方法，其中该步骤5还包括：将该航向反馈量和期望航向输入至底层控制器，该底层控制器根据该该航向反馈量和期望航向，生成调整控制量，以控制该拖拉机的舵机角度；

其中该底层控制器采用增量式PID控制算法，通过以下公式生成该调整控制量：

Δμ＝μ(k)-μ(k-1)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]

式中K_p，K_i和K_d分别为比例、积分和微分系数，μ(k)表示第k个采样时刻的控制量，e(k)表示第k个采样时刻的航向输入偏差，k为自然数，Δμ为该调整控制量。

该路径跟踪控制方法，其中该步骤3还包括，通过以下公式确定该航向变化率表达式：

为航向变化率，v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率，d为履带间距。

该路径跟踪控制方法，其中步骤4中的该航向变化预估量通过以下公式确定：

式中v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率，d为履带间距，T为采样周期，Δθ为该航向变化预估量。

本发明的目的是提供一种路径跟踪控制方法，以解决履带拖拉机自动驾驶系统在水田作业条件下路径跟踪误差较大的问题，同时提高控制系统对纵向速度、滑移工况等影响因素的自适应能力。本发明提出的路径跟踪控制方法，包括如下步骤：

履带拖拉机手动转向机构电控改造。本实施例选用的履带拖拉机采用液压差速阀控制两侧履带速度以实现转向功能。可以利用SUPER300大扭矩舵机或电动推杆等装置代替原手动转向机构，利用连杆装置带动液压差速阀转动，实现电控改造，如图1所示。

测算拖拉机左右两侧履带的理论速度。拖拉机两侧履带的理论速度可以通过主驱动轮半径及其转速进行计算，计算公式为：

v_r＝w_r·r (1)

v_l＝w_l·r (2)

式中，v_l和v_r分别为左右两侧履带的理论速度，w_r为右侧驱动轮转速，w_l为左侧驱动轮转速，r为驱动轮半径。

本实施例中驱动轮转速(w_r和w_l)可以通过FableBL50C-16384R2高精度编码器直接测量得到。

计算拖拉机左右两侧履带绝对速度。设v_l′和v′_r分别为左右两侧履带的绝对速度，其计算公式为：

v'_r＝v_r(1-a_r) (3)

v_l'＝v_l(1-a_l) (4)

式中a_l和a_r分别为左右两侧履带的滑移率，由于拖拉机作业的环境往往道路湿滑，导致理论速度往往大于绝对速度，如果不发生滑移现象，滑移率为0，即绝对速度和理论速度相等。

建立航向预估模型。图2所示为履带拖拉机运动学模型，为了反映左右两侧履带速度与地面间滑移作用对车辆纵向速度和航向变化的影响，在平面坐标系下建立履带拖拉机航向预估模型：

式中，v代表移动速度，

代表航向变化率，d为履带间距。

计算滑移率参数。滑移率由履带和地面间的相互作用决定，很难直接测量，因此有必要对其进行估计。车身的真实移动速度v可以利用RTK-GNSS定位系统直接测量得到，依据公式(5)可知，还需要一个等式联立才能解算出滑移率。

履带拖拉机有如下滑移特性：

(a)当两侧履带沿着同一方向转动时，左右履带的滑移率符号相反，履带速度快的一边滑移率为正值，速度慢的一边滑移率为负值。

(b)当两侧履带转动方向相反时，两个滑移率均为正值。

(c)速度较慢的履带滑移率大于速度快的履带。

综合考虑以上特性，建立滑移率与履带理论速度关系为：

式中n为可调参数，由履带宽度、长度以及履带与地面的接触特性决定，可通过试验辨识获得，sgn为个符号函数，具体定义为，x>0时，sgn(x)＝1；x<0时，sgn(x)＝-1；x＝0时，sgn(x)0。本实施例中n＝1/2。公式(5)和(7)联立可计算得到滑移率。

设计底层控制器。底层控制器采用增量式PID控制算法，如公式(8)所示。式中，K_p，K_i和K_d分别为比例、积分和微分系数，μ(k)表示第k个采样时刻的控制量，本实施例中为舵机角度，其中k为自然数；e(k)表示第k个采样时刻的航向输入偏差。

Δμ＝μ(k)-μ(k-1)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)] (8)

计算履带拖拉机航向变化预估量。航向预估算法的基本思想是预测履带拖拉机的航向变化趋势，并将其计入控制偏差，这样航向变化趋势就可以影响控制器的输出。设采样周期为T，则履带拖拉机的航向变化预估量Δθ(rad)：

基于航向预估模型实现路径跟踪控制。把拖拉机当前航向和航向变化预估量Δθ之和作为航向反馈量e(k)，把期望航向与航向反馈量e(k)差值作为步骤6所述底层控制器的输入以计算控制输出，利用控制输出，控制电控装置动作，实现履带拖拉机转向以跟踪期望路径。基于8航向预估模型的路径跟踪算法控制框图如图3所示。本实施例中履带拖拉机的真实航向和拖拉机速度采用Trimble 982RTK-GNSS双天线接收机测量得到，实际执行过程中也可以通过惯性传感器或多传感器融合方式测量得到。

以下为与上述方法实施例对应的装置实施例，本实施装置可与上述实施方式互相配合实施。上述实施方式中提到的相关技术细节在本实施装置中依然有效，为了减少重复，这里不再赘述。相应地，本实施装置中提到的相关技术细节也可应用在上述实施方式中。

本发明还提出了一种路径跟踪控制系统，其中包括以下模块：

测速模块，用于对拖拉机进行测速，得到该拖拉机的移动速度；

滑移率计算模块，用于根据该拖拉机履带与地面的接触特性、该移动速度以及该拖拉机两侧履带的理论速度，得到该拖拉机两侧履带的滑移率；

航向变化率表达模块，用于根据该滑移率、该拖拉机两侧履带的理论速度和该拖拉机的履带间距，获得该拖拉机的航向变化率表达式；

控制模块，用于根据该航向变化率表达式和采样周期，计算该拖拉机的航向变化预估量，将该航向变化预估量和该拖拉机的当前航向之和作为航向反馈量，根据该航向反馈量和期望航向，控制该拖拉机的舵机角度，以使该拖拉机跟踪期望路径。

该路径跟踪控制系统，其中该滑移率计算模块还包括：根据该拖拉机两侧驱动轮的转速和尺寸，得到该履带拖拉机两侧履带的该理论速度：

v_r＝w_r·r；v_l＝w_l·r

式中，v_l和v_r分别为左右两侧履带的理论速度，w_r为右侧驱动轮转速，w_l为左侧驱动轮转速，r为驱动轮半径。

该路径跟踪控制系统，其中该滑移率计算模块还包括，通过联立以下公式确定该滑移率：

式中n为可调参数，v为该移动速度，v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率。

该路径跟踪控制系统，其中该控制模块还包括：将该航向反馈量和期望航向输入至底层控制器，该底层控制器根据该该航向反馈量和期望航向，生成调整控制量，以控制该拖拉机的舵机角度；

其中该底层控制器采用增量式PID控制算法，通过以下公式生成该调整控制量：Δμ＝μ(k)-μ(k-1)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]，式中K_p，K_i和K_d分别为比例、积分和微分系数，μ(k)表示第k个采样时刻的控制量，e(k)表示第k个采样时刻的航向输入偏差，k为自然数，Δμ为该调整控制量。

如图5所示，本发明还提出了一种采用上述任意一种路径跟踪控制系统的拖拉机，拖拉机上层结构除发动机、传动、转向、液压悬挂、动力输出、电器仪表、驾驶操纵及牵引等系统以外还包括上述路径跟踪控制系统。

Claims (3)

1.一种路径跟踪控制方法，其特征在于，包括：利用拖拉机主驱动轮半径及其转速计算两侧履带的理论速度，结合履带拖拉机滑移特性建立滑移率与该理论速度，建立航向预估模型；根据采样周期和该航向预估模型计算得到航向变化预估量；把拖拉机当前航向和该航向变化预估量之和作为航向反馈量，底层控制器根据期望航向与该航向反馈量，计算得到控制输出，控制输出控制电控装置动作，实现履带拖拉机转向以跟踪期望路径；

所述的路径跟踪控制方法，具体包括以下步骤：

步骤1，通过定位系统对拖拉机进行测速，得到该拖拉机的移动速度；

步骤2，根据该拖拉机履带与地面的接触特性、该移动速度以及该拖拉机两侧履带的理论速度，得到该拖拉机两侧履带的滑移率；

步骤3，根据该滑移率、该拖拉机两侧履带的理论速度和该拖拉机的履带间距，获得该拖拉机的航向变化率表达式作为该航向预估模型；

步骤4，根据该航向预估模型和采样周期，计算该拖拉机的航向变化预估量；

步骤5，将该航向变化预估量和该拖拉机的当前航向之和作为航向反馈量，根据该航向反馈量和期望航向，控制该拖拉机的舵机角度，以使该拖拉机跟踪期望路径；

该步骤2还包括：根据该拖拉机两侧驱动轮的转速和尺寸，得到该履带拖拉机两侧履带的该理论速度：

v_r＝w_r·r

v_l＝w_l·r

式中，v_l和v_r分别为左右两侧履带的理论速度，w_r为右侧驱动轮转速，w_l为左侧驱动轮转速，r为驱动轮半径；

该步骤2还包括，通过联立以下公式确定该滑移率：

式中n为可调参数，v为该移动速度，v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率；

该步骤5还包括：将该航向反馈量和期望航向输入至底层控制器，该底层控制器根据该该航向反馈量和期望航向，生成调整控制量，以控制该拖拉机的舵机角度；

其中该底层控制器采用增量式PID控制算法，通过以下公式生成该调整控制量：

Δμ＝μ(k)-μ(k-1)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]

式中K_p，K_i和K_d分别为比例、积分和微分系数，μ(k)表示第k个采样时刻的控制量，e(k)表示第k个采样时刻的航向输入偏差，k为自然数，Δμ为该调整控制量；

该步骤3还包括，通过以下公式确定该航向变化率表达式：

为航向变化率，a_l和a_r分别为左右两侧履带的滑移率，d为履带间距；

步骤4中的该航向变化预估量通过以下公式确定：

式中v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率，d为履带间距，T为采样周期，Δθ为该航向变化预估量。

2.一种路径跟踪控制系统，其特征在于，包括以下模块：

测速模块，用于对拖拉机进行测速，得到该拖拉机的移动速度；

滑移率计算模块，用于根据该拖拉机履带与地面的接触特性、该移动速度以及该拖拉机两侧履带的理论速度，得到该拖拉机两侧履带的滑移率；

航向变化率表达模块，用于根据该滑移率、该拖拉机两侧履带的理论速度和该拖拉机的履带间距，获得该拖拉机的航向变化率表达式；

控制模块，用于根据该航向变化率表达式和采样周期，计算该拖拉机的航向变化预估量，将该航向变化预估量和该拖拉机的当前航向之和作为航向反馈量，根据该航向反馈量和期望航向，控制该拖拉机的舵机角度，以使该拖拉机跟踪期望路径；

路径跟踪控制方法，具体包括以下步骤：

步骤1，通过定位系统对拖拉机进行测速，得到该拖拉机的移动速度；

步骤2，根据该拖拉机履带与地面的接触特性、该移动速度以及该拖拉机两侧履带的理论速度，得到该拖拉机两侧履带的滑移率；

步骤3，根据该滑移率、该拖拉机两侧履带的理论速度和该拖拉机的履带间距，获得该拖拉机的航向变化率表达式作为该航向预估模型；

步骤4，根据该航向预估模型和采样周期，计算该拖拉机的航向变化预估量；

步骤5，将该航向变化预估量和该拖拉机的当前航向之和作为航向反馈量，根据该航向反馈量和期望航向，控制该拖拉机的舵机角度，以使该拖拉机跟踪期望路径；

该滑移率计算模块还包括：根据该拖拉机两侧驱动轮的转速和尺寸，得到该履带拖拉机两侧履带的该理论速度：

v_r＝w_r·r；v_l＝w_l·r

式中，v_l和v_r分别为左右两侧履带的理论速度，w_r为右侧驱动轮转速，w_l为左侧驱动轮转速，r为驱动轮半径；

该滑移率计算模块还包括，通过联立以下公式确定该滑移率：

式中n为可调参数，v为该移动速度，v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率；

该控制模块还包括：将该航向反馈量和期望航向输入至底层控制器，该底层控制器根据该该航向反馈量和期望航向，生成调整控制量，以控制该拖拉机的舵机角度；

其中该底层控制器采用增量式PID控制算法，通过以下公式生成该调整控制量：

Δμ＝μ(k)-μ(k-1)＝K_p[e(k)-e(k-1)]+K_ie(k)+K_d[e(k)-2e(k-1)+e(k-2)]

式中K_p，K_i和K_d分别为比例、积分和微分系数，μ(k)表示第k个采样时刻的控制量，e(k)表示第k个采样时刻的航向输入偏差，k为自然数，Δμ为该调整控制量；

通过以下公式确定该航向变化率表达式：

为航向变化率，a_l和a_r分别为左右两侧履带的滑移率，d为履带间距；

步骤4中的该航向变化预估量通过以下公式确定：

式中v_l和v_r分别为左右两侧履带的理论速度，a_l和a_r分别为左右两侧履带的滑移率，d为履带间距，T为采样周期，Δθ为该航向变化预估量。

3.一种采用如权利要求2所述的路径跟踪控制系统的拖拉机。

Images