一种基于稳定模型辨识算法的方向盘履带式拖拉机直线跟踪控制系统
本发明属于农业机械自动驾驶与智能控制，具体涉及履带式拖拉机路径跟踪控制技术，尤其涉及一种基于稳定模型辨识算法的方向盘履带式拖拉机直线跟踪控制系统。本发明面向方向盘操纵式履带拖拉机的自动导航作业需求，通过实车轨迹数据辨识方向盘角度与车辆转向曲率之间的非线性映射关系，并将辨识结果用于路径跟踪控制器的构建，以实现履带式拖拉机对参考路径的稳定跟踪。该技术可应用于智能农机、无人驾驶农业装备、农田导航作业系统及相关自动化控制场景。
背景技术：
1、随着智慧农业和农业装备自动化技术的发展，履带式拖拉机在耕整、开沟、播种、施肥、植保等农业作业场景中的自动导航需求不断增加。路径跟踪控制是农业车辆自动驾驶系统中的关键环节，其控制精度和稳定性直接影响农机作业精度、重复作业误差、地块利用效率以及整机自动化作业质量。因此，研究适用于履带式拖拉机的高精度、高稳定性路径跟踪控制方法具有重要意义。
2、现有农业车辆路径跟踪控制方法主要包括纯追踪控制方法、stanley控制方法以及模型预测控制方法等。纯追踪控制方法通过选取前视目标点计算转向控制量，算法结构简单、计算量小，但该方法主要依赖车辆与预瞄点之间的几何关系，对车辆真实转向响应特性考虑不足，在路径曲率变化明显或地面扰动较大的情况下，容易出现跟踪滞后、转向抖动和局部偏离等问题。stanley控制方法通过横向误差和航向误差构造控制律，在直线路径或缓弯路径当中具有一定效果，但其目标点或目标段的选取依赖当前位置与参考路径之间的匹配关系，当路径点分布密集、存在局部回环或作业边界复杂时，容易出现目标匹配不稳定的问题，从而影响闭环控制效果。
3、模型预测控制方法能够基于车辆预测模型滚动优化未来控制输入序列，在理论上可兼顾路径跟踪精度、控制平顺性和约束处理能力，在农业车辆自动驾驶中具有较好的应用前景。但模型预测控制的实际效果高度依赖预测模型的准确性。对于方向盘操纵式履带拖拉机而言，车辆虽然通过方向盘输入转向指令，但其实际转向过程由左右履带与地面之间的相互作用共同决定。受履带结构差异、履带张紧状态、地面附着条件、转向传动间隙、履带滑移以及整机装配误差等因素影响，方向盘角度与车辆转向曲率之间通常呈现明显的非线性关系，并且左转和右转特性并不完全对称。若模型预测控制器仍采用理想线性模型或经验模型，会很难去准确地描述车辆真实运动规律，容易造成预测轨迹与实际响应之间的模型失配，从而导致路径跟踪误差增大、方向盘控制量振荡以及闭环稳定性下降。
4、在农业作业环境中，车辆状态和试验数据还会受到多种非理想因素影响。例如，轮速传感器可能存在系统性测量偏差，履带滑移和地面附着变化会导致车辆纵向速度估计失准；gps或北斗定位信号易受噪声、漂移和采样不规则等因素影响，造成轨迹数据不平滑、初始航向估计不准确以及车辆状态估计不稳定。若不对上述数据误差进行处理和补偿，难以辨识得到能够真实反映车辆转向特性的模型，也难以进一步构建具有工程适用性的路径跟踪控制器。
5、因此，现有技术中仍缺乏一种能够面向方向盘履带式拖拉机实际转向特性，将实车轨迹数据辨识得到的非线性转向模型与路径跟踪控制器相结合的技术方案。特别是在复杂地面条件、非理想测量环境以及对控制平稳性和跟踪精度要求较高的农业作业场景中，需要一种能够同时考虑车辆转向非线性、左右转向不对称、速度测量偏差和定位噪声影响的路径跟踪控制方法，以提高履带式拖拉机自动导航作业的可靠性、稳定性和实用性。
技术实现思路
1、本发明的目的在于提供一种基于稳定模型辨识算法的方向盘履带式拖拉机直线跟踪控制系统，以解决现有农业履带车辆路径跟踪控制过程中预测模型真实性不足、车辆转向响应非线性难以准确表征、左右转向特性不对称以及控制器对复杂地面工况适应能力不足的问题。
2、本发明提供一种基于稳定模型辨识算法的方向盘履带式拖拉机直线跟踪控制系统，其包括：
3、数据采集单元，用于对方向盘履带式拖拉机进行多组固定方向盘角度试验，采集车辆在不同方向盘角度输入下的轨迹数据，轨迹数据包括时间戳、车辆平面位置坐标、左履带速度和右履带速度；
4、数据预处理单元，用于对轨迹数据进行预处理，对时间戳进行修正，对车辆平面位置坐标进行平滑处理，根据平滑轨迹起始段的运动方向估计初始车辆航向角；
5、建模模块，用于根据左履带速度、右履带速度、初始车辆航向角、速度校正因子以及方向盘角度所对应的转向曲率构建方向盘履带式拖拉机离散运动学模型；
6、稳定模型辨识单元，用于将所有试验轨迹的平均误差作为全局评价函数全局搜索最优速度校正因子，基于最优速度校正因子，对每次试验轨迹分别辨识最优转向曲率和初始航向修正量；基于各方向盘角度的最优转向曲率初始拟合左转区域转向曲率模型和右转区域转向曲率模型；将速度校正因子、左转区域转向曲率模型和右转区域转向曲率模型的多项式系数组成统一参数向量；利用初始航向修正量对初始车辆航向角进行修正后基于方向盘履带式拖拉机离散运动学模型进行轨迹拟合，将全部轨迹的轨迹拟合误差平方和作为目标函数进行多参数联合精调得到最优统一参数向量以得到方向盘角度和车辆转向曲率的稳定辨识模型；
7、路径跟踪模块，用于在路径跟踪过程中调用该辨识模型进行车辆状态预测和方向盘控制量优化以跟踪目标参考路径。
8、作为优选，轨迹数据表示为：
9、
10、其中，di表示第i条轨迹数据，t(k)为采样时间戳，x(k)、y(k)为车辆平面位置坐标，、分别为左履带速度和右履带速度，为该条轨迹对应的固定方向盘角度，为采样点数；
11、对所述车辆平面位置坐标进行平滑处理表示为：
12、
13、
14、其中，、为平滑后的参考轨迹坐标，表示滑动均值运算，win为平滑窗口宽度；
15、根据平滑轨迹起始段的运动方向估计初始航向角表示为：
16、
17、其中，为起始段中与轨迹起点距离超过设定阈值的采样点，该步骤能够降低采样异常、定位噪声和初始姿态误差对后续模型辨识的影响；
18、方向盘履带式拖拉机离散运动学模型的构建方式包括：
19、车辆平均速度表示为：
20、
21、引入速度校正因子α对履带滑移和轮速测量偏差进行补偿以得到车辆速度表示为：
22、
23、车辆角速度与转向曲率之间的关系表示为：
24、
25、其中，为方向盘角度对应的车辆转向曲率；
26、按照离散时间运动学方程递推车辆轨迹表示为：
27、
28、
29、
30、其中，、为模型仿真得到的轨迹坐标，为车辆航向角。
31、作为优选，将所有试验轨迹的平均误差作为速度校正因子的全局评价函数以全局搜索确定最优速度校正因子包括：
32、定义第i条轨迹在给定曲率和速度校正因子条件下的轨迹均方根误差表示为：
33、
34、以所有轨迹的平均误差作为速度校正因子的全局评价函数表示为：
35、
36、
37、其中，m为试验轨迹总数，α*为最优速度校正因子；
38、基于所述最优速度校正因子，对每次试验的运动轨迹分别辨识最优转向曲率和初始航向修正量表示为：
39、
40、修正后的初始航向角为：
41、；
42、其中，表示修正前的航向角。
43、作为优选，左转区域转向曲率模型和右转区域转向曲率模型的表达式为：
44、
45、
46、其中，表示左转区域转向曲率模型，表示右转区域转向曲率模型，至以及至分别为左右转向多项式系数；
47、统一参数向量表示为：
48、；
49、以全部试验轨迹的轨迹拟合误差平方和作为目标函数表示为：
50、
51、方向盘角度和车辆转向曲率的稳定辨识模型表示为：
52、。
53、作为优选，所述路径跟踪模块包括：
54、参考路径处理单元，用于对参考路径进行弧长参数化处理；设目标参考路径由离散路径点组成，对其按固定弧长间距ds进行重采样，建立路径查找表，路径航向角由相邻路径点计算，路径曲率采用三点法计算；搜索当前匹配参考点并提取预测窗口参考序列，在每个控制周期，根据车辆当前位置在路径查找表中进行局部窗口搜索以确定未来预测时域内的参考位置、参考航向角和参考路径曲率；
55、模型预测控制单元，用于构建内嵌方向盘角度和车辆转向曲率的稳定辨识模型的车辆预测方程，对于预测时域内第m个预测步，给定方向盘控制量，由方向盘角度和车辆转向曲率的稳定辨识模型计算预测转向曲率，并根据离散运动学模型预测车辆未来状态，未来状态包括预测位置和预测航向角；构建路径跟踪误差和前馈方向盘角，在每个预测步，定义车辆预测位置相对于参考路径的横向误差和航向角误差；根据预测窗口内的参考路径曲率通过辨识模型逆映射获得对应的前馈方向盘角；构建包含横向误差项、航向误差项、前馈偏差项和控制增量项的综合优化目标函数并设置控制约束，优化变量为未来控制时域内的方向盘角序列，求解滚动时域优化问题并输出控制指令。
56、作为优选，路径查找表表示为：
57、
58、其中，、为参考路径点坐标，为参考路径航向角，为参考路径曲率，为弧长索引；
59、参考路径航向角由相邻路径点计算表示为：
60、
61、参考路径曲率采用三点法计算表示为：
62、。
63、作为优选，根据车辆当前位置在路径查找表中进行局部窗口搜索表示为：
64、
65、
66、其中，p*表示当前时刻找到的最优匹配点的索引，为上一控制周期的参考点索引，w为局部搜索窗口宽度，p为当前控制周期更新后的匹配点索引；
67、为补偿计算延迟和模型误差，在当前匹配点前方设置前瞻距离，并确定预测窗口起始索引表示为：
68、
69、
70、其中，nla表示前视步数，nsafe表示安全边界；
71、根据车辆速度 v、控制周期 tc 和固定弧长间距 ds 确定预测窗口步长idxstep表示为：
72、
73、
74、其中，
wm表示第m个参考点；
75、由此得到预测时域内的参考序列表示为：
76、。
77、作为优选，构建内嵌辨识模型的车辆预测方程，对于预测时域内第 m步，给定方向盘控制量，由辨识模型计算车辆转向曲率表示为：
78、
79、并根据离散运动学模型预测车辆未来状态表示为：
80、
81、
82、；
83、在每个预测步，定义车辆预测位置相对于参考路径的横向误差表示为：
84、
85、定义航向角误差表示为：
86、
87、进一步根据预测窗口内的参考路径曲率，通过辨识模型逆映射获得对应的前馈方向盘角表示为：
88、
89、其中，可通过预先构建方向盘角度—转向曲率查找表并采用线性插值方式获得。
90、作为优选，构建包含横向误差项、航向误差项、前馈偏差项和控制增量项的综合优化目标函数并设置控制约束表示为：
91、设控制时域为，预测时域为，优化变量为未来控制时域内的方向盘角序列表示为：
92、
93、当 m>时，令=，即控制时域之后保持最后一个控制量不变；构建综合目标函数表示为：
94、
95、其中，为横向误差权重，为航向误差权重，为前馈偏差权重，为控制增量权重，w(m)为预测时域加权因子；
96、根据方向盘执行机构的物理限制，设置方向盘角度约束和方向盘变化率约束表示为：
97、
98、；
99、求解滚动时域优化问题并输出控制指令表示为：
100、
101、为提高求解效率，将上一控制周期的最优控制序列移位后作为当前周期的初始值：
102、
103、求得最优控制序列后，仅执行其中第一项：
104、。
105、相比于现有技术，本发明具有以下有益效果：
106、本发明针对方向盘操纵式履带拖拉机在实际作业中存在的履带滑移、轮速测量偏差、地面附着条件变化、定位数据噪声以及采样时间不规则等因素，本发明通过实车轨迹数据构建车辆转向特性辨识模型，对方向盘角度与车辆转向曲率之间的实际映射关系进行稳定辨识，并将速度尺度修正、初始航向修正和左右转向分段建模纳入统一辨识框架，从而提高模型对车辆真实运动规律的表达能力。
107、本发明的一种基于稳定模型辨识算法的方向盘履带式拖拉机直线跟踪控制系统进一步将所述稳定辨识模型嵌入模型预测控制器的预测环节，在滚动优化过程中不再依赖理想线性模型或经验转向参数，而是基于实车辨识得到的非线性转向曲率模型预测车辆未来运动状态。通过参考路径参数化、预测窗口前向提取、前馈转向补偿以及控制增量约束等机制，使车辆转向特性建模结果能够直接服务于路径跟踪控制过程，提高方向盘履带式拖拉机在农业作业路径上的跟踪精度、控制平顺性和闭环稳定性。
108、本发明旨在形成一种由“实车数据采集—轨迹误差驱动辨识—非线性转向模型建立—模型预测控制器构建—闭环执行反馈”组成的路径跟踪控制技术路线，使车辆模型辨识结果与控制器预测模型之间建立明确的数据传递关系和功能耦合关系，减少传统控制方法对理想化车辆模型、人工经验参数和反复试验整定的依赖，提高方向盘履带式拖拉机自动导航控制系统的工程适用性和推广价值。