用于农机自动驾驶的自适应前馈模型预测控制方法及系统

1.本发明属于农用车辆自动驾驶技术领域，具体涉及一种用于农机自动驾驶的自适应前馈模型预测控制方法及系统。


背景技术：

2.农用车辆无人化自动驾驶是智能农业研究的重点，而路径跟踪控制又是无人驾驶的核心技术。路径跟踪的目标是通过消除农机在行驶过程中实际行驶路径与规划路径之间的偏差，实现路径的准确跟踪。模型预测控制能够基于当前的状态量预测未来时刻的控制量，并且充分考虑状态量和控制量的约束条件，所以在路径跟踪控制领域得到应用。
3.中国专利(cn109884900a)公开了一种基于自适应模型预测控制的收获机路径跟踪控制方法，该方法根据跟踪路径和收获机的行驶速度自适应调整预测时间，实现自适应模型预测控制；该发明通过优选预测时域有效地解决了由于联合收获机质量大、惯性大造成的控制滞后问题，提高了路径跟踪效果；但该方法针对农机作业常出现的大角度转弯或180
°
调头等工况考虑不足，常出现路径跟踪误差较大的情况，同时该方法未考虑到横向误差较大时模型预测控制器的响应速度问题。


技术实现要素：

4.针对现有技术中存在不足，本发明提供了一种用于农机自动驾驶的自适应前馈模型预测控制方法及系统，在农机横向误差较大时，通过自适应前馈控制器补偿控制量，提高系统响应速度，在农机横向误差较小时，通过模型预测控制器保证跟踪精度。
5.本发明是通过以下技术手段实现上述技术目的的。
6.一种用于农机自动驾驶的自适应前馈模型预测控制方法，具体为：
7.导航定位模块实时获取农机行驶状态信息，包括当前位置(x，y)、航向角和车速v；当前位置(x，y)到已规划路径的最短距离记作行驶过程中的横向误差δx；
8.所述横向误差δx和车速v传输给自适应前馈控制器，确定前馈补偿角δq；所述当前位置(x，y)、航向角车速v及参考位置(xr，yr)、参考航向角参考前轮转角δ
fr
传输给模型预测控制器，求解得到前轮转角δm；
9.根据前馈补偿角δq与前轮转角δm得到最终前轮转角δ
final
，其中δ
final
＝min(δq+δm，δ
max
)，δ
max
为农机前轮最大转角。
10.进一步地，所述前馈补偿角δq是根据以下规则确定的：
[0011][0012]
其中：k为斜率，且k＝7.1396v-1.006
。
[0013]
进一步地，所述是基于车辆运动学模型求解
得到的，其中为t时刻第k次输出的控制量，为t时刻第k-1次输出的控制量，δu(k)为控制增量。
[0014]
进一步地，还包括：当转角传感器实时反馈的实际前轮转角，等于最终前轮转角δ
final
时，转向结束，实现农机的路径跟踪控制。
[0015]
进一步地，所述参考航向角其中(x
rn
,y
rn
)为下一时刻参考位置。
[0016]
进一步地，所述参考前轮转角其中为下一时刻参考航向角，l为农机车轮轴距。
[0017]
一种用于农机自动驾驶的自适应前馈模型预测控制系统，包括：
[0018]
导航定位模块，获取农机行驶状态信息；
[0019]
行车控制模块，根据农机行驶状态信息得到最终前轮转角δ
final
；
[0020]
转角传感器，实时反馈实际前轮转角；
[0021]
所述最终前轮转角δ
final
和实际前轮转角均传输给转向执行机构，用于控制农机转向。
[0022]
上述技术方案中，所述行车控制模块包括模型预测控制器和自适应前馈控制器，所述自适应前馈控制器用于确定前馈补偿角δq，所述模型预测控制器用于求解前轮转角δm。
[0023]
本发明的有益效果为：
[0024]
(1)本发明针对农机作业常出现路径跟踪误差较大的情况，设置不同情况下的前馈补偿角，具体为：当横向误差δx小于等于前馈补偿横向误差下限0.1m时，前馈补偿角为0；当横向误差δx大于前馈补偿横向误差下限0.1m且小于等于前馈补偿横向误差上限2m时，前馈补偿角为k(δx-0.1)；当横向误差δx大于前馈补偿横向误差上限2m时，前馈补偿角为1.9k；本发明可以通过自适应前馈控制器补偿控制量，提高系统响应速度，在横向误差减小后通过模型预测控制器保证跟踪精度，在保证路径跟踪精度的同时提高了路径跟踪的效率。
[0025]
(2)本发明将最终前轮转角设置为δ
final
＝min(δq+δm，δ
max
)，保证前轮转角不会超过机械结构极限，防止转向机构损坏。
附图说明
[0026]
图1为本发明所述用于农机自动驾驶的自适应前馈模型预测控制系统结构示意图；
[0027]
图2为本发明所述用于农机自动驾驶的自适应前馈模型预测控制方法流程图；
[0028]
图3为本发明所述农机路径跟踪示意图；
[0029]
图4为本发明所述车辆运动学模型图；
[0030]
图5为本发明所述不同车速v下前馈补偿角δq与横向误差δx的关系图；
[0031]
图6为直线换道路径下本发明控制方法与普通模型预测控制方法仿真结果对比图；
[0032]
图7为圆周路径下本发明控制方法与普通模型预测控制方法仿真结果对比图。
具体实施方式
[0033]
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
[0034]
如图1所示，一种用于农机自动驾驶的自适应前馈模型预测控制系统，包括导航定位模块、行车控制模块、转向执行机构和转角传感器；导航定位模块安装在农机机身上，导航定位模块将获取的农机行驶状态信息传递给行车控制模块，行车控制模块得到最终前轮转角δ
final
，并传递给转向执行机构控制农机转向，安装在农机前轮的转角传感器实时反馈实际前轮转角，当实际前轮转角等于最终前轮转角δ
final
时，转向结束，从而实现农机的路径跟踪控制；行车控制模块包括模型预测控制器和自适应前馈控制器，自适应前馈控制器根据横向误差δx和车速v确定前馈补偿角δq，模型预测控制器基于车辆运动学模型，根据当前位置(x，y)、航向角参考位置(xr,yr)、参考航向角参考前轮转角δ
fr
、车速v和系统约束求解前轮转角δm。
[0035]
如图2所示，一种用于农机自动驾驶的自适应前馈模型预测控制方法，具体包括如下步骤：
[0036]
步骤(1)，导航定位模块实时获取农机行驶状态信息，包括当前位置(x，y)、航向角和车速v；如图3所示，以田块的长边为x轴，以田块的宽边为y轴，以长宽交点为坐标原点，建立田块坐标系xoy；曲线d为已规划路径的一部分，它是由多个参考路径点连接而成；定义农机质心位置为农机当前位置(x，y)，农机车身与x轴的夹角为航向角已规划路径上离农机当前位置(x，y)最近的参考路径点为参考位置(xr,yr)，行驶过程中的横向误差δx是农机当前位置(x，y)到已规划路径的最短距离；对应参考路径点的航向角为参考航向角对应参考路径点的前轮转角为参考前轮转角δ
fr
，且：
[0037][0038][0039]
式中，已规划路径上离农机当前位置(x，y)第二近的参考路径点位置为(x
rn
,y
rn
)，即下一时刻参考位置，为下一时刻参考航向角。
[0040]
导航定位模块将获取的农机行驶状态信息传递给行车控制模块，其中横向误差δx和车速v传递给预置于行车控制模块中的自适应前馈控制器，当前位置(x，y)、航向角参考位置(xr,yr)、参考航向角参考前轮转角δ
fr
和车速v传递给模型预测控制器。
[0041]
步骤(2)，自适应前馈控制器根据横向误差δx和车速v，确定前馈补偿角δq；
[0042]
当横向误差δx小于等于前馈补偿横向误差下限0.1m时，前馈补偿角δq为0；当横向误差δx大于前馈补偿横向误差下限0.1m且小于等于前馈补偿横向误差上限2m时，前馈补偿角δq为k(δx-0.1)，其中k为斜率，且k＝7.1396v-1.006
(斜率k的获取方法为：基于粒子群算法pso，通过控制路径跟踪收敛后的横向误差和前轮转角增量，得到不同车速v下的最优解k。对不同车速v下的最优k进行曲线拟合和回归分析，得到车速v与k的近似关系k＝7.1396v-1.006
)，车速v的单位为m/s；当横向误差δx大于前馈补偿横向误差上限2m时，前馈补偿角δq为δ
qmax
＝1.9k；上述前馈补偿角δq的确定规则具体表示为：
[0043][0044]
步骤(3)，模型预测控制器基于车辆运动学模型，根据当前位置(x，y)、航向角参考位置(xr，yr)、参考航向角参考前轮转角δ
fr
、车速v和系统约束，求解得到前轮转角δm；
[0045]
模型预测控制器求解前轮转角δm的具体方法为：
[0046]
如图4所示，在田块坐标系下，建立车辆运动学方程：
[0047][0048]
其中，x和y分别为农机质心z的横向坐标和纵向坐标，为车身航向角，为纵向速度，为横向速度，为航向角速度，l为农机车轮轴距，δf为前轮转角，v为行驶速度，oz为转向中心。
[0049]
对于给定的参考路径，可以由农机的运动轨迹描述，运动轨迹上的每一个点都满足车辆运动学方程，用r代表参考量，车辆运动学方程的一般形式为：
[0050][0051]
设定农机行驶速度v与参考车速vr保持一致且恒定，将车辆运动学方程在参考点(xr，yr)处进行泰勒展开并忽略高阶项，可得：
[0052][0053]
将上式与参考点处的车辆运动学方程相减，可得：
[0054][0055]
并对其进行离散化处理，可得到最终的车辆运动学模型为：
[0056][0057]
式中，k为离散变量，为状态变量，为控制变量，状态量转移矩阵控制量转移矩阵t为采样周期。
[0058]
将状态变量与控制变量构造为新的状态量其中ζ(k|t)
为t时刻第k次采样的状态量，为t时刻第k-1次输出的控制量，得到新的状态空间表达式：
[0059][0060][0061]
式中，为控制增量，η为输出量，状态量转移矩阵为控制增量，η为输出量，状态量转移矩阵控制量转移矩阵输出量转移矩阵c
k，t
、i均为单位矩阵。
[0062]
为了方便计算，令
[0063]
设定模型预测控制器的预测时域为n
p
、控制时域为nc，模型预测控制器t时刻的输出为：
[0064]
y(t)＝ψ
t
ζ(k|t)+θ
t
δu(t)
[0065]
其中，预测输出量状态量预测参数控制增量序列控制增量序列预测参数
[0066]
将控制增量作为模型预测控制器目标函数的状态量，并且引入松弛因子，避免出现无可行解的情况，模型预测控制器的目标函数为：
[0067][0068]
式中，η
ref
为参考的输出量，q、f和ρ为权重矩阵，ε为松弛因子。
[0069]
路径跟踪过程中，需要对控制量和控制增量加以约束，模型预测控制器的约束如下：
[0070][0071]
δu
min
(t+j)≤δu(t+j)≤δu
max
(t+j)，j＝0，1...，n
c-1
[0072]
式中，和为控制量的最值，δu
min
和δu
max
为控制增量的最值。
[0073]
将求解带约束的目标函数j(k)转化为求解带约束的线性二次型规划问题，得到控制时域内时刻t的最优控制增量序列δu(t)，将序列的第一个元素δu(k|t)作用于模型预测控制器，模型预测控制器求解出前轮转角
[0074]
步骤(4)，行车控制模块根据前馈补偿角δq与前轮转角δm得到最终前轮转角δ
final
，并传递给转向执行机构控制农机转向，转角传感器实时反馈实际前轮转角，实际前轮转角等于最终前轮转角δ
final
时转向结束，实现农机的路径跟踪控制；其中：
[0075]
δ
final
＝min(δq+δm，δ
max
)
[0076]
式中，δ
max
为农机前轮最大转角。
[0077]
下面结合图1-图7以2个仿真试验说明本发明的实际控制效果。
[0078]
以东方红lf1104-c拖拉机的实车数据建立车辆运动学模型，并在carsim和simulink联合仿真平台上进行仿真试验，行车控制模块的主要参数如表1所示：
[0079]
表1行车控制模块的主要参数
[0080][0081]
图5是根据步骤(2)确定的不同车速v下前馈补偿角δq与车辆横向误差δx之间的关系图，自适应前馈补偿器根据车速v和横向误差δx，便可得到对应的前馈补偿角δq。
[0082]
为了验证本发明用于农机自动驾驶的自适应前馈模型预测控制方法的效果，设计
了两种不同的目标路径(直线换道路径和圆周路径)用于仿真试验，并将本发明提出的自适应前馈模型预测控制(自适应前馈mpc)和普通模型预测控制(普通mpc)进行了对比。图6和图7分别为直线换道路径和圆周路径下两种控制方法仿真结果对比。由仿真结果可知，相比于普通mpc，本发明自适应前馈mpc在跟踪换道路径时性能提升明显，稳定后的横向和航向平均误差分别减少50.9％和18.0％，误差标准差减少明显；相比于普通mpc，本发明自适应前馈mpc跟踪圆周路径时，拖拉机运行稳定后横向和航向平均误差分别减小1.4％和3.1％，误差标准差稍有减少。仿真试验结果表明本发明的路径跟踪控制方法相比于普通mpc有较好的路径跟踪效果，表2为两种路径跟踪的结果对比。
[0083]
表2是两种路径跟踪的结果对比
[0084][0085][0086]
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。