论文部分内容阅读
摘要 针对具有非线性特征的智能拖拉机运动控制问题，提出了一种基于模型预测控制（MPC）的横向轨迹实时跟踪控制策略。从非线性运动模型线性化和离散化入手，建立以前轮转角增量和纵向速度增量为状态量的目标函数;设计系统控制量极限约束和控制增量约束，并将MPC最优求解问题转化为二次规划问题，目标函数采用有松弛因子和无松弛因子2種形式。基于Matlab/Simulink平台设计MPC控制器，进行了2种目标函数的固定坐标系下的给定轨迹跟踪试验。结果表明：2种控制算法均可以快速跟踪期望路径，目标函数具有松弛因子的MPC控制器的拖拉机跟踪效果和平稳性更好，前轮转角始终被限定在约束范围内。
关键词 智能拖拉机;模型预测控制;轨迹跟踪
中图分类号 TP 24;S 219 文献标识码 A 文章编号 0517-6611（2021）19-0191-04
doi：10.3969/j.issn.0517-6611.2021.19.050
开放科学（资源服务）标识码（OSID）：
Path Tracking of Intelligent Tractor Based on MPC
ZHAO Xiang-jun，ZHANG Pei-pei
（School of Engineering， Zhejiang Agriculture and Forestry University， Hangzhou，Zhejiang 311300）
Abstract In order to investigate the motion control with high nonlinearity for intelligent tractor， a lateral control method was proposed based on model predictive control （MPC）. Although tractor model has high nonlinearity， the model selected the linear time-varying error model of kinematic model as the predictive model to improve operation velocity. Objective function that selected the control increments， longitudinal velocity increment and front wheel angle increment， as state variables was established. Meanwhile， two control schemes with and without relaxation factor were proposed. With the design of the limiting constraints of control variable and control increment， the solution of objective function was changed into quadratic programming problem. Both of model predictive controllers were designed based on Matlab/Simulink. Results showed that both of path tracking controllers effectively track the reference path and the scheme with relaxation factor had smaller displacement errors and heading errors. Meanwhile front wheel angle were always restricted within the constraint range.
Key words Intelligent tractor;MPC;Path tracking
基金项目 浙江省公益技术研究项目（2017C32070）。
作者简介 赵相君（1976—），男，甘肃会宁人，实验师，从事农用车辆的运用与维护研究。*通信作者，讲师，博士，从事车辆系统动力学研究。
收稿日期 2021-02-23
拖拉机无人驾驶技术已成为农业装备智能化研究的热点之一。无人驾驶车辆的路径跟踪控制是无人驾驶车辆运动控制的核心，路径跟踪技术研究内容包括横向控制与纵向控制。横向控制主要是通过控制车辆转向实现对期望路径的追踪，旨在保证跟踪过程中航向角偏差和横向偏差趋近于0[1-3]，是拖拉机自主行驶的一项核心技术。
关于横向控制的路径跟踪模型主要有转向几何学、动力学和运动学模型[4-5]，随着控制技术的发展，众多学者对各类路径跟踪模型提出了相应的控制策略[5-12]。应用较多的有PID控制、模糊控制、滑膜控制、最优控制、LQR控制、模型预测控制（model predictive control，MPC）、非线性模型预测控制（nonlinear model predictive control，NMPC）等。这些控制器在路径跟踪时车辆航向角和横向位移的精度上都取得了满意的效果。
MPC可以考虑系统的多约束条件，在过去的20多年内被广泛应用于路径跟踪控制。严国军等[13]设计的自适应MPC控制器在确保无人驾驶拖拉机轨迹跟踪效果的前提下，保证了控制系统的实时性;张广洁等[14]对比了线性模型预测控制、线性时变模型预测和非线性模型预测控制3种算法对AUV的直线跟踪效果;白国星等[15]为削弱控制器对路径曲率突变和航向突变的响应能力，基于NMPC提出了减少控制步数或降低控制频率2种实时优化方案，但这些研究采用的控制目标函数都不加松弛因子。 笔者采用有松弛因子和无松弛因子2种控制目标函数的MPC算法研究智能拖拉机的路径跟踪控制问题，并通过Matlab/Simulink对比分析2种方案的控制效果。
1 运动学模型的建立
拖拉机的速度较低，一般为0.5～3.0 m/s，因此可作如下假设：只作平行于地面的平面运动;车辆速度较低，车轮没有侧滑;转向过程中车辆质心侧偏角保持不变。Karkee等[10]研究表明当牵引速度小于4.5 m/s，输入频率小于1 rad/s时，采用运动学模型设计的轨迹跟踪控制器具有可靠的控制性能，因此采用运动学模型作为跟踪控制模型，如图1所示。XOY为固定坐标系;（ x，y ）为车辆后轴中心的坐标; l 为轴距，单位m; φ 为车体的航向角（°）; δ 为前轮转角（°）; v 为车辆后轴中心速度（m/s）; P 为车辆的瞬时转动中心; R 为后轮转向半径（m）; B 1、B 2 分别为前后轮轮距（m）; η 为车辆后轴中心的轴迹。
根据图1模型，建立拖拉机运动学微分方程：
x=v cos φ
y=v sin φ
φ=v tan δl（1）
拖拉机可以被看作一个控制量u=[v，δ] T 、状态量X=[x，y，φ] T的控制系统，同时该系统具有明显的非线性。
2 MPC控制器设计
MPC具有系统模型多约束处理的优势，能够与规划控制、感知过程的传感器数据预处理算法很好结合，具有较强的鲁棒性和较好的控制效果。MPC包括模型线性化模块、约束条件设计、求解目标函数3个部分。
采用有松弛因子和无松弛因子2种MPC算法来实现对拖拉机的横向跟踪控制，如图2所示。MPC控制器根据期望轨迹[ x ref， y ref， φ ref]T、参考速度 v ref和参考前轮转角 δ ref，基于车辆运动学模型求出前轮转角 δ ，将求解的控制量输出给车辆模型，同时加入延时模块。MPC控制器的控制算法主要包括将拖拉机的非线性模型线性化和离散化处理，得到线性化的智能拖拉机偏差模型，作为控制器预测方程;将参考轨迹离散化，设计具有约束条件的目标函数。
首先，对拖拉机的非线性模型进行线性化和离散化处理，将线性化的智能拖拉机偏差模型作为控制器预测方程。为得到线性化的系统偏差模型，将公式（1）在參考轨迹点采用泰勒级数展开并忽略高级项，设参考轨迹状态量 X ref=[ x ref， y ref， φ ref]T和控制量 u ref=[ v ref， δ ref]T，系统偏差 X e=[e x，e y，e φ ]T，得到：
X e = X - X ref= AX e + Bu e （2）
式中， A =00- v refsin φ ref
00 v refcos φ ref
000，
B =cos φ ref0
sin φ ref0
tan δ ref l v ref l cos2 δ ref
为了能将该模型应用于MPC控制器的设计，对偏差模型公式（2）进行离散化：
X e（k+1） = A k，t X e（k） + B k，t u e（k） （3）
式中， A k，t =I+ TA，B k，t = TB ， T 为采样时间。 X e（k） 和 u e（k）为在k时刻系统的状态量偏差和控制量偏差。
设 ξ （k/t）=X e（k/t）u e（k-1/t），则系统可表达为新的状态空间模型：
ξ （k+1/t）=A k，t ξ （k/t）+B k，t Δ U（k/t）
η（k/t）=C k，t ξ （k/t）（4）
式中，A k，t=A k，t B k，t0 m×n I m，B k，t=B k，tI m，m为控制量维数，n为状态量维数，m=3，n=2。简化计算，作如下假设：
A k，t=A t，t，B k，t=B t，t，k=1，…，t+N-1
因此可以得到预测输出模型：
Y（t）=Ψ t ξ （t/t）+Θ Δ U（t）（5）
式中，Y（t）=
η （t+1/t）
η （t+2/t）
η （t+N p/t），
Ψ t=
C t，tA t，t
C t，tA2 t，t
C t，tCN p t，t，
Θ t=
C t，tB t，t000
C t，tA t，tB t，tC t，tB t，t00
C t，tAN c-1 t，tB t，tC t，tAN c-2 t，tB t，t … C t，tB t，t
C t，tAN p-1 t，tB t，tC t，tAN p-2 t，tB t，t … C t，t A N p-N c-1 t，tB t，t
，
Δ U（t）= Δ u（t/t）
Δ u（t+1/t）
Δ u（t+N c/t）。
控制过程中约束条件主要是控制量极限约束和控制增量约束，即：
u min （t+k）≤u（t+k）≤u max （t+k）
Δ u min （t+k）≤ Δ u（t+k）≤ Δ u max （t+k）
，k=0，1， … ，N c-1。（6）
为了能够保证系统快速平稳地跟踪期望路线，就需要对系统状态量的偏差和控制量进行优化，实际求解中系统的控制量是未知的，需要设定合适的优化目标求解控制时域内的控制序列。
无松弛因子的目标函数：
min J（k）= N pi=1‖ Δ η（k+i/t）‖2 Q+ N c-1i=1‖ Δ U（k+i/t）‖2 R（7）
加入松弛因子的目标函数[16]：
min J（k）= N pi=1‖ Δ η（k+i/t）‖2 Q+ N c-1i=1‖ Δ U（k+i/t）‖2 R+ρε2（8）
式中， Δ η（k+i/t）为实际输出与期望轨迹之差， Δ U（k+i/t）=U（k+i+1/t）-U（k+i/t），N p为预测时域，N c为控制时域，Q为误差奖惩项权重矩阵，R为控制量增量奖惩项权重矩阵，ρ为权重系数，ε为松弛因子。
目标函数（8）求解的变量是控制时域内的控制量U t，设U t=1 Nc u （k-1），1 NC是行数为N c的列向量，为克罗内克积，u（k-1）为上一时刻实际的控制量，且满足：
U min ≤ U t+M Δ U t≤U max （9）
式中，U min 、U max 分别为控制时域内控制量的最大值和最小值集合，则：
M =
100…0
110…0
111…0
0
111…1I m。
最终目标函数进一步转化为标准二次型形式并结合约束条件，结合公式（6）～（9），无人驾驶拖拉机追踪期望路径的优化问题可以表达如下：
（1）无松弛因子。
J（k）= N pi=1‖ Δ η（k+i/t）‖2 Q+ N c-1i=1‖ Δ U（k+i/t）‖2 R
s.t. Δ U min ≤ Δ U t≤ Δ U max
U min ≤U t+M Δ U t≤U max （10）
（2）有松弛因子。
J[ ξ （t），u（t-1）， Δ U（t）]=[ Δ U（t） T ，ε] T H t[ Δ U（t） T ，ε]+G t[ Δ U（t） T ，ε]
s.t. Δ U min ≤ Δ U t≤ Δ U max （11）
U min ≤U t+M Δ U t≤U max
式中，H t=Θ T tQΘ t+R00ρ，G t=[2e T tQΘ t 0]，e t=Ψ t ξ （t/t）-Y ref （t），e t為预测时域内的跟踪偏差，Y ref （t）=[η ref （t+1/t），…，η ref （t+N p/t）] T 。
每个控制周期内完成公式（10）、（11）的求解后，得到控制时域内的一系列控制输入增量：
Δ U* t=[ Δ u* t Δ u* t+1 … Δ u* t+N c-1] T
将该控制序列中第一个元素 Δ u* t作为实际的控制输入增量作用于系统，则u（t）=u（t-1）+ Δ u* t ，进入下一个控制周期后重复上述过程，如此循环就可实现对拖拉机的轨迹跟踪。
3 路径跟踪仿真试验分析
根据上述MPC控制器的设计，搭建仿真模块并在Matlab/Simulink 环境下进行拖拉机运动仿真验证，参考典型拖拉机地头掉头行驶路径，设置期望路径如图2所示，主要由直线和圆弧组成，圆弧的半径为10 m，期望纵向速度为1 m/s。
设置初始时刻为 t 0=0，末端时刻 t 1=70 s，求解算法采用龙格-库塔数值解法，预测步长 N p 为60，控制步长 N c 为30，权重系数 ρ 为10，松弛因子 ε 为10，采样时间 T 为0.1 s。
控制量约束设置为-0.2 m/s≤ v-v ref≤0.2 m/s，-0.02 m/s≤Δ v ≤0.02 m/s，-17°≤ δ ≤17°，-0.974°≤Δ δ ≤ 0.974°，Δ v 为每个控制周期的速度增量，Δ δ 为每个控制周期的前轮转角增量。
拖拉机轨迹跟踪初始位置 x=0，y=0，φ=0 ，仿真试验结果如图3～5所示。
横向跟踪仿真结果如图3、4所示，由于拖拉机给定的初始状态与轨迹参考值存在初始偏差，无人驾驶车辆在2种MPC控制方案的作用下均能快速调整航向角跟踪期望轨迹。松弛因子 ε =10的控制方案，横向位置跟踪偏差和航向角偏差逐步减少收敛到0，系统稳定性好。无松弛因子的控制方案，待系统稳定后，横向位置跟踪偏差保持在（-0.15，0.15）m，同时航向角偏差会有一定的波动，在（-2°，2°）之间。由此可见，加入松弛因子的MPC控制方案，系统跟踪轨迹更加贴合期望路径，给定路径的跟踪能力更强、稳定性更好。 2种MPC控制方案控制量前轮转角的变化如图5所示，明显松弛因子 ε =10的控制方案优于无松弛因子的方案，在路径曲率发生变化时均可快速调整，随时间平稳变化，未有突变，始终在给定约束条件范围内。
4 结语
针对非线性智能拖拉机横向跟踪控制问题，分析了目标函数有松弛因子和无松弛因子2种基于MPC的纵横向跟踪控制方法。无松弛因子的目标函数优点在于容易转化为标准二次型。参考典型地头掉头行驶路径，完成参考路径和参考速度规划。基于拖拉机运动学模型和MPC控制器提供的速度预测序列和前轮转角序列预测拖拉机姿态变化，求解模型预测控制目标函数获得纵向速度和前轮转角控制量，并在Matlab/Simulink仿真平台进行了仿真验证。
仿真结果表明，加入松弛因子后航向角可以快速调整跟踪期望轨迹，横向位置跟踪偏差、航向角偏差逐步收敛到0，跟踪平稳性更好。
参考文献
[1] 吴延霞，赵博，毛恩荣.农业车辆自动导航系统综述[J].农机化研究，
2009，31（3）：242-245，252.
[2] 吕安涛，毛恩荣，宋正河，等.一种拖拉机自动驾驶复合模糊控制方法[J].农业机械学报，2006，37（4）：17-20.
[3] 郭景華，李克强，罗禹贡.智能车辆运动控制研究综述[J].汽车安全与节能学报，2016，7（2）：151-159.
[4] SNIDER J M.Automatic Steering Methods for Autonomous Automobile Path Tracking[R].Pittsburgh：Carnegie Mellon University，Robotics Institute，2009.
[5] 郭应时，蒋拯民，白艳，等.无人驾驶汽车路径跟踪控制方法拟人程度研究[J].中国公路学报，2018，31（8）：189-196.
[6] ZHANG X，GEIMER M，NOACK P O，et al.A semi-autonomous tractor in an intelligent master-slave vehicle system[J].Intelligent service robotics，2010，3（4）：263-269.
[7] 张硕，刘进一，杜岳峰，等.基于速度自适应的拖拉机自动导航控制方法[J].农业工程学报，2017，33（23）：48-55.
[8] LENAIN R，THUILOT B，CARIOU C，et al.Model predictive control for vehicle guidance in presence of sliding：Application to farm vehicles path tracking[C]//Proceedings of the 2005 IEEE International Conference on Robotics and Automation.Barcelona， Spain：ICRA，2005：885-890.
[9] BACKMAN J，OKSANEN T，VISALA A.Navigation system for agricultural machines：Nonlinear Model Predictive path tracking[J].Computers and electronics in agriculture，2012，82：32-43.
[10] KARKEE M，STEWARD B L.Study of the open and closed loop characteristics of a tractor and a single axle towed implement system[J].Journal of terramechanics，2010，47（6）：379-393.
[11] 张万枝，白文静，吕钊钦，等.线性时变模型预测控制器提高农业车辆导航路径自动跟踪精度[J].农业工程学报，2017，33（13）：104-111.
[12] 张光磊，贾鹤鸣.基于反步法的农业采摘机器人移动路径跟踪控制研究[J].安徽农业科学，2014，42（23）：8023-8026.
[13] 严国军，贲能军，顾建华，等.基于MPC的无人驾驶拖拉机轨迹跟踪控制[J].重庆交通大学学报（自然科学版），2019，38（9）：1-6.
[14] 张广洁，严卫生，高剑.基于模型预测控制的欠驱动AUV直线路径跟踪[J].水下无人系统学报，2017，25（2）：82-88.
[15] 白国星，刘丽，孟宇，等.基于非线性模型预测控制的移动机器人实时路径跟踪[J].农业机械学报，2020，51（9）：47-52.
[16] 李升波，王建强，李克强.软约束线性模型预测控制系统的稳定性方法[J].清华大学学报（自然科学版），2010，50（11）：1848-1852.
关键词 智能拖拉机;模型预测控制;轨迹跟踪
中图分类号 TP 24;S 219 文献标识码 A 文章编号 0517-6611（2021）19-0191-04
doi：10.3969/j.issn.0517-6611.2021.19.050
开放科学（资源服务）标识码（OSID）：
Path Tracking of Intelligent Tractor Based on MPC
ZHAO Xiang-jun，ZHANG Pei-pei
（School of Engineering， Zhejiang Agriculture and Forestry University， Hangzhou，Zhejiang 311300）
Abstract In order to investigate the motion control with high nonlinearity for intelligent tractor， a lateral control method was proposed based on model predictive control （MPC）. Although tractor model has high nonlinearity， the model selected the linear time-varying error model of kinematic model as the predictive model to improve operation velocity. Objective function that selected the control increments， longitudinal velocity increment and front wheel angle increment， as state variables was established. Meanwhile， two control schemes with and without relaxation factor were proposed. With the design of the limiting constraints of control variable and control increment， the solution of objective function was changed into quadratic programming problem. Both of model predictive controllers were designed based on Matlab/Simulink. Results showed that both of path tracking controllers effectively track the reference path and the scheme with relaxation factor had smaller displacement errors and heading errors. Meanwhile front wheel angle were always restricted within the constraint range.
Key words Intelligent tractor;MPC;Path tracking
基金项目 浙江省公益技术研究项目（2017C32070）。
作者简介 赵相君（1976—），男，甘肃会宁人，实验师，从事农用车辆的运用与维护研究。*通信作者，讲师，博士，从事车辆系统动力学研究。
收稿日期 2021-02-23
拖拉机无人驾驶技术已成为农业装备智能化研究的热点之一。无人驾驶车辆的路径跟踪控制是无人驾驶车辆运动控制的核心，路径跟踪技术研究内容包括横向控制与纵向控制。横向控制主要是通过控制车辆转向实现对期望路径的追踪，旨在保证跟踪过程中航向角偏差和横向偏差趋近于0[1-3]，是拖拉机自主行驶的一项核心技术。
关于横向控制的路径跟踪模型主要有转向几何学、动力学和运动学模型[4-5]，随着控制技术的发展，众多学者对各类路径跟踪模型提出了相应的控制策略[5-12]。应用较多的有PID控制、模糊控制、滑膜控制、最优控制、LQR控制、模型预测控制（model predictive control，MPC）、非线性模型预测控制（nonlinear model predictive control，NMPC）等。这些控制器在路径跟踪时车辆航向角和横向位移的精度上都取得了满意的效果。
MPC可以考虑系统的多约束条件，在过去的20多年内被广泛应用于路径跟踪控制。严国军等[13]设计的自适应MPC控制器在确保无人驾驶拖拉机轨迹跟踪效果的前提下，保证了控制系统的实时性;张广洁等[14]对比了线性模型预测控制、线性时变模型预测和非线性模型预测控制3种算法对AUV的直线跟踪效果;白国星等[15]为削弱控制器对路径曲率突变和航向突变的响应能力，基于NMPC提出了减少控制步数或降低控制频率2种实时优化方案，但这些研究采用的控制目标函数都不加松弛因子。 笔者采用有松弛因子和无松弛因子2种控制目标函数的MPC算法研究智能拖拉机的路径跟踪控制问题，并通过Matlab/Simulink对比分析2种方案的控制效果。
1 运动学模型的建立
拖拉机的速度较低，一般为0.5～3.0 m/s，因此可作如下假设：只作平行于地面的平面运动;车辆速度较低，车轮没有侧滑;转向过程中车辆质心侧偏角保持不变。Karkee等[10]研究表明当牵引速度小于4.5 m/s，输入频率小于1 rad/s时，采用运动学模型设计的轨迹跟踪控制器具有可靠的控制性能，因此采用运动学模型作为跟踪控制模型，如图1所示。XOY为固定坐标系;（ x，y ）为车辆后轴中心的坐标; l 为轴距，单位m; φ 为车体的航向角（°）; δ 为前轮转角（°）; v 为车辆后轴中心速度（m/s）; P 为车辆的瞬时转动中心; R 为后轮转向半径（m）; B 1、B 2 分别为前后轮轮距（m）; η 为车辆后轴中心的轴迹。
根据图1模型，建立拖拉机运动学微分方程：
x=v cos φ
y=v sin φ
φ=v tan δl（1）
拖拉机可以被看作一个控制量u=[v，δ] T 、状态量X=[x，y，φ] T的控制系统，同时该系统具有明显的非线性。
2 MPC控制器设计
MPC具有系统模型多约束处理的优势，能够与规划控制、感知过程的传感器数据预处理算法很好结合，具有较强的鲁棒性和较好的控制效果。MPC包括模型线性化模块、约束条件设计、求解目标函数3个部分。
采用有松弛因子和无松弛因子2种MPC算法来实现对拖拉机的横向跟踪控制，如图2所示。MPC控制器根据期望轨迹[ x ref， y ref， φ ref]T、参考速度 v ref和参考前轮转角 δ ref，基于车辆运动学模型求出前轮转角 δ ，将求解的控制量输出给车辆模型，同时加入延时模块。MPC控制器的控制算法主要包括将拖拉机的非线性模型线性化和离散化处理，得到线性化的智能拖拉机偏差模型，作为控制器预测方程;将参考轨迹离散化，设计具有约束条件的目标函数。
首先，对拖拉机的非线性模型进行线性化和离散化处理，将线性化的智能拖拉机偏差模型作为控制器预测方程。为得到线性化的系统偏差模型，将公式（1）在參考轨迹点采用泰勒级数展开并忽略高级项，设参考轨迹状态量 X ref=[ x ref， y ref， φ ref]T和控制量 u ref=[ v ref， δ ref]T，系统偏差 X e=[e x，e y，e φ ]T，得到：
X e = X - X ref= AX e + Bu e （2）
式中， A =00- v refsin φ ref
00 v refcos φ ref
000，
B =cos φ ref0
sin φ ref0
tan δ ref l v ref l cos2 δ ref
为了能将该模型应用于MPC控制器的设计，对偏差模型公式（2）进行离散化：
X e（k+1） = A k，t X e（k） + B k，t u e（k） （3）
式中， A k，t =I+ TA，B k，t = TB ， T 为采样时间。 X e（k） 和 u e（k）为在k时刻系统的状态量偏差和控制量偏差。
设 ξ （k/t）=X e（k/t）u e（k-1/t），则系统可表达为新的状态空间模型：
ξ （k+1/t）=A k，t ξ （k/t）+B k，t Δ U（k/t）
η（k/t）=C k，t ξ （k/t）（4）
式中，A k，t=A k，t B k，t0 m×n I m，B k，t=B k，tI m，m为控制量维数，n为状态量维数，m=3，n=2。简化计算，作如下假设：
A k，t=A t，t，B k，t=B t，t，k=1，…，t+N-1
因此可以得到预测输出模型：
Y（t）=Ψ t ξ （t/t）+Θ Δ U（t）（5）
式中，Y（t）=
η （t+1/t）
η （t+2/t）
η （t+N p/t），
Ψ t=
C t，tA t，t
C t，tA2 t，t
C t，tCN p t，t，
Θ t=
C t，tB t，t000
C t，tA t，tB t，tC t，tB t，t00
C t，tAN c-1 t，tB t，tC t，tAN c-2 t，tB t，t … C t，tB t，t
C t，tAN p-1 t，tB t，tC t，tAN p-2 t，tB t，t … C t，t A N p-N c-1 t，tB t，t
，
Δ U（t）= Δ u（t/t）
Δ u（t+1/t）
Δ u（t+N c/t）。
控制过程中约束条件主要是控制量极限约束和控制增量约束，即：
u min （t+k）≤u（t+k）≤u max （t+k）
Δ u min （t+k）≤ Δ u（t+k）≤ Δ u max （t+k）
，k=0，1， … ，N c-1。（6）
为了能够保证系统快速平稳地跟踪期望路线，就需要对系统状态量的偏差和控制量进行优化，实际求解中系统的控制量是未知的，需要设定合适的优化目标求解控制时域内的控制序列。
无松弛因子的目标函数：
min J（k）= N pi=1‖ Δ η（k+i/t）‖2 Q+ N c-1i=1‖ Δ U（k+i/t）‖2 R（7）
加入松弛因子的目标函数[16]：
min J（k）= N pi=1‖ Δ η（k+i/t）‖2 Q+ N c-1i=1‖ Δ U（k+i/t）‖2 R+ρε2（8）
式中， Δ η（k+i/t）为实际输出与期望轨迹之差， Δ U（k+i/t）=U（k+i+1/t）-U（k+i/t），N p为预测时域，N c为控制时域，Q为误差奖惩项权重矩阵，R为控制量增量奖惩项权重矩阵，ρ为权重系数，ε为松弛因子。
目标函数（8）求解的变量是控制时域内的控制量U t，设U t=1 Nc u （k-1），1 NC是行数为N c的列向量，为克罗内克积，u（k-1）为上一时刻实际的控制量，且满足：
U min ≤ U t+M Δ U t≤U max （9）
式中，U min 、U max 分别为控制时域内控制量的最大值和最小值集合，则：
M =
100…0
110…0
111…0
0
111…1I m。
最终目标函数进一步转化为标准二次型形式并结合约束条件，结合公式（6）～（9），无人驾驶拖拉机追踪期望路径的优化问题可以表达如下：
（1）无松弛因子。
J（k）= N pi=1‖ Δ η（k+i/t）‖2 Q+ N c-1i=1‖ Δ U（k+i/t）‖2 R
s.t. Δ U min ≤ Δ U t≤ Δ U max
U min ≤U t+M Δ U t≤U max （10）
（2）有松弛因子。
J[ ξ （t），u（t-1）， Δ U（t）]=[ Δ U（t） T ，ε] T H t[ Δ U（t） T ，ε]+G t[ Δ U（t） T ，ε]
s.t. Δ U min ≤ Δ U t≤ Δ U max （11）
U min ≤U t+M Δ U t≤U max
式中，H t=Θ T tQΘ t+R00ρ，G t=[2e T tQΘ t 0]，e t=Ψ t ξ （t/t）-Y ref （t），e t為预测时域内的跟踪偏差，Y ref （t）=[η ref （t+1/t），…，η ref （t+N p/t）] T 。
每个控制周期内完成公式（10）、（11）的求解后，得到控制时域内的一系列控制输入增量：
Δ U* t=[ Δ u* t Δ u* t+1 … Δ u* t+N c-1] T
将该控制序列中第一个元素 Δ u* t作为实际的控制输入增量作用于系统，则u（t）=u（t-1）+ Δ u* t ，进入下一个控制周期后重复上述过程，如此循环就可实现对拖拉机的轨迹跟踪。
3 路径跟踪仿真试验分析
根据上述MPC控制器的设计，搭建仿真模块并在Matlab/Simulink 环境下进行拖拉机运动仿真验证，参考典型拖拉机地头掉头行驶路径，设置期望路径如图2所示，主要由直线和圆弧组成，圆弧的半径为10 m，期望纵向速度为1 m/s。
设置初始时刻为 t 0=0，末端时刻 t 1=70 s，求解算法采用龙格-库塔数值解法，预测步长 N p 为60，控制步长 N c 为30，权重系数 ρ 为10，松弛因子 ε 为10，采样时间 T 为0.1 s。
控制量约束设置为-0.2 m/s≤ v-v ref≤0.2 m/s，-0.02 m/s≤Δ v ≤0.02 m/s，-17°≤ δ ≤17°，-0.974°≤Δ δ ≤ 0.974°，Δ v 为每个控制周期的速度增量，Δ δ 为每个控制周期的前轮转角增量。
拖拉机轨迹跟踪初始位置 x=0，y=0，φ=0 ，仿真试验结果如图3～5所示。
横向跟踪仿真结果如图3、4所示，由于拖拉机给定的初始状态与轨迹参考值存在初始偏差，无人驾驶车辆在2种MPC控制方案的作用下均能快速调整航向角跟踪期望轨迹。松弛因子 ε =10的控制方案，横向位置跟踪偏差和航向角偏差逐步减少收敛到0，系统稳定性好。无松弛因子的控制方案，待系统稳定后，横向位置跟踪偏差保持在（-0.15，0.15）m，同时航向角偏差会有一定的波动，在（-2°，2°）之间。由此可见，加入松弛因子的MPC控制方案，系统跟踪轨迹更加贴合期望路径，给定路径的跟踪能力更强、稳定性更好。 2种MPC控制方案控制量前轮转角的变化如图5所示，明显松弛因子 ε =10的控制方案优于无松弛因子的方案，在路径曲率发生变化时均可快速调整，随时间平稳变化，未有突变，始终在给定约束条件范围内。
4 结语
针对非线性智能拖拉机横向跟踪控制问题，分析了目标函数有松弛因子和无松弛因子2种基于MPC的纵横向跟踪控制方法。无松弛因子的目标函数优点在于容易转化为标准二次型。参考典型地头掉头行驶路径，完成参考路径和参考速度规划。基于拖拉机运动学模型和MPC控制器提供的速度预测序列和前轮转角序列预测拖拉机姿态变化，求解模型预测控制目标函数获得纵向速度和前轮转角控制量，并在Matlab/Simulink仿真平台进行了仿真验证。
仿真结果表明，加入松弛因子后航向角可以快速调整跟踪期望轨迹，横向位置跟踪偏差、航向角偏差逐步收敛到0，跟踪平稳性更好。
参考文献
[1] 吴延霞，赵博，毛恩荣.农业车辆自动导航系统综述[J].农机化研究，
2009，31（3）：242-245，252.
[2] 吕安涛，毛恩荣，宋正河，等.一种拖拉机自动驾驶复合模糊控制方法[J].农业机械学报，2006，37（4）：17-20.
[3] 郭景華，李克强，罗禹贡.智能车辆运动控制研究综述[J].汽车安全与节能学报，2016，7（2）：151-159.
[4] SNIDER J M.Automatic Steering Methods for Autonomous Automobile Path Tracking[R].Pittsburgh：Carnegie Mellon University，Robotics Institute，2009.
[5] 郭应时，蒋拯民，白艳，等.无人驾驶汽车路径跟踪控制方法拟人程度研究[J].中国公路学报，2018，31（8）：189-196.
[6] ZHANG X，GEIMER M，NOACK P O，et al.A semi-autonomous tractor in an intelligent master-slave vehicle system[J].Intelligent service robotics，2010，3（4）：263-269.
[7] 张硕，刘进一，杜岳峰，等.基于速度自适应的拖拉机自动导航控制方法[J].农业工程学报，2017，33（23）：48-55.
[8] LENAIN R，THUILOT B，CARIOU C，et al.Model predictive control for vehicle guidance in presence of sliding：Application to farm vehicles path tracking[C]//Proceedings of the 2005 IEEE International Conference on Robotics and Automation.Barcelona， Spain：ICRA，2005：885-890.
[9] BACKMAN J，OKSANEN T，VISALA A.Navigation system for agricultural machines：Nonlinear Model Predictive path tracking[J].Computers and electronics in agriculture，2012，82：32-43.
[10] KARKEE M，STEWARD B L.Study of the open and closed loop characteristics of a tractor and a single axle towed implement system[J].Journal of terramechanics，2010，47（6）：379-393.
[11] 张万枝，白文静，吕钊钦，等.线性时变模型预测控制器提高农业车辆导航路径自动跟踪精度[J].农业工程学报，2017，33（13）：104-111.
[12] 张光磊，贾鹤鸣.基于反步法的农业采摘机器人移动路径跟踪控制研究[J].安徽农业科学，2014，42（23）：8023-8026.
[13] 严国军，贲能军，顾建华，等.基于MPC的无人驾驶拖拉机轨迹跟踪控制[J].重庆交通大学学报（自然科学版），2019，38（9）：1-6.
[14] 张广洁，严卫生，高剑.基于模型预测控制的欠驱动AUV直线路径跟踪[J].水下无人系统学报，2017，25（2）：82-88.
[15] 白国星，刘丽，孟宇，等.基于非线性模型预测控制的移动机器人实时路径跟踪[J].农业机械学报，2020，51（9）：47-52.
[16] 李升波，王建强，李克强.软约束线性模型预测控制系统的稳定性方法[J].清华大学学报（自然科学版），2010，50（11）：1848-1852.