欢迎您访问《智慧农业（中英文）》官方网站！ English
智慧农业

智慧农业 >

2024 , Vol. 6 >Issue 3: 69 - 81

DOI: https://doi.org/10.12133/j.smartag.SA202306013

专题--丘陵山区智慧农业技术与机械

基于MPC延时补偿器的农机多机器人编队行驶轨迹跟踪方法

  • 栾世杰 1 ,
  • 孙叶丰 2 ,
  • 贡亮 , 2 ,
  • 张凯 3
展开
  • 1. 上海出版印刷高等专科学校, 上海 200093, 中国
  • 2. 上海交通大学 机械与动力工程学院, 上海 200240, 中国
  • 3. 南京信息工程大学 自动化学院, 江苏 南京 210044, 中国
贡 亮，教授，博士生导师，研究方向为农业机器人。E-mail：

（栾世杰、孙叶丰并列第一作者）

栾世杰，研究方向为车辆载荷特性、结构耐久性快速评价。E-mail：

孙叶丰，研究方向为农业机器人感知与建图。E-mail：

收稿日期: 2023-06-14

  网络出版日期: 2024-05-31

基金资助

上海市科技兴农项目(沪农科推字〔2022〕第3-1号)

国家重点研发计划项目(2022YFD2001201)

收起

Trajectory Tracking Method of Agricultural Machinery Multi-Robot Formation Operation Based on MPC Delay Compensator

  • LUAN Shijie 1 ,
  • SUN Yefeng 2 ,
  • GONG Liang , 2 ,
  • ZHANG Kai 3
Expand
  • 1. Shanghai Publishing and Printing College, Shanghai 200093, China
  • 2. School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China
  • 3. School of Automation, Nanjing University of Information Science & Technology, Nanjing 210044, China
GONG Liang, E-mail:

LUAN Shijie, E-mail:

SUN Yefeng, E-mail:

Received date: 2023-06-14

  Online published: 2024-05-31

Supported by

Shanghai Science and Technology Agriculture Project(Shanghai Nongke Tuzi 〔2022〕 No. 3-1)

Key R&D Project of Ministry of Science and Technology(2022YFD2001201)

Copyright

copyright©2024 by the authors
Fold

摘要

[目的/意义] 农机装备尺寸大、行驶慢等特点，在路面归库作业过程中容易造成严重道路拥堵。因此，在多机协同过程中，编队行驶被认为是未来道路上行驶的主要方式。然而，目前农机自动驾驶技术停留在单机阶段，多农机之间的协同仍是制约中国农业规模化自主生产的主要瓶颈。为解决多车编队协同控制中通信延时的问题及其补偿策略，本研究基于一种模型预测控制器（Model Predictive Control, MPC）延时补偿器的农机多机编队行驶的轨迹跟踪方法。 [方法] 以车联网技术为基础，聚焦多农机编队协同控制领域，针对控制器局域网总线（Controller Area Network, CAN）通信存在的延时问题而产生的横向控制精度差，基于线性二次调节器（Linear Quadratic Regulator, LQR）与MPC算法，设计了一种带有延时补偿的模型预测控制器，用于对通信延时进行补偿控制。最后对所提算法利用Carsim和Simulink软件进行联合仿真。 [结果和讨论] Carsim与MATLAB/Simulink可以有效兼容，实现软件与外部求解器的联合仿真。当延时步长d=5时，应用延时补偿，MPC反应速度更快，表现更为平滑；速度误差曲线响应更快，且能够逐渐稳定至零误差，没有出现振荡现象；1号车在较短时间内有效地变更车道，与头车保持在同一车道上。在更长的延时步长d=10情况下，未应用延时补偿的控制器表现出更显著的性能下降。即使在较高的延时条件下，应用延时补偿的MPC速度误差和纵向加速度仍然能够快速响应并逐渐稳定至零误差，避免了振荡现象。1号车的轨迹表明，延时补偿机制效果在极端延时条件下有所下降。 [结论] 本研究所设计的编队算法能够使得多车完成多车变道形成队列并保持一定距离和一定速度。通信延时补偿控制算法使得带有加入延时的车辆能较好完成编队任务，实现稳定的横纵向控制，验证了本研究带有延时补偿的模型预测控制器的可行性。

关键词： 车联网技术; 智能农机; 多机协同; 编队行驶; 轨迹跟踪

本文引用格式

栾世杰 , 孙叶丰 , 贡亮 , 张凯 . 基于MPC延时补偿器的农机多机器人编队行驶轨迹跟踪方法[J]. 智慧农业, 2024 , 6(3) : 69 -81 . DOI: 10.12133/j.smartag.SA202306013

Abstract

[Objective] The technology of multi-machine convoy driving has emerged as a focal point in the field of agricultural mechanization. By organizing multiple agricultural machinery units into convoys, unified control and collaborative operations can be achieved. This not only enhances operational efficiency and reduces costs, but also minimizes human labor input, thereby maximizing the operational potential of agricultural machinery. In order to solve the problem of communication delay in cooperative control of multi-vehicle formation and its compensation strategy, the trajectory control method of multi-vehicle formation was proposed based on model predictive control (MPC) delay compensator. [Methods] The multi-vehicle formation cooperative control strategy was designed, which introduced the four-vehicle formation cooperative scenario in three lanes, and then introduced the design of the multi-vehicle formation cooperative control architecture, which was respectively enough to establish the kinematics and dynamics model and equations of the agricultural machine model, and laied down a sturdy foundation for solving the formation following problem later. The testing and optimization of automatic driving algorithms based on real vehicles need to invest too much time and economic costs, and were subject to the difficulties of laws and regulations, scene reproduction and safety, etc. Simulation platform testing could effectively solve the above question. For the agricultural automatic driving multi-machine formation scenarios, the joint simulation platform Carsim and Simulink were used to simulate and validate the formation driving control of agricultural machines. Based on the single-machine dynamics model of the agricultural machine, a delay compensation controller based on MPC was designed. Feedback correction first detected the actual output of the object and then corrected the model-based predicted output with the actual output and performed a new optimization. Based on the above model, the nonlinear system of kinematics and dynamics was linearized and discretized in order to ensure the real-time solution. The objective function was designed so that the agricultural machine tracks on the desired trajectory as much as possible. And because the operation range of the actuator was limited, the control increment and control volume were designed with corresponding constraints. Finally, the control increment constraints were solved based on the front wheel angle constraints, front wheel angle increments, and control volume constraints of the agricultural machine. [Results and Discussions] Carsim and MATLAB/Simulink could be effectively compatible, enabling joint simulation of software with external solvers. When the delay step size d=5 was applied with delay compensation, the MPC response was faster and smoother; the speed error curve responded faster and gradually stabilized to zero error without oscillations. Vehicle 1 effectively changed lanes in a short time and maintains the same lane as the lead vehicle. In the case of a longer delay step size d =10, controllers without delay compensation showed more significant performance degradation. Even under higher delay conditions, MPC with delay compensation applied could still quickly respond with speed error and longitudinal acceleration gradually stabilizing to zero error, avoiding oscillations. The trajectory of Vehicle 1 indicated that the effectiveness of the delay compensation mechanism decreased under extreme delay conditions. The simulation results validated the effectiveness of the proposed formation control algorithm, ensuring that multiple vehicles could successfully change lanes to form queues while maintaining specific distances and speeds. Furthermore, the communication delay compensation control algorithm enables vehicles with added delay to effectively complete formation tasks, achieving stable longitudinal and lateral control. This confirmed the feasibility of the model predictive controller with delay compensation proposed. [Conclusions] At present, most of the multi-machine formation coordination is based on simulation platform for verification, which has the advantages of safety, economy, speed and other aspects, however, there's still a certain gap between the idealized model in the simulation platform and the real machine experiment. Therefore, multi-machine formation operation of agricultural equipment still needs to be tested on real machines under sound laws and regulations.

Key words： vehicle to everything; intelligent agricultural machinery; multi-machine collaboration; traveling in formation; trajectory tracking

0 引 言

随着人口老龄化、社会城镇化，以及青壮年就业观念的转变［1, 2］，促生了农业机械装备发展趋向于智能化、自动化和无人化。面对农田大规模，单机作业时间长、效率慢，农机自动驾驶协同技术成为了解决农业规模化、集约化和专业化发展的核心之一［3, 4］。农机装备尺寸大、行驶慢等特点，在路面归库作业过程中容易造成严重道路拥堵。因此，在多机协同过程中，编队行驶被认为是未来道路上行驶的主要方式［5］。这一技术允许分散在多条车道上的单机通过先进的车联网技术（Vehicle to Everything, V2X）通信技术进行协同控制，形成柔性车队［6］，从而能够节约车道资源、缩小农机间距、缓解交通拥堵、降低驾驶员操纵失误而引发的交通事故。
在多车编队控制研究中，Gu等［7］针对水面车辆编队形式的问题，提出了一种针对一组前驱动自主水面车辆的路径引导时变编队制导律设计，同时避免车辆之间的碰撞。Guan等［8］提出了基于物联网平台的机器人编队控制，基于物联网平台构建了机器人协作运动控制实时传输系统，采集并反馈多台机器人的运动轨迹。通过粒子群优化深度学习算法，更准确地识别、预测和指导机器人的下一步动作。Ding等［9］克服了单一视觉传感器系统的局限，使用深度相机作为信息感知源。在负梯度控制律的基础上，将约束条件融入机器人控制器的设计中。经过图像采集和图像处理后，外部环境信息和障碍物位置信号输入到领导者的控制系统中。Pan等［10］研究了3D受限空间中多架无人机（Multi-UAV）的路径规划和编队控制问题。考虑人工势函数的局部最小值，针对多机系统提出了一种基于改进人工势函数的有效路径规划方法。Obradović等［11］提出了一种基于强化学习算法的分散领导者-跟随者编队控制的分散算法，通过分解为两个更简单的任务，能够使用简单的基于学习的方法在接近阶段实现机器人的安全导航以及精确一致的位置保持。
目前，现有的多机编队协同控制多集中运用运动学模型［12］，旨在简化模型起到快速计算的作用。然而农机装备结构复杂，单纯利用运动学模型无法实现高精度编队作业。此外，理想化的控制方法没有考虑CAN通信延时的影响。尽管上述研究为多车编队协同控制提供了理论基础和技术框架，但它们大多忽略了通信延时这一关键技术挑战。由于CAN（Controller Area Network）通信系统在农业机械领域内的广泛应用，特别是在多机编队协同控制中，通信延时问题对作业的实时性和准确性提出了更高的要求。在横向控制精度上，通信延时可能导致车辆无法精确地保持在预定路径上，从而影响整体的作业精度和安全性［13, 14］。这种横向控制差不仅会增加作业过程中的重复性，还可能导致在复杂的农业环境中车辆出现偏离轨道的情况，从而提高了作业风险，降低了作业效率［15, 16］。
本研究提出采用模型预测控制（Model Predictive Control, MPC）技术优化多车编队协同控制，利用其强大的数据处理和实时反馈能力来补偿通信延时带来的影响。MPC通过预测未来的系统状态和控制行为，减少通信延时对编队控制精确性的影响，确保车队动作的同步性和准确性。
本研究基于车辆动力学模型和MPC技术，探讨多车编队协同控制中通信延时的问题及其补偿策略，并通过Carsim和Simulink的融合仿真来验证所提方法的有效性，以实现提升交通安全和道路通行效率、节省燃油消耗等目的。

1 农机多机编队协同的控制策略设计

1.1　多机编队协同场景描述

本研究所考虑的场景是4辆农机装备行驶在3条车道上，基于多机协同使4辆初速度不同的农机组合成同一编队，使其四者之间按某一速度均匀行驶，最终以达到编队内农机距离、速度保持一致，其初始状态机最终状态分别如图1和图2所示。在节约道路资源的同时，起到提升道路通行率、减少能源消耗［17］。在道路坐标系中，4辆农机装备的初始质心坐标及速度如表1所示。
图1 农机编队协同行驶初始状态示意图

Fig. 1 Initial state schematic diagram of agricultural machinery convoy cooperative driving

图2 农机编队协同行驶最终状态示意图

Fig. 2 Final state schematic diagram of agricultural machinery convoy cooperative driving

表1 农机编队初始状态参数

Table 1 Initial state parameters of agricultural machinery in formation form

参数符号 物理意义 数值
x 00 头车初始位置 （50，6）
v 00 头车初速度/（ m / s） 12
x 10 1号车初始位置 （40，10）
v 10 1号车初速度/（ m / s） 14
x 20 2号车初始位置 （20，2）
v 20 2号车初速度/（ m / s） 16
x 30 3号车初始位置 （1，6）
v 30 3号车初速度/（ m / s） 15

1.2　多机编队协同架构

1.2.1　车联网技术

V2X［18, 19］是一种全方位通信技术，集成了车辆与车辆（Vehicle to Vehicle, V2V）［20, 21］、车辆与设施（Vehicle to Infrastructure, V2I）［22, 23］、车辆与网络（Vehicle to Network, V2N）［23, 24］，以及车辆与行人（Vehicle to Pedestrian, V2P）［25, 26］4种通信方式，用于车辆与周边网络信息环境的通信，其目标是通过融合技术构建交通环境中各个实体之间的信息交互网络，实现车辆与其他交通参与者或基础设施之间的数据交互和共享［27］，同时建立实时可靠的通信网络，使车辆、道路和云结构的数据能够有效相互利用。

1.2.2　架构设计

由于单机感知技术受到环境、光照等影响存在误差，为增强编队中多机协同的安全性，编队车辆需要将车身传感器获取的信息基于V2X通信传输至路测单元（Road Side Unit, RSU），并通过路测单元将编队中各个车辆位置、速度、加速度等信息传输下发［28］。
如图3所示，本研究采用基于V2X通信技术的系统来集中处理车队中各车辆的传感器数据。这些数据包括每辆车的位置、速度和加速度等信息。这些信息首先被传送到RSU，在那里进行综合处理。处理后的数据，包括各车辆的期望速度和位姿，被发送回车队中的每辆车。随后，每辆车通过CAN总线将这些信息传送至其控制器和执行器［29］，从而实现对预定轨迹和位姿的精确跟踪控制。
图3 基于车联网技术的多车协同控制框架

Fig. 3 Multi-vehicle cooperative control framework based on vehicle to everything

2 农机运动学和动力学模型建立

2.1　农机运动学模型

本研究基于二自由度的车辆运动学模型构建了农机模型。在此模型中，忽略了农机在转弯过程中前轮左右两侧轮胎的侧偏角差值［30, 31］。在行驶时，前轮的转向角度保持在一个预定的范围内，而车轮在转弯时产生的侧向力与侧偏角成正比关系。基于这些假设，将所研究的农机模型简化为一个自行车模型，如图4所示。
图4 农机模型简化的自行车模型

注： 在道路坐标系下， l f 、 l r分别为前后轴距； φ为航向角； β为汽车质心的侧偏角； β f 、 β r分别为前后轮侧偏角，表示轮子速度和轮子朝向的夹角； δ为前轮转角； ω r表示横摆角速度； y ˙为质心侧向速度； x ˙为质心纵向速度； F x 1 、 F x 2分别为前后轮在X方向受到的力； F y f 、 F y r分别为前轮和后轮侧向力。

Fig. 4 Simplified model of agricultural machinery to bicycle model

在道路坐标系中，当 x , y为前轴中心坐标，前轴速度为 v f时，根据运动学公式得到车辆运动学方程，矩阵形式如公式（1）所示。
x ˙ y ˙ φ ˙ = v f c o s φ v f s i n φ ω
由牛顿定律得到车辆模型微分方程公式（2）。
m y ¨ + x ˙ ω r = F y f + F y r I z ω ˙ r = l f F y f - l r F y r
式中： m为车的质量， k g； I z为车辆转动惯量， k g ∙ m 2； F y f和 F y r分别为前后轮在y方向受到的力， N。
在车辆稳态转向下本研究基于自行车模型来推导前轮前馈转角 δ f f，以固定转向角和恒定速度行驶，下面用几何关系来推导车辆稳态转向方程，车辆稳态转向如图5所示，其中 c . g点为车辆的质心； l = l f + l r，为轮距。
图5 农机稳态转向模型

Fig. 5 Steady-state steering model of agricultural machinery model

车辆转弯半径 R > > l， α角很小，则 α = 1 R，可以得到 α = δ - β f + β r，进而可以推出转角 δ，如公式（3）所示。
δ = l R + β f - β r
由上述条件可知，前轮和后轮的侧偏角很小，而作用于轮子的侧向力正比于对应的侧偏角度，如公式（4）所示。
F y f = - 2 c f β f F y r = - 2 c r β r
根据阿克曼转向系几何关系，如图6所示，该车辆运动学模型经过整理如公式（5）所示。
图6 阿克曼转向系模型

Fig. 6 Ackermann steering system model

x ˙ y ˙ φ ˙ = c o s φ s i n φ t a n δ L v f

2.2　农机动力学模型

基于物体运动与力学之间的关系，为实现高精度轨迹跟踪以及鲁棒性控制稳定性，建立农机动力学模型［32, 33］。以运动学模型为基础，增加农机运动过程中侧向力等约束，建立如图7所示的农机动力学模型。
图7 基于自行车模型的农机单车动力学模型

Fig. 7 Agricultural machinery dynamics model built in this research based on bicycle model

假定如下条件。
1）车辆的重量不变，始终在重心上；
2）车辆行驶在平坦的直道上，且没有垂向运动；
3）忽略车辆受到的空气阻力。
在道路坐标系中， F x f和 F x r分别为前后轮在x方向受到的力 , N； F y f和 F y r分别为前后轮在y方向受到的力， N； x ˙为汽车在纵向上的速度， m / s； x ¨为汽车在纵向上的加速度， m / s 2； y ˙为汽车在横向上的速度， m / s； l f和 l r分别为汽车的前后轴距， m，其和为 l ⅈ； φ为汽车在全局坐标系下的航向角， r a d； φ ˙为汽车的横摆角速度， r a d / s； m为车重， k g； I z为转动惯量， k g ∙ m 2。
如图7所示，第i辆车的动力学特性可以写成如下形式，其中 i ∈ A = 1,2 , 3,4，表达式如公式（6）所示。
m x ¨ i = m y ˙ i φ ˙ i + F x f i + F x r i m y ¨ i = - m x ˙ i φ ˙ i + F y f i + F y r i I z φ ¨ i = l f F y f i - l r F y r i
车辆坐标系与道路坐标系转换形式如公式（7）所示。
X ˙ i = x ˙ i - y ˙ i φ i Y ˙ i = x ˙ i φ i + y ˙ i
根据魔术公式Pacejka轮胎模型，轮胎的受力在侧偏角和滑动率比较小时能够用线性函数近似表达，当横向加速度 a y ≤ 0.4   g、轮胎侧偏角 α ≤ 5 °时对轮胎的拟合精度较高。其侧向力的表达式如公式（8）所示。
F y f i = c f β f F y r i = c r β r
式中： c f和 c r分别为前后轮的侧偏刚度， N / r a d； β f和 β r分别为前后轮的侧偏角， r a d。
最后，车辆的动力学可以表示成公式（9）。
ξ ˙ i t = f ξ i t ,   μ i t
状态向量如公式（10）所示。
ξ i = y ˙ i x ˙ i φ i φ ˙ i Y i X i T
式中： y ˙ ⅈ为道路坐标系下横向速度 , m / s； x ˙ ⅈ为道路坐标系下纵向速度， m / s； φ ⅈ为航向角， r a d； φ ˙ ⅈ为横摆角速度， r a d / s； Y ⅈ为横向位置， m； X ⅈ为纵向位置， m。

3 车辆编队控制器设计

3.1　线性调节跟踪器设计

本节聚焦于归库过程中车辆在车道变换时的跟踪控制，基于线性二次型调节器（Linear Quadratic Regulator, LQR），建立跟踪误差的状态空间模型。以输入量和系统状态量为目标函数，基于Q、R两个参数，设计目标函数最小时的最优状态反馈矩阵 K。 其中，定义矩阵 A 和 B，如公式（11）和公式（12）所示。
A = 0 1 0 0 0 - c f + c r m v x - v e h c f + c r m l r c r - l f c f m v x - v e h 0 0 0 1 0 l r c r - l f c f I z v x - v e h - l r c r + l f c f I z - l r 2 c r - l f 2 c f I z v x - v e h
B = 0 c f m 0 l f c f I z
式中： c f 、 c r分别为前轮和后轮车辆侧偏刚度， N / r a d； l f 、 l r分别为前后轮轴距， m； I z为车子绕Z轴旋转的转动惯量， k g ∙ m 2。则最优状态反馈矩阵 K 如公式（13）所示。
K = e ˙ y _ v e h e ¨ y _ v e h θ ˙ e θ ¨ e = A e y - v e h e ˙ y - v e h θ e θ ˙ e + B δ + 0 l r c r - l f c f m v x - v e h - v x - v e h 0 - l r 2 c r - l f 2 c f I z v x - v e h φ ˙
目标函数如公式（14）所示。
J = ∑ k = 0 ∞ [ x T ( k ) Q x ( k ) + δ T ( k ) R δ ( k ) ]
当目标函数最小得到最优的状态反馈矩阵如公式（15）所示。
K = ( R + B 1 T P B 1 ) - 1 B 1 T P A
P为黎卡提方程的解或代价函数的解，用以衡量系统性能和控制代价，如公式（16）所示。
P = A T P A - A T P B 1 ( R + B 1 T P B 1 ) - 1 B 1 T P A + Q

3.2　延时补偿控制器设计

模型预测控制（Model Predictive Control， MPC）其包含自适应控制、内模控制等多种方法，其具有变量交互、处理约束等优势使其工业中占有率在逐步提高。随着机械智能化发展，MPC逐渐广泛应用于无人机和机器人等控制领域。1986年，Kuntze等［34］提出了以内部模型、轨迹参考和控制算法为核心的预测控制算法基本原理。至今，模型预测控制的主要3个部分已经发展成为模型预测、滚动优化和校正反馈。
如图8所示，MPC控制器在整个过程中始终还有参考轨迹。通过融合当前值与预测模型对预测时域内 k , k + N P的系统输出进行预测。基于对目标函数的求解，并通过约束优化可得到控制时域内 k , k + N c的控制序列。受控对象的实际控制量为k时刻，对 k + 1时刻不断进行滚动优化以实现受控对象的鲁棒性控制。
图8 MPC控制器原理

Fig. 8 Principle of MPC controller

3.2.1　线性化与离散化

农机动力学模型和运动学模型均为非线性系统。为了实现高精度实时控制，需对模型进行线性变化。对其模型在 t时刻动力学方程进行线性化后忽略 ξ i t ,   μ i t处泰勒展开式高阶项，可得公式（17）。
ξ ˙ ( t ) = f [ ξ 0 ( t ) , u 0 ( t ) ]               + ∂ f ∂ ξ ξ 0 ( t ) , u 0 ( t - 1 ) ξ ( t ) - ξ 0 ( t )               + ∂ f ∂ u ξ 0 ( t ) , u 0 ( t - 1 ) u ( t ) - u 0 ( t - 1 )
对其进行线性化、离散化，如公式（18）所示。
ξ [ i ] ( k + 1 ) = ξ ˙ [ i ] ( k + 1 )                            + A [ i ] ξ [ i ] ( k ) - ξ ^ [ i ] ( k )                            + B [ i ] u [ i ] ( k ) - u ^ [ k ] ( k )
整理得公式（19）。
ξ [ i ] ( k + 1 ) = A [ i ] ξ [ i ] ( k ) + B [ i ] u [ i ] ( k ) + d [ i ] ( k )

3.2.2　预测模型

基于自动驾驶中CAN总线与执行器存在的通信延时，设计了含有认为延时补偿的MPC控制器。以控制量增量作为输入可得新状态方程，如公式（20）所示。
ξ ˜ [ i ] ( k + 1 ) = A ˜ [ i ] ( k ) ξ ˜ [ i ] ( k ) + B ˜ [ i ] ( k ) ⊗ U [ i ] ( k ) + d ˜ [ i ] ( k ) y ( k ) = C ˜ [ i ] ( k ) ξ ˜ [ i ] ( k )
以预测时域内的预测输出与轨迹规划参考规划误差作为目标函数，则农机状态向量如公式（21）所示。
ξ [ i ] = [ y ˙ [ i ] , x ˙ [ i ] , φ ˙ [ i ] , Y [ i ] , X [ i ] ]
整理得公式（22）。
ξ ˜ [ i ] ( k + 1 | k ) = A ˜ [ i ] ( k ) ξ ˜ [ i ] ( k ) + B ˜ [ i ] ( k ) Δ U [ i ] ( k ) + d ˜ [ i ] ( k ) y [ i ] ( k ) = C ˜ [ i ] ( k ) ξ ˜ [ i ] ( k + 1 | k )
式中： N c表示控制时域； N p表示预测时域， N c ≤ N p，其中， k为系统预测模型在 k时刻进行预测； ξ ˜ ⅈ k + 1 k为系统在 k时刻预测 k + 1时刻的增广矩阵状态。
A ˜ [ i ] ( k )为子系统 i在时刻 k的状态转移矩阵。 ξ ˜ [ i ] ( k )为子系统 i在时刻 k的状态向量； B ˜ [ i ] ( k )为子系统 i在时刻 k的控制输入矩阵； Δ U [ i ] ( k )为子系统 i在时刻 k的控制输入增量； d ˜ [ i ] ( k )为子系统 i在时刻 k的干扰或偏置项； y [ i ] ( k )为子系统 i在时刻 k的输出向量； C ˜ [ i ] ( k )为子系统 i在时刻 k的输出矩阵。

3.2.3　目标函数

为了实现农机的鲁棒性轨迹跟踪控制，通过对状态量、状态增量和参考偏差进行优化后，可得到鲁棒性轨迹跟踪控制器目标函数，如公式（23）所示。
J ( k ) = ∏ j = 1 N x ˜ T ( k + j | k ) Q x ˜ ( k + j | k ) + u ˜ T ( k + j - 1 R u ˜ ) ( k + j - 1
式中： J ( k )用于评估在时刻 k的控制策略， j未来预测的时间步长索引； Q为状态量权重矩阵； R为控制量权重矩阵。

3.2.4　约束函数

由于车辆自身存在操作量的物理约束，因此设计如公式（24）的控制量与控制增量的相应约束。
U m i n i k + t ≤ U i k + t ≤ U m a x i k + t k = 0,1 , 2 , ⋅ ⋅ ⋅ , N c - 1
式中， U m i n i k + t为时刻 k + t系统的控制输入的最小约束值； U m a x i k + t为时刻 k + t系统的控制输入的最大约束值； U i k + t为时刻 k + t系统的控制输入。
其中，增益的约束如公式（25）所示。
Δ U m i n i k + t ≤ Δ U i k + t ≤ Δ U m a x i k + t k = 0,1 , 2 , ⋅ ⋅ ⋅ , N c - 1
式中， Δ U m i n i k + t为时刻 k + t系统的控制输入增量的最小约束值； Δ U m a x i k + t为时刻 k + t系统的控制输入增量的最大约束值； Δ U i k + t为时刻 k + t系统的控制输入增量，即 Δ U i k + t = U i k + t - U i k + t - 1。

4 基于Carsim和Simulink的联合仿真实验

4.1　联合仿真平台搭建

Carsim是由车辆动力学仿真软件开发公司MSC开发的专业软件，能够对SUV、轻型货车和私家车等车辆的操纵稳定性、制动性和燃油经济性等进行分析，可以仿真路面和空气阻力等外在要素对汽车行驶的影响。Carsim具有界面设置友好，方便使用的优势。
基于实车的自动驾驶算法测试与优化需投入过多时间、经济成本，且受到法律法规、场景复现以及安全性等多方面困难，仿真平台测试可以有效解决所有痛点。表2展示了在使用Carsim仿真过程中的车辆动力学相关参数设置。
表2 编队行驶农机单机动力学相关参数

Table 2 Relevant parameters of single machine dynamics for formation running agricultural machinery

参数符号 物理意义 数值
C c f 前轮侧偏刚度 61 135 N/rad
C c r 后轮侧偏刚度 61 135 N/rad
m 整车质量 1 465 kg
l f 前轴距 1.2 m
l r 后轴距 1.6 m
Carsim与MATLAB/Simulink可以有效兼容，实现软件与外部求解器的联合仿真。多车编队协作结构流程图如图9所示。
图9 多车编队协作结构流程图

Fig. 9 Flowchart of multi-vehicle convoy collaboration structure

Simulink中将传感器监测到的速度、位置、偏向角进行反馈，用以调节后车的节气门开度、油门、制动器、加速度、转向角等执行器参数。基于轨迹跟踪算法输出控制信号作为Carsim车辆模型输入信号，控制系统对后车的防撞、变道和跟随进行控制，实现鲁棒性安全跟随控制。

4.2　控制器参数设置

4.2.1　速度变换器

位置变换器是车辆操控系统的输入环节，其主要作用是把操纵杆位置变换为相应速度。模型如公式（26）所示。
v = 30 x + 50 , x ∈ [ 0,1 ]
式中：x为操纵杆位置；v为与其匹配的速度， m / s。

4.2.2　离散行驶控制器

速度控制器是车辆操控系统最重要的机构，它的作用是根据车辆速度与设置速度之差控制发动机输出牵引力。传统的PID控制器的数学表达式可以写成如下形式。
积分环节如公式（27）所示。
x ( t ) = x ( t - 1 ) + u ( t )
微分环节如公式（28）所示。
d ( t ) = u ( t ) - u ( t - 1 )
系统输出如公式（29）所示。
y ( t ) = K P u ( t ) + K i x ( t ) + K d d ( t )
式中： u ( t )为系统输入； y ( t )为系统输出； x ( t )为系统中车辆状态。PID控制器相应的控制参数分别为 K p（比例）、 K i（积分）和 K d（微分）。

4.2.3　车辆动力机构

车辆的发动机提供行驶的动力，其在车辆操控系统的控制下输出相应的牵引力来改变车辆的速度，使得车辆的速度达到设定值，其关系表达如公式（30）所示。
F = m v ˙ + b v
式中：v为车辆速度，m/s；F为车辆的牵引力，N；m为车重，1 465 kg；设置阻力因子b为20。

4.2.4　单车控制器参数设置

车辆的控制单元和底层的执行器之间的通信是通过CAN总线的传递，而信息的传递会产生延时。针对单车执行单元到底层执行器之间通信恒定时不变延时控制，而对时变延时不探讨。当最大延时为 τ i n在采样时间T时，延时步长在离散系统中的表示如公式（31）所示。
d = τ i n T
图10展示了带有控制量输入固定延时的系统。本节将最大延时的离散线性系统描述为公式（32），以表示单一农机的运动状态。
x k + 1 = A x k + B k u k - d
图10 控制量输入固定延时系统结构图

Fig. 10 Fixed delay system for control input

从车辆CAN总线到执行器整个过程中都伴随着延时，过大的输入量延时可能会使得车辆的稳定性得不到保障，可能会产生严重的后果。
轨迹跟踪控制器的预测时域为 N p = 40，控制时域为 N c = 20，采样周期为 T = 0.01   s。要对 Q （状态量）和 R （控制量）权重矩阵和松弛因子设置。
Q = 10 5 0 0 0 10 8 0 0 0 10 3 , R = 10 10 0 0 10 7 , ρ = 1000
式中： Q 中参考量和预测量误差分别为 Y e r r o r [ i ] , φ e r r o r [ i ] , v e r r o r [ i ]； R 中纵向加速度和转角分别对应为 x ¨ [ i ] , δ [ i ]。
车辆前轮转角约束为：
- 25 ° ≤ δ ≤ 25 °
式中：车辆方向盘右转为负；方向盘左转为正。
车辆前轮转角增量为：
- 0.48 ° ≤ Δ δ ≤ 0.48 °
车辆控制量约束条件为：
- 0.3 - 25 ≤ u ≤ 0.3 25
式中：车辆速度和期望速度差值为 ± 0.3   m / s；车辆前轮转角为 ± 25 °。
车辆控制增量约束条件为：
- 0.05 - 0.48 ≤ Δ u ≤ 0.05 0.48
式中：控制周期速度增量为 ± 0.05   m / s；车辆前轮转角增量为 ± 0.48 °。

5 基于MPC延时补偿器轨迹跟踪仿真

本节专注于探讨车载控制器到执行器之间的通信延时对车辆控制系统性能的影响。对比了应用和未应用延时补偿机制的MPC。

5.1　延时步长d=5

通过人为设定的最大延时 τ i n = 0.05   s，计算了相应的延时步长 d，其计算方式如公式（33）所示。
d = τ i n T = 5
式中：T为延时周期。
当d=5时，实验结果如图11所示，应用延时补偿的MPC反应速度更快，表现更为平滑；未应用延时补偿的MPC速度表现出显著的超调现象，反应速度缓慢，并且出现了振荡不收敛的现象。
图11 延时步长d=5时农机采用与未采用延时补偿控制器情况

Fig. 11 Graph of velocity comparison results between with and without MPC model when d=5

进一步地，在图12中展示了速度误差和纵向加速度的比较。从速度误差方面来看，应用了延时补偿的MPC响应更快，且能够逐渐稳定至零误差，没有出现振荡现象；而未应用延时补偿的MPC响应缓慢，出现了振荡不收敛的问题，导致车辆容易出现急加速或急减速的情况。
图12 延时步长d=5时，农机采用与未采用延时补偿控制器情况下的速度误差及纵向加速度对比

Fig. 12 Velocity error and longitudinal acceleration comparison between with and without MPC model when d=5

还研究了1号车在延时步长 d = 5时的轨迹表现，如图13所示。未应用延时补偿时，1号车的响应速度较慢，跟踪轨迹出现了明显的振荡。当应用延时补偿时，结果如图14所示。1号车能够在较短时间内有效地变更车道，与头车保持在同一车道上，展示了延时补偿在改善车辆跟踪性能方面的有效性。
图13 1号车延时步长d =5四车轨迹曲线

注：绿色、黑色、红色、蓝色曲线分别为头车、1号车、2号车、3号车在d=5时轨迹曲线。

Fig. 13 Trajectory curves of four vehicles with delay step length d=5 for Vehicle 1

图14 延时步长d=10时农机采用与未采用延时补偿控制器情况下速度对比结果图

Fig. 14 Velocity comparison results graph between with and without MPC model when d=10

5.2　延时步长d=10

本节探讨更长延时步长对车载控制系统性能的影响。设定的延时值加大至 τ i n = 0.10   s，从而在车载控制器到执行器的通信过程中引入了更长的延时。根据此设定，计算得到新的延时步长 d，如公式（34）所示。
d = τ i n T = 10
实验结果显示，在更长的延时步长 d = 10情况下，未应用延时补偿的控制器表现出更显著的性能下降。具体来说，超调量增大，响应速度进一步减慢，且存在更严重的振荡不收敛现象，如图14所示。
此外，图15展示了延时步长为d=10时的速度误差和纵向加速度对比结果。结果表明，即使在较高的延时条件下，应用延时补偿的MPC仍然能够快速响应并逐渐稳定至零误差，避免了振荡现象，显示其在保持系统稳定性和提升性能方面的有效性。然而，未应用延时补偿的控制器在此条件下的性能显著下降，反映在响应速度缓慢，存在不收敛的振荡，导致车辆容易发生急加速或急减速。
图15 延时步长d =10时农机采用与未采用延时补偿控制器情况下的速度误差及纵向加速度对比

Fig. 15 Comparison of velocity error and longitudinal acceleration between with and without MPC model when d =10

对1号车在延时步长d=10时的轨迹进行了观察，如图16所示。未应用延时补偿时，1号车的响应速度明显变慢，且振荡幅度大于延时步长d=5的情况。相比之下，应用延时补偿，1号车虽然需要更长时间来调整车道以与头车保持在同一车道，但仍能实现此目标。这一结果表明，尽管延时补偿机制在更高的延时步长下仍有效，但其效果在极端延时条件下有所下降。这需要在设计控制系统时予以特别考虑。
图16 1号车延时步长d = 10的4车轨迹曲线

Fig. 16 Trajectory curves of four vehicles with delay step length d = 10 for Vehicle 1

针对车辆编队中存在的车载控制器到执行器之间的CAN通信延时问题，成功设计并实施了一种带有延时补偿机制的MPC。通过在不同设定延时场景下对比应用延时补偿和未应用延时补偿的MPC发现，带有延时补偿的MPC能够有效地弥补由于通信延时导致的控制性能下降。相较于未加入延时补偿的普通MPC，后者在面对人为加入的通信延时，表现出了振荡幅值递增、响应速度变慢，并越来越倾向于发散的问题。特别是在轨迹跟踪控制过程中，随着延时步长的增加，普通MPC的性能逐步恶化，表现为更大的振荡幅度和更慢的响应时间。这导致了横向控制上的显著不稳定，表现为车辆在行驶过程中频繁地左右摇摆。在纵向控制方面，普通MPC导致了车辆的急加速和急减速行为，增加了与其他车辆发生碰撞的风险，同时也影响了乘客的驾乘舒适性。相比之下，应用延时补偿的MPC在这些方面表现出明显的优势，能够更有效地控制车辆，确保更平稳的驾驶体验，同时减少了发生交通事故的风险（图17和图18）。因此，本研究证明在车辆编队控制系统中，引入延时补偿机制对于提高系统的整体性能和安全性至关重要。
图17 带延时补偿4车轨迹曲线（d =5）

注： 标有×号的红色线条为头车轨迹曲线； 标有○的黄色、红色、蓝色曲线分别表示1号车、2号车、3号车的轨迹曲线。

Fig. 17 Trajectory curves of a four-vehicle convoy with delay compensation （d =5）

图18 带延时补偿4车轨迹曲线（d =10）

注： 标有×号的红色线条为头车轨迹曲线； 标有○的黄色、红色、蓝色曲线分别表示1号车、2号车、3号车的轨迹曲线。

Fig. 18 Trajectory curves of a four-vehicle convoy with delay compensation （d =10）

6 总结与展望

本研究基于融合LQR与MPC的控制器实现了农机装备的入库多机协同归库过程中的编队化轨迹跟踪，建立了农机单机二自由度动力学模型和运动学模型，为之后农机编队行驶问题打下基础。针对实际场景中车载控制器与CAN通信延时而产生的横向控制精度差这一问题，设计了针对不同工况下农机装备的MPC延时补偿器针对编队场景编队跟踪算法，基于Carsim和Simulink建立农机编队作业的联合仿真平台测试，实现多台农机编队鲁棒的协同控制，使多辆车辆能够在变道过程中保持定距离和定速度的轨迹跟踪。
目前，多机编队协同大多基于仿真平台进行验证，其具有安全、经济、快捷等多方面优点，然而仿真平台中理想化模型与真机实验仍存在一定差距。因此，农机装备多机编队作业仍需要进行在法律法规健全的情况下进行真机测试。此外，本研究仅考虑了CAN通信的不变延时，而实际场景中CAN通信延时还受网络传输速度、通信信号等因素的影响。因此，在后续研究中可以进一步探讨基于CAN通信时变延时的补偿方法。

利益冲突声明

本研究不存在研究者以及与公开研究成果有关的利益冲突。

参考文献

原文顺序 | 文献年度倒序 | 文中引用次数倒序
1
刘成良, 林洪振, 李彦明, 等. 农业装备智能控制技术研究现状与发展趋势分析[J]. 农业机械学报, 2020, 51(1): 1-18.

LIU C L, LIN H Z, LI Y M, et al. Analysis on status and development trend of intelligent control technology for agricultural equipment[J]. Transactions of the Chinese society for agricultural machinery, 2020, 51(1): 1-18.

2
刘成良, 贡亮, 苑进, 等. 农业机器人关键技术研究现状与发展趋势[J]. 农业机械学报, 2022, 53(7): 1-22, 55.

LIU C L, GONG L, YUAN J, et al. Current status and development trends of agricultural robots[J]. Transactions of the Chinese society for agricultural machinery, 2022, 53(7): 1-22, 55.

3
李志腾, 李彦明, 唐小涛, 等. 水稻穴直播机自主驾驶控制系统设计[J]. 农机化研究, 2019, 41(2): 150-154.

LI Z T, LI Y M, TANG X T, et al. Design of autonomous driving control system for rice hill-drop drilling machine[J]. Journal of agricultural mechanization research, 2019, 41(2): 150-154.

4
ELBASI E, MOSTAFA N, ALARNAOUT Z, et al. Artificial intelligence technology in the agricultural sector: A systematic literature review[J]. IEEE access, 2023, 11: 171-202.

5
FAGNANT D J, KOCKELMAN K. Preparing a nation for autonomous vehicles: Opportunities, barriers and policy recommendations[J]. Transportation research part A: Policy and practice, 2015, 77: 167-181.

6
ZHOU Y, LIN G J, TANG Y, et al. FLYOVER: A model-driven method to generate diverse highway interchanges for autonomous vehicle testing[C]// 2023 IEEE International Conference on Robotics and Automation (ICRA). Piscataway, New Jersey, USA: IEEE, 2023.

7
GU N, LIU Y J, PENG Z H, et al. Path-guided collision-free formation guidance law for a fleet of under-actuated autonomous surface vehicles[C]// 2019 Chinese Control Conference (CCC). Piscataway, New Jersey, USA: IEEE, 2019: 1199-1204.

8
GUAN J S, ZHOU W D, KANG S B, et al. Robot formation control based on Internet of Things technology platform[J]. IEEE access, 2020, 8: ID 96767.

9
DING W J, WANG S P, ZHANG C, et al. Formation control and obstacle avoidance of MAS with constraints of visibility and motion saturation[C]// 2021 IEEE 16th Conference on Industrial Electronics and Applications (ICIEA). Piscataway, New Jersey,UAS: IEEE, 2021: 2107-2113.

10
PAN Z H, ZHANG C X, XIA Y Q, et al. An improved artificial potential field method for path planning and formation control of the multi-UAV systems[J]. IEEE transactions on circuits and systems II: Express briefs, 2022, 69(3): 1129-1133.

11
OBRADOVIĆ J, KRIŽMANČIĆ M, BOGDAN S. Decentralized multi-robot formation control using reinforcement learning[C]// 2023 XXIX International Conference on Information, Communication and Automation Technologies (ICAT). Piscataway, New Jersey, USA: IEEE, 2023: 1-7.

12
胡鸿彬, 李彦明, 唐小涛, 等. 基于Android的农机导航管理系统研究与设计[J]. 农机化研究, 2019, 41(5): 179-183.

HU H B, LI Y M, TANG X T, et al. The research and design of agricultural machinery navigation management system based on android platform[J]. Journal of agricultural mechanization research, 2019, 41(5): 179-183.

13
ZHANG Y Q, XU Z H, WANG Z J, et al. Impacts of communication delay on vehicle platoon string stability and its compensation strategy: A review[J]. Journal of traffic and transportation engineering (english edition), 2023, 10(4): 508-529.

14
KHATTAK Z H, RIOS-TORRES J, FONTAINE M D. Impact of communications delay on safety and stability of connected and automated vehicle platoons: Empirical evidence from experimental data[J]. IEEE access, 2023, 11: 128549-128568.

15
HE J J. Integrated vehicle dynamics control using active steering, driveline and braking[D]. Leeds: University of Leeds, 2005.

16
ARTUÑEDO A, MORENO-GONZALEZ M, VILLAGRA J. Lateral control for autonomous vehicles: A comparative evaluation[J]. Annual reviews in control, 2024, 57: ID 100910.

17
CHEN Y J, YI G H, LI J F. Scheduling study of stereo garage access problem based on hybrid cross-entropy algorithm[C]// 2021 4th International Conference on Robotics, Control and Automation Engineering (RCAE). Piscataway, New Jersey, USA: IEEE, 2021.

18
刘思源. 车联网自适应协同定位技术的研究[D]. 上海: 上海交通大学, 2019.

LIU S Y. Resrarch of adaptive cooperative positioning technology for vehicular network[D]. Shanghai: Shanghai Jiao Tong University, 2019.

19
SEHLA K, NGUYEN T M T, PUJOLLE G, et al. Resource allocation modes in C-V2X: From LTE-V2X to 5G-V2X[J]. IEEE Internet of Things journal, 2022, 9(11): 8291-8314.

20
GAO H H, LIU C, LI Y, et al. V2VR: Reliable hybrid-network-oriented V2V data transmission and routing considering RSUs and connectivity probability[J]. IEEE transactions on intelligent transportation systems, 2021, 22(6): 3533-3546.

21
KAMAL M, SRIVASTAVA G, TARIQ M. Blockchain-based lightweight and secured V2V communication in the Internet of vehicles[J]. IEEE transactions on intelligent transportation systems, 2021, 22(7): 3997-4004.

22
KARAGIANNIS G, ALTINTAS O, EKICI E, et al. Vehicular networking: A survey and tutorial on requirements, architectures, challenges, standards and solutions[J]. IEEE communications surveys & tutorials, 2011, 13(4): 584-616.

23
DEINLEIN T, GERMAN R, DJANATLIEV A. 5G-sim-V2I/N: towards a simulation framework for the evaluation of 5G V2I/V2N use cases[C]// 2020 European Conference on Networks and Communications (EuCNC). Piscataway, New Jersey, USA: IEEE, 2020.

24
MARTIN-PEREZ J, KONDEPU K, DE VLEESCHAUWER D, et al. Dimensioning V2N services in 5G networks through forecast-based scaling[J]. IEEE access, 2022, 10: 9587-9602.

25
TAHIR M N, MAENPAA K, HIPPI M. Pedestrian motion detection & pedestrian communication (P2I & V2P)[C]// 2020 International Conference on Software, Telecommunications and Computer Networks (SoftCOM). Piscataway, New Jersey, USA: IEEE, 2020.

26
ITO H, MURASE T, SASAJIMA K. Congestion control and energy savings for V2P communication crash warnings with proximity relying[C]// 2016 International Conference on Connected Vehicles and Expo (ICCVE). Piscataway, New Jersey, USA: IEEE, 2016.

27
YU C R, GONG L, FANG R, et al. Integration of agricultural wireless sensor networks to web-of-things through an edge-computing-enriched WSNs/WoT gateway[J]. International journal of interdisciplinary telecommunications and networking, 2022, 14(1): 1-15.

28
刘博, 罗霞, 朱健. 无人驾驶车辆自动避障路径规划仿真研究[J]. 计算机仿真, 2018, 35(2): 105-110.

LIU B, LUO X, ZHU J. Research on simulation of planning obstacle avoidance path for automated vehicles[J]. Computer simulation, 2018, 35(2): 105-110.

29
熊思华, 李彦明, 焦中元, 等. 稻麦联合收获机械总线化监控系统研究[J]. 农机化研究, 2019, 41(4): 190-193, 199.

XIONG S H, LI Y M, JIAO Z Y, et al. Research on CAN-bus monitoring system of rice and wheat combine harvester[J]. Journal of agricultural mechanization research, 2019, 41(4): 190-193, 199.

30
GONG L, WU Y X, GAO B S, et al. Real-time dynamic planning and tracking control of auto-docking for efficient wireless charging[J]. IEEE transactions on intelligent vehicles, 2023, 8(3): 2123-2134.

31
QIN Z W, HSU C, WU Y X, et al. Multi-stage perception, positioning and planning for automatic wireless charging of AGVs[C]// 2021 6th International Conference on Automation, Control and Robotics Engineering (CACRE). Piscataway, New Jersey, USA: IEEE, 2021.

32
ASHOK KUMAR B, GAYATHRI S, SURENDHAR S, et al. Robust H-infinity controller for two degree of freedom helicopter[C]// 2019 IEEE International Conference on System, Computation, Automation and Networking (ICSCAN). Piscataway, New Jersey, USA: IEEE, 2019.

33
LI Y M, WU T, XIAO Y F, et al. Path planning in continuous adjacent farmlands and robust path-tracking control of a rice-seeding robot in paddy field[J]. Computers and electronics in agriculture, 2023, 210: ID 107900.

34
KUNTZE H B, JACUBASCH A, RICHALET J, et al. On the predictive functional control of an elastic industrial robot[C]// 1986 25th IEEE Conference on Decision and Control. Piscataway, New Jersey, USA: IEEE, 1986.

Options
文章导航

/

〈 〉

  • 国际刊号　ISSN 2097-485X（网络）
    　　　　　ISSN 2096-8094（印刷）
    国内刊号　CN 10-1681/S
    CODEN　 ZNZHD7
    主管：中华人民共和国农业农村部
    主办：中国农业科学院农业信息研究所
  • 地址：北京市海淀区中关村南大街12号中国农业科学院农业信息研究所
    邮编：100081
    电话：010-82109657
    E-mail：smartag@caas.cn
京ICP备09089781号-37