CN116679725A - 一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法 - Google Patents

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

Info

Publication number
CN116679725A
CN116679725A CN202310833636.3A CN202310833636A CN116679725A CN 116679725 A CN116679725 A CN 116679725A CN 202310833636 A CN202310833636 A CN 202310833636A CN 116679725 A CN116679725 A CN 116679725A
Authority
CN
China
Prior art keywords
tractor
path tracking
farm
farm tool
unit
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310833636.3A
Other languages
English (en)
Inventor
魏新华
王晔飞
刘成良
汪岸哲
王庆壮
邓屹
张永杰
杨家鑫
章少岑
吉鑫
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University
Original Assignee
Jiangsu University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu University filed Critical Jiangsu University
Priority to CN202310833636.3A priority Critical patent/CN116679725A/zh
Publication of CN116679725A publication Critical patent/CN116679725A/zh
Pending legal-status Critical Current

Links

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Guiding Agricultural Machines (AREA)

Abstract

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

Description

一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法
技术领域
本发明属于农用车辆自动驾驶技术领域，具体涉及拖拉机牵引机组作业路径跟踪控制方法。
背景技术
随着农机大型化的发展，单轴牵引式农具因其灵活、高效等特性被广泛应用于“耕、种、管、收”等农业全生产环节。在拖拉机牵引机组作业过程中，要求机具按照设定轨迹精确作业，以提高作业效率，降低作物损害。但单轴牵引式农具属于单点铰接式的牵引式农具，因其摆向灵活，在面对多曲线作业路径时，存在车头和机具不同轨的现象。
中国专利公布号为CN110568758B的文献中公开了一种自动驾驶汽车的参数自适应横向运动LQR控制方法，该方法基于车辆二自由度动力学模型，通过采集路径跟踪误差信息，更新当前状态下的控制器参数，再通过Riccati方程迭代求解得到最优控制量，实现了车辆单动力底盘路径跟踪控制。若将这种LQR控制方法应用于拖拉机控制时，存在的问题是：一、仅能保证拖拉机自身的路径跟踪精度，却无法保证拖拉机牵引式农具的作业轨迹跟踪精度，实际上，当无人驾驶时，拖拉机牵引式农具采用的单点铰接的方式，肯定会发生偏移；二、不同于现有的矿用铰接式车辆和半挂汽车动力学模型，具有不同的耦合关系，由拖拉机牵引机组构成的动力学模型，实时路径跟踪控制需要LQR控制器不停迭代由该模型构成的Riccati方程，计算反馈系数，这将长时间占用行车控制器芯片计算资源，降低控制算法的实时性，并且影响农具作业单元指令的执行；同时LQR控制器仅针对当前路径信息进行最优控制，无法即时跟踪曲率突变的作业路径。
中国专利公布号为CN111532283B的文献中公开了一种路径跟踪方法，通过限制被牵引车的侧向加速度来防止高速侧翻危险，但是，这种方法却无法实现在低速作业下牵引式机组在面对多曲率路径时的作业路径跟踪控制和大角度转弯后的快速、优先上线。
发明内容
本发明的目的是为了克服上述LQR控制方法以及路径跟踪控制方法在拖拉机牵引机组多曲线无人作业中存在的问题，提供了一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，依据牵引式农具的路径跟踪误差信息，通过控制拖拉机的前轮转角实现拖拉机牵引机组的作业路径的精准跟踪控制。
为实现上述目的，本发明一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法采用的技术方案是：
步骤1)：建立笛卡尔坐标系下的拖拉机牵引机组的三自由度动力学模型，将所述的三自由度动力学模型转化为Frenet坐标系下的拖拉机牵引机组误差模型，该拖拉机牵引机组误差模型包含有路径跟踪误差信息和沿农具质心行驶方向点处的纵向速度分量信息；
步骤2)：实时采集农具的经纬度坐标信息、行驶航向角、沿农具质心行驶方向点处的侧向速度分量以及纵向速度分量，预测出预瞄时间，计算出农具预瞄点的位姿信息，规划出作业路径，获得实时的路径跟踪误差信息以及距离所述的作业路径最近的索引点处的农具和拖拉机的曲率；将实时的路径跟踪误差信息以及实时的纵向速度分量输入到步骤1)中所述的拖拉机牵引机组误差模型中，获得当前时刻的拖拉机牵引机组误差模型；
步骤3)：计算所述的当前时刻的拖拉机牵引机组误差模型与上一时刻的拖拉机牵引机组误差模型的皮尔森相似度，判断是否更新当前时刻的拖拉机牵引机组误差模型，若是，则重新计算对应于所述的路径跟踪误差信息的最优反馈系数，若否，则存储当前的路径跟踪误差信息的最优反馈系数；
步骤4)：基于所述的最优反馈系数以及所述的农具和拖拉机的曲率，计算出农具和拖拉机的前馈角补偿量；
步骤5)：根据所述的前馈角补偿量、所述的最优反馈系数以及所述的路径跟踪误差信息计算出拖拉机的前轮转角最优控制量，实现拖拉机牵引机组的作业路径跟踪控制。
进一步地，步骤1)中所述的拖拉机牵引机组误差模型为：Ne是系统矩阵，Me是状态常数矩阵，Pe是控制矩阵，控制量u＝δ，δ为拖拉机前轮转角，所述的路径跟踪误差信息/>为农具从车身坐标系到期望坐标系的横向误差距离，/>分别为农具和拖拉机的航向误差，上标为/>的一阶导数。
更进一步地，步骤3)中，所述的皮尔森相似度将相似度/>与阈值ρref相比较，当相似度/>时，将Ne更新为/>重新计算最优反馈系数，/>Ne分别为当前时刻、上一时刻的拖拉机牵引机组误差模型，下标ij表示系统矩阵Ne第i行，第j列的元素，上标为其均值。
更进一步地，步骤3)中所述的最优反馈系数k1,k2,k3,k4,k5,k6分别为路径跟踪误差信息E中的/>的最优反馈系数，R为控制量的权重矩阵，/>I为单位矩阵，/>p1为正定矩阵，用拉格朗日乘数法构造在有约束情况下的代价函数并求代价函数的最小值，构建Riccati方程并迭代求解方程得到正定矩阵p1。
本发明采用上述技术方案后的有益效果为：
1、本发明针对拖拉机牵引机组在多曲线无人作业时对路径跟踪横向精度的需求，提出了一种改进LQR的路径跟踪控制方法，本发明所设计的误差模型依据单轴牵引式农具预瞄点处的路径跟踪误差信息以及拖拉机当前位姿信息，控制拖拉机前轮转角，解决了无人驾驶时牵引式农具作业轨迹偏移的问题，并提高了在规划路径曲率突变处的农具作业跟踪精度。
2、本发明采用改进的LQR控制方法，通过皮尔森相关性系数判断是否更新当前时刻系统误差模型和最优反馈系数，减少了相同作业工况下行车控制器的无效计算，提高了控制算法的实时性；通过引入农具预瞄点处和拖拉机当前坐标点处规划路径的曲率信息，加入前馈补偿量，提高了农具在多曲率路径跟踪时的作业精度和大角度转弯后的快速、优先上线。
附图说明
下面结合附图和具体实施方式对本发明作进一步详细描述：
图1为本发明一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法的控制框图；
图2为拖拉机牵引机组基于笛卡尔坐标系的动力学单轨模型的系统受力示意图；。
图3为本发明的拖拉机牵引机组基于笛卡尔坐标系的动力学单轨模型的系统速度示意图。
图4为本发明的拖拉机牵引机组基于Frenet坐标系的误差模型示意图。
具体实施方式
本发明提出的一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，其中，拖拉机牵引机组由拖拉机与单轴牵引式农具组成，两者通过单点铰接连接。本发明将铰接处的机组运动简化为仅产生绕Z轴的横摆运动，其中Z轴为图2中原点O处垂直于XY平面的坐标轴，依据农具的作业路径跟踪误差信息，控制拖拉机前轮转向角，尽可能使其沿着导航模块规划的作业路径行驶。
如图1所示，本发明首先建立笛卡尔坐标系下的拖拉机牵引机组的三自由度动力学模型，将三自由度动力学模型转化为Frenet坐标系下的拖拉机牵引机组误差模型，该拖拉机牵引机组误差模型包含有路径跟踪误差信息和沿农具质心行驶方向点处的纵向速度分量信息。将获得的拖拉机牵引机组误差模型与安装在机组上的导航单元(GNSS)和惯性测量单元(IMU)相连接，采用导航单元和惯性测量单元实时采集农具经纬度坐标信息、行驶航向角、垂直于农具质心点处的侧向速度分量以及沿农具质心行驶方向点处的纵向速度分量，经预测模块预测出预瞄时间，计算出农具预瞄点的位姿信息，再反馈给导航单元，导航单元规划出农具的作业路径，获得到拖拉机牵引机组的路径跟踪误差信息以及农具和拖拉机距离规划作业路径最近的索引点处的农具和拖拉机的曲率；再依据当前时刻的路径跟踪误差信息以及农具质心行驶方向点处的纵向速度分量得到当前时刻的拖拉机牵引机组误差模型，计算当前时刻的拖拉机牵引机组误差模型与上一时刻的拖拉机牵引机组误差模型的皮尔森相关性系数，判断是否更新当前时刻的拖拉机牵引机组误差模型，若是，则重新计算对应于所述的路径跟踪误差信息的最优反馈系数，若否，则存储当前的对应于所述的路径跟踪误差信息的最优反馈系数。最后，基于最优反馈系数以及农具和拖拉机的曲率计算出农具和拖拉机的前馈角补偿量和拖拉机的前轮转角最优控制量并输出，实现拖拉机牵引机组的作业路径跟踪控制。具体步骤如下：
步骤1：建立笛卡尔坐标系下的拖拉机牵引机组的三自由度动力学模型。
如图2和图3所示，设拖拉机牵引机组的运动是一个在二维平面上且左右轮胎对称的运动，先构建笛卡尔坐标系下的拖拉机牵引机组的动力学单轨模型：
将整个拖拉机牵引机组视为一个机组系统，图2是拖拉机牵引机组基于笛卡尔坐标系的系统受力图，其中，ct,cl分别为拖拉机和农具的车辆质心，h为机组的铰接点，a，b，c分别为拖拉机前轴、后轴和铰接点h到拖拉机质心ct的距离，d，e分别为机组的铰接点h、后轴到农具质心cl的距离；δ为拖拉机前轮转角，分别为拖拉机和农具的航向角，λ为机组铰接点h处的夹角，定义/>分别为垂直于拖拉机前轮、拖拉机后轮、铰接点h的侧向力；/>分别为垂直于单轴农具铰接点h和单轴农具后轮的侧向力。
参见图3中拖拉机牵引机组基于笛卡尔坐标系的动力学单轨模型的系统不同位置速度分量图，其中，分别为垂直于拖拉机质心ct点处、农具质心cl点处的侧向速度分量；/>分别为沿拖拉机质心ct行驶方向、农具质心cl行驶方向点处的纵向速度分量；ωt,ωl分别为拖拉机、农具的横摆角速度。
通过将拖拉机牵引机组简化为两个刚体的二维平面运动，根据牛顿-欧拉法，分别建立拖拉机和农具的二自由度动力学方程：
拖拉机的二自由度动力学方程如下：
农具的二自由度动力学方程如下：
式中mt,ml分别为拖拉机和农具的质量；分别为拖拉机和农具的转动惯量；分别为垂直于拖拉机质心ct、农具质心cl点处的侧向加速度；/>分别为拖拉机和农具的横摆角加速度。
通过对拖拉机与农具的多刚体运动学分析，将铰接点h处视为两个刚体的接触运动，在静力分析时候引入外部作用力和力矩，构建两者的耦合关系方程如下：
式中，分别为拖拉机前轮、拖拉机后轮和农具后轮的侧偏角。
通过将铰接点h处的上述的耦合关系方程带入上述的车辆二自由度动力学方程中，并假设牵引机组的轮胎始终工作在线性区域，即轮胎侧向力F＝Cα，式中C,α分别为轮胎的侧偏刚度和侧偏角，其中：轮胎侧向力 分别为拖拉机前轮，拖拉机后轮和农具后轮的侧偏刚度。
将垂直于农具质心cl点处的侧向速度分量拖拉机的横摆角速度ωt、农具的横摆角速度ωl、拖拉机的航向角/>农具的航向角/>作为状态变量，因此，定义状态变量为为实数集。如图2所示，根据状态变量X得到笛卡尔坐标系下基于牵引式农具侧向速度、牵引式农具横摆角速度和拖拉机横摆角速度的三自由度动力学模型的状态空间方程：/>式中，/>是X的一阶导数：
模型的状态常数矩阵模型的系统矩阵
模型的控制矩阵模型的控制量U＝δ，δ为拖拉机前轮转角，分别为拖拉机和农具的转动惯量。由系统矩阵N可知，该三自由度动力学模型包含沿农具质心行驶方向点处的纵向速度分纵向速度分量/>的信息。
步骤2：将笛卡尔坐标系下的拖拉机牵引机组的三自由度动力学模型转化为基于Frenet坐标系的拖拉机牵引机组的作业路径跟踪误差模型，即拖拉机牵引机组误差模型。
如图4所示的基于Frenet坐标系的牵引式农具机组的误差模型，其中S为通过设置在机组上的导航单元规划出的农具作业路径，定义沿作业路径S行进方向分别为农具期望坐标系纵轴拖拉机期望坐标系纵轴/>沿纵轴逆时针旋转90度分别为农具期望坐标系横轴/>拖拉机期望坐标系横轴/>定义沿农具和拖拉机质心处的行驶速度/>的方向分别为农具、拖拉机的车身坐标系纵轴τl、τt，沿纵轴逆时针旋转90度分别为农具、拖拉机车身坐标系横轴nl、nt。定义/>为农具从车身坐标系到期望坐标系的横向误差距离，/>分别为忽略质心侧偏角的农具和拖拉机的航向误差，基于Frenet坐标系可得两坐标系转换公式为：
式中，上标和分别为状态量对时间的一阶导数和二阶导数；/>为农具期望坐标系纵轴/>与笛卡尔坐标系X轴的夹角，并将其定义为农具期望航向角/>为拖拉机期望坐标系纵轴/>与笛卡尔坐标系X轴的夹角，并将其定义为拖拉机期望航向角/>
将两坐标系转换公式带入上述的三自由度动力学模型，定义状态误差变量即路径跟踪误差信息由此构建由牵引农具的横向、航向误差，拖拉机的航向误差组成的基于Frenet坐标系的拖拉机牵引机组误差模型为：/>式中：
u＝δ，δ为拖拉机前轮转角，/>分别为拖拉机和农具的转动惯量。
式中，Me,Ne,Pe,u分别为作业路径跟踪误差模型的状态常数矩阵，系统矩阵，控制矩阵和控制量。该拖拉机牵引机组误差模型包含有路径跟踪误差信息E和沿农具质心行驶方向点处的纵向速度分量的信息。
步骤3：计算农具预瞄点处的位姿信息，反馈导航单元，获取作业路径曲率信息Kl,Kt和机组路径跟踪误差信息E。
参见图1所示，机组通过安装其上的导航单元(GNSS)实时采集当时时刻的农具经纬度坐标信息通过安装其上的惯性测量单元(IMU)获得农具行驶航向角/>垂直于农具质心cl点处的侧向速度分量/>以及沿农具质心cl行驶方向点处的纵向速度分量/>导航单元和惯性测量单元将获得的信息全部传送给预测模块，预测模块接收到这些信息，经预测模块预测出预瞄时间ts，并根据下式计算出农具预瞄点的位姿信息，即农具预瞄点的经纬度坐标/>和航向角/>
将预瞄点的经纬度坐标和航向角/>作为农具的位姿信息反馈给导航单元，导航单元据此信息可以规划出作业路径。
导航单元根据预测模块反馈的农具的预瞄点的位姿信息和自身实时采集的拖拉机实时位姿信息/>分别寻找农具和拖拉机距离规划作业路径最近的索引点，获取索引点处的农具和拖拉机的曲率信息Kl,Kt，并将农具的预瞄点的位姿信息和农具最近索引点进行导航误差解算，拖拉机实时位姿信息/>和拖拉机最近索引点进行导航误差解算，得到实时计算的实时的拖拉机牵引机组路径跟踪误差信息E。其中,/>为拖拉机当前实时的经纬度坐标，/>为拖拉机当前实时航向角，Kl为农具索引点处的曲率，Kt为拖拉机索引点处的曲率。
步骤4：依据当前农具作业时其质心行驶方向点处的纵向速度分量判断是否更新上述拖拉机牵引机组误差模型。
假设拖拉机牵引机组车辆状态参数不变，将实时的路径跟踪误差信息E以及农具作业实时的纵向速度分量带入当前时刻的拖拉机牵引机组误差模型/>为最新的当前时刻的Ne，/>使用皮尔森(Pearson)相关性系数，计算/>与上一时刻的模型Ne的相似度，计算公式如下：
式中，下标ij表示系统矩阵Ne第i行，第j列的元素，上标为其均值。
依据行车控制器的芯片算力，设置更新阈值ρref，ρref∈[0.8,0.95]，将相似度与阈值ρref相比较，当相似度/>时，说明当前路径跟踪工况与上一时刻差异较大，执行步骤5，需将误差模型的系统矩阵更新为/>重新计算最优反馈系数k，将重新计算出的最优反馈系数k存储在在存储单元中；反之，则跳过步骤5，直接执行步骤6。
在该算法第一次循环时，由于存储单元中的最优反馈系数k为空值，则默认皮尔森相似度小于更新阈值ρref，即必须执行步骤5。
步骤5：将当前时刻的拖拉机牵引机组误差模型转化为离散系统，通过迭代Riccati计算出最优反馈系数k，并存储在存储单元中。
基于导航路径由离散的坐标点构成，对机组误差模型选取固定时间步长并采用向前和中点欧拉法进行离散化处理：
式中，下标τ代表离散化后的时刻， uτ＝δ，I为单位矩阵。
根据上述公式，使用拉格朗日乘数法构造在有约束情况下的代价函数J，利用向量导数求代价函数J的最小值，并构建Riccati方程pτ-1：
式中，τ＝1,2,3…n-1；n为迭代次数；上标T为矩阵转置；γ为约束条件系数；Q,R分别为状态量和控制量的权重矩阵。
令pτ＝Q，迭代求解Riccati方程得到稳态解p1，其中p1为正定矩阵，并计算反馈系数：
式中，其中k1,k2,k3,k4,k5,k6分别对应上述状态误差变量E中的农具横向误差变化率/>农具航向误差变化率/>拖拉机横向误差变化率农具横向误差/>农具航向误差/>拖拉机横向误差/>的反馈系数。
将更新后的最优反馈系数k取代系统上一时刻的最优反馈系数存储于行车控制器的存储单元。
步骤6：基于最优反馈系数k，计算拖拉机牵引机组的前馈角补偿量
为了增加拖拉机牵引机组作业路径跟踪误差模型Ne应对农具和拖拉机的规划路径曲率Kl,Kt突变的能力，并加快对误差控制量的收敛，对拖拉机前轮转角引入前馈控制。
其中由农具预瞄点处曲率引起的稳态误差计算农具的前馈角补偿量
由拖拉机位姿处曲率引起的稳态误差计算出拖拉机的前馈角补偿量δf t：
式中，为农具期望航向角，/>拖拉机期望航向角。
步骤7：根据前馈角补偿量计算拖拉机的前轮转角最优控制量u。
采集当前导航单元提供的拖拉机牵引机组的路径跟踪误差信息E，调用存储单元的反馈系数k和上述的前馈角补偿量计算得到拖拉机的前轮转角最优控制量：
输出前轮转角最优控制量u，控制拖拉机前轮转角，实现拖拉机牵引机组的作业路径跟踪控制，提高农具在多曲率路径跟踪时的作业精度。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意改进所组成的技术方案。

Claims (10)

1.一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，其特征是包括：
步骤1)：建立笛卡尔坐标系下的拖拉机牵引机组的三自由度动力学模型，将所述的三自由度动力学模型转化为Frenet坐标系下的拖拉机牵引机组误差模型，该拖拉机牵引机组误差模型包含有路径跟踪误差信息和沿农具质心行驶方向点处的纵向速度分量信息；
步骤2)：实时采集农具的经纬度坐标信息、行驶航向角、沿农具质心行驶方向点处的侧向速度分量以及纵向速度分量，预测出预瞄时间，计算出农具预瞄点的位姿信息，规划出作业路径，获得实时的路径跟踪误差信息以及距离所述的作业路径最近的索引点处的农具和拖拉机的曲率；将实时的路径跟踪误差信息以及实时的纵向速度分量输入到步骤1)中所述的拖拉机牵引机组误差模型中，获得当前时刻的拖拉机牵引机组误差模型；
步骤3)：计算所述的当前时刻的拖拉机牵引机组误差模型与上一时刻的拖拉机牵引机组误差模型的皮尔森相似度，判断是否更新当前时刻的拖拉机牵引机组误差模型，若是，则重新计算对应于所述的路径跟踪误差信息的最优反馈系数，若否，则存储当前的路径跟踪误差信息的最优反馈系数；
步骤4)：基于所述的最优反馈系数以及所述的农具和拖拉机的曲率，计算出农具和拖拉机的前馈角补偿量；
步骤5)：根据所述的前馈角补偿量、所述的最优反馈系数以及所述的路径跟踪误差信息计算出拖拉机的前轮转角最优控制量，实现拖拉机牵引机组的作业路径跟踪控制。
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的拖拉机牵引机组作业路径跟踪控制方法，其特征是：根据式
分别计算出农具的前馈角补偿量/>和拖拉机的前馈角补偿量/>Kl，Kt分别为步骤2)中所述的农具和拖拉机的曲率，a，b，c分别为拖拉机前轴、后轴和铰接点到拖拉机质心的距离，d，e分别为机组的铰接点、后轴到农具质心的距离；mt,ml分别为拖拉机和农具的质量，/>分别为拖拉机前轮，拖拉机后轮和农具后轮的侧偏刚度，θr为期望航向角。
6.根据权利要求5所述的一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，其特征是：步骤5)中所述的拖拉机的前轮转角最优控制量
7.根据权利要求1所述的一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，其特征是：步骤1)中，将拖拉机牵引机组简化为两个刚体的二维平面运动，分别建立拖拉机和农具的二自由度动力学方程；将拖拉机与农具的铰接点处视为两个刚体的接触运动，构建两者的耦合关系方程，将耦合关系方程带入所述的二自由度动力学方程中，定义状态变量，根据状态变量得到所述的三自由度动力学模型。
8.根据权利要求7所述的一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，其特征是：所述的状态变量三自由度动力学模型的状态空间方程/> 是X的一阶导数，M是状态常数矩阵；N是系统矩阵；P是控制矩阵，控制量U＝δ，δ为拖拉机前轮转角，/>为垂直于农具质心点处的侧向速度分量，ωt,ωl分别为拖拉机、农具的横摆角速度，/>分别为拖拉机和农具的航向角。
9.根据权利要求7所述的一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，其特征是：所述的状态常数矩阵
系统矩阵
控制矩阵Ic t、Ic l分别为拖拉机和农具的转动惯量，/>为沿农具质心行驶方向点处的纵向速度分量，a，b，c分别为拖拉机前轴、后轴和铰接点到拖拉机质心的距离，d，e分别为机组的铰接点、后轴到农具质心的距离；mt,ml分别为拖拉机和农具的质量，/>分别为拖拉机前轮、拖拉机后轮和农具后轮的侧偏刚度。
10.根据权利要求2所述的一种基于改进LQR的拖拉机牵引机组作业路径跟踪控制方法，其特征是：
Ic t、Ic l分别为拖拉机和农具的转动惯量，/>为沿农具质心行驶方向点处的纵向速度分量，a，b，c分别为拖拉机前轴、后轴和铰接点到拖拉机质心的距离，d，e分别为机组的铰接点、后轴到农具质心的距离；mt,ml分别为拖拉机和农具的质量，分别为拖拉机前轮、拖拉机后轮和农具后轮的侧偏刚度。
CN202310833636.3A 2023-07-07 2023-07-07 一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法 Pending CN116679725A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310833636.3A CN116679725A (zh) 2023-07-07 2023-07-07 一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310833636.3A CN116679725A (zh) 2023-07-07 2023-07-07 一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法

Publications (1)

Publication Number Publication Date
CN116679725A true CN116679725A (zh) 2023-09-01

Family

ID=87785652

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310833636.3A Pending CN116679725A (zh) 2023-07-07 2023-07-07 一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法

Country Status (1)

Country Link
CN (1) CN116679725A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117130375A (zh) * 2023-10-26 2023-11-28 北京首钢气体有限公司 针对多种型号半挂车的场内智能运输控制方法及设备

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117130375A (zh) * 2023-10-26 2023-11-28 北京首钢气体有限公司 针对多种型号半挂车的场内智能运输控制方法及设备
CN117130375B (zh) * 2023-10-26 2024-01-26 北京首钢气体有限公司 针对多种型号半挂车的场内智能运输控制方法及设备

Similar Documents

Publication Publication Date Title
CN111638712B (zh) 自动驾驶车辆横向运动控制方法、装置和自动驾驶车辆
US7400108B2 (en) System and method for controlling modular robots
CN111240187B (zh) 一种基于车辆误差模型的车辆轨迹跟踪控制算法
Chebly et al. Coupled longitudinal/lateral controllers for autonomous vehicles navigation, with experimental validation
US20200089229A1 (en) Systems and methods for using nonlinear model predictive control (mpc) for autonomous systems
CN116679725A (zh) 一种基于改进lqr的拖拉机牵引机组作业路径跟踪控制方法
CN110244735A (zh) 移动机器人跟踪预定轨迹的启发式动态规划控制方法
CN108508906B (zh) 户外环境下的多足机器人双边触觉遥操作系统及控制方法
CN112262067A (zh) 用于实施先进驾驶辅助设备的闭环的方法和设备
CN113467470B (zh) 一种无人自主小车的轨迹跟踪控制方法
CN112141101A (zh) 一种基于cnn和lstm的预瞄安全路径的方法及系统
CN112965373A (zh) 一种农用和矿用铰接式车辆路径跟踪控制方法
Jingtao et al. Cascaded navigation control for agricultural vehicles tracking straight paths
Tan et al. Path tracking control strategy for off-road 4WS4WD vehicle based on robust model predictive control
CN113359764A (zh) 一种基于gru的驾驶员模型优化方法
Hao et al. Formation planning and control of UGVs with trailers
CN115373287B (zh) 一种折腰转向拖拉机的自适应参数模型预测路径跟踪控制方法
Hao et al. Differential flatness-based formation following of a simulated autonomous small grain harvesting system
Simotwo et al. Control strategies to steer and drive an autonomous 4WS4WD ground vehicle: A review of MPC approaches
Wang et al. Parallel structure of six wheel-legged robot model predictive tracking control based on dynamic model
Su et al. Double-Iterative Gaussian Process Regression for Modeling Error Compensation in Autonomous Racing
Zhao et al. Research on path following control method of agricultural machinery autonomous navigation through LQR-feed forward control
CN112045687B (zh) 用于操作机械臂的动力学计算方法及机械臂的中央控制方法和系统
CN117519190B (zh) 一种新的铰接车辆控制方法
Simonelli et al. Optimal Path Tracking: Benchmarking an NMPC for a Wide-Span Autonomous Agricultural Machine

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination