0 引言
车辆的路径跟踪控制是智能交通系统中的关键技术之一[1-3]。在该领域中，目前常见的控制方法包括滑模控制(sliding mode control, SMC)[4]、模型预测控制(model predictive control, MPC)[5-6]等。相比其他控制方法，MPC可以显式地考虑系统约束[7-8]，因此该方法正日益广泛地应用于解决车辆、轮式机器人、无人机等移动装备的路径跟踪控制问题。
基于MPC的车辆路径跟踪控制方法包含两种，分别是线性时变模型预测控制(linear time-varying model predictive control, LTV-MPC)[5-6,9-10]和非线性模型预测控制(nonlinear model predictive control， NMPC)[9-18]。LTV-MPC控制器的实时性较好，但随着计算机硬件性能的飞速发展，NMPC控制器精确性更好的优势使其在无人驾驶领域中的地位变得日益重要[9-10]。
在基于NMPC的车辆路径跟踪控制研究成果中，文献[9-10]采用的是基本的NMPC控制器，文献[11]提出了用两个NMPC控制器同时控制车辆横向运动和纵向运动的方法，文献[12]采用遗传算法(genetic-algorithm, GA)优化了NMPC控制器的目标惩罚函数和约束条件，文献[13-14]基于强化学习改进了NMPC控制器的鲁棒性，文献[15-16]在基于NMPC的路径跟踪控制器中加入了避障功能，文献[17]着重考虑了轮胎滑移对路径跟踪控制的影响，文献[18]则考虑了控制时域与NMPC控制器稳定性的关系。
文献[11-18]的研究工作显著提高了NMPC路径跟踪控制器的性能，但上述文献与文献[9-10]一样未考虑预测时域对车辆路径跟踪控制效果的影响。笔者发现在考虑文献[5,9]中由实验获得的转向角速度约束时，预测时域和速度对路径跟踪控制性能具有显著的影响。如当预测时域过小或速度过快时，车辆无法及时达到期望的转向角，会产生较大的路径跟踪误差，甚至会导致路径跟踪控制失败。
本文研究了预测时域、速度与NMPC控制器性能之间的映射关系，提出了用于改进NMPC控制器的最佳预测时域和参考速度控制律，从而进一步提高了基于NMPC的车辆路径跟踪控制性能。考虑到文献[9-18]中的NMPC框架基本相同，为保证本研究成果的可拓展性，本文的研究基础同样采用文献[9-10]中传统的NMPC路径跟踪控制器。
1 现象分析
在文献[9-10]提出的NMPC路径跟踪控制器中，预测模型的基础是车辆的运动学模型，其表达式如下：
式中，x、y分别为车辆后桥中心的横坐标和纵坐标；θ为航向角；v为车速；δ为前轮转角；l为轴距。
可将上述运动学模型抽象为矢量形式，即
x=(x，y，θ)T u=(v，δ)T
式中，x为状态向量；u为控制输入；f(·)表示映射关系。
采用欧拉法对式(2)进行离散化即可获得预测模型：
式中，i表示t时刻的第i个预测时序；x(i|t)表示t时刻车辆的第i个预测位姿状态；u(i|t)表示t时刻的第i个预测控制输入向量；T为控制周期；Np为预测时域；Nc为控制时域。
在此基础上，t时刻车辆的第i个预测位姿与参考位姿间的误差可定义为
e(i|t)=x(i|t)-xr(i|t)
(4)
式中，xr(i|t)表示t时刻参考路径上的第i个参考点的位姿状态。
优化目标函数即为
式中，Q、R为权重矩阵。
由式(3)可知，若车速和控制周期固定，则当Np的值较大时，预测的距离较远，相当于距离车辆当前位置较远处误差的权重较大，因此会在车辆当前位置附近产生较大的误差，如图1a所示。当Np的值较小时，在文献[5,9]中提到的车辆转向轮的转向角速度约束影响下，车辆无法及时转向，从而产生较大的跟踪误差，甚至造成车辆路径跟踪控制失败，如图1b所示。
(a) Np值较大
(b) Np值较小
图1 预测时域对路径跟踪控制的影响
Fig.1 Effect of prediction horizon on path tracking control
对于速度对车辆路径跟踪控制的影响，虽然车辆的稳态转弯半径主要取决于转向角，但由于转向角速度约束的存在，车辆无法瞬间达到期望转向角，因此速度直接决定车辆达到期望转向角之前驶过的距离。如图2所示，当车速较低时，车辆达到期望转向角的距离较短；而当车速较高时，行驶距离较长，若路径曲率变化较大则会造成较大的跟踪误差。
图2 车速对转向性能的影响
Fig.2 Effect of vehicle velocity on steering performance
综上可知，预测时域和速度对车辆路径跟踪控制器的性能有显著的影响，因此有必要研究预测时域、速度与控制器性能之间的联系，进而改进基于NMPC的车辆路径跟踪控制器。
2 最佳预测时域及参考速度控制律
为获得预测时域、速度与控制器性能之间的联系，本文通过MATLAB/Simulink R2014b对文献[9-10]中提出的NMPC控制器进行了仿真测试。测试中NMPC控制器的固定参数如表1所示。
表1 NMPC控制器参数
Tab.1 NMPC controller parameters
参数数值车辆轴距l(m)2.458控制周期T(s)0.05纵向加速度约束v·lim(m/s2)-1^1转向角速度约束φ·lim((°)/s)-9.4^9.4转向角约束φlim(°)-25^25误差惩罚项权重矩阵Q100001000010控制量惩罚项权重矩阵R0.5000.5
测试中的参考路径为公路系统中常见的直线-圆弧路径，如图3所示。其中，圆弧路径的半径可通过稳态转向角给定，其表达式如下[5,9-10]：
图3 参考路径
Fig.3 Reference path
式中，r为圆弧路径半径；φ为稳态转向角。
此外选取控制时域Nc=Np-1，从而使测试输入变量只包括稳态转向角、预测时域和速度。测试输出的评价指标为横向误差，如图4所示。图4中，θ为航向角，v为速度，P0为参考路径上距离车辆P最近的点，P1P2为参考路径在P0点处的切线，ed为横向误差(即PP0的长度)，此外定义航向误差eh为车辆航向与参考路径在P0点处航向之差。
图4 横向误差和航向误差示意图
Fig.4 Diagram of lateral error and heading error
在测试中可以发现，存在能够使车辆在某一速度下横向误差最小的预测时域，而在预测时域确定时，纵向速度与横向误差的最大值在小范围内成近似线性的关系。如图5所示，以稳态转向角为25°为例，当速度固定为1.3 m/s、预测时域为41步时，路径跟踪的最大横向误差为0.086 6 m，预测时域为42步和40步时，最大横向误差值则分别为0.101 4 m和0.094 1 m；当预测时域固定为41步、速度为1.4 m/s时，最大横向误差值为0.110 9 m。
图5 稳态转向角为25°时的预测时域、速度与误差
Fig.5 Prediction horizon, velocity and error for steady-state steering angle is as 25°
在路径跟踪控制中，最大横向误差在0.1 m范围内可被视为控制性能良好，因此当稳态转向角为25°时，选取能够实现最大横向误差在0.1 m以内的速度1.3 m/s作为该稳态转向角对应的最大速度，并选取该速度对应的预测时域41步作为该稳态转向角对应的最佳预测时域。
基于上述方法，分别采集稳态转向角为24°、22°、20°、15°、10°和5°时的最大速度和最佳预测时域，测试获得的数据如表2所示。
表2 测试获得的数据
Tab.2 Data obtained from the test
稳态转向角φ(°)最大速度vmax(m/s)最佳预测时域Noptp251.341241.735222.131202.627154.021107.015515.710
基于三次多项式[19-20]对表2中的数据进行拟合，可得最大速度与稳态转向角的关系近似为
vmax=-0.003 351φ3+0.206 8φ2-4.172φ+31.79
(7)
最佳预测时域与稳态转向角的关系近似为
Np=0.005 037φ3-0.180 6φ2+3.012φ-1.375
(8)
因为预测步长固定时，速度越低，车辆的路径跟踪控制系统越稳定，所以为进一步保证控制的稳定性，设置参考速度为
vr=(1-0.05)vmax
(9)
将最佳预测时域向上取整，参考速度以0.1 m/s的分段向下取整，即可得到能够保证横向误差在0.1 m以内的最佳预测时域和参考速度控制律，如图6和图7所示。
图6 最佳预测时域控制律
Fig.6 Control law of the optimal prediction horizon
图7 参考速度控制律
Fig.7 Control law of the reference velocity
自变量稳态转向角则通过车辆前方一定距离内参考路径的最大曲率cmax确定，其表达式如下：
φ=arctan(lcmax)
(10)
上述距离的选取需要考虑较多因素。由于紧急停车通常由比路径跟踪控制优先级更高的程序控制，故本文不考虑紧急停车状态，并参考文献[5,9]中考虑舒适性的平稳减速约束，将车辆减速度上限设为1 m/s2。此外假设本文中的车辆仅在市区道路行驶，最高时速不超过60 km/h(即16.67 m/s)。不考虑紧急停车时，由16.67 m/s减速到零约需行驶140 m。然而若设置上述距离S=140 m，则当车辆前方140 m处路径的曲率较大且车辆当前正在以较低速度行驶时，车辆将以较低速度行驶较长的距离，进而会严重降低车辆的行驶效率。所以为了提高行驶效率，设置如下公式：
式中，
为车辆前方140 m内参考路径的最大曲率对应的参考速度。
通过式(11)求得S后，取S范围内参考路径的最大曲率代入式(10)，进而根据图6和图7中的控制律，即可获得当前时刻车辆路径跟踪控制器应选取的参考速度与最佳预测时域。使用上述参数替换文献[9-10]中NMPC控制器中的参考速度和最佳预测时域，即可得到改进后的NMPC路径跟踪控制器。
3 仿真验证
本文改进后的控制器通过MATLAB/Simulink和PreScan进行联合仿真验证，图8所示为PreScan模型。由于车辆位姿状态信息可由PreScan模型直接给出，故仿真系统中不包含定位等环境感知模块。
图8 PreScan模型
Fig.8 PreScan model
仿真对照组包括两个控制器。第一个是传统的NMPC控制器，其预测时域设为41步，参考速度设为14.8 m/s。第二个是采用本文提出的参考速度控制律但预测时域为定值的NMPC控制器，其预测时域设为41步。这两个控制器的其他参数与本文提出的控制器中的参数完全一致，参数取值见表1。
3.1 第一组仿真
第一组仿真的目的是在较典型的直线-圆弧路径上测试控制器的性能，参考路径的直线长度为140 m，圆弧半径为10 m，航向变化角共270°。图9显示了第一组仿真的轨迹，可以看出，采用最佳预测时域控制律和参考速度控制律的NMPC控制器控制效果最佳，单独采用参考速度控制律的NMPC控制器也能实现路径跟踪，不采用控制律的传统NMPC控制器在路径曲率发生较大变化时产生了很大的误差。
图9 第一组仿真轨迹
Fig.9 Trajectory of the first set of simulations
图10显示了第一组仿真的速度变化，单独采用参考速度控制律的NMPC控制器能够使车辆及时减速，而未采用参考速度控制律的传统NMPC控制器只是在车辆已经冲出路径时使速度产生了一些波动。图11显示了第一组仿真的转向角，未采用控制律的传统NMPC控制器未进行稳态转向，另两个控制器的稳态转向角约为0.241 0 rad(即13.81°)，等于式(6)中φ的计算值。如图10和图11所示，由曲线的变化速度和幅度可知，三个控制器的纵向加速度、转向角速度和转向角都在约束条件范围内。
图10 第一组仿真速度
Fig.10 Velocity of the first set of simulations
图11 第一组仿真转向角角度
Fig.11 Steering angle of the first set of simulations
图12显示了第一组仿真的预测时域变化，未采用最佳预测时域控制律的两个控制器的预测时域一直维持在41步，而采用了本文提出的最佳预测时域控制律的控制器，其预测时域发生了与路径对应的变化。
图12 第一组仿真预测时域
Fig.12 Prediction horizon of the first set of simulations
图13和图14分别显示了第一组仿真的横向误差和航向误差。采用最佳预测时域控制律和参考速度控制律的NMPC控制器的最大横向误差和最大航向误差分别为0.083 4 m和0.054 8 rad，单独采用参考速度控制律的NMPC控制器的最大横向误差和最大航向误差分别为0.365 0 m和0.116 6 rad，传统NMPC控制器的最大横向误差和最大航向误差分别为4.350 5 m和0.447 5 rad。
图13 第一组仿真横向误差
Fig.13 Lateral error of the first set of simulations
图14 第一组仿真航向误差
Fig.14 Heading error of the first set of simulations
由第一组仿真结果可知，采用本文提出的最佳预测时域控制律和参考速度控制律后，NMPC控制器的性能有了显著的提高。相比传统的NMPC控制器，本文提出的改进NMPC控制器的最大横向误差减小了4.267 1 m，最大航向误差减小了0.392 7 rad。此外，同时采用最佳预测时域控制律和参考速度控制律的NMPC控制器性能比单独采用参考速度控制律的NMPC控制器性能更优，最大横向误差和最大航向误差的差值分别为0.281 6 m和0.061 8 rad。
3.2 第二组仿真
为进一步验证本文提出的改进NMPC控制器的性能，选取更加复杂的路径作为参考路径。第二组仿真中的参考路径依然是公路系统中常见的直线-圆弧路径，其中圆弧半径分别为12.5 m、6.67 m、20 m和10 m，各圆弧处的航向变化分别为180°、270°、180°和90°。图15显示了第二组仿真的PreScan模型。图16显示了第二组仿真的轨迹，同时采用最佳预测时域控制律和参考速度控制律的NMPC控制器控制效果较好，单独采用参考速度控制律的NMPC控制器误差较大，但依然能实现路径跟踪，未采用控制律的NMPC控制器在路径曲率变化较大时误差很大，可以视为路径跟踪失败。
图15 第二组仿真路径
Fig.15 Path of the second set of simulations
图16 第二组仿真轨迹
Fig.16 Trajectory of the second set of simulations
图17和图18分别显示了第二组仿真的速度和转向角，由曲线的变化速度和幅度可知，三个控制器的纵向加速度、转向角速度和转向角都在约束条件范围内。图19显示了第二组仿真的预测时域。图20和图21分别显示了横向误差和航向误差。采用最佳预测时域控制律和参考速度控制律的NMPC控制器的最大横向误差和最大航向误差分别为0.092 8 m和0.072 4 rad。单独采用参考速度控制律的NMPC控制器的最大横向误差和最大航向误差分别为1.116 0 m和0.142 9 rad。未采用控制律的NMPC控制器的最大横向误差和最大航向误差23.465 1 m和1.180 0 rad。由上述数据可知，在不采用控制律的情况下，在误差最大处车辆已经远离车道，极易发生危险。
图17 第二组仿真速度
Fig.17 Velocity of the second set of simulations
图18 第二组仿真转向角
Fig.18 Steering angle of the second set of simulations
图19 第二组仿真预测时域
Fig.19 Pradiction horizon of the second set of simulations
图20 第二组仿真横向误差
Fig.20 Lateral error of the second set of simulations
图21 第二组仿真航向误差
Fig.21 Heading error of the second set of simulations
由第二组仿真结果可知，在复杂道路环境下，未采用控制律的传统NMPC控制器的性能很差，本文提出的改进NMPC控制器相比传统NMPC控制器将最大横向误差减小了23.372 3 m，将最大航向误差减小了1.107 6 rad。同时采用最佳预测时域控制律和参考速度控制律的NMPC控制器相比单独采用参考速度控制律的NMPC控制器将最大横向误差减小了1.023 2 m，将最大航向误差减小了0.070 5 rad。
4 结论与展望
(1)在基于非线性模型预测控制(NMPC)的车辆路径跟踪控制中，存在稳态转向角与在该稳态转向角下达到某一跟踪精度的最大速度的映射关系，且该稳态转向角、最大速度存在对应的最佳预测时域值。
(2)基于本文提出的最佳预测时域控制律和参考速度控制律，可以使NMPC路径跟踪控制系统在跟踪复杂路径时更加精确。在本文的仿真中，采用最佳预测时域控制律和参考速度控制律的NMPC控制器的最大横向误差为0.092 8 m，最大航向误差为0.072 4 rad。
(3)在本文的仿真中，基于最佳预测时域控制律和参考速度控制律，改进后的NMPC控制器相比传统控制器将最大横向误差和最大航向误差分别减小了4.267 1 m以上和0.392 7 rad以上。
(4)相比只采用参考速度控制律的NMPC控制器，同时采用最佳预测时域控制律和参考速度控制律的NMPC控制性能更佳。由此可以证明本文提出的最佳预测时域控制律和参考速度控制律具有有效性。
由于在实际的公路交通系统中，车辆的行驶速度还需要考虑交通状况，因此，在未来的工作中，需要将本文得到的速度控制律与基于交通系统的速度决策系统进行整合，从而进一步提高基于NMPC的车辆路径跟踪控制系统的实用性。
[1] JI X, LIU Y, HE X, et al. Interactive Control Paradigm Based Robust Lateral Stability Controller Design for Autonomous Automobile Path Tracking with Uncertain Disturbance: a Dynamic Game Approach[J]. IEEE Transactions on Vehicular Technology, 2018, 67(8):6906-6920.
[2] CHEEIN F A. Intelligent Sampling Technique for Path Tracking Controllers[J]. IEEE Transactions on Control Systems Technology, 2016, 24(2):747-755.
[3] 陈慧岩, 张玉. 军用地面无人机动平台技术发展综述[J]. 兵工学报, 2014, 35(10):1696-1706.
CHEN Huiyan, ZHANG Yu. An Overview of Research on Military Unmanned Ground Vehicles[J]. Acta Armamentarii, 2014, 35(10):1696-1706.
[4] 李逃昌. 农业轮式移动机器人自适应滑模路径跟踪控制[J]. 中国机械工程, 2018, 29(5):579-584.
LI Taochang. Adaptive Sliding Mode Path Tracking Control of Agricultural Wheeled Mobile Robots[J]. China Mechanical Engineering, 2018, 29(5):579-584.
[5] 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-443.
[6] 武星, 楼佩煌. 基于运动预测的路径跟踪最优控制研究[J]. 控制与决策, 2009, 24(4):565-569.
WU Xing, LOU Peihuang. Optimal Path Tracking Control Based on Motion Prediction[J]. Control and Decision, 2009, 24(4):565-569.
[7] MACIEJOWSKI J M. Predictive Control with Constraints[M]. New York: Pearson Education, 2002.
[8] 席裕庚，李德伟，林姝.模型预测控制——现状与挑战[J].自动化学报，2013，39(3)：222-236.
XI Yugeng, LI Dewei, LIN SHu. Model Predictive Control—Status and Challenges[J]. Acta Automatica Sinica, 2013, 39(3):222-236.
[9] 龚建伟，姜岩，徐威.无人驾驶车辆模型预测控制[M].北京：北京理工大学出版社，2014：18-65.
GONG Jianwei, JIANG Yan, XU Wei. Model Predictive Control for Self-driving Vehicles[M]. Beijing: Beijing Institute of Technology Press, 2014:18-65.
[10] OYELERE S S. The Application of Model Predictive Control(MPC) to Fast Systems Such as Autonomous Ground Vehicles(AGV)[J]. IOSR Journal of Computer Engineering, 2014, 3(3):27-37.
[11] ATTIA R, ORJUELA R, BASSET M. Combined Longitudinal and Lateral Control for Automated Vehicle Guidance[J]. Vehicle System Dynamics, 2014, 52(2):261-279.
[12] DU X, HTET K K K, TAN K K. Development of a Genetic-algorithm-based Nonlinear Model Predictive Control Scheme on Velocity and Steering of Autonomous Vehicles[J]. IEEE Transactions on Industrial Electronics, 2016, 63(11):6970-6977.
[13] OSTAFEW C J, SCHOELLIG A P, BARFOOT T D, et al. Learning-based Nonlinear Model Predictive Control to Improve Vision-based Mobile Robot Path Tracking[J]. Journal of Field Robotics, 2016, 33(1):133-152.
[14] OSTAFEW C J, SCHOELLIG A P, BARFOOT T D. Robust Constrained Learning-based NMPC Enabling Reliable Mobile Robot Path Tracking[J]. The International Journal of Robotics Research, 2016, 35(13):1547-1563.
[15] YOON Y, SHIN J, KIM H J, et al. Model-predictive Active Steering and Obstacle Avoidance for Autonomous Ground Vehicles[J]. Control Engineering Practice, 2009, 17(7):741-750.
[16] 王怿, 祝小平, 周洲,等. 3维动态环境下的无人机路径跟踪算法[J]. 机器人, 2014, 36(1):83-91.
WANG Yi, ZHU Xiaoping, ZHOU Zhou, et al. UAV Path Following in 3-D Dynamic Environment[J]. Robot, 2014, 36(1):83-91.
[17] KRAUS T, FERREAU H J, KAYACAN E, et al. Moving Horizon Estimation and Nonlinear Model Predictive Control for Autonomous Agricultural Vehicles[J]. Computers and Electronics in Agriculture, 2013, 98:25-33.
[18] FRUCHARD M, ALLIBERT G, COURTIAL E. Choice of the Control Horizon in an NMPC Strategy for the Full-state Control of Nonholonomic Systems[C]//2012 American Control Conference (ACC). Montreal: IEEE Press, 2012:4149-4154.
[19] SCIUTTO S J. Polyfit:a Package for Polynomial Fitting[J]. Computer Physics Communications, 1989, 52(3):427-442.
[20] 苏金明, 张莲花, 刘波. MATLAB工具箱应用[M]. 北京：电子工业出版社, 2004.
SU Jinming, ZHANG Lianhua, LIU Bo. Application of MATLAB Toolbox[M]. Beijing: Publishing House of Electronics Industry, 2004.
