基于自适应时域模型预测的拖拉机路径跟踪控制方法及系统与流程

1.本发明属于车辆自动驾驶技术领域，具体涉及一种基于自适应时域模型预测的拖拉机路径跟踪控制方法及系统。


背景技术：

2.随着智慧农业的不断发展，农用车辆自动驾驶越来越受到人们的关注。农用车辆自动驾驶的关键在于路径规划和路径跟踪控制。路径跟踪是自动驾驶中重要的一环，它的好坏直接影响到整个驾驶系统能否完成给定的任务。
3.模型预测控制因其具有良好跟踪精度和抑制滞后能力在路径跟踪中得到应用，但传统的模型预测较少考虑车速和路径曲率变化的影响，大多采用固定的预测时域和控制时域，对环境的适应性较差，难以达到期望的跟踪效果。


技术实现要素：

4.针对现有技术中存在不足，本发明提供了一种基于自适应时域模型预测的拖拉机路径跟踪控制方法及系统，根据车速和规划路径曲率自适应调整预测时域和控制时域，提高控制方法对不同路径的适应性和路径跟踪效果。
5.本发明是通过以下技术手段实现上述技术目的的。
6.一种基于自适应时域模型预测的拖拉机路径跟踪控制方法，具体为：
7.时域优化模块根据当前车速v，寻找对应的预测时域n
p
和控制时域nc的最优解；时域优化模块再根据参考路径曲率kr，对求解的预测时域n
p
和控制时域nc进行修正；
8.利用修正后的预测时域n
pc
和控制时域n
cc
更新自适应时域模型预测控制器中的预测时域和控制时域；
9.自适应时域模型预测控制器根据当前位置(x，y)、航向角参考位置(xr，yr)、参考航向角参考前轮转角δ
fr
、当前车速v、修正后的预测时域n
pc
、修正后的控制时域n
cc
和系统约束优化求解出前进车速vm和前轮转角δm；
10.当前车速v达到前进车速vm时，加减速结束；实际前轮转角δ达到前轮转角δm时，转向结束；实现拖拉机的路径跟踪控制。
11.进一步地，所述对求解的预测时域n
p
和控制时域nc进行修正为：
12.n
pc
＝round(γ*kr n
p
)
13.n
cc
＝round(∈*kr nc)
14.γ≥∈
15.式中，n
pc
和n
cc
分别为修正后的预测时域和控制时域，γ为预测时域的曲率增益系数，∈为控制时域的曲率增益系数。
16.进一步地，寻找对应的预测时域n
p
和控制时域nc的最优解采用改进粒子群优化算法，改进粒子群优化算法的适应度函数为：
17.18.n
p
≥nc＞0
19.式中，α和β为权重系数，是路径跟踪的平均横向误差，δδv是跟踪过程中前轮转角增量的最大值。
20.进一步地，所述前进车速vm和前轮转角δm为：
[0021][0022]
其中：为t时刻第k次输出的控制量，为t时刻第k-1次输出的控制量，δu(k)为控制增量。
[0023]
进一步地，所述参考航向角其中(x
rn
，y
rn
)为下一时刻参考位置。
[0024]
更进一步地，所述参考前轮转角其中为下一时刻参考航向角，l为农机车轮轴距。
[0025]
更进一步地，所述路径曲率其中分别为yr对xr一阶导数和二阶导数。
[0026]
一种基于自适应时域模型预测的拖拉机路径跟踪控制系统，包括：
[0027]
导航定位模块，采集拖拉机的当前位置、航向角和当前车速；
[0028]
时域优化模块，对预测时域和控制时域进行修正；
[0029]
自适应时域模型预测控制器，求解前进车速和前轮转角；
[0030]
转角传感器，实时反馈实际前轮转角；
[0031]
动作执行机构，根据前进车速和前轮转角实现拖拉机的路径跟踪。
[0032]
本发明的有益效果为：本发明中时域优化模块寻找出预测时域和控制时域的最优解，并对求解的预测时域和控制时域进行修正，利用修正后的预测时域和控制时域更新自适应时域模型预测控制器中的预测时域和控制时域；本发明中实现了对预测时域和控制时域实时在线调整，保证了控制器对车速和曲率变化的适应性，提高了路径跟踪的精度和稳定性。
附图说明
[0033]
图1为本发明所述基于自适应时域模型预测的拖拉机路径跟踪控制系统结构示意图；
[0034]
图2为本发明所述基于自适应时域模型预测的拖拉机路径跟踪控制方法流程图；
[0035]
图3为本发明所述拖拉机路径跟踪示意图；
[0036]
图4为本发明所述改进粒子群算法流程图；
[0037]
图5为本发明所述车辆运动学模型图。
具体实施方式
[0038]
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
[0039]
如图1所示，本发明一种基于自适应时域模型预测的拖拉机路径跟踪控制系统，包括导航定位模块、行车控制模块、动作执行机构和状态反馈模块(包括导航定位模块和安装在拖拉机前轮的转角传感器)，行车控制模块包括时域优化模块和自适应时域模型预测控制器。
[0040]
导航定位模块实时采集拖拉机行驶状态信息，包括当前位置(x，y)、航向角和当前车速v，定义已规划路径上离拖拉机当前位置(x，y)最近的路径点为参考位置(xr，yr)，参考位置对应路径点的航向角为参考航向角参考位置对应路径点的前轮转角为参考前轮转角δ
fr
，参考位置对应路径点的路径曲率为参考路径曲率kr。导航定位模块将采集到的拖拉机行驶状态信息发送给行车控制模块，其中当前车速v和参考路径曲率kr传递给时域优化模块，当前位置(x，y)、航向角参考位置(xr，yr)、参考航向角参考前轮转角δ
fr
和当前车速v传递给自适应时域模型预测控制器；时域优化模块先根据当前车速v，采用改进粒子群算法得到对应车速的预测时域n
p
和控制时域nc，时域优化模块再根据参考路径曲率kr，对预测时域n
p
和控制时域nc进行修正，并利用修正后的预测时域n
pc
和控制时域n
cc
更新自适应时域模型预测控制器的预测时域和控制时域；自适应时域模型预测控制器基于车辆运动学模型，根据当前位置(x，y)、航向角参考位置(xr，yr)、参考航向角参考前轮转角δ
fr
、当前车速v、修正后的预测时域n
pc
、控制时域n
cc
和系统约束优化，求解出前进车速vm和前轮转角δm。动作执行机构具体为：电子油门、电子刹车和转向执行机构，电子油门和刹车根据前进车速vm输出相应的油门和刹车电压值，转向执行机构根据前轮转角δm转动相应的角度。导航定位模块向动作执行机构实时反馈拖拉机实际当前车速v，当前车速v达到最终前进车速vm时加减速结束；安装在拖拉机前轮的转角传感器向动作执行机构实时反馈实际前轮转角δ，实际前轮转角δ达到最终前轮转角δm时转向结束。
[0041]
如图2所示，本发明一种基于自适应时域模型预测的拖拉机路径跟踪控制方法，具体包括如下步骤：
[0042]
步骤(1)，导航定位模块实时采集拖拉机行驶状态信息，并发送给行车控制模块
[0043]
如图3所示，以田块的长边为x轴，以田块的宽边为y轴，以长宽交点为坐标原点建立田块坐标系xoy；曲线d为已规划路径的一部分，它是由多个参考路径点连接而成；定义拖拉机质心位置为拖拉机当前位置(x，y)，拖拉机车身与x轴的夹角为航向角已规划路径上离拖拉机当前位置(x，y)最近的参考路径点为参考位置(xr，yr)，行驶过程中的横向误差δx是拖拉机当前位置(x，y)到已规划路径的最短距离；参考位置对应参考路径点的航向角为参考航向角参考位置对应路径点的前轮转角为参考前轮转角δ
fr
，参考位置对应路径点的路径曲率为参考路径曲率kr，且：
[0044][0045][0046][0047]
其中，已规划路径上离拖拉机当前位置(x，y)第二近的参考路径点位置为(x
rn
，y
rn
)，即下一时刻参考位置，为下一时刻参考航向角，分别为yr对xr一阶导数和二
阶导数。
[0048]
步骤(2)，时域优化模块先根据当前车速v，采用改进粒子群优化算法寻找对应的预测时域n
p
和控制时域nc的最优解；
[0049]
如图4所示，在每次迭代过程中，粒子通过个体极值群体极值和惯性因子更新自身的速度和位置更新公式如下：
[0050][0051][0052][0053]
式中，i＝1，2，...，n，n是粒子总数；d是当前迭代次数；c1和c2是学习因子；r1和r2是介于(0，1)之间的随机数；ω
max
、ω
min
是预设的最大惯性系数和最小惯性系数；是预设的最大惯性系数和最小惯性系数；即迭代d次时所有粒子的最小适应度值；是迭代d次时所有粒子的平均适应度值；适应度函数为：
[0054][0055]np
≥nc＞0
[0056]
式中，α和β为权重系数；是路径跟踪的平均横向误差，反映控制系统的精度；δδv是跟踪过程中前轮转角增量的最大值，反映控制系统的稳定性。
[0057]
惯性因子对于粒子群算法的收敛性有较大的影响，较大的惯性因子有利于全局搜索，较小的惯性因子有利于局部搜索；固定的惯性因子在整体求解的过程中存在不少缺陷。因而，引入随适应度实时调整的惯性因子，提高算法的寻优速度。适应度越小，说明距离最优解越近，减小惯性因子提高局部搜索能力；反之，适应度越大，说明距离最优解越远，增大惯性因子提高全局搜索能力。
[0058]
将拖拉机最低稳定车速到最高车速的范围平均分为10个区间，分别基于改进粒子群算法得到对应的预测时域n
p
和控制时域nc最优解。
[0059]
步骤(3)，时域优化模块再根据参考路径曲率kr，对改进粒子群算法求解的预测时域n
p
和控制时域nc进行修正：
[0060]npc
＝round(γ*kr n
p
)
[0061]ncc
＝round(∈*kr nc)
[0062]
γ≥∈
[0063]
式中，n
pc
和n
cc
分别为修正后的预测时域和控制时域，γ为预测时域的曲率增益系数，∈为控制时域的曲率增益系数。
[0064]
最后用修正后的预测时域n
pc
和控制时域n
cc
更新自适应时域模型预测控制器中的预测时域和控制时域。
[0065]
步骤(4)，自适应时域模型预测控制器基于车辆运动学模型，根据当前位置(x，y)、航向角参考位置(xr，yr)、参考航向角参考前轮转角δ
fr
、当前车速v、修正后的预测时域n
pc
、控制时域n
cc
和系统约束优化求解出前进车速vm和前轮转角δm，具体建模及计算过程
如下：
[0066]
如图5所示，在田块坐标系下，建立车辆运动学方程：
[0067][0068]
其中，x和y分别为拖拉机质心z的横向坐标和纵向坐标，为纵向速度，为横向速度，为航向角速度，l为轴距，δf为前轮转角，oz为转向中心。
[0069]
对于给定的参考路径，可以由拖拉机的运动轨迹描述，其上的每一个点都满足上述车辆运动学方程，用r代表参考量，一般形式为：
[0070][0071]
设定拖拉机的参考车速vr恒定。
[0072]
将车辆运动学方程在参考点(xr，yr)处进行泰勒展开并忽略高阶项，可得：
[0073][0074]
将泰勒展开式与参考点处的车辆运动学方程相减，可得：
[0075][0076]
对其进行离散化处理，可得到最终的车辆运动学模型：
[0077][0078]
式中，k为离散变量，为状态变量，为控制变量，状态量转移矩阵控制量转移矩阵t为采样周期。
[0079]
将状态变量与控制变量构造为新的状态量其中ζ(k|t)为t时刻第k次采样的状态量，为t时刻第k-1次输出的控制量，得到新的状态空间表达式：
[0080][0081][0082]
式中，为控制增量，η为输出量，状态量转移矩阵为控制增量，η为输出量，状态量转移矩阵控制增量转移矩阵输出量转移矩阵c
k，t
、i均为单位矩阵。
[0083]
为了方便计算，令
[0084]
用修正后的预测时域n
pc
和控制时域n
cc
更新预测模型中的预测时域和控制时域。自适应模型预测控制器t时刻的输出为：
[0085]
y(t)＝ψ
t
ζ(k|t) θ
t
δu(t)
[0086]
其中，预测输出量状态量预测参数控制增量序列控制增量序列预测参数
[0087]
将控制增量作为自适应时域模型预测控制器目标函数的状态量，并且引入松弛因子避免出现无可行解的情况；模型预测控制的目标函数表达式为：
[0088][0089]
式中，η
ref
为参考的输出量，q、f和ρ为权重矩阵，ε为松弛因子。
[0090]
路径跟踪过程中，需要对控制量和控制增量加以约束，约束条件如下：
[0091][0092]
δu
min
(t j)≤δu(t j)≤δu
max
(t j)，j＝0，1...，n
cc-1
[0093]
式中，和为控制量的最值，δu
min
和δux
ax
为控制增量的最值。
[0094]
将求解带约束的目标函数j(k)转化为求解带约束的线性二次型规划问题，得到控制时域内时刻t的最优控制增量序列δu(t)，将序列的第一个元素δu(k|t)作用于模型预测控制器，模型预测控制器求解出前进车速vm和前轮转角δm：
[0095][0096]
步骤(5)，根据自适应时域模型预测控制器输出的前进车速vm和前轮转角δm，电子油门和电子刹车输出相应的油门和刹车电压值，转向执行机构转动相应的角度。
[0097]
导航定位模块实时反馈拖拉机实际当前车速v，当前车速v达到最终前进车速vm时加减速结束；安装在拖拉机前轮的转角传感器实时反馈实际前轮转角δ，实际前轮转角δ达到最终前轮转角δm时转向结束；由此实现拖拉机的路径跟踪控制。
[0098]
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。