一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法

本发明属于农用车辆自动驾驶，具体涉及拖拉机牵引机组作业路径跟踪控制方法。

背景技术：

1、随着农机大型化的发展，单轴牵引式农具因其灵活、高效等特性被广泛应用于“耕、种、管、收”等农业全生产环节。在拖拉机牵引机组作业过程中，要求机具按照设定轨迹精确作业，以提高作业效率，降低作物损害。但单轴牵引式农具属于单点铰接式的牵引式农具，因其摆向灵活，在面对多曲线作业路径时，存在车头和机具不同轨的现象。

2、中国专利公布号为cn110568758b的文献中公开了一种自动驾驶汽车的参数自适应横向运动lqr控制方法，该方法基于车辆二自由度动力学模型，通过采集路径跟踪误差信息，更新当前状态下的控制器参数，再通过riccati方程迭代求解得到最优控制量，实现了车辆单动力底盘路径跟踪控制。若将这种lqr控制方法应用于拖拉机控制时，存在的问题是：一、仅能保证拖拉机自身的路径跟踪精度，却无法保证拖拉机牵引式农具的作业轨迹跟踪精度，实际上，当无人驾驶时，拖拉机牵引式农具采用的单点铰接的方式，肯定会发生偏移；二、不同于现有的矿用铰接式车辆和半挂汽车动力学模型，具有不同的耦合关系，由拖拉机牵引机组构成的动力学模型，实时路径跟踪控制需要lqr控制器不停迭代由该模型构成的riccati方程，计算反馈系数，这将长时间占用行车控制器芯片计算资源，降低控制算法的实时性，并且影响农具作业单元指令的执行；同时lqr控制器仅针对当前路径信息进行最优控制，无法即时跟踪曲率突变的作业路径。

3、中国专利公布号为cn111532283b的文献中公开了一种路径跟踪方法，通过限制被牵引车的侧向加速度来防止高速侧翻危险，但是，这种方法却无法实现在低速作业下牵引式机组在面对多曲率路径时的作业路径跟踪控制和大角度转弯后的快速、优先上线。

技术实现思路

1、本发明的目的是为了克服上述lqr控制方法以及路径跟踪控制方法在拖拉机牵引机组多曲线无人作业中存在的问题，提供了一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，依据牵引式农具的路径跟踪误差信息，通过控制拖拉机的前轮转角实现拖拉机牵引机组的作业路径的精准跟踪控制。

2、为实现上述目的，本发明一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法采用的技术方案是：

3、步骤1)：建立笛卡尔坐标系下的拖拉机牵引机组的三自由度动力学模型，将所述的三自由度动力学模型转化为frenet坐标系下的拖拉机牵引机组误差模型，该拖拉机牵引机组误差模型包含有路径跟踪误差信息和沿农具质心行驶方向点处的纵向速度分量信息；

4、步骤2)：实时采集农具的经纬度坐标信息、行驶航向角、沿农具质心行驶方向点处的侧向速度分量以及纵向速度分量，预测出预瞄时间，计算出农具预瞄点的位姿信息，规划出作业路径，获得实时的路径跟踪误差信息以及距离所述的作业路径最近的索引点处的农具和拖拉机的曲率；将实时的路径跟踪误差信息以及实时的纵向速度分量输入到步骤1)中所述的拖拉机牵引机组误差模型中，获得当前时刻的拖拉机牵引机组误差模型；

5、步骤3)：计算所述的当前时刻的拖拉机牵引机组误差模型与上一时刻的拖拉机牵引机组误差模型的皮尔森相似度，判断是否更新当前时刻的拖拉机牵引机组误差模型，若是，则重新计算对应于所述的路径跟踪误差信息的最优反馈系数，若否，则存储当前的路径跟踪误差信息的最优反馈系数；

6、步骤4)：基于所述的最优反馈系数以及所述的农具和拖拉机的曲率，计算出农具和拖拉机的前馈角补偿量；

7、步骤5)：根据所述的前馈角补偿量、所述的最优反馈系数以及所述的路径跟踪误差信息计算出拖拉机的前轮转角最优控制量，实现拖拉机牵引机组的作业路径跟踪控制。

8、进一步地，步骤1)中所述的拖拉机牵引机组误差模型为：ne是系统矩阵，me是状态常数矩阵，pe是控制矩阵，控制量u＝δ，δ为拖拉机前轮转角，所述的路径跟踪误差信息为农具从车身坐标系到期望坐标系的横向误差距离，分别为农具和拖拉机的航向误差，上标为的一阶导数。

9、更进一步地，步骤3)中，所述的皮尔森相似度将相似度与阈值ρref相比较，当相似度时，将ne更新为重新计算最优反馈系数，ne分别为当前时刻、上一时刻的拖拉机牵引机组误差模型，下标ij表示系统矩阵ne第i行，第j列的元素，上标为其均值。

10、更进一步地，步骤3)中所述的最优反馈系数k1,k2,k3,k4,k5,k6分别为路径跟踪误差信息e中的的最优反馈系数，r为控制量的权重矩阵，i为单位矩阵，p1为正定矩阵，用拉格朗日乘数法构造在有约束情况下的代价函数并求代价函数的最小值，构建riccati方程并迭代求解方程得到正定矩阵p1。

11、本发明采用上述技术方案后的有益效果为：

12、1、本发明针对拖拉机牵引机组在多曲线无人作业时对路径跟踪横向精度的需求，提出了一种改进lqr的路径跟踪控制方法，本发明所设计的误差模型依据单轴牵引式农具预瞄点处的路径跟踪误差信息以及拖拉机当前位姿信息，控制拖拉机前轮转角，解决了无人驾驶时牵引式农具作业轨迹偏移的问题，并提高了在规划路径曲率突变处的农具作业跟踪精度。

13、2、本发明采用改进的lqr控制方法，通过皮尔森相关性系数判断是否更新当前时刻系统误差模型和最优反馈系数，减少了相同作业工况下行车控制器的无效计算，提高了控制算法的实时性；通过引入农具预瞄点处和拖拉机当前坐标点处规划路径的曲率信息，加入前馈补偿量，提高了农具在多曲率路径跟踪时的作业精度和大角度转弯后的快速、优先上线。

技术特征：

1.一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是包括：

2.根据权利要求1所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：步骤1)中所述的拖拉机牵引机组误差模型为：ne是系统矩阵，me是状态常数矩阵，pe是控制矩阵，控制量u＝δ，δ为拖拉机前轮转角，所述的路径跟踪误差信息为农具从车身坐标系到期望坐标系的横向误差距离，分别为农具和拖拉机的航向误差，上标为的一阶导数。

3.根据权利要求2所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：步骤3)中，所述的皮尔森相似度将相似度与阈值ρref相比较，当相似度时，将ne更新为重新计算最优反馈系数，ne分别为当前时刻、上一时刻的拖拉机牵引机组误差模型，下标ij表示系统矩阵ne第i行，第j列的元素，上标为其均值。

4.根据权利要求3所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：步骤3)中，所述的最优反馈系数k1,k2,k3,k4,k5,k6分别为路径跟踪误差信息e中的的最优反馈系数，r为控制量的权重矩阵，i为单位矩阵，p1为正定矩阵，用拉格朗日乘数法构造在有约束情况下的代价函数并求代价函数的最小值，构建riccati方程并迭代求解方程得到正定矩阵p1。

5.根据权利要求4所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：根据式

6.根据权利要求5所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：步骤5)中所述的拖拉机的前轮转角最优控制量

7.根据权利要求1所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：步骤1)中，将拖拉机牵引机组简化为两个刚体的二维平面运动，分别建立拖拉机和农具的二自由度动力学方程；将拖拉机与农具的铰接点处视为两个刚体的接触运动，构建两者的耦合关系方程，将耦合关系方程带入所述的二自由度动力学方程中，定义状态变量，根据状态变量得到所述的三自由度动力学模型。

8.根据权利要求7所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：所述的状态变量三自由度动力学模型的状态空间方程是x的一阶导数，m是状态常数矩阵；n是系统矩阵；p是控制矩阵，控制量u＝δ，δ为拖拉机前轮转角，为垂直于农具质心点处的侧向速度分量，ωt,ωl分别为拖拉机、农具的横摆角速度，分别为拖拉机和农具的航向角。

9.根据权利要求7所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：所述的状态常数矩阵

10.根据权利要求2所述的一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法，其特征是：

技术总结
本发明公开农用车辆自动驾驶领域中的一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，将笛卡尔坐标系下的拖拉机牵引机组的三自由度动力学模型转化为Frenet坐标系下的拖拉机牵引机组误差模型，将实时的路径跟踪误差信息以及实时的纵向速度分量输入误差模型中得当前时刻的误差模型；计算当前时刻的误差模型与上一时刻的误差模型的皮尔森相似度，判断是否更新当前时刻的误差模型，若是，则重新计算对应于路径跟踪误差信息的最优反馈系数，最后计算出农具和拖拉机的前馈角补偿量和拖拉机的前轮转角最优控制量；本发明解决了无人驾驶时牵引式农具作业轨迹偏移的问题，提高了农具在多曲率路径跟踪时的作业精度和大角度转弯后的快速、优先上线。

技术研发人员：魏新华,王晔飞,刘成良,汪岸哲,王庆壮,邓屹,张永杰,杨家鑫,章少岑,吉鑫
受保护的技术使用者：江苏大学
技术研发日：
技术公布日：2024/1/15