欢迎您访问《智慧农业(中英文)》官方网站! English
专题--丘陵山区智慧农业技术与机械

割草机器人自适应时域MPC路径跟踪控制方法

  • 贺庆 1 ,
  • 冀杰 , 1 ,
  • 冯伟 2 ,
  • 赵立军 3 ,
  • 张博涵 1
展开
  • 1. 西南大学 工程技术学院, 重庆 400715, 中国
  • 2. 重庆市农业科学研究院 农业机械研究所, 重庆 401329, 中国
  • 3. 重庆文理学院 智能制造工程学院, 重庆 402160, 中国
冀 杰,博士,副教授,研究方向为智能车辆与智能农机的道路环境感知、驾驶行为决策及底盘动力学综合控制等。E-mail:

贺 庆,研究方向为智能车辆和智能农机运动规划与运动控制。E-mail:

收稿日期: 2024-01-10

  网络出版日期: 2024-06-28

基金资助

重庆市研究生科研创新项目(CYS23207)

重庆市科学技术局农业农村领域重点研发项目(cstc2021jscx-gksbX0003)

重庆市教育委员会科学技术研究项目(KJZD-M202201302)

重庆市科技局创新发展联合基金项目(CSTB2022NSCQ-LZX0024)

Adaptive Time Horizon MPC Path Tracking Control Method for Mowing Robot

  • HE Qing 1 ,
  • JI Jie , 1 ,
  • FENG Wei 2 ,
  • ZHAO Lijun 3 ,
  • ZHANG Bohan 1
Expand
  • 1. College of Engineering and Technology, Southwest University, Chongqing 400715, China
  • 2. Institute of Agricultural Machinery, Chongqing Academy of Agricultural Sciences, Chongqing 401329, China
  • 3. School of Intelligent Manufacturing Engineering, Chongqing University of Arts and Sciences, Chongqing 402160, China
JI Jie, E-mail:

HE Qing, E-mail:

Received date: 2024-01-10

  Online published: 2024-06-28

Supported by

Chongqing Graduate Research Innovation Project(CYS23207)

Chongqing Science and Technology Bureau Agriculture and Rural Key Research and Development Project(cstc2021jscx-gksbX0003)

Science and Technology Research Project of Chongqing Education Commission(KJZD-M202201302)

Chongqing Science and Technology Bureau Innovation Development Joint Fund Project(CSTB2022NSCQ-LZX0024)

Copyright

copyright©2024 by the authors

摘要

[目的/意义] 传统路径跟踪模型预测控制(Model Predictive Control, MPC)大多采用固定时域,较少考虑道路弯曲和曲率变化的影响,使得机器人在曲线路径作业过程中的跟踪效果和适应性都较差。因此,设计了一种自适应时域MPC控制器并使其满足自主割草等复杂作业要求。 [方法] 首先,根据割草机器人的速度确定前方参考路径的预瞄区域,并计算预瞄区域内的参考路径曲度因子和曲度变化因子,分别用于描述曲率和曲率变化大小。然后,将二者作为模糊控制器的输入信息,用于自适应调节MPC的预测时域,同时,根据预测时域及曲度变化因子调整控制时域,以增强控制器对路径弯曲变化的适应性并降低计算资源。此外,设计一种MPC事件触发执行机制,进一步提升MPC的实时性。 [结果和讨论] 与固定时域的MPC进行对比试验,自适应时域MPC控制器的最大横向误差绝对值和最大航向误差绝对值分别控制在11 cm和0.13 rad以内,其平均求解时间比最大时域MPC减少10.9 ms。 [结论] 自适应时域MPC不仅能够保证割草机器人对曲线路径的跟踪精度,同时降低了MPC求解计算量并提高了控制实时性,解决了固定时域MPC的控制精度与计算量之间的矛盾。

本文引用格式

贺庆 , 冀杰 , 冯伟 , 赵立军 , 张博涵 . 割草机器人自适应时域MPC路径跟踪控制方法[J]. 智慧农业, 2024 , 6(3) : 82 -93 . DOI: 10.12133/j.smartag.SA202401010

Abstract

[Objective] The traditional predictive control approach usually employs a fixed time horizon and often overlooks the impact of changes in curvature and road bends. This oversight leads to subpar tracking performance and inadequate adaptability of robots for navigating curves and paths. Although extending the time horizon of the standard fixed time horizon model predictive control (MPC) can improve curve path tracking accuracy, it comes with high computational costs, making it impractical in situations with restricted computing resources. Consequently, an adaptive time horizon MPC controller was developed to meet the requirements of complex tasks such as autonomous mowing. [Methods] Initially, it was crucial to establish a kinematic model for the mowing robot, which required employing Taylor linearization and Euler method discretization techniques to ensure accurate path tracking. The prediction equation for the error model was derived after conducting a comprehensive analysis of the robot's kinematics model employed in mowing. Second, the size of the previewing area was determined by utilizing the speed data and reference path information gathered from the mowing robot. The region located a certain distance ahead of the robot's current position, was identified to as the preview region, enabling a more accurate prediction of the robot's future traveling conditions. Calculations for both the curve factor and curve change factor were carried out within this preview region. The curvature factor represented the initial curvature of the path, while the curvature change factor indicated the extent of curvature variation in this region. These two variables were then fed into a fuzzy controller, which adjusted the prediction time horizon of the MPC. The integration enabled the mowing robot to promptly adjust to changes in the path's curvature, thereby improving its accuracy in tracking the desired trajectory. Additionally, a novel technique for triggering MPC execution was developed to reduce computational load and improve real-time performance. This approach ensured that MPC activation occurred only when needed, rather than at every time step, resulting in reduced computational expenses especially during periods of smooth robot motion where unnecessary computation overhead could be minimized. By meeting kinematic and dynamic constraints, the optimization algorithm successfully identified an optimal control sequence, ultimately enhancing stability and reliability of the control system. Consequently, these set of control algorithms facilitated precise path tracking while considering both kinematic and dynamic limitations in complex environments. [Results and Discussion] The adaptive time-horizon MPC controller effectively limited the maximum absolute heading error and maximum absolute lateral error to within 0.13 rad and 11 cm, respectively, surpassing the performance of the MPC controller in the control group. Moreover, compared to both the first and fourth groups, the adaptive time-horizon MPC controller achieved a remarkable reduction of 75.39% and 57.83% in mean values for lateral error and heading error, respectively (38.38% and 31.84%, respectively). Additionally, it demonstrated superior tracking accuracy as evidenced by its significantly smaller absolute standard deviation of lateral error (0.025 6 m) and course error (0.025 5 rad), outperforming all four fixed time-horizon MPC controllers tested in the study. Furthermore, this adaptive approach ensured precise tracking and control capabilities for the mowing robot while maintaining a remarkably low average solution time of only 0.004 9 s, notably faster than that observed with other control data sets-reducing computational load by approximately 10.9 ms compared to maximum time-horizon MPC. [Conclusions] The experimental results demonstrated that the adaptive time-horizon MPC tracking approach effectively addressed the trade-off between control accuracy and computational complexity encountered in fixed time-horizon MPC. By dynamically adjusting the time horizon length the and performing MPC calculations based on individual events, this approach can more effectively handle scenarios with restricted computational resources, ensuring superior control precision and stability. Furthermore, it achieves a balance between control precision and real-time performance in curve route tracking for mowing robots, offering a more practical and reliable solution for their practical application.

0 引 言

随着国家对工业智能化的重视,移动机器人领域正在迅速发展。智能割草机器人作为机器人技术的一个分支,正在逐步取代传统的人工割草作业,减少了传统手工作业的劳动强度和环境污染1。而路径跟踪控制作为智能农机自动导航的关键技术之一,对于提高智能割草机器人作业效率和作业质量发挥至关重要的作用2。目前,用于路径跟踪控制的主流方法有:比例积分微分(Proportional Integral Derivative, PID)3、模糊控制4、纯跟踪(Pure Pursuit)5、滑模控制6、线性二次调节器(Linear Quadratic Regulator, LQR)7、模型预测控制(Model Predictive Control, MPC)8, 9等。与其他方法相比,MPC在处理多目标约束优化问题上具有明显的优势,故在路径跟踪控制领域得到了广泛应用10, 11
目前,国内外对基于MPC的路径跟踪控制进行了大量的研究。有研究基于模型预测理论设计跟踪控制器,采用泰勒级数对农业机械模型进行线性化处理,有效减少路径跟踪过程的计算负担12。Tan等13在MPC控制器设计过程中,综合考虑了由分配误差和偏置模型中外力引起的扰动,增强了MPC在路径跟踪中的鲁棒性和稳定性。Bettega和Richiedei14设计了一种新型的基于线性化模型的MPC公式,并以可忽略的误差和延迟跟踪时变作为参考,实现更高精度的路径跟踪。李俊等15通过确定车辆在直线道路上不打滑的最大纵向速度与路面曲率的函数关系设计了一种改进的MPC算法,提高了车辆在复杂道路条件下的路径跟踪准确性、平稳性与安全性。寇发荣等16将状态扩展模型预测与转角补偿相结合,利用状态轨迹的近似线性化方法对非线性模型进行线性化,降低了最优控制量的求解时间。
上述MPC控制器全都采用固定时域,而预测时域和控制时域对路径跟踪控制精度及稳定性会产生很大影响,固定时域难以使MPC在复杂环境中达到期望的路径跟踪控制效果17, 18。目前,对于变时域的MPC方法研究较少。范贤波等19将横向和纵向车速作为模糊控制器输入,用于自适应调节MPC的预测时域和控制时域,提高了车辆在高速时的稳定性。Lin等20采用模糊理论确定在线优化的时域因子,再根据加权因子调整预测时域和控制时域,用于提高不同速度下的适应性和稳定性。另外,Zhou等21将遗传算法与MPC相结合,利用遗传算法计算实时车速、车辆姿态和路况下的最优时域参数,实现自适应MPC。
在上述研究中,变时域MPC主要基于车速、横向误差等指标调节时域大小,但没有考虑路径曲率以及曲率变化对MPC控制精度的影响,难以保证割草机器人在曲率变化较大的曲线路径中完成精准作业。为了有针对性地解决割草机器人对曲线路径跟踪精度不高,以及MPC计算求解量较大的问题,本研究设计了一种自适应时域MPC控制器,能够根据参考路径的曲率以及曲率变化优化MPC的时域,从而保证割草机器人对曲线路径的跟踪精度,并降低MPC求解计算量。另外,设计了一种MPC事件触发执行机制,进一步减少MPC计算负荷。

1 割草机器人建模

图1是自主设计的割草机器人模型俯视图,通过两个后轮差速驱动实现运动控制。图中XOY表示全局坐标系;Gxy表示割草机器人的局部坐标系;点G表示两后轮轴中心位置; 2 a为两后轮中心距离; r为后轮半径。
图1 割草机器人模型俯视图

Fig. 1 Top view of the mowing robot model

如果采用多自由度动力学建立割草机器人模型,虽然可以提高控制器的精确性,但是模型的复杂度会大大增加控制器的计算量而导致实时性较差。因此,本研究采用割草机器人的运动学模型,以降低问题的求解难度,提高控制的实时性,如公式(1)所示。
x ˙ y ˙ θ ˙ = c o s θ 0 s i n θ 0 0 1 v w
式中: x y分别为割草机器人在坐标系XOY中的横坐标和纵坐标,m; θ为航向角,rad; v为线速度,m/s; w为角速度,rad/s。
假设参考路径上任意点都满足上述运动学方程,在参考点 x r , y r , θ r处对公式(1)利用泰勒级数展开进行线性化处理,得到公式(2)
x ˙ - x ˙ r y ˙ - y ˙ r θ ˙ - θ ˙ r = 0 0 - v r s i n θ r 0 0 v r c o s θ r 0 0 0 x - x r y - y r θ - θ r                    + c o s θ r 0 s i n θ r 0 0 1 v - v r w - w r
式中: v r为参考点处的线速度,m/s; w r为参考点处的角速度,rad/s。
设采样时间为 T,使用欧拉公式对公式(2)进行离散化处理,得到割草机器人的离散化误差方程,如公式(3)所示。
x ˜ ( k + 1 ) = a x ˜ ( k ) + b u ˜ ( k ) a = 1 0 - T v r s i n θ r 0 1 T v r c o s θ r 0 0 1 b = T c o s θ r 0 T s i n θ r 0 0 T

2 自适应时域模型预测控制器设计

本研究设计的自适应时域模型预测控制器的系统结构如图2所示。首先,基于公式(3)表示的离散化误差方程,推导出割草机器人的预测模型。然后,根据参考路径信息和割草机器人作业速度,动态确定预瞄区域范围。再通过计算曲度因子 f s和曲度变化因子 f s c分别反映预瞄区域起始处的参考路径弯曲程度大小以及整个预瞄区域内的参考路径弯曲变化程度大小。其次,将 f s f s c作为MPC自适应时域模块中的模糊控制器的输入,对MPC的预测时域 N p进行自适应优化,同时,根据所获得的 N p以及 f s c合理调整控制时域 N c,以提高控制器对曲率变化的曲线路径的适应性并在一定程度上减少计算量。再者,通过构建MPC事件触发执行机制,进一步降低MPC求解的计算负荷,提高控制实时性。最后,在满足运动学和动力学约束的条件下,求解出驱动电机的最优控制量,保证割草机器人的路径跟踪控制精度。
图2 自适应时域MPC控制系统结构框图

Fig. 2 Structural diagram of adaptive time horizon MPC control system

2.1 MPC控制器设计

结合公式(3),将当前时刻的状态量与前一时刻的控制量组合为新的状态量,如公式(4)所示。
ξ ( k ) = x ˜ ( k ) u ˜ ( k - 1 )
则新的状态空间表达式如公式(5)所示。
ξ ( k + 1 ) = A ξ ( k ) + B Δ u ˜ ( k ) η ( k ) = C ξ ( k ) A = a b 0 I m B = b I m C = I n 0 Δ u ˜ ( k ) = u ˜ ( k ) - u ˜ ( k - 1 )
式中: I m I n分别为 m阶和 n阶的单位矩阵; m为控制量维度; n为状态量维度。
MPC的预测时域设为 N p,控制时域设为 N c。假设当前时刻为 k,利用当前状态信息定义未来时域内系统的输入向量 Δ U N c ( k )和预测输出向量 Y N p ( k ),如公式(6)公式(7)所示。
Δ U N c ( k ) = d e f Δ u ˜ ( k | k ) Δ u ˜ ( k + 1 | k ) Δ u ˜ k + N c - 1 | k
Y N p ( k ) = d e f η ( k + 1 k ) η ( k + 2 k ) η k + N p k
最终经过递推,得到预测输出方程如公式(8)所示。
Y N p ( k ) = A x ξ ( k ) + B u Δ U N c ( k ) A x = C A , C A 2 C A N p T B u = C B 0 0 C A B C B 0 C A N p - 1 B C A N p - 2 B C A N p - N c B
为保证割草机器人能够高效、稳定地跟踪参考路径,将系统状态误差和控制误差变化引入到目标函数中,得到如公式(9)所示的目标函数。
m i n J = Y N p ( k ) - Y N p ( k ) r e f Q 2 + Δ U N c ( k ) R 2 + ρ ε 2
式中: ρ为权重系数; ε为松弛因子; Q R为权重矩阵; Y N p ( k ) r e f为预测时域内的参考输出。目标函数的第1项反映了控制系统对参考路径的跟随能力;第2项反映了对控制量变化的约束,以保证割草机器人在运动过程中尽可能不会出现速度、航向角、加速度的剧烈变化,在一定程度上保证割草机器人的稳定性。第3项通过引入松弛因子来增强可行域的解,从而保证优化问题存在最优解。
为了得到多约束条件下的最优控制序列,采用二次规划方法进行求解,即将带约束的最优问题转化为二次规划(Quadratic Programming, QP)问题进行数值求解。将公式(8)带入目标函数中,则公式(9)可以转换成如公式(10)所示的二次规划一般形式。
m i n Δ U N c , ε J = Δ U N c ( k ) T , ε T H Δ U N c ( k ) T , ε + g T Δ U N c ( k ) T , ε   s . t .   Δ U m i n - ε Δ U N c ( k ) Δ U m a x + ε U m i n A I Δ U N c ( k ) + U k - 1 U m a x 0 ε ε m a x H = B u T Q B u + R 0 0 ρ g = E T Q B u , 0 , E = A x ξ ( k ) Δ U m i n = Δ u ˜ m i n Δ u ˜ m i n . . . Δ u ˜ m i n , Δ U m a x = Δ u ˜ m a x Δ u ˜ m a x . . . Δ u ˜ m a x U m i n = u ˜ m i n u ˜ m i n . . . u ˜ m i n , U m a x = u ˜ m a x u ˜ m a x . . . u ˜ m a x U k - 1 = u ˜ ( k - 1 ) u ˜ ( k - 1 ) . . . u ˜ ( k - 1 ) A I = I 2 0 0 0 I 2 I 2 0 0 I 2 I 2 I 2 0 0 I 2 I 2 I 2 I 2

2.2 预瞄区域的确定

确定预瞄区域是为了提前发现并适应前方参考路径弯曲程度的变化,以确保割草机器人做出相应的控制动作,实现准确的路径跟踪。预瞄区域的起点由匹配点 P m ( x m , y m )(割草机器人自身位置距离参考路径最近的点)确定,然后根据割草机器人的速度确定预瞄区域范围 l v,如图3所示, P v ( x v , y v )为预瞄区域终点。
图3 前方参考路径的预瞄区域

Fig. 3 Preview area of the reference path ahead

随着割草机器人速度增加,其在短时间内行驶的距离也相应增加。为了精确预测未来参考路径的弯曲变化程度,需适当增加预瞄区域的范围,以便控制器能够更精确、更及时地做出控制决策。综上分析,提出预瞄区域 l v的计算如公式(11)所示。
l v = l v m i n , v v m i n α v + β , v m i n < v < v m a x l v m a x , v v m a x
式中: l v m i n l v m a x分别为预瞄区域的最小、最大长度,m; α β为预设常数,根据场地试验确定; v m i n v m a x分别为线速度最小值与最大值,m/s。
用折线段将预瞄区域内的离散点连接,以便更好描述区域内的参考路径弯曲变化程度,如图4所示。
图4 前方参考路径的预瞄区域曲度计算示意图

Fig. 4 Schematic diagram of the curvature calculation of the preview area of the reference path ahead

预瞄区域内相邻折线段之间的夹角计算如公式(12)所示。
φ i = a r c c o s P i + 1 P i P i + 2 P i + 1 P i + 1 P i P i + 2 P i + 1
式中: φ i [ 0 , π ] i [ 1 , κ - 1 ] κ为折线段的数量。
为反映割草机器人在当前位置处距离参考路径最近点的弯曲度大小,以及整个预瞄区域内的参考路径弯曲变化情况,分别定义曲度因子 f s和曲度变化因子 f s c,如公式(13)~公式(15)所示。
f s = φ 1 - φ m i n φ m a x - φ m i n
f s c = Δ φ - Δ φ m i n Δ φ m a x - Δ φ m i n
Δ φ = 1 κ - 1 i = 1 κ - 1 ( φ i + 1 - φ i )
式中: φ m a x φ m i n Δ φ m a x Δ φ m i n分别为 φ i Δ φ的最大值与最小值,根据实际试验确定; f s [ 0,1 ] f s c [ 0,1 ]
由定义可知,当 f s f s c越大时,则表明参考路径在匹配点处的弯曲程度以及在整个预瞄区域内的弯曲变化程度也就越大。值得注意的是,当预瞄区域的范围到达全局参考路径的终点 P t ( x t , y t )时,此时的预瞄区域变为匹配点 P m ( x m , y m )与终点 P t ( x t , y t )的区域,预瞄区域的目标点将停在全局参考路径终点位置,如图5所示。此刻计算出来的 f s c将作为割草机器人后续路径跟踪过程中的曲度变化因子,直到路径跟踪控制结束。
图5 前方参考路径的预瞄区域范围到达参考路径终点

Fig. 5 The extent of the preview area of the reference path ahead reaches the end of the reference path

2.3 MPC自适应时域模块设计

2.3.1 MPC自适应时域优化策略

在MPC控制系统中,影响路径跟踪精度和稳定性的参数主要有预测时域 N p和控制时域 N c 18。在实际路径跟踪过程中,针对不同的作用对象或者不同的外界环境,需要根据不同的标准选取MPC时域的大小,因此,固定时域的MPC并不能满足割草机器人在曲线路径跟踪作业过程中的控制精度要求。对此,需要针对性地分析MPC时域对割草机器人在曲线路径跟踪控制过程中的影响,并提出自适应时域优化策略。
图6所示,在MPC控制器其他参数保持不变的情况下, N p越大,MPC可以在更长时间内预测未来的路径,获得更多的割草机器人状态信息19, 22。但如果 N p过大,则会增大割草机器人当前位置较远处的误差权重,导致割草机器人在当前位置附近的路径跟踪精度降低,同时也会增加MPC算法的计算量,降低系统的实时性23, 24。但是,如果 N p过小,获得的割草机器人未来时刻的状态信息过少,且存在系统控制量约束的情况下,会导致割草机器人无法及时转向,路径跟踪精度严重降低的同时,稳定性也得不到保证,甚至造成割草机器人路径跟踪失败25, 26
图6 MPC预测时域对路径跟踪控制的影响

注: Np 为预测时域。

Fig. 6 Influence of MPC prediction horizon on path following

当割草机器人的作业路径弯曲程度较大或弯曲变化较快时,即曲度因子 f s和曲度变化因子 f s c较大时,意味着其需要更早地做出转弯或调整行驶方向的决策。在一定范围内适当增加 N p,MPC可以提前预测到未来路径曲率大小以及曲率的变化,同时也能更好地考虑系统的动态响应延迟,生成更及时的控制指令。既保证了割草机器人在当前位置附近的路径跟踪精度在可接受范围内适度降低,同时,也能够更准确地跟踪曲线路径,避免发生失控。而当 f s f s c较小,即相对平直时,较小的 N p可以提高割草机器人当前位置附近的跟踪精度,同时,还可以在一定程度上提高MPC算法的计算效率。
图7所示,当其他控制参数不变的情况下, N c越大,控制精度和平顺性就越高。但是过大的 N c会降低MPC控制器的求解速度,实时性也随之下降。相反, N c过小时,求解出的控制动作为了保证预测时域内割草机器人平均误差最小,会牺牲割草机器人当前位置附近路径跟踪精度25-28
图7 控制时域对路径跟踪控制的影响

注: Nc 为控制时域。

Fig. 7 Influence of control horizon on path following control

所以,在 f s f s c较大时,可以适当增大 N c,以便提供更多的时间来调整割草机器人的控制输入,以适应弯曲路径,提高控制精度和稳定性。相反,则适当减少 N c,从而减少控制系统计算量。
综上所述,提出如下MPC自适应时域优化策略:采用模糊控制算法对时域进行实时优化,根据预瞄区域内的曲度因子 f s和曲度变化因子 f s c,自适应优化预测时域 N p。由于控制时域 N c与预测时域 N p是相互关联的, N c的取值一般不超过 N p。所以,根据所获得的 N p以及 f s c进一步合理调整 N c

2.3.2 模糊输入与模糊规则

经上述分析,选择曲度因子 f s和曲度变化因子 f s c作为模糊控制器的输入,MPC的预测时域 N p作为模糊控制器的输出。图8分别为模糊控制算法输入参数、输出参数的隶属函数图。其中,输入量化等级VL、L、M、H、VH分别表示为极小、小、中、大、极大。输出量化等级VS、S、MS、M、ML、L、VL分别表示为极短、短、中短、中、中长、长、极长。各变量的论域与量化因子如表1所示。
图8 自适应时域模块的模糊控制器输入和输出隶属度函数

Fig. 8 Membership function of input and output of fuzzy controller for adaptive time horizon module

表1 模糊输入输出的量化因子

Table 1 Quantization factor of fuzzy input and output

变量 曲度因子 f s 曲度变化因子 f s c 预测时域 N p
基本论域 [0,1] [0,1] 15,36]
模糊论域 [0,1] [0,1] [0,1]
量化因子 1 1 1/21
根据自适应时域优化策略,模糊规则需要遵循以下规则: f s f s c越小, N p越短; f s f s c越大, N p越长;其他情况按照专家规则进行定性分析。由此设计模糊规则如表2所示;模糊控制的输入与输出的响应曲面如图9所示。
表2 MPC预测时域模糊规则

Table 2 Fuzzy rules for the MPC prediction horizon

f s c f s
VL L M H VH
VL VS S ML L VL
L S MS ML L VL
M MS M L VL VL
H M ML VL VL VL
VH ML L VL VL VL
图9 模糊控制器的输入和输出响应曲面

Fig. 9 Fuzzy controller input and output response surfaces

最终,经过模糊推理得到模糊输出量,选择重心法进行解模糊运算,得到具体的预测时域 N p,并结合 N p f s c适当调整控制时域 N c。根据上述自适应时域优化策略分析,在参考路径曲度变化较大情况下,可以适当增大 N c,提高控制精度。所以,设计控制时域表达式如公式(16)所示。
N c = r o u n d λ N p ( 1 + γ f s c )
式中: λ为时域权重系数, 0 < λ < 0.5 γ为曲度变化增益系数, 0 < γ < 1

2.4 MPC事件触发执行模块设计

对现有的控制单元来说,MPC的滚动优化求解是一个沉重的计算负担29。为降低MPC求解计算量,本研究引入事件触发执行机制。MPC通常在每个时间步长都需要进行优化求解,但是,不一定需要在每个时间步长都执行控制计算,只有在特定事件发生时才执行MPC计算,否则可沿用上一周期优化的结果作为控制策略。通过合理的触发条件,可以在保证控制性能的前提下减少计算负担。
为保持割草机器人在路径跟踪过程中的控制稳定性和控制精度,根据控制过程的累计横向误差 S e d和累计航向误差 S e p h i以及预瞄区域的曲度因子 f s与曲度变化因子 f s c设置事件触发条件。当预瞄区域的曲度因子 f s与曲度变化因子 f s c小于一定阈值时,才开启MPC事件触发执行机制。而在参考路径曲度或曲度变化程度较大时,需要MPC频繁在每个采样时间进行优化求解,以便割草机器人更好地应对不断变化的参考路径,从而更好地跟踪路径,同时维持稳定性。当累计横向误差 S e d和累计航向误差 S e p h i大于一定阈值时,重新进行MPC优化求解,防止割草机器人控制系统进入不稳定状态,确保割草机器人在长期运动中能够更好地跟踪期望路径。自适应时域MPC控制器的事件触发执行机制具体实现伪代码如下。
算法:MPC事件触发执行机制

1. 初始化:设置触发条件阈值:横向误差阈值 t h r e d、航向误差阈值 t h r e p h i、曲度因子阈值 t h r f s、曲度变化因子阈值 t h r f s c。定义误差缓存队列: Q e d Q e p h i

2. while未到达参考路径终点

3. 将横向误差 e d和航向误差 e p h i分别存入缓存队列 Q e d Q e p h i

4. if f s > t h r f s or f s c > t h r f s c

5. MPC求解最优控制序列 U (1:m× N c),并将 U (1:m作为控制输入,清空误差缓存队列: Q e d Q e p h i

6. else

7. 计算 S e d = s u m [ a b s ( Q e d ) ] S e p h i = s u m [ a b s ( Q e p h i ) ]

8. if 第一次运行

9. MPC求解最优控制序列 U (1:m× N c

10. else

11. If S e d > t h r e d or S e p h i > t h r e p h i or size U <m

12. MPC求解最优控制序列 U (1:m× N c),清空误差缓存队列: Q e d Q e p h i

13. end if

14. end if

15. 将 U (1:m)作为控制输入

16. 将控制序列前m个移除得到 U m+1:end)

17. end if

18. end while

MPC触发执行机制的引入虽然是在每个采样时间加入一步额外计算,但是事件触发的本质是逻辑判断,其算法计算量远远小于MPC在每个采样时间进行的滚动优化求解的计算量,所以引入MPC触发执行机制可以有效节约计算资源。

3 试验验证

3.1 试验平台

为验证提出的控制算法的有效性,在自主设计的割草机器人平台上进行试验(图10)。割草机器人的运动方式为差速驱动转向,通过调节两独立直流伺服电机的转速,再经过链条传动到后轮,从而实现运动控制。前轮采用端面轴承连接,能够实现360°灵活转动。割草机器人后轮半径为0.215 m,后轮轮距为1.034 m。该平台上配备了英伟达Jetson Xavier NX控制主机、16线velodyne激光雷达、惯性测量单元(Inertial Measurement Unit, IMU)、额定功率2 000 W和额定转速100 r/min的直流伺服电机,同时直流伺服电机自带双驱伺服控制器,支持增量式编码器与霍尔元器件等多种反馈元件。
图10 割草机器人平台

Fig. 10 Mowing robot platform

割草机构如图11所示,主要由割草刀盘、割草电机、电推杆等组成。根据割草机器人整机宽度,筛选合适的割草刀盘,选择的割草刀盘作业宽度为1 m,切割部分由两个长0.7 m的刀片构成,具有向右侧排草功能。将两台割草电机安装在刀片转轴正上方,通过联轴器将电机输出轴与转轴相连,实现电机直接驱动进行割草作业。此外,为了适应不同草坪,通过控制电推杆的伸缩长短,来调节割草刀盘的高度。当电推杆拉长或缩短时,带动旋转三角板往上或往下移动,使得链条可以拖动割草刀盘上下运动,从而能够更好满足不同割草高度的作业要求。
图11 割草机构

Fig. 11 Structure of lawn mower

试验算法在机器人操作系统(Robot Operating System, ROS)上进行验证,通过在ROS上搭建路径跟踪控制节点发布割草机器人运动控制指令,再利用直流伺服电机驱动器的串口协议,向左右后轮驱动电机发送最终速度控制指令,从而实现割草机器人的路径跟踪控制。割草机器人的定位主要采用LeGO-LOAM(Lightweight and Groud-Optimized Lidar Odometry and Mapping on Variable Terrain)算法30,根据激光雷达和IMU传感器数据,来实时获取割草机器人的位姿信息,以及根据直流伺服电机自带的反馈元件来获取割草机器人速度信息。

3.2 试验设置

割草机器人主要采用往复式路径进行作业,包括直线路径以及掉头转弯的曲线路径,所以采用如图12所示的S形路径作为参考路径进行跟踪控制测试。并根据割草机器人的割草刀盘作业宽度,设置两段参考直线路径间距为1 m,曲线路径的曲率为2   m - 1。同时为了保证割草质量,其中直线路径的参考速度设置为1 m/s,曲线路径的参考速度设置为0.5 m/s。根据参考路径以及割草机器人结构尺寸,试验场地选择如图13所示的一处草坪。
图12 割草机器人路径跟踪试验参考路径

Fig. 12 Reference path for path tracking test of mowing robot

图13 割草机器人路径跟踪实地测试

Fig. 13 Field testing of path tracking for mowing robots

本研究设置了4组固定时域MPC控制器进行对比试验,条件如下。第1组: N p = 14 N c = 5;第2组: N p = 22 N c = 14;第3组: N p = 27 N c = 21;第4组: N p = 32 N c = 27。为了更好地比较不同对照组的控制性能,引入最大横向误差绝对值 E d m a x、横向误差绝对值均值 E d m、横向误差绝对值标准差 E d s、最大航向误差绝对值 E θ m a x、航向误差绝对值均值 E θ m、横向误差绝对值标准差 E θ s、MPC平均求解时间 t m等性能指标对跟踪效果进行评价。同时在试验中,本研究所设计的控制器的主要控制参数如表3所示。
表3 割草机器人自适应时域MPC控制试验参数

Table 3 Experimental parameters for adaptive time horizon MPC control of mowing robots

参数 数值
采样周期 T/s 0.1
权重矩阵 Q 100   I 3 N p
权重矩阵 R I 2 N c
松弛因子最大值 ε m a x 1
松弛因子权重系数 ρ 10
最大线速度 v m a x/(m/s) 2
最小线速度 v m i n/(m/s) 0.3
预瞄区域最小长度 l v m i n/m 1.5
预瞄区域最大长度 l v m a x/m 5
时域权重系数 λ 0.5
曲度变化增益系数 γ 0.8
横向误差阈值 t h r e d 0.02
航向误差阈值 t h r e p h i 0.02
曲度因子阈值 t h r f s 0.5
曲度变化因子阈值 t h r f s c 0.5

3.3 试验结果与分析

当MPC选择不同固定时域与自适应时域时,对参考路径的跟踪效果也存在一定差异。从图14中可以看出,采用4组固定时域与自适应时域均能跟踪参考路径,但在不同路段的跟踪效果有所不同。当割草机器人由直线段进入第1个曲线路径进行转弯掉头时,较大的时域能够提前预测未来曲率的变化情况,会更早地进行转弯控制,由于第4组与自适应时域在曲线段的时域最大,故在此段路径的偏离距离最小。而第1组和第2组由于时域较小,割草机器人难以及时转向,偏离参考路径较大。第3组偏离距离居中。与此类似,当割草机器人进入第2个曲线路径时,时域最短的第1组偏离参考路径最远,而自适应时域由于可以根据前方路径曲率以及曲率变化情况自适应调节时域大小,故能够相对及时地进行转弯掉头。
图14 割草机器人期望路径的跟踪效果对比

Fig. 14 Comparison of tracking effects on the expected path of mowing robots

另外,通过对比割草机器人路径跟踪过程中的误差,也能够直观显示不同时域对照组与自适应时域的控制效果,如图15所示,时域越大,横向误差和航向误差的峰值以及整体零偏幅度越小。在对照组中,由于第4组时域最大,横向误差和航向误差的峰值和零偏幅度最小,跟踪效果最好,而本研究的自适应时域MPC控制器的跟踪效果比第4组固定时域更好。在设计的MPC控制器中,预测时域与控制时域能够随曲度因子,以及曲度变化因子自适应调节,是重要的创新点之一。从图16图17可以看出,预测时域为16~36,而控制时域为8~31。在参考路径的曲率以及曲率变化较大的地方,MPC自适应时域模块会及时适当增大预测时域与控制时域,以适应参考路径曲率的变化,进而提高割草机器人对曲线路径的跟踪控制精度。相反,则会适当减少时域,以降低控制器的计算负载。
图15 割草机器人路径跟踪误差对比图

Fig. 15 Comparison chart of path tracking errors for mowing robots

图16 预瞄区域的曲度因子及曲度变化因子

Fig. 16 Curvature factor and curvature variation factor of the preview area

图17 自适应时域MPC的预测时域与控制时域自适应变化

Fig.17 Adaptive time horizon MPC with predictive time horizon and control time horizon adaptive changes

表4统计了不同时域对照组和自适应时域在不同控制性能指标下的数据。可以看出,对照组中,第4组在跟踪控制精度方面最好;第1组的控制效果最差;第2组和第3组各项数据适中。但是,由于第4组的时域最大,MPC的求解计算量也最大,平均求解时间 t m达到0.015 8 s,而第1组的求解时间 t m只有0.006 7 s。相较于固定时域,本研究设计的自适应时域MPC控制器的最大横向误差绝对值 E d m a x和最大航向误差绝对值 E θ m a x分别控制在了11 cm和0.13 rad以内,比固定时域对照组都要小。同时,自适应时域MPC控制器的横向误差绝对值均值 E d m和航向误差绝对值均值 E θ m比第1组分别减少 75.39 % 57.83 %,比第4组分别减少 38.38 % 31.84 %。并且,自适应时域MPC控制器的横向误差绝对值标准差 E d s和航向误差绝对值标准差 E θ s分别只有0.025 6 m和0.025 5 rad,均比4组固定时域MPC控制器的 E d s E θ s要小,表明跟踪误差被控制在较小范围内,有效保证了割草机器人跟踪控制精度。同时自适应时域MPC的平均求解时间 t m只有0.004 9 s,比最大时域第4组减少了10.9 ms,有效降低了计算负荷。由图18图19可以看出,在参考路径曲率和曲率变化率较小处,MPC执行触发的次数较少。相反,MPC执行触发的次数越多,在保证控制精度和稳定性的前提下,进一步减少了控制器的计算量。
表4 割草机器人试验固定时域与自适应时域MPC控制性能

Table 4 MPC Control performance of fixed time horizon and adaptive time horizon in mowing robot experiment

MPC预测时域

第1组

N p = 14

第2组

N p = 22

第3组

N p = 27

第4组

N p = 32

自适应时域
MPC控制时域 N c = 5 N c = 14 N c = 21 N c = 27
E d m a x/m 0.257 2 0.197 8 0.145 9 0.125 4 0.104 5
E d m/m 0.071 1 0.046 5 0.032 7 0.028 4 0.017 5
E d s/m 0.082 8 0.060 6 0.041 9 0.033 0 0.025 6
E θ m a x/rad 0.236 5 0.174 8 0.152 8 0.140 3 0.128 3
E θ m/rad 0.039 6 0.033 7 0.027 8 0.024 5 0.016 7
E θ s/rad 0.041 0 0.037 5 0.035 0 0.029 4 0.025 5
t m/s 0.006 7 0.010 3 0.012 6 0.015 8 0.004 9
图18 割草机器人路径跟踪中的参考路径曲率

Fig. 18 Reference path curvature in mowing robots path tracking

图19 自适应时域MPC控制器的事件触发执行次数变化

Fig. 19 Changes in event triggering execution times of adaptive time horizon MPC controller

4 结 论

为解决传统固定时域MPC时域调节与计算量之间的矛盾,以及提高割草机器人对于曲线路径的跟踪精度,本研究提出一种自适应时域MPC路径跟踪控制方法,并在自主设计的割草机器人试验平台进行了验证,主要结论如下。
1)设计的自适应时域MPC控制器能够根据前方预瞄区域的曲度因子以及曲度变化因子,利用模糊控制器自适应优化MPC的预测时域,并根据预测时域和曲度变化因子适当调整控制时域,在保证割草机器人对曲线路径跟踪精度的同时,有效降低整个MPC计算求解资源。同时设计的MPC事件触发执行机制能够根据前方预瞄区域的曲度与曲度变化因子以及累计误差,在保证控制精度与稳定性的前提下,进一步降低MPC的计算量。
2)自适应时域MPC控制器的最大横向误差绝对值和最大航向误差绝对值分别控制在11 cm和0.13 rad以内,表明该控制器能够使割草机器人有效跟踪曲率变化较大的曲线路径,并实现自主作业。
3)与最大固定时域MPC对照组相比,自适应时域MPC控制器的平均求解时间减少10.9 ms,有效降低了MPC的计算负荷,提高控制的实时性。

利益冲突声明

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

1
ZHOU R, LI M. Mowing robot trajectory tracking control algorithm research[C]// 2023 IEEE 3rd International Conference on Information Technology, Big Data and Artificial Intelligence (ICIBA). Piscataway, New Jersey, USA: IEEE, 2023, 3: 572-577.

2
SONG L P, HUANG J P, LIANG Q, et al. Trajectory tracking strategy for sliding mode control with double closed-loop for lawn mowing robot based on ESO[J]. IEEE Access, 2022, 11: 1867-1882.

3
WANG S R, YIN C Q, GAO J, et al. Lateral displacement control for agricultural tractor based on cascade control structure[J]. International journal of control, automation and systems, 2020, 18(9): 2375-2385.

4
MONTAZERI M, YOUSEFI M R, SHOJAEI K, et al. Design of fast variable structure adaptive fuzzy control for nonlinear state-delay systems with uncertainty[J]. IETE journal of research, 2022, 68(6): 4577-4589.

5
ZHAO S, ZHAO G, HE Y, et al. Biomimetic adaptive pure pursuit control for robot path tracking inspired by natural motion constraints[J]. Biomimetics, 2024, 9(1): 41.

6
丁承君, 施正, 冯玉伯, 等. 四轮驱动AGV自适应反演终端滑模轨迹跟踪控制[J]. 制造业自动化, 2023, 45(7): 139-144.

DING C J, SHI Z, FENG Y B, et al. Four-wheel drive AGV adaptive inversion terminal sliding mode trajectory tracking control[J]. Manufacturing automation, 2023, 45(7): 139-144.

7
WANG Z Q, SUN K Y, MA S Q, et al. Improved linear quadratic regulator lateral path tracking approach based on a real-time updated algorithm with fuzzy control and cosine similarity for autonomous vehicles[J]. Electronics, 2022, 11(22): ID 3703.

8
曹兴飞, 刘树锋, 史国友, 等. 基于模型预测控制的无人艇轨迹跟踪[J]. 船舶工程, 2023, 45(S1): 354-357.

CAO X F, LIU S F, SHI G Y, et al. Trajectory tracking of unmanned boat based on model predictive control[J]. Ship engineering, 2023, 45(S1): 354-357.

9
ZHOU Z D, ROTHER C, CHEN J. Event-triggered model predictive control for autonomous vehicle path tracking: Validation using CARLA simulator[J]. IEEE transactions on intelligent vehicles, 2023, 8(6): 3547-3555.

10
WURTS J, STEIN J L, ERSAL T. Collision imminent steering at high speed using nonlinear model predictive control[J]. IEEE transactions on vehicular technology, 2020, 69(8): 8278-8289.

11
XI Y G, LI D W, LIN S. Model predictive control-status and challenges[J]. Acta automatica sinica, 2014, 39(3): 222-236.

12
JEONG Y. Integrated vehicle controller for path tracking with rollover prevention of autonomous articulated electric vehicle based on model predictive control[J]. Actuators, 2023, 12(1): ID 41.

13
TAN Q F, QIU C, HUANG J, et al. Path tracking control strategy for off-road 4WS4WD vehicle based on robust model predictive control[J]. Robotics and autonomous systems, 2022, 158: ID 104267.

14
BETTEGA J, RICHIEDEI D. Trajectory tracking in an underactuated, non-minimum phase two-link multibody system through model predictive control with embedded reference dynamics[J]. Mechanism and machine theory, 2023, 180: ID 105165.

15
李骏, 万文星, 郝三强, 等. 复杂路况下无人驾驶路径跟踪模型预测控制研究[J]. 汽车工程, 2022, 44(5): 664-674.

LI J, WAN W X, HAO S Q, et al. Research on model predictive control of autonomous vehicle path tracking under complex road condition[J]. Automotive engineering, 2022, 44(5): 664-674.

16
寇发荣, 郑文博, 张新乾, 等. 采用状态扩展MPC与转角补偿的无人车路径跟踪控制[J]. 机械科学与技术, 2023, 42(9): 1533-1541.

KOU F R, ZHENG W B, ZHANG X Q, et al. Path tracking control of unmanned vehicle using state extended model predictive control and angle compensation[J]. Mechanical science and technology for aerospace engineering, 2023, 42(9): 1533-1541.

17
杜荣华, 胡鸿飞, 高凯, 等. 基于变预测时域MPC的自动驾驶汽车轨迹跟踪控制研究[J]. 机械工程学报, 2022, 58(24): 275-288.

DU R H, HU H F, GAO K, et al. Research on trajectory tracking control of autonomous vehicle based on MPC with variable predictive horizon[J]. Journal of mechanical engineering, 2022, 58(24): 275-288.

18
XUE W Y, ZHENG L. Active collision avoidance system design based on model predictive control with varying sampling time[J]. Automotive innovation, 2020, 3(1): 62-72.

19
范贤波, 彭育辉, 钟聪. 基于自适应MPC的自动驾驶汽车轨迹跟踪控制[J]. 福州大学学报(自然科学版), 2021, 49(4): 500-507.

FAN X B, PENG Y H, ZHONG C. Trajectory tracking control of autonomous vehicles based on adaptive MPC[J]. Journal of Fuzhou university (natural science edition), 2021, 49(4): 500-507.

20
LIN X Y, TANG Y L, ZHOU B H. Improved model predictive control path tracking strategy based an online updating algorithm with cosine similarity and a horizon factor[J]. IEEE transactions on intelligent transportation systems, 2022, 23(8): 12429-12438.

21
ZHOU B C, SU X, YU H J, et al. Research on path tracking of articulated steering tractor based on modified model predictive control[J]. Agriculture, 2023, 13(4): ID 871.

22
白国星, 孟宇, 刘立, 等. 基于可变预测时域及速度的车辆路径跟踪控制[J]. 中国机械工程, 2020, 31(11): 1277-1284.

BAI G X, MENG Y, LIU L, et al. Path tracking control of vehicles based on variable prediction horizon and velocity[J]. China mechanical engineering, 2020, 31(11): 1277-1284.

23
GONG J W, XU W, JIANG Y, et al. Multi-constrained model predictive control for autonomous ground vehicle trajectory tracking[J]. Journal of Beijing institute of technology, 2015, 24(4): 441-448.

24
龚建伟, 刘凯, 齐建永, 等. 无人驾驶车辆模型预测控制[M]. 北京: 人民邮电出版社, 2020.

GONG J W, LIU K, QI J Y, et al. Model predictive control for autonomous vehicles[M]. Beijing: People's Posts and Telecommunications Publishing House, 2020.

25
李耀华, 范吉康, 刘洋, 等. 自适应双时域参数MPC的智能车辆路径规划与跟踪控制[J]. 汽车安全与节能学报, 2021, 12(4): 528-539.

LI Y H, FAN J K, LIU Y, et al. Path planning and path tracking control for autonomous vehicle based on MPC with adaptive dual-horizon-parameters[J]. Journal of automotive safety and energy, 2021, 12(4): 528-539.

26
LI Y H, FAN J K, LIU Y, et al. Path planning and path tracking for autonomous vehicle based on MPC with adaptive dual-horizon-parameters[J]. International journal of automotive technology, 2022, 23(5): 1239-1253.

27
胡珉珉. 基于自适应模型预测控制的拖拉机路径跟踪控制研究[D]. 镇江: 江苏大学, 2022.

HU M M. Research on tractor path tracking control based on adaptive model predictive control[D]. Zhenjiang: Jiangsu University, 2022.

28
石佳晨. 基于MPC的智能稻麦收获机路径追踪方法研究[D]. 南京: 东南大学, 2020.

SHI J C. Research on path tracking method for intelligent harvester based on model predictive control[D].  Nanjing: Southeast University, 2020.

29
ZHANG H, ZHANG H M, WANG Z P, et al. Adaptive event based predictive lateral following control for unmanned ground vehicle system[J]. International journal of robust and nonlinear control, 2021, 31(10): 4744-4763.

30
SHAN T X, ENGLOT B. LeGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]// 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). New York, USA: ACM, 2018: 4758-4765.

文章导航

/