一种智能农机的路径跟踪控制方法与流程

1.本发明涉及智能农机技术领域，具体为一种智能农机的路径跟踪控制方法。


背景技术：

2.路径跟踪误差定义为车辆当前位置与期望路径之间的最短距离，良好的路径跟踪的控制器应保证尽可能小的车辆实际的行驶路径和期望路径之间的平均偏差和最大偏差，自动农用车精确的路径跟踪是提高农业生产效率和质量的有效途径之一。例如手动或自动喷洒农药的喷雾机，由于不精确的路径跟踪而导致实际作业区域发生间隙或重叠，部分作物农药量过量，部分作物农药量不足，进而导致作物产量和质量的降低。由于农用车工作环境复杂，存在未知的噪声和干扰，实现农用车精确的路径跟踪是具有挑战性的。
3.现有的路径跟踪方法可分为三大类：几何方法、运动控制律和最优控制，它们在精度、成本等方面都有各自的问题；现有的智能农机的路径跟踪控制方法，如基于模糊逻辑在线整定控制参数的pid控制方法、基于模糊pid控制与线性二次调节器(lqr)的纵横向动力学控制方法，然而它们都只是在某种特定条件下实现良好的跟踪性能，不是基于能够更加充分考虑农机动力学特性，有助于提高路径跟踪精度的动力学模型的路径跟踪控制方法；还有的是基于运动学的模型预测控制(mpc)方法、鲁棒模型预测控制(rmpc)方法，虽然考虑到了动力学模型，但仅仅在仿真环境里面检验了控制算法的有效性，并没有在实际的农田里检验控制器的实时性、鲁棒性和路径跟踪性能，也就是说，现有的一些可实施的路径跟踪控制方法精度较差，而另一些可以实现高精度路径跟踪的算法又受到了嵌入式系统性能、成本的限制，暂不可部署到真实的农机上。


技术实现要素：

4.本发明的目的在于提供一种智能农机的路径跟踪控制方法，以解决上述背景技术中提出的问题。
5.为实现上述目的，本发明提供如下技术方案：一种智能农机的路径跟踪控制方法，包括以下步骤：步骤一，设计控制系统架构；步骤二，设计智能农机动力学模型；步骤三，设计上层控制器架构；步骤四，基于双无迹卡尔曼滤波的带约束智能农机横向动力学参数估计的实现；步骤五，基于动力学模型的自适应模型预测控制；
6.其中在上述步骤一中，上层控制器获取传感器采集到的农机当前状态信息，并进行状态估计，再根据路径规划器生成的农田全覆盖路径，计算生成农机的目标速度和转角，由车速控制器和转向控制器这些下层控制器最终执行相应的动作，控制系统传感器的配置采用实时动态gps(rtk-gps)+惯性导航系统(imu)，两个rtk-gps接收器分别布置在车头和车尾，惯性导航单元(imu)布置在平台质心附近；
7.其中在上述步骤二中，设计智能农机动力学模型包括以下步骤：
8.1)横向运动和横摆运动建模：由于农机行驶速度较低，非线性动力学特性表现不明显，因此可只考虑其横向运动和横摆运动，这两个运动用线性二自由度模型来描述；横向
运动和横摆运动可由如下的微分方程描述：
[0009][0010]
其中，m为喷雾机质量，iz为喷雾机yaw inertia moment，u为纵向速度，β为质心侧偏角，γ为横摆角速度，δf为前轮转角，δr为后轮转角，cf为前轮侧偏刚度，cr为后轮侧偏刚度，lf为质心到前轴距离，lr为质心到后轴距离；
[0011]
2)转向系统建模：农机采用以液压推杆和连杆为主要部件的转向机构，在合理的控制器的设计下，转向控制器和转向系统的闭环响应可近似为一个一阶系统的闭环响应，因此包括控制器在内的整个转向系统可以建模为：
[0012][0013]
其中，δ为前轮或后轮转角，δe为前轮或后轮的期望转角，τ
δ
为转向系统的时间延迟常数；
[0014]
其中在上述步骤三中，上层控制器结构由一个dukf估计器和一个ampc组成，估计器融合提供于不同传感器的车辆状态信息，来自于传感器或估计器的车辆状态和参数以及由路径规划器生成的参考路径作为ampc的输入，参与到控制量，即期望前轮转角δ
f,e
的计算中，期望前轮转角输入到下层控制器实现转向机构的动作，整个过程按照一定的周期在线进行；
[0015]
其中在上述步骤四中，采用两个无迹卡尔曼滤波器分别用于估计参数和状态，对应了两个独立的状态空间表达式，当估计出一组较好的参数后，则可以暂时关闭参数估计器，以减小上层控制器的计算负担，当当前估计的状态不可信时，则可以重启参数估计器，以实现精确的状态估计；同时，由于估计变量较多，而可用的传感器测量量相对较少，仅凭dukf估计器实现对农机状态和参数的精确估计是很困难的，可能会导致得到没有物理意义的估计值，因此采用pdf截断方法，实现带约束的dukf，此处为提高计算效率，仅对待估计的参数施加约束；具体为：
[0016]
1)获取参数估计和状态估计的离散状态空间表达式：利用一阶向前欧拉公式将式(1)分别变换为用于参数估计和状态估计的离散状态空间表达式；
[0017]
参数估计状态空间表达式为：
[0018]
x
p
(k+1)＝x
p
(k)+w
p
(k)
[0019]zp
(k)＝h
p
(x
p
(k))+v
p
(k)；
ꢀꢀꢀ
(3)
[0020]
其中，x
p
＝[c
f cr]
t
，z
p
＝[βγ]
t
，w
p
和v
p
分别为参数估计的过程噪声和观测噪声；
[0021]
状态估计状态空间表达式：
[0022]
xs(k+1)＝fs(xs(k))+ws(k)
[0023]zs
(k)＝xs(k)+vs(k)；
ꢀꢀꢀ
(4)
[0024]
其中，xs＝[βγ]
t
，zs＝[βγ]
t
，ws和vs分别为状态估计的过程噪声和观测噪声；
[0025]
2)对待估计的参数施加约束：假设在k时刻存在一个对m个参数的无迹卡尔曼滤波
估计x
p
(k|k)，其均值为协方差为p
p
(k|k)，相应地，有m个标量状态约束：
[0026][0027]
其中，lb
ki
≤ub
ki
，θ
ki
为第i个元素为1，其余元素为0的m个元素的列向量；在此条件下，去截断高斯进而找到被截断的pdf的均值和协方差即带约束的参数估计的均值和协方差，定义为执行了前i个约束后的状态估计，为的协方差；
[0028]
3)采用估计器对智能农机状态和参数的实时估计；
[0029]
其中在上述步骤六中，定义a为车辆所在点，b为车辆距离参考轨迹最近的点，航向误差为a点航向和b点航向(路径切线方向)之差，横向误差δy为a点和b点之间的距离，在给定的期望路径下，航向误差和横向误差δy满足：
[0030][0031]
假设航向误差和侧偏角始终较小，则有vy≈v
x
β，则上式可以简化为：
[0032][0033]
其中，γ为喷雾机横摆角速度，v
x
为行驶速度，β为质心侧偏角，ρ为期望路径的曲率；
[0034]
结合式(1)、(2)、(26)，利用一阶向前欧拉公式，得到离散时间系统的状态方程：
[0035]
x(k+1)＝akx(k)+bku(k)+d(k)
[0036]
y(k)＝cx(k)；
ꢀꢀꢀ
(27)
[0037]
该技术方案下的智能农机转向方式为协调转向，可认为前轮转角与后轮转角大小相等方向相反，即δr＝-δf，因此系统的状态变量控制量为期望的前轮转角δ
f,e
，即u＝δ
f,e
，输出量
[0038]
矩阵a为：
[0039]
[0040]
矩阵b为：
[0041][0042]
空间参数d(k)为：
[0043][0044]
矩阵c为：
[0045][0046]
为了避免控制量突变，影响喷雾机路径跟踪的精度和稳定性，因此采用期望的前轮转角增量作为系统的控制量，则系统的状态方程可改写为：
[0047][0048]
其中，
[0049][0050]
根据上述的状态和参数的估计可获得k时刻的状态以及参数结合式(28)，则带空间参数的约束mpc的优化问题可描述为：
[0051][0052]
满足动力学(i＝0,1,
…
,n
p
)
[0053][0054]
满足时域约束：
[0055]umin
(k+i)≤u(k+i)≤u
max
(k+i),i＝0,1,
…
,n
c-1
[0056][0057]
其中，
[0058][0059]
上面的优化问题中，ωy和ωu是加权矩阵，给定为：
[0060][0061][0062]
为控制增量序列，作为约束优化问题的独立变量，定义为：
[0063][0064]
y(k+1|k)是k时刻基于系统(28)预测的n
p
步控制输出，定义为：
[0065][0066]
为避免在求解过程中出现无解的情况，因此在式(29)中引入松弛因子ε，λ为给定常数；联立式(28)、(29)，可得系统预测的n
p
步控制输出：
[0067][0068]
其中，
[0069][0070][0071]
[0072][0073]
由于约束条件存在，一般情况下，无法得到优化问题的解析解，因此，需要采用数值求解方法，也就是需要将约束优化问题转化为二次规划问题描述；
[0074]
二次规划问题的目标函数的标准形式为：
[0075][0076]
变换式(29)为标准形式，忽略与无关项，得
[0077][0078][0079]
求解上式，并将的第一个元素作为控制量，每个周期重复上述过程，实现完整的路径跟踪。
[0080]
优选的，所述步骤一中，平台的路径规划器、上层控制器、下层控制器、rtk-gps以及imu之间均通过can通信。
[0081]
优选的，所述步骤三中，车辆状态信息包括来自于rtk-gps的质心侧偏角测量值βm和车速v，来自于imu的横摆角速度测量值γm，以及来自于霍尔角度传感器的前轮转角δf，待估计的参数为前轴和后轴等效的侧偏刚度cf和cr，待估计的状态为质心侧偏角β和横摆角速度γ。
[0082]
优选的，所述步骤三中，期望的车辆速度是由操作者设定，并由下层控制器操作执行机构实现，而提出的上层控制器与速度无关，故控制器在不同车速下，可以自适应地实现精确的路径跟踪。
[0083]
优选的，所述步骤四3)中，dukf估计器的运行步骤如下：
[0084]
3.1)设定初值
[0085][0086][0087]
3.2)参数预测
[0088]
构造2n+1个样本点：
[0089][0090]
计算参数预测样本点：
[0091][0092]
计算参数预测样本点的均值和方差：
[0093][0094][0095]
3.3)状态预测
[0096]
构造2n+1个样本点：
[0097][0098]
将步骤2中参数预测值带入式(25)计算状态预测样本点：
[0099][0100]
计算状态预测样本点的均值和方差：
[0101][0102][0103]
3.4)状态矫正
[0104]
当获得新的测量值zs(k)后，对状态均值和方差进行更新：
[0105][0106][0107][0108]
其中，
[0109][0110][0111][0112]
3.5)参数矫正
[0113]
当获得新的测量值z
p
(k)，即后，对参数均值和方差进行更新：
[0114]
[0115][0116][0117]
其中，
[0118][0119][0120][0121]
3.6)pdf截断；
[0122]
3.7)重复上述步骤3.2-3.6，即可实现对智能农机状态和参数的实时估计。优选的，所述步骤3.6)pdf截断，具体包括以下步骤：
[0123]
3.6.1)初始化
[0124]
i＝0；
[0125][0126][0127]
其中，被定义为执行了前i个约束后的参数估计，相应的协方差为
[0128]
3.6.2)令i＝1，对进行jordan标准分解得到正交矩阵t
ki
和对角矩阵w
ki
，即：
[0129][0130]
3.6.3)利用gram-schmidt正交化计算m
×
m的正交矩阵ψ
ki
，其满足：
[0131][0132]
定义ψ
ki
是由行向量ψ
ki,j
(j＝1,
…
,m)构成，即：
[0133]
ψ
ki
＝[ψ
ki,1
…
ψ
ki,m
]
t
；
ꢀꢀꢀ
(9)
[0134]
则ψ
ki
的第一行由如下公式计算：
[0135][0136]
对于j＝2,
…
,m，执行如下操作：
[0137][0138]
其中，ej是单位向量，即ej是一个m个元素的除第j个元素为1之外全为0的列向量；
[0139]
若上式计算的ψ
ki,j
为0，则用下式替代：
[0140]
[0141]
再正则化ψ
ki,j
：
[0142][0143]
3.6.4)定义v
ki
为：
[0144][0145]
由式(3)-(5)可知，v
ki
的均值为0，协方差矩阵为恒等(单位)；
[0146]
变换式(1)下界lb
ki
和上界ub
ki
，得到：
[0147][0148][0149]
得到标准化后的标量约束：
[0150]aki
≤[1 0
…
0]v
ki
≤b
ki
；
ꢀꢀꢀ
(17)
[0151]
由此可见，在执行约束之前，v
ki
的第一个元素的分布为n(0,1)，但是约束要求v
ki
必须位于a
ki
和b
ki
之间；
[0152]
3.6.5)定义随机变量v
k,i+1
为pdf被截断后的v
ki
，即：
[0153]
pdf(v
k,i+1
)＝truncated pdf(v
ki
)；
ꢀꢀꢀ
(18)
[0154]
执行第一个约束后的转换参数估计的均值和方差为：
[0155][0156][0157]
其中，
[0158][0159][0160][0161]
则执行第一个约束后参数估计的均值和方差为：
[0162][0163][0164]
将i增加1，并重复步骤6.2-6.5的过程，以获得执行下一个约束后的参数估计；注意，是时间k处的无约束参数估计，是执行第一个约束后在时间k处的参数状态，是执行前两个约束后在时间k处的参数状态，以此类推，经过这个过程m次(每个约束一次)后，得到了最终的约束状态估计和时间k的协方差：
[0165][0166][0167]
与现有技术相比，本发明的有益效果是：相较于现有的路径跟踪控制方法，本发明通过结合带约束的双无迹卡尔曼滤波与自适应模型跟踪控制，提高了智能农机路径跟踪的精度，从而实现了对农机作业效率、作业质量以及经济效益的提高；本发明所提出的路径跟踪控制系统可在实际的农机中部署，在实现良好跟踪精度的前提下，做到了嵌入式系统的可靠性、实时性与成本要求的兼顾。
附图说明
[0168]
图1为本发明的方法流程图；
[0169]
图2为控制系统架构图；
[0170]
图3为农机二自由度动力学模型图；
[0171]
图4为上层控制器架构图；
[0172]
图5为横向误差与航向误差曲线图；
[0173]
图6为前轮转角实际值曲线图；
[0174]
图7为横摆角速度对比曲线图；
[0175]
图8为质心侧偏角对比曲线图；
[0176]
图9为前后轮侧偏刚度估计曲线图；
[0177]
图10为转换到utm坐标系下的农机路径图；
[0178]
图11为农机行驶过程中的横向误差曲线图。
具体实施方式
[0179]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0180]
请参阅图1-5，本发明提供的一种实施例：一种智能农机的路径跟踪控制方法，包括以下步骤：步骤一，设计控制系统架构；步骤二，设计智能农机动力学模型；步骤三，设计上层控制器架构；步骤四，基于双无迹卡尔曼滤波的带约束智能农机横向动力学参数估计的实现；步骤五，基于动力学模型的自适应模型预测控制；
[0181]
其中在上述步骤一中，上层控制器获取传感器采集到的农机当前状态信息，并进行状态估计，再根据路径规划器生成的农田全覆盖路径，计算生成农机的目标速度和转角，由车速控制器和转向控制器这些下层控制器最终执行相应的动作，控制系统传感器的配置采用实时动态gps(rtk-gps)+惯性导航系统(imu)，两个rtk-gps接收器分别布置在车头和车尾，惯性导航单元(imu)布置在平台质心附近，平台的路径规划器、上层控制器、下层控制器、rtk-gps以及imu之间均通过can通信；
[0182]
其中在上述步骤二中，设计智能农机动力学模型包括以下步骤：
[0183]
1)横向运动和横摆运动建模：由于农机行驶速度较低，非线性动力学特性表现不
明显，因此可只考虑其横向运动和横摆运动，这两个运动用线性二自由度模型来描述；横向运动和横摆运动可由如下的微分方程描述：
[0184][0185]
其中，m为喷雾机质量，iz为喷雾机yaw inertia moment，u为纵向速度，β为质心侧偏角，γ为横摆角速度，δf为前轮转角，δr为后轮转角，cf为前轮侧偏刚度，cr为后轮侧偏刚度，lf为质心到前轴距离，lr为质心到后轴距离；
[0186]
2)转向系统建模：农机采用以液压推杆和连杆为主要部件的转向机构，在合理的控制器的设计下，转向控制器和转向系统的闭环响应可近似为一个一阶系统的闭环响应，因此包括控制器在内的整个转向系统可以建模为：
[0187][0188]
其中，δ为前轮或后轮转角，δe为前轮或后轮的期望转角，τ
δ
为转向系统的时间延迟常数；
[0189]
其中在上述步骤三中，上层控制器结构由一个dukf估计器和一个ampc组成，估计器融合提供于不同传感器的车辆状态信息，车辆状态信息包括来自于rtk-gps的质心侧偏角测量值βm和车速v，来自于imu的横摆角速度测量值γm，以及来自于霍尔角度传感器的前轮转角δf，待估计的参数为前轴和后轴等效的侧偏刚度cf和cr，待估计的状态为质心侧偏角β和横摆角速度γ，来自于传感器或估计器的车辆状态和参数以及由路径规划器生成的参考路径作为ampc的输入，参与到控制量，即期望前轮转角δ
f,e
的计算中，期望前轮转角输入到下层控制器实现转向机构的动作，整个过程按照一定的周期在线进行，期望的车辆速度是由操作者设定，并由下层控制器操作执行机构实现，而提出的上层控制器与速度无关，故控制器在不同车速下，可以自适应地实现精确的路径跟踪；
[0190]
其中在上述步骤四中，采用两个无迹卡尔曼滤波器分别用于估计参数和状态，对应了两个独立的状态空间表达式，当估计出一组较好的参数后，则可以暂时关闭参数估计器，以减小上层控制器的计算负担，当当前估计的状态不可信时，则可以重启参数估计器，以实现精确的状态估计；同时，由于估计变量较多，而可用的传感器测量量相对较少，仅凭dukf估计器实现对农机状态和参数的精确估计是很困难的，可能会导致得到没有物理意义的估计值，因此采用pdf截断方法，实现带约束的dukf，此处为提高计算效率，仅对待估计的参数施加约束；具体为：
[0191]
1)获取参数估计和状态估计的离散状态空间表达式：利用一阶向前欧拉公式将式(1)分别变换为用于参数估计和状态估计的离散状态空间表达式；
[0192]
参数估计状态空间表达式为：
[0193]
x
p
(k+1)＝x
p
(k)+w
p
(k)
[0194]zp
(k)＝h
p
(x
p
(k))+v
p
(k)；
ꢀꢀꢀ
(3)
[0195]
其中，x
p
＝[c
f cr]
t
，z
p
＝[βγ]
t
，w
p
和v
p
分别为参数估计的过程噪声和观测噪声；
[0196]
状态估计状态空间表达式：
[0197]
xs(k+1)＝fs(xs(k))+ws(k)
[0198]zs
(k)＝xs(k)+vs(k)；
ꢀꢀꢀ
(4)
[0199]
其中，xs＝[βγ]
t
，zs＝[βγ]
t
，ws和vs分别为状态估计的过程噪声和观测噪声；
[0200]
2)对待估计的参数施加约束：假设在k时刻存在一个对m个参数的无迹卡尔曼滤波估计x
p
(k|k)，其均值为协方差为p
p
(k|k)，相应地，有m个标量状态约束：
[0201][0202]
其中，lb
ki
≤ub
ki
，θ
ki
为第i个元素为1，其余元素为0的m个元素的列向量；在此条件下，去截断高斯进而找到被截断的pdf的均值和协方差即带约束的参数估计的均值和协方差，定义为执行了前i个约束后的状态估计，为的协方差；
[0203]
3)采用估计器对智能农机状态和参数的实时估计，具体包括以下步骤：
[0204]
3.1)设定初值
[0205][0206][0207]
3.2)参数预测
[0208]
构造2n+1个样本点：
[0209][0210]
计算参数预测样本点：
[0211][0212]
计算参数预测样本点的均值和方差：
[0213][0214][0215]
3.3)状态预测
[0216]
构造2n+1个样本点：
[0217][0218]
将步骤2中参数预测值带入式(25)计算状态预测样本点：
[0219]
[0220]
计算状态预测样本点的均值和方差：
[0221][0222][0223]
3.4)状态矫正
[0224]
当获得新的测量值zs(k)后，对状态均值和方差进行更新：
[0225][0226][0227][0228]
其中，
[0229][0230][0231][0232]
3.5)参数矫正
[0233]
当获得新的测量值z
p
(k)，即后，对参数均值和方差进行更新：
[0234][0235][0236][0237]
其中，
[0238][0239][0240][0241]
3.6)pdf截断，具体包括以下步骤：
[0242]
3.6.1)初始化
[0243]
i＝0；
[0244][0245]
[0246]
其中，被定义为执行了前i个约束后的参数估计，相应的协方差为
[0247]
3.6.2)令i＝1，对进行jordan标准分解得到正交矩阵t
ki
和对角矩阵w
ki
，即：
[0248][0249]
3.6.3)利用gram-schmidt正交化计算m
×
m的正交矩阵ψ
ki
，其满足：
[0250][0251]
定义ψ
ki
是由行向量ψ
ki,j
(j＝1,
…
,m)构成，即：
[0252]
ψ
ki
＝[ψ
ki,1
…
ψ
ki,m
]
t
；
ꢀꢀꢀ
(9)
[0253]
则ψ
ki
的第一行由如下公式计算：
[0254][0255]
对于j＝2,
…
,m，执行如下操作：
[0256][0257]
其中，ej是单位向量，即ej是一个m个元素的除第j个元素为1之外全为0的列向量；
[0258]
若上式计算的ψ
ki,j
为0，则用下式替代：
[0259][0260]
再正则化ψ
ki,j
：
[0261][0262]
3.6.4)定义v
ki
为：
[0263][0264]
由式(3)-(5)可知，v
ki
的均值为0，协方差矩阵为恒等(单位)；
[0265]
变换式(1)下界lb
ki
和上界ub
ki
，得到：
[0266][0267][0268]
得到标准化后的标量约束：
[0269]aki
≤[1 0
…
0]v
ki
≤b
ki
；
ꢀꢀꢀ
(17)
[0270]
由此可见，在执行约束之前，v
ki
的第一个元素的分布为n(0,1)，但是约束要求v
ki
必须位于a
ki
和b
ki
之间；
[0271]
3.6.5)定义随机变量v
k,i+1
为pdf被截断后的v
ki
，即：
[0272]
pdf(v
k,i+1
)＝truncated pdf(v
ki
)；
ꢀꢀꢀ
(18)
[0273]
执行第一个约束后的转换参数估计的均值和方差为：
[0274][0275][0276]
其中，
[0277][0278][0279][0280]
则执行第一个约束后参数估计的均值和方差为：
[0281][0282][0283]
将i增加1，并重复步骤6.2-6.5的过程，以获得执行下一个约束后的参数估计；注意，是时间k处的无约束参数估计，是执行第一个约束后在时间k处的参数状态，是执行前两个约束后在时间k处的参数状态，以此类推，经过这个过程m次(每个约束一次)后，得到了最终的约束状态估计和时间k的协方差：
[0284][0285][0286]
3.7)重复上述步骤3.2-3.6，即可实现对智能农机状态和参数的实时估计；
[0287]
其中在上述步骤六中，定义a为车辆所在点，b为车辆距离参考轨迹最近的点，航向误差为a点航向和b点航向(路径切线方向)之差，横向误差δy为a点和b点之间的距离，在给定的期望路径下，航向误差和横向误差δy满足：
[0288][0289][0290]
假设航向误差和侧偏角始终较小，则有vy≈v
x
β，则上式可以简化为：
[0291][0292][0293]
其中，γ为喷雾机横摆角速度，v
x
为行驶速度，β为质心侧偏角，ρ为期望路径的曲率；
[0294]
结合式(1)、(2)、(26)，利用一阶向前欧拉公式，得到离散时间系统的状态方程：
[0295]
x(k+1)＝akx(k)+bku(k)+d(k)
[0296]
y(k)＝cx(k)；
ꢀꢀꢀ
(27)
[0297]
该技术方案下的智能农机转向方式为协调转向，可认为前轮转角与后轮转角大小相等方向相反，即δr＝-δf，因此系统的状态变量控制量为期望的前轮转角δ
f,e
，即u＝δ
f,e
，输出量
[0298]
矩阵a为：
[0299][0300]
矩阵b为：
[0301][0302]
空间参数d(k)为：
[0303][0304]
矩阵c为：
[0305][0306]
为了避免控制量突变，影响喷雾机路径跟踪的精度和稳定性，因此采用期望的前轮转角增量作为系统的控制量，则系统的状态方程可改写为：
[0307][0308][0309]
其中，
[0310][0311]
根据上述的状态和参数的估计可获得k时刻的状态以及参数结合式(28)，则带空间参数的约束mpc的优化问题可描述为：
[0312][0313]
满足动力学(i＝0,1,
…
,n
p
)
[0314][0315][0316][0317]
y(k|k)＝y(k)；
[0318]
满足时域约束：
[0319]umin
(k+i)≤u(k+i)≤u
max
(k+i),i＝0,1,
…
,n
c-1
[0320][0321]
其中，
[0322][0323]
上面的优化问题中，ωy和ωu是加权矩阵，给定为：
[0324][0325][0326]
为控制增量序列，作为约束优化问题的独立变量，定义为：
[0327][0328]
y(k+1|k)是k时刻基于系统(28)预测的n
p
步控制输出，定义为：
[0329][0330]
为避免在求解过程中出现无解的情况，因此在式(29)中引入松弛因子ε，λ为给定常数；联立式(28)、(29)，可得系统预测的n
p
步控制输出：
[0331]
[0332]
其中，
[0333][0334][0335][0336][0337]
由于约束条件存在，一般情况下，无法得到优化问题的解析解，因此，需要采用数值求解方法，也就是需要将约束优化问题转化为二次规划问题描述；
[0338]
二次规划问题的目标函数的标准形式为：
[0339][0340]
变换式(29)为标准形式，忽略与无关项，得
[0341][0342]
求解上式，并将的第一个元素作为控制量，每个周期重复上述过程，实现完整的路径跟踪。
[0343]
将上述实施例中所提出的方法运用在实际的农机中进行试验，并采用“s”型路径作为农机路径跟踪的参考路径；试验结果见图6-11，其中，图7-9反映了带约束的状态和参数估计器的工作情况，由于初始不知道侧偏刚度真实值，因此前后轮侧偏刚度从一个指定
的初始值出发，随着农机的行驶，估计的前后轮侧偏刚度逐渐收敛到真实值，同时估计的质心侧偏角也逐渐收敛到了真实值；结果表明，所设计的带约束的状态和参数估计器可以实时估计出农机真实的动力学状态和参数，为基于动力学模型的模型预测控制提供了可靠的保障；图10和图11反映了农机实际的行驶路径以及与期望路径之间的偏差，可以看到，在农机行驶的整个过程，其最大横向偏差不超过0.03m，表明控制器可以实现智能农机的高精度路径跟踪，进而可以提高作业的质量与效率。
[0344]
基于上述，本发明提出了智能农机路径跟踪控制方法，通过结合pdf截断的双无迹卡尔曼滤波(dukf)和带空间参数的自适应mpc，设计了一个适用于智能农机路径跟踪的控制器，可以有效提高智能农机路径跟踪的精度，该方法可运用在实际的农机中，在保证良好跟踪精度的前提下，做到了嵌入式系统的可靠性，实时性与成本要求的兼顾。
[0345]
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。