CN117406748A - 一种工程机械行驶路径跟踪控制方法及装置 - Google Patents
一种工程机械行驶路径跟踪控制方法及装置 Download PDFInfo
- Publication number
- CN117406748A CN117406748A CN202311595203.5A CN202311595203A CN117406748A CN 117406748 A CN117406748 A CN 117406748A CN 202311595203 A CN202311595203 A CN 202311595203A CN 117406748 A CN117406748 A CN 117406748A
- Authority
- CN
- China
- Prior art keywords
- loader
- vector
- operator
- koopman
- control
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 76
- 239000013598 vector Substances 0.000 claims abstract description 120
- 238000004088 simulation Methods 0.000 claims abstract description 25
- 238000004364 calculation method Methods 0.000 claims abstract description 16
- 238000010276 construction Methods 0.000 claims description 27
- 238000004422 calculation algorithm Methods 0.000 claims description 17
- 238000012937 correction Methods 0.000 claims description 8
- 230000009471 action Effects 0.000 claims description 6
- 230000001902 propagating effect Effects 0.000 claims description 4
- 230000006870 function Effects 0.000 description 87
- 239000011159 matrix material Substances 0.000 description 10
- 230000008569 process Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 9
- 230000007246 mechanism Effects 0.000 description 9
- 230000000694 effects Effects 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 230000004044 response Effects 0.000 description 5
- 238000004590 computer program Methods 0.000 description 4
- 238000000354 decomposition reaction Methods 0.000 description 4
- 230000006399 behavior Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000009977 dual effect Effects 0.000 description 2
- 238000005312 nonlinear dynamic Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000012731 temporal analysis Methods 0.000 description 2
- 238000000700 time series analysis Methods 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 238000004378 air conditioning Methods 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012417 linear regression Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Landscapes
- Operation Control Of Excavators (AREA)
Abstract
本申请公开了一种工程机械行驶路径跟踪控制方法及装置，可以提高装载机的行驶轨迹的预测准确率。其中，工程机械行驶路径跟踪控制方法包括：基于装载机的模拟数据构建装载机的非线性运动学模型；构建Koopman算子；根据所述Koopman算子对所述非线性运动学模型进行线性化，以构建整车动力学预测模型；获取所述装载机的当前状态向量与目标状态向量之间的偏差，以及当前控制向量与目标控制向量之间的控制增量；将所述偏差以及所述控制增量输入至所述整车动力学预测模型进行计算，以生成所述装载机的最优预测轨迹。
Description
技术领域
本申请涉及路径规划技术领域，具体涉及一种工程机械行驶路径跟踪控制方法及装置。
背景技术
装载机由前、后车体两部分构成，两个车体之间通过铰接点和摆动环相连，利用液压执行机构使前、后车体相对偏转实现整车的转向。这种结构形式，减小轮式装载机的转向半径，提高整车的机动性能，使其对作业环境具有良好的适应能力，然而，这种转向机构引入了高度非线性动力学，目前的动力学模型均是乘用车的运动学模型，而乘用车与装载机运动方式不同，直接采用乘用车的运动学模型预测装载机的行驶轨迹，将导致预测准确率较低，影响装载机的工作效率。
发明内容
为了解决上述技术问题，提出了本申请。本申请的实施例提供了一种工程机械行驶路径跟踪控制方法及装置，可以提高装载机的行驶轨迹的预测准确率。
根据本申请的一个方面，提供了一种工程机械行驶路径跟踪控制方法，包括：基于装载机的模拟数据构建装载机的非线性运动学模型；构建Koopman算子；根据所述Koopman算子对所述非线性运动学模型进行线性化，以构建整车动力学预测模型；获取所述装载机的当前状态向量与目标状态向量之间的偏差，以及当前控制向量与目标控制向量之间的控制增量；将所述偏差以及所述控制增量输入至所述整车动力学预测模型进行计算，以生成所述装载机的最优预测轨迹。
在一实施例中，所述构建Koopman算子，包括：构建Koopman算子作用于xk+1＝f(xk，uk)所表示的空间如下：其中，xk+1＝f(xk，uk)为非线性运动学模型改写的离散时间非线性***；其中K为Koopman算子，ψ为状态向量和控制向量的标量函数，h(xk,uk)是将输入在时间上向前传播的函数；以及用递归最小二乘法计算Koopman算子；其中，所述Koopman算子用于定义标量函数ψ在时间上的观测函数。
在一实施例中，用递归最小二乘法计算Koopman算子，包括：获取多个时刻的轨迹数据，所述轨迹数据包括一个时刻时的状态向量和控制向量；根据多个时刻的轨迹数据以及所述观测函数的向量ψ，可确定所述观测函数的过去值和未来值，其中，所述观测函数的未来值与所述观测函数的过去值之间的关系式为：Yf＝KYp+ξ；其中，Yf为观测函数的未来值，K为Koopman算子，Yp为观测函数的过去值，其中ξ为近似误差；基于所述观测函数的未来值与所述观测函数的过去值之间的关系式，通过最小二乘法确定近似误差值最小时的K为所述Koopman算子。
在一实施例中，所述Koopman算子为：其中，Yf为观测函数的未来值，Yp为观测函数的过去值，/>为Moore-Pensose伪逆。
在一实施例中，在构建Koopman算子之后，所述工程机械行驶路径跟踪控制方法还包括：基于带遗忘因子的递归最小二乘算法，对所述Koopman算子进行更新。
在一实施例中，基于带遗忘因子的递归最小二乘算法，对所述Koopman算子进行更新，包括：基于所述Koopman算子计算当前时刻的算子的近似值，所述当前时刻的算子的近似值为：其中，Yf为观测函数的未来值，Yp为观测函数的过去值；Γk＝(YpYpT)-1；根据修正向量、遗忘因子λ以及Γk计算Γk+1；根据当前时刻的轨迹数据以及下一时刻的轨迹数据，编写观测函数的新未来值以及所述观测函数的新过去值；以及基于Γk+1、观测函数的新未来值、所述观测函数的新过去值更新所述Koopman算子。
在一实施例中，所述遗忘因子λ为：0.9<λ<0.995。
在一实施例中，根据所述Koopman算子对所述非线性运动学模型进行线性化，以构建整车动力学预测模型，包括：基于所述非线性运动学模型以及所述Koopman算子，确定观测值向量；基于所述观测值向量以及所述Koopman算子计算所述Koopman算子对于所述观测值向量的作用关系式；以及根据所述作用关系式构建观测值向量之间的线性关系式；以及以所述线性关系式构建所述整车动力学预测模型。
在一实施例中，基于装载机的模拟数据构建装载机的非线性运动学模型，包括：建立所述装载机的行驶运动学方程；确定所述装载机在模拟任务时的模拟状态向量以及模拟控制向量；根据所述装载机的移动方向选择状态表示，并根据所述模拟状态向量、所述模拟控制向量以及所述行驶运动学方程，确定非线性运动学模型。
根据本申请的另一个方面，提供了一种工程机械行驶路径跟踪控制装置，包括：第一构建模块，用于基于装载机的模拟数据构建装载机的非线性运动学模型；第二构建模块，用于构建Koopman算子；线性化模块，用于根据所述Koopman算子对所述非线性运动学模型进行线性化，以构建整车动力学预测模型；获取模块，用于获取所述装载机的当前状态向量与目标状态向量之间的偏差，以及当前控制向量与目标控制向量之间的控制增量；计算模块，用于将所述偏差以及所述控制增量输入至所述整车动力学预测模型进行计算，以生成所述装载机的最优预测轨迹。
本申请提供的工程机械行驶路径跟踪控制方法及装置，基于Koopman理论将非线性运动学模型进行线性化，以构建整车动力学预测模型。非线性运动学模型可以适应装载机的结构，而将非线性运动学模型线性化可以解决非线性***的控制问题，线性化模型可通过模型预测控制获得***的控制输入，也就是实现***控制。Koopman理论和模型预测控制结合，为非线性***设计一个线性模型预测控制，从而对装载机的轨迹进行预测，并通过控制增量、偏差提高模型预测控制的准确度，以实现更精确的控制和响应，从而生成装载机的最优预测轨迹。
附图说明
通过结合附图对本申请实施例进行更详细的描述，本申请的上述以及其他目的、特征和优势将变得更加明显。附图用来提供对本申请实施例的进一步理解，并且构成说明书的一部分，与本申请实施例一起用于解释本申请，并不构成对本申请的限制。在附图中，相同的参考标号通常代表相同部件或步骤。
图1是本申请一示例性实施例提供的模型预测轨迹跟踪控制***的***原理图。
图2是本申请一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图。
图3是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图。
图4是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图。
图5是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图。
图6是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图。
图7是本申请一示例性实施例提供的装载机的双轮运动学的示意图。
图8是本申请一示例性实施例提供的工程机械行驶路径跟踪控制装置的结构示意图。
图9是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制装置的结构示意图。
图10是本申请一示例性实施例提供的电子设备的结构图。
具体实施方式
下面，将参考附图详细地描述根据本申请的示例实施例。显然，所描述的实施例仅仅是本申请的一部分实施例，而不是本申请的全部实施例，应理解，本申请不受这里描述的示例实施例的限制。
申请概述
装载机由前、后车体两部分构成，两个车体之间通过铰接点和摆动环相连，利用液压执行机构使前、后车体相对偏转实现整车的转向，转向过程中车轮与车架之间无相对偏转，从而省略了转向驱动桥等部件。这种结构形式与传统乘用车结构不同，能够减小轮式装载机的转向半径，然而，目前装载机的转向机构引入了高度非线性动力学，目前的动力学模型均是乘用车的运动学模型，乘用车的运动学模型并不能很好的解决装载机的运动问题，在采用乘用车的运动模型预测装载机的行驶轨迹时，误差较大，难以得到较优的行驶轨迹，给行驶轨迹跟踪控制问题带来了额外的复杂性，也和乘用车自动驾驶有较大不同。
本申请提供的工程机械行驶路径跟踪控制方法及装置，引入了Koopman辨识理论对强非线性模型进行线性化，其本质是有限维非线性***可以提升为无限维线性***，这样就可以保留原***的信息。基于在Koopman理论下构造的线性化模型，可通过模型预测控制(Model Predict Control，MPC)获得***的控制输入，也就能实现***控制。并且，通过在线更新模型来提升模型的预测精度，即使用带有遗忘因子的递归最小二乘(RecursiveLeast Square-Forget Factor，RLS-FF)算法。RLS、Koopman理论和MPC相结合，这里称之为K-R-MPC控制器。通过这个方法构造数据驱动控制器，可解决非线性***的控制问题，而且在保证控制精度的前提下，计算时间比非线性模型预测控制器(Nonlinear Model PredictControl,NMPC)有较大节省。另外，构建了装载机的非线性运动学模型，然后在非线性运动学模型的基础上对装载机的行驶轨迹进行预测，提高了装载机的行驶轨迹的预测准确率，以获得装载机的最优行驶轨迹。
除装载机外，本申请提供的工程机械行驶路径跟踪控制方法及装置还可以应用于其他与装载机具有类似结构的工程机械上。
示例性***
针对装载机动力学模型的强非线性问题，本示例性实施例通过Koopman理论构建线性化装载机动力学状态空间方程，考虑行驶过程中的横向位置偏差和航向角偏差，以加速度和铰接角为控制输入，以车速、横向位置偏差和航向角偏差为控制输出，建立整车动力学预测模型，以加速度、铰接角和车速为约束条件，将目标函数转换为二次规划问题。基于在Koopman理论下构造的线性化模型，可通过模型预测控制(Model Predict Control，MPC)获得***的控制输入，也就可以实现***控制。针对模型准确度不高的问题，本申请采用RLS-FF算法来提高模型的预测精度，与此同时，还使用了一种更新机制来决定是否更新模型。综上所述，基于Koopman算子和RLS建模的MPC控制***，建立满足装载机作业需求的模型预测轨迹跟踪控制***，图1是本申请一示例性实施例提供的模型预测轨迹跟踪控制***的***原理图，如图1所示，其中，将rk、ek输入至模型预测控制41(Model PredictControl，MPC)中，ek表示状态与其参考的偏差，用于对模型预测控制41进行修正，从MPC中输出uk，再传输至被控对象42(Plant)以及模型更新43(Model Update)中，被控对象42输出yk和xk，yk以及ek可反馈至模型预测控制41，对控制信号进行微小的调整或增量，可以用于逐步调整***的状态或输出，以实现更精确的控制和响应。xk和uk被输入至模型更新43中，生成更新后的K，K表示Koopman算子，模型更新43采用RLS-FF算法来提高模型的预测精度，rk表示目标状态，ek表示状态与其参考的偏差，xk表示状态向量，uk表示时刻k的输入向量，yk表示修正向量。
示例性方法
图2是本申请一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图，如图2所示，工程机械行驶路径跟踪控制方法，包括：
步骤100：基于装载机的模拟数据构建装载机的非线性运动学模型。
装载机主要在低速工况下运行，滑移效应较小，因此，忽略装载机的滑移效应，也就是说，装载机前(后)车身速度的方向与前(后)车身的航向是一致的，装载机的非线性运动学模型可以用以下方程来描述:
式中，(xf,yf)表示前轮轴中心点的位置，(xr,yr)表示后轮轴中心点的位置，θ为后车体航向角，γ为铰接角，也就是前后车身航向角之差，Lf表示铰接点到前桥的距离，Lr表示铰接点到后桥的距离，vf表示前车体的速度，vr表示后车体的速度。
步骤200：构建Koopman算子。
在已知装载机的非线性运动学模型的情况下，一旦知道了后车体航向角和铰接角，可根据非线性运动学模型表示装载机的状态向量和控制向量，获取状态向量和控制向量后，非线性运动学模型可以重写为离散时间非线性***：xk+1＝f(xk，uk)；根据离散时间非线性***构建Koopman算子。Koopman算子公式的核心思想是将动力***终得状态变量表示为函数的形式，具体的，koopman算子将一个函数映射到另一个函数，描述了状态变量在相空间中的变化情况。Koopman算子能够将状态变量随时间演化的规律转化为函数的演化过程。因此，Koopman算子可以描述非线性***的可观测状态量在高维空间中的线性演化过程，可以将非线性问题转化为线性问题。
步骤300：根据Koopman算子对非线性运动学模型进行线性化，以构建整车动力学预测模型。
根据Koopman理论对非线性运动学模型进行线性化，考虑行驶过程中装载机的横向位置偏差和航向角偏差，以加速度和铰接角为控制输入，以车速、横向位置偏差和航向角偏差为控制输出，建立整车动力学预测模型。
步骤400：获取装载机的当前状态向量与目标状态向量之间的偏差，以及当前控制向量与目标控制向量之间的控制增量。
偏差(Bias)是指预测模型在对输入数据进行预测时的***性误差或偏离真实值的程度。控制增量(Control Increment)是指在控制***中，根据当前的控制信号和控制器输出的基础上，对控制信号进行微小的调整或增量。这种增量调整可以用于逐步调整***的状态或输出，以实现更精确的控制和响应。通过反馈偏差以及控制增量，可以进一步调整对预测模型产生影响，包括提高***响应速度、控制精度、稳定性和模型适应性。
步骤500：将偏差以及控制增量输入至整车动力学预测模型进行计算，以生成装载机的最优预测轨迹。
最小化形式的无偏性能指标：
式中，ek+i＝rk+i-xk+i为状态与其参考的偏差(当前状态向量与目标状态向量之间的偏差)，输入增量(当前控制向量与目标控制向量之间的控制增量)为Δuk+i，N为预测步长。矩阵T为正半定，R为正定。
对于线性预测控制，采用线性时不变(Linear Time Invatiation，LTI)模型。该模型需要输入的非增量值，因此用输入的过去值来增强状态，从而得到：
使用该模型和成本函数，最优控制问题(Optimal Control Problem，OCP)表示为：
以获得装载机的最优行驶轨迹。
本申请提供的工程机械行驶路径跟踪控制方法，基于Koopman理论将非线性运动学模型进行线性化，以构建整车动力学预测模型。非线性运动学模型可以适应装载机的结构，而将非线性运动学模型线性化可以解决非线性***的控制问题，线性化模型可通过模型预测控制获得***的控制输入，也就是实现***控制。Koopman理论和模型预测控制结合，为非线性***设计一个线性模型预测控制，从而对装载机的轨迹进行预测，并通过控制增量、偏差提高模型预测控制的准确度，以实现更精确的控制和响应，从而生成装载机的最优预测轨迹。
在一实施例中，上述步骤200可以包括：构建Koopman算子作用于xk+1＝f(xk，uk)所表示的空间如下：
其中，xk+1＝f(xk，uk)为非线性运动学模型改写的离散时间非线性***；其中K为Koopman算子，ψ为状态向量和控制向量的标量函数，h(xk,uk)是将输入在时间上向前传播的函数。用递归最小二乘法计算Koopman算子。其中，Koopman算子用于定义标量函数ψ在时间上的观测函数。
将Koopman算子作用于xk+1＝f(xk，uk)，所表示的空间为其中K为Koopman算子，ψ为状态向量和控制向量的标量函数，h(xk,uk)是将输入在时间上向前传播的函数，因此，Koopman算子定义标量函数ψ在时间上的演化，也表示为可观测函数。Koopman算子公式的核心思想是将动力***终得状态变量表示为函数的形式，具体的，koopman算子将一个函数映射到另一个函数，描述了状态变量在相空间中的变化情况。因此，Koopman算子可以描述非线性***的可观测状态量在高维空间中的线性演化过程,可以将非线性问题转化为线性问题。
用递归最小二乘法计算K，称为扩展动态模态分解：
在一实施例中，用递归最小二乘法计算Koopman算子，包括：获取多个时刻的轨迹数据，轨迹数据包括一个时刻时的状态向量和控制向量。根据多个时刻的轨迹数据以及观测函数的向量ψ，可确定观测函数的过去值和未来值。其中，观测函数的未来值与观测函数的过去值之间的关系式为：Yf＝KYp+ξ；其中，Yf为观测函数的未来值，K为Koopman算子，Yp为观测函数的过去值，其中ξ为近似误差。基于观测函数的未来值与观测函数的过去值之间的关系式，通过最小二乘法确定近似误差值最小时的K为Koopman算子。
对于强非线性模型，可利用Koopman辨识理论对其线性化，其本质是有限维非线性***可以提升为无限维线性***，这样就可以保留原***的信息。但计算无限维的Koopman算子是不可能的，因此，通过扩展动态模式分解(Extensive Dynamic ModeDecomposition，DMD)对算子进行有限维近似。基于EDMD方法，非线性***的全局线性化可以通过数据驱动来实现，这些数据包括控制输入和关于非线性***的其它信息。
因此，用递归最小二乘法计算K，称为扩展动态模态分解：
可以用以下方式收集状态以及输入轨迹：
其中(xi,ui)是由时刻i的状态和输入组成的元组，xi是表示时刻i的状态，ui是表示时刻i的输入，标量值P是所取样本的个数。
通过使用可观察函数的向量Ψ，可以得到以下矩阵：
其中Yp和Yf是可观测函数的过去值和未来值，由以下方程联系起来的：
Yf＝KYp+ξ，
其中ξ为近似误差。通过最小二乘法，寻找矩阵K，让ξ最小，即：
此时让ξ最小的K确定为构建的Koopman算子。
在一实施例中，Koopman算子为：其中，Yf为观测函数的未来值，Yp为观测函数的过去值，/>为Moore-Pensose伪逆。
观测函数是指在时间序列分析或信号处理中，根据给定的时间点或时刻，对某个***或过程进行观测或测量得到的数值。观测函数的未来值是指根据已有观测值和模型，对未来时间点的***状态或数值进行预测或估计得到的值。观测函数的过去值是指在时间序列分析或信号处理中，根据给定的时间点或时刻，对某个***或过程进行观测或测量得到的数值，观测函数的过去值用于构建时间序列模型，通过分析过去的观测数据，可以揭示***或过程的特征、趋势、周期性和季节性等，观测函数的过去值和已有的模型可以用于预测未来的观测值。即，观测函数的未来值可以包括状态向量和控制向量的未来值，观测函数的过去值可以包括状态向量和控制向量的过去值。基于过去时刻的状态向量和控制向量，可以对未来时间点的状态向量和控制向量进行分析和预测。
通过最小二乘法，寻找矩阵K，让ξ最小，即：
该优化问题有解析解，即/>其中，/>为Moore-Pensose伪逆。Moore-Penrose伪逆(Moore-Penrose Pseudoinverse)是一种矩阵的广义逆，用于求解线性方程组的最小二乘解。
图3是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图，如图3所示，在上述步骤200之后，工程机械行驶路径跟踪控制方法还可以包括：
步骤600：基于带遗忘因子的递归最小二乘算法，对Koopman算子进行更新。
模型预测控制的性能取决于整车动力学预测模型的预测精度，因此，可以通过在线更新模型来提高整车动力学预测模型的预测精度，即使用带有遗忘因子的递归最小二乘(Recursive Least Square-Forget Factor，RLS-FF)算法。带有遗忘因子的递归最小二乘算法是一种用于在线估计线性回归模型参数的方法。该算法通过递归地更新参数估计，同时考虑最新观测数据和过去观测数据的权重，以适应数据流的变化。结合带有遗忘因子的递归最小二乘算法、Koopman理论以及模型预测控制，构造K-R-MPC控制器，可解决非线性***的控制问题，而且在保证控制精度的前提下，计算时间比非线性模型预测控制器更少。
在构建完Koopman算子(步骤200)后，可以判断Koopman算子是否需要更新，如果不需要更新，可以直接执行步骤300，如果需要对Koopman算子进行更新，则先执行步骤600，再执行步骤300。
图4是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图，如图4所示，上述步骤600可以包括：
步骤610：基于Koopman算子计算当前时刻的算子的近似值，当前时刻的算子的近似值为：
其中，Yf为观测函数的未来值，Yp为观测函数的过去值；Γk＝(YpYpT)-1；
步骤620：根据修正向量、遗忘因子λ以及Γk计算Γk+1。
步骤630：根据当前时刻的轨迹数据以及下一时刻的轨迹数据，编写观测函数的新未来值以及观测函数的新过去值。
步骤640：基于Γk+1、观测函数的新未来值、观测函数的新过去值更新Koopman算子。
考虑Koopman算子从1到k的数据为：
其中，Γk＝(YpYpT)-1，并且Kk是Koopman算子在时刻k的近似值。现在，在下一个时刻，有一对新的数据点(xk,uk)和(xk+1,uk+1)可用来更新Koopman算子，考虑到下一时刻的新数据点，重新编写矩阵Yp和Yf：
其中，Ψk和Ψk+1代表映射Ψ在新数据点处求值。现在，Koopman算子的更新近似k+1由：
其中，Γk+1可重写为：
通过展开式可以得到：
通过注意到KkΓk -1＝YfYp T，由此可得：
将和/>代入中，可以得到：
其中，γk是修正向量，该方程涉及三个主要组成部分，即Koopman算子的初值、预测误差和修正向量。
式要求每次进行更新时，k+1都计算矩阵Γk+1。但该计算需要矩阵反转，通过下述方程式可以避免矩阵反转问题：
Kk+1＝Kk+(Ψk+1-KΨk)γk，
其中，λ是遗忘因子，取值可以在0.9<λ<0.995之间，它代表了干扰抑制和变化参数的更快跟踪之间的折衷。如果λ＝1，则没有遗忘因子。带有遗忘因子的递归最小二乘算法的核心思想是在更新参数估计时考虑过去观测数据的权重。通过引入遗忘因子，算法可以对最新的观测数据赋予更高的权重，而对过去的观测数据赋予较低的权重，从而适应数据流的变化。
在构建完Koopman算子(步骤200)后，可以判断Koopman算子是否需要更新，如果不需要更新，可以直接执行步骤300，如果需要对Koopman算子进行更新，则先执行步骤610-步骤640，再执行步骤300。
图5是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图，如图5所示，上述步骤300可以包括：
步骤310：基于非线性运动学模型以及Koopman算子，确定观测值向量。
步骤320：基于观测值向量以及Koopman算子计算Koopman算子对于观测值向量的作用关系式。
步骤330：根据作用关系式构建观测值向量之间的线性关系式。
步骤340：以线性关系式构建整车动力学预测模型。
基于Koopman算子的近似K来设计模型预测控制策略，假设得到离散时间非线性***中的Koopman算子的近似K，其观测值向量为：
其中，ψnh(xk)T是一个h向量值非线性函数，其n元素h依赖于状态。对应K对观测值向量的作用为：
通过定义zk＝[xk T ψnh(xk)T]可得线性模型：
zk+1＝Azk+Buk；
其中，A是***矩阵，B是输入矩阵。KMPC背后的思想是使用这个预测模型为非线性***设计一个线性模型预测控制。
图6是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制方法的流程示意图，如图6所示，上述步骤100可以包括：
步骤110：建立装载机的行驶运动学方程。
步骤120：确定装载机在模拟任务时的模拟状态向量以及模拟控制向量。
步骤130：根据装载机的移动方向选择状态表示，并根据模拟状态向量、模拟控制向量以及行驶运动学方程，确定非线性运动学模型。
考虑到轮式装载机主要在低速工况下运行，滑移效应较小。因此，没有考虑轮胎的滑移角和作用在轮胎上的侧向力。图7是本申请一示例性实施例提供的装载机的双轮运动学的示意图，如图7所示，其中由两个车轮组成的每个轴由一个独特的车轮代替，(xf,yf)表示前轮轴中心点的位置，(xr,yr)表示后轮轴中心点的位置，θ为后车体航向角，γ为铰接角，也就是前后车身航向角之差，Lf表示铰接点到前桥的距离，Lr表示铰接点到后桥的距离，vf表示前车体的速度，vr表示后车体的速度。假设车辆的速度和加速度很小，将滑移效应忽略不计，则前后车身速度的方向与前后车身的航向是一致的，装载机的行驶运动学(非线性运动学模型)可以用以下方程来描述：
式中，(xf,yf)表示前轮轴中心点的位置，(xr,yr)表示后轮轴中心点的位置，θ为后车体航向角，γ为铰接角，也就是前后车身航向角之差，Lf表示铰接点到前桥的距离，Lr表示铰接点到后桥的距离，vf表示前车体的速度，vr表示后车体的速度。
根据方程计算得到后车体航向角以及铰接角后，装载机的状态就可以通过前后车体的位置来充分描述，因此，可以表示状态向量以及控制向量为：
式中，x为状态向量，即装载机当前所处的状态，例如后轮轴中心点的位置、后车体航向角以及铰接角；或前轮轴中心点的位置、后车体航向角以及铰接角。u为控制向量，即装载机当前所需要控制的控制量，也为装载机下一刻的控制输入，例如，装载机的车体速度以及铰接角的角速度(通过控制铰接角的角速度即可控制装载机的铰接角)，当前需要控制的控制量为：车体速度以及铰接角的角速度，也就是装载机下一刻的控制输入。
确定状态向量和控制向量后，可以将非线性运动学模型改写为离散时间非线性***：xk+1＝f(xk,uk)；式中，xk+1为装载机在k+1时刻时的状态向量；xk为装载机在k时刻时的状态向量；uk为装载机在k时刻时的控制向量。
离散时间非线性***(discrete-time nonlinearsystem)为在离散的时间点上描述的非线性控制***，在MPC(Model Predictive Control，模型预测控制)算法中，需要一个描述对象动态行为的模型，这个模型的作用是预测***未来的动态。即能够根据***k时刻的状态和k时刻的控制输入，预测到k+1时刻的输出。在这里k时刻的输入正是用来控制***k+1时间的输出，使其最大限度的接近k+1时刻的期望值。在本申请中，由非线性运动学模型来描述对象动态行为，改写为离散时间非线性***后，可预测***未来的动态。
本示例性方法针对装载机动力学模型的强非线性问题，通过Koopman理论构建线性化装载机动力学状态空间方程，考虑行驶过程中的横向位置偏差和航向角偏差，以加速度和铰接角为控制输入，以车速、横向位置偏差和航向角偏差为控制输出，建立整车动力学预测模型，以加速度、铰接角和车速为约束条件，将目标函数转换为二次规划问题。针对模型准确度不高的问题，采用RLS-FF算法来提高模型的预测精度，与此同时，还使用了一种更新机制来决定是否更新模型。综上所述，基于Koopman算子和RLS建模的MPC控制***，建立满足装载机作业需求的模型预测轨迹跟踪控制***，通过该方法构造K-R-MPC数据驱动控制器，可解决非线性***的控制问题，而且在保证控制精度的前提下，计算时间比非线性模型预测控制器(Nonlinear Model Predict Control,NMPC)更加节省，另外，构建了装载机的非线性运动学模型，然后在非线性运动学模型的基础上对装载机的行驶轨迹进行预测，契合装载机基于特殊结构的运动模式，提高了装载机的行驶轨迹的预测准确率，以获得装载机的最优行驶轨迹，解决了行驶轨迹跟踪控制问题。
示例性装置
图8是本申请一示例性实施例提供的工程机械行驶路径跟踪控制装置的结构示意图，如图8所示，工程机械行驶路径跟踪控制装置2，包括：第一构建模块21，用于基于装载机的模拟数据构建装载机的非线性运动学模型；第二构建模块22，用于构建Koopman算子；线性化模块23，用于根据Koopman算子对非线性运动学模型进行线性化，以构建整车动力学预测模型；获取模块24，用于获取装载机的当前状态向量与目标状态向量之间的偏差，以及当前控制向量与目标控制向量之间的控制增量；计算模块25，用于将偏差以及控制增量输入至整车动力学预测模型进行计算，以生成装载机的最优预测轨迹。
在一实施例中，上述第二构建模块22可以配置为：构建Koopman算子作用于xk+1＝f(xk，uk)所表示的空间如下：其中，xk+1＝f(xk，uk)为非线性运动学模型改写的离散时间非线性***；其中K为Koopman算子，ψ为状态向量和控制向量的标量函数，h(xk,uk)是将输入在时间上向前传播的函数；以及用递归最小二乘法计算Koopman算子；其中，Koopman算子用于定义标量函数ψ在时间上的观测函数。
在一实施例中，上述第二构建模块22可以配置为：获取多个时刻的轨迹数据，轨迹数据包括一个时刻时的状态向量和控制向量；根据多个时刻的轨迹数据以及观测函数的向量ψ，可确定观测函数的过去值和未来值，其中，观测函数的未来值与观测函数的过去值之间的关系式为：Yf＝KYp+ξ；其中，Yf为观测函数的未来值，K为Koopman算子，Yp为观测函数的过去值，其中ξ为近似误差；基于观测函数的未来值与观测函数的过去值之间的关系式，通过最小二乘法确定近似误差值最小时的K为Koopman算子。
在一实施例中，上述第二构建模块22可以配置为：Koopman算子为：其中，Yf为观测函数的未来值，Yp为观测函数的过去值，/>为Moore-Pensose伪逆。
图9是本申请另一示例性实施例提供的工程机械行驶路径跟踪控制装置的结构示意图，如图9所示，工程机械行驶路径跟踪控制装置2还可以配置为：更新模块26，用于基于带遗忘因子的递归最小二乘算法，对Koopman算子进行更新。
在一实施例中，如图9所示，更新模块26还可以配置为：第一更新单元261，基于Koopman算子计算当前时刻的算子的近似值，当前时刻的算子的近似值为：其中，Yf为观测函数的未来值，Yp为观测函数的过去值；Γk＝(YpYpT)-1；第二更新单元262，用于根据修正向量、遗忘因子λ以及Γk计算Γk+1；第三更新单元263，用于根据当前时刻的轨迹数据以及下一时刻的轨迹数据，编写观测函数的新未来值以及观测函数的新过去值；以及第四更新单元264，用于基于Γk+1、观测函数的新未来值、观测函数的新过去值更新Koopman算子。
在一实施例中，工程机械行驶路径跟踪控制装置2还可以配置为：遗忘因子λ为：0.9<λ<0.995。
在一实施例中，如图9所示，上述线性化模块23可以配置为：第一线性化单元231，用于基于非线性运动学模型以及Koopman算子，确定观测值向量；第二线性化单元232，用于基于观测值向量以及Koopman算子计算Koopman算子对于观测值向量的作用关系式；以及第三线性化单元233，用于根据作用关系式构建观测值向量之间的线性关系式；以及第四线性化单元234，用于以线性关系式构建整车动力学预测模型。
在一实施例中，上述第一构建模块21可以配置为：建立单元211，用于建立装载机的行驶运动学方程；确定向量单元212，用于确定装载机在模拟任务时的模拟状态向量以及模拟控制向量；确定模型单元213，用于根据装载机的移动方向选择状态表示，并根据模拟状态向量、模拟控制向量以及行驶运动学方程，确定非线性运动学模型。
示例性电子设备
一种电子设备，电子设备包括：处理器；用于存储处理器可执行指令的存储器；处理器，用于执行本申请提供的实施例所述的工程机械行驶路径跟踪控制方法。
下面，参考图10来描述根据本申请实施例的电子设备。该电子设备可以是第一设备和第二设备中的任一个或两者、或与它们独立的单机设备，该单机设备可以与第一设备和第二设备进行通信，以从它们接收所采集到的输入信号。
图10图示了根据本申请实施例的电子设备的框图。
如图10所示，电子设备10包括一个或多个处理器11和存储器12。
处理器11可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备10中的其他组件以执行期望的功能。
存储器12可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器11可以运行所述程序指令，以实现上文所述的本申请的各个实施例的工程机械行驶路径跟踪控制方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如输入信号、信号分量、噪声分量等各种内容。
在一个示例中，电子设备10还可以包括：输入装置13和输出装置14，这些组件通过总线***和/或其他形式的连接机构(未示出)互连。
在该电子设备是单机设备时，该输入装置13可以是通信网络连接器，用于从第一设备和第二设备接收所采集的输入信号。
此外，该输入装置13还可以包括例如键盘、鼠标等等。
该输出装置14可以向外部输出各种信息，包括确定出的距离信息、方向信息等。该输出装置14可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
当然，为了简化，图10中仅示出了该电子设备10中与本申请有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备10还可以包括任何其他适当的组件。
所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
一种计算机可读存储介质，存储介质存储有计算机程序，计算机程序用于执行本申请提供的实施例所述的工程机械行驶路径跟踪控制方法。
所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的***、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。
Claims (10)
1.一种工程机械行驶路径跟踪控制方法，其特征在于，包括：
基于装载机的模拟数据构建装载机的非线性运动学模型；
构建Koopman算子；
根据所述Koopman算子对所述非线性运动学模型进行线性化，以构建整车动力学预测模型；
获取所述装载机的当前状态向量与目标状态向量之间的偏差，以及当前控制向量与目标控制向量之间的控制增量；
将所述偏差以及所述控制增量输入至所述整车动力学预测模型进行计算，以生成所述装载机的最优预测轨迹。
2.根据权利要求1所述的工程机械行驶路径跟踪控制方法，其特征在于，所述构建Koopman算子，包括：
构建Koopman算子作用于xk+1＝f(xk，uk)所表示的空间如下：其中，xk+1＝f(xk，uk)为非线性运动学模型改写的离散时间非线性***；
其中K为Koopman算子，ψ为状态向量和控制向量的标量函数，h(xk,uk)是将输入在时间上向前传播的函数；以及
用递归最小二乘法计算Koopman算子；
其中，所述Koopman算子用于定义标量函数ψ在时间上的观测函数。
3.根据权利要求2所述的工程机械行驶路径跟踪控制方法，其特征在于，用递归最小二乘法计算Koopman算子，包括：
获取多个时刻的轨迹数据，所述轨迹数据包括一个时刻时的状态向量和控制向量；
根据多个时刻的轨迹数据以及所述观测函数的向量ψ，可确定所述观测函数的过去值和未来值，其中，所述观测函数的未来值与所述观测函数的过去值之间的关系式为：
YF＝KYp+ξ；
其中，Yf为观测函数的未来值，K为Koopman算子，Yp为观测函数的过去值，其中ξ为近似误差；
基于所述观测函数的未来值与所述观测函数的过去值之间的关系式，通过最小二乘法确定近似误差值最小时的K为所述Koopman算子。
4.根据权利要求3所述的工程机械行驶路径跟踪控制方法，其特征在于，所述Koopman算子为：
其中，Yf为观测函数的未来值，Yp为观测函数的过去值，为Moore-Pensose伪逆。
5.根据权利要求4所述的工程机械行驶路径跟踪控制方法，其特征在于，在构建Koopman算子之后，所述工程机械行驶路径跟踪控制方法还包括：
基于带遗忘因子的递归最小二乘算法，对所述Koopman算子进行更新。
6.根据权利要求5所述的工程机械行驶路径跟踪控制方法，其特征在于，基于带遗忘因子的递归最小二乘算法，对所述Koopman算子进行更新，包括：
基于所述Koopman算子计算当前时刻的算子的近似值，所述当前时刻的算子的近似值为：
其中，Yf为观测函数的未来值，Yp为观测函数的过去值；Γk＝(YpYpT)-1；
根据修正向量、遗忘因子λ以及Γk计算Γk+1；
根据当前时刻的轨迹数据以及下一时刻的轨迹数据，编写观测函数的新未来值以及所述观测函数的新过去值；以及
基于Γk+1、观测函数的新未来值、所述观测函数的新过去值更新所述Koopman算子。
7.根据权利要求6所述的工程机械行驶路径跟踪控制方法，其特征在于，所述遗忘因子λ为：0.9<λ<0.995。
8.根据权利要求1所述的工程机械行驶路径跟踪控制方法，其特征在于，根据所述Koopman算子对所述非线性运动学模型进行线性化，以构建整车动力学预测模型，包括：
基于所述非线性运动学模型以及所述Koopman算子，确定观测值向量；
基于所述观测值向量以及所述Koopman算子计算所述Koopman算子对于所述观测值向量的作用关系式；以及
根据所述作用关系式构建观测值向量之间的线性关系式；以及
以所述线性关系式构建所述整车动力学预测模型。
9.根据权利要求1所述的工程机械行驶路径跟踪控制方法，其特征在于，基于装载机的模拟数据构建装载机的非线性运动学模型，包括：
建立所述装载机的行驶运动学方程；
确定所述装载机在模拟任务时的模拟状态向量以及模拟控制向量；
根据所述装载机的移动方向选择状态表示，并根据所述模拟状态向量、所述模拟控制向量以及所述行驶运动学方程，确定非线性运动学模型。
10.一种工程机械行驶路径跟踪控制装置，其特征在于，包括：
第一构建模块，用于基于装载机的模拟数据构建装载机的非线性运动学模型；
第二构建模块，用于构建Koopman算子；
线性化模块，用于根据所述Koopman算子对所述非线性运动学模型进行线性化，以构建整车动力学预测模型；
获取模块，用于获取所述装载机的当前状态向量与目标状态向量之间的偏差，以及当前控制向量与目标控制向量之间的控制增量；
计算模块，用于将所述偏差以及所述控制增量输入至所述整车动力学预测模型进行计算，以生成所述装载机的最优预测轨迹。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311595203.5A CN117406748A (zh) | 2023-11-27 | 2023-11-27 | 一种工程机械行驶路径跟踪控制方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311595203.5A CN117406748A (zh) | 2023-11-27 | 2023-11-27 | 一种工程机械行驶路径跟踪控制方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117406748A true CN117406748A (zh) | 2024-01-16 |
Family
ID=89498177
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311595203.5A Pending CN117406748A (zh) | 2023-11-27 | 2023-11-27 | 一种工程机械行驶路径跟踪控制方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117406748A (zh) |
-
2023
- 2023-11-27 CN CN202311595203.5A patent/CN117406748A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109885883B (zh) | 一种基于gk聚类算法模型预测的无人车横向运动的控制方法 | |
| Tuchner et al. | Vehicle platoon formation using interpolating control: A laboratory experimental analysis | |
| EP3935580B1 (en) | Apparatus and method for controlling operation of machine subject, and storage medium | |
| Liu et al. | Multi-kernel online reinforcement learning for path tracking control of intelligent vehicles | |
| CN110095983B (zh) | 一种基于路径参数化的移动机器人预测跟踪控制方法 | |
| Liu et al. | Path tracking control of a self‐driving wheel excavator via an enhanced data‐driven model‐free adaptive control approach | |
| Schnelle et al. | Adaptive nonlinear model predictive control design of a flexible-link manipulator with uncertain parameters | |
| Sun | Kinematics model identification and motion control of robot based on fast learning neural network | |
| Zengin et al. | Blending based multiple-model adaptive control of multivariable systems with application to lateral vehicle motion control | |
| CN116819973B (zh) | 一种轨迹跟踪控制方法 | |
| JP7357813B2 (ja) | データ駆動型モデル適応を用いる制御のための装置および方法 | |
| CN115933647B (zh) | 基于复合控制算法的omr轨迹跟踪控制方法及存储介质 | |
| CN117406748A (zh) | 一种工程机械行驶路径跟踪控制方法及装置 | |
| CN112764347B (zh) | 一种基于最大相关熵准则的智能车辆路径跟踪方法 | |
| CN115343950A (zh) | 一种适应复杂路面的车辆路径跟踪控制方法及控制*** | |
| CN115179290A (zh) | 一种机械臂及其轨迹控制方法与装置 | |
| Esfahani et al. | Sliding mode controller design based on state estimation for underactuated AUVs in the presence of time-varying disturbances and uncertainties | |
| Kanai et al. | Linear model predictive control with accurate prediction based on low-dimensional lifting linearization | |
| Voros et al. | Lateral vehicle control using finite spectrum assignment | |
| Garcia et al. | A Comprehensive Analysis of Model Predictive Control for Lane Keeping Assist System | |
| de Curtò et al. | Hybrid State Estimation: Integrating Physics-Informed Neural Networks with Adaptive UKF for Dynamic Systems | |
| Zhuang et al. | Robust control design for zero-sum differential games problem based on off-policy reinforcement learning technique | |
| CN117406600A (zh) | 一种装载机行驶路径规划方法及*** | |
| Kumar et al. | An optimal lateral trajectory stabilization of vehicle using differential dynamic programming | |
| Tang et al. | Multiple Model Adaptive Control of Nonlinear Systems Based on Dynamic Optimization |
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 |